From 3ff709a814fd9133bb89939c416ae530d1cace89 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 30 Apr 2022 16:52:22 +0200 Subject: [PATCH] removed changes can rx task --- .../libcsp/src/drivers/can/can_socketcan.c | 297 +++++++++--------- 1 file changed, 144 insertions(+), 153 deletions(-) diff --git a/thirdparty/libcsp/src/drivers/can/can_socketcan.c b/thirdparty/libcsp/src/drivers/can/can_socketcan.c index b3146870..00d6444e 100644 --- a/thirdparty/libcsp/src/drivers/can/can_socketcan.c +++ b/thirdparty/libcsp/src/drivers/can/can_socketcan.c @@ -19,192 +19,183 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */ /* SocketCAN driver */ -#include #include -#include + +#include +#include + +#include +#include +#include +#include #include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + +#include +#include #ifdef CSP_HAVE_LIBSOCKETCAN #include #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; }