v1.12.0 #269
297
thirdparty/libcsp/src/drivers/can/can_socketcan.c
vendored
297
thirdparty/libcsp/src/drivers/can/can_socketcan.c
vendored
@ -19,192 +19,183 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
/* SocketCAN driver */
|
||||
#include <csp/csp.h>
|
||||
#include <csp/drivers/can_socketcan.h>
|
||||
#include <csp/interfaces/csp_if_can.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <time.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <pthread.h>
|
||||
#include <semaphore.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <net/if.h>
|
||||
#include <sys/queue.h>
|
||||
#include <sys/uio.h>
|
||||
#include <sys/time.h>
|
||||
#include <net/if.h>
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <linux/socket.h>
|
||||
#include <net/if.h>
|
||||
#include <pthread.h>
|
||||
#include <semaphore.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/queue.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/uio.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <csp/csp.h>
|
||||
#include <csp/interfaces/csp_if_can.h>
|
||||
|
||||
#ifdef CSP_HAVE_LIBSOCKETCAN
|
||||
#include <libsocketcan.h>
|
||||
#endif
|
||||
|
||||
#define CAN_RX_TASK_PRIO 80
|
||||
|
||||
static struct can_socketcan_s {
|
||||
int socket;
|
||||
csp_iface_t interface;
|
||||
int socket;
|
||||
csp_iface_t interface;
|
||||
} socketcan[1] = {
|
||||
{
|
||||
.interface =
|
||||
{
|
||||
.name = "CAN",
|
||||
.nexthop = csp_can_tx,
|
||||
.mtu = CSP_CAN_MTU,
|
||||
.driver = &socketcan[0],
|
||||
},
|
||||
},
|
||||
{
|
||||
.interface = {
|
||||
.name = "CAN",
|
||||
.nexthop = csp_can_tx,
|
||||
.mtu = CSP_CAN_MTU,
|
||||
.driver = &socketcan[0],
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
static void *socketcan_rx_thread(void *parameters) {
|
||||
struct can_frame frame;
|
||||
int nbytes;
|
||||
static void * socketcan_rx_thread(void * parameters)
|
||||
{
|
||||
struct can_frame frame;
|
||||
int nbytes;
|
||||
|
||||
while (1) {
|
||||
/* Read CAN frame */
|
||||
nbytes = read(socketcan[0].socket, &frame, sizeof(frame));
|
||||
if (nbytes < 0) {
|
||||
csp_log_error("read: %s", strerror(errno));
|
||||
continue;
|
||||
}
|
||||
while (1) {
|
||||
/* Read CAN frame */
|
||||
nbytes = read(socketcan[0].socket, &frame, sizeof(frame));
|
||||
if (nbytes < 0) {
|
||||
csp_log_error("read: %s", strerror(errno));
|
||||
continue;
|
||||
}
|
||||
|
||||
if (nbytes != sizeof(frame)) {
|
||||
csp_log_warn("Read incomplete CAN frame");
|
||||
continue;
|
||||
}
|
||||
if (nbytes != sizeof(frame)) {
|
||||
csp_log_warn("Read incomplete CAN frame");
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Frame type */
|
||||
if (frame.can_id & (CAN_ERR_FLAG | CAN_RTR_FLAG) || !(frame.can_id & CAN_EFF_FLAG)) {
|
||||
/* Drop error and remote frames */
|
||||
csp_log_warn("Discarding ERR/RTR/SFF frame");
|
||||
continue;
|
||||
}
|
||||
/* Frame type */
|
||||
if (frame.can_id & (CAN_ERR_FLAG | CAN_RTR_FLAG) || !(frame.can_id & CAN_EFF_FLAG)) {
|
||||
/* Drop error and remote frames */
|
||||
csp_log_warn("Discarding ERR/RTR/SFF frame");
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Strip flags */
|
||||
frame.can_id &= CAN_EFF_MASK;
|
||||
/* Strip flags */
|
||||
frame.can_id &= CAN_EFF_MASK;
|
||||
|
||||
/* Call RX callbacsp_can_rx_frameck */
|
||||
csp_can_rx(&socketcan[0].interface, frame.can_id, frame.data, frame.can_dlc, NULL);
|
||||
}
|
||||
/* Call RX callbacsp_can_rx_frameck */
|
||||
csp_can_rx(&socketcan[0].interface, frame.can_id, frame.data, frame.can_dlc, NULL);
|
||||
}
|
||||
|
||||
/* We should never reach this point */
|
||||
pthread_exit(NULL);
|
||||
/* We should never reach this point */
|
||||
pthread_exit(NULL);
|
||||
}
|
||||
|
||||
static int create_receive_thread() {
|
||||
pthread_t rx_thread;
|
||||
pthread_attr_t attributes;
|
||||
if (pthread_attr_init(&attributes) != 0) {
|
||||
return 1;
|
||||
}
|
||||
if (pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED) != 0) {
|
||||
return 1;
|
||||
}
|
||||
if (pthread_attr_setschedpolicy(&attributes, SCHED_FIFO) != 0) {
|
||||
return 1;
|
||||
}
|
||||
struct sched_param schedule_params;
|
||||
schedule_params.__sched_priority = CAN_RX_TASK_PRIO;
|
||||
if (pthread_attr_setschedparam(&attributes, &schedule_params) != 0) {
|
||||
return 1;
|
||||
}
|
||||
if (pthread_create(&rx_thread, NULL, socketcan_rx_thread, NULL) != 0) {
|
||||
csp_log_error("pthread_create: %s", strerror(errno));
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
|
||||
int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t * data, uint8_t dlc)
|
||||
{
|
||||
struct can_frame frame;
|
||||
int i, tries = 0;
|
||||
memset(&frame, 0, sizeof(frame));
|
||||
if (dlc > 8)
|
||||
return -1;
|
||||
|
||||
/* Copy identifier */
|
||||
frame.can_id = id | CAN_EFF_FLAG;
|
||||
|
||||
/* Copy data to frame */
|
||||
for (i = 0; i < dlc; i++)
|
||||
frame.data[i] = data[i];
|
||||
|
||||
/* Set DLC */
|
||||
frame.can_dlc = dlc;
|
||||
|
||||
/* Send frame */
|
||||
while (write(socketcan[0].socket, &frame, sizeof(frame)) != sizeof(frame)) {
|
||||
if (++tries < 1000 && errno == ENOBUFS) {
|
||||
/* Wait 10 ms and try again */
|
||||
usleep(10000);
|
||||
} else {
|
||||
csp_log_error("write: %s", strerror(errno));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t *data, uint8_t dlc) {
|
||||
struct can_frame frame;
|
||||
int i, tries = 0;
|
||||
memset(&frame, 0, sizeof(frame));
|
||||
if (dlc > 8) return -1;
|
||||
csp_iface_t * csp_can_socketcan_init(const char * ifc, int bitrate, int promisc)
|
||||
{
|
||||
struct ifreq ifr;
|
||||
struct sockaddr_can addr;
|
||||
pthread_t rx_thread;
|
||||
|
||||
/* Copy identifier */
|
||||
frame.can_id = id | CAN_EFF_FLAG;
|
||||
|
||||
/* Copy data to frame */
|
||||
for (i = 0; i < dlc; i++) frame.data[i] = data[i];
|
||||
|
||||
/* Set DLC */
|
||||
frame.can_dlc = dlc;
|
||||
|
||||
/* Send frame */
|
||||
while (write(socketcan[0].socket, &frame, sizeof(frame)) != sizeof(frame)) {
|
||||
if (++tries < 1000 && errno == ENOBUFS) {
|
||||
/* Wait 10 ms and try again */
|
||||
usleep(10000);
|
||||
} else {
|
||||
csp_log_error("write: %s", strerror(errno));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
csp_iface_t *csp_can_socketcan_init(const char *ifc, int bitrate, int promisc) {
|
||||
struct ifreq ifr;
|
||||
struct sockaddr_can addr;
|
||||
|
||||
// printf("-I-: Initiating CAN interface %s\n", ifc);
|
||||
//printf("-I-: Initiating CAN interface %s\n", ifc);
|
||||
|
||||
#ifdef CSP_HAVE_LIBSOCKETCAN
|
||||
/* Set interface up */
|
||||
if (bitrate > 0) {
|
||||
can_do_stop(ifc);
|
||||
can_set_bitrate(ifc, bitrate);
|
||||
can_set_restart_ms(ifc, 100);
|
||||
can_do_start(ifc);
|
||||
}
|
||||
/* Set interface up */
|
||||
if (bitrate > 0) {
|
||||
can_do_stop(ifc);
|
||||
can_set_bitrate(ifc, bitrate);
|
||||
can_set_restart_ms(ifc, 100);
|
||||
can_do_start(ifc);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Create socket */
|
||||
if ((socketcan[0].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
csp_log_error("socket: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
/* Create socket */
|
||||
if ((socketcan[0].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
csp_log_error("socket: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* Locate interface */
|
||||
strncpy(ifr.ifr_name, ifc, IFNAMSIZ - 1);
|
||||
if (ioctl(socketcan[0].socket, SIOCGIFINDEX, &ifr) < 0) {
|
||||
csp_log_error("ioctl: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
memset(&addr, 0, sizeof(addr));
|
||||
/* Bind the socket to CAN interface */
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
if (bind(socketcan[0].socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
csp_log_error("bind: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
/* Locate interface */
|
||||
strncpy(ifr.ifr_name, ifc, IFNAMSIZ - 1);
|
||||
if (ioctl(socketcan[0].socket, SIOCGIFINDEX, &ifr) < 0) {
|
||||
csp_log_error("ioctl: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
memset(&addr, 0, sizeof(addr));
|
||||
/* Bind the socket to CAN interface */
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
if (bind(socketcan[0].socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
csp_log_error("bind: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* Set filter mode */
|
||||
if (promisc == 0) {
|
||||
struct can_filter filter;
|
||||
filter.can_id = CFP_MAKE_DST(csp_get_address());
|
||||
filter.can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
|
||||
/* Set filter mode */
|
||||
if (promisc == 0) {
|
||||
|
||||
if (setsockopt(socketcan[0].socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
|
||||
csp_log_error("setsockopt: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
struct can_filter filter;
|
||||
filter.can_id = CFP_MAKE_DST(csp_get_address());
|
||||
filter.can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
|
||||
|
||||
if (create_receive_thread() != 0) {
|
||||
return NULL;
|
||||
}
|
||||
if (setsockopt(socketcan[0].socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
|
||||
csp_log_error("setsockopt: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
|
||||
csp_iflist_add(&socketcan[0].interface);
|
||||
}
|
||||
|
||||
return &socketcan[0].interface;
|
||||
/* Create receive thread */
|
||||
if (pthread_create(&rx_thread, NULL, socketcan_rx_thread, NULL) != 0) {
|
||||
csp_log_error("pthread_create: %s", strerror(errno));
|
||||
return NULL;
|
||||
}
|
||||
|
||||
csp_iflist_add(&socketcan[0].interface);
|
||||
|
||||
return &socketcan[0].interface;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user