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