Free (GPLv2) TCP/IP stack developed by TASS Belgium
Fork of PicoTCP by
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?
User | Revision | Line number | New 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 | } |