print_802_15_4.c: extend 802.15.4 printer to dump header information
Signed-off-by: Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
diff --git a/print-802_15_4.c b/print-802_15_4.c
index 57bc7cf..a536412 100644
--- 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;
}
