From 5600c6dc513bd7e11e31b044ca07d522c8eddcf3 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Wed, 29 Oct 2014 17:00:21 +0100 Subject: [PATCH] Add cansend examples --- .gitignore | 1 + candump-recvmmsg.c | 12 +++--- cansend-mmap.c | 91 ++++++++++++++++++++++++++++++++++++++++++++++ cansend-sendmmsg.c | 74 +++++++++++++++++++++++++++++++++++++ cansend-write.c | 63 ++++++++++++++++++++++++++++++++ common.h | 35 ++++++++++++++++++ 6 files changed, 270 insertions(+), 6 deletions(-) create mode 100644 .gitignore create mode 100644 cansend-mmap.c create mode 100644 cansend-sendmmsg.c create mode 100644 cansend-write.c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b25c15b --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +*~ diff --git a/candump-recvmmsg.c b/candump-recvmmsg.c index 2b44993..66408f8 100644 --- a/candump-recvmmsg.c +++ b/candump-recvmmsg.c @@ -21,9 +21,9 @@ struct mmsghdr msgs[MMSG_MAX]; struct iovec iovecs[MMSG_MAX]; -char bufs[MMSG_MAX][sizeof(struct can_frame)]; +struct can_frame cf[MMSG_MAX]; -int init_recvmmsg_socket(const char *dev) +int init_recvmmsg(const char *dev) { int sock; struct sockaddr_can addr; @@ -46,8 +46,8 @@ int init_recvmmsg_socket(const char *dev) memset(msgs, 0, sizeof(msgs)); memset(iovecs, 0, sizeof(iovecs)); for (i = 0; i < MMSG_MAX; i++) { - iovecs[i].iov_base = bufs[i]; - iovecs[i].iov_len = sizeof(struct can_frame); + iovecs[i].iov_base = &cf[i]; + iovecs[i].iov_len = sizeof(cf[i]); msgs[i].msg_hdr.msg_iov = &iovecs[i]; msgs[i].msg_hdr.msg_iovlen = 1; } @@ -62,7 +62,7 @@ void can_recvmmsg(int sock) printf("Received %d CAN frames in one system call\n", received); for (i = 0; i < received; i++) - print_can_frame((struct can_frame*)bufs[i]); + print_can_frame(&cf[i]); } int main(int argc, char *argv[]) @@ -72,7 +72,7 @@ int main(int argc, char *argv[]) dev = candump_parse_args(argc, argv); - sock = init_recvmmsg_socket(dev); + sock = init_recvmmsg(dev); while (1) can_recvmmsg(sock); diff --git a/cansend-mmap.c b/cansend-mmap.c new file mode 100644 index 0000000..e37ffd7 --- /dev/null +++ b/cansend-mmap.c @@ -0,0 +1,91 @@ +/**************************************************************/ +/* CAN system calls example */ +/* Author: Michal Sojka, Czech Technical University in Prague */ +/* License: Public domain */ +/**************************************************************/ + +#include "common.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include /* the L2 protocols */ +#include +#include +#include +#include + +#define FRAME_SIZE 128 +#define BLOCK_SIZE 4096 +#define BLOCK_NR 2 +#define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE)) + +void *tx_ring_buffer; + +int init_mmap_socket(const char *dev) +{ + int sock; + struct sockaddr_ll addr; + struct ifreq ifr; + + sock = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL))); + + int val = TPACKET_V2; + CHECK(setsockopt(sock, SOL_PACKET, PACKET_VERSION, &val, sizeof(val))); + + strncpy (ifr.ifr_name, dev, sizeof(ifr.ifr_name)); + CHECK(ioctl(sock, SIOCGIFINDEX, &ifr)); + + addr.sll_family = AF_PACKET; + addr.sll_protocol = htons(ETH_P_CAN); + addr.sll_ifindex = ifr.ifr_ifindex; + + CHECK(bind(sock, (struct sockaddr *)&addr, sizeof(struct sockaddr_ll))); + + struct tpacket_req req = { + .tp_block_size = BLOCK_SIZE, + .tp_frame_size = FRAME_SIZE, + .tp_block_nr = BLOCK_NR, + .tp_frame_nr = FRAME_NR, + }; + CHECK(setsockopt(sock, SOL_PACKET, PACKET_TX_RING, (char *)&req, sizeof(req))); + + tx_ring_buffer = (void*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, sock, 0)); + + return sock; +} + +void can_tx_mmap(int sock, const struct can_frame *cf, int count) +{ + int i; + for (i = 0; i < count; i++) { + volatile struct tpacket2_hdr *hdr = tx_ring_buffer + i * FRAME_SIZE; + + struct can_frame *cf_mmap = (void*)hdr + TPACKET_HDRLEN - sizeof(struct sockaddr_ll); + *cf_mmap = cf[i]; + hdr->tp_len = sizeof(cf[i]); + hdr->tp_status = TP_STATUS_SEND_REQUEST; + } + + CHECK(send(sock, NULL, 0, 0)); +} + +int main(int argc, char *argv[]) +{ + const char *dev; + int sock, count; + struct can_frame cf[FRAME_NR]; + + dev = cansend_parse_args(argc, argv, &count, cf, FRAME_NR); + + sock = init_mmap_socket(dev); + + can_tx_mmap(sock, cf, count); + sleep(1); + return 0; +} diff --git a/cansend-sendmmsg.c b/cansend-sendmmsg.c new file mode 100644 index 0000000..c14f4cc --- /dev/null +++ b/cansend-sendmmsg.c @@ -0,0 +1,74 @@ +/**************************************************************/ +/* CAN system calls example */ +/* Author: Michal Sojka, Czech Technical University in Prague */ +/* License: Public domain */ +/**************************************************************/ + +#define _GNU_SOURCE + +#include "common.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#define MMSG_MAX 64 + +struct mmsghdr msgs[MMSG_MAX]; +struct iovec iovecs[MMSG_MAX]; +struct can_frame cf[MMSG_MAX]; + +int init_sendmmsg(const char *dev) +{ + int sock, i; + struct sockaddr_can addr; + struct ifreq ifr; + + sock = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW)); + + strncpy(ifr.ifr_name, dev, sizeof(ifr.ifr_name)); + if (-1 == ioctl(sock, SIOCGIFINDEX, &ifr)) { + perror(dev); + exit(1); + } + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + CHECK(bind(sock, (struct sockaddr *)&addr, sizeof(addr))); + + memset(msgs, 0, sizeof(msgs)); + memset(iovecs, 0, sizeof(iovecs)); + for (i = 0; i < MMSG_MAX; i++) { + iovecs[i].iov_base = &cf[i]; + iovecs[i].iov_len = sizeof(cf[i]); + msgs[i].msg_hdr.msg_iov = &iovecs[i]; + msgs[i].msg_hdr.msg_iovlen = 1; + } + + return sock; +} + +void can_sendmmsg(int sock, int count) +{ + CHECK(sendmmsg(sock, msgs, count, 0)); +} + +int main(int argc, char *argv[]) +{ + const char *dev; + int sock, count; + + dev = cansend_parse_args(argc, argv, &count, cf, MMSG_MAX); + + sock = init_sendmmsg(dev); + + can_sendmmsg(sock, count); + + return 0; +} diff --git a/cansend-write.c b/cansend-write.c new file mode 100644 index 0000000..87d31b1 --- /dev/null +++ b/cansend-write.c @@ -0,0 +1,63 @@ +/**************************************************************/ +/* CAN system calls example */ +/* Author: Michal Sojka, Czech Technical University in Prague */ +/* License: Public domain */ +/**************************************************************/ + +#include "common.h" + +#include +#include +#include +#include +#include +#include +#include + +int init_write_socket(const char *dev) +{ + int sock; + struct sockaddr_can addr; + struct ifreq ifr; + + sock = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW)); + + strncpy(ifr.ifr_name, dev, sizeof(ifr.ifr_name)); + if (-1 == ioctl(sock, SIOCGIFINDEX, &ifr)) { + perror(dev); + exit(1); + } + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + CHECK(bind(sock, (struct sockaddr *)&addr, sizeof(addr))); + + return sock; +} + +void can_write(int sock, struct can_frame *cf) +{ + int ret = write(sock, cf, sizeof(*cf)); + + if (ret != sizeof(*cf)) { + perror("write"); + exit(1); + } +} + +int main(int argc, char *argv[]) +{ + const char *dev; + int sock, i, count; + struct can_frame cf[100]; + + dev = cansend_parse_args(argc, argv, &count, cf, 100); + + sock = init_write_socket(dev); + + for (i = 0; i < count; i++) + can_write(sock, &cf[i]); + + return 0; +} diff --git a/common.h b/common.h index 8d7f3ab..dd26da2 100644 --- a/common.h +++ b/common.h @@ -11,6 +11,8 @@ #include #include +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + #define STRINGIFY(val) #val #define TOSTRING(val) STRINGIFY(val) @@ -42,5 +44,38 @@ void print_can_frame(const struct can_frame *cf) printf("\n"); } +void can_str2frame(const char *str, struct can_frame *cf) +{ + char *extra; + unsigned char *d = cf->data; + int ret = sscanf(str, + "%x#%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%ms", + &cf->can_id, + d+0, d+1, d+2, d+3, d+4, d+5, d+6, d+7, &extra); + if (ret < 1) { + fprintf(stderr, "Error parsing CAN frame '%s'\n", str); + exit(1); + } + if (ret > 9) { + fprintf(stderr, "Error: Extra input '%s'\n", extra); + exit(1); + } + cf->can_dlc = ret - 1; +} + +char *cansend_parse_args(int argc, char *argv[], int *count, struct can_frame *cf_array, size_t max_count) +{ + int i; + if (argc < 3) { + fprintf(stderr, "Usage: %s ...\n", argv[0]); + fprintf(stderr, " CAN frame is ID#DATA, e.g. 123#4567\n"); + exit(1); + } + + for (i = 0; i < MIN(argc - 2, max_count); i++) + can_str2frame(argv[i + 2], &cf_array[i]); + *count = argc - 2; + return argv[1]; +} #endif -- 2.39.2