Skip to content

Commit b6791ff

Browse files
dpgeorgedpgeorge
committedFeb 26, 2019
lib/netutils: Add function to print tracing info for Ethernet frames.
·
vc4-early2vc
1 parent 39ea132 commit b6791ff

File tree

2 files changed

+156
-0
lines changed

2 files changed

+156
-0
lines changed
 

‎lib/netutils/netutils.h

+6
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,10 @@
2929

3030
#define NETUTILS_IPV4ADDR_BUFSIZE 4
3131

32+
#define NETUTILS_TRACE_IS_TX (0x0001)
33+
#define NETUTILS_TRACE_PAYLOAD (0x0002)
34+
#define NETUTILS_TRACE_NEWLINE (0x0004)
35+
3236
typedef enum _netutils_endian_t {
3337
NETUTILS_LITTLE,
3438
NETUTILS_BIG,
@@ -47,4 +51,6 @@ void netutils_parse_ipv4_addr(mp_obj_t addr_in, uint8_t *out_ip, netutils_endian
4751
// puts IP in out_ip (which must take at least IPADDR_BUF_SIZE bytes).
4852
mp_uint_t netutils_parse_inet_addr(mp_obj_t addr_in, uint8_t *out_ip, netutils_endian_t endian);
4953

54+
void netutils_ethernet_trace(const mp_print_t *print, size_t len, const uint8_t *buf, unsigned int flags);
55+
5056
#endif // MICROPY_INCLUDED_LIB_NETUTILS_NETUTILS_H

‎lib/netutils/trace.c

+150
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
/*
2+
* This file is part of the MicroPython project, http://micropython.org/
3+
*
4+
* The MIT License (MIT)
5+
*
6+
* Copyright (c) 2019 Damien P. George
7+
*
8+
* Permission is hereby granted, free of charge, to any person obtaining a copy
9+
* of this software and associated documentation files (the "Software"), to deal
10+
* in the Software without restriction, including without limitation the rights
11+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12+
* copies of the Software, and to permit persons to whom the Software is
13+
* furnished to do so, subject to the following conditions:
14+
*
15+
* The above copyright notice and this permission notice shall be included in
16+
* all copies or substantial portions of the Software.
17+
*
18+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23+
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
24+
* THE SOFTWARE.
25+
*/
26+
27+
#include "py/mphal.h"
28+
#include "lib/netutils/netutils.h"
29+
30+
static uint32_t get_be16(const uint8_t *buf) {
31+
return buf[0] << 8 | buf[1];
32+
}
33+
34+
static uint32_t get_be32(const uint8_t *buf) {
35+
return buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
36+
}
37+
38+
static void dump_hex_bytes(const mp_print_t *print, size_t len, const uint8_t *buf) {
39+
for (size_t i = 0; i < len; ++i) {
40+
mp_printf(print, " %02x", buf[i]);
41+
}
42+
}
43+
44+
static const char *ethertype_str(uint16_t type) {
45+
// A value between 0x0000 - 0x05dc (inclusive) indicates a length, not type
46+
switch (type) {
47+
case 0x0800: return "IPv4";
48+
case 0x0806: return "ARP";
49+
case 0x86dd: return "IPv6";
50+
default: return NULL;
51+
}
52+
}
53+
54+
void netutils_ethernet_trace(const mp_print_t *print, size_t len, const uint8_t *buf, unsigned int flags) {
55+
mp_printf(print, "[% 8d] ETH%cX len=%u", mp_hal_ticks_ms(), flags & NETUTILS_TRACE_IS_TX ? 'T' : 'R', len);
56+
mp_printf(print, " dst=%02x:%02x:%02x:%02x:%02x:%02x", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
57+
mp_printf(print, " src=%02x:%02x:%02x:%02x:%02x:%02x", buf[6], buf[7], buf[8], buf[9], buf[10], buf[11]);
58+
59+
const char *ethertype = ethertype_str(buf[12] << 8 | buf[13]);
60+
if (ethertype) {
61+
mp_printf(print, " type=%s", ethertype);
62+
} else {
63+
mp_printf(print, " type=0x%04x", buf[12] << 8 | buf[13]);
64+
}
65+
if (len > 14) {
66+
len -= 14;
67+
buf += 14;
68+
if (buf[-2] == 0x08 && buf[-1] == 0x00 && buf[0] == 0x45) {
69+
// IPv4 packet
70+
len = get_be16(buf + 2);
71+
mp_printf(print, " srcip=%u.%u.%u.%u dstip=%u.%u.%u.%u",
72+
buf[12], buf[13], buf[14], buf[15],
73+
buf[16], buf[17], buf[18], buf[19]);
74+
uint8_t prot = buf[9];
75+
buf += 20;
76+
len -= 20;
77+
if (prot == 6) {
78+
// TCP packet
79+
uint16_t srcport = get_be16(buf);
80+
uint16_t dstport = get_be16(buf + 2);
81+
uint32_t seqnum = get_be32(buf + 4);
82+
uint32_t acknum = get_be32(buf + 8);
83+
uint16_t dataoff_flags = get_be16(buf + 12);
84+
uint16_t winsz = get_be16(buf + 14);
85+
mp_printf(print, " TCP srcport=%u dstport=%u seqnum=%u acknum=%u dataoff=%u flags=%x winsz=%u",
86+
srcport, dstport, (unsigned)seqnum, (unsigned)acknum, dataoff_flags >> 12, dataoff_flags & 0x1ff, winsz);
87+
buf += 20;
88+
len -= 20;
89+
if (dataoff_flags >> 12 > 5) {
90+
mp_printf(print, " opts=");
91+
size_t opts_len = ((dataoff_flags >> 12) - 5) * 4;
92+
dump_hex_bytes(print, opts_len, buf);
93+
buf += opts_len;
94+
len -= opts_len;
95+
}
96+
} else if (prot == 17) {
97+
// UDP packet
98+
uint16_t srcport = get_be16(buf);
99+
uint16_t dstport = get_be16(buf + 2);
100+
mp_printf(print, " UDP srcport=%u dstport=%u", srcport, dstport);
101+
len = get_be16(buf + 4);
102+
buf += 8;
103+
if ((srcport == 67 && dstport == 68) || (srcport == 68 && dstport == 67)) {
104+
// DHCP
105+
if (srcport == 67) {
106+
mp_printf(print, " DHCPS");
107+
} else {
108+
mp_printf(print, " DHCPC");
109+
}
110+
dump_hex_bytes(print, 12 + 16 + 16 + 64, buf);
111+
size_t n = 12 + 16 + 16 + 64 + 128;
112+
len -= n;
113+
buf += n;
114+
mp_printf(print, " opts:");
115+
switch (buf[6]) {
116+
case 1: mp_printf(print, " DISCOVER"); break;
117+
case 2: mp_printf(print, " OFFER"); break;
118+
case 3: mp_printf(print, " REQUEST"); break;
119+
case 4: mp_printf(print, " DECLINE"); break;
120+
case 5: mp_printf(print, " ACK"); break;
121+
case 6: mp_printf(print, " NACK"); break;
122+
case 7: mp_printf(print, " RELEASE"); break;
123+
case 8: mp_printf(print, " INFORM"); break;
124+
}
125+
}
126+
} else {
127+
// Non-UDP packet
128+
mp_printf(print, " prot=%u", prot);
129+
}
130+
} else if (buf[-2] == 0x86 && buf[-1] == 0xdd && (buf[0] >> 4) == 6) {
131+
// IPv6 packet
132+
uint32_t h = get_be32(buf);
133+
uint16_t l = get_be16(buf + 4);
134+
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]);
135+
mp_printf(print, " srcip=");
136+
dump_hex_bytes(print, 16, buf + 8);
137+
mp_printf(print, " dstip=");
138+
dump_hex_bytes(print, 16, buf + 24);
139+
buf += 40;
140+
len -= 40;
141+
}
142+
if (flags & NETUTILS_TRACE_PAYLOAD) {
143+
mp_printf(print, " data=");
144+
dump_hex_bytes(print, len, buf);
145+
}
146+
}
147+
if (flags & NETUTILS_TRACE_NEWLINE) {
148+
mp_printf(print, "\n");
149+
}
150+
}

0 commit comments

Comments
 (0)
Please sign in to comment.