pcap-canusb-linux.c revision 241199
1241199Sdelphij/* 2241199Sdelphij * Copyright (c) 2009 Felix Obenhuber 3241199Sdelphij * All rights reserved. 4241199Sdelphij * 5241199Sdelphij * Redistribution and use in source and binary forms, with or without 6241199Sdelphij * modification, are permitted provided that the following conditions 7241199Sdelphij * are met: 8241199Sdelphij * 9241199Sdelphij * 1. Redistributions of source code must retain the above copyright 10241199Sdelphij * notice, this list of conditions and the following disclaimer. 11241199Sdelphij * 2. Redistributions in binary form must reproduce the above copyright 12241199Sdelphij * notice, this list of conditions and the following disclaimer in the 13241199Sdelphij * documentation and/or other materials provided with the distribution. 14241199Sdelphij * 3. The name of the author may not be used to endorse or promote 15241199Sdelphij * products derived from this software without specific prior written 16241199Sdelphij * permission. 17241199Sdelphij * 18241199Sdelphij * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19241199Sdelphij * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20241199Sdelphij * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 21241199Sdelphij * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22241199Sdelphij * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23241199Sdelphij * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24241199Sdelphij * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25241199Sdelphij * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 26241199Sdelphij * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27241199Sdelphij * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28241199Sdelphij * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29241199Sdelphij * 30241199Sdelphij * Sockettrace sniffing API implementation for Linux platform 31241199Sdelphij * By Felix Obenhuber <felix@obenhuber.de> 32241199Sdelphij * 33241199Sdelphij */ 34241199Sdelphij 35241199Sdelphij#ifdef HAVE_CONFIG_H 36241199Sdelphij#include "config.h" 37241199Sdelphij#endif 38241199Sdelphij 39241199Sdelphij#include <libusb-1.0/libusb.h> 40241199Sdelphij 41241199Sdelphij#include "pcap-int.h" 42241199Sdelphij#include <stdlib.h> 43241199Sdelphij#include <unistd.h> 44241199Sdelphij#include <fcntl.h> 45241199Sdelphij#include <string.h> 46241199Sdelphij 47241199Sdelphij 48241199Sdelphij#define CANUSB_IFACE "canusb" 49241199Sdelphij 50241199Sdelphij#define CANUSB_VID 0x0403 51241199Sdelphij#define CANUSB_PID 0x8990 52241199Sdelphij 53241199Sdelphij#define USE_THREAD 1 54241199Sdelphij 55241199Sdelphij#if USE_THREAD == 0 56241199Sdelphij#include <signal.h> 57241199Sdelphij#endif 58241199Sdelphij 59241199Sdelphij 60241199Sdelphij/* forward declaration */ 61241199Sdelphijstatic int canusb_activate(pcap_t *); 62241199Sdelphijstatic int canusb_read_linux(pcap_t *, int , pcap_handler , u_char *); 63241199Sdelphijstatic int canusb_inject_linux(pcap_t *, const void *, size_t); 64241199Sdelphijstatic int canusb_setfilter_linux(pcap_t *, struct bpf_program *); 65241199Sdelphijstatic int canusb_setdirection_linux(pcap_t *, pcap_direction_t); 66241199Sdelphijstatic int canusb_stats_linux(pcap_t *, struct pcap_stat *); 67241199Sdelphij 68241199Sdelphijstruct CAN_Msg 69241199Sdelphij{ 70241199Sdelphij uint32_t timestamp; 71241199Sdelphij uint32_t id; 72241199Sdelphij uint32_t length; 73241199Sdelphij uint8_t data[8]; 74241199Sdelphij}; 75241199Sdelphij 76241199Sdelphijstruct canusb_t 77241199Sdelphij{ 78241199Sdelphij libusb_context *ctx; 79241199Sdelphij libusb_device_handle *dev; 80241199Sdelphij char* src; 81241199Sdelphij pthread_t worker; 82241199Sdelphij int rdpipe, wrpipe; 83241199Sdelphij volatile int* loop; 84241199Sdelphij}; 85241199Sdelphij 86241199Sdelphijstatic struct canusb_t canusb; 87241199Sdelphijstatic volatile int loop; 88241199Sdelphij 89241199Sdelphij 90241199Sdelphij 91241199Sdelphijint canusb_platform_finddevs(pcap_if_t **alldevsp, char *err_str) 92241199Sdelphij{ 93241199Sdelphij libusb_context *fdctx; 94241199Sdelphij libusb_device** devs; 95241199Sdelphij unsigned char sernum[65]; 96241199Sdelphij unsigned char buf[96]; 97241199Sdelphij int cnt, i; 98241199Sdelphij 99241199Sdelphij libusb_init(&fdctx); 100241199Sdelphij 101241199Sdelphij cnt = libusb_get_device_list(fdctx,&devs); 102241199Sdelphij 103241199Sdelphij for(i=0;i<cnt;i++) 104241199Sdelphij { 105241199Sdelphij int ret; 106241199Sdelphij // Check if this device is interesting. 107241199Sdelphij struct libusb_device_descriptor desc; 108241199Sdelphij libusb_get_device_descriptor(devs[i],&desc); 109241199Sdelphij 110241199Sdelphij if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID)) 111241199Sdelphij continue; //It is not, check next device 112241199Sdelphij 113241199Sdelphij //It is! 114241199Sdelphij libusb_device_handle *dh = NULL; 115241199Sdelphij 116241199Sdelphij if (ret = libusb_open(devs[i],&dh) == 0) 117241199Sdelphij { 118241199Sdelphij char dev_name[30]; 119241199Sdelphij char dev_descr[50]; 120241199Sdelphij int n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,sernum,64); 121241199Sdelphij sernum[n] = 0; 122241199Sdelphij 123241199Sdelphij snprintf(dev_name, 30, CANUSB_IFACE"%s", sernum); 124241199Sdelphij snprintf(dev_descr, 50, "CanUSB [%s]", sernum); 125241199Sdelphij 126241199Sdelphij libusb_close(dh); 127241199Sdelphij 128241199Sdelphij if (pcap_add_if(alldevsp, dev_name, 0, dev_descr, err_str) < 0) 129241199Sdelphij { 130241199Sdelphij libusb_free_device_list(devs,1); 131241199Sdelphij return -1; 132241199Sdelphij } 133241199Sdelphij } 134241199Sdelphij } 135241199Sdelphij 136241199Sdelphij libusb_free_device_list(devs,1); 137241199Sdelphij libusb_exit(fdctx); 138241199Sdelphij return 0; 139241199Sdelphij} 140241199Sdelphij 141241199Sdelphijstatic libusb_device_handle* canusb_opendevice(struct libusb_context *ctx, char* devserial) 142241199Sdelphij{ 143241199Sdelphij libusb_device_handle* dh; 144241199Sdelphij libusb_device** devs; 145241199Sdelphij unsigned char serial[65]; 146241199Sdelphij int cnt,i,n; 147241199Sdelphij 148241199Sdelphij cnt = libusb_get_device_list(ctx,&devs); 149241199Sdelphij 150241199Sdelphij for(i=0;i<cnt;i++) 151241199Sdelphij { 152241199Sdelphij // Check if this device is interesting. 153241199Sdelphij struct libusb_device_descriptor desc; 154241199Sdelphij libusb_get_device_descriptor(devs[i],&desc); 155241199Sdelphij 156241199Sdelphij if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID)) 157241199Sdelphij continue; 158241199Sdelphij 159241199Sdelphij //Found one! 160241199Sdelphij libusb_device_handle *dh = NULL; 161241199Sdelphij 162241199Sdelphij if (libusb_open(devs[i],&dh) != 0) continue; 163241199Sdelphij 164241199Sdelphij n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,serial,64); 165241199Sdelphij serial[n] = 0; 166241199Sdelphij 167241199Sdelphij if ((devserial) && (strcmp(serial,devserial) != 0)) 168241199Sdelphij { 169241199Sdelphij libusb_close(dh); 170241199Sdelphij continue; 171241199Sdelphij } 172241199Sdelphij 173241199Sdelphij if ((libusb_kernel_driver_active(dh,0)) && (libusb_detach_kernel_driver(dh,0) != 0)) 174241199Sdelphij { 175241199Sdelphij libusb_close(dh); 176241199Sdelphij continue; 177241199Sdelphij } 178241199Sdelphij 179241199Sdelphij if (libusb_set_configuration(dh,1) != 0) 180241199Sdelphij { 181241199Sdelphij libusb_close(dh); 182241199Sdelphij continue; 183241199Sdelphij } 184241199Sdelphij 185241199Sdelphij if (libusb_claim_interface(dh,0) != 0) 186241199Sdelphij { 187241199Sdelphij libusb_close(dh); 188241199Sdelphij continue; 189241199Sdelphij } 190241199Sdelphij 191241199Sdelphij //Fount it! 192241199Sdelphij libusb_free_device_list(devs,1); 193241199Sdelphij return dh; 194241199Sdelphij } 195241199Sdelphij 196241199Sdelphij libusb_free_device_list(devs,1); 197241199Sdelphij return NULL; 198241199Sdelphij} 199241199Sdelphij 200241199Sdelphij 201241199Sdelphijpcap_t * 202241199Sdelphijcanusb_create(const char *device, char *ebuf) 203241199Sdelphij{ 204241199Sdelphij pcap_t* p; 205241199Sdelphij 206241199Sdelphij libusb_init(&canusb.ctx); 207241199Sdelphij 208241199Sdelphij p = pcap_create_common(device, ebuf); 209241199Sdelphij if (p == NULL) 210241199Sdelphij return (NULL); 211241199Sdelphij 212241199Sdelphij memset(&canusb, 0x00, sizeof(canusb)); 213241199Sdelphij 214241199Sdelphij 215241199Sdelphij p->activate_op = canusb_activate; 216241199Sdelphij 217241199Sdelphij canusb.src = strdup(p->opt.source); 218241199Sdelphij return (p); 219241199Sdelphij} 220241199Sdelphij 221241199Sdelphij 222241199Sdelphijstatic void* canusb_capture_thread(struct canusb_t *canusb) 223241199Sdelphij{ 224241199Sdelphij struct libusb_context *ctx; 225241199Sdelphij libusb_device_handle *dev; 226241199Sdelphij 227241199Sdelphij int i, n; 228241199Sdelphij struct 229241199Sdelphij { 230241199Sdelphij uint8_t rxsz, txsz; 231241199Sdelphij } status; 232241199Sdelphij 233241199Sdelphij libusb_init(&ctx); 234241199Sdelphij 235241199Sdelphij char *serial = canusb->src + strlen(CANUSB_IFACE); 236241199Sdelphij dev = canusb_opendevice(ctx, serial); 237241199Sdelphij 238241199Sdelphij fcntl(canusb->wrpipe, F_SETFL, O_NONBLOCK); 239241199Sdelphij 240241199Sdelphij while(*canusb->loop) 241241199Sdelphij { 242241199Sdelphij int sz, ret; 243241199Sdelphij struct CAN_Msg msg; 244241199Sdelphij 245241199Sdelphij libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100); 246241199Sdelphij //HACK!!!!! -> drop buffered data, read new one by reading twice. 247241199Sdelphij ret = libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100); 248241199Sdelphij 249241199Sdelphij for(i = 0; i<status.rxsz; i++) 250241199Sdelphij { 251241199Sdelphij libusb_bulk_transfer(dev, 0x85, (unsigned char*)&msg, sizeof(msg), &sz, 100); 252241199Sdelphij n = write(canusb->wrpipe, &msg, sizeof(msg)); 253241199Sdelphij } 254241199Sdelphij 255241199Sdelphij } 256241199Sdelphij 257241199Sdelphij libusb_close(dev); 258241199Sdelphij libusb_exit(ctx); 259241199Sdelphij 260241199Sdelphij return NULL; 261241199Sdelphij} 262241199Sdelphij 263241199Sdelphijstatic int canusb_startcapture(struct canusb_t* this) 264241199Sdelphij{ 265241199Sdelphij int pipefd[2]; 266241199Sdelphij 267241199Sdelphij if (pipe(pipefd) == -1) return -1; 268241199Sdelphij 269241199Sdelphij canusb.rdpipe = pipefd[0]; 270241199Sdelphij canusb.wrpipe = pipefd[1]; 271241199Sdelphij canusb.loop = &loop; 272241199Sdelphij 273241199Sdelphij loop = 1; 274241199Sdelphij pthread_create(&this->worker, NULL, canusb_capture_thread, &canusb); 275241199Sdelphij 276241199Sdelphij return canusb.rdpipe; 277241199Sdelphij} 278241199Sdelphij 279241199Sdelphijstatic void canusb_clearbufs(struct canusb_t* this) 280241199Sdelphij{ 281241199Sdelphij unsigned char cmd[16]; 282241199Sdelphij int al; 283241199Sdelphij 284241199Sdelphij cmd[0] = 1; //Empty incoming buffer 285241199Sdelphij cmd[1] = 1; //Empty outgoing buffer 286241199Sdelphij cmd[3] = 0; //Not a write to serial number 287241199Sdelphij memset(&cmd[4],0,16-4); 288241199Sdelphij 289241199Sdelphij libusb_interrupt_transfer(this->dev, 0x1,cmd,16,&al,100); 290241199Sdelphij} 291241199Sdelphij 292241199Sdelphij 293241199Sdelphijstatic void canusb_close(pcap_t* handle) 294241199Sdelphij{ 295241199Sdelphij loop = 0; 296241199Sdelphij pthread_join(canusb.worker, NULL); 297241199Sdelphij 298241199Sdelphij if (canusb.dev) 299241199Sdelphij { 300241199Sdelphij libusb_close(canusb.dev); 301241199Sdelphij canusb.dev = NULL; 302241199Sdelphij } 303241199Sdelphij} 304241199Sdelphij 305241199Sdelphij 306241199Sdelphij 307241199Sdelphijstatic int canusb_activate(pcap_t* handle) 308241199Sdelphij{ 309241199Sdelphij handle->read_op = canusb_read_linux; 310241199Sdelphij 311241199Sdelphij handle->inject_op = canusb_inject_linux; 312241199Sdelphij handle->setfilter_op = canusb_setfilter_linux; 313241199Sdelphij handle->setdirection_op = canusb_setdirection_linux; 314241199Sdelphij handle->getnonblock_op = pcap_getnonblock_fd; 315241199Sdelphij handle->setnonblock_op = pcap_setnonblock_fd; 316241199Sdelphij handle->stats_op = canusb_stats_linux; 317241199Sdelphij handle->cleanup_op = canusb_close; 318241199Sdelphij 319241199Sdelphij /* Initialize some components of the pcap structure. */ 320241199Sdelphij handle->bufsize = 32; 321241199Sdelphij handle->offset = 8; 322241199Sdelphij handle->linktype = DLT_CAN_SOCKETCAN; 323241199Sdelphij handle->set_datalink_op = NULL; 324241199Sdelphij 325241199Sdelphij char* serial = handle->opt.source + strlen("canusb"); 326241199Sdelphij 327241199Sdelphij canusb.dev = canusb_opendevice(canusb.ctx,serial); 328241199Sdelphij if (!canusb.dev) 329241199Sdelphij { 330241199Sdelphij snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "Can't open USB Device:"); 331241199Sdelphij return PCAP_ERROR; 332241199Sdelphij } 333241199Sdelphij 334241199Sdelphij canusb_clearbufs(&canusb); 335241199Sdelphij 336241199Sdelphij handle->fd = canusb_startcapture(&canusb); 337241199Sdelphij handle->selectable_fd = handle->fd; 338241199Sdelphij 339241199Sdelphij return 0; 340241199Sdelphij} 341241199Sdelphij 342241199Sdelphij 343241199Sdelphij 344241199Sdelphij 345241199Sdelphijstatic int 346241199Sdelphijcanusb_read_linux(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user) 347241199Sdelphij{ 348241199Sdelphij static struct timeval firstpacket = { -1, -1}; 349241199Sdelphij 350241199Sdelphij int msgsent = 0; 351241199Sdelphij int i = 0; 352241199Sdelphij struct CAN_Msg msg; 353241199Sdelphij struct pcap_pkthdr pkth; 354241199Sdelphij 355241199Sdelphij while(i < max_packets) 356241199Sdelphij { 357241199Sdelphij usleep(10 * 1000); 358241199Sdelphij int n = read(handle->fd, &msg, sizeof(msg)); 359241199Sdelphij if (n <= 0) break; 360241199Sdelphij pkth.caplen = pkth.len = n; 361241199Sdelphij pkth.caplen -= 4; 362241199Sdelphij pkth.caplen -= 8 - msg.length; 363241199Sdelphij 364241199Sdelphij if ((firstpacket.tv_sec == -1) && (firstpacket.tv_usec == -1)) 365241199Sdelphij gettimeofday(&firstpacket, NULL); 366241199Sdelphij 367241199Sdelphij pkth.ts.tv_usec = firstpacket.tv_usec + (msg.timestamp % 100) * 10000; 368241199Sdelphij pkth.ts.tv_sec = firstpacket.tv_usec + (msg.timestamp / 100); 369241199Sdelphij if (pkth.ts.tv_usec > 1000000) 370241199Sdelphij { 371241199Sdelphij pkth.ts.tv_usec -= 1000000; 372241199Sdelphij pkth.ts.tv_sec++; 373241199Sdelphij } 374241199Sdelphij 375241199Sdelphij callback(user, &pkth, (void*)&msg.id); 376241199Sdelphij i++; 377241199Sdelphij } 378241199Sdelphij 379241199Sdelphij return i; 380241199Sdelphij} 381241199Sdelphij 382241199Sdelphij 383241199Sdelphijstatic int 384241199Sdelphijcanusb_inject_linux(pcap_t *handle, const void *buf, size_t size) 385241199Sdelphij{ 386241199Sdelphij /* not yet implemented */ 387241199Sdelphij snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "inject not supported on canusb devices"); 388241199Sdelphij return (-1); 389241199Sdelphij} 390241199Sdelphij 391241199Sdelphij 392241199Sdelphijstatic int 393241199Sdelphijcanusb_stats_linux(pcap_t *handle, struct pcap_stat *stats) 394241199Sdelphij{ 395241199Sdelphij /* not yet implemented */ 396241199Sdelphij stats->ps_recv = 0; /* number of packets received */ 397241199Sdelphij stats->ps_drop = 0; /* number of packets dropped */ 398241199Sdelphij stats->ps_ifdrop = 0; /* drops by interface -- only supported on some platforms */ 399241199Sdelphij return 0; 400241199Sdelphij} 401241199Sdelphij 402241199Sdelphij 403241199Sdelphijstatic int 404241199Sdelphijcanusb_setfilter_linux(pcap_t *p, struct bpf_program *fp) 405241199Sdelphij{ 406241199Sdelphij /* not yet implemented */ 407241199Sdelphij return 0; 408241199Sdelphij} 409241199Sdelphij 410241199Sdelphij 411241199Sdelphijstatic int 412241199Sdelphijcanusb_setdirection_linux(pcap_t *p, pcap_direction_t d) 413241199Sdelphij{ 414241199Sdelphij /* no support for PCAP_D_OUT */ 415241199Sdelphij if (d == PCAP_D_OUT) 416241199Sdelphij { 417241199Sdelphij snprintf(p->errbuf, sizeof(p->errbuf), 418241199Sdelphij "Setting direction to PCAP_D_OUT is not supported on this interface"); 419241199Sdelphij return -1; 420241199Sdelphij } 421241199Sdelphij 422241199Sdelphij p->direction = d; 423241199Sdelphij 424241199Sdelphij return 0; 425241199Sdelphij} 426241199Sdelphij 427241199Sdelphij 428241199Sdelphij/* eof */ 429