Daniele Lacamera / PicoTCP Featured

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

Dependents:   lpc1768-picotcp-demo ZeroMQ_PicoTCP_Publisher_demo TCPSocket_HelloWorld_PicoTCP Pico_TCP_UDP_Test ... more

PicoTCP. Copyright (c) 2013 TASS Belgium NV.

Released under the GNU General Public License, version 2.

Different licensing models may exist, at the sole discretion of the Copyright holders.

Official homepage: http://www.picotcp.com

Bug tracker: https://github.com/tass-belgium/picotcp/issues

Development steps:

  • initial integration with mbed RTOS
  • generic mbed Ethernet driver
  • high performance NXP LPC1768 specific Ethernet driver
  • Multi-threading support for mbed RTOS
  • Berkeley sockets and integration with the New Socket API
  • Fork of the apps running on top of the New Socket API
  • Scheduling optimizations
  • Debugging/benchmarking/testing

Demo application (measuring TCP sender performance):

Import programlpc1768-picotcp-demo

A PicoTCP demo app testing the ethernet throughput on the lpc1768 mbed board.

Last commit 17 May 2013 by TASS Belgium

modules/pico_ipv4.c@49:4b404dd2c97a, 2013-08-05 (annotated)

Committer:
tass
Date:
Mon Aug 05 12:58:56 2013 +0000
Revision:
49:4b404dd2c97a
Parent:
10:dd7111d4279f
Child:
51:ab4529a384a6
updated from master branch.

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 Authors: Daniele Lacamera, Markian Yskout
daniele 10:dd7111d4279f 6 *********************************************************************/
daniele 10:dd7111d4279f 7
daniele 10:dd7111d4279f 8
daniele 10:dd7111d4279f 9 #include "pico_config.h"
daniele 10:dd7111d4279f 10 #include "pico_ipfilter.h"
daniele 10:dd7111d4279f 11 #include "pico_ipv4.h"
daniele 10:dd7111d4279f 12 #include "pico_icmp4.h"
daniele 10:dd7111d4279f 13 #include "pico_stack.h"
daniele 10:dd7111d4279f 14 #include "pico_eth.h"
daniele 10:dd7111d4279f 15 #include "pico_udp.h"
daniele 10:dd7111d4279f 16 #include "pico_tcp.h"
daniele 10:dd7111d4279f 17 #include "pico_socket.h"
daniele 10:dd7111d4279f 18 #include "pico_device.h"
daniele 10:dd7111d4279f 19 #include "pico_nat.h"
daniele 10:dd7111d4279f 20 #include "pico_igmp.h"
daniele 10:dd7111d4279f 21 #include "pico_tree.h"
daniele 10:dd7111d4279f 22
daniele 10:dd7111d4279f 23 #ifdef PICO_SUPPORT_IPV4
daniele 10:dd7111d4279f 24
daniele 10:dd7111d4279f 25 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 26 # define ip_mcast_dbg(...) do{}while(0) /* so_mcast_dbg in pico_socket.c */
daniele 10:dd7111d4279f 27 # define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */
daniele 10:dd7111d4279f 28 /* Default network interface for multicast transmission */
daniele 10:dd7111d4279f 29 static struct pico_ipv4_link *mcast_default_link = NULL;
daniele 10:dd7111d4279f 30 #endif
daniele 10:dd7111d4279f 31 #ifdef PICO_SUPPORT_IPFRAG
daniele 10:dd7111d4279f 32 # define reassembly_dbg(...) do{}while(0)
daniele 10:dd7111d4279f 33 #endif
daniele 10:dd7111d4279f 34
daniele 10:dd7111d4279f 35 /* Queues */
daniele 10:dd7111d4279f 36 static struct pico_queue in = {};
daniele 10:dd7111d4279f 37 static struct pico_queue out = {};
daniele 10:dd7111d4279f 38
daniele 10:dd7111d4279f 39 /* Functions */
daniele 10:dd7111d4279f 40 static int ipv4_route_compare(void *ka, void * kb);
daniele 10:dd7111d4279f 41
daniele 10:dd7111d4279f 42 int pico_ipv4_to_string(char *ipbuf, const uint32_t ip)
daniele 10:dd7111d4279f 43 {
daniele 10:dd7111d4279f 44 const unsigned char *addr = (unsigned char *) &ip;
daniele 10:dd7111d4279f 45 int i;
daniele 10:dd7111d4279f 46
daniele 10:dd7111d4279f 47 if (!ipbuf) {
daniele 10:dd7111d4279f 48 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 49 return -1;
daniele 10:dd7111d4279f 50 }
daniele 10:dd7111d4279f 51
daniele 10:dd7111d4279f 52 for(i = 0; i < 4; i++)
daniele 10:dd7111d4279f 53 {
daniele 10:dd7111d4279f 54 if(addr[i] > 99){
daniele 10:dd7111d4279f 55 *ipbuf++ = '0' + (addr[i] / 100);
daniele 10:dd7111d4279f 56 *ipbuf++ = '0' + ((addr[i] % 100) / 10);
daniele 10:dd7111d4279f 57 *ipbuf++ = '0' + ((addr[i] % 100) % 10);
daniele 10:dd7111d4279f 58 }else if(addr[i] > 9){
daniele 10:dd7111d4279f 59 *ipbuf++ = '0' + (addr[i] / 10);
daniele 10:dd7111d4279f 60 *ipbuf++ = '0' + (addr[i] % 10);
daniele 10:dd7111d4279f 61 }else{
daniele 10:dd7111d4279f 62 *ipbuf++ = '0' + addr[i];
daniele 10:dd7111d4279f 63 }
daniele 10:dd7111d4279f 64 if(i < 3)
daniele 10:dd7111d4279f 65 *ipbuf++ = '.';
daniele 10:dd7111d4279f 66 }
daniele 10:dd7111d4279f 67 *ipbuf = '\0';
daniele 10:dd7111d4279f 68
daniele 10:dd7111d4279f 69 return 0;
daniele 10:dd7111d4279f 70 }
daniele 10:dd7111d4279f 71
daniele 10:dd7111d4279f 72 int pico_string_to_ipv4(const char *ipstr, uint32_t *ip)
daniele 10:dd7111d4279f 73 {
daniele 10:dd7111d4279f 74 unsigned char buf[4] = {0};
daniele 10:dd7111d4279f 75 int cnt = 0;
daniele 10:dd7111d4279f 76 int p;
daniele 10:dd7111d4279f 77
daniele 10:dd7111d4279f 78 if(!ipstr || !ip) {
daniele 10:dd7111d4279f 79 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 80 return -1;
daniele 10:dd7111d4279f 81 }
daniele 10:dd7111d4279f 82
daniele 10:dd7111d4279f 83 while((p = *ipstr++) != 0)
daniele 10:dd7111d4279f 84 {
daniele 10:dd7111d4279f 85 if(pico_is_digit(p)){
daniele 10:dd7111d4279f 86 buf[cnt] = (10 * buf[cnt]) + (p - '0');
daniele 10:dd7111d4279f 87 }else if(p == '.'){
daniele 10:dd7111d4279f 88 cnt++;
daniele 10:dd7111d4279f 89 }else{
daniele 10:dd7111d4279f 90 return -1;
daniele 10:dd7111d4279f 91 }
daniele 10:dd7111d4279f 92 }
daniele 10:dd7111d4279f 93
daniele 10:dd7111d4279f 94 /* Handle short notation */
daniele 10:dd7111d4279f 95 if(cnt == 1){
daniele 10:dd7111d4279f 96 buf[3] = buf[1];
daniele 10:dd7111d4279f 97 buf[1] = 0;
daniele 10:dd7111d4279f 98 buf[2] = 0;
daniele 10:dd7111d4279f 99 }else if (cnt == 2){
daniele 10:dd7111d4279f 100 buf[3] = buf[2];
daniele 10:dd7111d4279f 101 buf[2] = 0;
daniele 10:dd7111d4279f 102 }else if(cnt != 3){
daniele 10:dd7111d4279f 103 /* String could not be parsed, return error */
daniele 10:dd7111d4279f 104 return -1;
daniele 10:dd7111d4279f 105 }
daniele 10:dd7111d4279f 106
daniele 10:dd7111d4279f 107 *ip = long_from(buf);
daniele 10:dd7111d4279f 108
daniele 10:dd7111d4279f 109 return 0;
daniele 10:dd7111d4279f 110
daniele 10:dd7111d4279f 111 }
daniele 10:dd7111d4279f 112
daniele 10:dd7111d4279f 113 int pico_ipv4_valid_netmask(uint32_t mask)
daniele 10:dd7111d4279f 114 {
daniele 10:dd7111d4279f 115 int cnt = 0;
daniele 10:dd7111d4279f 116 int end = 0;
daniele 10:dd7111d4279f 117 int i;
daniele 10:dd7111d4279f 118 uint32_t mask_swap = long_be(mask);
daniele 10:dd7111d4279f 119
daniele 10:dd7111d4279f 120 /*
daniele 10:dd7111d4279f 121 * Swap bytes for convenient parsing
daniele 10:dd7111d4279f 122 * e.g. 0x..f8ff will become 0xfff8..
daniele 10:dd7111d4279f 123 * Then, we count the consecutive bits
daniele 10:dd7111d4279f 124 *
daniele 10:dd7111d4279f 125 * */
daniele 10:dd7111d4279f 126
daniele 10:dd7111d4279f 127 for(i = 0; i < 32; i++){
daniele 10:dd7111d4279f 128 if((mask_swap << i) & (1 << 31)){
daniele 10:dd7111d4279f 129 if(end) {
daniele 10:dd7111d4279f 130 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 131 return -1;
daniele 10:dd7111d4279f 132 }
daniele 10:dd7111d4279f 133 cnt++;
daniele 10:dd7111d4279f 134 }else{
daniele 10:dd7111d4279f 135 end = 1;
daniele 10:dd7111d4279f 136 }
daniele 10:dd7111d4279f 137 }
daniele 10:dd7111d4279f 138 return cnt;
daniele 10:dd7111d4279f 139 }
daniele 10:dd7111d4279f 140
daniele 10:dd7111d4279f 141 int pico_ipv4_is_unicast(uint32_t address)
daniele 10:dd7111d4279f 142 {
daniele 10:dd7111d4279f 143 const unsigned char *addr = (unsigned char *) &address;
daniele 10:dd7111d4279f 144 if((addr[0] & 0xe0) == 0xe0)
daniele 10:dd7111d4279f 145 return 0; /* multicast */
daniele 10:dd7111d4279f 146
daniele 10:dd7111d4279f 147 return 1;
daniele 10:dd7111d4279f 148 }
daniele 10:dd7111d4279f 149
daniele 10:dd7111d4279f 150 int pico_ipv4_is_multicast(uint32_t address)
daniele 10:dd7111d4279f 151 {
daniele 10:dd7111d4279f 152 const unsigned char *addr = (unsigned char *) &address;
daniele 10:dd7111d4279f 153 if((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0))
daniele 10:dd7111d4279f 154 return 1; /* multicast */
daniele 10:dd7111d4279f 155
daniele 10:dd7111d4279f 156 return 0;
daniele 10:dd7111d4279f 157 }
daniele 10:dd7111d4279f 158
daniele 10:dd7111d4279f 159 static int pico_ipv4_checksum(struct pico_frame *f)
daniele 10:dd7111d4279f 160 {
daniele 10:dd7111d4279f 161 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 162 if (!hdr)
daniele 10:dd7111d4279f 163 return -1;
daniele 10:dd7111d4279f 164 hdr->crc = 0;
daniele 10:dd7111d4279f 165 hdr->crc = short_be(pico_checksum(hdr, f->net_len));
daniele 10:dd7111d4279f 166 return 0;
daniele 10:dd7111d4279f 167 }
daniele 10:dd7111d4279f 168
daniele 10:dd7111d4279f 169 #ifdef PICO_SUPPORT_IPFRAG
daniele 10:dd7111d4279f 170 struct pico_ipv4_fragmented_packet {
daniele 10:dd7111d4279f 171 uint16_t id;
daniele 10:dd7111d4279f 172 uint8_t proto;
daniele 10:dd7111d4279f 173 struct pico_ip4 src;
daniele 10:dd7111d4279f 174 struct pico_ip4 dst;
daniele 10:dd7111d4279f 175 uint16_t total_len;
daniele 10:dd7111d4279f 176 struct pico_tree *t;
daniele 10:dd7111d4279f 177 };
daniele 10:dd7111d4279f 178
daniele 10:dd7111d4279f 179 static int pico_ipv4_fragmented_packet_cmp(void *ka, void *kb)
daniele 10:dd7111d4279f 180 {
daniele 10:dd7111d4279f 181 struct pico_ipv4_fragmented_packet *a = ka, *b = kb;
daniele 10:dd7111d4279f 182
daniele 10:dd7111d4279f 183 if (a->id < b->id)
daniele 10:dd7111d4279f 184 return -1;
daniele 10:dd7111d4279f 185 else if (a->id > b->id)
daniele 10:dd7111d4279f 186 return 1;
daniele 10:dd7111d4279f 187 else {
daniele 10:dd7111d4279f 188 if (a->proto < b->proto)
daniele 10:dd7111d4279f 189 return -1;
daniele 10:dd7111d4279f 190 else if (a->proto > b->proto)
daniele 10:dd7111d4279f 191 return 1;
daniele 10:dd7111d4279f 192 else {
daniele 10:dd7111d4279f 193 if (a->src.addr < b->src.addr)
daniele 10:dd7111d4279f 194 return -1;
daniele 10:dd7111d4279f 195 else if (a->src.addr > b->src.addr)
daniele 10:dd7111d4279f 196 return 1;
daniele 10:dd7111d4279f 197 else {
daniele 10:dd7111d4279f 198 if (a->dst.addr < b->dst.addr)
daniele 10:dd7111d4279f 199 return -1;
daniele 10:dd7111d4279f 200 else if (a->dst.addr > b->dst.addr)
daniele 10:dd7111d4279f 201 return 1;
daniele 10:dd7111d4279f 202 else
daniele 10:dd7111d4279f 203 return 0;
daniele 10:dd7111d4279f 204 }
daniele 10:dd7111d4279f 205 }
daniele 10:dd7111d4279f 206 }
daniele 10:dd7111d4279f 207 }
daniele 10:dd7111d4279f 208
daniele 10:dd7111d4279f 209 static int pico_ipv4_fragmented_element_cmp(void *ka, void *kb)
daniele 10:dd7111d4279f 210 {
daniele 10:dd7111d4279f 211 struct pico_frame *frame_a = ka, *frame_b = kb;
daniele 10:dd7111d4279f 212 struct pico_ipv4_hdr *a, *b;
daniele 10:dd7111d4279f 213 a = (struct pico_ipv4_hdr *) frame_a->net_hdr;
daniele 10:dd7111d4279f 214 b = (struct pico_ipv4_hdr *) frame_b->net_hdr;
daniele 10:dd7111d4279f 215
daniele 10:dd7111d4279f 216 if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) < short_be((b->frag & PICO_IPV4_FRAG_MASK)))
daniele 10:dd7111d4279f 217 return -1;
daniele 10:dd7111d4279f 218 else if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) > short_be((b->frag & PICO_IPV4_FRAG_MASK)))
daniele 10:dd7111d4279f 219 return 1;
daniele 10:dd7111d4279f 220 else
daniele 10:dd7111d4279f 221 return 0;
daniele 10:dd7111d4279f 222 }
daniele 10:dd7111d4279f 223
daniele 10:dd7111d4279f 224 PICO_TREE_DECLARE(pico_ipv4_fragmented_tree, pico_ipv4_fragmented_packet_cmp);
daniele 10:dd7111d4279f 225
daniele 10:dd7111d4279f 226 static inline void pico_ipv4_fragmented_cleanup(struct pico_ipv4_fragmented_packet *pfrag)
daniele 10:dd7111d4279f 227 {
daniele 10:dd7111d4279f 228 struct pico_tree_node *index = NULL, *_tmp = NULL;
daniele 10:dd7111d4279f 229 struct pico_frame *f_frag = NULL;
daniele 10:dd7111d4279f 230
daniele 10:dd7111d4279f 231 pico_tree_foreach_safe(index, pfrag->t, _tmp) {
daniele 10:dd7111d4279f 232 f_frag = index->keyValue;
daniele 10:dd7111d4279f 233 reassembly_dbg("REASSEMBLY: remove packet with offset %u\n", short_be(((struct pico_ipv4_hdr *)f_frag->net_hdr)->frag) & PICO_IPV4_FRAG_MASK);
daniele 10:dd7111d4279f 234 pico_tree_delete(pfrag->t, f_frag);
daniele 10:dd7111d4279f 235 pico_frame_discard(f_frag);
daniele 10:dd7111d4279f 236 }
daniele 10:dd7111d4279f 237 pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
daniele 10:dd7111d4279f 238 pico_free(pfrag->t);
daniele 10:dd7111d4279f 239 pico_free(pfrag);
daniele 10:dd7111d4279f 240 }
daniele 10:dd7111d4279f 241 #endif /* PICO_SUPPORT_IPFRAG */
daniele 10:dd7111d4279f 242
daniele 10:dd7111d4279f 243 #ifdef PICO_SUPPORT_IPFRAG
daniele 10:dd7111d4279f 244 static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
daniele 10:dd7111d4279f 245 {
daniele 10:dd7111d4279f 246 uint8_t *running_pointer = NULL;
daniele 10:dd7111d4279f 247 uint16_t running_offset = 0;
daniele 10:dd7111d4279f 248 uint16_t offset = 0;
daniele 10:dd7111d4279f 249 uint16_t data_len = 0;
daniele 10:dd7111d4279f 250 struct pico_ipv4_hdr *f_frag_hdr = NULL, *hdr = (struct pico_ipv4_hdr *) (*f)->net_hdr;
daniele 10:dd7111d4279f 251 struct pico_udp_hdr *udp_hdr = NULL;
daniele 10:dd7111d4279f 252 struct pico_tcp_hdr *tcp_hdr = NULL;
daniele 10:dd7111d4279f 253 struct pico_ipv4_fragmented_packet *pfrag = NULL, frag;
daniele 10:dd7111d4279f 254 struct pico_frame *f_new = NULL, *f_frag = NULL;
daniele 10:dd7111d4279f 255 struct pico_tree_node *index, *_tmp;
daniele 10:dd7111d4279f 256
daniele 10:dd7111d4279f 257 data_len = short_be(hdr->len) - (*f)->net_len;
daniele 10:dd7111d4279f 258 offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
daniele 10:dd7111d4279f 259 if (short_be(hdr->frag) & PICO_IPV4_MOREFRAG) {
daniele 10:dd7111d4279f 260 if (!offset) {
daniele 10:dd7111d4279f 261 reassembly_dbg("REASSEMBLY: first element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
daniele 10:dd7111d4279f 262 if (!pico_tree_empty(&pico_ipv4_fragmented_tree)) {
daniele 10:dd7111d4279f 263 reassembly_dbg("REASSEMBLY: cleanup tree\n");
daniele 10:dd7111d4279f 264 // only one entry allowed in this tree
daniele 10:dd7111d4279f 265 pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
daniele 10:dd7111d4279f 266 pico_ipv4_fragmented_cleanup(pfrag);
daniele 10:dd7111d4279f 267 }
daniele 10:dd7111d4279f 268 // add entry in tree for this ID and create secondary tree to contain fragmented elements
daniele 10:dd7111d4279f 269 pfrag = pico_zalloc(sizeof(struct pico_ipv4_fragmented_packet));
daniele 10:dd7111d4279f 270 if (!pfrag) {
daniele 10:dd7111d4279f 271 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 272 return -1;
daniele 10:dd7111d4279f 273 }
daniele 10:dd7111d4279f 274 pfrag->id = short_be(hdr->id);
daniele 10:dd7111d4279f 275 pfrag->proto = hdr->proto;
daniele 10:dd7111d4279f 276 pfrag->src.addr = long_be(hdr->src.addr);
daniele 10:dd7111d4279f 277 pfrag->dst.addr = long_be(hdr->dst.addr);
daniele 10:dd7111d4279f 278 pfrag->total_len = short_be(hdr->len) - (*f)->net_len;
daniele 10:dd7111d4279f 279 pfrag->t = pico_zalloc(sizeof(struct pico_tree));
daniele 10:dd7111d4279f 280 if (!pfrag->t) {
daniele 10:dd7111d4279f 281 pico_free(pfrag);
daniele 10:dd7111d4279f 282 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 283 return -1;
daniele 10:dd7111d4279f 284 }
daniele 10:dd7111d4279f 285 pfrag->t->root = &LEAF;
daniele 10:dd7111d4279f 286 pfrag->t->compare = pico_ipv4_fragmented_element_cmp;
daniele 10:dd7111d4279f 287
daniele 10:dd7111d4279f 288 pico_tree_insert(pfrag->t, *f);
daniele 10:dd7111d4279f 289 pico_tree_insert(&pico_ipv4_fragmented_tree, pfrag);
daniele 10:dd7111d4279f 290 return 0;
daniele 10:dd7111d4279f 291 }
daniele 10:dd7111d4279f 292 else {
daniele 10:dd7111d4279f 293 reassembly_dbg("REASSEMBLY: intermediate element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
daniele 10:dd7111d4279f 294 frag.id = short_be(hdr->id);
daniele 10:dd7111d4279f 295 frag.proto = hdr->proto;
daniele 10:dd7111d4279f 296 frag.src.addr = long_be(hdr->src.addr);
daniele 10:dd7111d4279f 297 frag.dst.addr = long_be(hdr->dst.addr);
daniele 10:dd7111d4279f 298 pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
daniele 10:dd7111d4279f 299 if (pfrag) {
daniele 10:dd7111d4279f 300 pfrag->total_len += (short_be(hdr->len) - (*f)->net_len);
daniele 10:dd7111d4279f 301 pico_tree_insert(pfrag->t, *f);
daniele 10:dd7111d4279f 302 return 0;
daniele 10:dd7111d4279f 303 } else {
daniele 10:dd7111d4279f 304 reassembly_dbg("REASSEMBLY: silently discard intermediate frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
daniele 10:dd7111d4279f 305 pico_frame_discard(*f);
daniele 10:dd7111d4279f 306 return 0;
daniele 10:dd7111d4279f 307 }
daniele 10:dd7111d4279f 308 }
daniele 10:dd7111d4279f 309 } else if (offset) {
daniele 10:dd7111d4279f 310 reassembly_dbg("REASSEMBLY: last element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
daniele 10:dd7111d4279f 311 frag.id = short_be(hdr->id);
daniele 10:dd7111d4279f 312 frag.proto = hdr->proto;
daniele 10:dd7111d4279f 313 frag.src.addr = long_be(hdr->src.addr);
daniele 10:dd7111d4279f 314 frag.dst.addr = long_be(hdr->dst.addr);
daniele 10:dd7111d4279f 315 pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
daniele 10:dd7111d4279f 316 if (pfrag) {
daniele 10:dd7111d4279f 317 pfrag->total_len += (short_be(hdr->len) - (*f)->net_len);
daniele 10:dd7111d4279f 318 reassembly_dbg("REASSEMBLY: fragmented packet in tree, reassemble packet of %u data bytes\n", pfrag->total_len);
daniele 10:dd7111d4279f 319 f_new = self->alloc(self, pfrag->total_len);
daniele 10:dd7111d4279f 320
daniele 10:dd7111d4279f 321 f_frag = pico_tree_first(pfrag->t);
daniele 10:dd7111d4279f 322 reassembly_dbg("REASSEMBLY: copy IP header information len = %lu\n", f_frag->net_len);
daniele 10:dd7111d4279f 323 f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
daniele 10:dd7111d4279f 324 data_len = short_be(f_frag_hdr->len) - f_frag->net_len;
daniele 10:dd7111d4279f 325 memcpy(f_new->net_hdr, f_frag->net_hdr, f_frag->net_len);
daniele 10:dd7111d4279f 326 memcpy(f_new->transport_hdr, f_frag->transport_hdr, data_len);
daniele 10:dd7111d4279f 327 running_pointer = f_new->transport_hdr + data_len;
daniele 10:dd7111d4279f 328 offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
daniele 10:dd7111d4279f 329 running_offset = data_len / 8;
daniele 10:dd7111d4279f 330 pico_tree_delete(pfrag->t, f_frag);
daniele 10:dd7111d4279f 331 pico_frame_discard(f_frag);
daniele 10:dd7111d4279f 332 reassembly_dbg("REASSEMBLY: reassembled first packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
daniele 10:dd7111d4279f 333
daniele 10:dd7111d4279f 334 pico_tree_foreach_safe(index, pfrag->t, _tmp)
daniele 10:dd7111d4279f 335 {
daniele 10:dd7111d4279f 336 f_frag = index->keyValue;
daniele 10:dd7111d4279f 337 f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
daniele 10:dd7111d4279f 338 data_len = short_be(f_frag_hdr->len) - f_frag->net_len;
daniele 10:dd7111d4279f 339 memcpy(running_pointer, f_frag->transport_hdr, data_len);
daniele 10:dd7111d4279f 340 running_pointer += data_len;
daniele 10:dd7111d4279f 341 offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
daniele 10:dd7111d4279f 342 if (offset != running_offset) {
daniele 10:dd7111d4279f 343 reassembly_dbg("REASSEMBLY: error reassembling intermediate packet: offset %u != expected offset %u (missing fragment)\n", offset, running_offset);
daniele 10:dd7111d4279f 344 pico_ipv4_fragmented_cleanup(pfrag);
daniele 10:dd7111d4279f 345 return -1;
daniele 10:dd7111d4279f 346 }
daniele 10:dd7111d4279f 347 running_offset += (data_len / 8);
daniele 10:dd7111d4279f 348 pico_tree_delete(pfrag->t, f_frag);
daniele 10:dd7111d4279f 349 pico_frame_discard(f_frag);
daniele 10:dd7111d4279f 350 reassembly_dbg("REASSEMBLY: reassembled intermediate packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
daniele 10:dd7111d4279f 351 }
daniele 10:dd7111d4279f 352 pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
daniele 10:dd7111d4279f 353 pico_free(pfrag);
daniele 10:dd7111d4279f 354
daniele 10:dd7111d4279f 355 data_len = short_be(hdr->len) - (*f)->net_len;
daniele 10:dd7111d4279f 356 memcpy(running_pointer, (*f)->transport_hdr, data_len);
daniele 10:dd7111d4279f 357 offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
daniele 10:dd7111d4279f 358 pico_frame_discard(*f);
daniele 10:dd7111d4279f 359 reassembly_dbg("REASSEMBLY: reassembled last packet of %u data bytes, offset = %u\n", data_len, offset);
daniele 10:dd7111d4279f 360
daniele 10:dd7111d4279f 361 hdr = (struct pico_ipv4_hdr *)f_new->net_hdr;
daniele 10:dd7111d4279f 362 hdr->len = pfrag->total_len;
daniele 10:dd7111d4279f 363 hdr->frag = 0; /* flags cleared and no offset */
daniele 10:dd7111d4279f 364 hdr->crc = 0;
daniele 10:dd7111d4279f 365 hdr->crc = short_be(pico_checksum(hdr, f_new->net_len));
daniele 10:dd7111d4279f 366 /* Optional, the UDP/TCP CRC should already be correct */
daniele 10:dd7111d4279f 367 if (0) {
daniele 10:dd7111d4279f 368 #ifdef PICO_SUPPORT_TCP
daniele 10:dd7111d4279f 369 } else if (hdr->proto == PICO_PROTO_TCP) {
daniele 10:dd7111d4279f 370 tcp_hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
daniele 10:dd7111d4279f 371 tcp_hdr->crc = 0;
daniele 10:dd7111d4279f 372 tcp_hdr->crc = short_be(pico_tcp_checksum_ipv4(f_new));
daniele 10:dd7111d4279f 373 #endif
daniele 10:dd7111d4279f 374 #ifdef PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 375 } else if (hdr->proto == PICO_PROTO_UDP){
daniele 10:dd7111d4279f 376 udp_hdr = (struct pico_udp_hdr *) f_new->transport_hdr;
daniele 10:dd7111d4279f 377 udp_hdr->crc = 0;
daniele 10:dd7111d4279f 378 udp_hdr->crc = short_be(pico_udp_checksum_ipv4(f_new));
daniele 10:dd7111d4279f 379 #endif
daniele 10:dd7111d4279f 380 }
daniele 10:dd7111d4279f 381 reassembly_dbg("REASSEMBLY: packet with id %X reassembled correctly\n", short_be(hdr->id));
daniele 10:dd7111d4279f 382 *f = f_new;
daniele 10:dd7111d4279f 383 return 1;
daniele 10:dd7111d4279f 384 } else {
daniele 10:dd7111d4279f 385 reassembly_dbg("REASSEMBLY: silently discard last frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
daniele 10:dd7111d4279f 386 pico_frame_discard(*f);
daniele 10:dd7111d4279f 387 return 0;
daniele 10:dd7111d4279f 388 }
daniele 10:dd7111d4279f 389 } else {
daniele 10:dd7111d4279f 390 return 1;
daniele 10:dd7111d4279f 391 }
daniele 10:dd7111d4279f 392 }
daniele 10:dd7111d4279f 393 #else
daniele 10:dd7111d4279f 394 static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
daniele 10:dd7111d4279f 395 {
daniele 10:dd7111d4279f 396 return 1;
daniele 10:dd7111d4279f 397 }
daniele 10:dd7111d4279f 398 #endif /* PICO_SUPPORT_IPFRAG */
daniele 10:dd7111d4279f 399
daniele 10:dd7111d4279f 400 #ifdef PICO_SUPPORT_CRC
daniele 10:dd7111d4279f 401 static inline int pico_ipv4_crc_check(struct pico_frame *f)
daniele 10:dd7111d4279f 402 {
daniele 10:dd7111d4279f 403 uint16_t checksum_invalid = 1;
daniele 10:dd7111d4279f 404 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 405
daniele 10:dd7111d4279f 406 checksum_invalid = short_be(pico_checksum(hdr, f->net_len));
daniele 10:dd7111d4279f 407 if (checksum_invalid) {
daniele 10:dd7111d4279f 408 dbg("IP: checksum failed!\n");
daniele 10:dd7111d4279f 409 pico_frame_discard(f);
daniele 10:dd7111d4279f 410 return 0;
daniele 10:dd7111d4279f 411 }
daniele 10:dd7111d4279f 412 return 1;
daniele 10:dd7111d4279f 413 }
daniele 10:dd7111d4279f 414 #else
daniele 10:dd7111d4279f 415 static inline int pico_ipv4_crc_check(struct pico_frame *f)
daniele 10:dd7111d4279f 416 {
daniele 10:dd7111d4279f 417 return 1;
daniele 10:dd7111d4279f 418 }
daniele 10:dd7111d4279f 419 #endif /* PICO_SUPPORT_CRC */
daniele 10:dd7111d4279f 420
daniele 10:dd7111d4279f 421 static int pico_ipv4_forward(struct pico_frame *f);
daniele 10:dd7111d4279f 422 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 423 static int pico_ipv4_mcast_filter(struct pico_frame *f);
daniele 10:dd7111d4279f 424 #endif
daniele 10:dd7111d4279f 425
daniele 10:dd7111d4279f 426 static int ipv4_link_compare(void *ka, void *kb)
daniele 10:dd7111d4279f 427 {
daniele 10:dd7111d4279f 428 struct pico_ipv4_link *a = ka, *b =kb;
daniele 10:dd7111d4279f 429 if (a->address.addr < b->address.addr)
daniele 10:dd7111d4279f 430 return -1;
daniele 10:dd7111d4279f 431 if (a->address.addr > b->address.addr)
daniele 10:dd7111d4279f 432 return 1;
daniele 10:dd7111d4279f 433
daniele 10:dd7111d4279f 434 //zero can be assigned multiple times (e.g. for DHCP)
daniele 10:dd7111d4279f 435 if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY){
daniele 10:dd7111d4279f 436 if (a->dev < b->dev)
daniele 10:dd7111d4279f 437 return -1;
daniele 10:dd7111d4279f 438 if (a->dev > b->dev)
daniele 10:dd7111d4279f 439 return 1;
daniele 10:dd7111d4279f 440 }
daniele 10:dd7111d4279f 441 return 0;
daniele 10:dd7111d4279f 442 }
daniele 10:dd7111d4279f 443
daniele 10:dd7111d4279f 444 PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare);
daniele 10:dd7111d4279f 445
daniele 10:dd7111d4279f 446 static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f)
daniele 10:dd7111d4279f 447 {
daniele 10:dd7111d4279f 448 uint8_t option_len = 0;
daniele 10:dd7111d4279f 449 int ret = 0;
daniele 10:dd7111d4279f 450 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 451 struct pico_ipv4_link test = {.address = {.addr = PICO_IP4_ANY}, .dev = NULL};
daniele 10:dd7111d4279f 452
daniele 10:dd7111d4279f 453 /* NAT needs transport header information */
daniele 10:dd7111d4279f 454 if(((hdr->vhl) & 0x0F )> 5){
daniele 10:dd7111d4279f 455 option_len = 4*(((hdr->vhl) & 0x0F)-5);
daniele 10:dd7111d4279f 456 }
daniele 10:dd7111d4279f 457 f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len;
daniele 10:dd7111d4279f 458 f->transport_len = short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len;
daniele 10:dd7111d4279f 459 f->net_len = PICO_SIZE_IP4HDR + option_len;
daniele 10:dd7111d4279f 460
daniele 10:dd7111d4279f 461 #ifdef PICO_SUPPORT_IPFILTER
daniele 10:dd7111d4279f 462 if (ipfilter(f)) {
daniele 10:dd7111d4279f 463 /*pico_frame is discarded as result of the filtering*/
daniele 10:dd7111d4279f 464 return 0;
daniele 10:dd7111d4279f 465 }
daniele 10:dd7111d4279f 466 #endif
daniele 10:dd7111d4279f 467
daniele 10:dd7111d4279f 468 /* ret == 1 indicates to continue the function */
daniele 10:dd7111d4279f 469 ret = pico_ipv4_crc_check(f);
daniele 10:dd7111d4279f 470 if (ret < 1)
daniele 10:dd7111d4279f 471 return ret;
daniele 10:dd7111d4279f 472 ret = pico_ipv4_fragmented_check(self, &f);
daniele 10:dd7111d4279f 473 if (ret < 1)
daniele 10:dd7111d4279f 474 return ret;
daniele 10:dd7111d4279f 475
daniele 10:dd7111d4279f 476 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 477 /* Multicast address in source, discard quietly */
daniele 10:dd7111d4279f 478 if (pico_ipv4_is_multicast(hdr->src.addr)) {
daniele 10:dd7111d4279f 479 ip_mcast_dbg("MCAST: ERROR multicast address %08X in source address\n", hdr->src.addr);
daniele 10:dd7111d4279f 480 pico_frame_discard(f);
daniele 10:dd7111d4279f 481 return 0;
daniele 10:dd7111d4279f 482 }
daniele 10:dd7111d4279f 483 #endif
daniele 10:dd7111d4279f 484 if (hdr->frag & 0x80) {
daniele 10:dd7111d4279f 485 pico_frame_discard(f); //RFC 3514
daniele 10:dd7111d4279f 486 return 0;
daniele 10:dd7111d4279f 487 }
daniele 10:dd7111d4279f 488 if (0) {
daniele 10:dd7111d4279f 489 #ifdef PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 490 } else if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
daniele 10:dd7111d4279f 491 /* Receiving UDP broadcast datagram */
daniele 10:dd7111d4279f 492 f->flags |= PICO_FRAME_FLAG_BCAST;
daniele 10:dd7111d4279f 493 pico_enqueue(pico_proto_udp.q_in, f);
daniele 10:dd7111d4279f 494 #endif
daniele 10:dd7111d4279f 495 } else if (pico_ipv4_is_multicast(hdr->dst.addr)) {
daniele 10:dd7111d4279f 496 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 497 /* Receiving UDP multicast datagram TODO set f->flags? */
daniele 10:dd7111d4279f 498 if (hdr->proto == PICO_PROTO_IGMP) {
daniele 10:dd7111d4279f 499 ip_mcast_dbg("MCAST: received IGMP message\n");
daniele 10:dd7111d4279f 500 pico_transport_receive(f, PICO_PROTO_IGMP);
daniele 10:dd7111d4279f 501 } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
daniele 10:dd7111d4279f 502 pico_enqueue(pico_proto_udp.q_in, f);
daniele 10:dd7111d4279f 503 } else {
daniele 10:dd7111d4279f 504 pico_frame_discard(f);
daniele 10:dd7111d4279f 505 }
daniele 10:dd7111d4279f 506 #endif
daniele 10:dd7111d4279f 507 } else if (pico_ipv4_link_find(&hdr->dst)) {
daniele 10:dd7111d4279f 508 if (pico_ipv4_nat_isenabled_in(f) == 0) { /* if NAT enabled (dst port registerd), do NAT */
daniele 10:dd7111d4279f 509 if(pico_ipv4_nat(f, hdr->dst) != 0) {
daniele 10:dd7111d4279f 510 return -1;
daniele 10:dd7111d4279f 511 }
daniele 10:dd7111d4279f 512 pico_ipv4_forward(f); /* Local packet became forward packet after NAT */
daniele 10:dd7111d4279f 513 } else { /* no NAT so enqueue to next layer */
daniele 10:dd7111d4279f 514 pico_transport_receive(f, hdr->proto);
daniele 10:dd7111d4279f 515 }
daniele 10:dd7111d4279f 516 } else if (pico_tree_findKey(&Tree_dev_link, &test)){
daniele 10:dd7111d4279f 517 #ifdef PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 518 //address of this device is apparently 0.0.0.0; might be a DHCP packet
daniele 10:dd7111d4279f 519 pico_enqueue(pico_proto_udp.q_in, f);
daniele 10:dd7111d4279f 520 #endif
daniele 10:dd7111d4279f 521 } else {
tass 49:4b404dd2c97a 522
tass 49:4b404dd2c97a 523 if((pico_ipv4_is_broadcast(hdr->dst.addr)))
tass 49:4b404dd2c97a 524 {
tass 49:4b404dd2c97a 525 /* don't forward broadcast frame, discard! */
daniele 10:dd7111d4279f 526 pico_frame_discard(f);
tass 49:4b404dd2c97a 527 } else if (pico_ipv4_forward(f) != 0) {
tass 49:4b404dd2c97a 528 /* Packet is not local. Try to forward. */
tass 49:4b404dd2c97a 529 pico_frame_discard(f);
daniele 10:dd7111d4279f 530 }
daniele 10:dd7111d4279f 531 }
daniele 10:dd7111d4279f 532 return 0;
daniele 10:dd7111d4279f 533 }
daniele 10:dd7111d4279f 534
daniele 10:dd7111d4279f 535 PICO_TREE_DECLARE(Routes, ipv4_route_compare);
daniele 10:dd7111d4279f 536
daniele 10:dd7111d4279f 537
daniele 10:dd7111d4279f 538 static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f)
daniele 10:dd7111d4279f 539 {
daniele 10:dd7111d4279f 540 f->start = (uint8_t*) f->net_hdr;
daniele 10:dd7111d4279f 541 #ifdef PICO_SUPPORT_IPFILTER
daniele 10:dd7111d4279f 542 if (ipfilter(f)) {
daniele 10:dd7111d4279f 543 /*pico_frame is discarded as result of the filtering*/
daniele 10:dd7111d4279f 544 return 0;
daniele 10:dd7111d4279f 545 }
daniele 10:dd7111d4279f 546 #endif
daniele 10:dd7111d4279f 547 return pico_sendto_dev(f);
daniele 10:dd7111d4279f 548 }
daniele 10:dd7111d4279f 549
daniele 10:dd7111d4279f 550
daniele 10:dd7111d4279f 551 static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, int size)
daniele 10:dd7111d4279f 552 {
daniele 10:dd7111d4279f 553 struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
daniele 10:dd7111d4279f 554 if (!f)
daniele 10:dd7111d4279f 555 return NULL;
daniele 10:dd7111d4279f 556 f->datalink_hdr = f->buffer;
daniele 10:dd7111d4279f 557 f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
daniele 10:dd7111d4279f 558 f->net_len = PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 559 f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 560 f->transport_len = size;
daniele 10:dd7111d4279f 561 f->len = size + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 562 return f;
daniele 10:dd7111d4279f 563 }
daniele 10:dd7111d4279f 564
daniele 10:dd7111d4279f 565 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f);
daniele 10:dd7111d4279f 566
daniele 10:dd7111d4279f 567 /* Interface: protocol definition */
daniele 10:dd7111d4279f 568 struct pico_protocol pico_proto_ipv4 = {
daniele 10:dd7111d4279f 569 .name = "ipv4",
daniele 10:dd7111d4279f 570 .proto_number = PICO_PROTO_IPV4,
daniele 10:dd7111d4279f 571 .layer = PICO_LAYER_NETWORK,
daniele 10:dd7111d4279f 572 .alloc = pico_ipv4_alloc,
daniele 10:dd7111d4279f 573 .process_in = pico_ipv4_process_in,
daniele 10:dd7111d4279f 574 .process_out = pico_ipv4_process_out,
daniele 10:dd7111d4279f 575 .push = pico_ipv4_frame_sock_push,
daniele 10:dd7111d4279f 576 .q_in = &in,
daniele 10:dd7111d4279f 577 .q_out = &out,
daniele 10:dd7111d4279f 578 };
daniele 10:dd7111d4279f 579
daniele 10:dd7111d4279f 580 struct pico_ipv4_route
daniele 10:dd7111d4279f 581 {
daniele 10:dd7111d4279f 582 struct pico_ip4 dest;
daniele 10:dd7111d4279f 583 struct pico_ip4 netmask;
daniele 10:dd7111d4279f 584 struct pico_ip4 gateway;
daniele 10:dd7111d4279f 585 struct pico_ipv4_link *link;
daniele 10:dd7111d4279f 586 uint32_t metric;
daniele 10:dd7111d4279f 587 };
daniele 10:dd7111d4279f 588
daniele 10:dd7111d4279f 589
daniele 10:dd7111d4279f 590 static int ipv4_route_compare(void *ka, void * kb)
daniele 10:dd7111d4279f 591 {
daniele 10:dd7111d4279f 592 struct pico_ipv4_route *a = ka, *b = kb;
daniele 10:dd7111d4279f 593
daniele 10:dd7111d4279f 594 /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
daniele 10:dd7111d4279f 595 if (long_be(a->netmask.addr) < long_be(b->netmask.addr))
daniele 10:dd7111d4279f 596 return -1;
daniele 10:dd7111d4279f 597
daniele 10:dd7111d4279f 598 if (long_be(a->netmask.addr) > long_be(b->netmask.addr))
daniele 10:dd7111d4279f 599 return 1;
daniele 10:dd7111d4279f 600
daniele 10:dd7111d4279f 601 if (a->dest.addr < b->dest.addr)
daniele 10:dd7111d4279f 602 return -1;
daniele 10:dd7111d4279f 603
daniele 10:dd7111d4279f 604 if (a->dest.addr > b->dest.addr)
daniele 10:dd7111d4279f 605 return 1;
daniele 10:dd7111d4279f 606
daniele 10:dd7111d4279f 607 if (a->metric < b->metric)
daniele 10:dd7111d4279f 608 return -1;
daniele 10:dd7111d4279f 609
daniele 10:dd7111d4279f 610 if (a->metric > b->metric)
daniele 10:dd7111d4279f 611 return 1;
daniele 10:dd7111d4279f 612
daniele 10:dd7111d4279f 613 return 0;
daniele 10:dd7111d4279f 614 }
daniele 10:dd7111d4279f 615
daniele 10:dd7111d4279f 616 static struct pico_ipv4_route *route_find(struct pico_ip4 *addr)
daniele 10:dd7111d4279f 617 {
daniele 10:dd7111d4279f 618 struct pico_ipv4_route *r;
daniele 10:dd7111d4279f 619 struct pico_tree_node * index;
daniele 10:dd7111d4279f 620
daniele 10:dd7111d4279f 621 if(addr->addr != PICO_IP4_BCAST)
daniele 10:dd7111d4279f 622 {
daniele 10:dd7111d4279f 623 pico_tree_foreach_reverse(index, &Routes) {
daniele 10:dd7111d4279f 624 r = index->keyValue;
daniele 10:dd7111d4279f 625 if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
daniele 10:dd7111d4279f 626 return r;
daniele 10:dd7111d4279f 627 }
daniele 10:dd7111d4279f 628 }
daniele 10:dd7111d4279f 629 }
daniele 10:dd7111d4279f 630 else
daniele 10:dd7111d4279f 631 {
daniele 10:dd7111d4279f 632 r = pico_tree_first(&Routes);
daniele 10:dd7111d4279f 633 if(!r->netmask.addr)
daniele 10:dd7111d4279f 634 {
daniele 10:dd7111d4279f 635 return r;
daniele 10:dd7111d4279f 636 }
daniele 10:dd7111d4279f 637 else
daniele 10:dd7111d4279f 638 {
daniele 10:dd7111d4279f 639 dbg("WARNING: no default route for a global broadcast found\n");
daniele 10:dd7111d4279f 640 }
daniele 10:dd7111d4279f 641 }
daniele 10:dd7111d4279f 642
daniele 10:dd7111d4279f 643 return NULL;
daniele 10:dd7111d4279f 644 }
daniele 10:dd7111d4279f 645
daniele 10:dd7111d4279f 646 struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
daniele 10:dd7111d4279f 647 {
daniele 10:dd7111d4279f 648 struct pico_ip4 nullip;
daniele 10:dd7111d4279f 649 struct pico_ipv4_route *route;
daniele 10:dd7111d4279f 650 nullip.addr = 0U;
daniele 10:dd7111d4279f 651
daniele 10:dd7111d4279f 652 if(!addr) {
daniele 10:dd7111d4279f 653 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 654 return nullip;
daniele 10:dd7111d4279f 655 }
daniele 10:dd7111d4279f 656
daniele 10:dd7111d4279f 657 route = route_find(addr);
daniele 10:dd7111d4279f 658 if (!route) {
daniele 10:dd7111d4279f 659 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 660 return nullip;
daniele 10:dd7111d4279f 661 }
daniele 10:dd7111d4279f 662 else
daniele 10:dd7111d4279f 663 return route->gateway;
daniele 10:dd7111d4279f 664 }
daniele 10:dd7111d4279f 665
daniele 10:dd7111d4279f 666 struct pico_ip4 *pico_ipv4_source_find(struct pico_ip4 *dst)
daniele 10:dd7111d4279f 667 {
daniele 10:dd7111d4279f 668 struct pico_ip4 *myself = NULL;
daniele 10:dd7111d4279f 669 struct pico_ipv4_route *rt;
daniele 10:dd7111d4279f 670
daniele 10:dd7111d4279f 671 if(!dst) {
daniele 10:dd7111d4279f 672 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 673 return NULL;
daniele 10:dd7111d4279f 674 }
daniele 10:dd7111d4279f 675
daniele 10:dd7111d4279f 676 rt = route_find(dst);
daniele 10:dd7111d4279f 677 if (rt) {
daniele 10:dd7111d4279f 678 myself = &rt->link->address;
daniele 10:dd7111d4279f 679 } else
daniele 10:dd7111d4279f 680 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 681 return myself;
daniele 10:dd7111d4279f 682 }
daniele 10:dd7111d4279f 683
daniele 10:dd7111d4279f 684
daniele 10:dd7111d4279f 685 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 686 /* link
daniele 10:dd7111d4279f 687 * |
daniele 10:dd7111d4279f 688 * MCASTGroups
daniele 10:dd7111d4279f 689 * | | |
daniele 10:dd7111d4279f 690 * ------------ | ------------
daniele 10:dd7111d4279f 691 * | | |
daniele 10:dd7111d4279f 692 * MCASTSources MCASTSources MCASTSources
daniele 10:dd7111d4279f 693 * | | | | | | | | | | | |
daniele 10:dd7111d4279f 694 * S S S S S S S S S S S S
daniele 10:dd7111d4279f 695 *
daniele 10:dd7111d4279f 696 * MCASTGroups: RBTree(mcast_group)
daniele 10:dd7111d4279f 697 * MCASTSources: RBTree(source)
daniele 10:dd7111d4279f 698 */
daniele 10:dd7111d4279f 699 static int ipv4_mcast_groups_cmp(void * ka, void * kb)
daniele 10:dd7111d4279f 700 {
daniele 10:dd7111d4279f 701 struct pico_mcast_group *a = ka, *b = kb;
daniele 10:dd7111d4279f 702 if (a->mcast_addr.addr < b->mcast_addr.addr) {
daniele 10:dd7111d4279f 703 return -1;
daniele 10:dd7111d4279f 704 } else if (a->mcast_addr.addr > b->mcast_addr.addr) {
daniele 10:dd7111d4279f 705 return 1;
daniele 10:dd7111d4279f 706 } else {
daniele 10:dd7111d4279f 707 return 0;
daniele 10:dd7111d4279f 708 }
daniele 10:dd7111d4279f 709 }
daniele 10:dd7111d4279f 710
daniele 10:dd7111d4279f 711 static int ipv4_mcast_sources_cmp(void *ka, void *kb)
daniele 10:dd7111d4279f 712 {
daniele 10:dd7111d4279f 713 struct pico_ip4 *a = ka, *b = kb;
daniele 10:dd7111d4279f 714 if (a->addr < b->addr)
daniele 10:dd7111d4279f 715 return -1;
daniele 10:dd7111d4279f 716 if (a->addr > b->addr)
daniele 10:dd7111d4279f 717 return 1;
daniele 10:dd7111d4279f 718 return 0;
daniele 10:dd7111d4279f 719 }
daniele 10:dd7111d4279f 720
daniele 10:dd7111d4279f 721 static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
daniele 10:dd7111d4279f 722 {
daniele 10:dd7111d4279f 723 uint16_t i = 0;
daniele 10:dd7111d4279f 724 struct pico_mcast_group __attribute__ ((unused)) *g = NULL;
daniele 10:dd7111d4279f 725 struct pico_ip4 __attribute__ ((unused)) *source = NULL;
daniele 10:dd7111d4279f 726 struct pico_tree_node *index = NULL, *index2 = NULL;
daniele 10:dd7111d4279f 727
daniele 10:dd7111d4279f 728 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
daniele 10:dd7111d4279f 729 ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name);
daniele 10:dd7111d4279f 730 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
daniele 10:dd7111d4279f 731 ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n");
daniele 10:dd7111d4279f 732 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
daniele 10:dd7111d4279f 733
daniele 10:dd7111d4279f 734 pico_tree_foreach(index, mcast_link->MCASTGroups)
daniele 10:dd7111d4279f 735 {
daniele 10:dd7111d4279f 736 g = index->keyValue;
daniele 10:dd7111d4279f 737 ip_mcast_dbg("+ %04d | %16s | %08X | %05u | %u | %8s +\n", i, mcast_link->dev->name, g->mcast_addr.addr, g->reference_count, g->filter_mode, "");
daniele 10:dd7111d4279f 738 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 739 {
daniele 10:dd7111d4279f 740 source = index2->keyValue;
daniele 10:dd7111d4279f 741 ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr);
daniele 10:dd7111d4279f 742 }
daniele 10:dd7111d4279f 743 i++;
daniele 10:dd7111d4279f 744 }
daniele 10:dd7111d4279f 745 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
daniele 10:dd7111d4279f 746 }
daniele 10:dd7111d4279f 747
daniele 10:dd7111d4279f 748 int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
daniele 10:dd7111d4279f 749 {
daniele 10:dd7111d4279f 750 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 751 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 752 struct pico_tree_node *index = NULL, *_tmp = NULL;
daniele 10:dd7111d4279f 753 struct pico_ip4 *source = NULL;
daniele 10:dd7111d4279f 754
daniele 10:dd7111d4279f 755 if (mcast_link)
daniele 10:dd7111d4279f 756 link = pico_ipv4_link_get(mcast_link);
daniele 10:dd7111d4279f 757 else
daniele 10:dd7111d4279f 758 link = mcast_default_link;
daniele 10:dd7111d4279f 759
daniele 10:dd7111d4279f 760 test.mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 761 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 762 if (g) {
daniele 10:dd7111d4279f 763 if (reference_count)
daniele 10:dd7111d4279f 764 g->reference_count++;
daniele 10:dd7111d4279f 765 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
daniele 10:dd7111d4279f 766 } else {
daniele 10:dd7111d4279f 767 g = pico_zalloc(sizeof(struct pico_mcast_group));
daniele 10:dd7111d4279f 768 if (!g) {
daniele 10:dd7111d4279f 769 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 770 return -1;
daniele 10:dd7111d4279f 771 }
daniele 10:dd7111d4279f 772 /* "non-existent" state of filter mode INCLUDE and empty source list */
daniele 10:dd7111d4279f 773 g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
daniele 10:dd7111d4279f 774 g->reference_count = 1;
daniele 10:dd7111d4279f 775 g->mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 776 g->MCASTSources.root = &LEAF;
daniele 10:dd7111d4279f 777 g->MCASTSources.compare = ipv4_mcast_sources_cmp;
daniele 10:dd7111d4279f 778 pico_tree_insert(link->MCASTGroups, g);
daniele 10:dd7111d4279f 779 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
daniele 10:dd7111d4279f 780 }
daniele 10:dd7111d4279f 781
daniele 10:dd7111d4279f 782 /* cleanup filter */
daniele 10:dd7111d4279f 783 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 784 {
daniele 10:dd7111d4279f 785 source = index->keyValue;
daniele 10:dd7111d4279f 786 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 787 pico_free(source);
daniele 10:dd7111d4279f 788 }
daniele 10:dd7111d4279f 789 /* insert new filter */
daniele 10:dd7111d4279f 790 if (MCASTFilter) {
daniele 10:dd7111d4279f 791 pico_tree_foreach(index, MCASTFilter)
daniele 10:dd7111d4279f 792 {
daniele 10:dd7111d4279f 793 source = pico_zalloc(sizeof(struct pico_ip4));
daniele 10:dd7111d4279f 794 if (!source) {
daniele 10:dd7111d4279f 795 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 796 return -1;
daniele 10:dd7111d4279f 797 }
daniele 10:dd7111d4279f 798 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
daniele 10:dd7111d4279f 799 pico_tree_insert(&g->MCASTSources, source);
daniele 10:dd7111d4279f 800 }
daniele 10:dd7111d4279f 801 }
daniele 10:dd7111d4279f 802 g->filter_mode = filter_mode;
daniele 10:dd7111d4279f 803
daniele 10:dd7111d4279f 804 pico_ipv4_mcast_print_groups(link);
daniele 10:dd7111d4279f 805 return 0;
daniele 10:dd7111d4279f 806 }
daniele 10:dd7111d4279f 807
daniele 10:dd7111d4279f 808 int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
daniele 10:dd7111d4279f 809 {
daniele 10:dd7111d4279f 810
daniele 10:dd7111d4279f 811 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 812 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 813 struct pico_tree_node *index = NULL, *_tmp = NULL;
daniele 10:dd7111d4279f 814 struct pico_ip4 *source = NULL;
daniele 10:dd7111d4279f 815
daniele 10:dd7111d4279f 816 if (mcast_link)
daniele 10:dd7111d4279f 817 link = pico_ipv4_link_get(mcast_link);
daniele 10:dd7111d4279f 818 else
daniele 10:dd7111d4279f 819 link = mcast_default_link;
daniele 10:dd7111d4279f 820
daniele 10:dd7111d4279f 821 test.mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 822 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 823 if (!g) {
daniele 10:dd7111d4279f 824 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 825 return -1;
daniele 10:dd7111d4279f 826 } else {
daniele 10:dd7111d4279f 827 if (reference_count && (--(g->reference_count) < 1)) {
daniele 10:dd7111d4279f 828 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
daniele 10:dd7111d4279f 829 /* cleanup filter */
daniele 10:dd7111d4279f 830 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 831 {
daniele 10:dd7111d4279f 832 source = index->keyValue;
daniele 10:dd7111d4279f 833 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 834 pico_free(source);
daniele 10:dd7111d4279f 835 }
daniele 10:dd7111d4279f 836 pico_tree_delete(link->MCASTGroups, g);
daniele 10:dd7111d4279f 837 pico_free(g);
daniele 10:dd7111d4279f 838 } else {
daniele 10:dd7111d4279f 839 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
daniele 10:dd7111d4279f 840 /* cleanup filter */
daniele 10:dd7111d4279f 841 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 842 {
daniele 10:dd7111d4279f 843 source = index->keyValue;
daniele 10:dd7111d4279f 844 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 845 pico_free(source);
daniele 10:dd7111d4279f 846 }
daniele 10:dd7111d4279f 847 /* insert new filter */
daniele 10:dd7111d4279f 848 if (MCASTFilter) {
daniele 10:dd7111d4279f 849 pico_tree_foreach(index, MCASTFilter)
daniele 10:dd7111d4279f 850 {
daniele 10:dd7111d4279f 851 source = pico_zalloc(sizeof(struct pico_ip4));
daniele 10:dd7111d4279f 852 if (!source) {
daniele 10:dd7111d4279f 853 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 854 return -1;
daniele 10:dd7111d4279f 855 }
daniele 10:dd7111d4279f 856 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
daniele 10:dd7111d4279f 857 pico_tree_insert(&g->MCASTSources, source);
daniele 10:dd7111d4279f 858 }
daniele 10:dd7111d4279f 859 }
daniele 10:dd7111d4279f 860 g->filter_mode = filter_mode;
daniele 10:dd7111d4279f 861 }
daniele 10:dd7111d4279f 862 }
daniele 10:dd7111d4279f 863
daniele 10:dd7111d4279f 864 pico_ipv4_mcast_print_groups(link);
daniele 10:dd7111d4279f 865 return 0;
daniele 10:dd7111d4279f 866 }
daniele 10:dd7111d4279f 867
daniele 10:dd7111d4279f 868 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
daniele 10:dd7111d4279f 869 {
daniele 10:dd7111d4279f 870 return mcast_default_link;
daniele 10:dd7111d4279f 871 }
daniele 10:dd7111d4279f 872
daniele 10:dd7111d4279f 873 static int pico_ipv4_mcast_filter(struct pico_frame *f)
daniele 10:dd7111d4279f 874 {
daniele 10:dd7111d4279f 875 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 876 struct pico_tree_node *index = NULL, *index2 = NULL;
daniele 10:dd7111d4279f 877 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 878 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 879
daniele 10:dd7111d4279f 880 test.mcast_addr = hdr->dst;
daniele 10:dd7111d4279f 881
daniele 10:dd7111d4279f 882 pico_tree_foreach(index, &Tree_dev_link)
daniele 10:dd7111d4279f 883 {
daniele 10:dd7111d4279f 884 link = index->keyValue;
daniele 10:dd7111d4279f 885 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 886 if (g) {
daniele 10:dd7111d4279f 887 if (f->dev == link->dev) {
daniele 10:dd7111d4279f 888 ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
daniele 10:dd7111d4279f 889 /* perform source filtering */
daniele 10:dd7111d4279f 890 switch (g->filter_mode)
daniele 10:dd7111d4279f 891 {
daniele 10:dd7111d4279f 892 case PICO_IP_MULTICAST_INCLUDE:
daniele 10:dd7111d4279f 893 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 894 {
daniele 10:dd7111d4279f 895 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
daniele 10:dd7111d4279f 896 ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 897 return 0;
daniele 10:dd7111d4279f 898 }
daniele 10:dd7111d4279f 899 }
daniele 10:dd7111d4279f 900 ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 901 return -1;
daniele 10:dd7111d4279f 902 break;
daniele 10:dd7111d4279f 903
daniele 10:dd7111d4279f 904 case PICO_IP_MULTICAST_EXCLUDE:
daniele 10:dd7111d4279f 905 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 906 {
daniele 10:dd7111d4279f 907 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
daniele 10:dd7111d4279f 908 ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 909 return -1;
daniele 10:dd7111d4279f 910 }
daniele 10:dd7111d4279f 911 }
daniele 10:dd7111d4279f 912 ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 913 return 0;
daniele 10:dd7111d4279f 914 break;
daniele 10:dd7111d4279f 915
daniele 10:dd7111d4279f 916 default:
daniele 10:dd7111d4279f 917 return -1;
daniele 10:dd7111d4279f 918 break;
daniele 10:dd7111d4279f 919 }
daniele 10:dd7111d4279f 920 } else {
daniele 10:dd7111d4279f 921 ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name);
daniele 10:dd7111d4279f 922 }
daniele 10:dd7111d4279f 923 } else {
daniele 10:dd7111d4279f 924 ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name);
daniele 10:dd7111d4279f 925 }
daniele 10:dd7111d4279f 926 }
daniele 10:dd7111d4279f 927 return -1;
daniele 10:dd7111d4279f 928 }
daniele 10:dd7111d4279f 929
daniele 10:dd7111d4279f 930 #else
daniele 10:dd7111d4279f 931
daniele 10:dd7111d4279f 932 int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
daniele 10:dd7111d4279f 933 {
daniele 10:dd7111d4279f 934 pico_err = PICO_ERR_EPROTONOSUPPORT;
daniele 10:dd7111d4279f 935 return -1;
daniele 10:dd7111d4279f 936 }
daniele 10:dd7111d4279f 937 int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
daniele 10:dd7111d4279f 938 {
daniele 10:dd7111d4279f 939 pico_err = PICO_ERR_EPROTONOSUPPORT;
daniele 10:dd7111d4279f 940 return -1;
daniele 10:dd7111d4279f 941 }
daniele 10:dd7111d4279f 942 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
daniele 10:dd7111d4279f 943 {
daniele 10:dd7111d4279f 944 pico_err = PICO_ERR_EPROTONOSUPPORT;
daniele 10:dd7111d4279f 945 return NULL;
daniele 10:dd7111d4279f 946 }
daniele 10:dd7111d4279f 947 #endif /* PICO_SUPPORT_MCAST */
daniele 10:dd7111d4279f 948
daniele 10:dd7111d4279f 949 int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto)
daniele 10:dd7111d4279f 950 {
daniele 10:dd7111d4279f 951
daniele 10:dd7111d4279f 952 struct pico_ipv4_route *route;
daniele 10:dd7111d4279f 953 struct pico_ipv4_link *link;
daniele 10:dd7111d4279f 954 struct pico_ipv4_hdr *hdr;
daniele 10:dd7111d4279f 955 uint8_t ttl = PICO_IPV4_DEFAULT_TTL;
daniele 10:dd7111d4279f 956 uint8_t vhl = 0x45; /* version 4, header length 20 */
daniele 10:dd7111d4279f 957 static uint16_t ipv4_progressive_id = 0x91c0;
daniele 10:dd7111d4279f 958 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 959 struct pico_tree_node *index;
daniele 10:dd7111d4279f 960 #endif
daniele 10:dd7111d4279f 961
daniele 10:dd7111d4279f 962 if(!f || !dst) {
daniele 10:dd7111d4279f 963 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 964 return -1;
daniele 10:dd7111d4279f 965 }
daniele 10:dd7111d4279f 966 hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 967 if (!hdr) {
daniele 10:dd7111d4279f 968 dbg("IP header error\n");
daniele 10:dd7111d4279f 969 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 970 goto drop;
daniele 10:dd7111d4279f 971 }
daniele 10:dd7111d4279f 972
daniele 10:dd7111d4279f 973 if (dst->addr == 0) {
daniele 10:dd7111d4279f 974 dbg("IP destination addr error\n");
daniele 10:dd7111d4279f 975 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 976 goto drop;
daniele 10:dd7111d4279f 977 }
daniele 10:dd7111d4279f 978
daniele 10:dd7111d4279f 979 route = route_find(dst);
daniele 10:dd7111d4279f 980 if (!route) {
daniele 10:dd7111d4279f 981 dbg("Route to %08x not found.\n", long_be(dst->addr));
daniele 10:dd7111d4279f 982 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 983 goto drop;
daniele 10:dd7111d4279f 984 } else {
daniele 10:dd7111d4279f 985 link = route->link;
daniele 10:dd7111d4279f 986 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 987 if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */
daniele 10:dd7111d4279f 988 switch (proto) {
daniele 10:dd7111d4279f 989 case PICO_PROTO_UDP:
daniele 10:dd7111d4279f 990 if(pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
daniele 10:dd7111d4279f 991 ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
daniele 10:dd7111d4279f 992 break;
daniele 10:dd7111d4279f 993 case PICO_PROTO_IGMP:
daniele 10:dd7111d4279f 994 vhl = 0x46; /* header length 24 */
daniele 10:dd7111d4279f 995 ttl = 1;
daniele 10:dd7111d4279f 996 /* router alert (RFC 2113) */
daniele 10:dd7111d4279f 997 hdr->options[0] = 0x94;
daniele 10:dd7111d4279f 998 hdr->options[1] = 0x04;
daniele 10:dd7111d4279f 999 hdr->options[2] = 0x00;
daniele 10:dd7111d4279f 1000 hdr->options[3] = 0x00;
daniele 10:dd7111d4279f 1001 if (f->dev && link->dev != f->dev) { /* default link is not requested link */
daniele 10:dd7111d4279f 1002 pico_tree_foreach(index, &Tree_dev_link) {
daniele 10:dd7111d4279f 1003 link = index->keyValue;
daniele 10:dd7111d4279f 1004 if (link->dev == f->dev)
daniele 10:dd7111d4279f 1005 break;
daniele 10:dd7111d4279f 1006 }
daniele 10:dd7111d4279f 1007 }
daniele 10:dd7111d4279f 1008 break;
daniele 10:dd7111d4279f 1009 default:
daniele 10:dd7111d4279f 1010 ttl = PICO_IPV4_DEFAULT_TTL;
daniele 10:dd7111d4279f 1011 }
daniele 10:dd7111d4279f 1012 }
daniele 10:dd7111d4279f 1013 #endif
daniele 10:dd7111d4279f 1014 }
daniele 10:dd7111d4279f 1015
daniele 10:dd7111d4279f 1016 hdr->vhl = vhl;
daniele 10:dd7111d4279f 1017 hdr->len = short_be(f->transport_len + f->net_len);
daniele 10:dd7111d4279f 1018 if (f->transport_hdr != f->payload)
daniele 10:dd7111d4279f 1019 ipv4_progressive_id++;
daniele 10:dd7111d4279f 1020 hdr->id = short_be(ipv4_progressive_id);
daniele 10:dd7111d4279f 1021 hdr->dst.addr = dst->addr;
daniele 10:dd7111d4279f 1022 hdr->src.addr = link->address.addr;
daniele 10:dd7111d4279f 1023 hdr->ttl = ttl;
daniele 10:dd7111d4279f 1024 hdr->proto = proto;
daniele 10:dd7111d4279f 1025 hdr->frag = short_be(PICO_IPV4_DONTFRAG);
daniele 10:dd7111d4279f 1026 #ifdef PICO_SUPPORT_IPFRAG
daniele 10:dd7111d4279f 1027 # ifdef PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 1028 if (proto == PICO_PROTO_UDP) {
daniele 10:dd7111d4279f 1029 /* first fragment, can not use transport_len to calculate IP length */
daniele 10:dd7111d4279f 1030 if (f->transport_hdr != f->payload)
daniele 10:dd7111d4279f 1031 hdr->len = short_be(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len);
daniele 10:dd7111d4279f 1032 /* set fragmentation flags and offset calculated in socket layer */
daniele 10:dd7111d4279f 1033 hdr->frag = f->frag;
daniele 10:dd7111d4279f 1034 }
daniele 10:dd7111d4279f 1035 # endif /* PICO_SUPPORT_UDP */
daniele 10:dd7111d4279f 1036 #endif /* PICO_SUPPORT_IPFRAG */
daniele 10:dd7111d4279f 1037 pico_ipv4_checksum(f);
daniele 10:dd7111d4279f 1038
daniele 10:dd7111d4279f 1039 if (f->sock && f->sock->dev){
daniele 10:dd7111d4279f 1040 //if the socket has its device set, use that (currently used for DHCP)
daniele 10:dd7111d4279f 1041 f->dev = f->sock->dev;
daniele 10:dd7111d4279f 1042 } else {
daniele 10:dd7111d4279f 1043 f->dev = link->dev;
daniele 10:dd7111d4279f 1044 }
daniele 10:dd7111d4279f 1045
daniele 10:dd7111d4279f 1046 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1047 if (pico_ipv4_is_multicast(hdr->dst.addr)) {
daniele 10:dd7111d4279f 1048 struct pico_frame *cpy;
daniele 10:dd7111d4279f 1049 /* Sending UDP multicast datagram, am I member? If so, loopback copy */
daniele 10:dd7111d4279f 1050 if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) {
daniele 10:dd7111d4279f 1051 ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n");
daniele 10:dd7111d4279f 1052 cpy = pico_frame_copy(f);
daniele 10:dd7111d4279f 1053 pico_enqueue(&in, cpy);
daniele 10:dd7111d4279f 1054 }
daniele 10:dd7111d4279f 1055 }
daniele 10:dd7111d4279f 1056 #endif
daniele 10:dd7111d4279f 1057
daniele 10:dd7111d4279f 1058 if(pico_ipv4_link_get(&hdr->dst)){
daniele 10:dd7111d4279f 1059 //it's our own IP
daniele 10:dd7111d4279f 1060 return pico_enqueue(&in, f);
daniele 10:dd7111d4279f 1061 }else{
daniele 10:dd7111d4279f 1062 /* TODO: Check if there are members subscribed here */
daniele 10:dd7111d4279f 1063 return pico_enqueue(&out, f);
daniele 10:dd7111d4279f 1064 }
daniele 10:dd7111d4279f 1065
daniele 10:dd7111d4279f 1066 drop:
daniele 10:dd7111d4279f 1067 pico_frame_discard(f);
daniele 10:dd7111d4279f 1068 return -1;
daniele 10:dd7111d4279f 1069 }
daniele 10:dd7111d4279f 1070
daniele 10:dd7111d4279f 1071
daniele 10:dd7111d4279f 1072 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f)
daniele 10:dd7111d4279f 1073 {
daniele 10:dd7111d4279f 1074 struct pico_ip4 *dst;
daniele 10:dd7111d4279f 1075 struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
daniele 10:dd7111d4279f 1076 if (!f->sock) {
daniele 10:dd7111d4279f 1077 pico_frame_discard(f);
daniele 10:dd7111d4279f 1078 return -1;
daniele 10:dd7111d4279f 1079 }
daniele 10:dd7111d4279f 1080
daniele 10:dd7111d4279f 1081 if (remote_duple) {
daniele 10:dd7111d4279f 1082 dst = &remote_duple->remote_addr.ip4;
daniele 10:dd7111d4279f 1083 } else {
daniele 10:dd7111d4279f 1084 dst = &f->sock->remote_addr.ip4;
daniele 10:dd7111d4279f 1085 }
daniele 10:dd7111d4279f 1086
daniele 10:dd7111d4279f 1087 return pico_ipv4_frame_push(f, dst, f->sock->proto->proto_number);
daniele 10:dd7111d4279f 1088 }
daniele 10:dd7111d4279f 1089
daniele 10:dd7111d4279f 1090
daniele 10:dd7111d4279f 1091 #ifdef DEBUG_ROUTE
daniele 10:dd7111d4279f 1092 static void dbg_route(void)
daniele 10:dd7111d4279f 1093 {
daniele 10:dd7111d4279f 1094 struct pico_ipv4_route *r;
daniele 10:dd7111d4279f 1095 struct pico_tree_node * index;
daniele 10:dd7111d4279f 1096 pico_tree_foreach(index,&Routes){
daniele 10:dd7111d4279f 1097 r = index->keyValue;
daniele 10:dd7111d4279f 1098 dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric);
daniele 10:dd7111d4279f 1099 }
daniele 10:dd7111d4279f 1100 }
daniele 10:dd7111d4279f 1101 #else
daniele 10:dd7111d4279f 1102 #define dbg_route() do{ }while(0)
daniele 10:dd7111d4279f 1103 #endif
daniele 10:dd7111d4279f 1104
daniele 10:dd7111d4279f 1105 int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
daniele 10:dd7111d4279f 1106 {
daniele 10:dd7111d4279f 1107 struct pico_ipv4_route test, *new;
daniele 10:dd7111d4279f 1108 test.dest.addr = address.addr;
daniele 10:dd7111d4279f 1109 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1110 test.metric = metric;
daniele 10:dd7111d4279f 1111
daniele 10:dd7111d4279f 1112 if(pico_tree_findKey(&Routes,&test)){
daniele 10:dd7111d4279f 1113 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1114 return -1;
daniele 10:dd7111d4279f 1115 }
daniele 10:dd7111d4279f 1116
daniele 10:dd7111d4279f 1117 new = pico_zalloc(sizeof(struct pico_ipv4_route));
daniele 10:dd7111d4279f 1118 if (!new) {
daniele 10:dd7111d4279f 1119 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1120 return -1;
daniele 10:dd7111d4279f 1121 }
daniele 10:dd7111d4279f 1122 new->dest.addr = address.addr;
daniele 10:dd7111d4279f 1123 new->netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1124 new->gateway.addr = gateway.addr;
daniele 10:dd7111d4279f 1125 new->metric = metric;
daniele 10:dd7111d4279f 1126 if (gateway.addr == 0) {
daniele 10:dd7111d4279f 1127 /* No gateway provided, use the link */
daniele 10:dd7111d4279f 1128 new->link = link;
daniele 10:dd7111d4279f 1129 } else {
daniele 10:dd7111d4279f 1130 struct pico_ipv4_route *r = route_find(&gateway);
daniele 10:dd7111d4279f 1131 if (!r ) { /* Specified Gateway is unreachable */
daniele 10:dd7111d4279f 1132 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 1133 pico_free(new);
daniele 10:dd7111d4279f 1134 return -1;
daniele 10:dd7111d4279f 1135 }
daniele 10:dd7111d4279f 1136 if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
daniele 10:dd7111d4279f 1137 pico_err = PICO_ERR_ENETUNREACH;
daniele 10:dd7111d4279f 1138 pico_free(new);
daniele 10:dd7111d4279f 1139 return -1;
daniele 10:dd7111d4279f 1140 }
daniele 10:dd7111d4279f 1141 new->link = r->link;
daniele 10:dd7111d4279f 1142 }
daniele 10:dd7111d4279f 1143 if (!new->link) {
daniele 10:dd7111d4279f 1144 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1145 pico_free(new);
daniele 10:dd7111d4279f 1146 return -1;
daniele 10:dd7111d4279f 1147 }
daniele 10:dd7111d4279f 1148
daniele 10:dd7111d4279f 1149 pico_tree_insert(&Routes,new);
daniele 10:dd7111d4279f 1150 dbg_route();
daniele 10:dd7111d4279f 1151 return 0;
daniele 10:dd7111d4279f 1152 }
daniele 10:dd7111d4279f 1153
daniele 10:dd7111d4279f 1154 int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
daniele 10:dd7111d4279f 1155 {
daniele 10:dd7111d4279f 1156 struct pico_ipv4_route test, *found;
daniele 10:dd7111d4279f 1157 if (!link) {
daniele 10:dd7111d4279f 1158 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1159 return -1;
daniele 10:dd7111d4279f 1160 }
daniele 10:dd7111d4279f 1161 test.dest.addr = address.addr;
daniele 10:dd7111d4279f 1162 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1163 test.metric = metric;
daniele 10:dd7111d4279f 1164
daniele 10:dd7111d4279f 1165 found = pico_tree_findKey(&Routes,&test);
daniele 10:dd7111d4279f 1166 if (found) {
daniele 10:dd7111d4279f 1167
daniele 10:dd7111d4279f 1168 pico_tree_delete(&Routes,found);
daniele 10:dd7111d4279f 1169 pico_free(found);
daniele 10:dd7111d4279f 1170
daniele 10:dd7111d4279f 1171 dbg_route();
daniele 10:dd7111d4279f 1172 return 0;
daniele 10:dd7111d4279f 1173 }
daniele 10:dd7111d4279f 1174 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1175 return -1;
daniele 10:dd7111d4279f 1176 }
daniele 10:dd7111d4279f 1177
daniele 10:dd7111d4279f 1178
daniele 10:dd7111d4279f 1179 int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
daniele 10:dd7111d4279f 1180 {
daniele 10:dd7111d4279f 1181 struct pico_ipv4_link test, *new;
daniele 10:dd7111d4279f 1182 struct pico_ip4 network, gateway;
daniele 10:dd7111d4279f 1183 char ipstr[30];
daniele 10:dd7111d4279f 1184
daniele 10:dd7111d4279f 1185 if(!dev) {
daniele 10:dd7111d4279f 1186 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1187 return -1;
daniele 10:dd7111d4279f 1188 }
daniele 10:dd7111d4279f 1189 test.address.addr = address.addr;
daniele 10:dd7111d4279f 1190 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1191 test.dev = dev;
daniele 10:dd7111d4279f 1192 /** XXX: Valid netmask / unicast address test **/
daniele 10:dd7111d4279f 1193
daniele 10:dd7111d4279f 1194 if(pico_tree_findKey(&Tree_dev_link, &test)) {
daniele 10:dd7111d4279f 1195 dbg("IPv4: Trying to assign an invalid address (in use)\n");
daniele 10:dd7111d4279f 1196 pico_err = PICO_ERR_EADDRINUSE;
daniele 10:dd7111d4279f 1197 return -1;
daniele 10:dd7111d4279f 1198 }
daniele 10:dd7111d4279f 1199
daniele 10:dd7111d4279f 1200 /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/
daniele 10:dd7111d4279f 1201 new = pico_zalloc(sizeof(struct pico_ipv4_link));
daniele 10:dd7111d4279f 1202 if (!new) {
daniele 10:dd7111d4279f 1203 dbg("IPv4: Out of memory!\n");
daniele 10:dd7111d4279f 1204 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1205 return -1;
daniele 10:dd7111d4279f 1206 }
daniele 10:dd7111d4279f 1207 new->address.addr = address.addr;
daniele 10:dd7111d4279f 1208 new->netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1209 new->dev = dev;
daniele 10:dd7111d4279f 1210 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1211 new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree));
daniele 10:dd7111d4279f 1212 if (!new->MCASTGroups) {
daniele 10:dd7111d4279f 1213 pico_free(new);
daniele 10:dd7111d4279f 1214 dbg("IPv4: Out of memory!\n");
daniele 10:dd7111d4279f 1215 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1216 return -1;
daniele 10:dd7111d4279f 1217 }
daniele 10:dd7111d4279f 1218
daniele 10:dd7111d4279f 1219 new->MCASTGroups->root = &LEAF;
daniele 10:dd7111d4279f 1220 new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
daniele 10:dd7111d4279f 1221 new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
daniele 10:dd7111d4279f 1222 new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
daniele 10:dd7111d4279f 1223 #endif
daniele 10:dd7111d4279f 1224
daniele 10:dd7111d4279f 1225 pico_tree_insert(&Tree_dev_link, new);
daniele 10:dd7111d4279f 1226 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1227 do {
daniele 10:dd7111d4279f 1228 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
daniele 10:dd7111d4279f 1229 if (!mcast_default_link) {
daniele 10:dd7111d4279f 1230 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
daniele 10:dd7111d4279f 1231 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
daniele 10:dd7111d4279f 1232 mcast_gw.addr = long_be(0x00000000);
daniele 10:dd7111d4279f 1233 mcast_default_link = new;
daniele 10:dd7111d4279f 1234 pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
daniele 10:dd7111d4279f 1235 }
daniele 10:dd7111d4279f 1236 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
daniele 10:dd7111d4279f 1237 pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
daniele 10:dd7111d4279f 1238 } while(0);
daniele 10:dd7111d4279f 1239 #endif
daniele 10:dd7111d4279f 1240
daniele 10:dd7111d4279f 1241 network.addr = address.addr & netmask.addr;
daniele 10:dd7111d4279f 1242 gateway.addr = 0U;
daniele 10:dd7111d4279f 1243 pico_ipv4_route_add(network, netmask, gateway, 1, new);
daniele 10:dd7111d4279f 1244 pico_ipv4_to_string(ipstr, new->address.addr);
daniele 10:dd7111d4279f 1245 dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
daniele 10:dd7111d4279f 1246 return 0;
daniele 10:dd7111d4279f 1247 }
daniele 10:dd7111d4279f 1248
daniele 10:dd7111d4279f 1249
daniele 10:dd7111d4279f 1250 int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
daniele 10:dd7111d4279f 1251 {
daniele 10:dd7111d4279f 1252 struct pico_ipv4_link test, *found;
daniele 10:dd7111d4279f 1253 struct pico_ip4 network;
daniele 10:dd7111d4279f 1254
daniele 10:dd7111d4279f 1255 if(!dev) {
daniele 10:dd7111d4279f 1256 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1257 return -1;
daniele 10:dd7111d4279f 1258 }
daniele 10:dd7111d4279f 1259 test.address.addr = address.addr;
daniele 10:dd7111d4279f 1260 test.dev = dev;
daniele 10:dd7111d4279f 1261 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1262 if (!found) {
daniele 10:dd7111d4279f 1263 pico_err = PICO_ERR_ENXIO;
daniele 10:dd7111d4279f 1264 return -1;
daniele 10:dd7111d4279f 1265 }
daniele 10:dd7111d4279f 1266
daniele 10:dd7111d4279f 1267 network.addr = found->address.addr & found->netmask.addr;
daniele 10:dd7111d4279f 1268 pico_ipv4_route_del(network, found->netmask,pico_ipv4_route_get_gateway(&found->address), 1, found);
daniele 10:dd7111d4279f 1269 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1270 do {
daniele 10:dd7111d4279f 1271 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
daniele 10:dd7111d4279f 1272 struct pico_mcast_group *g = NULL;
daniele 10:dd7111d4279f 1273 struct pico_tree_node * index, * _tmp;
daniele 10:dd7111d4279f 1274 if (found == mcast_default_link) {
daniele 10:dd7111d4279f 1275 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
daniele 10:dd7111d4279f 1276 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
daniele 10:dd7111d4279f 1277 mcast_gw.addr = long_be(0x00000000);
daniele 10:dd7111d4279f 1278 mcast_default_link = NULL;
daniele 10:dd7111d4279f 1279 pico_ipv4_route_del(mcast_addr, mcast_nm, mcast_gw, 1, found);
daniele 10:dd7111d4279f 1280 }
daniele 10:dd7111d4279f 1281 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
daniele 10:dd7111d4279f 1282 pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
daniele 10:dd7111d4279f 1283 pico_tree_foreach_safe(index,found->MCASTGroups, _tmp)
daniele 10:dd7111d4279f 1284 {
daniele 10:dd7111d4279f 1285 g = index->keyValue;
daniele 10:dd7111d4279f 1286 pico_tree_delete(found->MCASTGroups, g);
daniele 10:dd7111d4279f 1287 pico_free(g);
daniele 10:dd7111d4279f 1288 }
daniele 10:dd7111d4279f 1289 } while(0);
daniele 10:dd7111d4279f 1290 #endif
daniele 10:dd7111d4279f 1291
daniele 10:dd7111d4279f 1292 pico_tree_delete(&Tree_dev_link, found);
daniele 10:dd7111d4279f 1293 /* XXX: pico_free(found); */
daniele 10:dd7111d4279f 1294 /* XXX: cleanup all routes containing the removed link */
daniele 10:dd7111d4279f 1295 return 0;
daniele 10:dd7111d4279f 1296 }
daniele 10:dd7111d4279f 1297
daniele 10:dd7111d4279f 1298
daniele 10:dd7111d4279f 1299 struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address)
daniele 10:dd7111d4279f 1300 {
daniele 10:dd7111d4279f 1301 struct pico_ipv4_link test = {0}, *found = NULL;
daniele 10:dd7111d4279f 1302 test.address.addr = address->addr;
daniele 10:dd7111d4279f 1303
daniele 10:dd7111d4279f 1304 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1305 if (!found)
daniele 10:dd7111d4279f 1306 return NULL;
daniele 10:dd7111d4279f 1307 else
daniele 10:dd7111d4279f 1308 return found;
daniele 10:dd7111d4279f 1309 }
daniele 10:dd7111d4279f 1310
daniele 10:dd7111d4279f 1311 struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev)
daniele 10:dd7111d4279f 1312 {
daniele 10:dd7111d4279f 1313 struct pico_tree_node *index = NULL;
daniele 10:dd7111d4279f 1314 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 1315
daniele 10:dd7111d4279f 1316 pico_tree_foreach(index, &Tree_dev_link)
daniele 10:dd7111d4279f 1317 {
daniele 10:dd7111d4279f 1318 link = index->keyValue;
daniele 10:dd7111d4279f 1319 if (link->dev == dev)
daniele 10:dd7111d4279f 1320 return link;
daniele 10:dd7111d4279f 1321 }
daniele 10:dd7111d4279f 1322 return NULL;
daniele 10:dd7111d4279f 1323 }
daniele 10:dd7111d4279f 1324
daniele 10:dd7111d4279f 1325
daniele 10:dd7111d4279f 1326 struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
daniele 10:dd7111d4279f 1327 {
daniele 10:dd7111d4279f 1328 struct pico_ipv4_link test, *found;
daniele 10:dd7111d4279f 1329 if(!address) {
daniele 10:dd7111d4279f 1330 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1331 return NULL;
daniele 10:dd7111d4279f 1332 }
daniele 10:dd7111d4279f 1333 test.dev = NULL;
daniele 10:dd7111d4279f 1334 test.address.addr = address->addr;
daniele 10:dd7111d4279f 1335 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1336 if (!found) {
daniele 10:dd7111d4279f 1337 pico_err = PICO_ERR_ENXIO;
daniele 10:dd7111d4279f 1338 return NULL;
daniele 10:dd7111d4279f 1339 }
daniele 10:dd7111d4279f 1340 return found->dev;
daniele 10:dd7111d4279f 1341 }
daniele 10:dd7111d4279f 1342
daniele 10:dd7111d4279f 1343 int pico_ipv4_rebound(struct pico_frame *f)
daniele 10:dd7111d4279f 1344 {
daniele 10:dd7111d4279f 1345 struct pico_ip4 dst;
daniele 10:dd7111d4279f 1346 struct pico_ipv4_hdr *hdr;
daniele 10:dd7111d4279f 1347 if(!f) {
daniele 10:dd7111d4279f 1348 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1349 return -1;
daniele 10:dd7111d4279f 1350 }
daniele 10:dd7111d4279f 1351
daniele 10:dd7111d4279f 1352 hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 1353 if (!hdr) {
daniele 10:dd7111d4279f 1354 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1355 return -1;
daniele 10:dd7111d4279f 1356 }
daniele 10:dd7111d4279f 1357 dst.addr = hdr->src.addr;
daniele 10:dd7111d4279f 1358 return pico_ipv4_frame_push(f, &dst, hdr->proto);
daniele 10:dd7111d4279f 1359 }
daniele 10:dd7111d4279f 1360
daniele 10:dd7111d4279f 1361 static int pico_ipv4_forward(struct pico_frame *f)
daniele 10:dd7111d4279f 1362 {
daniele 10:dd7111d4279f 1363 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
daniele 10:dd7111d4279f 1364 struct pico_ipv4_route *rt;
daniele 10:dd7111d4279f 1365 if (!hdr) {
daniele 10:dd7111d4279f 1366 return -1;
daniele 10:dd7111d4279f 1367 }
daniele 10:dd7111d4279f 1368
daniele 10:dd7111d4279f 1369 //dbg("IP> FORWARDING.\n");
daniele 10:dd7111d4279f 1370 rt = route_find(&hdr->dst);
daniele 10:dd7111d4279f 1371 if (!rt) {
daniele 10:dd7111d4279f 1372 pico_notify_dest_unreachable(f);
daniele 10:dd7111d4279f 1373 return -1;
daniele 10:dd7111d4279f 1374 }
daniele 10:dd7111d4279f 1375 //dbg("ROUTE: valid..\n");
daniele 10:dd7111d4279f 1376 f->dev = rt->link->dev;
daniele 10:dd7111d4279f 1377 hdr->ttl-=1;
daniele 10:dd7111d4279f 1378 if (hdr->ttl < 1) {
daniele 10:dd7111d4279f 1379 pico_notify_ttl_expired(f);
daniele 10:dd7111d4279f 1380 return -1;
daniele 10:dd7111d4279f 1381 }
daniele 10:dd7111d4279f 1382 hdr->crc++;
daniele 10:dd7111d4279f 1383
daniele 10:dd7111d4279f 1384 /* check if NAT enbled on link and do NAT if so */
daniele 10:dd7111d4279f 1385 if (pico_ipv4_nat_isenabled_out(rt->link) == 0)
daniele 10:dd7111d4279f 1386 pico_ipv4_nat(f, rt->link->address);
daniele 10:dd7111d4279f 1387
daniele 10:dd7111d4279f 1388 //dbg("Routing towards %s\n", f->dev->name);
daniele 10:dd7111d4279f 1389 f->start = f->net_hdr;
daniele 10:dd7111d4279f 1390 if(f->dev->eth != NULL)
daniele 10:dd7111d4279f 1391 f->len -= PICO_SIZE_ETHHDR;
daniele 10:dd7111d4279f 1392 pico_sendto_dev(f);
daniele 10:dd7111d4279f 1393 return 0;
daniele 10:dd7111d4279f 1394
daniele 10:dd7111d4279f 1395 }
daniele 10:dd7111d4279f 1396
daniele 10:dd7111d4279f 1397 int pico_ipv4_is_broadcast(uint32_t addr)
daniele 10:dd7111d4279f 1398 {
daniele 10:dd7111d4279f 1399 struct pico_ipv4_link *link;
daniele 10:dd7111d4279f 1400 struct pico_tree_node * index;
daniele 10:dd7111d4279f 1401 if (addr == PICO_IP4_ANY)
daniele 10:dd7111d4279f 1402 return 1;
daniele 10:dd7111d4279f 1403 if (addr == PICO_IP4_BCAST)
daniele 10:dd7111d4279f 1404 return 1;
daniele 10:dd7111d4279f 1405
daniele 10:dd7111d4279f 1406 pico_tree_foreach(index,&Tree_dev_link) {
daniele 10:dd7111d4279f 1407 link = index->keyValue;
daniele 10:dd7111d4279f 1408 if ((link->address.addr | (~link->netmask.addr)) == addr)
daniele 10:dd7111d4279f 1409 return 1;
daniele 10:dd7111d4279f 1410 }
daniele 10:dd7111d4279f 1411 return 0;
daniele 10:dd7111d4279f 1412 }
daniele 10:dd7111d4279f 1413
daniele 10:dd7111d4279f 1414 void pico_ipv4_unreachable(struct pico_frame *f, int err)
daniele 10:dd7111d4279f 1415 {
daniele 10:dd7111d4279f 1416 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 1417 #if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 1418 f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 1419 pico_transport_error(f, hdr->proto, err);
daniele 10:dd7111d4279f 1420 #endif
daniele 10:dd7111d4279f 1421 }
daniele 10:dd7111d4279f 1422
daniele 10:dd7111d4279f 1423 #endif