Daniele Lacamera / PicoTCP-Experimental_CDC_ECM_Branch

Free (GPLv2) TCP/IP stack developed by TASS Belgium

Fork of PicoTCP by Daniele Lacamera

stack/pico_device.c@51:18637a3d071f, 2013-08-03 (annotated)

Committer:
daniele
Date:
Sat Aug 03 08:50:27 2013 +0000
Revision:
51:18637a3d071f
Parent:
10:dd7111d4279f 
Branch for CDC-ECM: Work in progress

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniele 10:dd7111d4279f 1 /*********************************************************************
daniele 10:dd7111d4279f 2 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
daniele 10:dd7111d4279f 3 See LICENSE and COPYING for usage.
daniele 10:dd7111d4279f 4
daniele 10:dd7111d4279f 5 .
daniele 10:dd7111d4279f 6
daniele 10:dd7111d4279f 7 Authors: Daniele Lacamera
daniele 10:dd7111d4279f 8 *********************************************************************/
daniele 10:dd7111d4279f 9
daniele 10:dd7111d4279f 10
daniele 10:dd7111d4279f 11 #include "pico_config.h"
daniele 10:dd7111d4279f 12 #include "pico_device.h"
daniele 10:dd7111d4279f 13 #include "pico_stack.h"
daniele 10:dd7111d4279f 14 #include "pico_protocol.h"
daniele 10:dd7111d4279f 15 #include "pico_tree.h"
daniele 10:dd7111d4279f 16
daniele 10:dd7111d4279f 17
daniele 10:dd7111d4279f 18 static int pico_dev_cmp(void *ka, void *kb)
daniele 10:dd7111d4279f 19 {
daniele 10:dd7111d4279f 20 struct pico_device *a = ka, *b = kb;
daniele 10:dd7111d4279f 21 if (a->hash < b->hash)
daniele 10:dd7111d4279f 22 return -1;
daniele 10:dd7111d4279f 23 if (a->hash > b->hash)
daniele 10:dd7111d4279f 24 return 1;
daniele 10:dd7111d4279f 25 return 0;
daniele 10:dd7111d4279f 26 }
daniele 10:dd7111d4279f 27
daniele 10:dd7111d4279f 28 PICO_TREE_DECLARE(Device_tree,pico_dev_cmp);
daniele 10:dd7111d4279f 29
daniele 10:dd7111d4279f 30 int pico_device_init(struct pico_device *dev, char *name, uint8_t *mac)
daniele 10:dd7111d4279f 31 {
daniele 10:dd7111d4279f 32 int len = strlen(name);
daniele 10:dd7111d4279f 33 if(len>MAX_DEVICE_NAME)
daniele 10:dd7111d4279f 34 len = MAX_DEVICE_NAME;
daniele 10:dd7111d4279f 35 memcpy(dev->name, name, len);
daniele 10:dd7111d4279f 36 dev->hash = pico_hash(dev->name);
daniele 10:dd7111d4279f 37
daniele 10:dd7111d4279f 38 pico_tree_insert(&Device_tree,dev);
daniele 10:dd7111d4279f 39 dev->q_in = pico_zalloc(sizeof(struct pico_queue));
daniele 10:dd7111d4279f 40 dev->q_out = pico_zalloc(sizeof(struct pico_queue));
daniele 10:dd7111d4279f 41
daniele 10:dd7111d4279f 42 if (mac) {
daniele 10:dd7111d4279f 43 dev->eth = pico_zalloc(sizeof(struct pico_ethdev));
daniele 10:dd7111d4279f 44 memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH);
daniele 10:dd7111d4279f 45 } else {
daniele 10:dd7111d4279f 46 dev->eth = NULL;
daniele 10:dd7111d4279f 47 }
daniele 10:dd7111d4279f 48
daniele 10:dd7111d4279f 49 if (!dev->q_in || !dev->q_out || (mac && !dev->eth))
daniele 10:dd7111d4279f 50 return -1;
daniele 10:dd7111d4279f 51 return 0;
daniele 10:dd7111d4279f 52 }
daniele 10:dd7111d4279f 53
daniele 10:dd7111d4279f 54 void pico_device_destroy(struct pico_device *dev)
daniele 10:dd7111d4279f 55 {
daniele 10:dd7111d4279f 56 if (dev->destroy)
daniele 10:dd7111d4279f 57 dev->destroy(dev);
daniele 10:dd7111d4279f 58
daniele 10:dd7111d4279f 59 if (dev->q_in) {
daniele 10:dd7111d4279f 60 pico_queue_empty(dev->q_in);
daniele 10:dd7111d4279f 61 pico_free(dev->q_in);
daniele 10:dd7111d4279f 62 }
daniele 10:dd7111d4279f 63 if (dev->q_out) {
daniele 10:dd7111d4279f 64 pico_queue_empty(dev->q_out);
daniele 10:dd7111d4279f 65 pico_free(dev->q_out);
daniele 10:dd7111d4279f 66 }
daniele 10:dd7111d4279f 67
daniele 10:dd7111d4279f 68 if (dev->eth)
daniele 10:dd7111d4279f 69 pico_free(dev->eth);
daniele 10:dd7111d4279f 70
daniele 10:dd7111d4279f 71 pico_tree_delete(&Device_tree,dev);
daniele 10:dd7111d4279f 72 pico_free(dev);
daniele 10:dd7111d4279f 73 }
daniele 10:dd7111d4279f 74
daniele 10:dd7111d4279f 75 static int devloop(struct pico_device *dev, int loop_score, int direction)
daniele 10:dd7111d4279f 76 {
daniele 10:dd7111d4279f 77 struct pico_frame *f;
daniele 10:dd7111d4279f 78
daniele 10:dd7111d4279f 79 /* If device supports interrupts, read the value of the condition and trigger the dsr */
daniele 10:dd7111d4279f 80 if ((dev->__serving_interrupt) && (dev->dsr)) {
daniele 10:dd7111d4279f 81 /* call dsr routine */
daniele 10:dd7111d4279f 82 loop_score = dev->dsr(dev, loop_score);
daniele 10:dd7111d4279f 83 }
daniele 10:dd7111d4279f 84
daniele 10:dd7111d4279f 85 /* If device supports polling, give control. Loop score is managed internally,
daniele 10:dd7111d4279f 86 * remaining loop points are returned. */
daniele 10:dd7111d4279f 87 if (dev->poll) {
daniele 10:dd7111d4279f 88 loop_score = dev->poll(dev, loop_score);
daniele 10:dd7111d4279f 89 }
daniele 10:dd7111d4279f 90
daniele 10:dd7111d4279f 91 if (direction == PICO_LOOP_DIR_OUT) {
daniele 10:dd7111d4279f 92
daniele 10:dd7111d4279f 93 while(loop_score > 0) {
daniele 10:dd7111d4279f 94 if (dev->q_out->frames <= 0)
daniele 10:dd7111d4279f 95 break;
daniele 10:dd7111d4279f 96
daniele 10:dd7111d4279f 97 /* Device dequeue + send */
daniele 10:dd7111d4279f 98 f = pico_dequeue(dev->q_out);
daniele 10:dd7111d4279f 99 if (f) {
daniele 10:dd7111d4279f 100 if (dev->eth) {
daniele 10:dd7111d4279f 101 int ret = pico_ethernet_send(f);
daniele 10:dd7111d4279f 102 if (0 == ret) {
daniele 10:dd7111d4279f 103 loop_score--;
daniele 10:dd7111d4279f 104 continue;
daniele 10:dd7111d4279f 105 } if (ret < 0) {
daniele 10:dd7111d4279f 106 if (!pico_source_is_local(f)) {
daniele 10:dd7111d4279f 107 dbg("Destination unreachable -------> SEND ICMP\n");
daniele 10:dd7111d4279f 108 pico_notify_dest_unreachable(f);
daniele 10:dd7111d4279f 109 } else {
daniele 10:dd7111d4279f 110 dbg("Destination unreachable -------> LOCAL\n");
daniele 10:dd7111d4279f 111 }
daniele 10:dd7111d4279f 112 pico_frame_discard(f);
daniele 10:dd7111d4279f 113 continue;
daniele 10:dd7111d4279f 114 }
daniele 10:dd7111d4279f 115 } else {
daniele 10:dd7111d4279f 116 dev->send(dev, f->start, f->len);
daniele 10:dd7111d4279f 117 }
daniele 10:dd7111d4279f 118 pico_frame_discard(f);
daniele 10:dd7111d4279f 119 loop_score--;
daniele 10:dd7111d4279f 120 }
daniele 10:dd7111d4279f 121 }
daniele 10:dd7111d4279f 122
daniele 10:dd7111d4279f 123 } else if (direction == PICO_LOOP_DIR_IN) {
daniele 10:dd7111d4279f 124
daniele 10:dd7111d4279f 125 while(loop_score > 0) {
daniele 10:dd7111d4279f 126 if (dev->q_in->frames <= 0)
daniele 10:dd7111d4279f 127 break;
daniele 10:dd7111d4279f 128
daniele 10:dd7111d4279f 129 /* Receive */
daniele 10:dd7111d4279f 130 f = pico_dequeue(dev->q_in);
daniele 10:dd7111d4279f 131 if (f) {
daniele 10:dd7111d4279f 132 if (dev->eth) {
daniele 10:dd7111d4279f 133 f->datalink_hdr = f->buffer;
daniele 10:dd7111d4279f 134 pico_ethernet_receive(f);
daniele 10:dd7111d4279f 135 } else {
daniele 10:dd7111d4279f 136 f->net_hdr = f->buffer;
daniele 10:dd7111d4279f 137 pico_network_receive(f);
daniele 10:dd7111d4279f 138 }
daniele 10:dd7111d4279f 139 loop_score--;
daniele 10:dd7111d4279f 140 }
daniele 10:dd7111d4279f 141 }
daniele 10:dd7111d4279f 142 }
daniele 10:dd7111d4279f 143
daniele 10:dd7111d4279f 144 return loop_score;
daniele 10:dd7111d4279f 145 }
daniele 10:dd7111d4279f 146
daniele 10:dd7111d4279f 147
daniele 10:dd7111d4279f 148 #define DEV_LOOP_MIN 16
daniele 10:dd7111d4279f 149
daniele 10:dd7111d4279f 150 int pico_devices_loop(int loop_score, int direction)
daniele 10:dd7111d4279f 151 {
daniele 10:dd7111d4279f 152 struct pico_device *start;
daniele 10:dd7111d4279f 153 static struct pico_device *next = NULL, *next_in = NULL, *next_out = NULL;
daniele 10:dd7111d4279f 154 static struct pico_tree_node * next_node, * in_node, * out_node;
daniele 10:dd7111d4279f 155
daniele 10:dd7111d4279f 156 if (next_in == NULL) {
daniele 10:dd7111d4279f 157 in_node = pico_tree_firstNode(Device_tree.root);
daniele 10:dd7111d4279f 158 next_in = in_node->keyValue;
daniele 10:dd7111d4279f 159 }
daniele 10:dd7111d4279f 160 if (next_out == NULL) {
daniele 10:dd7111d4279f 161 out_node = pico_tree_firstNode(Device_tree.root);
daniele 10:dd7111d4279f 162 next_out = out_node->keyValue;
daniele 10:dd7111d4279f 163 }
daniele 10:dd7111d4279f 164
daniele 10:dd7111d4279f 165 if (direction == PICO_LOOP_DIR_IN)
daniele 10:dd7111d4279f 166 {
daniele 10:dd7111d4279f 167 next_node = in_node;
daniele 10:dd7111d4279f 168 next = next_in;
daniele 10:dd7111d4279f 169 }
daniele 10:dd7111d4279f 170 else if (direction == PICO_LOOP_DIR_OUT)
daniele 10:dd7111d4279f 171 {
daniele 10:dd7111d4279f 172 next_node = out_node;
daniele 10:dd7111d4279f 173 next = next_out;
daniele 10:dd7111d4279f 174 }
daniele 10:dd7111d4279f 175
daniele 10:dd7111d4279f 176 /* init start node */
daniele 10:dd7111d4279f 177 start = next;
daniele 10:dd7111d4279f 178
daniele 10:dd7111d4279f 179 /* round-robin all devices, break if traversed all devices */
daniele 10:dd7111d4279f 180 while (loop_score > DEV_LOOP_MIN && next != NULL) {
daniele 10:dd7111d4279f 181 loop_score = devloop(next, loop_score, direction);
daniele 10:dd7111d4279f 182
daniele 10:dd7111d4279f 183 next_node = pico_tree_next(next_node);
daniele 10:dd7111d4279f 184 next = next_node->keyValue;
daniele 10:dd7111d4279f 185
daniele 10:dd7111d4279f 186 if (next == NULL)
daniele 10:dd7111d4279f 187 {
daniele 10:dd7111d4279f 188 next_node = pico_tree_firstNode(Device_tree.root);
daniele 10:dd7111d4279f 189 next = next_node->keyValue;
daniele 10:dd7111d4279f 190 }
daniele 10:dd7111d4279f 191 if (next == start)
daniele 10:dd7111d4279f 192 break;
daniele 10:dd7111d4279f 193 }
daniele 10:dd7111d4279f 194
daniele 10:dd7111d4279f 195 if (direction == PICO_LOOP_DIR_IN)
daniele 10:dd7111d4279f 196 {
daniele 10:dd7111d4279f 197 in_node = next_node;
daniele 10:dd7111d4279f 198 next_in = next;
daniele 10:dd7111d4279f 199 }
daniele 10:dd7111d4279f 200 else if (direction == PICO_LOOP_DIR_OUT)
daniele 10:dd7111d4279f 201 {
daniele 10:dd7111d4279f 202 out_node = next_node;
daniele 10:dd7111d4279f 203 next_out = next;
daniele 10:dd7111d4279f 204 }
daniele 10:dd7111d4279f 205
daniele 10:dd7111d4279f 206 return loop_score;
daniele 10:dd7111d4279f 207 }
daniele 10:dd7111d4279f 208
daniele 10:dd7111d4279f 209 struct pico_device* pico_get_device(char* name)
daniele 10:dd7111d4279f 210 {
daniele 10:dd7111d4279f 211 struct pico_device *dev;
daniele 10:dd7111d4279f 212 struct pico_tree_node * index;
daniele 10:dd7111d4279f 213 pico_tree_foreach(index, &Device_tree){
daniele 10:dd7111d4279f 214 dev = index->keyValue;
daniele 10:dd7111d4279f 215 if(strcmp(name, dev->name) == 0)
daniele 10:dd7111d4279f 216 return dev;
daniele 10:dd7111d4279f 217 }
daniele 10:dd7111d4279f 218 return NULL;
daniele 10:dd7111d4279f 219 }
daniele 10:dd7111d4279f 220
daniele 10:dd7111d4279f 221 int pico_device_broadcast(struct pico_frame * f)
daniele 10:dd7111d4279f 222 {
daniele 10:dd7111d4279f 223 struct pico_tree_node * index;
daniele 10:dd7111d4279f 224 int ret = -1;
daniele 10:dd7111d4279f 225
daniele 10:dd7111d4279f 226 pico_tree_foreach(index,&Device_tree)
daniele 10:dd7111d4279f 227 {
daniele 10:dd7111d4279f 228 struct pico_device * dev = index->keyValue;
daniele 10:dd7111d4279f 229 if(dev != f->dev)
daniele 10:dd7111d4279f 230 {
daniele 10:dd7111d4279f 231 struct pico_frame * copy = pico_frame_copy(f);
daniele 10:dd7111d4279f 232
daniele 10:dd7111d4279f 233 if(!copy)
daniele 10:dd7111d4279f 234 return -1;
daniele 10:dd7111d4279f 235 copy->dev = dev;
daniele 10:dd7111d4279f 236 copy->dev->send(copy->dev, copy->start, copy->len);
daniele 10:dd7111d4279f 237 pico_frame_discard(copy);
daniele 10:dd7111d4279f 238 }
daniele 10:dd7111d4279f 239 else
daniele 10:dd7111d4279f 240 {
daniele 10:dd7111d4279f 241 ret = f->dev->send(f->dev, f->start, f->len);
daniele 10:dd7111d4279f 242 }
daniele 10:dd7111d4279f 243 }
daniele 10:dd7111d4279f 244
daniele 10:dd7111d4279f 245 return ret;
daniele 10:dd7111d4279f 246 }