pcap-canusb-linux.c revision 251129
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{ 78251129Sdelphij libusb_context *ctx; 79251129Sdelphij libusb_device_handle *dev; 80251129Sdelphij char *serial; 81251129Sdelphij pthread_t worker; 82251129Sdelphij int rdpipe, wrpipe; 83251129Sdelphij volatile int* loop; 84241199Sdelphij}; 85241199Sdelphij 86241199Sdelphijstatic struct canusb_t canusb; 87241199Sdelphijstatic volatile int loop; 88241199Sdelphij 89251129Sdelphijint canusb_findalldevs(pcap_if_t **alldevsp, char *err_str) 90241199Sdelphij{ 91241199Sdelphij libusb_context *fdctx; 92241199Sdelphij libusb_device** devs; 93241199Sdelphij unsigned char sernum[65]; 94241199Sdelphij unsigned char buf[96]; 95241199Sdelphij int cnt, i; 96241199Sdelphij 97251129Sdelphij if (libusb_init(&fdctx) != 0) { 98251129Sdelphij /* 99251129Sdelphij * XXX - if this doesn't just mean "no USB file system mounted", 100251129Sdelphij * perhaps we should report a real error rather than just 101251129Sdelphij * saying "no CANUSB devices". 102251129Sdelphij */ 103251129Sdelphij return 0; 104251129Sdelphij } 105251129Sdelphij 106241199Sdelphij cnt = libusb_get_device_list(fdctx,&devs); 107241199Sdelphij 108241199Sdelphij for(i=0;i<cnt;i++) 109241199Sdelphij { 110241199Sdelphij int ret; 111241199Sdelphij // Check if this device is interesting. 112241199Sdelphij struct libusb_device_descriptor desc; 113241199Sdelphij libusb_get_device_descriptor(devs[i],&desc); 114241199Sdelphij 115241199Sdelphij if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID)) 116251129Sdelphij continue; //It is not, check next device 117241199Sdelphij 118241199Sdelphij //It is! 119241199Sdelphij libusb_device_handle *dh = NULL; 120241199Sdelphij 121241199Sdelphij if (ret = libusb_open(devs[i],&dh) == 0) 122241199Sdelphij { 123251129Sdelphij char dev_name[30]; 124251129Sdelphij char dev_descr[50]; 125241199Sdelphij int n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,sernum,64); 126241199Sdelphij sernum[n] = 0; 127241199Sdelphij 128251129Sdelphij snprintf(dev_name, 30, CANUSB_IFACE"%s", sernum); 129251129Sdelphij snprintf(dev_descr, 50, "CanUSB [%s]", sernum); 130241199Sdelphij 131241199Sdelphij libusb_close(dh); 132241199Sdelphij 133241199Sdelphij if (pcap_add_if(alldevsp, dev_name, 0, dev_descr, err_str) < 0) 134241199Sdelphij { 135251129Sdelphij libusb_free_device_list(devs,1); 136251129Sdelphij return -1; 137241199Sdelphij } 138241199Sdelphij } 139241199Sdelphij } 140241199Sdelphij 141241199Sdelphij libusb_free_device_list(devs,1); 142241199Sdelphij libusb_exit(fdctx); 143241199Sdelphij return 0; 144241199Sdelphij} 145241199Sdelphij 146241199Sdelphijstatic libusb_device_handle* canusb_opendevice(struct libusb_context *ctx, char* devserial) 147241199Sdelphij{ 148241199Sdelphij libusb_device_handle* dh; 149241199Sdelphij libusb_device** devs; 150241199Sdelphij unsigned char serial[65]; 151241199Sdelphij int cnt,i,n; 152241199Sdelphij 153241199Sdelphij cnt = libusb_get_device_list(ctx,&devs); 154241199Sdelphij 155241199Sdelphij for(i=0;i<cnt;i++) 156241199Sdelphij { 157241199Sdelphij // Check if this device is interesting. 158241199Sdelphij struct libusb_device_descriptor desc; 159241199Sdelphij libusb_get_device_descriptor(devs[i],&desc); 160241199Sdelphij 161241199Sdelphij if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID)) 162241199Sdelphij continue; 163241199Sdelphij 164241199Sdelphij //Found one! 165241199Sdelphij libusb_device_handle *dh = NULL; 166241199Sdelphij 167241199Sdelphij if (libusb_open(devs[i],&dh) != 0) continue; 168241199Sdelphij 169241199Sdelphij n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,serial,64); 170241199Sdelphij serial[n] = 0; 171241199Sdelphij 172241199Sdelphij if ((devserial) && (strcmp(serial,devserial) != 0)) 173241199Sdelphij { 174241199Sdelphij libusb_close(dh); 175241199Sdelphij continue; 176241199Sdelphij } 177241199Sdelphij 178241199Sdelphij if ((libusb_kernel_driver_active(dh,0)) && (libusb_detach_kernel_driver(dh,0) != 0)) 179241199Sdelphij { 180241199Sdelphij libusb_close(dh); 181241199Sdelphij continue; 182241199Sdelphij } 183241199Sdelphij 184241199Sdelphij if (libusb_set_configuration(dh,1) != 0) 185241199Sdelphij { 186241199Sdelphij libusb_close(dh); 187241199Sdelphij continue; 188241199Sdelphij } 189241199Sdelphij 190241199Sdelphij if (libusb_claim_interface(dh,0) != 0) 191241199Sdelphij { 192241199Sdelphij libusb_close(dh); 193241199Sdelphij continue; 194241199Sdelphij } 195241199Sdelphij 196241199Sdelphij //Fount it! 197241199Sdelphij libusb_free_device_list(devs,1); 198241199Sdelphij return dh; 199241199Sdelphij } 200241199Sdelphij 201241199Sdelphij libusb_free_device_list(devs,1); 202241199Sdelphij return NULL; 203241199Sdelphij} 204241199Sdelphij 205241199Sdelphij 206241199Sdelphijpcap_t * 207251129Sdelphijcanusb_create(const char *device, char *ebuf, int *is_ours) 208241199Sdelphij{ 209251129Sdelphij const char *cp; 210251129Sdelphij char *cpend; 211251129Sdelphij long devnum; 212251129Sdelphij pcap_t* p; 213251129Sdelphij 214251129Sdelphij libusb_init(&canusb.ctx); 215251129Sdelphij 216251129Sdelphij /* Does this look like a DAG device? */ 217251129Sdelphij cp = strrchr(device, '/'); 218251129Sdelphij if (cp == NULL) 219251129Sdelphij cp = device; 220251129Sdelphij /* Does it begin with "canusb"? */ 221251129Sdelphij if (strncmp(cp, "canusb", 6) != 0) { 222251129Sdelphij /* Nope, doesn't begin with "canusb" */ 223251129Sdelphij *is_ours = 0; 224251129Sdelphij return NULL; 225251129Sdelphij } 226251129Sdelphij /* Yes - is "canusb" followed by a number? */ 227251129Sdelphij cp += 6; 228251129Sdelphij devnum = strtol(cp, &cpend, 10); 229251129Sdelphij if (cpend == cp || *cpend != '\0') { 230251129Sdelphij /* Not followed by a number. */ 231251129Sdelphij *is_ours = 0; 232251129Sdelphij return NULL; 233251129Sdelphij } 234251129Sdelphij if (devnum < 0) { 235251129Sdelphij /* Followed by a non-valid number. */ 236251129Sdelphij *is_ours = 0; 237251129Sdelphij return NULL; 238251129Sdelphij } 239251129Sdelphij 240251129Sdelphij /* OK, it's probably ours. */ 241251129Sdelphij *is_ours = 1; 242251129Sdelphij 243251129Sdelphij p = pcap_create_common(device, ebuf); 244251129Sdelphij if (p == NULL) 245251129Sdelphij return (NULL); 246251129Sdelphij 247251129Sdelphij memset(&canusb, 0x00, sizeof(canusb)); 248251129Sdelphij 249251129Sdelphij p->activate_op = canusb_activate; 250251129Sdelphij 251251129Sdelphij return (p); 252241199Sdelphij} 253241199Sdelphij 254241199Sdelphij 255241199Sdelphijstatic void* canusb_capture_thread(struct canusb_t *canusb) 256241199Sdelphij{ 257251129Sdelphij struct libusb_context *ctx; 258251129Sdelphij libusb_device_handle *dev; 259251129Sdelphij int i, n; 260251129Sdelphij struct 261251129Sdelphij { 262251129Sdelphij uint8_t rxsz, txsz; 263251129Sdelphij } status; 264251129Sdelphij char *serial; 265241199Sdelphij 266251129Sdelphij libusb_init(&ctx); 267241199Sdelphij 268251129Sdelphij serial = canusb->serial; 269251129Sdelphij dev = canusb_opendevice(ctx, serial); 270241199Sdelphij 271251129Sdelphij fcntl(canusb->wrpipe, F_SETFL, O_NONBLOCK); 272241199Sdelphij 273251129Sdelphij while(*canusb->loop) 274251129Sdelphij { 275251129Sdelphij int sz, ret; 276251129Sdelphij struct CAN_Msg msg; 277241199Sdelphij 278251129Sdelphij libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100); 279251129Sdelphij //HACK!!!!! -> drop buffered data, read new one by reading twice. 280251129Sdelphij ret = libusb_interrupt_transfer(dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100); 281241199Sdelphij 282251129Sdelphij for(i = 0; i<status.rxsz; i++) 283251129Sdelphij { 284251129Sdelphij libusb_bulk_transfer(dev, 0x85, (unsigned char*)&msg, sizeof(msg), &sz, 100); 285251129Sdelphij n = write(canusb->wrpipe, &msg, sizeof(msg)); 286251129Sdelphij } 287251129Sdelphij 288241199Sdelphij } 289241199Sdelphij 290251129Sdelphij libusb_close(dev); 291251129Sdelphij libusb_exit(ctx); 292241199Sdelphij 293251129Sdelphij return NULL; 294241199Sdelphij} 295241199Sdelphij 296241199Sdelphijstatic int canusb_startcapture(struct canusb_t* this) 297241199Sdelphij{ 298251129Sdelphij int pipefd[2]; 299241199Sdelphij 300251129Sdelphij if (pipe(pipefd) == -1) 301251129Sdelphij return -1; 302241199Sdelphij 303251129Sdelphij canusb.rdpipe = pipefd[0]; 304251129Sdelphij canusb.wrpipe = pipefd[1]; 305251129Sdelphij canusb.loop = &loop; 306241199Sdelphij 307251129Sdelphij loop = 1; 308251129Sdelphij pthread_create(&this->worker, NULL, canusb_capture_thread, &canusb); 309241199Sdelphij 310251129Sdelphij return canusb.rdpipe; 311241199Sdelphij} 312241199Sdelphij 313241199Sdelphijstatic void canusb_clearbufs(struct canusb_t* this) 314241199Sdelphij{ 315251129Sdelphij unsigned char cmd[16]; 316251129Sdelphij int al; 317241199Sdelphij 318251129Sdelphij cmd[0] = 1; //Empty incoming buffer 319251129Sdelphij cmd[1] = 1; //Empty outgoing buffer 320251129Sdelphij cmd[3] = 0; //Not a write to serial number 321251129Sdelphij memset(&cmd[4],0,16-4); 322241199Sdelphij 323251129Sdelphij libusb_interrupt_transfer(this->dev, 0x1,cmd,16,&al,100); 324241199Sdelphij} 325241199Sdelphij 326241199Sdelphij 327241199Sdelphijstatic void canusb_close(pcap_t* handle) 328241199Sdelphij{ 329251129Sdelphij loop = 0; 330251129Sdelphij pthread_join(canusb.worker, NULL); 331241199Sdelphij 332251129Sdelphij if (canusb.dev) 333251129Sdelphij { 334251129Sdelphij libusb_close(canusb.dev); 335251129Sdelphij canusb.dev = NULL; 336251129Sdelphij } 337241199Sdelphij} 338241199Sdelphij 339241199Sdelphij 340241199Sdelphij 341241199Sdelphijstatic int canusb_activate(pcap_t* handle) 342241199Sdelphij{ 343251129Sdelphij char *serial; 344241199Sdelphij 345251129Sdelphij handle->read_op = canusb_read_linux; 346241199Sdelphij 347251129Sdelphij handle->inject_op = canusb_inject_linux; 348251129Sdelphij handle->setfilter_op = canusb_setfilter_linux; 349251129Sdelphij handle->setdirection_op = canusb_setdirection_linux; 350251129Sdelphij handle->getnonblock_op = pcap_getnonblock_fd; 351251129Sdelphij handle->setnonblock_op = pcap_setnonblock_fd; 352251129Sdelphij handle->stats_op = canusb_stats_linux; 353251129Sdelphij handle->cleanup_op = canusb_close; 354241199Sdelphij 355251129Sdelphij /* Initialize some components of the pcap structure. */ 356251129Sdelphij handle->bufsize = 32; 357251129Sdelphij handle->offset = 8; 358251129Sdelphij handle->linktype = DLT_CAN_SOCKETCAN; 359251129Sdelphij handle->set_datalink_op = NULL; 360241199Sdelphij 361251129Sdelphij serial = handle->opt.source + strlen(CANUSB_IFACE); 362251129Sdelphij canusb.serial = strdup(serial); 363241199Sdelphij 364251129Sdelphij canusb.dev = canusb_opendevice(canusb.ctx,serial); 365251129Sdelphij if (!canusb.dev) 366251129Sdelphij { 367251129Sdelphij snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "Can't open USB Device:"); 368251129Sdelphij return PCAP_ERROR; 369251129Sdelphij } 370241199Sdelphij 371251129Sdelphij canusb_clearbufs(&canusb); 372251129Sdelphij 373251129Sdelphij handle->fd = canusb_startcapture(&canusb); 374251129Sdelphij handle->selectable_fd = handle->fd; 375251129Sdelphij 376251129Sdelphij return 0; 377241199Sdelphij} 378241199Sdelphij 379241199Sdelphij 380241199Sdelphij 381241199Sdelphij 382241199Sdelphijstatic int 383241199Sdelphijcanusb_read_linux(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user) 384241199Sdelphij{ 385251129Sdelphij static struct timeval firstpacket = { -1, -1}; 386241199Sdelphij 387251129Sdelphij int msgsent = 0; 388251129Sdelphij int i = 0; 389251129Sdelphij struct CAN_Msg msg; 390251129Sdelphij struct pcap_pkthdr pkth; 391241199Sdelphij 392251129Sdelphij while(i < max_packets) 393251129Sdelphij { 394251129Sdelphij int n; 395251129Sdelphij usleep(10 * 1000); 396251129Sdelphij n = read(handle->fd, &msg, sizeof(msg)); 397251129Sdelphij if (n <= 0) 398251129Sdelphij break; 399251129Sdelphij pkth.caplen = pkth.len = n; 400251129Sdelphij pkth.caplen -= 4; 401251129Sdelphij pkth.caplen -= 8 - msg.length; 402241199Sdelphij 403251129Sdelphij if ((firstpacket.tv_sec == -1) && (firstpacket.tv_usec == -1)) 404251129Sdelphij gettimeofday(&firstpacket, NULL); 405241199Sdelphij 406251129Sdelphij pkth.ts.tv_usec = firstpacket.tv_usec + (msg.timestamp % 100) * 10000; 407251129Sdelphij pkth.ts.tv_sec = firstpacket.tv_usec + (msg.timestamp / 100); 408251129Sdelphij if (pkth.ts.tv_usec > 1000000) 409251129Sdelphij { 410251129Sdelphij pkth.ts.tv_usec -= 1000000; 411251129Sdelphij pkth.ts.tv_sec++; 412251129Sdelphij } 413251129Sdelphij 414251129Sdelphij callback(user, &pkth, (void*)&msg.id); 415251129Sdelphij i++; 416241199Sdelphij } 417241199Sdelphij 418251129Sdelphij return i; 419241199Sdelphij} 420241199Sdelphij 421241199Sdelphij 422241199Sdelphijstatic int 423241199Sdelphijcanusb_inject_linux(pcap_t *handle, const void *buf, size_t size) 424241199Sdelphij{ 425251129Sdelphij /* not yet implemented */ 426251129Sdelphij snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "inject not supported on canusb devices"); 427251129Sdelphij return (-1); 428241199Sdelphij} 429241199Sdelphij 430241199Sdelphij 431241199Sdelphijstatic int 432241199Sdelphijcanusb_stats_linux(pcap_t *handle, struct pcap_stat *stats) 433241199Sdelphij{ 434251129Sdelphij /* not yet implemented */ 435251129Sdelphij stats->ps_recv = 0; /* number of packets received */ 436251129Sdelphij stats->ps_drop = 0; /* number of packets dropped */ 437251129Sdelphij stats->ps_ifdrop = 0; /* drops by interface -- only supported on some platforms */ 438251129Sdelphij return 0; 439241199Sdelphij} 440241199Sdelphij 441241199Sdelphij 442241199Sdelphijstatic int 443241199Sdelphijcanusb_setfilter_linux(pcap_t *p, struct bpf_program *fp) 444241199Sdelphij{ 445251129Sdelphij /* not yet implemented */ 446251129Sdelphij return 0; 447241199Sdelphij} 448241199Sdelphij 449241199Sdelphij 450241199Sdelphijstatic int 451241199Sdelphijcanusb_setdirection_linux(pcap_t *p, pcap_direction_t d) 452241199Sdelphij{ 453251129Sdelphij /* no support for PCAP_D_OUT */ 454251129Sdelphij if (d == PCAP_D_OUT) 455251129Sdelphij { 456251129Sdelphij snprintf(p->errbuf, sizeof(p->errbuf), 457251129Sdelphij "Setting direction to PCAP_D_OUT is not supported on this interface"); 458251129Sdelphij return -1; 459251129Sdelphij } 460241199Sdelphij 461251129Sdelphij p->direction = d; 462241199Sdelphij 463251129Sdelphij return 0; 464241199Sdelphij} 465241199Sdelphij 466241199Sdelphij 467241199Sdelphij/* eof */ 468