1 /********************************************************************* 2 PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. 3 See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. 4 5 . 6 7 Authors: Daniele Lacamera 8 *********************************************************************/ 9 10#include "pico_config.h" 11#include "pico_stack.h" 12#include "pico_ipv4.h" 13#include "pico_ipv6.h" 14#include "pico_icmp4.h" 15#include "pico_icmp6.h" 16#include "pico_arp.h" 17#include "pico_ethernet.h" 18 19#define IS_LIMITED_BCAST(f) (((struct pico_ipv4_hdr *) f->net_hdr)->dst.addr == PICO_IP4_BCAST) 20 21#ifdef PICO_SUPPORT_ETH 22 23const uint8_t PICO_ETHADDR_ALL[6] = { 24 0xff, 0xff, 0xff, 0xff, 0xff, 0xff 25}; 26 27# define PICO_SIZE_MCAST 3 28static const uint8_t PICO_ETHADDR_MCAST[6] = { 29 0x01, 0x00, 0x5e, 0x00, 0x00, 0x00 30}; 31 32#ifdef PICO_SUPPORT_IPV6 33# define PICO_SIZE_MCAST6 2 34static const uint8_t PICO_ETHADDR_MCAST6[6] = { 35 0x33, 0x33, 0x00, 0x00, 0x00, 0x00 36}; 37#endif 38 39/* DATALINK LEVEL: interface from network to the device 40 * and vice versa. 41 */ 42 43/* The pico_ethernet_receive() function is used by 44 * those devices supporting ETH in order to push packets up 45 * into the stack. 46 */ 47 48/* Queues */ 49static struct pico_queue ethernet_in = { 50 0 51}; 52static struct pico_queue ethernet_out = { 53 0 54}; 55 56int32_t MOCKABLE pico_ethernet_send(struct pico_frame *f); 57static int32_t pico_ethernet_receive(struct pico_frame *f); 58 59static int pico_ethernet_process_out(struct pico_protocol *self, struct pico_frame *f) 60{ 61 IGNORE_PARAMETER(self); 62 return pico_ethernet_send(f); 63} 64 65static int pico_ethernet_process_in(struct pico_protocol *self, struct pico_frame *f) 66{ 67 IGNORE_PARAMETER(self); 68 return (pico_ethernet_receive(f) <= 0); /* 0 on success, which is ret > 0 */ 69} 70 71static struct pico_frame *pico_ethernet_alloc(struct pico_protocol *self, struct pico_device *dev, uint16_t size) 72{ 73 struct pico_frame *f = NULL; 74 uint32_t overhead = 0; 75 IGNORE_PARAMETER(self); 76 77 if (dev) 78 overhead = dev->overhead; 79 80 f = pico_frame_alloc((uint32_t)(overhead + size + PICO_SIZE_ETHHDR)); 81 if (!f) 82 return NULL; 83 84 f->dev = dev; 85 f->datalink_hdr = f->buffer + overhead; 86 f->net_hdr = f->datalink_hdr + PICO_SIZE_ETHHDR; 87 /* Stay of the rest, higher levels will take care */ 88 89 return f; 90} 91 92/* Interface: protocol definition */ 93struct pico_protocol pico_proto_ethernet = { 94 .name = "ethernet", 95 .layer = PICO_LAYER_DATALINK, 96 .alloc = pico_ethernet_alloc, 97 .process_in = pico_ethernet_process_in, 98 .process_out = pico_ethernet_process_out, 99 .q_in = ðernet_in, 100 .q_out = ðernet_out, 101}; 102 103static int destination_is_bcast(struct pico_frame *f) 104{ 105 if (!f) 106 return 0; 107 108 if (IS_IPV6(f)) 109 return 0; 110 111#ifdef PICO_SUPPORT_IPV4 112 else { 113 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 114 return pico_ipv4_is_broadcast(hdr->dst.addr); 115 } 116#else 117 return 0; 118#endif 119} 120 121static int destination_is_mcast(struct pico_frame *f) 122{ 123 int ret = 0; 124 if (!f) 125 return 0; 126 127#ifdef PICO_SUPPORT_IPV6 128 if (IS_IPV6(f)) { 129 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr; 130 ret = pico_ipv6_is_multicast(hdr->dst.addr); 131 } 132 133#endif 134#ifdef PICO_SUPPORT_IPV4 135 else { 136 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 137 ret = pico_ipv4_is_multicast(hdr->dst.addr); 138 } 139#endif 140 141 return ret; 142} 143 144#ifdef PICO_SUPPORT_IPV4 145static int32_t pico_ipv4_ethernet_receive(struct pico_frame *f) 146{ 147 if (IS_IPV4(f)) { 148 if (pico_enqueue(pico_proto_ipv4.q_in, f) < 0) { 149 pico_frame_discard(f); 150 return -1; 151 } 152 } else { 153 (void)pico_icmp4_param_problem(f, 0); 154 pico_frame_discard(f); 155 return -1; 156 } 157 158 return (int32_t)f->buffer_len; 159} 160#endif 161 162#ifdef PICO_SUPPORT_IPV6 163static int32_t pico_ipv6_ethernet_receive(struct pico_frame *f) 164{ 165 if (IS_IPV6(f)) { 166 if (pico_enqueue(pico_proto_ipv6.q_in, f) < 0) { 167 pico_frame_discard(f); 168 return -1; 169 } 170 } else { 171 /* Wrong version for link layer type */ 172 pico_frame_discard(f); 173 return -1; 174 } 175 176 return (int32_t)f->buffer_len; 177} 178#endif 179 180static int32_t pico_eth_receive(struct pico_frame *f) 181{ 182 struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr; 183 f->net_hdr = f->datalink_hdr + sizeof(struct pico_eth_hdr); 184 185#if (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH) 186 if (hdr->proto == PICO_IDETH_ARP) 187 return pico_arp_receive(f); 188#endif 189 190#if defined (PICO_SUPPORT_IPV4) 191 if (hdr->proto == PICO_IDETH_IPV4) 192 return pico_ipv4_ethernet_receive(f); 193#endif 194 195#if defined (PICO_SUPPORT_IPV6) 196 if (hdr->proto == PICO_IDETH_IPV6) 197 return pico_ipv6_ethernet_receive(f); 198#endif 199 200 pico_frame_discard(f); 201 return -1; 202} 203 204static void pico_eth_check_bcast(struct pico_frame *f) 205{ 206 struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr; 207 /* Indicate a link layer broadcast packet */ 208 if (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) == 0) 209 f->flags |= PICO_FRAME_FLAG_BCAST; 210} 211 212static int32_t pico_ethernet_receive(struct pico_frame *f) 213{ 214 struct pico_eth_hdr *hdr; 215 if (!f || !f->dev || !f->datalink_hdr) 216 { 217 pico_frame_discard(f); 218 return -1; 219 } 220 221 hdr = (struct pico_eth_hdr *) f->datalink_hdr; 222 if ((memcmp(hdr->daddr, f->dev->eth->mac.addr, PICO_SIZE_ETH) != 0) && 223 (memcmp(hdr->daddr, PICO_ETHADDR_MCAST, PICO_SIZE_MCAST) != 0) && 224#ifdef PICO_SUPPORT_IPV6 225 (memcmp(hdr->daddr, PICO_ETHADDR_MCAST6, PICO_SIZE_MCAST6) != 0) && 226#endif 227 (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) != 0)) 228 { 229 pico_frame_discard(f); 230 return -1; 231 } 232 233 pico_eth_check_bcast(f); 234 return pico_eth_receive(f); 235} 236 237static struct pico_eth *pico_ethernet_mcast_translate(struct pico_frame *f, uint8_t *pico_mcast_mac) 238{ 239 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 240 241 /* place 23 lower bits of IP in lower 23 bits of MAC */ 242 pico_mcast_mac[5] = (long_be(hdr->dst.addr) & 0x000000FFu); 243 pico_mcast_mac[4] = (uint8_t)((long_be(hdr->dst.addr) & 0x0000FF00u) >> 8u); 244 pico_mcast_mac[3] = (uint8_t)((long_be(hdr->dst.addr) & 0x007F0000u) >> 16u); 245 246 return (struct pico_eth *)pico_mcast_mac; 247} 248 249 250#ifdef PICO_SUPPORT_IPV6 251static struct pico_eth *pico_ethernet_mcast6_translate(struct pico_frame *f, uint8_t *pico_mcast6_mac) 252{ 253 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 254 255 /* first 2 octets are 0x33, last four are the last four of dst */ 256 pico_mcast6_mac[5] = hdr->dst.addr[PICO_SIZE_IP6 - 1]; 257 pico_mcast6_mac[4] = hdr->dst.addr[PICO_SIZE_IP6 - 2]; 258 pico_mcast6_mac[3] = hdr->dst.addr[PICO_SIZE_IP6 - 3]; 259 pico_mcast6_mac[2] = hdr->dst.addr[PICO_SIZE_IP6 - 4]; 260 261 return (struct pico_eth *)pico_mcast6_mac; 262} 263#endif 264 265static int pico_ethernet_ipv6_dst(struct pico_frame *f, struct pico_eth *const dstmac) 266{ 267 int retval = -1; 268 if (!dstmac) 269 return -1; 270 271 #ifdef PICO_SUPPORT_IPV6 272 if (destination_is_mcast(f)) { 273 uint8_t pico_mcast6_mac[6] = { 274 0x33, 0x33, 0x00, 0x00, 0x00, 0x00 275 }; 276 pico_ethernet_mcast6_translate(f, pico_mcast6_mac); 277 memcpy(dstmac, pico_mcast6_mac, PICO_SIZE_ETH); 278 retval = 0; 279 } else { 280 struct pico_eth *neighbor = pico_ipv6_get_neighbor(f); 281 if (neighbor) 282 { 283 memcpy(dstmac, neighbor, PICO_SIZE_ETH); 284 retval = 0; 285 } 286 } 287 288 #else 289 (void)f; 290 pico_err = PICO_ERR_EPROTONOSUPPORT; 291 #endif 292 return retval; 293} 294 295 296/* Ethernet send, first attempt: try our own address. 297 * Returns 0 if the packet is not for us. 298 * Returns 1 if the packet is cloned to our own receive queue, so the caller can discard the original frame. 299 * */ 300static int32_t pico_ethsend_local(struct pico_frame *f, struct pico_eth_hdr *hdr) 301{ 302 if (!hdr) 303 return 0; 304 305 /* Check own mac */ 306 if(!memcmp(hdr->daddr, hdr->saddr, PICO_SIZE_ETH)) { 307 struct pico_frame *clone = pico_frame_copy(f); 308 dbg("sending out packet destined for our own mac\n"); 309 if (pico_ethernet_receive(clone) < 0) { 310 dbg("pico_ethernet_receive() failed\n"); 311 } 312 return 1; 313 } 314 315 return 0; 316} 317 318/* Ethernet send, second attempt: try bcast. 319 * Returns 0 if the packet is not bcast, so it will be handled somewhere else. 320 * Returns 1 if the packet is handled by the pico_device_broadcast() function, so it can be discarded. 321 * */ 322static int32_t pico_ethsend_bcast(struct pico_frame *f) 323{ 324 if (IS_LIMITED_BCAST(f)) { 325 (void)pico_device_broadcast(f); /* We can discard broadcast even if it's not sent. */ 326 return 1; 327 } 328 329 return 0; 330} 331 332/* Ethernet send, third attempt: try unicast. 333 * If the device driver is busy, we return 0, so the stack won't discard the frame. 334 * In case of success, we can safely return 1. 335 */ 336static int32_t pico_ethsend_dispatch(struct pico_frame *f) 337{ 338 return (pico_sendto_dev(f) > 0); // Return 1 on success, ret > 0 339} 340 341/* Checks whether or not there's enough headroom allocated in the frame to 342 * prepend the Ethernet header. Reallocates if this is not the case. */ 343static int eth_check_headroom(struct pico_frame *f) 344{ 345 uint32_t headroom = (uint32_t)(f->net_hdr - f->buffer); 346 uint32_t grow = (uint32_t)(PICO_SIZE_ETHHDR - headroom); 347 if (headroom < (uint32_t)PICO_SIZE_ETHHDR) { 348 return pico_frame_grow_head(f, (uint32_t)(f->buffer_len + grow)); 349 } 350 return 0; 351} 352 353/* This function looks for the destination mac address 354 * in order to send the frame being processed. 355 */ 356int32_t MOCKABLE pico_ethernet_send(struct pico_frame *f) 357{ 358 struct pico_eth dstmac; 359 uint8_t dstmac_valid = 0; 360 uint16_t proto = PICO_IDETH_IPV4; 361 362#ifdef PICO_SUPPORT_IPV6 363 /* Step 1: If the frame has an IPv6 packet, 364 * destination address is taken from the ND tables 365 */ 366 if (IS_IPV6(f)) { 367 if (pico_ethernet_ipv6_dst(f, &dstmac) < 0) 368 { 369 /* Enqueue copy of frame in IPv6 ND-module to retry later. Discard 370 * frame, otherwise we have a duplicate in IPv6-ND */ 371 pico_ipv6_nd_postpone(f); 372 return (int32_t)f->len; 373 } 374 375 dstmac_valid = 1; 376 proto = PICO_IDETH_IPV6; 377 } 378 else 379#endif 380 381 /* In case of broadcast (IPV4 only), dst mac is FF:FF:... */ 382 if (IS_BCAST(f) || destination_is_bcast(f)) 383 { 384 memcpy(&dstmac, PICO_ETHADDR_ALL, PICO_SIZE_ETH); 385 dstmac_valid = 1; 386 } 387 388 /* In case of multicast, dst mac is translated from the group address */ 389 else if (destination_is_mcast(f)) { 390 uint8_t pico_mcast_mac[6] = { 391 0x01, 0x00, 0x5e, 0x00, 0x00, 0x00 392 }; 393 pico_ethernet_mcast_translate(f, pico_mcast_mac); 394 memcpy(&dstmac, pico_mcast_mac, PICO_SIZE_ETH); 395 dstmac_valid = 1; 396 } 397 398#if (defined PICO_SUPPORT_IPV4) 399 else { 400 struct pico_eth *arp_get; 401 arp_get = pico_arp_get(f); 402 if (arp_get) { 403 memcpy(&dstmac, arp_get, PICO_SIZE_ETH); 404 dstmac_valid = 1; 405 } else { 406 /* Enqueue copy of frame in ARP-module to retry later. Discard 407 * frame otherwise we have a duplicate */ 408 pico_arp_postpone(f); 409 return (int32_t)f->len; 410 } 411 } 412#endif 413 414 /* This sets destination and source address, then pushes the packet to the device. */ 415 if (dstmac_valid) { 416 struct pico_eth_hdr *hdr; 417 if (!eth_check_headroom(f)) { 418 hdr = (struct pico_eth_hdr *) f->datalink_hdr; 419 if ((f->start > f->buffer) && ((f->start - f->buffer) >= PICO_SIZE_ETHHDR)) 420 { 421 f->start -= PICO_SIZE_ETHHDR; 422 f->len += PICO_SIZE_ETHHDR; 423 f->datalink_hdr = f->start; 424 hdr = (struct pico_eth_hdr *) f->datalink_hdr; 425 memcpy(hdr->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH); 426 memcpy(hdr->daddr, &dstmac, PICO_SIZE_ETH); 427 hdr->proto = proto; 428 } 429 430 if (pico_ethsend_local(f, hdr) || pico_ethsend_bcast(f) || pico_ethsend_dispatch(f)) { 431 /* one of the above functions has delivered the frame accordingly. 432 * (returned != 0). It is safe to directly return successfully. 433 * Lower level queue has frame, so don't discard */ 434 return (int32_t)f->len; 435 } 436 } 437 } 438 439 /* Failure, frame could not be be enqueued in lower-level layer, safe 440 * to discard since something clearly went wrong */ 441 pico_frame_discard(f); 442 return 0; 443} 444 445#endif /* PICO_SUPPORT_ETH */ 446 447 448