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 = &ethernet_in,
100    .q_out = &ethernet_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