candump-mmap.c

   1 /**************************************************************/
   2 /* CAN system calls example                                   */
   3 /* Author: Michal Sojka, Czech Technical University in Prague */
   4 /* License: Public domain                                     */
   5 /**************************************************************/
   6
   7 #include "common.h"
   8
   9 #include <linux/can.h>
  10 #include <net/if.h>
  11 #include <unistd.h>
  12 #include <fcntl.h>
  13 #include <stdbool.h>
  14 #include <string.h>
  15 #include <sys/socket.h>
  16 #include <linux/if_packet.h>
  17 #include <net/ethernet.h> /* the L2 protocols */
  18 #include <sys/ioctl.h>
  19 #include <arpa/inet.h>
  20 #include <sys/mman.h>
  21 #include <poll.h>
  22
  23 #define FRAME_SIZE 128
  24 #define BLOCK_SIZE 4096
  25 #define BLOCK_NR 2
  26 #define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE))
  27
  28 void *rx_ring_buffer;
  29 int hdrlen;
  30 int current;
  31
  32 int init_mmap_socket(const char *dev)
  33 {
  34         int sock;
  35         struct sockaddr_ll addr;
  36         struct ifreq ifr;
  37
  38         sock = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
  39
  40         int val = TPACKET_V2;
  41         CHECK(setsockopt(sock, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
  42         socklen_t len = sizeof(hdrlen);
  43         CHECK(getsockopt(sock, SOL_PACKET, PACKET_HDRLEN, &hdrlen, &len));
  44
  45         strncpy (ifr.ifr_name, dev, sizeof(ifr.ifr_name));
  46         CHECK(ioctl(sock, SIOCGIFINDEX, &ifr));
  47
  48         addr.sll_family = AF_PACKET;
  49         addr.sll_protocol = htons(ETH_P_ALL);
  50         addr.sll_ifindex =  ifr.ifr_ifindex;
  51
  52         CHECK(bind(sock, (struct sockaddr *)&addr, sizeof(struct sockaddr_ll)));
  53
  54         struct tpacket_req req = {
  55                 .tp_block_size = BLOCK_SIZE,
  56                 .tp_frame_size = FRAME_SIZE,
  57                 .tp_block_nr   = BLOCK_NR,
  58                 .tp_frame_nr   = FRAME_NR,
  59         };
  60         CHECK(setsockopt(sock, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
  61
  62         rx_ring_buffer = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, sock, 0));
  63
  64         current = 0;
  65         return sock;
  66 }
  67
  68 void can_rx_mmap(int sock)
  69 {
  70         volatile struct tpacket2_hdr *hdr = rx_ring_buffer + current*FRAME_SIZE;
  71
  72         while (hdr->tp_status == TP_STATUS_KERNEL) {
  73                 struct pollfd pfd = {.fd = sock, .revents = 0,
  74                                      .events = POLLIN|POLLRDNORM|POLLERR };
  75                 /* Wait for a message to arrive. If the task wake-up
  76                  * overhead is too high, poll() can be commented out
  77                  * and the application will busy-wait here. */
  78                 CHECK(poll(&pfd, 1, -1));
  79         }
  80
  81         struct can_frame *cf = (void*)hdr + hdr->tp_mac;
  82         printf("Frame #%d received\n", current);
  83         print_can_frame(cf);
  84         hdr->tp_status = 0;     /* Mark the frame as processed */
  85
  86         current = (current + 1) % FRAME_NR;
  87 }
  88
  89 int main(int argc, char *argv[])
  90 {
  91         const char *dev;
  92         int sock;
  93
  94         dev = candump_parse_args(argc, argv);
  95
  96         sock = init_mmap_socket(dev);
  97
  98         while (1)
  99                 can_rx_mmap(sock);
 100 }