release/src/router/libpcap/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 "pcap-int.h"
  42 #include <stdlib.h>
  43 #include <unistd.h>
  44 #include <fcntl.h>
  45 #include <string.h>
  46
  47
  48 #define CANUSB_IFACE "canusb"
  49
  50 #define CANUSB_VID 0x0403
  51 #define CANUSB_PID 0x8990
  52
  53 #define USE_THREAD 1
  54
  55 #if USE_THREAD == 0
  56 #include <signal.h>
  57 #endif
  58
  59
  60 /* forward declaration */
  61 static int canusb_activate(pcap_t *);
  62 static int canusb_read_linux(pcap_t *, int , pcap_handler , u_char *);
  63 static int canusb_inject_linux(pcap_t *, const void *, size_t);
  64 static int canusb_setfilter_linux(pcap_t *, struct bpf_program *);
  65 static int canusb_setdirection_linux(pcap_t *, pcap_direction_t);
  66 static int canusb_stats_linux(pcap_t *, struct pcap_stat *);
  67
  68 struct CAN_Msg
  69 {
  70     uint32_t timestamp;
  71     uint32_t id;
  72     uint32_t length;
  73     uint8_t data[8];
  74 };
  75
  76 struct canusb_t
  77 {
  78   libusb_context *ctx;
  79   libusb_device_handle *dev;
  80   char* src;
  81   pthread_t worker;
  82   int rdpipe, wrpipe;
  83   volatile int* loop;
  84 };
  85
  86 static struct canusb_t canusb;
  87 static volatile int loop;
  88
  89
  90
  91 int canusb_platform_finddevs(pcap_if_t **alldevsp, char *err_str)
  92 {
  93     libusb_context *fdctx;
  94     libusb_device** devs;
  95     unsigned char sernum[65];
  96     unsigned char buf[96];
  97     int cnt, i;
  98
  99     libusb_init(&fdctx);
 100
 101     cnt = libusb_get_device_list(fdctx,&devs);
 102
 103     for(i=0;i<cnt;i++)
 104     {
 105         int ret;
 106         // Check if this device is interesting.
 107         struct libusb_device_descriptor desc;
 108         libusb_get_device_descriptor(devs[i],&desc);
 109
 110         if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID))
 111           continue; //It is not, check next device
 112
 113         //It is!
 114         libusb_device_handle *dh = NULL;
 115
 116         if (ret = libusb_open(devs[i],&dh) == 0)
 117         {
 118                 char dev_name[30];
 119                   char dev_descr[50];
 120             int n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,sernum,64);
 121             sernum[n] = 0;
 122
 123                 snprintf(dev_name, 30, CANUSB_IFACE"%s", sernum);
 124                 snprintf(dev_descr, 50, "CanUSB [%s]", sernum);
 125
 126             libusb_close(dh);
 127
 128             if (pcap_add_if(alldevsp, dev_name, 0, dev_descr, err_str) < 0)
 129             {
 130               libusb_free_device_list(devs,1);
 131               return -1;
 132             }
 133         }
 134     }
 135
 136     libusb_free_device_list(devs,1);
 137     libusb_exit(fdctx);
 138     return 0;
 139 }
 140
 141 static libusb_device_handle* canusb_opendevice(struct libusb_context *ctx, char* devserial)
 142 {
 143     libusb_device_handle* dh;
 144     libusb_device** devs;
 145     unsigned char serial[65];
 146     int cnt,i,n;
 147
 148     cnt = libusb_get_device_list(ctx,&devs);
 149
 150     for(i=0;i<cnt;i++)
 151     {
 152         // Check if this device is interesting.
 153         struct libusb_device_descriptor desc;
 154         libusb_get_device_descriptor(devs[i],&desc);
 155
 156         if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID))
 157           continue;
 158
 159         //Found one!
 160         libusb_device_handle *dh = NULL;
 161
 162         if (libusb_open(devs[i],&dh) != 0) continue;
 163
 164         n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,serial,64);
 165         serial[n] = 0;
 166
 167         if ((devserial) && (strcmp(serial,devserial) != 0))
 168         {
 169             libusb_close(dh);
 170             continue;
 171         }
 172
 173         if ((libusb_kernel_driver_active(dh,0)) && (libusb_detach_kernel_driver(dh,0) != 0))
 174         {
 175             libusb_close(dh);
 176             continue;
 177         }
 178
 179         if (libusb_set_configuration(dh,1) != 0)
 180         {
 181             libusb_close(dh);
 182             continue;
 183         }
 184
 185         if (libusb_claim_interface(dh,0) != 0)
 186         {
 187             libusb_close(dh);
 188             continue;
 189         }
 190
 191         //Fount it!
 192         libusb_free_device_list(devs,1);
 193         return dh;
 194     }
 195
 196     libusb_free_device_list(devs,1);
 197     return NULL;
 198 }
 199
 200
 201 pcap_t *
 202 canusb_create(const char *device, char *ebuf)
 203 {
 204   pcap_t* p;
 205
 206   libusb_init(&canusb.ctx);
 207
 208         p = pcap_create_common(device, ebuf);
 209         if (p == NULL)
 210                 return (NULL);
 211
 212   memset(&canusb, 0x00, sizeof(canusb));
 213
 214
 215         p->activate_op = canusb_activate;
 216
 217         canusb.src = strdup(p->opt.source);
 218         return (p);
 219 }
 220
 221
 222 static void* canusb_capture_thread(struct canusb_t *canusb)
 223 {
 224   struct libusb_context *ctx;
 225   libusb_device_handle *dev;
 226
 227   int i, n;
 228   struct
 229   {
 230     uint8_t rxsz, txsz;
 231   } status;
 232
 233   libusb_init(&ctx);
 234
 235   char *serial = canusb->src + strlen(CANUSB_IFACE);
 236   dev = canusb_opendevice(ctx, serial);
 237
 238   fcntl(canusb->wrpipe, F_SETFL, O_NONBLOCK);
 239
 240   while(*canusb->loop)
 241   {
 242     int sz, ret;
 243     struct CAN_Msg msg;
 244
 245     libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100);
 246     //HACK!!!!! -> drop buffered data, read new one by reading twice.
 247     ret = libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100);
 248
 249     for(i = 0; i<status.rxsz; i++)
 250     {
 251       libusb_bulk_transfer(dev, 0x85, (unsigned char*)&msg, sizeof(msg), &sz, 100);
 252       n = write(canusb->wrpipe, &msg, sizeof(msg));
 253     }
 254
 255   }
 256
 257   libusb_close(dev);
 258   libusb_exit(ctx);
 259
 260   return NULL;
 261 }
 262
 263 static int canusb_startcapture(struct canusb_t* this)
 264 {
 265   int pipefd[2];
 266
 267   if (pipe(pipefd) == -1) return -1;
 268
 269   canusb.rdpipe = pipefd[0];
 270   canusb.wrpipe = pipefd[1];
 271   canusb.loop = &loop;
 272
 273   loop = 1;
 274   pthread_create(&this->worker, NULL, canusb_capture_thread, &canusb);
 275
 276   return canusb.rdpipe;
 277 }
 278
 279 static void canusb_clearbufs(struct canusb_t* this)
 280 {
 281         unsigned char cmd[16];
 282         int al;
 283
 284         cmd[0] = 1;  //Empty incoming buffer
 285         cmd[1] = 1;  //Empty outgoing buffer
 286         cmd[3] = 0;  //Not a write to serial number
 287         memset(&cmd[4],0,16-4);
 288
 289         libusb_interrupt_transfer(this->dev, 0x1,cmd,16,&al,100);
 290 }
 291
 292
 293 static void canusb_close(pcap_t* handle)
 294 {
 295   loop = 0;
 296   pthread_join(canusb.worker, NULL);
 297
 298   if (canusb.dev)
 299   {
 300     libusb_close(canusb.dev);
 301     canusb.dev = NULL;
 302   }
 303 }
 304
 305
 306
 307 static int canusb_activate(pcap_t* handle)
 308 {
 309         handle->read_op = canusb_read_linux;
 310
 311         handle->inject_op = canusb_inject_linux;
 312         handle->setfilter_op = canusb_setfilter_linux;
 313         handle->setdirection_op = canusb_setdirection_linux;
 314         handle->getnonblock_op = pcap_getnonblock_fd;
 315         handle->setnonblock_op = pcap_setnonblock_fd;
 316         handle->stats_op = canusb_stats_linux;
 317         handle->cleanup_op = canusb_close;
 318
 319         /* Initialize some components of the pcap structure. */
 320         handle->bufsize = 32;
 321         handle->offset = 8;
 322         handle->linktype = DLT_CAN_SOCKETCAN;
 323         handle->set_datalink_op = NULL;
 324
 325   char* serial = handle->opt.source + strlen("canusb");
 326
 327   canusb.dev = canusb_opendevice(canusb.ctx,serial);
 328   if (!canusb.dev)
 329   {
 330         snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "Can't open USB Device:");
 331         return PCAP_ERROR;
 332         }
 333
 334   canusb_clearbufs(&canusb);
 335
 336   handle->fd = canusb_startcapture(&canusb);
 337         handle->selectable_fd = handle->fd;
 338
 339         return 0;
 340 }
 341
 342
 343
 344
 345 static int
 346 canusb_read_linux(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user)
 347 {
 348   static struct timeval firstpacket = { -1, -1};
 349
 350   int msgsent = 0;
 351   int i = 0;
 352   struct CAN_Msg msg;
 353         struct pcap_pkthdr pkth;
 354
 355   while(i < max_packets)
 356   {
 357     usleep(10 * 1000);
 358     int n = read(handle->fd, &msg, sizeof(msg));
 359     if (n <= 0) break;
 360     pkth.caplen = pkth.len = n;
 361     pkth.caplen -= 4;
 362     pkth.caplen -= 8 - msg.length;
 363
 364     if ((firstpacket.tv_sec == -1) && (firstpacket.tv_usec == -1))
 365       gettimeofday(&firstpacket, NULL);
 366
 367     pkth.ts.tv_usec = firstpacket.tv_usec + (msg.timestamp % 100) * 10000;
 368     pkth.ts.tv_sec = firstpacket.tv_usec + (msg.timestamp / 100);
 369     if (pkth.ts.tv_usec > 1000000)
 370     {
 371       pkth.ts.tv_usec -= 1000000;
 372       pkth.ts.tv_sec++;
 373     }
 374
 375     callback(user, &pkth, (void*)&msg.id);
 376     i++;
 377   }
 378
 379   return i;
 380 }
 381
 382
 383 static int
 384 canusb_inject_linux(pcap_t *handle, const void *buf, size_t size)
 385 {
 386         /* not yet implemented */
 387         snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "inject not supported on canusb devices");
 388         return (-1);
 389 }
 390
 391
 392 static int
 393 canusb_stats_linux(pcap_t *handle, struct pcap_stat *stats)
 394 {
 395         /* not yet implemented */
 396         stats->ps_recv = 0;                      /* number of packets received */
 397         stats->ps_drop = 0;                      /* number of packets dropped */
 398         stats->ps_ifdrop = 0;            /* drops by interface -- only supported on some platforms */
 399         return 0;
 400 }
 401
 402
 403 static int
 404 canusb_setfilter_linux(pcap_t *p, struct bpf_program *fp)
 405 {
 406         /* not yet implemented */
 407         return 0;
 408 }
 409
 410
 411 static int
 412 canusb_setdirection_linux(pcap_t *p, pcap_direction_t d)
 413 {
 414         /* no support for PCAP_D_OUT */
 415         if (d == PCAP_D_OUT)
 416         {
 417                 snprintf(p->errbuf, sizeof(p->errbuf),
 418                         "Setting direction to PCAP_D_OUT is not supported on this interface");
 419                 return -1;
 420         }
 421
 422         p->direction = d;
 423
 424         return 0;
 425 }
 426
 427
 428 /* eof */