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 current_frame;
  30
  31 int init_mmap_socket(const char *dev)
  32 {
  33         int sock;
  34         struct sockaddr_ll addr;
  35         struct ifreq ifr;
  36
  37         sock = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
  38
  39         int val = TPACKET_V2;
  40         CHECK(setsockopt(sock, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
  41
  42         strncpy (ifr.ifr_name, dev, sizeof(ifr.ifr_name));
  43         CHECK(ioctl(sock, SIOCGIFINDEX, &ifr));
  44
  45         addr.sll_family = AF_PACKET;
  46         addr.sll_protocol = htons(ETH_P_ALL);
  47         addr.sll_ifindex =  ifr.ifr_ifindex;
  48
  49         CHECK(bind(sock, (struct sockaddr *)&addr, sizeof(struct sockaddr_ll)));
  50
  51         struct tpacket_req req = {
  52                 .tp_block_size = BLOCK_SIZE,
  53                 .tp_frame_size = FRAME_SIZE,
  54                 .tp_block_nr   = BLOCK_NR,
  55                 .tp_frame_nr   = FRAME_NR,
  56         };
  57         CHECK(setsockopt(sock, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
  58
  59         rx_ring_buffer = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, sock, 0));
  60
  61         current_frame = 0;
  62         return sock;
  63 }
  64
  65 void can_rx_mmap(int sock)
  66 {
  67         volatile struct tpacket2_hdr *hdr = rx_ring_buffer + current_frame*FRAME_SIZE;
  68
  69         /* Wait for a message to arrive. If the task wake-up overhead
  70          * is too high, poll() can be commented out and the
  71          * application will busy-wait here. */
  72         while (hdr->tp_status == TP_STATUS_KERNEL) {
  73                 struct pollfd pfd = {.fd = sock, .revents = 0,
  74                                      .events = POLLIN|POLLRDNORM|POLLERR };
  75                 CHECK(poll(&pfd, 1, -1));
  76         }
  77
  78         struct can_frame *cf = (void*)hdr + hdr->tp_mac;
  79         print_can_frame(cf);
  80         hdr->tp_status = 0;     /* Mark the frame as processed */
  81
  82         current_frame = (current_frame + 1) % FRAME_NR;
  83 }
  84
  85 int main(int argc, char *argv[])
  86 {
  87         const char *dev;
  88         int sock;
  89
  90         dev = candump_parse_args(argc, argv);
  91
  92         sock = init_mmap_socket(dev);
  93
  94         while (1)
  95                 can_rx_mmap(sock);
  96 }