print_802_15_4.c: extend 802.15.4 printer to dump header information

Signed-off-by: Dmitry Eremin-Solenikov <[email protected]>

diff --git a/print-802_15_4.c b/print-802_15_4.c
index 57bc7cfebedb84530b872effc91cdbc7902a6fe6..a5364124338b5b8e4928993fd07b0f4f7d68bb35 100644 (file)
--- a/print-802_15_4.c
+++ b/print-802_15_4.c
@@ -30,9 +30,144 @@
 #include <pcap.h>
 #include <string.h>
 
+#include "interface.h"
+#include "addrtoname.h"
+
+#include "extract.h"
+
+static const char *ftypes[] = {
+       "Beacon",                       /* 0 */
+       "Data",                         /* 1 */
+       "ACK",                          /* 2 */
+       "Command",                      /* 3 */
+       "Reserved",                     /* 4 */
+       "Reserved",                     /* 5 */
+       "Reserved",                     /* 6 */
+       "Reserved",                     /* 7 */
+};
+
+static int
+extract_header_length(u_int16_t fc)
+{
+       int len = 0;
+
+       switch ((fc >> 10) & 0x3) {
+       case 0x0:
+               if (fc & (1 << 6)) /* intra-PAN with none dest addr */
+                       return -1;
+               break;
+       case 0x1:
+               return -1;
+       case 0x02:
+               len += 4;
+               break;
+       case 0x03:
+               len += 10;
+               break;
+       }
+
+       switch ((fc >> 14) & 0x3) {
+       case 0x0:
+               break;
+       case 0x1:
+               return -1;
+       case 0x02:
+               len += 4;
+               break;
+       case 0x03:
+               len += 10;
+               break;
+       }
+
+       if (fc & (1 << 6))
+               len -= 2;
+
+       return len;
+}
+
+
 u_int
 ieee802_15_4_if_print(const struct pcap_pkthdr *h, const u_char *p)
 {
-       printf("IEEE 802.15.4 packet");
+       u_int caplen = h->caplen;
+       u_int hdrlen;
+       u_int16_t fc;
+       u_int8_t seq;
+
+       if (caplen < 3) {
+               printf("[|802.15.4] %x", caplen);
+               return caplen;
+       }
+
+       fc = EXTRACT_LE_16BITS(p);
+       hdrlen = extract_header_length(fc);
+
+       seq = EXTRACT_LE_8BITS(p + 2);
+
+       p += 3;
+       caplen -= 3;
+
+       printf("IEEE 802.15.4 %s packet ", ftypes[fc & 0x7]);
+       if (vflag)
+               printf("seq %02x ", seq);
+       if (hdrlen == -1) {
+               printf("malformed! ");
+               return caplen;
+       }
+
+
+       if (!vflag) {
+               p+= hdrlen;
+               caplen -= hdrlen;
+       } else {
+               u_int16_t panid;
+
+               switch ((fc >> 10) & 0x3) {
+               case 0x00:
+                       printf("none ");
+                       break;
+               case 0x02:
+                       panid = EXTRACT_LE_16BITS(p);
+                       p += 2;
+                       printf("%04x:%04x ", panid, EXTRACT_LE_16BITS(p));
+                       p += 2;
+                       break;
+               case 0x03:
+                       panid = EXTRACT_LE_16BITS(p);
+                       p += 2;
+                       printf("%04x:%s ", panid, le64addr_string(p));
+                       p += 8;
+                       break;
+               }
+               printf("< ");
+
+               switch ((fc >> 14) & 0x3) {
+               case 0x00:
+                       printf("none ");
+                       break;
+               case 0x02:
+                       if (!(fc & (1 << 6))) {
+                               panid = EXTRACT_LE_16BITS(p);
+                               p += 2;
+                       }
+                       printf("%04x:%04x ", panid, EXTRACT_LE_16BITS(p));
+                       p += 2;
+                       break;
+               case 0x03:
+                       if (!(fc & (1 << 6))) {
+                               panid = EXTRACT_LE_16BITS(p);
+                               p += 2;
+                       }
+                       printf("%04x:%s ", panid, le64addr_string(p));
+                       p += 8;
+                       break;
+               }
+
+               caplen -= hdrlen;
+       }
+
+       if (!suppress_default_print)
+               default_print(p, caplen);
+
        return 0;
 }