pcap-canusb-linux.c revision 241199
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 */ 61static int canusb_activate(pcap_t *); 62static int canusb_read_linux(pcap_t *, int , pcap_handler , u_char *); 63static int canusb_inject_linux(pcap_t *, const void *, size_t); 64static int canusb_setfilter_linux(pcap_t *, struct bpf_program *); 65static int canusb_setdirection_linux(pcap_t *, pcap_direction_t); 66static int canusb_stats_linux(pcap_t *, struct pcap_stat *); 67 68struct CAN_Msg 69{ 70 uint32_t timestamp; 71 uint32_t id; 72 uint32_t length; 73 uint8_t data[8]; 74}; 75 76struct 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 86static struct canusb_t canusb; 87static volatile int loop; 88 89 90 91int 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 141static 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 201pcap_t * 202canusb_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 222static 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 263static 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 279static 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 293static 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 307static 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 345static int 346canusb_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 383static int 384canusb_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 392static int 393canusb_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 403static int 404canusb_setfilter_linux(pcap_t *p, struct bpf_program *fp) 405{ 406 /* not yet implemented */ 407 return 0; 408} 409 410 411static int 412canusb_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 */ 429