lib/netutils: Add function to print tracing info for Ethernet frames.

2 files changed, 156 insertions, 0 deletions

diff --git a/lib/netutils/netutils.h b/lib/netutils/netutils.h
index 4befc90db..926133785 100644
--- a/lib/netutils/netutils.h
+++ b/lib/netutils/netutils.h
@@ -29,6 +29,10 @@
 
 #define NETUTILS_IPV4ADDR_BUFSIZE    4
 
+#define NETUTILS_TRACE_IS_TX        (0x0001)
+#define NETUTILS_TRACE_PAYLOAD      (0x0002)
+#define NETUTILS_TRACE_NEWLINE      (0x0004)
+
 typedef enum _netutils_endian_t {
     NETUTILS_LITTLE,
     NETUTILS_BIG,
@@ -47,4 +51,6 @@ void netutils_parse_ipv4_addr(mp_obj_t addr_in, uint8_t *out_ip, netutils_endian
 // puts IP in out_ip (which must take at least IPADDR_BUF_SIZE bytes).
 mp_uint_t netutils_parse_inet_addr(mp_obj_t addr_in, uint8_t *out_ip, netutils_endian_t endian);
 
+void netutils_ethernet_trace(const mp_print_t *print, size_t len, const uint8_t *buf, unsigned int flags);
+
 #endif // MICROPY_INCLUDED_LIB_NETUTILS_NETUTILS_H
diff --git a/lib/netutils/trace.c b/lib/netutils/trace.c
new file mode 100644
index 000000000..7c79713b3
--- /dev/null
+++ b/lib/netutils/trace.c
@@ -0,0 +1,150 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2019 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "py/mphal.h"
+#include "lib/netutils/netutils.h"
+
+static uint32_t get_be16(const uint8_t *buf) {
+    return buf[0] << 8 | buf[1];
+}
+
+static uint32_t get_be32(const uint8_t *buf) {
+    return buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
+}
+
+static void dump_hex_bytes(const mp_print_t *print, size_t len, const uint8_t *buf) {
+    for (size_t i = 0; i < len; ++i) {
+        mp_printf(print, " %02x", buf[i]);
+    }
+}
+
+static const char *ethertype_str(uint16_t type) {
+    // A value between 0x0000 - 0x05dc (inclusive) indicates a length, not type
+    switch (type) {
+        case 0x0800: return "IPv4";
+        case 0x0806: return "ARP";
+        case 0x86dd: return "IPv6";
+        default: return NULL;
+    }
+}
+
+void netutils_ethernet_trace(const mp_print_t *print, size_t len, const uint8_t *buf, unsigned int flags) {
+    mp_printf(print, "[% 8d] ETH%cX len=%u", mp_hal_ticks_ms(), flags & NETUTILS_TRACE_IS_TX ? 'T' : 'R', len);
+    mp_printf(print, " dst=%02x:%02x:%02x:%02x:%02x:%02x", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+    mp_printf(print, " src=%02x:%02x:%02x:%02x:%02x:%02x", buf[6], buf[7], buf[8], buf[9], buf[10], buf[11]);
+
+    const char *ethertype = ethertype_str(buf[12] << 8 | buf[13]);
+    if (ethertype) {
+        mp_printf(print, " type=%s", ethertype);
+    } else {
+        mp_printf(print, " type=0x%04x", buf[12] << 8 | buf[13]);
+    }
+    if (len > 14) {
+        len -= 14;
+        buf += 14;
+        if (buf[-2] == 0x08 && buf[-1] == 0x00 && buf[0] == 0x45) {
+            // IPv4 packet
+            len = get_be16(buf + 2);
+            mp_printf(print, " srcip=%u.%u.%u.%u dstip=%u.%u.%u.%u",
+                buf[12], buf[13], buf[14], buf[15],
+                buf[16], buf[17], buf[18], buf[19]);
+            uint8_t prot = buf[9];
+            buf += 20;
+            len -= 20;
+            if (prot == 6) {
+                // TCP packet
+                uint16_t srcport = get_be16(buf);
+                uint16_t dstport = get_be16(buf + 2);
+                uint32_t seqnum = get_be32(buf + 4);
+                uint32_t acknum = get_be32(buf + 8);
+                uint16_t dataoff_flags = get_be16(buf + 12);
+                uint16_t winsz = get_be16(buf + 14);
+                mp_printf(print, " TCP srcport=%u dstport=%u seqnum=%u acknum=%u dataoff=%u flags=%x winsz=%u",
+                    srcport, dstport, (unsigned)seqnum, (unsigned)acknum, dataoff_flags >> 12, dataoff_flags & 0x1ff, winsz);
+                buf += 20;
+                len -= 20;
+                if (dataoff_flags >> 12 > 5) {
+                    mp_printf(print, " opts=");
+                    size_t opts_len = ((dataoff_flags >> 12) - 5) * 4;
+                    dump_hex_bytes(print, opts_len, buf);
+                    buf += opts_len;
+                    len -= opts_len;
+                }
+            } else if (prot == 17) {
+                // UDP packet
+                uint16_t srcport = get_be16(buf);
+                uint16_t dstport = get_be16(buf + 2);
+                mp_printf(print, " UDP srcport=%u dstport=%u", srcport, dstport);
+                len = get_be16(buf + 4);
+                buf += 8;
+                if ((srcport == 67 && dstport == 68) || (srcport == 68 && dstport == 67)) {
+                    // DHCP
+                    if (srcport == 67) {
+                        mp_printf(print, " DHCPS");
+                    } else {
+                        mp_printf(print, " DHCPC");
+                    }
+                    dump_hex_bytes(print, 12 + 16 + 16 + 64, buf);
+                    size_t n = 12 + 16 + 16 + 64 + 128;
+                    len -= n;
+                    buf += n;
+                    mp_printf(print, " opts:");
+                    switch (buf[6]) {
+                        case 1: mp_printf(print, " DISCOVER"); break;
+                        case 2: mp_printf(print, " OFFER"); break;
+                        case 3: mp_printf(print, " REQUEST"); break;
+                        case 4: mp_printf(print, " DECLINE"); break;
+                        case 5: mp_printf(print, " ACK"); break;
+                        case 6: mp_printf(print, " NACK"); break;
+                        case 7: mp_printf(print, " RELEASE"); break;
+                        case 8: mp_printf(print, " INFORM"); break;
+                    }
+                }
+            } else {
+                // Non-UDP packet
+                mp_printf(print, " prot=%u", prot);
+            }
+        } else if (buf[-2] == 0x86 && buf[-1] == 0xdd && (buf[0] >> 4) == 6) {
+            // IPv6 packet
+            uint32_t h = get_be32(buf);
+            uint16_t l = get_be16(buf + 4);
+            mp_printf(print, " tclass=%u flow=%u len=%u nexthdr=%u hoplimit=%u", (unsigned)((h >> 20) & 0xff), (unsigned)(h & 0xfffff), l, buf[6], buf[7]);
+            mp_printf(print, " srcip=");
+            dump_hex_bytes(print, 16, buf + 8);
+            mp_printf(print, " dstip=");
+            dump_hex_bytes(print, 16, buf + 24);
+            buf += 40;
+            len -= 40;
+        }
+        if (flags & NETUTILS_TRACE_PAYLOAD) {
+            mp_printf(print, " data=");
+            dump_hex_bytes(print, len, buf);
+        }
+    }
+    if (flags & NETUTILS_TRACE_NEWLINE) {
+        mp_printf(print, "\n");
+    }
+}