The Tcpdump Group git mirrors - libpcap/blob - pcap-canusb-linux.c

   1 /*
   2  * Copyright (c) 2009 Felix Obenhuber
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  *
   9  * 1. Redistributions of source code must retain the above copyright
  10  * notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  * notice, this list of conditions and the following disclaimer in the
  13  * documentation and/or other materials provided with the distribution.
  14  * 3. The name of the author may not be used to endorse or promote
  15  * products derived from this software without specific prior written
  16  * permission.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  19  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  21  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  22  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  23  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  24  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  25  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  26  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  28  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  29  *
  30  * Sockettrace sniffing API implementation for Linux platform
  31  * By Felix Obenhuber <felix@obenhuber.de>
  32  *
  33  */
  34
  35 #ifdef HAVE_CONFIG_H
  36 #include "config.h"
  37 #endif
  38
  39 #include <libusb-1.0/libusb.h>
  40
  41 #include <stdlib.h>
  42 #include <unistd.h>
  43 #include <fcntl.h>
  44 #include <string.h>
  45
  46 #include "pcap-int.h"
  47 #include "pcap-canusb-linux.h"
  48
  49 #define CANUSB_IFACE "canusb"
  50
  51 #define CANUSB_VID 0x0403
  52 #define CANUSB_PID 0x8990
  53
  54 #define USE_THREAD 1
  55
  56 #if USE_THREAD == 0
  57 #include <signal.h>
  58 #endif
  59
  60
  61 /* forward declaration */
  62 static int canusb_activate(pcap_t *);
  63 static int canusb_read_linux(pcap_t *, int , pcap_handler , u_char *);
  64 static int canusb_inject_linux(pcap_t *, const void *, size_t);
  65 static int canusb_setfilter_linux(pcap_t *, struct bpf_program *);
  66 static int canusb_setdirection_linux(pcap_t *, pcap_direction_t);
  67 static int canusb_stats_linux(pcap_t *, struct pcap_stat *);
  68
  69 struct CAN_Msg
  70 {
  71     uint32_t timestamp;
  72     uint32_t id;
  73     uint32_t length;
  74     uint8_t data[8];
  75 };
  76
  77 struct canusb_t
  78 {
  79     libusb_context *ctx;
  80     libusb_device_handle *dev;
  81     char *serial;
  82     pthread_t worker;
  83     int rdpipe, wrpipe;
  84     volatile int* loop;
  85 };
  86
  87 static struct canusb_t canusb;
  88 static volatile int loop;
  89
  90 int canusb_findalldevs(pcap_if_t **alldevsp, char *err_str)
  91 {
  92     libusb_context *fdctx;
  93     libusb_device** devs;
  94     unsigned char sernum[65];
  95     unsigned char buf[96];
  96     int cnt, i;
  97
  98     if (libusb_init(&fdctx) != 0) {
  99         /*
 100          * XXX - if this doesn't just mean "no USB file system mounted",
 101          * perhaps we should report a real error rather than just
 102          * saying "no CANUSB devices".
 103          */
 104         return 0;
 105     }
 106
 107     cnt = libusb_get_device_list(fdctx,&devs);
 108
 109     for(i=0;i<cnt;i++)
 110     {
 111         int ret;
 112         // Check if this device is interesting.
 113         struct libusb_device_descriptor desc;
 114         libusb_get_device_descriptor(devs[i],&desc);
 115
 116         if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID))
 117             continue; //It is not, check next device
 118
 119         //It is!
 120         libusb_device_handle *dh = NULL;
 121
 122         if (ret = libusb_open(devs[i],&dh) == 0)
 123         {
 124             char dev_name[30];
 125             char dev_descr[50];
 126             int n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,sernum,64);
 127             sernum[n] = 0;
 128
 129             snprintf(dev_name, 30, CANUSB_IFACE"%s", sernum);
 130             snprintf(dev_descr, 50, "CanUSB [%s]", sernum);
 131
 132             libusb_close(dh);
 133
 134             if (pcap_add_if(alldevsp, dev_name, 0, dev_descr, err_str) < 0)
 135             {
 136                 libusb_free_device_list(devs,1);
 137                 return -1;
 138             }
 139         }
 140     }
 141
 142     libusb_free_device_list(devs,1);
 143     libusb_exit(fdctx);
 144     return 0;
 145 }
 146
 147 static libusb_device_handle* canusb_opendevice(struct libusb_context *ctx, char* devserial)
 148 {
 149     libusb_device_handle* dh;
 150     libusb_device** devs;
 151     unsigned char serial[65];
 152     int cnt,i,n;
 153
 154     cnt = libusb_get_device_list(ctx,&devs);
 155
 156     for(i=0;i<cnt;i++)
 157     {
 158         // Check if this device is interesting.
 159         struct libusb_device_descriptor desc;
 160         libusb_get_device_descriptor(devs[i],&desc);
 161
 162         if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID))
 163           continue;
 164
 165         //Found one!
 166         libusb_device_handle *dh = NULL;
 167
 168         if (libusb_open(devs[i],&dh) != 0) continue;
 169
 170         n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,serial,64);
 171         serial[n] = 0;
 172
 173         if ((devserial) && (strcmp(serial,devserial) != 0))
 174         {
 175             libusb_close(dh);
 176             continue;
 177         }
 178
 179         if ((libusb_kernel_driver_active(dh,0)) && (libusb_detach_kernel_driver(dh,0) != 0))
 180         {
 181             libusb_close(dh);
 182             continue;
 183         }
 184
 185         if (libusb_set_configuration(dh,1) != 0)
 186         {
 187             libusb_close(dh);
 188             continue;
 189         }
 190
 191         if (libusb_claim_interface(dh,0) != 0)
 192         {
 193             libusb_close(dh);
 194             continue;
 195         }
 196
 197         //Fount it!
 198         libusb_free_device_list(devs,1);
 199         return dh;
 200     }
 201
 202     libusb_free_device_list(devs,1);
 203     return NULL;
 204 }
 205
 206
 207 pcap_t *
 208 canusb_create(const char *device, char *ebuf, int *is_ours)
 209 {
 210     const char *cp;
 211     char *cpend;
 212     long devnum;
 213     pcap_t* p;
 214
 215     libusb_init(&canusb.ctx);
 216
 217     /* Does this look like a DAG device? */
 218     cp = strrchr(device, '/');
 219     if (cp == NULL)
 220         cp = device;
 221     /* Does it begin with "canusb"? */
 222     if (strncmp(cp, "canusb", 6) != 0) {
 223         /* Nope, doesn't begin with "canusb" */
 224         *is_ours = 0;
 225         return NULL;
 226     }
 227     /* Yes - is "canusb" followed by a number? */
 228     cp += 6;
 229     devnum = strtol(cp, &cpend, 10);
 230     if (cpend == cp || *cpend != '\0') {
 231         /* Not followed by a number. */
 232         *is_ours = 0;
 233         return NULL;
 234     }
 235     if (devnum < 0) {
 236         /* Followed by a non-valid number. */
 237         *is_ours = 0;
 238         return NULL;
 239     }
 240
 241     /* OK, it's probably ours. */
 242     *is_ours = 1;
 243
 244     p = pcap_create_common(device, ebuf);
 245     if (p == NULL)
 246         return (NULL);
 247
 248     memset(&canusb, 0x00, sizeof(canusb));
 249
 250     p->activate_op = canusb_activate;
 251
 252     return (p);
 253 }
 254
 255
 256 static void* canusb_capture_thread(struct canusb_t *canusb)
 257 {
 258     struct libusb_context *ctx;
 259     libusb_device_handle *dev;
 260     int i, n;
 261     struct
 262     {
 263       uint8_t rxsz, txsz;
 264     } status;
 265     char *serial;
 266
 267     libusb_init(&ctx);
 268
 269     serial = canusb->serial;
 270     dev = canusb_opendevice(ctx, serial);
 271
 272     fcntl(canusb->wrpipe, F_SETFL, O_NONBLOCK);
 273
 274     while(*canusb->loop)
 275     {
 276         int sz, ret;
 277         struct CAN_Msg msg;
 278
 279         libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100);
 280         //HACK!!!!! -> drop buffered data, read new one by reading twice.
 281         ret = libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100);
 282
 283         for(i = 0; i<status.rxsz; i++)
 284         {
 285             libusb_bulk_transfer(dev, 0x85, (unsigned char*)&msg, sizeof(msg), &sz, 100);
 286             n = write(canusb->wrpipe, &msg, sizeof(msg));
 287         }
 288
 289     }
 290
 291     libusb_close(dev);
 292     libusb_exit(ctx);
 293
 294     return NULL;
 295 }
 296
 297 static int canusb_startcapture(struct canusb_t* this)
 298 {
 299     int pipefd[2];
 300
 301     if (pipe(pipefd) == -1)
 302         return -1;
 303
 304     canusb.rdpipe = pipefd[0];
 305     canusb.wrpipe = pipefd[1];
 306     canusb.loop = &loop;
 307
 308     loop = 1;
 309     pthread_create(&this->worker, NULL, canusb_capture_thread, &canusb);
 310
 311     return canusb.rdpipe;
 312 }
 313
 314 static void canusb_clearbufs(struct canusb_t* this)
 315 {
 316     unsigned char cmd[16];
 317     int al;
 318
 319     cmd[0] = 1;  //Empty incoming buffer
 320     cmd[1] = 1;  //Empty outgoing buffer
 321     cmd[3] = 0;  //Not a write to serial number
 322     memset(&cmd[4],0,16-4);
 323
 324     libusb_interrupt_transfer(this->dev, 0x1,cmd,16,&al,100);
 325 }
 326
 327
 328 static void canusb_close(pcap_t* handle)
 329 {
 330     loop = 0;
 331     pthread_join(canusb.worker, NULL);
 332
 333     if (canusb.dev)
 334     {
 335         libusb_close(canusb.dev);
 336         canusb.dev = NULL;
 337     }
 338 }
 339
 340
 341
 342 static int canusb_activate(pcap_t* handle)
 343 {
 344     char *serial;
 345
 346     handle->read_op = canusb_read_linux;
 347
 348     handle->inject_op = canusb_inject_linux;
 349     handle->setfilter_op = canusb_setfilter_linux;
 350     handle->setdirection_op = canusb_setdirection_linux;
 351     handle->getnonblock_op = pcap_getnonblock_fd;
 352     handle->setnonblock_op = pcap_setnonblock_fd;
 353     handle->stats_op = canusb_stats_linux;
 354     handle->cleanup_op = canusb_close;
 355
 356     /* Initialize some components of the pcap structure. */
 357     handle->bufsize = 32;
 358     handle->offset = 8;
 359     handle->linktype = DLT_CAN_SOCKETCAN;
 360     handle->set_datalink_op = NULL;
 361
 362     serial = handle->opt.source + strlen(CANUSB_IFACE);
 363     canusb.serial = strdup(serial);
 364
 365     canusb.dev = canusb_opendevice(canusb.ctx,serial);
 366     if (!canusb.dev)
 367     {
 368         snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "Can't open USB Device:");
 369         return PCAP_ERROR;
 370     }
 371
 372     canusb_clearbufs(&canusb);
 373
 374     handle->fd = canusb_startcapture(&canusb);
 375     handle->selectable_fd = handle->fd;
 376
 377     return 0;
 378 }
 379
 380
 381
 382
 383 static int
 384 canusb_read_linux(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user)
 385 {
 386     static struct timeval firstpacket = { -1, -1};
 387
 388     int msgsent = 0;
 389     int i = 0;
 390     struct CAN_Msg msg;
 391     struct pcap_pkthdr pkth;
 392
 393     while(i < max_packets)
 394     {
 395         int n;
 396         usleep(10 * 1000);
 397         n = read(handle->fd, &msg, sizeof(msg));
 398         if (n <= 0)
 399             break;
 400         pkth.caplen = pkth.len = n;
 401         pkth.caplen -= 4;
 402         pkth.caplen -= 8 - msg.length;
 403
 404         if ((firstpacket.tv_sec == -1) && (firstpacket.tv_usec == -1))
 405             gettimeofday(&firstpacket, NULL);
 406
 407         pkth.ts.tv_usec = firstpacket.tv_usec + (msg.timestamp % 100) * 10000;
 408         pkth.ts.tv_sec = firstpacket.tv_usec + (msg.timestamp / 100);
 409         if (pkth.ts.tv_usec > 1000000)
 410         {
 411             pkth.ts.tv_usec -= 1000000;
 412             pkth.ts.tv_sec++;
 413         }
 414
 415         callback(user, &pkth, (void*)&msg.id);
 416         i++;
 417     }
 418
 419     return i;
 420 }
 421
 422
 423 static int
 424 canusb_inject_linux(pcap_t *handle, const void *buf, size_t size)
 425 {
 426     /* not yet implemented */
 427     snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "inject not supported on canusb devices");
 428     return (-1);
 429 }
 430
 431
 432 static int
 433 canusb_stats_linux(pcap_t *handle, struct pcap_stat *stats)
 434 {
 435     /* not yet implemented */
 436     stats->ps_recv = 0;     /* number of packets received */
 437     stats->ps_drop = 0;     /* number of packets dropped */
 438     stats->ps_ifdrop = 0;   /* drops by interface -- only supported on some platforms */
 439     return 0;
 440 }
 441
 442
 443 static int
 444 canusb_setfilter_linux(pcap_t *p, struct bpf_program *fp)
 445 {
 446     /* not yet implemented */
 447     return 0;
 448 }
 449
 450
 451 static int
 452 canusb_setdirection_linux(pcap_t *p, pcap_direction_t d)
 453 {
 454     /* no support for PCAP_D_OUT */
 455     if (d == PCAP_D_OUT)
 456     {
 457         snprintf(p->errbuf, sizeof(p->errbuf),
 458             "Setting direction to PCAP_D_OUT is not supported on this interface");
 459         return -1;
 460     }
 461
 462     p->direction = d;
 463
 464     return 0;
 465 }
 466
 467
 468 /* eof */