Deleted Added
sdiff udiff text old ( 233037 ) new ( 233039 )
full compact
usbdump.c (233037) usbdump.c (233039)
1/*-
2 * Copyright (c) 2010 Weongyo Jeong <weongyo@freebsd.org>
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 * 1. Redistributions of source code must retain the above copyright

--- 12 unchanged lines hidden (view full) ---

21 * THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY,
22 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
25 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
27 * THE POSSIBILITY OF SUCH DAMAGES.
28 *
1/*-
2 * Copyright (c) 2010 Weongyo Jeong <weongyo@freebsd.org>
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 * 1. Redistributions of source code must retain the above copyright

--- 12 unchanged lines hidden (view full) ---

21 * THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY,
22 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
25 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
27 * THE POSSIBILITY OF SUCH DAMAGES.
28 *
29 * $FreeBSD: head/usr.sbin/usbdump/usbdump.c 233037 2012-03-16 16:29:21Z hselasky $
29 * $FreeBSD: head/usr.sbin/usbdump/usbdump.c 233039 2012-03-16 17:30:22Z hselasky $
30 */
31
32#include <sys/param.h>
33#include <sys/endian.h>
34#include <sys/ioctl.h>
35#include <sys/socket.h>
36#include <sys/stat.h>
37#include <sys/utsname.h>

--- 52 unchanged lines hidden (view full) ---

90 uint8_t major;
91 uint8_t minor;
92 uint8_t reserved[26];
93} __packed;
94
95#define HEADER_ALIGN(x,a) (((x) + (a) - 1) & ~((a) - 1))
96
97struct header_32 {
30 */
31
32#include <sys/param.h>
33#include <sys/endian.h>
34#include <sys/ioctl.h>
35#include <sys/socket.h>
36#include <sys/stat.h>
37#include <sys/utsname.h>

--- 52 unchanged lines hidden (view full) ---

90 uint8_t major;
91 uint8_t minor;
92 uint8_t reserved[26];
93} __packed;
94
95#define HEADER_ALIGN(x,a) (((x) + (a) - 1) & ~((a) - 1))
96
97struct header_32 {
98 /* capture timestamp */
98 uint32_t ts_sec;
99 uint32_t ts_usec;
99 uint32_t ts_sec;
100 uint32_t ts_usec;
101 /* data length and alignment information */
100 uint32_t caplen;
101 uint32_t datalen;
102 uint32_t caplen;
103 uint32_t datalen;
102 uint16_t hdrlen;
103 uint16_t dummy;
104 uint8_t hdrlen;
105 uint8_t align;
104} __packed;
105
106} __packed;
107
106struct header_64 {
107 uint64_t ts_sec;
108 uint64_t ts_usec;
109 uint32_t caplen;
110 uint32_t datalen;
111 uint16_t hdrlen;
112 uint16_t dummy;
113} __packed;
114
115static int doexit = 0;
116static int pkt_captured = 0;
117static int verbose = 0;
108static int doexit = 0;
109static int pkt_captured = 0;
110static int verbose = 0;
111static int uf_minor;
118static const char *i_arg = "usbus0";
119static const char *r_arg = NULL;
120static const char *w_arg = NULL;
121static const char *errstr_table[USB_ERR_MAX] = {
122 [USB_ERR_NORMAL_COMPLETION] = "0",
123 [USB_ERR_PENDING_REQUESTS] = "PENDING_REQUESTS",
124 [USB_ERR_NOT_STARTED] = "NOT_STARTED",
125 [USB_ERR_INVAL] = "INVAL",

--- 403 unchanged lines hidden (view full) ---

529 }
530 if (verbose >= 2)
531 print_flags(up->up_flags);
532 if (verbose >= 3)
533 print_status(up->up_status);
534}
535
536static void
112static const char *i_arg = "usbus0";
113static const char *r_arg = NULL;
114static const char *w_arg = NULL;
115static const char *errstr_table[USB_ERR_MAX] = {
116 [USB_ERR_NORMAL_COMPLETION] = "0",
117 [USB_ERR_PENDING_REQUESTS] = "PENDING_REQUESTS",
118 [USB_ERR_NOT_STARTED] = "NOT_STARTED",
119 [USB_ERR_INVAL] = "INVAL",

--- 403 unchanged lines hidden (view full) ---

523 }
524 if (verbose >= 2)
525 print_flags(up->up_flags);
526 if (verbose >= 3)
527 print_status(up->up_status);
528}
529
530static void
537print_packets(uint8_t *data, const int datalen)
531fix_packets(uint8_t *data, const int datalen)
538{
539 struct header_32 temp;
540 uint8_t *ptr;
541 uint8_t *next;
532{
533 struct header_32 temp;
534 uint8_t *ptr;
535 uint8_t *next;
536 uint32_t hdrlen;
537 uint32_t caplen;
542
543 for (ptr = data; ptr < (data + datalen); ptr = next) {
544
538
539 for (ptr = data; ptr < (data + datalen); ptr = next) {
540
545 /* automatically figure out endian and size of header */
541 const struct bpf_hdr *hdr;
546
542
547 if (r_arg != NULL) {
543 hdr = (const struct bpf_hdr *)ptr;
548
544
549 const struct header_32 *hdr32;
550 const struct header_64 *hdr64;
545 temp.ts_sec = htole32(hdr->bh_tstamp.tv_sec);
546 temp.ts_usec = htole32(hdr->bh_tstamp.tv_usec);
547 temp.caplen = htole32(hdr->bh_caplen);
548 temp.datalen = htole32(hdr->bh_datalen);
549 temp.hdrlen = hdr->bh_hdrlen;
550 temp.align = BPF_WORDALIGN(1);
551
551
552 hdr32 = (const struct header_32 *)ptr;
553 hdr64 = (const struct header_64 *)ptr;
552 hdrlen = hdr->bh_hdrlen;
553 caplen = hdr->bh_caplen;
554
554
555 temp.hdrlen = le16toh(hdr32->hdrlen);
556 temp.dummy = le16toh(hdr32->dummy);
555 if ((hdrlen >= sizeof(temp)) && (hdrlen <= 255) &&
556 ((ptr + hdrlen) <= (data + datalen))) {
557 memcpy(ptr, &temp, sizeof(temp));
558 memset(ptr + sizeof(temp), 0, hdrlen - sizeof(temp));
559 } else {
560 err(EXIT_FAILURE, "Invalid header length %d", hdrlen);
561 }
557
562
558 if ((temp.hdrlen != 18 && temp.hdrlen != 20) || (temp.dummy != 0)) {
559 temp.hdrlen = be16toh(hdr32->hdrlen);
560 temp.dummy = be16toh(hdr32->dummy);
563 next = ptr + BPF_WORDALIGN(hdrlen + caplen);
561
564
562 if ((temp.hdrlen != 18 && temp.hdrlen != 20) || (temp.dummy != 0)) {
563 temp.hdrlen = le16toh(hdr64->hdrlen);
564 temp.dummy = le16toh(hdr64->dummy);
565 if (next <= ptr)
566 err(EXIT_FAILURE, "Invalid length");
567 }
568}
565
569
566 if ((temp.hdrlen != 28 && temp.hdrlen != 32) || (temp.dummy != 0)) {
567 temp.hdrlen = be16toh(hdr64->hdrlen);
568 temp.dummy = be16toh(hdr64->dummy);
570static void
571print_packets(uint8_t *data, const int datalen)
572{
573 struct header_32 temp;
574 uint8_t *ptr;
575 uint8_t *next;
569
576
570 if ((temp.hdrlen != 28 && temp.hdrlen != 32) || (temp.dummy != 0)) {
571 err(EXIT_FAILURE, "Invalid header detected");
572 next = NULL;
573 } else {
574 temp.ts_sec = be64toh(hdr64->ts_sec);
575 temp.ts_usec = be64toh(hdr64->ts_usec);
576 temp.caplen = be32toh(hdr64->caplen);
577 temp.datalen = be32toh(hdr64->datalen);
578 next = ptr + HEADER_ALIGN(temp.hdrlen + temp.caplen, 8);
579 }
580 } else {
581 temp.ts_sec = le64toh(hdr64->ts_sec);
582 temp.ts_usec = le64toh(hdr64->ts_usec);
583 temp.caplen = le32toh(hdr64->caplen);
584 temp.datalen = le32toh(hdr64->datalen);
585 next = ptr + HEADER_ALIGN(temp.hdrlen + temp.caplen, 8);
586 }
587 } else {
588 temp.ts_sec = be32toh(hdr32->ts_sec);
589 temp.ts_usec = be32toh(hdr32->ts_usec);
590 temp.caplen = be32toh(hdr32->caplen);
591 temp.datalen = be32toh(hdr32->datalen);
592 next = ptr + HEADER_ALIGN(temp.hdrlen + temp.caplen, 4);
593 }
594 } else {
595 temp.ts_sec = le32toh(hdr32->ts_sec);
596 temp.ts_usec = le32toh(hdr32->ts_usec);
597 temp.caplen = le32toh(hdr32->caplen);
598 temp.datalen = le32toh(hdr32->datalen);
599 next = ptr + HEADER_ALIGN(temp.hdrlen + temp.caplen, 4);
600 }
601 } else {
602 const struct bpf_hdr *hdr;
577 for (ptr = data; ptr < (data + datalen); ptr = next) {
603
578
604 hdr = (const struct bpf_hdr *)ptr;
605 temp.ts_sec = hdr->bh_tstamp.tv_sec;
606 temp.ts_usec = hdr->bh_tstamp.tv_usec;
607 temp.caplen = hdr->bh_caplen;
608 temp.datalen = hdr->bh_datalen;
609 temp.hdrlen = hdr->bh_hdrlen;
610 next = ptr + BPF_WORDALIGN(temp.hdrlen + temp.caplen);
611 }
579 const struct header_32 *hdr32;
612
580
581 hdr32 = (const struct header_32 *)ptr;
582
583 temp.ts_sec = le32toh(hdr32->ts_sec);
584 temp.ts_usec = le32toh(hdr32->ts_usec);
585 temp.caplen = le32toh(hdr32->caplen);
586 temp.datalen = le32toh(hdr32->datalen);
587 temp.hdrlen = hdr32->hdrlen;
588 temp.align = hdr32->align;
589
590 next = ptr + HEADER_ALIGN(temp.hdrlen + temp.caplen, temp.align);
591
613 if (next <= ptr)
592 if (next <= ptr)
614 err(EXIT_FAILURE, "Invalid header length");
593 err(EXIT_FAILURE, "Invalid length");
615
594
616 if (w_arg == NULL) {
595 if (w_arg == NULL || r_arg != NULL) {
617 print_apacket(&temp, ptr +
618 temp.hdrlen, temp.caplen);
619 }
620 pkt_captured++;
621 }
622}
623
624static void

--- 26 unchanged lines hidden (view full) ---

651 data = malloc(datalen);
652 if (data == NULL)
653 errx(EX_SOFTWARE, "Out of memory.");
654 ret = read(p->rfd, data, datalen);
655 if (ret != datalen) {
656 err(EXIT_FAILURE, "Could not read complete "
657 "USB data payload");
658 }
596 print_apacket(&temp, ptr +
597 temp.hdrlen, temp.caplen);
598 }
599 pkt_captured++;
600 }
601}
602
603static void

--- 26 unchanged lines hidden (view full) ---

630 data = malloc(datalen);
631 if (data == NULL)
632 errx(EX_SOFTWARE, "Out of memory.");
633 ret = read(p->rfd, data, datalen);
634 if (ret != datalen) {
635 err(EXIT_FAILURE, "Could not read complete "
636 "USB data payload");
637 }
638 if (uf_minor == 2)
639 fix_packets(data, datalen);
640
659 print_packets(data, datalen);
660 free(data);
661 }
662}
663
664static void
665do_loop(struct usbcap *p)
666{

--- 8 unchanged lines hidden (view full) ---

675 default:
676 fprintf(stderr, "read: %s\n", strerror(errno));
677 return;
678 }
679 continue;
680 }
681 if (cc == 0)
682 continue;
641 print_packets(data, datalen);
642 free(data);
643 }
644}
645
646static void
647do_loop(struct usbcap *p)
648{

--- 8 unchanged lines hidden (view full) ---

657 default:
658 fprintf(stderr, "read: %s\n", strerror(errno));
659 return;
660 }
661 continue;
662 }
663 if (cc == 0)
664 continue;
665
666 fix_packets(p->buffer, cc);
667
683 if (w_arg != NULL)
684 write_packets(p, p->buffer, cc);
685 print_packets(p->buffer, cc);
686 }
687}
688
689static void
690init_rfile(struct usbcap *p)

--- 15 unchanged lines hidden (view full) ---

706 errx(EX_SOFTWARE, "Invalid magic field(0x%08x) "
707 "in USB capture file header.",
708 (unsigned int)le32toh(uf.magic));
709 }
710 if (uf.major != 0) {
711 errx(EX_SOFTWARE, "Invalid major version(%d) "
712 "field in USB capture file header.", (int)uf.major);
713 }
668 if (w_arg != NULL)
669 write_packets(p, p->buffer, cc);
670 print_packets(p->buffer, cc);
671 }
672}
673
674static void
675init_rfile(struct usbcap *p)

--- 15 unchanged lines hidden (view full) ---

691 errx(EX_SOFTWARE, "Invalid magic field(0x%08x) "
692 "in USB capture file header.",
693 (unsigned int)le32toh(uf.magic));
694 }
695 if (uf.major != 0) {
696 errx(EX_SOFTWARE, "Invalid major version(%d) "
697 "field in USB capture file header.", (int)uf.major);
698 }
714 if (uf.minor != 2) {
699
700 uf_minor = uf.minor;
701
702 if (uf.minor != 3 && uf.minor != 2) {
715 errx(EX_SOFTWARE, "Invalid minor version(%d) "
716 "field in USB capture file header.", (int)uf.minor);
717 }
718}
719
720static void
721init_wfile(struct usbcap *p)
722{
723 struct usbcap_filehdr uf;
724 int ret;
725
726 p->wfd = open(w_arg, O_CREAT | O_TRUNC | O_WRONLY, S_IRUSR | S_IWUSR);
727 if (p->wfd < 0) {
728 err(EXIT_FAILURE, "Could not open "
703 errx(EX_SOFTWARE, "Invalid minor version(%d) "
704 "field in USB capture file header.", (int)uf.minor);
705 }
706}
707
708static void
709init_wfile(struct usbcap *p)
710{
711 struct usbcap_filehdr uf;
712 int ret;
713
714 p->wfd = open(w_arg, O_CREAT | O_TRUNC | O_WRONLY, S_IRUSR | S_IWUSR);
715 if (p->wfd < 0) {
716 err(EXIT_FAILURE, "Could not open "
729 "'%s' for write", r_arg);
717 "'%s' for write", w_arg);
730 }
731 memset(&uf, 0, sizeof(uf));
732 uf.magic = htole32(USBCAP_FILEHDR_MAGIC);
733 uf.major = 0;
718 }
719 memset(&uf, 0, sizeof(uf));
720 uf.magic = htole32(USBCAP_FILEHDR_MAGIC);
721 uf.major = 0;
734 uf.minor = 2;
722 uf.minor = 3;
735 ret = write(p->wfd, (const void *)&uf, sizeof(uf));
736 if (ret != sizeof(uf)) {
737 err(EXIT_FAILURE, "Could not write "
738 "USB capture header");
739 }
740}
741
742static void

--- 157 unchanged lines hidden --- 		723 ret = write(p->wfd, (const void *)&uf, sizeof(uf));
724 if (ret != sizeof(uf)) {
725 err(EXIT_FAILURE, "Could not write "
726 "USB capture header");
727 }
728}
729
730static void

--- 157 unchanged lines hidden ---