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