Handle ETH_P_CANFD as well as ETH_P_CAN.

[metze/wireshark/wip.git] / wiretap / pcap-common.c

diff --git a/wiretap/pcap-common.c b/wiretap/pcap-common.c
index 16e3589b8c734e218c994ca4edca0c4474969674..27e3fd687f5939d88ef4185d9817db80ef55d103 100644 (file)
--- a/wiretap/pcap-common.c
+++ b/wiretap/pcap-common.c
@@ -326,6 +326,8 @@ static const struct {
        { 181,          WTAP_ENCAP_JUNIPER_CHDLC },
        /* VOIP Frames prepended with meta-information */
        { 183,          WTAP_ENCAP_JUNIPER_VP },
+       /* Virtual Network Frames prepended with meta-information */
+       { 184,          WTAP_ENCAP_JUNIPER_VN },
        /* USB packets from FreeBSD's USB BPF tap */
        { 186,          WTAP_ENCAP_USB_FREEBSD },
        /* Bluetooth HCI UART transport (part H:4) frames, like hcidump */
@@ -742,6 +744,18 @@ wtap_encap_requires_phdr(int wtap_encap)
  */
 #define NOKIA_LEN      4       /* length of the header */
 
+/*
+ * The fake link-layer header of Linux cooked packets.
+ */
+#define LINUX_SLL_PROTOCOL_OFFSET      14      /* protocol */
+#define LINUX_SLL_LEN                  16      /* length of the header */
+
+/*
+ * The protocols we have to check for.
+ */
+#define LINUX_SLL_P_CAN                        0x000C  /* Controller Area Network */
+#define LINUX_SLL_P_CANFD              0x000D  /* Controller Area Network flexible data rate */
+
 /*
  * The fake link-layer header of IrDA packets as introduced by Jean Tourrilhes
  * to libpcap.
@@ -1134,7 +1148,6 @@ struct usb_device_setup_hdr {
        guint16 wLength;
 };
 
-
 /*
  * Offset of the *end* of a field within a particular structure.
  */
@@ -1169,6 +1182,57 @@ struct usb_device_setup_hdr {
                PBSWAP64((guint8 *)fieldp); \
        }
 
+struct can_socketcan_hdr {
+       guint32 can_id;                 /* CAN ID and flags */
+       guint8 payload_length;          /* Frame payload length */
+       guint8 padding;
+       guint8 reserved1;
+       guint8 reserved2;
+};
+
+static void
+pcap_byteswap_linux_sll_pseudoheader(struct wtap_pkthdr *phdr, guint8 *pd)
+{
+       guint packet_size;
+       guint16 protocol;
+       struct can_socketcan_hdr *can_socketcan_phdr;
+
+       /*
+        * Minimum of captured and actual length (just in case the
+        * actual length < the captured length, which Should Never
+        * Happen).
+        */
+       packet_size = phdr->caplen;
+       if (packet_size > phdr->len)
+               packet_size = phdr->len;
+
+       if (packet_size < LINUX_SLL_LEN) {
+               /* Not enough data to have the protocol */
+               return;
+       }
+
+       protocol = pntoh16(&pd[LINUX_SLL_PROTOCOL_OFFSET]);
+       if (protocol != LINUX_SLL_P_CAN && protocol != LINUX_SLL_P_CANFD) {
+               /* Not a CAN packet; nothing to fix */
+               return;
+       }
+
+       /*
+        * Greasy hack, but we never directly dereference any of
+        * the fields in *can_socketcan_phdr, we just get offsets
+        * of and addresses of its members and byte-swap it with a
+        * byte-at-a-time macro, so it's alignment-safe.
+        */
+       can_socketcan_phdr = (struct can_socketcan_hdr *)(void *)(pd + LINUX_SLL_LEN);
+
+       if (packet_size < LINUX_SLL_LEN + sizeof(can_socketcan_phdr->can_id)) {
+               /* Not enough data to have the full CAN ID */
+               return;
+       }
+
+       PBSWAP32((guint8 *)&can_socketcan_phdr->can_id);
+}
+
 static void
 pcap_byteswap_linux_usb_pseudoheader(struct wtap_pkthdr *phdr, guint8 *pd,
     gboolean header_len_64_bytes)
@@ -1869,6 +1933,11 @@ pcap_read_post_process(int file_type, int wtap_encap,
                phdr->pseudo_header.eth.fcs_len = fcs_len;
                break;
 
+       case WTAP_ENCAP_SLL:
+               if (bytes_swapped)
+                       pcap_byteswap_linux_sll_pseudoheader(phdr, pd);
+               break;
+
        case WTAP_ENCAP_USB_LINUX:
                if (bytes_swapped)
                        pcap_byteswap_linux_usb_pseudoheader(phdr, pd, FALSE);
@@ -1892,6 +1961,7 @@ pcap_read_post_process(int file_type, int wtap_encap,
                if (bytes_swapped)
                        pcap_byteswap_nflog_pseudoheader(phdr, pd);
                break;
+
        case WTAP_ENCAP_ERF:
                /*
                 * Update packet size to account for ERF padding and snapping.