ShuriZma
/
flycast
mirror of https://github.com/flyinghead/flycast.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
flycast/core/deps/picotcp/stack/pico_device.c

483 lines
13 KiB
C
Raw Blame History

/*********************************************************************
PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved.
See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage.
.
Authors: Daniele Lacamera
*********************************************************************/
#include "pico_config.h"
#include "pico_device.h"
#include "pico_stack.h"
#include "pico_protocol.h"
#include "pico_tree.h"
#include "pico_ipv6.h"
#include "pico_ipv4.h"
#include "pico_icmp6.h"
#include "pico_eth.h"
#include "pico_802154.h"
#include "pico_6lowpan.h"
#include "pico_6lowpan_ll.h"
#include "pico_addressing.h"
#define PICO_DEVICE_DEFAULT_MTU (1500)
struct pico_devices_rr_info {
struct pico_tree_node *node_in, *node_out;
};
static struct pico_devices_rr_info Devices_rr_info = {
NULL, NULL
};
static int pico_dev_cmp(void *ka, void *kb)
{
struct pico_device *a = ka, *b = kb;
if (a->hash < b->hash)
return -1;
if (a->hash > b->hash)
return 1;
return 0;
}
PICO_TREE_DECLARE(Device_tree, pico_dev_cmp);
#ifdef PICO_SUPPORT_6LOWPAN
static struct pico_ipv6_link * pico_6lowpan_link_add(struct pico_device *dev, const struct pico_ip6 *prefix)
{
struct pico_ip6 netmask64 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
struct pico_6lowpan_info *info = (struct pico_6lowpan_info *)dev->eth;
struct pico_ipv6_link *link = NULL; /* Make sure to return NULL */
struct pico_ip6 newaddr;
memcpy(newaddr.addr, prefix->addr, PICO_SIZE_IP6);
memcpy(newaddr.addr + 8, info->addr_ext.addr, SIZE_6LOWPAN_EXT);
newaddr.addr[8] = newaddr.addr[8] ^ 0x02; /* Toggle U/L bit */
/* RFC6775: No Duplicate Address Detection (DAD) is performed if
* EUI-64-based IPv6 addresses are used (as these addresses are assumed
* to be globally unique). */
if ((link = pico_ipv6_link_add_no_dad(dev, newaddr, netmask64))) {
if (pico_ipv6_is_linklocal(newaddr.addr))
pico_6lp_nd_start_soliciting(link, NULL);
else
pico_6lp_nd_register(link);
}
return link;
}
static int pico_6lowpan_store_info(struct pico_device *dev, const uint8_t *mac)
{
if ((dev->eth = PICO_ZALLOC(sizeof(struct pico_6lowpan_info)))) {
memcpy(dev->eth, mac, sizeof(struct pico_6lowpan_info));
return 0;
} else {
pico_err = PICO_ERR_ENOMEM;
return -1;
}
}
#endif
#ifdef PICO_SUPPORT_IPV6
static void device_init_ipv6_final(struct pico_device *dev, struct pico_ip6 *linklocal)
{
dev->hostvars.basetime = PICO_ND_REACHABLE_TIME;
/* RFC 4861 $6.3.2 value between 0.5 and 1.5 times basetime */
dev->hostvars.reachabletime = ((5 + (pico_rand() % 10)) * PICO_ND_REACHABLE_TIME) / 10;
dev->hostvars.retranstime = PICO_ND_RETRANS_TIMER;
pico_icmp6_router_solicitation(dev, linklocal, NULL);
dev->hostvars.hoplimit = PICO_IPV6_DEFAULT_HOP;
}
struct pico_ipv6_link *pico_ipv6_link_add_local(struct pico_device *dev, const struct pico_ip6 *prefix)
{
struct pico_ip6 netmask64 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
struct pico_ipv6_link *link = NULL; /* Make sure to return NULL */
struct pico_ip6 newaddr;
if (0) {}
#ifdef PICO_SUPPORT_6LOWPAN
else if (PICO_DEV_IS_6LOWPAN(dev)) {
link = pico_6lowpan_link_add(dev, prefix);
}
#endif
else {
memcpy(newaddr.addr, prefix->addr, PICO_SIZE_IP6);
/* modified EUI-64 + invert universal/local bit */
newaddr.addr[8] = (dev->eth->mac.addr[0] ^ 0x02);
newaddr.addr[9] = dev->eth->mac.addr[1];
newaddr.addr[10] = dev->eth->mac.addr[2];
newaddr.addr[11] = 0xff;
newaddr.addr[12] = 0xfe;
newaddr.addr[13] = dev->eth->mac.addr[3];
newaddr.addr[14] = dev->eth->mac.addr[4];
newaddr.addr[15] = dev->eth->mac.addr[5];
if ((link = pico_ipv6_link_add(dev, newaddr, netmask64))) {
device_init_ipv6_final(dev, &newaddr);
}
}
return link;
}
#endif
static int device_init_mac(struct pico_device *dev, const uint8_t *mac)
{
#ifdef PICO_SUPPORT_IPV6
struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}};
#endif
if (0) {}
#ifdef PICO_SUPPORT_6LOWPAN
else if (PICO_DEV_IS_6LOWPAN(dev)) {
if (pico_6lowpan_store_info(dev, mac))
return -1;
}
#endif
else {
if ((dev->eth = PICO_ZALLOC(sizeof(struct pico_ethdev)))) {
memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH);
} else {
pico_err = PICO_ERR_ENOMEM;
return -1;
}
}
#ifdef PICO_SUPPORT_IPV6
if (pico_ipv6_link_add_local(dev, &linklocal) == NULL) {
PICO_FREE(dev->q_in);
PICO_FREE(dev->q_out);
PICO_FREE(dev->eth);
return -1;
}
#endif
return 0;
}
int pico_device_ipv6_random_ll(struct pico_device *dev)
{
#ifdef PICO_SUPPORT_IPV6
struct pico_ip6 linklocal = {{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xaa, 0xaa, 0xaa, 0xff, 0xfe, 0xaa, 0xaa, 0xaa}};
struct pico_ip6 netmask6 = {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
uint32_t len = (uint32_t)strlen(dev->name);
if (strcmp(dev->name, "loop")) {
do {
/* privacy extension + unset universal/local and individual/group bit */
len = pico_rand();
linklocal.addr[8] = (uint8_t)((len & 0xffu) & (uint8_t)(~0x03));
linklocal.addr[9] = (uint8_t)(len >> 8);
linklocal.addr[10] = (uint8_t)(len >> 16);
linklocal.addr[11] = (uint8_t)(len >> 24);
len = pico_rand();
linklocal.addr[12] = (uint8_t)len;
linklocal.addr[13] = (uint8_t)(len >> 8);
linklocal.addr[14] = (uint8_t)(len >> 16);
linklocal.addr[15] = (uint8_t)(len >> 24);
pico_rand_feed(dev->hash);
} while (pico_ipv6_link_get(&linklocal));
if (pico_ipv6_link_add(dev, linklocal, netmask6) == NULL) {
return -1;
}
}
#endif
return 0;
}
static int device_init_nomac(struct pico_device *dev)
{
if (pico_device_ipv6_random_ll(dev) < 0) {
PICO_FREE(dev->q_in);
PICO_FREE(dev->q_out);
return -1;
}
dev->eth = NULL;
return 0;
}
#define DEBUG_IPV6(ip) { \
char ipstr[40] = { 0 }; \
pico_ipv6_to_string(ipstr, (ip).addr); \
dbg("IPv6 (%s)\n", ipstr); \
}
int pico_device_init(struct pico_device *dev, const char *name, const uint8_t *mac)
{
uint32_t len = (uint32_t)strlen(name);
int ret = 0;
if(len > MAX_DEVICE_NAME)
len = MAX_DEVICE_NAME;
memcpy(dev->name, name, len);
dev->hash = pico_hash(dev->name, len);
Devices_rr_info.node_in = NULL;
Devices_rr_info.node_out = NULL;
dev->q_in = PICO_ZALLOC(sizeof(struct pico_queue));
if (!dev->q_in)
return -1;
dev->q_out = PICO_ZALLOC(sizeof(struct pico_queue));
if (!dev->q_out) {
PICO_FREE(dev->q_in);
return -1;
}
if (pico_tree_insert(&Device_tree, dev)) {
PICO_FREE(dev->q_in);
PICO_FREE(dev->q_out);
return -1;
}
if (!dev->mtu)
dev->mtu = PICO_DEVICE_DEFAULT_MTU;
#ifdef PICO_SUPPORT_6LOWPAN
if (PICO_DEV_IS_6LOWPAN(dev) && LL_MODE_ETHERNET == dev->mode)
return -1;
#endif
if (mac) {
ret = device_init_mac(dev, mac);
} else {
if (!dev->mode) {
ret = device_init_nomac(dev);
}
#ifdef PICO_SUPPORT_6LOWPAN
else {
/* RFC6775: Link Local to be formed based on EUI-64 as per RFC6775 */
dbg("Link local address to be formed based on EUI-64\n");
return -1;
}
#endif
}
return ret;
}
static void pico_queue_destroy(struct pico_queue *q)
{
if (q) {
pico_queue_empty(q);
PICO_FREE(q);
}
}
void pico_device_destroy(struct pico_device *dev)
{
pico_queue_destroy(dev->q_in);
pico_queue_destroy(dev->q_out);
if (!dev->mode && dev->eth)
PICO_FREE(dev->eth);
#ifdef PICO_SUPPORT_IPV4
pico_ipv4_cleanup_links(dev);
#endif
#ifdef PICO_SUPPORT_IPV6
pico_ipv6_cleanup_links(dev);
#endif
pico_tree_delete(&Device_tree, dev);
if (dev->destroy)
dev->destroy(dev);
Devices_rr_info.node_in = NULL;
Devices_rr_info.node_out = NULL;
PICO_FREE(dev);
}
static int check_dev_serve_interrupt(struct pico_device *dev, int loop_score)
{
if ((dev->__serving_interrupt) && (dev->dsr)) {
/* call dsr routine */
loop_score = dev->dsr(dev, loop_score);
}
return loop_score;
}
static int check_dev_serve_polling(struct pico_device *dev, int loop_score)
{
if (dev->poll) {
loop_score = dev->poll(dev, loop_score);
}
return loop_score;
}
static int devloop_in(struct pico_device *dev, int loop_score)
{
struct pico_frame *f;
while(loop_score > 0) {
if (dev->q_in->frames == 0)
break;
/* Receive */
f = pico_dequeue(dev->q_in);
if (f) {
pico_datalink_receive(f);
loop_score--;
}
}
return loop_score;
}
static int devloop_sendto_dev(struct pico_device *dev, struct pico_frame *f)
{
#ifdef PICO_SUPPORT_6LOWPAN
if (PICO_DEV_IS_6LOWPAN(dev)) {
return (pico_6lowpan_ll_sendto_dev(dev, f) <= 0);
}
#endif
return (dev->send(dev, f->start, (int)f->len) <= 0);
}
static int devloop_out(struct pico_device *dev, int loop_score)
{
struct pico_frame *f;
while(loop_score > 0) {
if (dev->q_out->frames == 0)
break;
/* Device dequeue + send */
f = pico_queue_peek(dev->q_out);
if (!f)
break;
if (devloop_sendto_dev(dev, f) == 0) { /* success. */
f = pico_dequeue(dev->q_out);
pico_frame_discard(f); /* SINGLE POINT OF DISCARD for OUTGOING FRAMES */
loop_score--;
} else
break; /* Don't discard */
}
return loop_score;
}
static int devloop(struct pico_device *dev, int loop_score, int direction)
{
/* If device supports interrupts, read the value of the condition and trigger the dsr */
loop_score = check_dev_serve_interrupt(dev, loop_score);
/* If device supports polling, give control. Loop score is managed internally,
* remaining loop points are returned. */
loop_score = check_dev_serve_polling(dev, loop_score);
if (direction == PICO_LOOP_DIR_OUT)
loop_score = devloop_out(dev, loop_score);
else
loop_score = devloop_in(dev, loop_score);
return loop_score;
}
static struct pico_tree_node *pico_dev_roundrobin_start(int direction)
{
if (Devices_rr_info.node_in == NULL)
Devices_rr_info.node_in = pico_tree_firstNode(Device_tree.root);
if (Devices_rr_info.node_out == NULL)
Devices_rr_info.node_out = pico_tree_firstNode(Device_tree.root);
if (direction == PICO_LOOP_DIR_IN)
return Devices_rr_info.node_in;
else
return Devices_rr_info.node_out;
}
static void pico_dev_roundrobin_end(int direction, struct pico_tree_node *last)
{
if (direction == PICO_LOOP_DIR_IN)
Devices_rr_info.node_in = last;
else
Devices_rr_info.node_out = last;
}
#define DEV_LOOP_MIN 16
int pico_devices_loop(int loop_score, int direction)
{
struct pico_device *start, *next;
struct pico_tree_node *next_node = pico_dev_roundrobin_start(direction);
if (!next_node)
return loop_score;
next = next_node->keyValue;
start = next;
/* round-robin all devices, break if traversed all devices */
while ((loop_score > DEV_LOOP_MIN) && (next != NULL)) {
loop_score = devloop(next, loop_score, direction);
next_node = pico_tree_next(next_node);
next = next_node->keyValue;
if (next == NULL)
{
next_node = pico_tree_firstNode(Device_tree.root);
next = next_node->keyValue;
}
if (next == start)
break;
}
pico_dev_roundrobin_end(direction, next_node);
return loop_score;
}
struct pico_device *pico_get_device(const char*name)
{
struct pico_device *dev;
struct pico_tree_node *index;
pico_tree_foreach(index, &Device_tree){
dev = index->keyValue;
if(strcmp(name, dev->name) == 0)
return dev;
}
return NULL;
}
int32_t pico_device_broadcast(struct pico_frame *f)
{
struct pico_tree_node *index;
int32_t ret = -1;
pico_tree_foreach(index, &Device_tree)
{
struct pico_device *dev = index->keyValue;
if(dev != f->dev)
{
struct pico_frame *copy = pico_frame_copy(f);
if(!copy)
break;
copy->dev = dev;
copy->dev->send(copy->dev, copy->start, (int)copy->len);
pico_frame_discard(copy);
}
else
{
ret = f->dev->send(f->dev, f->start, (int)f->len);
}
}
return ret;
}
int pico_device_link_state(struct pico_device *dev)
{
if (!dev->link_state)
return 1; /* Not supported, assuming link is always up */
return dev->link_state(dev);
}
Reference in New Issue View Git Blame Copy Permalink