1 /* $NetBSD: pcap-canusb-linux.c,v 1.3 2015/03/31 21:39:42 christos Exp $ */
2
3 /*
4 * Copyright (c) 2009 Felix Obenhuber
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * 1. Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * 2. Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in the
15 * documentation and/or other materials provided with the distribution.
16 * 3. The name of the author may not be used to endorse or promote
17 * products derived from this software without specific prior written
18 * permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 *
32 * Sockettrace sniffing API implementation for Linux platform
33 * By Felix Obenhuber <felix@obenhuber.de>
34 *
35 */
36
37 #include <sys/cdefs.h>
38 __RCSID("$NetBSD: pcap-canusb-linux.c,v 1.3 2015/03/31 21:39:42 christos Exp $");
39
40 #ifdef HAVE_CONFIG_H
41 #include "config.h"
42 #endif
43
44 #include <libusb-1.0/libusb.h>
45
46 #include <stdlib.h>
47 #include <unistd.h>
48 #include <fcntl.h>
49 #include <errno.h>
50 #include <string.h>
51 #include <pthread.h>
52
53 #include "pcap-int.h"
54 #include "pcap-canusb-linux.h"
55
56 #define CANUSB_IFACE "canusb"
57
58 #define CANUSB_VID 0x0403
59 #define CANUSB_PID 0x8990
60
61 #define USE_THREAD 1
62
63 #if USE_THREAD == 0
64 #include <signal.h>
65 #endif
66
67
68 /* forward declaration */
69 static int canusb_activate(pcap_t *);
70 static int canusb_read_linux(pcap_t *, int , pcap_handler , u_char *);
71 static int canusb_inject_linux(pcap_t *, const void *, size_t);
72 static int canusb_setfilter_linux(pcap_t *, struct bpf_program *);
73 static int canusb_setdirection_linux(pcap_t *, pcap_direction_t);
74 static int canusb_stats_linux(pcap_t *, struct pcap_stat *);
75
76 struct CAN_Msg
77 {
78 uint32_t timestamp;
79 uint32_t id;
80 uint32_t length;
81 uint8_t data[8];
82 };
83
84 /*
85 * Private data for capturing on Linux CANbus USB devices.
86 */
87 struct pcap_canusb {
88 libusb_context *ctx;
89 libusb_device_handle *dev;
90 pthread_t worker;
91 int rdpipe, wrpipe;
92 volatile int loop;
93 };
94
canusb_findalldevs(pcap_if_t ** alldevsp,char * err_str)95 int canusb_findalldevs(pcap_if_t **alldevsp, char *err_str)
96 {
97 libusb_context *fdctx;
98 libusb_device** devs;
99 unsigned char sernum[65];
100 int cnt, i;
101
102 if (libusb_init(&fdctx) != 0) {
103 /*
104 * XXX - if this doesn't just mean "no USB file system mounted",
105 * perhaps we should report a real error rather than just
106 * saying "no CANUSB devices".
107 */
108 return 0;
109 }
110
111 cnt = libusb_get_device_list(fdctx,&devs);
112
113 for(i=0;i<cnt;i++)
114 {
115 int ret;
116 // Check if this device is interesting.
117 struct libusb_device_descriptor desc;
118 libusb_get_device_descriptor(devs[i],&desc);
119
120 if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID))
121 continue; //It is not, check next device
122
123 //It is!
124 libusb_device_handle *dh = NULL;
125
126 if ((ret = libusb_open(devs[i],&dh)) == 0)
127 {
128 char dev_name[30];
129 char dev_descr[50];
130 int n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,sernum,64);
131 sernum[n] = 0;
132
133 snprintf(dev_name, 30, CANUSB_IFACE"%s", sernum);
134 snprintf(dev_descr, 50, "CanUSB [%s]", sernum);
135
136 libusb_close(dh);
137
138 if (pcap_add_if(alldevsp, dev_name, 0, dev_descr, err_str) < 0)
139 {
140 libusb_free_device_list(devs,1);
141 libusb_exit(fdctx);
142 return -1;
143 }
144 }
145 }
146
147 libusb_free_device_list(devs,1);
148 libusb_exit(fdctx);
149 return 0;
150 }
151
canusb_opendevice(struct libusb_context * ctx,char * devserial)152 static libusb_device_handle* canusb_opendevice(struct libusb_context *ctx, char* devserial)
153 {
154 libusb_device** devs;
155 unsigned char serial[65];
156 int cnt,i,n;
157
158 cnt = libusb_get_device_list(ctx,&devs);
159
160 for(i=0;i<cnt;i++)
161 {
162 // Check if this device is interesting.
163 struct libusb_device_descriptor desc;
164 libusb_get_device_descriptor(devs[i],&desc);
165
166 if ((desc.idVendor != CANUSB_VID) || (desc.idProduct != CANUSB_PID))
167 continue;
168
169 //Found one!
170 libusb_device_handle *dh = NULL;
171
172 if (libusb_open(devs[i],&dh) != 0) continue;
173
174 n = libusb_get_string_descriptor_ascii(dh,desc.iSerialNumber,serial,64);
175 serial[n] = 0;
176
177 if ((devserial) && (strcmp((char *)serial,devserial) != 0))
178 {
179 libusb_close(dh);
180 continue;
181 }
182
183 if ((libusb_kernel_driver_active(dh,0)) && (libusb_detach_kernel_driver(dh,0) != 0))
184 {
185 libusb_close(dh);
186 continue;
187 }
188
189 if (libusb_set_configuration(dh,1) != 0)
190 {
191 libusb_close(dh);
192 continue;
193 }
194
195 if (libusb_claim_interface(dh,0) != 0)
196 {
197 libusb_close(dh);
198 continue;
199 }
200
201 //Fount it!
202 libusb_free_device_list(devs,1);
203 return dh;
204 }
205
206 libusb_free_device_list(devs,1);
207 return NULL;
208 }
209
210
211 pcap_t *
canusb_create(const char * device,char * ebuf,int * is_ours)212 canusb_create(const char *device, char *ebuf, int *is_ours)
213 {
214 const char *cp;
215 char *cpend;
216 long devnum;
217 pcap_t* p;
218 struct pcap_canusb *canusb;
219
220 /* Does this look like a DAG device? */
221 cp = strrchr(device, '/');
222 if (cp == NULL)
223 cp = device;
224 /* Does it begin with "canusb"? */
225 if (strncmp(cp, "canusb", 6) != 0) {
226 /* Nope, doesn't begin with "canusb" */
227 *is_ours = 0;
228 return NULL;
229 }
230 /* Yes - is "canusb" followed by a number? */
231 cp += 6;
232 devnum = strtol(cp, &cpend, 10);
233 if (cpend == cp || *cpend != '\0') {
234 /* Not followed by a number. */
235 *is_ours = 0;
236 return NULL;
237 }
238 if (devnum < 0) {
239 /* Followed by a non-valid number. */
240 *is_ours = 0;
241 return NULL;
242 }
243
244 /* OK, it's probably ours. */
245 *is_ours = 1;
246
247 p = pcap_create_common(device, ebuf, sizeof (struct pcap_canusb));
248 if (p == NULL)
249 return (NULL);
250
251 canusb = p->priv;
252 canusb->ctx = NULL;
253 canusb->dev = NULL;
254 canusb->rdpipe = -1;
255 canusb->wrpipe = -1;
256
257 p->activate_op = canusb_activate;
258
259 return (p);
260 }
261
262
canusb_capture_thread(void * arg)263 static void* canusb_capture_thread(void *arg)
264 {
265 struct pcap_canusb *canusb = arg;
266 int i;
267 struct
268 {
269 uint8_t rxsz, txsz;
270 } status;
271
272 fcntl(canusb->wrpipe, F_SETFL, O_NONBLOCK);
273
274 while(canusb->loop)
275 {
276 int sz;
277 struct CAN_Msg msg;
278
279 libusb_interrupt_transfer(canusb->dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100);
280 //HACK!!!!! -> drop buffered data, read new one by reading twice.
281 libusb_interrupt_transfer(canusb->dev, 0x81, (unsigned char*)&status, sizeof(status), &sz, 100);
282
283 for(i = 0; i<status.rxsz; i++)
284 {
285 libusb_bulk_transfer(canusb->dev, 0x85, (unsigned char*)&msg, sizeof(msg), &sz, 100);
286 if(write(canusb->wrpipe, &msg, sizeof(msg)) < 0)
287 fprintf(stderr,"write() error: %s\n", strerror(errno));
288 }
289
290 }
291
292 return NULL;
293 }
294
canusb_startcapture(struct pcap_canusb * this)295 static int canusb_startcapture(struct pcap_canusb* this)
296 {
297 int pipefd[2];
298
299 if (pipe(pipefd) == -1)
300 return -1;
301
302 this->rdpipe = pipefd[0];
303 this->wrpipe = pipefd[1];
304
305 this->loop = 1;
306 pthread_create(&this->worker, NULL, canusb_capture_thread, this);
307
308 return this->rdpipe;
309 }
310
canusb_clearbufs(struct pcap_canusb * this)311 static void canusb_clearbufs(struct pcap_canusb* this)
312 {
313 unsigned char cmd[16];
314 int al;
315
316 cmd[0] = 1; //Empty incoming buffer
317 cmd[1] = 1; //Empty outgoing buffer
318 cmd[3] = 0; //Not a write to serial number
319 memset(&cmd[4],0,16-4);
320
321 libusb_interrupt_transfer(this->dev, 0x1,cmd,16,&al,100);
322 }
323
324
canusb_close(pcap_t * handle)325 static void canusb_close(pcap_t* handle)
326 {
327 struct pcap_canusb *canusb = handle->priv;
328
329 canusb->loop = 0;
330 pthread_join(canusb->worker, NULL);
331
332 if (canusb->dev)
333 {
334 libusb_close(canusb->dev);
335 canusb->dev = NULL;
336 }
337 if (canusb->ctx)
338 {
339 libusb_exit(canusb->ctx);
340 canusb->ctx = NULL;
341 }
342 }
343
344
345
canusb_activate(pcap_t * handle)346 static int canusb_activate(pcap_t* handle)
347 {
348 struct pcap_canusb *canusb = handle->priv;
349 char *serial;
350
351 if (libusb_init(&canusb->ctx) != 0) {
352 /*
353 * XXX - what causes this to fail?
354 */
355 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "libusb_init() failed");
356 return PCAP_ERROR;
357 }
358
359 handle->read_op = canusb_read_linux;
360
361 handle->inject_op = canusb_inject_linux;
362 handle->setfilter_op = canusb_setfilter_linux;
363 handle->setdirection_op = canusb_setdirection_linux;
364 handle->getnonblock_op = pcap_getnonblock_fd;
365 handle->setnonblock_op = pcap_setnonblock_fd;
366 handle->stats_op = canusb_stats_linux;
367 handle->cleanup_op = canusb_close;
368
369 /* Initialize some components of the pcap structure. */
370 handle->bufsize = 32;
371 handle->offset = 8;
372 handle->linktype = DLT_CAN_SOCKETCAN;
373 handle->set_datalink_op = NULL;
374
375 serial = handle->opt.source + strlen(CANUSB_IFACE);
376
377 canusb->dev = canusb_opendevice(canusb->ctx, serial);
378 if (!canusb->dev)
379 {
380 libusb_exit(canusb->ctx);
381 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "Can't open USB Device");
382 return PCAP_ERROR;
383 }
384
385 canusb_clearbufs(canusb);
386
387 handle->fd = canusb_startcapture(canusb);
388 handle->selectable_fd = handle->fd;
389
390 return 0;
391 }
392
393
394
395
396 static int
canusb_read_linux(pcap_t * handle,int max_packets,pcap_handler callback,u_char * user)397 canusb_read_linux(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user)
398 {
399 static struct timeval firstpacket = { -1, -1};
400 int i = 0;
401 struct CAN_Msg msg;
402 struct pcap_pkthdr pkth;
403
404 while(i < max_packets)
405 {
406 int n;
407 usleep(10 * 1000);
408 n = read(handle->fd, &msg, sizeof(msg));
409 if (n <= 0)
410 break;
411 pkth.caplen = pkth.len = n;
412 pkth.caplen -= 4;
413 pkth.caplen -= 8 - msg.length;
414
415 if ((firstpacket.tv_sec == -1) && (firstpacket.tv_usec == -1))
416 gettimeofday(&firstpacket, NULL);
417
418 pkth.ts.tv_usec = firstpacket.tv_usec + (msg.timestamp % 100) * 10000;
419 pkth.ts.tv_sec = firstpacket.tv_usec + (msg.timestamp / 100);
420 if (pkth.ts.tv_usec > 1000000)
421 {
422 pkth.ts.tv_usec -= 1000000;
423 pkth.ts.tv_sec++;
424 }
425
426 callback(user, &pkth, (void*)&msg.id);
427 i++;
428 }
429
430 return i;
431 }
432
433
434 static int
canusb_inject_linux(pcap_t * handle,const void * buf,size_t size)435 canusb_inject_linux(pcap_t *handle, const void *buf, size_t size)
436 {
437 /* not yet implemented */
438 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "inject not supported on canusb devices");
439 return (-1);
440 }
441
442
443 static int
canusb_stats_linux(pcap_t * handle,struct pcap_stat * stats)444 canusb_stats_linux(pcap_t *handle, struct pcap_stat *stats)
445 {
446 /* not yet implemented */
447 stats->ps_recv = 0; /* number of packets received */
448 stats->ps_drop = 0; /* number of packets dropped */
449 stats->ps_ifdrop = 0; /* drops by interface -- only supported on some platforms */
450 return 0;
451 }
452
453
454 static int
canusb_setfilter_linux(pcap_t * p,struct bpf_program * fp)455 canusb_setfilter_linux(pcap_t *p, struct bpf_program *fp)
456 {
457 /* not yet implemented */
458 return 0;
459 }
460
461
462 static int
canusb_setdirection_linux(pcap_t * p,pcap_direction_t d)463 canusb_setdirection_linux(pcap_t *p, pcap_direction_t d)
464 {
465 /* no support for PCAP_D_OUT */
466 if (d == PCAP_D_OUT)
467 {
468 snprintf(p->errbuf, sizeof(p->errbuf),
469 "Setting direction to PCAP_D_OUT is not supported on this interface");
470 return -1;
471 }
472
473 p->direction = d;
474
475 return 0;
476 }
477
478
479 /* eof */
480