1
2 // Copyright 2012-2013 Ettus Research LLC
3
4 #include <u3_net_stack.h>
5 #include <string.h> //memcmp
6 #include <trace.h>
7
8 #define MAX_NETHS 4
9
10 typedef struct
11 {
12 padded_eth_hdr_t eth;
13 struct arp_eth_ipv4 arp;
14 } padded_arp_t;
15
16 typedef struct
17 {
18 padded_eth_hdr_t eth;
19 struct ip_hdr ip;
20 struct icmp_echo_hdr icmp;
21 } padded_icmp_t;
22
23 typedef struct
24 {
25 padded_eth_hdr_t eth;
26 struct ip_hdr ip;
27 struct udp_hdr udp;
28 } padded_udp_t;
29
30 /***********************************************************************
31 * declares for internal handlers
32 **********************************************************************/
handle_icmp_echo_packet(const uint8_t ethno,const struct ip_addr * src,const struct ip_addr * dst,const uint16_t id,const uint16_t seq,const void * buff,const size_t num_bytes)33 static void handle_icmp_echo_packet(
34 const uint8_t ethno,
35 const struct ip_addr *src, const struct ip_addr *dst,
36 const uint16_t id, const uint16_t seq,
37 const void *buff, const size_t num_bytes
38 ){
39 u3_net_stack_send_icmp_pkt(ethno, ICMP_ER, 0, id, seq, src, buff, num_bytes);
40 }
41
42 static void handle_icmp_dur_packet(
43 const uint8_t ethno,
44 const struct ip_addr *src, const struct ip_addr *dst,
45 const uint16_t id, const uint16_t seq,
46 const void *buff, const size_t num_bytes
47 );
48
49 /***********************************************************************
50 * 16-bit one's complement sum
51 **********************************************************************/
chksum_buffer(uint16_t * buf,size_t nshorts,uint32_t initial_chksum)52 static uint32_t chksum_buffer(
53 uint16_t *buf, size_t nshorts,
54 uint32_t initial_chksum
55 ){
56 uint32_t chksum = initial_chksum;
57 for (size_t i = 0; i < nshorts; i++) chksum += buf[i];
58
59 while (chksum >> 16) chksum = (chksum & 0xffff) + (chksum >> 16);
60
61 return chksum;
62 }
63
64 /***********************************************************************
65 * ARP Cache implementation
66 **********************************************************************/
67 #define ARP_CACHE_NENTRIES 32
68
69 static size_t arp_cache_wr_index;
70
71 static struct ip_addr arp_cache_ips[ARP_CACHE_NENTRIES];
72 static eth_mac_addr_t arp_cache_macs[ARP_CACHE_NENTRIES];
73 static uint8_t arp_cache_eths[ARP_CACHE_NENTRIES];
74
u3_net_stack_arp_cache_update(const struct ip_addr * ip_addr,const eth_mac_addr_t * mac_addr,const uint8_t ethno)75 void u3_net_stack_arp_cache_update(const struct ip_addr *ip_addr, const eth_mac_addr_t *mac_addr, const uint8_t ethno)
76 {
77 for (size_t i = 0; i < ARP_CACHE_NENTRIES; i++)
78 {
79 if (memcmp(ip_addr, arp_cache_ips+i, sizeof(struct ip_addr)) == 0)
80 {
81 memcpy(arp_cache_macs+i, mac_addr, sizeof(eth_mac_addr_t));
82 arp_cache_eths[i] = ethno;
83 return;
84 }
85 }
86 if (arp_cache_wr_index >= ARP_CACHE_NENTRIES) arp_cache_wr_index = 0;
87 memcpy(arp_cache_ips+arp_cache_wr_index, ip_addr, sizeof(struct ip_addr));
88 memcpy(arp_cache_macs+arp_cache_wr_index, mac_addr, sizeof(eth_mac_addr_t));
89 arp_cache_eths[arp_cache_wr_index] = ethno;
90 arp_cache_wr_index++;
91 }
92
u3_net_stack_arp_cache_lookup(const struct ip_addr * ip_addr)93 const eth_mac_addr_t *u3_net_stack_arp_cache_lookup(const struct ip_addr *ip_addr)
94 {
95 //do a local look up on our own ports
96 for (size_t e = 0; e < MAX_NETHS; e++)
97 {
98 if (memcmp(ip_addr, u3_net_stack_get_ip_addr(e), sizeof(struct ip_addr)) == 0)
99 {
100 return u3_net_stack_get_mac_addr(e);
101 }
102 }
103 //now check the arp cache
104 for (size_t i = 0; i < ARP_CACHE_NENTRIES; i++)
105 {
106 if (memcmp(ip_addr, arp_cache_ips+i, sizeof(struct ip_addr)) == 0)
107 {
108 return &arp_cache_macs[i];
109 }
110 }
111 return NULL;
112 }
113
resolve_ip(const struct ip_addr * ip_addr,eth_mac_addr_t * mac_addr)114 bool resolve_ip(const struct ip_addr *ip_addr, eth_mac_addr_t *mac_addr)
115 {
116 for (size_t e = 0; e < MAX_NETHS; e++)
117 {
118 if (memcmp(u3_net_stack_get_bcast(e), ip_addr, sizeof(struct ip_addr)) == 0)
119 {
120 memset(mac_addr, 0xff, sizeof(eth_mac_addr_t));
121 return true;
122 }
123 }
124 const eth_mac_addr_t *r = u3_net_stack_arp_cache_lookup(ip_addr);
125 if (r != NULL)
126 {
127 memcpy(mac_addr, r, sizeof(eth_mac_addr_t));
128 return true;
129 }
130 return false;
131 }
132
133 /***********************************************************************
134 * Net stack config
135 **********************************************************************/
136 static wb_pkt_iface64_config_t *pkt_iface_config = NULL;
137
u3_net_stack_init(wb_pkt_iface64_config_t * config)138 void u3_net_stack_init(wb_pkt_iface64_config_t *config)
139 {
140 pkt_iface_config = config;
141 u3_net_stack_register_icmp_handler(ICMP_ECHO, 0, &handle_icmp_echo_packet);
142 u3_net_stack_register_icmp_handler(ICMP_DUR, ICMP_DUR_PORT, &handle_icmp_dur_packet);
143 }
144
145 static struct ip_addr net_conf_ips[MAX_NETHS];
146 static eth_mac_addr_t net_conf_macs[MAX_NETHS];
147 static struct ip_addr net_conf_subnets[MAX_NETHS];
148 static struct ip_addr net_conf_bcasts[MAX_NETHS];
149 static uint32_t net_stat_counts[MAX_NETHS];
150
u3_net_stack_init_eth(const uint8_t ethno,const eth_mac_addr_t * mac,const struct ip_addr * ip,const struct ip_addr * subnet)151 void u3_net_stack_init_eth(
152 const uint8_t ethno,
153 const eth_mac_addr_t *mac,
154 const struct ip_addr *ip,
155 const struct ip_addr *subnet
156 )
157 {
158 memcpy(&net_conf_macs[ethno], mac, sizeof(eth_mac_addr_t));
159 memcpy(&net_conf_ips[ethno], ip, sizeof(struct ip_addr));
160 memcpy(&net_conf_subnets[ethno], subnet, sizeof(struct ip_addr));
161 net_stat_counts[ethno] = 0;
162 }
163
u3_net_stack_get_ip_addr(const uint8_t ethno)164 const struct ip_addr *u3_net_stack_get_ip_addr(const uint8_t ethno)
165 {
166 return &net_conf_ips[ethno];
167 }
168
u3_net_stack_get_subnet(const uint8_t ethno)169 const struct ip_addr *u3_net_stack_get_subnet(const uint8_t ethno)
170 {
171 return &net_conf_subnets[ethno];
172 }
173
u3_net_stack_get_bcast(const uint8_t ethno)174 const struct ip_addr *u3_net_stack_get_bcast(const uint8_t ethno)
175 {
176 const uint32_t ip = u3_net_stack_get_ip_addr(ethno)->addr;
177 const uint32_t subnet = u3_net_stack_get_subnet(ethno)->addr;
178 net_conf_bcasts[ethno].addr = ip | (~subnet);
179 return &net_conf_bcasts[ethno];
180 }
181
u3_net_stack_get_mac_addr(const uint8_t ethno)182 const eth_mac_addr_t *u3_net_stack_get_mac_addr(const uint8_t ethno)
183 {
184 return &net_conf_macs[ethno];
185 }
186
187 /***********************************************************************
188 * Ethernet activity stats
189 **********************************************************************/
u3_net_stack_get_stat_counts(const uint8_t ethno)190 uint32_t u3_net_stack_get_stat_counts(const uint8_t ethno)
191 {
192 return net_stat_counts[ethno];
193 }
194
incr_stat_counts(const void * p)195 static void incr_stat_counts(const void *p)
196 {
197 const padded_eth_hdr_t *eth = (const padded_eth_hdr_t *)p;
198 if (eth->ethno < MAX_NETHS) net_stat_counts[eth->ethno]++;
199 }
200
201 /***********************************************************************
202 * Ethernet handlers - send packet w/ payload
203 **********************************************************************/
send_eth_pkt(const void * p0,const size_t l0,const void * p1,const size_t l1,const void * p2,const size_t l2)204 static void send_eth_pkt(
205 const void *p0, const size_t l0,
206 const void *p1, const size_t l1,
207 const void *p2, const size_t l2
208 )
209 {
210 incr_stat_counts(p0);
211 void *ptr = wb_pkt_iface64_tx_claim(pkt_iface_config);
212 size_t buff_i = 0;
213
214 uint32_t *buff32 = (uint32_t *)ptr;
215 for (size_t i = 0; i < (l0+3)/4; i++)
216 {
217 buff32[buff_i++] = ((const uint32_t *)p0)[i];
218 }
219 for (size_t i = 0; i < (l1+3)/4; i++)
220 {
221 buff32[buff_i++] = ((const uint32_t *)p1)[i];
222 }
223 for (size_t i = 0; i < (l2+3)/4; i++)
224 {
225 buff32[buff_i++] = ((const uint32_t *)p2)[i];
226 }
227
228 // Fixes issue where we don't write an even number of 32bit words leaving last data stranded in H/W.
229 if ((buff_i%2) == 1) buff32[buff_i++] = 0;
230
231 wb_pkt_iface64_tx_submit(pkt_iface_config, l0 + l1 + l2);
232 }
233
234 /***********************************************************************
235 * ARP handlers
236 **********************************************************************/
send_arp_reply(const uint8_t ethno,const struct arp_eth_ipv4 * req,const eth_mac_addr_t * our_mac)237 static void send_arp_reply(
238 const uint8_t ethno,
239 const struct arp_eth_ipv4 *req,
240 const eth_mac_addr_t *our_mac
241 ){
242 padded_arp_t reply;
243 reply.eth.ethno = ethno;
244 memcpy(&reply.eth.dst, (eth_mac_addr_t *)req->ar_sha, sizeof(eth_mac_addr_t));
245 memcpy(&reply.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t));
246 reply.eth.ethertype = ETHERTYPE_ARP;
247
248 reply.arp.ar_hrd = req->ar_hrd;
249 reply.arp.ar_pro = req->ar_pro;
250 reply.arp.ar_hln = req->ar_hln;
251 reply.arp.ar_pln = req->ar_pln;
252 reply.arp.ar_op = ARPOP_REPLY;
253 memcpy(reply.arp.ar_sha, our_mac, sizeof(eth_mac_addr_t));
254 memcpy(reply.arp.ar_sip, req->ar_tip, sizeof(struct ip_addr));
255 memcpy(reply.arp.ar_tha, req->ar_sha, sizeof(eth_mac_addr_t));
256 memcpy(reply.arp.ar_tip, req->ar_sip, sizeof(struct ip_addr));
257
258 send_eth_pkt(&reply, sizeof(reply), NULL, 0, NULL, 0);
259 }
260
u3_net_stack_send_arp_request(const uint8_t ethno,const struct ip_addr * addr)261 void u3_net_stack_send_arp_request(const uint8_t ethno, const struct ip_addr *addr)
262 {
263 padded_arp_t req;
264 req.eth.ethno = ethno;
265 memset(&req.eth.dst, 0xff, sizeof(eth_mac_addr_t)); //bcast
266 memcpy(&req.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t));
267 req.eth.ethertype = ETHERTYPE_ARP;
268
269 req.arp.ar_hrd = ARPHRD_ETHER;
270 req.arp.ar_pro = ETHERTYPE_IPV4;
271 req.arp.ar_hln = sizeof(eth_mac_addr_t);
272 req.arp.ar_pln = sizeof(struct ip_addr);
273 req.arp.ar_op = ARPOP_REQUEST;
274 memcpy(req.arp.ar_sha, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t));
275 memcpy(req.arp.ar_sip, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr));
276 memset(req.arp.ar_tha, 0x00, sizeof(eth_mac_addr_t));
277 memcpy(req.arp.ar_tip, addr, sizeof(struct ip_addr));
278
279 send_eth_pkt(&req, sizeof(req), NULL, 0, NULL, 0);
280 }
281
handle_arp_packet(const uint8_t ethno,const struct arp_eth_ipv4 * p)282 static void handle_arp_packet(const uint8_t ethno, const struct arp_eth_ipv4 *p)
283 {
284 UHD_FW_TRACE(DEBUG, "handle_arp_packet");
285 if (p->ar_hrd != ARPHRD_ETHER
286 || p->ar_pro != ETHERTYPE_IPV4
287 || p->ar_hln != sizeof(eth_mac_addr_t)
288 || p->ar_pln != sizeof(struct ip_addr))
289 return;
290
291 //got an arp reply -- injest it into the arp cache
292 if (p->ar_op == ARPOP_REPLY)
293 {
294 UHD_FW_TRACE(DEBUG, "ARPOP_REPLY");
295 struct ip_addr ip_addr;
296 memcpy(&ip_addr, p->ar_sip, sizeof(ip_addr));
297 eth_mac_addr_t mac_addr;
298 memcpy(&mac_addr, p->ar_sha, sizeof(mac_addr));
299 u3_net_stack_arp_cache_update(&ip_addr, &mac_addr, ethno);
300 }
301
302 //got an arp request -- reply if its for our address
303 if (p->ar_op == ARPOP_REQUEST)
304 {
305 UHD_FW_TRACE(DEBUG, "ARPOP_REQUEST");
306 if (memcmp(p->ar_tip, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr)) == 0)
307 {
308 send_arp_reply(ethno, p, u3_net_stack_get_mac_addr(ethno));
309 }
310 }
311 }
312
313 /***********************************************************************
314 * UDP handlers
315 **********************************************************************/
316 #define UDP_NHANDLERS 16
317
318 static uint16_t udp_handler_ports[UDP_NHANDLERS];
319 static u3_net_stack_udp_handler_t udp_handlers[UDP_NHANDLERS];
320 static size_t udp_handlers_index = 0;
321
u3_net_stack_register_udp_handler(const uint16_t port,const u3_net_stack_udp_handler_t handler)322 void u3_net_stack_register_udp_handler(
323 const uint16_t port,
324 const u3_net_stack_udp_handler_t handler
325 )
326 {
327 if (udp_handlers_index < UDP_NHANDLERS)
328 {
329 udp_handler_ports[udp_handlers_index] = port;
330 udp_handlers[udp_handlers_index] = handler;
331 udp_handlers_index++;
332 }
333 }
334
u3_net_stack_send_udp_pkt(const uint8_t ethno,const struct ip_addr * dst,const uint16_t src_port,const uint16_t dst_port,const void * buff,const size_t num_bytes)335 void u3_net_stack_send_udp_pkt(
336 const uint8_t ethno,
337 const struct ip_addr *dst,
338 const uint16_t src_port,
339 const uint16_t dst_port,
340 const void *buff,
341 const size_t num_bytes
342 )
343 {
344 eth_mac_addr_t dst_mac_addr;
345 if (!resolve_ip(dst, &dst_mac_addr))
346 {
347 UHD_FW_TRACE(WARN, "u3_net_stack_send_udp_pkt arp_cache_lookup fail");
348 return;
349 }
350
351 padded_udp_t pkt;
352
353 pkt.eth.ethno = ethno;
354 memcpy(&pkt.eth.dst, &dst_mac_addr, sizeof(eth_mac_addr_t));
355 memcpy(&pkt.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t));
356 pkt.eth.ethertype = ETHERTYPE_IPV4;
357
358 IPH_VHLTOS_SET(&pkt.ip, 4, 5, 0);
359 IPH_LEN_SET(&pkt.ip, IP_HLEN + UDP_HLEN + num_bytes);
360 IPH_ID_SET(&pkt.ip, 0);
361 IPH_OFFSET_SET(&pkt.ip, IP_DF); /* don't fragment */
362 IPH_TTL_SET(&pkt.ip, 32);
363 IPH_PROTO_SET(&pkt.ip, IP_PROTO_UDP);
364 IPH_CHKSUM_SET(&pkt.ip, 0);
365 memcpy(&pkt.ip.src, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr));
366 memcpy(&pkt.ip.dest, dst, sizeof(struct ip_addr));
367
368 IPH_CHKSUM_SET(&pkt.ip, ~chksum_buffer(
369 (unsigned short *) &pkt.ip, sizeof(pkt.ip)/sizeof(short), 0
370 ));
371
372 pkt.udp.src = src_port;
373 pkt.udp.dest = dst_port;
374 pkt.udp.len = UDP_HLEN + num_bytes;
375 pkt.udp.chksum = 0;
376
377 send_eth_pkt(&pkt, sizeof(pkt), buff, num_bytes, NULL, 0);
378 }
379
handle_udp_packet(const uint8_t ethno,const struct ip_addr * src,const struct ip_addr * dst,const struct udp_hdr * udp,const size_t num_bytes)380 static void handle_udp_packet(
381 const uint8_t ethno,
382 const struct ip_addr *src,
383 const struct ip_addr *dst,
384 const struct udp_hdr *udp,
385 const size_t num_bytes
386 ){
387 for (size_t i = 0; i < udp_handlers_index; i++)
388 {
389 if (udp_handler_ports[i] == udp->dest)
390 {
391 udp_handlers[i](
392 ethno, src, u3_net_stack_get_ip_addr(ethno), udp->src, udp->dest,
393 ((const uint8_t *)udp) + sizeof(struct udp_hdr),
394 num_bytes - UDP_HLEN
395 );
396 return;
397 }
398 }
399 UHD_FW_TRACE_FSTR(ERROR, "Unhandled UDP packet src=%u, dest=%u", udp->src, udp->dest);
400 //TODO send destination unreachable
401 }
402
403 /***********************************************************************
404 * ICMP handlers
405 **********************************************************************/
406 #define ICMP_NHANDLERS 8
407
408 static uint8_t icmp_handler_types[ICMP_NHANDLERS];
409 static uint8_t icmp_handler_codes[ICMP_NHANDLERS];
410 static u3_net_stack_icmp_handler_t icmp_handlers[ICMP_NHANDLERS];
411 static size_t icmp_handlers_index = 0;
412
u3_net_stack_register_icmp_handler(const uint8_t type,const uint8_t code,const u3_net_stack_icmp_handler_t handler)413 void u3_net_stack_register_icmp_handler(
414 const uint8_t type,
415 const uint8_t code,
416 const u3_net_stack_icmp_handler_t handler
417 )
418 {
419 if (icmp_handlers_index < ICMP_NHANDLERS)
420 {
421 icmp_handler_types[icmp_handlers_index] = type;
422 icmp_handler_codes[icmp_handlers_index] = code;
423 icmp_handlers[icmp_handlers_index] = handler;
424 icmp_handlers_index++;
425 }
426 }
427
handle_icmp_packet(const uint8_t ethno,const struct ip_addr * src,const struct ip_addr * dst,const struct icmp_echo_hdr * icmp,const size_t num_bytes)428 static void handle_icmp_packet(
429 const uint8_t ethno,
430 const struct ip_addr *src,
431 const struct ip_addr *dst,
432 const struct icmp_echo_hdr *icmp,
433 const size_t num_bytes
434 ){
435
436 for (size_t i = 0; i < icmp_handlers_index; i++)
437 {
438 if (icmp_handler_types[i] == icmp->type && icmp_handler_codes[i] == icmp->code)
439 {
440 icmp_handlers[i](
441 ethno, src, u3_net_stack_get_ip_addr(ethno), icmp->id, icmp->seqno,
442 ((const uint8_t *)icmp) + sizeof(struct icmp_echo_hdr),
443 num_bytes - sizeof(struct icmp_echo_hdr)
444 );
445 return;
446 }
447 }
448 UHD_FW_TRACE_FSTR(ERROR, "Unhandled ICMP packet type=%u", icmp->type);
449 }
450
handle_icmp_dur_packet(const uint8_t ethno,const struct ip_addr * src,const struct ip_addr * dst,const uint16_t id,const uint16_t seq,const void * buff,const size_t num_bytes)451 static void handle_icmp_dur_packet(
452 const uint8_t ethno,
453 const struct ip_addr *src, const struct ip_addr *dst,
454 const uint16_t id, const uint16_t seq,
455 const void *buff, const size_t num_bytes
456 ){
457 struct ip_hdr *ip = (struct ip_hdr *)buff;
458 struct udp_hdr *udp = (struct udp_hdr *)(((char *)ip) + IP_HLEN);
459 if (IPH_PROTO(ip) != IP_PROTO_UDP) return;
460 for (size_t i = 0; i < udp_handlers_index; i++)
461 {
462 if (udp_handler_ports[i] == udp->src)
463 {
464 udp_handlers[i](ethno,
465 src, u3_net_stack_get_ip_addr(ethno),
466 udp->src, udp->dest, NULL, 0
467 );
468 return;
469 }
470 }
471 }
472
u3_net_stack_send_icmp_pkt(const uint8_t ethno,const uint8_t type,const uint8_t code,const uint16_t id,const uint16_t seq,const struct ip_addr * dst,const void * buff,const size_t num_bytes)473 void u3_net_stack_send_icmp_pkt(
474 const uint8_t ethno,
475 const uint8_t type,
476 const uint8_t code,
477 const uint16_t id,
478 const uint16_t seq,
479 const struct ip_addr *dst,
480 const void *buff,
481 const size_t num_bytes
482 )
483 {
484 eth_mac_addr_t dst_mac_addr;
485 if (!resolve_ip(dst, &dst_mac_addr))
486 {
487 UHD_FW_TRACE(WARN, "u3_net_stack_send_echo_request arp_cache_lookup fail");
488 return;
489 }
490
491 padded_icmp_t pkt;
492
493 pkt.eth.ethno = ethno;
494 memcpy(&pkt.eth.dst, &dst_mac_addr, sizeof(eth_mac_addr_t));
495 memcpy(&pkt.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t));
496 pkt.eth.ethertype = ETHERTYPE_IPV4;
497
498 IPH_VHLTOS_SET(&pkt.ip, 4, 5, 0);
499 IPH_LEN_SET(&pkt.ip, IP_HLEN + sizeof(pkt.icmp) + num_bytes);
500 IPH_ID_SET(&pkt.ip, 0);
501 IPH_OFFSET_SET(&pkt.ip, IP_DF); /* don't fragment */
502 IPH_TTL_SET(&pkt.ip, 32);
503 IPH_PROTO_SET(&pkt.ip, IP_PROTO_ICMP);
504 IPH_CHKSUM_SET(&pkt.ip, 0);
505 memcpy(&pkt.ip.src, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr));
506 memcpy(&pkt.ip.dest, dst, sizeof(struct ip_addr));
507
508 IPH_CHKSUM_SET(&pkt.ip, ~chksum_buffer(
509 (unsigned short *) &pkt.ip, sizeof(pkt.ip)/sizeof(short), 0
510 ));
511
512 pkt.icmp.type = type;
513 pkt.icmp.code = code;
514 pkt.icmp.chksum = 0;
515 pkt.icmp.id = id;
516 pkt.icmp.seqno = seq;
517 pkt.icmp.chksum = ~chksum_buffer( //data checksum
518 (unsigned short *)buff,
519 num_bytes/sizeof(short),
520 chksum_buffer( //header checksum
521 (unsigned short *)&pkt.icmp,
522 sizeof(pkt.icmp)/sizeof(short),
523 0)
524 );
525
526 send_eth_pkt(&pkt, sizeof(pkt), buff, num_bytes, NULL, 0);
527 }
528
529 /***********************************************************************
530 * Ethernet handlers
531 **********************************************************************/
handle_eth_packet(const void * buff,const size_t num_bytes)532 static void handle_eth_packet(const void *buff, const size_t num_bytes)
533 {
534 const padded_eth_hdr_t *eth_hdr = (padded_eth_hdr_t *)buff;
535 const uint8_t *eth_body = ((const uint8_t *)buff) + sizeof(padded_eth_hdr_t);
536 UHD_FW_TRACE_FSTR(DEBUG, "handle_eth_packet got ethertype 0x%x", (unsigned)eth_hdr->ethertype);
537
538 if (eth_hdr->ethertype == ETHERTYPE_ARP)
539 {
540 UHD_FW_TRACE(DEBUG, "eth_hdr->ethertype == ETHERTYPE_ARP");
541 const struct arp_eth_ipv4 *arp = (const struct arp_eth_ipv4 *)eth_body;
542 handle_arp_packet(eth_hdr->ethno, arp);
543 }
544 else if (eth_hdr->ethertype == ETHERTYPE_IPV4)
545 {
546 UHD_FW_TRACE(DEBUG, "eth_hdr->ethertype == ETHERTYPE_IPV4");
547 const struct ip_hdr *ip = (const struct ip_hdr *)eth_body;
548 const uint8_t *ip_body = eth_body + IP_HLEN;
549
550 if (IPH_V(ip) != 4 || IPH_HL(ip) != 5) return;// ignore pkts w/ bad version or options
551 if (IPH_OFFSET(ip) & (IP_MF | IP_OFFMASK)) return;// ignore fragmented packets
552
553 //TODO -- only handle when the mac is bcast or IP is for us
554
555 u3_net_stack_arp_cache_update(&ip->src, ð_hdr->src, eth_hdr->ethno);
556
557 if (IPH_PROTO(ip) == IP_PROTO_UDP)
558 {
559 handle_udp_packet(
560 eth_hdr->ethno, &ip->src, &ip->dest,
561 (const struct udp_hdr *)ip_body,
562 IPH_LEN(ip) - IP_HLEN
563 );
564 }
565
566 if (IPH_PROTO(ip) == IP_PROTO_ICMP)
567 {
568 handle_icmp_packet(
569 eth_hdr->ethno, &ip->src, &ip->dest,
570 (const struct icmp_echo_hdr *)ip_body,
571 IPH_LEN(ip) - IP_HLEN
572 );
573 }
574 }
575 else return; // Not ARP or IPV4, ignore
576 }
577
u3_net_stack_handle_one(void)578 void u3_net_stack_handle_one(void)
579 {
580 size_t num_bytes = 0;
581 const void *ptr = wb_pkt_iface64_rx_try_claim(pkt_iface_config, &num_bytes);
582 if (ptr != NULL)
583 {
584 UHD_FW_TRACE_FSTR(DEBUG, "u3_net_stack_handle_one got %u bytes", (unsigned)num_bytes);
585 incr_stat_counts(ptr);
586 handle_eth_packet(ptr, num_bytes);
587 wb_pkt_iface64_rx_release(pkt_iface_config);
588 }
589 }
590