1171 lines
28 KiB
C
1171 lines
28 KiB
C
/*********************************************************************
|
|
PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
|
|
See LICENSE and COPYING for usage.
|
|
|
|
.
|
|
|
|
Authors: Daniele Lacamera
|
|
*********************************************************************/
|
|
|
|
|
|
#include "pico_config.h"
|
|
#include "pico_frame.h"
|
|
#include "pico_device.h"
|
|
#include "pico_protocol.h"
|
|
#include "pico_stack.h"
|
|
#include "pico_addressing.h"
|
|
#include "pico_dns_client.h"
|
|
|
|
#include "pico_olsr.h"
|
|
#include "pico_aodv.h"
|
|
#include "pico_eth.h"
|
|
#include "pico_arp.h"
|
|
#include "pico_ipv4.h"
|
|
#include "pico_ipv6.h"
|
|
#include "pico_icmp4.h"
|
|
#include "pico_icmp6.h"
|
|
#include "pico_igmp.h"
|
|
#include "pico_udp.h"
|
|
#include "pico_tcp.h"
|
|
#include "pico_socket.h"
|
|
#include "heap.h"
|
|
|
|
#define IS_LIMITED_BCAST(f) (((struct pico_ipv4_hdr *) f->net_hdr)->dst.addr == PICO_IP4_BCAST)
|
|
|
|
const uint8_t PICO_ETHADDR_ALL[6] = {
|
|
0xff, 0xff, 0xff, 0xff, 0xff, 0xff
|
|
};
|
|
|
|
# define PICO_SIZE_MCAST 3
|
|
const uint8_t PICO_ETHADDR_MCAST[6] = {
|
|
0x01, 0x00, 0x5e, 0x00, 0x00, 0x00
|
|
};
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
# define PICO_SIZE_MCAST6 2
|
|
const uint8_t PICO_ETHADDR_MCAST6[6] = {
|
|
0x33, 0x33, 0x00, 0x00, 0x00, 0x00
|
|
};
|
|
#endif
|
|
|
|
|
|
volatile pico_time pico_tick;
|
|
volatile pico_err_t pico_err;
|
|
|
|
static uint32_t _rand_seed;
|
|
|
|
void WEAK pico_rand_feed(uint32_t feed)
|
|
{
|
|
if (!feed)
|
|
return;
|
|
|
|
_rand_seed *= 1664525;
|
|
_rand_seed += 1013904223;
|
|
_rand_seed ^= ~(feed);
|
|
}
|
|
|
|
uint32_t WEAK pico_rand(void)
|
|
{
|
|
pico_rand_feed((uint32_t)pico_tick);
|
|
return _rand_seed;
|
|
}
|
|
|
|
void pico_to_lowercase(char *str)
|
|
{
|
|
int i = 0;
|
|
if (!str)
|
|
return;
|
|
|
|
while(str[i]) {
|
|
if ((str[i] <= 'Z') && (str[i] >= 'A'))
|
|
str[i] = (char) (str[i] - (char)('A' - 'a'));
|
|
|
|
i++;
|
|
}
|
|
}
|
|
|
|
/* NOTIFICATIONS: distributed notifications for stack internal errors.
|
|
*/
|
|
|
|
int pico_notify_socket_unreachable(struct pico_frame *f)
|
|
{
|
|
if (0) {}
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
else if (IS_IPV4(f)) {
|
|
pico_icmp4_port_unreachable(f);
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
else if (IS_IPV6(f)) {
|
|
pico_icmp6_port_unreachable(f);
|
|
}
|
|
#endif
|
|
|
|
return 0;
|
|
}
|
|
|
|
int pico_notify_proto_unreachable(struct pico_frame *f)
|
|
{
|
|
if (0) {}
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
else if (IS_IPV4(f)) {
|
|
pico_icmp4_proto_unreachable(f);
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
else if (IS_IPV6(f)) {
|
|
pico_icmp6_proto_unreachable(f);
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
int pico_notify_dest_unreachable(struct pico_frame *f)
|
|
{
|
|
if (0) {}
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
else if (IS_IPV4(f)) {
|
|
pico_icmp4_dest_unreachable(f);
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
else if (IS_IPV6(f)) {
|
|
pico_icmp6_dest_unreachable(f);
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
int pico_notify_ttl_expired(struct pico_frame *f)
|
|
{
|
|
if (0) {}
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
else if (IS_IPV4(f)) {
|
|
pico_icmp4_ttl_expired(f);
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
else if (IS_IPV6(f)) {
|
|
pico_icmp6_ttl_expired(f);
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
int pico_notify_frag_expired(struct pico_frame *f)
|
|
{
|
|
if (0) {}
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
else if (IS_IPV4(f)) {
|
|
pico_icmp4_frag_expired(f);
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
else if (IS_IPV6(f)) {
|
|
pico_icmp6_frag_expired(f);
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
int pico_notify_pkt_too_big(struct pico_frame *f)
|
|
{
|
|
if (0) {}
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
else if (IS_IPV4(f)) {
|
|
pico_icmp4_mtu_exceeded(f);
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
else if (IS_IPV6(f)) {
|
|
pico_icmp6_pkt_too_big(f);
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
|
|
/* Transport layer */
|
|
MOCKABLE int32_t pico_transport_receive(struct pico_frame *f, uint8_t proto)
|
|
{
|
|
int32_t ret = -1;
|
|
switch (proto) {
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
case PICO_PROTO_ICMP4:
|
|
ret = pico_enqueue(pico_proto_icmp4.q_in, f);
|
|
break;
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
case PICO_PROTO_ICMP6:
|
|
ret = pico_enqueue(pico_proto_icmp6.q_in, f);
|
|
break;
|
|
#endif
|
|
|
|
|
|
#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
|
|
case PICO_PROTO_IGMP:
|
|
ret = pico_enqueue(pico_proto_igmp.q_in, f);
|
|
break;
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_UDP
|
|
case PICO_PROTO_UDP:
|
|
ret = pico_enqueue(pico_proto_udp.q_in, f);
|
|
break;
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_TCP
|
|
case PICO_PROTO_TCP:
|
|
ret = pico_enqueue(pico_proto_tcp.q_in, f);
|
|
break;
|
|
#endif
|
|
|
|
default:
|
|
/* Protocol not available */
|
|
dbg("pkt: no such protocol (%d)\n", proto);
|
|
pico_notify_proto_unreachable(f);
|
|
pico_frame_discard(f);
|
|
ret = -1;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
int32_t pico_network_receive(struct pico_frame *f)
|
|
{
|
|
if (0) {}
|
|
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
else if (IS_IPV4(f)) {
|
|
pico_enqueue(pico_proto_ipv4.q_in, f);
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
else if (IS_IPV6(f)) {
|
|
pico_enqueue(pico_proto_ipv6.q_in, f);
|
|
}
|
|
#endif
|
|
else {
|
|
dbg("Network not found.\n");
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
return (int32_t)f->buffer_len;
|
|
}
|
|
|
|
/* Network layer: interface towards socket for frame sending */
|
|
int32_t pico_network_send(struct pico_frame *f)
|
|
{
|
|
if (!f || !f->sock || !f->sock->net) {
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
return f->sock->net->push(f->sock->net, f);
|
|
}
|
|
|
|
int pico_source_is_local(struct pico_frame *f)
|
|
{
|
|
if (0) { }
|
|
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
else if (IS_IPV4(f)) {
|
|
struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
|
|
if (hdr->src.addr == PICO_IPV4_INADDR_ANY)
|
|
return 1;
|
|
|
|
if (pico_ipv4_link_find(&hdr->src))
|
|
return 1;
|
|
}
|
|
#endif
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
else if (IS_IPV6(f)) {
|
|
struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
|
|
if (pico_ipv6_is_unspecified(hdr->src.addr) || pico_ipv6_link_find(&hdr->src))
|
|
return 1;
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
#ifdef PICO_SUPPORT_ETH
|
|
/* DATALINK LEVEL: interface from network to the device
|
|
* and vice versa.
|
|
*/
|
|
|
|
/* The pico_ethernet_receive() function is used by
|
|
* those devices supporting ETH in order to push packets up
|
|
* into the stack.
|
|
*/
|
|
|
|
static int destination_is_bcast(struct pico_frame *f)
|
|
{
|
|
if (!f)
|
|
return 0;
|
|
|
|
if (IS_IPV6(f))
|
|
return 0;
|
|
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
else {
|
|
struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
|
|
return pico_ipv4_is_broadcast(hdr->dst.addr);
|
|
}
|
|
#else
|
|
return 0;
|
|
#endif
|
|
}
|
|
|
|
static int destination_is_mcast(struct pico_frame *f)
|
|
{
|
|
int ret = 0;
|
|
if (!f)
|
|
return 0;
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
if (IS_IPV6(f)) {
|
|
struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr;
|
|
ret = pico_ipv6_is_multicast(hdr->dst.addr);
|
|
}
|
|
|
|
#endif
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
else {
|
|
struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
|
|
ret = pico_ipv4_is_multicast(hdr->dst.addr);
|
|
}
|
|
#endif
|
|
|
|
return ret;
|
|
}
|
|
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
static int32_t pico_ipv4_ethernet_receive(struct pico_frame *f)
|
|
{
|
|
if (IS_IPV4(f)) {
|
|
pico_enqueue(pico_proto_ipv4.q_in, f);
|
|
} else {
|
|
(void)pico_icmp4_param_problem(f, 0);
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
return (int32_t)f->buffer_len;
|
|
}
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
static int32_t pico_ipv6_ethernet_receive(struct pico_frame *f)
|
|
{
|
|
if (IS_IPV6(f)) {
|
|
pico_enqueue(pico_proto_ipv6.q_in, f);
|
|
} else {
|
|
/* Wrong version for link layer type */
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
return (int32_t)f->buffer_len;
|
|
}
|
|
#endif
|
|
|
|
static int32_t pico_ll_receive(struct pico_frame *f)
|
|
{
|
|
struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
|
|
f->net_hdr = f->datalink_hdr + sizeof(struct pico_eth_hdr);
|
|
|
|
#if (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH)
|
|
if (hdr->proto == PICO_IDETH_ARP)
|
|
return pico_arp_receive(f);
|
|
|
|
#endif
|
|
|
|
#if defined (PICO_SUPPORT_IPV4)
|
|
if (hdr->proto == PICO_IDETH_IPV4)
|
|
return pico_ipv4_ethernet_receive(f);
|
|
|
|
#endif
|
|
|
|
#if defined (PICO_SUPPORT_IPV6)
|
|
if (hdr->proto == PICO_IDETH_IPV6)
|
|
return pico_ipv6_ethernet_receive(f);
|
|
|
|
#endif
|
|
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
static void pico_ll_check_bcast(struct pico_frame *f)
|
|
{
|
|
struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
|
|
/* Indicate a link layer broadcast packet */
|
|
if (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) == 0)
|
|
f->flags |= PICO_FRAME_FLAG_BCAST;
|
|
}
|
|
|
|
int32_t pico_ethernet_receive(struct pico_frame *f)
|
|
{
|
|
struct pico_eth_hdr *hdr;
|
|
if (!f || !f->dev || !f->datalink_hdr)
|
|
{
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
hdr = (struct pico_eth_hdr *) f->datalink_hdr;
|
|
if ((memcmp(hdr->daddr, f->dev->eth->mac.addr, PICO_SIZE_ETH) != 0) &&
|
|
(memcmp(hdr->daddr, PICO_ETHADDR_MCAST, PICO_SIZE_MCAST) != 0) &&
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
(memcmp(hdr->daddr, PICO_ETHADDR_MCAST6, PICO_SIZE_MCAST6) != 0) &&
|
|
#endif
|
|
(memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) != 0))
|
|
{
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
pico_ll_check_bcast(f);
|
|
return pico_ll_receive(f);
|
|
}
|
|
|
|
static struct pico_eth *pico_ethernet_mcast_translate(struct pico_frame *f, uint8_t *pico_mcast_mac)
|
|
{
|
|
struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
|
|
|
|
/* place 23 lower bits of IP in lower 23 bits of MAC */
|
|
pico_mcast_mac[5] = (long_be(hdr->dst.addr) & 0x000000FFu);
|
|
pico_mcast_mac[4] = (uint8_t)((long_be(hdr->dst.addr) & 0x0000FF00u) >> 8u);
|
|
pico_mcast_mac[3] = (uint8_t)((long_be(hdr->dst.addr) & 0x007F0000u) >> 16u);
|
|
|
|
return (struct pico_eth *)pico_mcast_mac;
|
|
}
|
|
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
static struct pico_eth *pico_ethernet_mcast6_translate(struct pico_frame *f, uint8_t *pico_mcast6_mac)
|
|
{
|
|
struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
|
|
|
|
/* first 2 octets are 0x33, last four are the last four of dst */
|
|
pico_mcast6_mac[5] = hdr->dst.addr[PICO_SIZE_IP6 - 1];
|
|
pico_mcast6_mac[4] = hdr->dst.addr[PICO_SIZE_IP6 - 2];
|
|
pico_mcast6_mac[3] = hdr->dst.addr[PICO_SIZE_IP6 - 3];
|
|
pico_mcast6_mac[2] = hdr->dst.addr[PICO_SIZE_IP6 - 4];
|
|
|
|
return (struct pico_eth *)pico_mcast6_mac;
|
|
}
|
|
#endif
|
|
|
|
static int pico_ethernet_ipv6_dst(struct pico_frame *f, struct pico_eth *const dstmac)
|
|
{
|
|
int retval = -1;
|
|
if (!dstmac)
|
|
return -1;
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
if (destination_is_mcast(f)) {
|
|
uint8_t pico_mcast6_mac[6] = {
|
|
0x33, 0x33, 0x00, 0x00, 0x00, 0x00
|
|
};
|
|
pico_ethernet_mcast6_translate(f, pico_mcast6_mac);
|
|
memcpy(dstmac, pico_mcast6_mac, PICO_SIZE_ETH);
|
|
retval = 0;
|
|
} else {
|
|
struct pico_eth *neighbor = pico_ipv6_get_neighbor(f);
|
|
if (neighbor)
|
|
{
|
|
memcpy(dstmac, neighbor, PICO_SIZE_ETH);
|
|
retval = 0;
|
|
}
|
|
}
|
|
|
|
#else
|
|
(void)f;
|
|
pico_err = PICO_ERR_EPROTONOSUPPORT;
|
|
#endif
|
|
return retval;
|
|
}
|
|
|
|
|
|
/* Ethernet send, first attempt: try our own address.
|
|
* Returns 0 if the packet is not for us.
|
|
* Returns 1 if the packet is cloned to our own receive queue, so the caller can discard the original frame.
|
|
* */
|
|
static int32_t pico_ethsend_local(struct pico_frame *f, struct pico_eth_hdr *hdr)
|
|
{
|
|
if (!hdr)
|
|
return 0;
|
|
|
|
/* Check own mac */
|
|
if(!memcmp(hdr->daddr, hdr->saddr, PICO_SIZE_ETH)) {
|
|
struct pico_frame *clone = pico_frame_copy(f);
|
|
dbg("sending out packet destined for our own mac\n");
|
|
(void)pico_ethernet_receive(clone);
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Ethernet send, second attempt: try bcast.
|
|
* Returns 0 if the packet is not bcast, so it will be handled somewhere else.
|
|
* Returns 1 if the packet is handled by the pico_device_broadcast() function, so it can be discarded.
|
|
* */
|
|
static int32_t pico_ethsend_bcast(struct pico_frame *f)
|
|
{
|
|
if (IS_LIMITED_BCAST(f)) {
|
|
(void)pico_device_broadcast(f); /* We can discard broadcast even if it's not sent. */
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Ethernet send, third attempt: try unicast.
|
|
* If the device driver is busy, we return 0, so the stack won't discard the frame.
|
|
* In case of success, we can safely return 1.
|
|
*/
|
|
static int32_t pico_ethsend_dispatch(struct pico_frame *f)
|
|
{
|
|
int ret = f->dev->send(f->dev, f->start, (int) f->len);
|
|
if (ret <= 0)
|
|
return 0; /* Failure to deliver! */
|
|
else {
|
|
return 1; /* Frame is in flight by now. */
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
/* This function looks for the destination mac address
|
|
* in order to send the frame being processed.
|
|
*/
|
|
|
|
int32_t MOCKABLE pico_ethernet_send(struct pico_frame *f)
|
|
{
|
|
struct pico_eth dstmac;
|
|
uint8_t dstmac_valid = 0;
|
|
uint16_t proto = PICO_IDETH_IPV4;
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
/* Step 1: If the frame has an IPv6 packet,
|
|
* destination address is taken from the ND tables
|
|
*/
|
|
if (IS_IPV6(f)) {
|
|
if (pico_ethernet_ipv6_dst(f, &dstmac) < 0)
|
|
{
|
|
pico_ipv6_nd_postpone(f);
|
|
return 0; /* I don't care if frame was actually postponed. If there is no room in the ND table, discard safely. */
|
|
}
|
|
|
|
dstmac_valid = 1;
|
|
proto = PICO_IDETH_IPV6;
|
|
}
|
|
else
|
|
#endif
|
|
|
|
/* In case of broadcast (IPV4 only), dst mac is FF:FF:... */
|
|
if (IS_BCAST(f) || destination_is_bcast(f))
|
|
{
|
|
memcpy(&dstmac, PICO_ETHADDR_ALL, PICO_SIZE_ETH);
|
|
dstmac_valid = 1;
|
|
}
|
|
|
|
/* In case of multicast, dst mac is translated from the group address */
|
|
else if (destination_is_mcast(f)) {
|
|
uint8_t pico_mcast_mac[6] = {
|
|
0x01, 0x00, 0x5e, 0x00, 0x00, 0x00
|
|
};
|
|
pico_ethernet_mcast_translate(f, pico_mcast_mac);
|
|
memcpy(&dstmac, pico_mcast_mac, PICO_SIZE_ETH);
|
|
dstmac_valid = 1;
|
|
}
|
|
|
|
#if (defined PICO_SUPPORT_IPV4)
|
|
else {
|
|
struct pico_eth *arp_get;
|
|
arp_get = pico_arp_get(f);
|
|
if (arp_get) {
|
|
memcpy(&dstmac, arp_get, PICO_SIZE_ETH);
|
|
dstmac_valid = 1;
|
|
} else {
|
|
/* At this point, ARP will discard the frame in any case.
|
|
* It is safe to return without discarding.
|
|
*/
|
|
pico_arp_postpone(f);
|
|
return 0;
|
|
/* Same case as for IPv6 ... */
|
|
}
|
|
|
|
}
|
|
#endif
|
|
|
|
/* This sets destination and source address, then pushes the packet to the device. */
|
|
if (dstmac_valid) {
|
|
struct pico_eth_hdr *hdr;
|
|
hdr = (struct pico_eth_hdr *) f->datalink_hdr;
|
|
if ((f->start > f->buffer) && ((f->start - f->buffer) >= PICO_SIZE_ETHHDR))
|
|
{
|
|
f->start -= PICO_SIZE_ETHHDR;
|
|
f->len += PICO_SIZE_ETHHDR;
|
|
f->datalink_hdr = f->start;
|
|
hdr = (struct pico_eth_hdr *) f->datalink_hdr;
|
|
memcpy(hdr->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH);
|
|
memcpy(hdr->daddr, &dstmac, PICO_SIZE_ETH);
|
|
hdr->proto = proto;
|
|
}
|
|
|
|
if (pico_ethsend_local(f, hdr) || pico_ethsend_bcast(f) || pico_ethsend_dispatch(f)) {
|
|
/* one of the above functions has delivered the frame accordingly. (returned != 0)
|
|
* It is safe to directly return success.
|
|
* */
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/* Failure: do not dequeue the frame, keep it for later. */
|
|
return -1;
|
|
}
|
|
|
|
#endif /* PICO_SUPPORT_ETH */
|
|
|
|
|
|
void pico_store_network_origin(void *src, struct pico_frame *f)
|
|
{
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
struct pico_ip4 *ip4;
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
struct pico_ip6 *ip6;
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
if (IS_IPV4(f)) {
|
|
struct pico_ipv4_hdr *hdr;
|
|
hdr = (struct pico_ipv4_hdr *) f->net_hdr;
|
|
ip4 = (struct pico_ip4 *) src;
|
|
ip4->addr = hdr->src.addr;
|
|
}
|
|
|
|
#endif
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
if (IS_IPV6(f)) {
|
|
struct pico_ipv6_hdr *hdr;
|
|
hdr = (struct pico_ipv6_hdr *) f->net_hdr;
|
|
ip6 = (struct pico_ip6 *) src;
|
|
memcpy(ip6->addr, hdr->src.addr, PICO_SIZE_IP6);
|
|
}
|
|
|
|
#endif
|
|
}
|
|
|
|
int pico_address_compare(union pico_address *a, union pico_address *b, uint16_t proto)
|
|
{
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
if (proto == PICO_PROTO_IPV6) {
|
|
return pico_ipv6_compare(&a->ip6, &b->ip6);
|
|
}
|
|
|
|
#endif
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
if (proto == PICO_PROTO_IPV4) {
|
|
return pico_ipv4_compare(&a->ip4, &b->ip4);
|
|
}
|
|
|
|
#endif
|
|
return 0;
|
|
|
|
}
|
|
|
|
int pico_frame_dst_is_unicast(struct pico_frame *f)
|
|
{
|
|
if (0) {
|
|
return 0;
|
|
}
|
|
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
if (IS_IPV4(f)) {
|
|
struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
|
|
if (pico_ipv4_is_multicast(hdr->dst.addr) || pico_ipv4_is_broadcast(hdr->dst.addr))
|
|
return 0;
|
|
|
|
return 1;
|
|
}
|
|
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
if (IS_IPV6(f)) {
|
|
struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
|
|
if (pico_ipv6_is_multicast(hdr->dst.addr) || pico_ipv6_is_unspecified(hdr->dst.addr))
|
|
return 0;
|
|
|
|
return 1;
|
|
}
|
|
|
|
#endif
|
|
else return 0;
|
|
}
|
|
|
|
|
|
/* LOWEST LEVEL: interface towards devices. */
|
|
/* Device driver will call this function which returns immediately.
|
|
* Incoming packet will be processed later on in the dev loop.
|
|
*/
|
|
extern int32_t pico_stack_recv(struct pico_device *dev, uint8_t *buffer, uint32_t len)
|
|
{
|
|
struct pico_frame *f;
|
|
int32_t ret;
|
|
if (len == 0)
|
|
return -1;
|
|
|
|
f = pico_frame_alloc(len);
|
|
if (!f)
|
|
{
|
|
dbg("Cannot alloc incoming frame!\n");
|
|
return -1;
|
|
}
|
|
|
|
/* Association to the device that just received the frame. */
|
|
f->dev = dev;
|
|
|
|
/* Setup the start pointer, length. */
|
|
f->start = f->buffer;
|
|
f->len = f->buffer_len;
|
|
if (f->len > 8) {
|
|
uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
|
|
mid_frame -= (mid_frame % 4);
|
|
memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
|
|
pico_rand_feed(rand);
|
|
}
|
|
|
|
memcpy(f->buffer, buffer, len);
|
|
ret = pico_enqueue(dev->q_in, f);
|
|
if (ret <= 0) {
|
|
pico_frame_discard(f);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int32_t _pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len, int ext_buffer, void (*notify_free)(uint8_t *))
|
|
{
|
|
struct pico_frame *f;
|
|
int ret;
|
|
if (len == 0)
|
|
return -1;
|
|
|
|
f = pico_frame_alloc_skeleton(len, ext_buffer);
|
|
if (!f)
|
|
{
|
|
dbg("Cannot alloc incoming frame!\n");
|
|
return -1;
|
|
}
|
|
|
|
if (pico_frame_skeleton_set_buffer(f, buffer) < 0)
|
|
{
|
|
dbg("Invalid zero-copy buffer!\n");
|
|
PICO_FREE(f->usage_count);
|
|
PICO_FREE(f);
|
|
return -1;
|
|
}
|
|
|
|
if (notify_free) {
|
|
f->notify_free = notify_free;
|
|
}
|
|
|
|
f->dev = dev;
|
|
ret = pico_enqueue(dev->q_in, f);
|
|
if (ret <= 0) {
|
|
pico_frame_discard(f);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int32_t pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len)
|
|
{
|
|
return _pico_stack_recv_zerocopy(dev, buffer, len, 0, NULL);
|
|
}
|
|
|
|
int32_t pico_stack_recv_zerocopy_ext_buffer(struct pico_device *dev, uint8_t *buffer, uint32_t len)
|
|
{
|
|
return _pico_stack_recv_zerocopy(dev, buffer, len, 1, NULL);
|
|
}
|
|
|
|
int32_t pico_stack_recv_zerocopy_ext_buffer_notify(struct pico_device *dev, uint8_t *buffer, uint32_t len, void (*notify_free)(uint8_t *buffer))
|
|
{
|
|
return _pico_stack_recv_zerocopy(dev, buffer, len, 1, notify_free);
|
|
}
|
|
|
|
int32_t pico_sendto_dev(struct pico_frame *f)
|
|
{
|
|
if (!f->dev) {
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
} else {
|
|
if (f->len > 8) {
|
|
uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
|
|
mid_frame -= (mid_frame % 4);
|
|
memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
|
|
pico_rand_feed(rand);
|
|
}
|
|
|
|
return pico_enqueue(f->dev->q_out, f);
|
|
}
|
|
}
|
|
|
|
struct pico_timer
|
|
{
|
|
void *arg;
|
|
void (*timer)(pico_time timestamp, void *arg);
|
|
};
|
|
|
|
|
|
static uint32_t tmr_id = 0u;
|
|
struct pico_timer_ref
|
|
{
|
|
pico_time expire;
|
|
uint32_t id;
|
|
struct pico_timer *tmr;
|
|
};
|
|
|
|
typedef struct pico_timer_ref pico_timer_ref;
|
|
|
|
DECLARE_HEAP(pico_timer_ref, expire);
|
|
|
|
static heap_pico_timer_ref *Timers;
|
|
|
|
int32_t pico_seq_compare(uint32_t a, uint32_t b)
|
|
{
|
|
uint32_t thresh = ((uint32_t)(-1)) >> 1;
|
|
|
|
if (a > b) /* return positive number, if not wrapped */
|
|
{
|
|
if ((a - b) > thresh) /* b wrapped */
|
|
return -(int32_t)(b - a); /* b = very small, a = very big */
|
|
else
|
|
return (int32_t)(a - b); /* a = biggest, b = a bit smaller */
|
|
|
|
}
|
|
|
|
if (a < b) /* return negative number, if not wrapped */
|
|
{
|
|
if ((b - a) > thresh) /* a wrapped */
|
|
return (int32_t)(a - b); /* a = very small, b = very big */
|
|
else
|
|
return -(int32_t)(b - a); /* b = biggest, a = a bit smaller */
|
|
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
extern int pico_ntimers()
|
|
{
|
|
return Timers->n;
|
|
}
|
|
|
|
static void pico_check_timers(void)
|
|
{
|
|
struct pico_timer *t;
|
|
struct pico_timer_ref tref_unused, *tref = heap_first(Timers);
|
|
pico_tick = PICO_TIME_MS();
|
|
while((tref) && (tref->expire < pico_tick)) {
|
|
t = tref->tmr;
|
|
if (t && t->timer)
|
|
t->timer(pico_tick, t->arg);
|
|
|
|
if (t)
|
|
{
|
|
PICO_FREE(t);
|
|
}
|
|
|
|
t = NULL;
|
|
heap_peek(Timers, &tref_unused);
|
|
tref = heap_first(Timers);
|
|
}
|
|
}
|
|
|
|
void MOCKABLE pico_timer_cancel(uint32_t id)
|
|
{
|
|
uint32_t i;
|
|
struct pico_timer_ref *tref = Timers->top;
|
|
if (id == 0u)
|
|
return;
|
|
for (i = 1; i <= Timers->n; i++) {
|
|
if (tref[i].id == id) {
|
|
PICO_FREE(Timers->top[i].tmr);
|
|
Timers->top[i].tmr = NULL;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
#define PROTO_DEF_NR 11
|
|
#define PROTO_DEF_AVG_NR 4
|
|
#define PROTO_DEF_SCORE 32
|
|
#define PROTO_MIN_SCORE 32
|
|
#define PROTO_MAX_SCORE 128
|
|
#define PROTO_LAT_IND 3 /* latency indication 0-3 (lower is better latency performance), x1, x2, x4, x8 */
|
|
#define PROTO_MAX_LOOP (PROTO_MAX_SCORE << PROTO_LAT_IND) /* max global loop score, so per tick */
|
|
|
|
static int calc_score(int *score, int *index, int avg[][PROTO_DEF_AVG_NR], int *ret)
|
|
{
|
|
int temp, i, j, sum;
|
|
int max_total = PROTO_MAX_LOOP, total = 0;
|
|
|
|
/* dbg("USED SCORES> "); */
|
|
|
|
for (i = 0; i < PROTO_DEF_NR; i++) {
|
|
|
|
/* if used looped score */
|
|
if (ret[i] < score[i]) {
|
|
temp = score[i] - ret[i]; /* remaining loop score */
|
|
|
|
/* dbg("%3d - ",temp); */
|
|
|
|
if (index[i] >= PROTO_DEF_AVG_NR)
|
|
index[i] = 0; /* reset index */
|
|
|
|
j = index[i];
|
|
avg[i][j] = temp;
|
|
|
|
index[i]++;
|
|
|
|
if (ret[i] == 0 && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] * 2)) < max_total)) { /* used all loop score -> increase next score directly */
|
|
score[i] *= 2;
|
|
total += score[i];
|
|
continue;
|
|
}
|
|
|
|
sum = 0;
|
|
for (j = 0; j < PROTO_DEF_AVG_NR; j++)
|
|
sum += avg[i][j]; /* calculate sum */
|
|
|
|
sum /= 4; /* divide by 4 to get average used score */
|
|
|
|
/* criterion to increase next loop score */
|
|
if (sum > (score[i] - (score[i] / 4)) && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] / 2)) < max_total)) { /* > 3/4 */
|
|
score[i] *= 2; /* double loop score */
|
|
total += score[i];
|
|
continue;
|
|
}
|
|
|
|
/* criterion to decrease next loop score */
|
|
if ((sum < (score[i] / 4)) && ((score[i] / 2) >= PROTO_MIN_SCORE)) { /* < 1/4 */
|
|
score[i] /= 2; /* half loop score */
|
|
total += score[i];
|
|
continue;
|
|
}
|
|
|
|
/* also add non-changed scores */
|
|
total += score[i];
|
|
}
|
|
else if (ret[i] == score[i]) {
|
|
/* no used loop score - gradually decrease */
|
|
|
|
/* dbg("%3d - ",0); */
|
|
|
|
if (index[i] >= PROTO_DEF_AVG_NR)
|
|
index[i] = 0; /* reset index */
|
|
|
|
j = index[i];
|
|
avg[i][j] = 0;
|
|
|
|
index[i]++;
|
|
|
|
sum = 0;
|
|
for (j = 0; j < PROTO_DEF_AVG_NR; j++)
|
|
sum += avg[i][j]; /* calculate sum */
|
|
|
|
sum /= 2; /* divide by 4 to get average used score */
|
|
|
|
if ((sum == 0) && ((score[i] / 2) >= PROTO_MIN_SCORE)) {
|
|
score[i] /= 2; /* half loop score */
|
|
total += score[i];
|
|
for (j = 0; j < PROTO_DEF_AVG_NR; j++)
|
|
avg[i][j] = score[i];
|
|
}
|
|
|
|
}
|
|
}
|
|
/* dbg("\n"); */
|
|
|
|
return 0;
|
|
}
|
|
|
|
extern void pico_stack_tick(void)
|
|
{
|
|
static int score[PROTO_DEF_NR] = {
|
|
PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE
|
|
};
|
|
static int index[PROTO_DEF_NR] = {
|
|
0, 0, 0, 0, 0, 0
|
|
};
|
|
static int avg[PROTO_DEF_NR][PROTO_DEF_AVG_NR];
|
|
static int ret[PROTO_DEF_NR] = {
|
|
0
|
|
};
|
|
|
|
pico_check_timers();
|
|
|
|
/* dbg("LOOP_SCORES> %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d\n",score[0],score[1],score[2],score[3],score[4],score[5],score[6],score[7],score[8],score[9],score[10]); */
|
|
|
|
/* score = pico_protocols_loop(100); */
|
|
|
|
ret[0] = pico_devices_loop(score[0], PICO_LOOP_DIR_IN);
|
|
pico_rand_feed((uint32_t)ret[0]);
|
|
|
|
ret[1] = pico_protocol_datalink_loop(score[1], PICO_LOOP_DIR_IN);
|
|
pico_rand_feed((uint32_t)ret[1]);
|
|
|
|
ret[2] = pico_protocol_network_loop(score[2], PICO_LOOP_DIR_IN);
|
|
pico_rand_feed((uint32_t)ret[2]);
|
|
|
|
ret[3] = pico_protocol_transport_loop(score[3], PICO_LOOP_DIR_IN);
|
|
pico_rand_feed((uint32_t)ret[3]);
|
|
|
|
|
|
ret[5] = score[5];
|
|
#if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6)
|
|
#if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP)
|
|
ret[5] = pico_sockets_loop(score[5]); /* swapped */
|
|
pico_rand_feed((uint32_t)ret[5]);
|
|
#endif
|
|
#endif
|
|
|
|
ret[4] = pico_protocol_socket_loop(score[4], PICO_LOOP_DIR_IN);
|
|
pico_rand_feed((uint32_t)ret[4]);
|
|
|
|
|
|
ret[6] = pico_protocol_socket_loop(score[6], PICO_LOOP_DIR_OUT);
|
|
pico_rand_feed((uint32_t)ret[6]);
|
|
|
|
ret[7] = pico_protocol_transport_loop(score[7], PICO_LOOP_DIR_OUT);
|
|
pico_rand_feed((uint32_t)ret[7]);
|
|
|
|
ret[8] = pico_protocol_network_loop(score[8], PICO_LOOP_DIR_OUT);
|
|
pico_rand_feed((uint32_t)ret[8]);
|
|
|
|
ret[9] = pico_protocol_datalink_loop(score[9], PICO_LOOP_DIR_OUT);
|
|
pico_rand_feed((uint32_t)ret[9]);
|
|
|
|
ret[10] = pico_devices_loop(score[10], PICO_LOOP_DIR_OUT);
|
|
pico_rand_feed((uint32_t)ret[10]);
|
|
|
|
/* calculate new loop scores for next iteration */
|
|
calc_score(score, index, (int (*)[])avg, ret);
|
|
}
|
|
|
|
void pico_stack_loop(void)
|
|
{
|
|
while(1) {
|
|
pico_stack_tick();
|
|
PICO_IDLE();
|
|
}
|
|
}
|
|
|
|
MOCKABLE uint32_t pico_timer_add(pico_time expire, void (*timer)(pico_time, void *), void *arg)
|
|
{
|
|
struct pico_timer *t = PICO_ZALLOC(sizeof(struct pico_timer));
|
|
struct pico_timer_ref tref;
|
|
|
|
/* zero is guard for timers */
|
|
if (tmr_id == 0u)
|
|
tmr_id++;
|
|
|
|
if (!t) {
|
|
pico_err = PICO_ERR_ENOMEM;
|
|
return 0;
|
|
}
|
|
|
|
tref.expire = PICO_TIME_MS() + expire;
|
|
t->arg = arg;
|
|
t->timer = timer;
|
|
tref.tmr = t;
|
|
tref.id = tmr_id++;
|
|
heap_insert(Timers, &tref);
|
|
if (Timers->n > PICO_MAX_TIMERS) {
|
|
dbg("Warning: I have %d timers\n", (int)Timers->n);
|
|
}
|
|
|
|
return tref.id;
|
|
}
|
|
|
|
extern int pico_stack_init(void)
|
|
{
|
|
|
|
#ifdef PICO_SUPPORT_IPV4
|
|
pico_protocol_init(&pico_proto_ipv4);
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
pico_protocol_init(&pico_proto_ipv6);
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_ICMP4
|
|
pico_protocol_init(&pico_proto_icmp4);
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_ICMP6
|
|
pico_protocol_init(&pico_proto_icmp6);
|
|
#endif
|
|
|
|
#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
|
|
pico_protocol_init(&pico_proto_igmp);
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_UDP
|
|
pico_protocol_init(&pico_proto_udp);
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_TCP
|
|
pico_protocol_init(&pico_proto_tcp);
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_DNS_CLIENT
|
|
pico_dns_client_init();
|
|
#endif
|
|
|
|
pico_rand_feed(123456);
|
|
|
|
/* Initialize timer heap */
|
|
Timers = heap_init();
|
|
if (!Timers)
|
|
return -1;
|
|
|
|
#if ((defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH))
|
|
/* Initialize ARP module */
|
|
pico_arp_init();
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_IPV6
|
|
/* Initialize Neighbor discovery module */
|
|
pico_ipv6_nd_init();
|
|
#endif
|
|
|
|
#ifdef PICO_SUPPORT_OLSR
|
|
pico_olsr_init();
|
|
#endif
|
|
#ifdef PICO_SUPPORT_AODV
|
|
pico_aodv_init();
|
|
#endif
|
|
|
|
pico_stack_tick();
|
|
pico_stack_tick();
|
|
pico_stack_tick();
|
|
return 0;
|
|
}
|
|
|