1 /*
2 * Copyright 2009-2012 Ettus Research LLC
3 *
4 * This program is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #ifdef HAVE_CONFIG_H
19 #include <config.h>
20 #endif
21 #include "net_common.h"
22 #include "banal.h"
23 #include <hal_io.h>
24 #include <memory_map.h>
25 #include <memcpy_wa.h>
26 #include <ethernet.h>
27 #include <net/padded_eth_hdr.h>
28 #include <lwip/ip.h>
29 #include <lwip/udp.h>
30 #include <lwip/icmp.h>
31 #include <stdlib.h>
32 #include <nonstdio.h>
33 #include "arp_cache.h"
34 #include "if_arp.h"
35 #include <ethertype.h>
36 #include <string.h>
37 #include "pkt_ctrl.h"
38
39 /***********************************************************************
40 * Constants + Globals
41 **********************************************************************/
42 static const bool debug = false;
43 static const size_t out_buff_size = 2048;
44 static const eth_mac_addr_t BCAST_MAC_ADDR = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff}};
45 #define MAX_UDP_LISTENERS 10
46
47 /***********************************************************************
48 * 16-bit one's complement sum
49 **********************************************************************/
chksum_buffer(uint16_t * buf,size_t nshorts,uint32_t initial_chksum)50 static uint32_t chksum_buffer(
51 uint16_t *buf, size_t nshorts,
52 uint32_t initial_chksum
53 ){
54 uint32_t chksum = initial_chksum;
55 for (size_t i = 0; i < nshorts; i++) chksum += buf[i];
56
57 while (chksum >> 16) chksum = (chksum & 0xffff) + (chksum >> 16);
58
59 return chksum;
60 }
61
62 /***********************************************************************
63 * Listener registry
64 **********************************************************************/
65 static eth_mac_addr_t _local_mac_addr;
66 static struct ip_addr _local_ip_addr;
register_addrs(const eth_mac_addr_t * mac_addr,const struct ip_addr * ip_addr)67 void register_addrs(const eth_mac_addr_t *mac_addr, const struct ip_addr *ip_addr){
68 _local_mac_addr = *mac_addr;
69 _local_ip_addr = *ip_addr;
70 }
71
72 struct listener_entry {
73 unsigned short port;
74 udp_receiver_t rcvr;
75 };
76
77 static struct listener_entry listeners[MAX_UDP_LISTENERS];
78
init_udp_listeners(void)79 void init_udp_listeners(void){
80 for (int i = 0; i < MAX_UDP_LISTENERS; i++){
81 listeners[i].rcvr = NULL;
82 }
83 }
84
85 static struct listener_entry *
find_listener_by_port(unsigned short port)86 find_listener_by_port(unsigned short port)
87 {
88 for (int i = 0; i < MAX_UDP_LISTENERS; i++){
89 if (port == listeners[i].port)
90 return &listeners[i];
91 }
92 return 0;
93 }
94
95 static struct listener_entry *
find_free_listener(void)96 find_free_listener(void)
97 {
98 for (int i = 0; i < MAX_UDP_LISTENERS; i++){
99 if (listeners[i].rcvr == NULL)
100 return &listeners[i];
101 }
102 abort();
103 }
104
105 void
register_udp_listener(int port,udp_receiver_t rcvr)106 register_udp_listener(int port, udp_receiver_t rcvr)
107 {
108 struct listener_entry *lx = find_listener_by_port(port);
109 if (lx)
110 lx->rcvr = rcvr;
111 else {
112 lx = find_free_listener();
113 lx->port = port;
114 lx->rcvr = rcvr;
115 }
116 }
117
118 /***********************************************************************
119 * Protocol framer
120 **********************************************************************/
setup_framer(eth_mac_addr_t eth_dst,eth_mac_addr_t eth_src,struct socket_address sock_dst,struct socket_address sock_src,size_t which)121 void setup_framer(
122 eth_mac_addr_t eth_dst,
123 eth_mac_addr_t eth_src,
124 struct socket_address sock_dst,
125 struct socket_address sock_src,
126 size_t which
127 ){
128 struct {
129 padded_eth_hdr_t eth;
130 struct ip_hdr ip;
131 struct udp_hdr udp;
132 } frame;
133
134 //-- load Ethernet header --//
135 frame.eth.dst = eth_dst;
136 frame.eth.src = eth_src;
137 frame.eth.ethertype = ETHERTYPE_IPV4;
138
139 //-- load IPv4 header --//
140 IPH_VHLTOS_SET(&frame.ip, 4, 5, 0);
141 IPH_LEN_SET(&frame.ip, 0);
142 IPH_ID_SET(&frame.ip, 0);
143 IPH_OFFSET_SET(&frame.ip, IP_DF); // don't fragment
144 IPH_TTL_SET(&frame.ip, 32);
145 IPH_PROTO_SET(&frame.ip, IP_PROTO_UDP);
146 IPH_CHKSUM_SET(&frame.ip, 0);
147 frame.ip.src = sock_src.addr;
148 frame.ip.dest = sock_dst.addr;
149 IPH_CHKSUM_SET(&frame.ip, chksum_buffer(
150 (unsigned short *) &frame.ip,
151 sizeof(frame.ip)/sizeof(short), 0
152 ));
153
154 //-- load UDP header --//
155 frame.udp.src = sock_src.port;
156 frame.udp.dest = sock_dst.port;
157 frame.udp.len = 0;
158 frame.udp.chksum = 0;
159
160 //copy into the framer table registers
161 memcpy_wa((void *)(sr_proto_framer_regs->table[which].entry + 1), &frame, sizeof(frame));
162 }
163
164 /***********************************************************************
165 * Slow-path packet framing and transmission
166 **********************************************************************/
167 /*!
168 * low level routine to assembly an ethernet frame and send it.
169 *
170 * \param dst destination mac address
171 * \param ethertype ethertype field
172 * \param buf0 first part of data
173 * \param len0 length of first part of data
174 * \param buf1 second part of data
175 * \param len1 length of second part of data
176 * \param buf2 third part of data
177 * \param len2 length of third part of data
178 */
179 static void
send_pkt(eth_mac_addr_t dst,int ethertype,const void * buf0,size_t len0,const void * buf1,size_t len1,const void * buf2,size_t len2)180 send_pkt(
181 eth_mac_addr_t dst, int ethertype,
182 const void *buf0, size_t len0,
183 const void *buf1, size_t len1,
184 const void *buf2, size_t len2
185 ){
186
187 //control word for framed data
188 uint32_t ctrl_word = 0x0;
189
190 //assemble the ethernet header
191 padded_eth_hdr_t ehdr;
192 ehdr.pad = 0;
193 ehdr.dst = dst;
194 ehdr.src = _local_mac_addr;
195 ehdr.ethertype = ethertype;
196
197 //grab an out buffer and pointer
198 uint8_t *buff = (uint8_t *)pkt_ctrl_claim_outgoing_buffer();
199 uint8_t *p = buff;
200 size_t total_len = 0;
201
202 //create a list of all buffers to copy
203 const void *buffs[] = {&ctrl_word, &ehdr, buf0, buf1, buf2};
204 size_t lens[] = {sizeof(ctrl_word), sizeof(ehdr), len0, len1, (len2 + 3) & ~3};
205
206 //copy each buffer into the out buffer
207 for (size_t i = 0; i < sizeof(buffs)/sizeof(buffs[0]); i++){
208 total_len += lens[i]; //use full length (not clipped)
209 size_t bytes_remaining = out_buff_size - (size_t)(p - buff);
210 if (lens[i] > bytes_remaining) lens[i] = bytes_remaining;
211 if (lens[i] && ((lens[i] & 0x3) || (intptr_t) buffs[i] & 0x3))
212 printf("send_pkt: bad alignment of len and/or buf\n");
213 memcpy_wa(p, buffs[i], lens[i]);
214 p += lens[i];
215 }
216
217 //ensure that minimum length requirements are met
218 if (total_len < 64) total_len = 64; //60 + ctrl word
219
220 pkt_ctrl_commit_outgoing_buffer(total_len/sizeof(uint32_t));
221 if (debug) printf("sent %d bytes\n", (int)total_len);
222 }
223
224 void
send_ip_pkt(struct ip_addr dst,int protocol,const void * buf0,size_t len0,const void * buf1,size_t len1)225 send_ip_pkt(struct ip_addr dst, int protocol,
226 const void *buf0, size_t len0,
227 const void *buf1, size_t len1)
228 {
229 struct ip_hdr ip;
230 IPH_VHLTOS_SET(&ip, 4, 5, 0);
231 IPH_LEN_SET(&ip, IP_HLEN + len0 + len1);
232 IPH_ID_SET(&ip, 0);
233 IPH_OFFSET_SET(&ip, IP_DF); /* don't fragment */
234 IPH_TTL_SET(&ip, 32);
235 IPH_PROTO_SET(&ip, protocol);
236 IPH_CHKSUM_SET(&ip, 0);
237 ip.src = _local_ip_addr;
238 ip.dest = dst;
239
240 IPH_CHKSUM_SET(&ip, ~chksum_buffer(
241 (unsigned short *) &ip, sizeof(ip)/sizeof(short), 0
242 ));
243
244 eth_mac_addr_t dst_mac;
245 bool found = arp_cache_lookup_mac(&ip.dest, &dst_mac);
246 if (!found){
247 printf("net_common: failed to hit cache looking for ");
248 print_ip_addr(&ip.dest);
249 newline();
250 return;
251 }
252
253 send_pkt(dst_mac, ETHERTYPE_IPV4,
254 &ip, sizeof(ip), buf0, len0, buf1, len1);
255 }
256
257 void
send_udp_pkt(int src_port,struct socket_address dst,const void * buf,size_t len)258 send_udp_pkt(int src_port, struct socket_address dst,
259 const void *buf, size_t len)
260 {
261 struct udp_hdr udp _AL4;
262 udp.src = src_port;
263 udp.dest = dst.port;
264 udp.len = UDP_HLEN + len;
265 udp.chksum = 0;
266
267 send_ip_pkt(dst.addr, IP_PROTO_UDP,
268 &udp, sizeof(udp), buf, len);
269 }
270
271 static void
handle_udp_packet(struct ip_addr src_ip,struct ip_addr dst_ip,struct udp_hdr * udp,size_t len)272 handle_udp_packet(struct ip_addr src_ip, struct ip_addr dst_ip,
273 struct udp_hdr *udp, size_t len)
274 {
275 if (len != udp->len){
276 printf("UDP inconsistent lengths: %d %d\n", (int)len, udp->len);
277 return;
278 }
279
280 unsigned char *payload = ((unsigned char *) udp) + UDP_HLEN;
281 int payload_len = len - UDP_HLEN;
282
283 if (0){
284 printf("\nUDP: src = %d dst = %d len = %d\n",
285 udp->src, udp->dest, udp->len);
286
287 //print_bytes(0, payload, payload_len);
288 }
289
290 struct listener_entry *lx = find_listener_by_port(udp->dest);
291 if (lx){
292 struct socket_address src = make_socket_address(src_ip, udp->src);
293 struct socket_address dst = make_socket_address(dst_ip, udp->dest);
294 lx->rcvr(src, dst, payload, payload_len);
295 }
296 }
297
298 static void
handle_icmp_packet(struct ip_addr src,struct ip_addr dst,struct icmp_echo_hdr * icmp,size_t len)299 handle_icmp_packet(struct ip_addr src, struct ip_addr dst,
300 struct icmp_echo_hdr *icmp, size_t len)
301 {
302 switch (icmp->type){
303 case ICMP_DUR: // Destinatino Unreachable
304 if (icmp->code == ICMP_DUR_PORT){ // port unreachable
305 //handle destination port unreachable (the host ctrl+c'd the app):
306
307 //filter out non udp data response
308 struct ip_hdr *ip = (struct ip_hdr *)(((uint8_t*)icmp) + sizeof(struct icmp_echo_hdr));
309 struct udp_hdr *udp = (struct udp_hdr *)(((char *)ip) + IP_HLEN);
310 if (IPH_PROTO(ip) != IP_PROTO_UDP) break;
311
312 struct listener_entry *lx = find_listener_by_port(udp->src);
313 if (lx){
314 struct socket_address src = make_socket_address(ip->src, udp->src);
315 struct socket_address dst = make_socket_address(ip->dest, udp->dest);
316 lx->rcvr(src, dst, NULL, 0);
317 }
318
319 putchar('i');
320 }
321 else {
322 //printf("icmp dst unr (code: %d)", icmp->code);
323 putchar('i');
324 }
325 break;
326
327 case ICMP_ECHO:{
328 const void *icmp_data_buff = ((uint8_t*)icmp) + sizeof(struct icmp_echo_hdr);
329 size_t icmp_data_len = len - sizeof(struct icmp_echo_hdr);
330
331 struct icmp_echo_hdr echo_reply;
332 echo_reply.type = 0;
333 echo_reply.code = 0;
334 echo_reply.chksum = 0;
335 echo_reply.id = icmp->id;
336 echo_reply.seqno = icmp->seqno;
337 echo_reply.chksum = ~chksum_buffer( //data checksum
338 (unsigned short *)icmp_data_buff,
339 icmp_data_len/sizeof(short),
340 chksum_buffer( //header checksum
341 (unsigned short *)&echo_reply,
342 sizeof(echo_reply)/sizeof(short),
343 0)
344 );
345
346 send_ip_pkt(
347 src, IP_PROTO_ICMP,
348 &echo_reply, sizeof(echo_reply),
349 icmp_data_buff, icmp_data_len
350 );
351 break;
352 }
353 default:
354 break;
355 }
356 }
357
358 static void
send_arp_reply(struct arp_eth_ipv4 * req,eth_mac_addr_t our_mac)359 send_arp_reply(struct arp_eth_ipv4 *req, eth_mac_addr_t our_mac)
360 {
361 struct arp_eth_ipv4 reply _AL4;
362 reply.ar_hrd = req->ar_hrd;
363 reply.ar_pro = req->ar_pro;
364 reply.ar_hln = req->ar_hln;
365 reply.ar_pln = req->ar_pln;
366 reply.ar_op = ARPOP_REPLY;
367 memcpy(reply.ar_sha, &our_mac, 6);
368 memcpy(reply.ar_sip, req->ar_tip, 4);
369 memcpy(reply.ar_tha, req->ar_sha, 6);
370 memcpy(reply.ar_tip, req->ar_sip, 4);
371
372 eth_mac_addr_t t;
373 memcpy(t.addr, reply.ar_tha, 6);
374 send_pkt(t, ETHERTYPE_ARP, &reply, sizeof(reply), 0, 0, 0, 0);
375 }
376
net_common_send_arp_request(const struct ip_addr * addr)377 void net_common_send_arp_request(const struct ip_addr *addr){
378 struct arp_eth_ipv4 req _AL4;
379 req.ar_hrd = ARPHRD_ETHER;
380 req.ar_pro = ETHERTYPE_IPV4;
381 req.ar_hln = sizeof(eth_mac_addr_t);
382 req.ar_pln = sizeof(struct ip_addr);
383 req.ar_op = ARPOP_REQUEST;
384 memcpy(req.ar_sha, ethernet_mac_addr(), sizeof(eth_mac_addr_t));
385 memcpy(req.ar_sip, get_ip_addr(), sizeof(struct ip_addr));
386 memset(req.ar_tha, 0x00, sizeof(eth_mac_addr_t));
387 memcpy(req.ar_tip, addr, sizeof(struct ip_addr));
388
389 //send the request with a broadcast ethernet mac address
390 send_pkt(BCAST_MAC_ADDR, ETHERTYPE_ARP, &req, sizeof(req), 0, 0, 0, 0);
391 }
392
send_gratuitous_arp(void)393 void send_gratuitous_arp(void){
394 struct arp_eth_ipv4 req _AL4;
395 req.ar_hrd = ARPHRD_ETHER;
396 req.ar_pro = ETHERTYPE_IPV4;
397 req.ar_hln = sizeof(eth_mac_addr_t);
398 req.ar_pln = sizeof(struct ip_addr);
399 req.ar_op = ARPOP_REQUEST;
400 memcpy(req.ar_sha, ethernet_mac_addr(), sizeof(eth_mac_addr_t));
401 memcpy(req.ar_sip, get_ip_addr(), sizeof(struct ip_addr));
402 memset(req.ar_tha, 0x00, sizeof(eth_mac_addr_t));
403 memcpy(req.ar_tip, get_ip_addr(), sizeof(struct ip_addr));
404
405 //send the request with a broadcast ethernet mac address
406 send_pkt(BCAST_MAC_ADDR, ETHERTYPE_ARP, &req, sizeof(req), 0, 0, 0, 0);
407 }
408
409 static void
handle_arp_packet(struct arp_eth_ipv4 * p,size_t size)410 handle_arp_packet(struct arp_eth_ipv4 *p, size_t size)
411 {
412 if (size < sizeof(struct arp_eth_ipv4)){
413 printf("\nhandle_arp: weird size = %d\n", (int)size);
414 return;
415 }
416
417 if (0){
418 printf("ar_hrd = %d\n", p->ar_hrd);
419 printf("ar_pro = %d\n", p->ar_pro);
420 printf("ar_hln = %d\n", p->ar_hln);
421 printf("ar_pln = %d\n", p->ar_pln);
422 printf("ar_op = %d\n", p->ar_op);
423 printf("ar_sha = "); print_mac_addr(p->ar_sha); newline();
424 printf("ar_sip = "); print_ip_addr (p->ar_sip); newline();
425 printf("ar_tha = "); print_mac_addr(p->ar_tha); newline();
426 printf("ar_tip = "); print_ip_addr (p->ar_tip); newline();
427 }
428
429 if (p->ar_hrd != ARPHRD_ETHER
430 || p->ar_pro != ETHERTYPE_IPV4
431 || p->ar_hln != 6
432 || p->ar_pln != 4)
433 return;
434
435 if (p->ar_op == ARPOP_REPLY){
436 struct ip_addr ip_addr;
437 memcpy(&ip_addr, p->ar_sip, sizeof(ip_addr));
438 eth_mac_addr_t mac_addr;
439 memcpy(&mac_addr, p->ar_sha, sizeof(mac_addr));
440 arp_cache_update(&ip_addr, &mac_addr);
441 }
442
443 if (p->ar_op != ARPOP_REQUEST)
444 return;
445
446 struct ip_addr sip;
447 struct ip_addr tip;
448
449 sip.addr = get_int32(p->ar_sip);
450 tip.addr = get_int32(p->ar_tip);
451
452 if (memcmp(&tip, &_local_ip_addr, sizeof(_local_ip_addr)) == 0){ // They're looking for us...
453 send_arp_reply(p, _local_mac_addr);
454 }
455 }
456
457 void
handle_eth_packet(uint32_t * p,size_t nlines)458 handle_eth_packet(uint32_t *p, size_t nlines)
459 {
460 static size_t bcount = 0;
461 if (debug) printf("===> %d\n", (int)bcount++);
462 if (debug) print_buffer(p, nlines);
463
464 padded_eth_hdr_t *eth_hdr = (padded_eth_hdr_t *)p;
465
466 if (eth_hdr->ethertype == ETHERTYPE_ARP){
467 struct arp_eth_ipv4 *arp = (struct arp_eth_ipv4 *)(p + 4);
468 handle_arp_packet(arp, nlines*sizeof(uint32_t) - 14);
469 }
470 else if (eth_hdr->ethertype == ETHERTYPE_IPV4){
471 struct ip_hdr *ip = (struct ip_hdr *)(p + 4);
472 if (IPH_V(ip) != 4 || IPH_HL(ip) != 5) // ignore pkts w/ bad version or options
473 return;
474
475 if (IPH_OFFSET(ip) & (IP_MF | IP_OFFMASK)) // ignore fragmented packets
476 return;
477
478 // filter on dest ip addr (should be broadcast or for us)
479 bool is_bcast = memcmp(ð_hdr->dst, &BCAST_MAC_ADDR, sizeof(BCAST_MAC_ADDR)) == 0;
480 bool is_my_ip = memcmp(&ip->dest, &_local_ip_addr, sizeof(_local_ip_addr)) == 0;
481 if (!is_bcast && !is_my_ip) return;
482
483 arp_cache_update(&ip->src, (eth_mac_addr_t *)(((char *)p)+8));
484
485 int protocol = IPH_PROTO(ip);
486 int len = IPH_LEN(ip) - IP_HLEN;
487
488 switch (protocol){
489 case IP_PROTO_UDP:
490 handle_udp_packet(ip->src, ip->dest, (struct udp_hdr *)(((char *)ip) + IP_HLEN), len);
491 break;
492
493 case IP_PROTO_ICMP:
494 handle_icmp_packet(ip->src, ip->dest, (struct icmp_echo_hdr *)(((char *)ip) + IP_HLEN), len);
495 break;
496
497 default: // ignore
498 break;
499 }
500 }
501 else
502 return; // Not ARP or IPV4, ignore
503 }
504