Handle ETH_P_CANFD as well as ETH_P_CAN.
[metze/wireshark/wip.git] / wiretap / pcap-common.c
index 297387af07498d26e0b3021119de103fe4e43d5c..27e3fd687f5939d88ef4185d9817db80ef55d103 100644 (file)
@@ -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)
@@ -1487,6 +1551,7 @@ pcap_read_erf_subheader(FILE_T fh, union wtap_pseudo_header *pseudo_header,
        case ERF_TYPE_MC_ATM:
        case ERF_TYPE_MC_RAW_CHANNEL:
        case ERF_TYPE_MC_AAL5:
+       case ERF_TYPE_MC_AAL2:
        case ERF_TYPE_COLOR_MC_HDLC_POS:
                /* Extract the Multi Channel header to include it in the pseudo header part */
                if (!wtap_read_bytes(fh, erf_subhdr, sizeof(erf_mc_header_t), err, err_info))
@@ -1494,7 +1559,7 @@ pcap_read_erf_subheader(FILE_T fh, union wtap_pseudo_header *pseudo_header,
                pseudo_header->erf.subhdr.mc_hdr = pntoh32(&erf_subhdr[0]);
                *psize = sizeof(erf_mc_header_t);
                break;
-       case ERF_TYPE_MC_AAL2:
+       case ERF_TYPE_AAL2:
                /* Extract the AAL2 header to include it in the pseudo header part */
                if (!wtap_read_bytes(fh, erf_subhdr, sizeof(erf_aal2_header_t), err, err_info))
                        return FALSE;
@@ -1504,6 +1569,7 @@ pcap_read_erf_subheader(FILE_T fh, union wtap_pseudo_header *pseudo_header,
        case ERF_TYPE_ETH:
        case ERF_TYPE_COLOR_ETH:
        case ERF_TYPE_DSM_COLOR_ETH:
+       case ERF_TYPE_COLOR_HASH_ETH:
                /* Extract the Ethernet additional header to include it in the pseudo header part */
                if (!wtap_read_bytes(fh, erf_subhdr, sizeof(erf_eth_header_t), err, err_info))
                        return FALSE;
@@ -1736,7 +1802,7 @@ pcap_process_pseudo_header(FILE_T fh, int file_type, int wtap_encap,
        case WTAP_ENCAP_NFC_LLCP:
                if (check_packet_size && packet_size < LLCP_HEADER_LEN) {
                        *err = WTAP_ERR_BAD_FILE;
-                       *err_info = g_strdup_printf("pcap: libpcap llcp file too short");
+                       *err_info = g_strdup("pcap: libpcap llcp file too short");
                        return -1;
                }
                if (!pcap_read_llcp_pseudoheader(fh, &phdr->pseudo_header, err, err_info))
@@ -1867,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);
@@ -1891,6 +1962,16 @@ pcap_read_post_process(int file_type, int wtap_encap,
                        pcap_byteswap_nflog_pseudoheader(phdr, pd);
                break;
 
+       case WTAP_ENCAP_ERF:
+               /*
+                * Update packet size to account for ERF padding and snapping.
+                * Captured length is minimum of wlen and previously calculated
+                * caplen (which would have included padding but not phdr).
+                */
+               phdr->len = phdr->pseudo_header.erf.phdr.wlen;
+               phdr->caplen = MIN(phdr->len, phdr->caplen);
+               break;
+
        default:
                break;
        }
@@ -1936,10 +2017,14 @@ pcap_get_phdr_size(int encap, const union wtap_pseudo_header *pseudo_header)
                case ERF_TYPE_COLOR_MC_HDLC_POS:
                        hdrsize += (int)sizeof(struct erf_mc_hdr);
                        break;
+               case ERF_TYPE_AAL2:
+                       hdrsize += (int)sizeof(struct erf_aal2_hdr);
+                       break;
 
                case ERF_TYPE_ETH:
                case ERF_TYPE_COLOR_ETH:
                case ERF_TYPE_DSM_COLOR_ETH:
+               case ERF_TYPE_COLOR_HASH_ETH:
                        hdrsize += (int)sizeof(struct erf_eth_hdr);
                        break;
 
@@ -1998,11 +2083,13 @@ pcap_write_phdr(wtap_dumper *wdh, int encap, const union wtap_pseudo_header *pse
        guint8 mtp2_hdr[MTP2_HDR_LEN];
        guint8 sita_hdr[SITA_HDR_LEN];
        guint8 erf_hdr[ sizeof(struct erf_mc_phdr)];
+       guint8 erf_subhdr[sizeof(union erf_subhdr)];
        struct i2c_file_hdr i2c_hdr;
        struct libpcap_bt_phdr bt_hdr;
        struct libpcap_bt_monitor_phdr bt_monitor_hdr;
        struct libpcap_ppp_phdr ppp_hdr;
        size_t size;
+       size_t subhdr_size = 0;
 
        switch (encap) {
 
@@ -2111,7 +2198,17 @@ pcap_write_phdr(wtap_dumper *wdh, int encap, const union wtap_pseudo_header *pse
                phtolell(&erf_hdr[0], pseudo_header->erf.phdr.ts);
                erf_hdr[8] = pseudo_header->erf.phdr.type;
                erf_hdr[9] = pseudo_header->erf.phdr.flags;
-               phtons(&erf_hdr[10], pseudo_header->erf.phdr.rlen);
+
+               /*
+                * Recalculate rlen as padding (and maybe extension headers)
+                * have been stripped from caplen.
+                *
+                * XXX: Since we don't have phdr->caplen here, assume caplen was
+                * calculated correctly and recalculate from wlen.
+                */
+               phtons(&erf_hdr[10],
+                       MIN(pseudo_header->erf.phdr.rlen, pseudo_header->erf.phdr.wlen + pcap_get_phdr_size(WTAP_ENCAP_ERF, pseudo_header)));
+
                phtons(&erf_hdr[12], pseudo_header->erf.phdr.lctr);
                phtons(&erf_hdr[14], pseudo_header->erf.phdr.wlen);
                size = sizeof(struct erf_phdr);
@@ -2122,19 +2219,21 @@ pcap_write_phdr(wtap_dumper *wdh, int encap, const union wtap_pseudo_header *pse
                case ERF_TYPE_MC_ATM:
                case ERF_TYPE_MC_RAW_CHANNEL:
                case ERF_TYPE_MC_AAL5:
+               case ERF_TYPE_MC_AAL2:
                case ERF_TYPE_COLOR_MC_HDLC_POS:
-                       phtonl(&erf_hdr[16], pseudo_header->erf.subhdr.mc_hdr);
-                       size += (int)sizeof(struct erf_mc_hdr);
+                       phtonl(&erf_subhdr[0], pseudo_header->erf.subhdr.mc_hdr);
+                       subhdr_size += (int)sizeof(struct erf_mc_hdr);
                        break;
-               case ERF_TYPE_MC_AAL2:
-                       phtonl(&erf_hdr[16], pseudo_header->erf.subhdr.aal2_hdr);
-                       size += (int)sizeof(struct erf_aal2_hdr);
+               case ERF_TYPE_AAL2:
+                       phtonl(&erf_subhdr[0], pseudo_header->erf.subhdr.aal2_hdr);
+                       subhdr_size += (int)sizeof(struct erf_aal2_hdr);
                        break;
                case ERF_TYPE_ETH:
                case ERF_TYPE_COLOR_ETH:
                case ERF_TYPE_DSM_COLOR_ETH:
-                       memcpy(&erf_hdr[16], &pseudo_header->erf.subhdr.eth_hdr, sizeof pseudo_header->erf.subhdr.eth_hdr);
-                       size += (int)sizeof(struct erf_eth_hdr);
+               case ERF_TYPE_COLOR_HASH_ETH:
+                       memcpy(&erf_subhdr[0], &pseudo_header->erf.subhdr.eth_hdr, sizeof pseudo_header->erf.subhdr.eth_hdr);
+                       subhdr_size += (int)sizeof(struct erf_eth_hdr);
                        break;
                default:
                        break;
@@ -2154,12 +2253,23 @@ pcap_write_phdr(wtap_dumper *wdh, int encap, const union wtap_pseudo_header *pse
                        do {
                                phtonll(erf_exhdr, pseudo_header->erf.ehdr_list[i].ehdr);
                                type = erf_exhdr[0];
+                               /* Clear more extension headers bit if > 8 */
+                               if(i == max-1)
+                                       erf_exhdr[0] = erf_exhdr[0] & 0x7F;
+
                                if (!wtap_dump_file_write(wdh, erf_exhdr, 8, err))
                                        return FALSE;
                                wdh->bytes_dumped += 8;
                                i++;
                        } while (type & 0x80 && i < max);
                }
+
+               /*
+                * Now write out the subheader.
+                */
+               if(!wtap_dump_file_write(wdh, erf_subhdr, subhdr_size, err))
+                       return FALSE;
+               wdh->bytes_dumped += subhdr_size;
                break;
 
        case WTAP_ENCAP_I2C: