eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

v1.12.0 #269

Merged
muellerr merged 493 commits from develop into main 2022-07-04 11:19:05 +02:00
Conversation 0 Commits 493 Files Changed 264 +144 -153
1 changed files with 144 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 3ff709a814 - Show all commits

297
thirdparty/libcsp/src/drivers/can/can_socketcan.c vendored
View File

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