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@10:dd7111d4279f, 2013-06-06 (annotated)

Committer:
daniele
Date:
Thu Jun 06 00:38:54 2013 +0000
Revision:
10:dd7111d4279f
Child:
49:4b404dd2c97a
Update from masterbranch;

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 {
daniele 10:dd7111d4279f 522 /* Packet is not local. Try to forward. */
daniele 10:dd7111d4279f 523 if (pico_ipv4_forward(f) != 0) {
daniele 10:dd7111d4279f 524 pico_frame_discard(f);
daniele 10:dd7111d4279f 525 }
daniele 10:dd7111d4279f 526 }
daniele 10:dd7111d4279f 527 return 0;
daniele 10:dd7111d4279f 528 }
daniele 10:dd7111d4279f 529
daniele 10:dd7111d4279f 530 PICO_TREE_DECLARE(Routes, ipv4_route_compare);
daniele 10:dd7111d4279f 531
daniele 10:dd7111d4279f 532
daniele 10:dd7111d4279f 533 static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f)
daniele 10:dd7111d4279f 534 {
daniele 10:dd7111d4279f 535 f->start = (uint8_t*) f->net_hdr;
daniele 10:dd7111d4279f 536 #ifdef PICO_SUPPORT_IPFILTER
daniele 10:dd7111d4279f 537 if (ipfilter(f)) {
daniele 10:dd7111d4279f 538 /*pico_frame is discarded as result of the filtering*/
daniele 10:dd7111d4279f 539 return 0;
daniele 10:dd7111d4279f 540 }
daniele 10:dd7111d4279f 541 #endif
daniele 10:dd7111d4279f 542 return pico_sendto_dev(f);
daniele 10:dd7111d4279f 543 }
daniele 10:dd7111d4279f 544
daniele 10:dd7111d4279f 545
daniele 10:dd7111d4279f 546 static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, int size)
daniele 10:dd7111d4279f 547 {
daniele 10:dd7111d4279f 548 struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
daniele 10:dd7111d4279f 549 if (!f)
daniele 10:dd7111d4279f 550 return NULL;
daniele 10:dd7111d4279f 551 f->datalink_hdr = f->buffer;
daniele 10:dd7111d4279f 552 f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
daniele 10:dd7111d4279f 553 f->net_len = PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 554 f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 555 f->transport_len = size;
daniele 10:dd7111d4279f 556 f->len = size + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 557 return f;
daniele 10:dd7111d4279f 558 }
daniele 10:dd7111d4279f 559
daniele 10:dd7111d4279f 560 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f);
daniele 10:dd7111d4279f 561
daniele 10:dd7111d4279f 562 /* Interface: protocol definition */
daniele 10:dd7111d4279f 563 struct pico_protocol pico_proto_ipv4 = {
daniele 10:dd7111d4279f 564 .name = "ipv4",
daniele 10:dd7111d4279f 565 .proto_number = PICO_PROTO_IPV4,
daniele 10:dd7111d4279f 566 .layer = PICO_LAYER_NETWORK,
daniele 10:dd7111d4279f 567 .alloc = pico_ipv4_alloc,
daniele 10:dd7111d4279f 568 .process_in = pico_ipv4_process_in,
daniele 10:dd7111d4279f 569 .process_out = pico_ipv4_process_out,
daniele 10:dd7111d4279f 570 .push = pico_ipv4_frame_sock_push,
daniele 10:dd7111d4279f 571 .q_in = &in,
daniele 10:dd7111d4279f 572 .q_out = &out,
daniele 10:dd7111d4279f 573 };
daniele 10:dd7111d4279f 574
daniele 10:dd7111d4279f 575 struct pico_ipv4_route
daniele 10:dd7111d4279f 576 {
daniele 10:dd7111d4279f 577 struct pico_ip4 dest;
daniele 10:dd7111d4279f 578 struct pico_ip4 netmask;
daniele 10:dd7111d4279f 579 struct pico_ip4 gateway;
daniele 10:dd7111d4279f 580 struct pico_ipv4_link *link;
daniele 10:dd7111d4279f 581 uint32_t metric;
daniele 10:dd7111d4279f 582 };
daniele 10:dd7111d4279f 583
daniele 10:dd7111d4279f 584
daniele 10:dd7111d4279f 585 static int ipv4_route_compare(void *ka, void * kb)
daniele 10:dd7111d4279f 586 {
daniele 10:dd7111d4279f 587 struct pico_ipv4_route *a = ka, *b = kb;
daniele 10:dd7111d4279f 588
daniele 10:dd7111d4279f 589 /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
daniele 10:dd7111d4279f 590 if (long_be(a->netmask.addr) < long_be(b->netmask.addr))
daniele 10:dd7111d4279f 591 return -1;
daniele 10:dd7111d4279f 592
daniele 10:dd7111d4279f 593 if (long_be(a->netmask.addr) > long_be(b->netmask.addr))
daniele 10:dd7111d4279f 594 return 1;
daniele 10:dd7111d4279f 595
daniele 10:dd7111d4279f 596 if (a->dest.addr < b->dest.addr)
daniele 10:dd7111d4279f 597 return -1;
daniele 10:dd7111d4279f 598
daniele 10:dd7111d4279f 599 if (a->dest.addr > b->dest.addr)
daniele 10:dd7111d4279f 600 return 1;
daniele 10:dd7111d4279f 601
daniele 10:dd7111d4279f 602 if (a->metric < b->metric)
daniele 10:dd7111d4279f 603 return -1;
daniele 10:dd7111d4279f 604
daniele 10:dd7111d4279f 605 if (a->metric > b->metric)
daniele 10:dd7111d4279f 606 return 1;
daniele 10:dd7111d4279f 607
daniele 10:dd7111d4279f 608 return 0;
daniele 10:dd7111d4279f 609 }
daniele 10:dd7111d4279f 610
daniele 10:dd7111d4279f 611 static struct pico_ipv4_route *route_find(struct pico_ip4 *addr)
daniele 10:dd7111d4279f 612 {
daniele 10:dd7111d4279f 613 struct pico_ipv4_route *r;
daniele 10:dd7111d4279f 614 struct pico_tree_node * index;
daniele 10:dd7111d4279f 615
daniele 10:dd7111d4279f 616 if(addr->addr != PICO_IP4_BCAST)
daniele 10:dd7111d4279f 617 {
daniele 10:dd7111d4279f 618 pico_tree_foreach_reverse(index, &Routes) {
daniele 10:dd7111d4279f 619 r = index->keyValue;
daniele 10:dd7111d4279f 620 if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
daniele 10:dd7111d4279f 621 return r;
daniele 10:dd7111d4279f 622 }
daniele 10:dd7111d4279f 623 }
daniele 10:dd7111d4279f 624 }
daniele 10:dd7111d4279f 625 else
daniele 10:dd7111d4279f 626 {
daniele 10:dd7111d4279f 627 r = pico_tree_first(&Routes);
daniele 10:dd7111d4279f 628 if(!r->netmask.addr)
daniele 10:dd7111d4279f 629 {
daniele 10:dd7111d4279f 630 return r;
daniele 10:dd7111d4279f 631 }
daniele 10:dd7111d4279f 632 else
daniele 10:dd7111d4279f 633 {
daniele 10:dd7111d4279f 634 dbg("WARNING: no default route for a global broadcast found\n");
daniele 10:dd7111d4279f 635 }
daniele 10:dd7111d4279f 636 }
daniele 10:dd7111d4279f 637
daniele 10:dd7111d4279f 638 return NULL;
daniele 10:dd7111d4279f 639 }
daniele 10:dd7111d4279f 640
daniele 10:dd7111d4279f 641 struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
daniele 10:dd7111d4279f 642 {
daniele 10:dd7111d4279f 643 struct pico_ip4 nullip;
daniele 10:dd7111d4279f 644 struct pico_ipv4_route *route;
daniele 10:dd7111d4279f 645 nullip.addr = 0U;
daniele 10:dd7111d4279f 646
daniele 10:dd7111d4279f 647 if(!addr) {
daniele 10:dd7111d4279f 648 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 649 return nullip;
daniele 10:dd7111d4279f 650 }
daniele 10:dd7111d4279f 651
daniele 10:dd7111d4279f 652 route = route_find(addr);
daniele 10:dd7111d4279f 653 if (!route) {
daniele 10:dd7111d4279f 654 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 655 return nullip;
daniele 10:dd7111d4279f 656 }
daniele 10:dd7111d4279f 657 else
daniele 10:dd7111d4279f 658 return route->gateway;
daniele 10:dd7111d4279f 659 }
daniele 10:dd7111d4279f 660
daniele 10:dd7111d4279f 661 struct pico_ip4 *pico_ipv4_source_find(struct pico_ip4 *dst)
daniele 10:dd7111d4279f 662 {
daniele 10:dd7111d4279f 663 struct pico_ip4 *myself = NULL;
daniele 10:dd7111d4279f 664 struct pico_ipv4_route *rt;
daniele 10:dd7111d4279f 665
daniele 10:dd7111d4279f 666 if(!dst) {
daniele 10:dd7111d4279f 667 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 668 return NULL;
daniele 10:dd7111d4279f 669 }
daniele 10:dd7111d4279f 670
daniele 10:dd7111d4279f 671 rt = route_find(dst);
daniele 10:dd7111d4279f 672 if (rt) {
daniele 10:dd7111d4279f 673 myself = &rt->link->address;
daniele 10:dd7111d4279f 674 } else
daniele 10:dd7111d4279f 675 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 676 return myself;
daniele 10:dd7111d4279f 677 }
daniele 10:dd7111d4279f 678
daniele 10:dd7111d4279f 679
daniele 10:dd7111d4279f 680 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 681 /* link
daniele 10:dd7111d4279f 682 * |
daniele 10:dd7111d4279f 683 * MCASTGroups
daniele 10:dd7111d4279f 684 * | | |
daniele 10:dd7111d4279f 685 * ------------ | ------------
daniele 10:dd7111d4279f 686 * | | |
daniele 10:dd7111d4279f 687 * MCASTSources MCASTSources MCASTSources
daniele 10:dd7111d4279f 688 * | | | | | | | | | | | |
daniele 10:dd7111d4279f 689 * S S S S S S S S S S S S
daniele 10:dd7111d4279f 690 *
daniele 10:dd7111d4279f 691 * MCASTGroups: RBTree(mcast_group)
daniele 10:dd7111d4279f 692 * MCASTSources: RBTree(source)
daniele 10:dd7111d4279f 693 */
daniele 10:dd7111d4279f 694 static int ipv4_mcast_groups_cmp(void * ka, void * kb)
daniele 10:dd7111d4279f 695 {
daniele 10:dd7111d4279f 696 struct pico_mcast_group *a = ka, *b = kb;
daniele 10:dd7111d4279f 697 if (a->mcast_addr.addr < b->mcast_addr.addr) {
daniele 10:dd7111d4279f 698 return -1;
daniele 10:dd7111d4279f 699 } else if (a->mcast_addr.addr > b->mcast_addr.addr) {
daniele 10:dd7111d4279f 700 return 1;
daniele 10:dd7111d4279f 701 } else {
daniele 10:dd7111d4279f 702 return 0;
daniele 10:dd7111d4279f 703 }
daniele 10:dd7111d4279f 704 }
daniele 10:dd7111d4279f 705
daniele 10:dd7111d4279f 706 static int ipv4_mcast_sources_cmp(void *ka, void *kb)
daniele 10:dd7111d4279f 707 {
daniele 10:dd7111d4279f 708 struct pico_ip4 *a = ka, *b = kb;
daniele 10:dd7111d4279f 709 if (a->addr < b->addr)
daniele 10:dd7111d4279f 710 return -1;
daniele 10:dd7111d4279f 711 if (a->addr > b->addr)
daniele 10:dd7111d4279f 712 return 1;
daniele 10:dd7111d4279f 713 return 0;
daniele 10:dd7111d4279f 714 }
daniele 10:dd7111d4279f 715
daniele 10:dd7111d4279f 716 static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
daniele 10:dd7111d4279f 717 {
daniele 10:dd7111d4279f 718 uint16_t i = 0;
daniele 10:dd7111d4279f 719 struct pico_mcast_group __attribute__ ((unused)) *g = NULL;
daniele 10:dd7111d4279f 720 struct pico_ip4 __attribute__ ((unused)) *source = NULL;
daniele 10:dd7111d4279f 721 struct pico_tree_node *index = NULL, *index2 = NULL;
daniele 10:dd7111d4279f 722
daniele 10:dd7111d4279f 723 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
daniele 10:dd7111d4279f 724 ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name);
daniele 10:dd7111d4279f 725 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
daniele 10:dd7111d4279f 726 ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n");
daniele 10:dd7111d4279f 727 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
daniele 10:dd7111d4279f 728
daniele 10:dd7111d4279f 729 pico_tree_foreach(index, mcast_link->MCASTGroups)
daniele 10:dd7111d4279f 730 {
daniele 10:dd7111d4279f 731 g = index->keyValue;
daniele 10:dd7111d4279f 732 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 733 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 734 {
daniele 10:dd7111d4279f 735 source = index2->keyValue;
daniele 10:dd7111d4279f 736 ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr);
daniele 10:dd7111d4279f 737 }
daniele 10:dd7111d4279f 738 i++;
daniele 10:dd7111d4279f 739 }
daniele 10:dd7111d4279f 740 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
daniele 10:dd7111d4279f 741 }
daniele 10:dd7111d4279f 742
daniele 10:dd7111d4279f 743 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 744 {
daniele 10:dd7111d4279f 745 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 746 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 747 struct pico_tree_node *index = NULL, *_tmp = NULL;
daniele 10:dd7111d4279f 748 struct pico_ip4 *source = NULL;
daniele 10:dd7111d4279f 749
daniele 10:dd7111d4279f 750 if (mcast_link)
daniele 10:dd7111d4279f 751 link = pico_ipv4_link_get(mcast_link);
daniele 10:dd7111d4279f 752 else
daniele 10:dd7111d4279f 753 link = mcast_default_link;
daniele 10:dd7111d4279f 754
daniele 10:dd7111d4279f 755 test.mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 756 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 757 if (g) {
daniele 10:dd7111d4279f 758 if (reference_count)
daniele 10:dd7111d4279f 759 g->reference_count++;
daniele 10:dd7111d4279f 760 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
daniele 10:dd7111d4279f 761 } else {
daniele 10:dd7111d4279f 762 g = pico_zalloc(sizeof(struct pico_mcast_group));
daniele 10:dd7111d4279f 763 if (!g) {
daniele 10:dd7111d4279f 764 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 765 return -1;
daniele 10:dd7111d4279f 766 }
daniele 10:dd7111d4279f 767 /* "non-existent" state of filter mode INCLUDE and empty source list */
daniele 10:dd7111d4279f 768 g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
daniele 10:dd7111d4279f 769 g->reference_count = 1;
daniele 10:dd7111d4279f 770 g->mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 771 g->MCASTSources.root = &LEAF;
daniele 10:dd7111d4279f 772 g->MCASTSources.compare = ipv4_mcast_sources_cmp;
daniele 10:dd7111d4279f 773 pico_tree_insert(link->MCASTGroups, g);
daniele 10:dd7111d4279f 774 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
daniele 10:dd7111d4279f 775 }
daniele 10:dd7111d4279f 776
daniele 10:dd7111d4279f 777 /* cleanup filter */
daniele 10:dd7111d4279f 778 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 779 {
daniele 10:dd7111d4279f 780 source = index->keyValue;
daniele 10:dd7111d4279f 781 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 782 pico_free(source);
daniele 10:dd7111d4279f 783 }
daniele 10:dd7111d4279f 784 /* insert new filter */
daniele 10:dd7111d4279f 785 if (MCASTFilter) {
daniele 10:dd7111d4279f 786 pico_tree_foreach(index, MCASTFilter)
daniele 10:dd7111d4279f 787 {
daniele 10:dd7111d4279f 788 source = pico_zalloc(sizeof(struct pico_ip4));
daniele 10:dd7111d4279f 789 if (!source) {
daniele 10:dd7111d4279f 790 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 791 return -1;
daniele 10:dd7111d4279f 792 }
daniele 10:dd7111d4279f 793 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
daniele 10:dd7111d4279f 794 pico_tree_insert(&g->MCASTSources, source);
daniele 10:dd7111d4279f 795 }
daniele 10:dd7111d4279f 796 }
daniele 10:dd7111d4279f 797 g->filter_mode = filter_mode;
daniele 10:dd7111d4279f 798
daniele 10:dd7111d4279f 799 pico_ipv4_mcast_print_groups(link);
daniele 10:dd7111d4279f 800 return 0;
daniele 10:dd7111d4279f 801 }
daniele 10:dd7111d4279f 802
daniele 10:dd7111d4279f 803 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 804 {
daniele 10:dd7111d4279f 805
daniele 10:dd7111d4279f 806 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 807 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 808 struct pico_tree_node *index = NULL, *_tmp = NULL;
daniele 10:dd7111d4279f 809 struct pico_ip4 *source = NULL;
daniele 10:dd7111d4279f 810
daniele 10:dd7111d4279f 811 if (mcast_link)
daniele 10:dd7111d4279f 812 link = pico_ipv4_link_get(mcast_link);
daniele 10:dd7111d4279f 813 else
daniele 10:dd7111d4279f 814 link = mcast_default_link;
daniele 10:dd7111d4279f 815
daniele 10:dd7111d4279f 816 test.mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 817 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 818 if (!g) {
daniele 10:dd7111d4279f 819 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 820 return -1;
daniele 10:dd7111d4279f 821 } else {
daniele 10:dd7111d4279f 822 if (reference_count && (--(g->reference_count) < 1)) {
daniele 10:dd7111d4279f 823 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
daniele 10:dd7111d4279f 824 /* cleanup filter */
daniele 10:dd7111d4279f 825 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 826 {
daniele 10:dd7111d4279f 827 source = index->keyValue;
daniele 10:dd7111d4279f 828 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 829 pico_free(source);
daniele 10:dd7111d4279f 830 }
daniele 10:dd7111d4279f 831 pico_tree_delete(link->MCASTGroups, g);
daniele 10:dd7111d4279f 832 pico_free(g);
daniele 10:dd7111d4279f 833 } else {
daniele 10:dd7111d4279f 834 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
daniele 10:dd7111d4279f 835 /* cleanup filter */
daniele 10:dd7111d4279f 836 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 837 {
daniele 10:dd7111d4279f 838 source = index->keyValue;
daniele 10:dd7111d4279f 839 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 840 pico_free(source);
daniele 10:dd7111d4279f 841 }
daniele 10:dd7111d4279f 842 /* insert new filter */
daniele 10:dd7111d4279f 843 if (MCASTFilter) {
daniele 10:dd7111d4279f 844 pico_tree_foreach(index, MCASTFilter)
daniele 10:dd7111d4279f 845 {
daniele 10:dd7111d4279f 846 source = pico_zalloc(sizeof(struct pico_ip4));
daniele 10:dd7111d4279f 847 if (!source) {
daniele 10:dd7111d4279f 848 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 849 return -1;
daniele 10:dd7111d4279f 850 }
daniele 10:dd7111d4279f 851 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
daniele 10:dd7111d4279f 852 pico_tree_insert(&g->MCASTSources, source);
daniele 10:dd7111d4279f 853 }
daniele 10:dd7111d4279f 854 }
daniele 10:dd7111d4279f 855 g->filter_mode = filter_mode;
daniele 10:dd7111d4279f 856 }
daniele 10:dd7111d4279f 857 }
daniele 10:dd7111d4279f 858
daniele 10:dd7111d4279f 859 pico_ipv4_mcast_print_groups(link);
daniele 10:dd7111d4279f 860 return 0;
daniele 10:dd7111d4279f 861 }
daniele 10:dd7111d4279f 862
daniele 10:dd7111d4279f 863 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
daniele 10:dd7111d4279f 864 {
daniele 10:dd7111d4279f 865 return mcast_default_link;
daniele 10:dd7111d4279f 866 }
daniele 10:dd7111d4279f 867
daniele 10:dd7111d4279f 868 static int pico_ipv4_mcast_filter(struct pico_frame *f)
daniele 10:dd7111d4279f 869 {
daniele 10:dd7111d4279f 870 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 871 struct pico_tree_node *index = NULL, *index2 = NULL;
daniele 10:dd7111d4279f 872 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 873 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 874
daniele 10:dd7111d4279f 875 test.mcast_addr = hdr->dst;
daniele 10:dd7111d4279f 876
daniele 10:dd7111d4279f 877 pico_tree_foreach(index, &Tree_dev_link)
daniele 10:dd7111d4279f 878 {
daniele 10:dd7111d4279f 879 link = index->keyValue;
daniele 10:dd7111d4279f 880 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 881 if (g) {
daniele 10:dd7111d4279f 882 if (f->dev == link->dev) {
daniele 10:dd7111d4279f 883 ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
daniele 10:dd7111d4279f 884 /* perform source filtering */
daniele 10:dd7111d4279f 885 switch (g->filter_mode)
daniele 10:dd7111d4279f 886 {
daniele 10:dd7111d4279f 887 case PICO_IP_MULTICAST_INCLUDE:
daniele 10:dd7111d4279f 888 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 889 {
daniele 10:dd7111d4279f 890 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
daniele 10:dd7111d4279f 891 ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 892 return 0;
daniele 10:dd7111d4279f 893 }
daniele 10:dd7111d4279f 894 }
daniele 10:dd7111d4279f 895 ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 896 return -1;
daniele 10:dd7111d4279f 897 break;
daniele 10:dd7111d4279f 898
daniele 10:dd7111d4279f 899 case PICO_IP_MULTICAST_EXCLUDE:
daniele 10:dd7111d4279f 900 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 901 {
daniele 10:dd7111d4279f 902 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
daniele 10:dd7111d4279f 903 ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 904 return -1;
daniele 10:dd7111d4279f 905 }
daniele 10:dd7111d4279f 906 }
daniele 10:dd7111d4279f 907 ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 908 return 0;
daniele 10:dd7111d4279f 909 break;
daniele 10:dd7111d4279f 910
daniele 10:dd7111d4279f 911 default:
daniele 10:dd7111d4279f 912 return -1;
daniele 10:dd7111d4279f 913 break;
daniele 10:dd7111d4279f 914 }
daniele 10:dd7111d4279f 915 } else {
daniele 10:dd7111d4279f 916 ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name);
daniele 10:dd7111d4279f 917 }
daniele 10:dd7111d4279f 918 } else {
daniele 10:dd7111d4279f 919 ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name);
daniele 10:dd7111d4279f 920 }
daniele 10:dd7111d4279f 921 }
daniele 10:dd7111d4279f 922 return -1;
daniele 10:dd7111d4279f 923 }
daniele 10:dd7111d4279f 924
daniele 10:dd7111d4279f 925 #else
daniele 10:dd7111d4279f 926
daniele 10:dd7111d4279f 927 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 928 {
daniele 10:dd7111d4279f 929 pico_err = PICO_ERR_EPROTONOSUPPORT;
daniele 10:dd7111d4279f 930 return -1;
daniele 10:dd7111d4279f 931 }
daniele 10:dd7111d4279f 932 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 933 {
daniele 10:dd7111d4279f 934 pico_err = PICO_ERR_EPROTONOSUPPORT;
daniele 10:dd7111d4279f 935 return -1;
daniele 10:dd7111d4279f 936 }
daniele 10:dd7111d4279f 937 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
daniele 10:dd7111d4279f 938 {
daniele 10:dd7111d4279f 939 pico_err = PICO_ERR_EPROTONOSUPPORT;
daniele 10:dd7111d4279f 940 return NULL;
daniele 10:dd7111d4279f 941 }
daniele 10:dd7111d4279f 942 #endif /* PICO_SUPPORT_MCAST */
daniele 10:dd7111d4279f 943
daniele 10:dd7111d4279f 944 int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto)
daniele 10:dd7111d4279f 945 {
daniele 10:dd7111d4279f 946
daniele 10:dd7111d4279f 947 struct pico_ipv4_route *route;
daniele 10:dd7111d4279f 948 struct pico_ipv4_link *link;
daniele 10:dd7111d4279f 949 struct pico_ipv4_hdr *hdr;
daniele 10:dd7111d4279f 950 uint8_t ttl = PICO_IPV4_DEFAULT_TTL;
daniele 10:dd7111d4279f 951 uint8_t vhl = 0x45; /* version 4, header length 20 */
daniele 10:dd7111d4279f 952 static uint16_t ipv4_progressive_id = 0x91c0;
daniele 10:dd7111d4279f 953 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 954 struct pico_tree_node *index;
daniele 10:dd7111d4279f 955 #endif
daniele 10:dd7111d4279f 956
daniele 10:dd7111d4279f 957 if(!f || !dst) {
daniele 10:dd7111d4279f 958 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 959 return -1;
daniele 10:dd7111d4279f 960 }
daniele 10:dd7111d4279f 961 hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 962 if (!hdr) {
daniele 10:dd7111d4279f 963 dbg("IP header error\n");
daniele 10:dd7111d4279f 964 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 965 goto drop;
daniele 10:dd7111d4279f 966 }
daniele 10:dd7111d4279f 967
daniele 10:dd7111d4279f 968 if (dst->addr == 0) {
daniele 10:dd7111d4279f 969 dbg("IP destination addr error\n");
daniele 10:dd7111d4279f 970 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 971 goto drop;
daniele 10:dd7111d4279f 972 }
daniele 10:dd7111d4279f 973
daniele 10:dd7111d4279f 974 route = route_find(dst);
daniele 10:dd7111d4279f 975 if (!route) {
daniele 10:dd7111d4279f 976 dbg("Route to %08x not found.\n", long_be(dst->addr));
daniele 10:dd7111d4279f 977 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 978 goto drop;
daniele 10:dd7111d4279f 979 } else {
daniele 10:dd7111d4279f 980 link = route->link;
daniele 10:dd7111d4279f 981 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 982 if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */
daniele 10:dd7111d4279f 983 switch (proto) {
daniele 10:dd7111d4279f 984 case PICO_PROTO_UDP:
daniele 10:dd7111d4279f 985 if(pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
daniele 10:dd7111d4279f 986 ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
daniele 10:dd7111d4279f 987 break;
daniele 10:dd7111d4279f 988 case PICO_PROTO_IGMP:
daniele 10:dd7111d4279f 989 vhl = 0x46; /* header length 24 */
daniele 10:dd7111d4279f 990 ttl = 1;
daniele 10:dd7111d4279f 991 /* router alert (RFC 2113) */
daniele 10:dd7111d4279f 992 hdr->options[0] = 0x94;
daniele 10:dd7111d4279f 993 hdr->options[1] = 0x04;
daniele 10:dd7111d4279f 994 hdr->options[2] = 0x00;
daniele 10:dd7111d4279f 995 hdr->options[3] = 0x00;
daniele 10:dd7111d4279f 996 if (f->dev && link->dev != f->dev) { /* default link is not requested link */
daniele 10:dd7111d4279f 997 pico_tree_foreach(index, &Tree_dev_link) {
daniele 10:dd7111d4279f 998 link = index->keyValue;
daniele 10:dd7111d4279f 999 if (link->dev == f->dev)
daniele 10:dd7111d4279f 1000 break;
daniele 10:dd7111d4279f 1001 }
daniele 10:dd7111d4279f 1002 }
daniele 10:dd7111d4279f 1003 break;
daniele 10:dd7111d4279f 1004 default:
daniele 10:dd7111d4279f 1005 ttl = PICO_IPV4_DEFAULT_TTL;
daniele 10:dd7111d4279f 1006 }
daniele 10:dd7111d4279f 1007 }
daniele 10:dd7111d4279f 1008 #endif
daniele 10:dd7111d4279f 1009 }
daniele 10:dd7111d4279f 1010
daniele 10:dd7111d4279f 1011 hdr->vhl = vhl;
daniele 10:dd7111d4279f 1012 hdr->len = short_be(f->transport_len + f->net_len);
daniele 10:dd7111d4279f 1013 if (f->transport_hdr != f->payload)
daniele 10:dd7111d4279f 1014 ipv4_progressive_id++;
daniele 10:dd7111d4279f 1015 hdr->id = short_be(ipv4_progressive_id);
daniele 10:dd7111d4279f 1016 hdr->dst.addr = dst->addr;
daniele 10:dd7111d4279f 1017 hdr->src.addr = link->address.addr;
daniele 10:dd7111d4279f 1018 hdr->ttl = ttl;
daniele 10:dd7111d4279f 1019 hdr->proto = proto;
daniele 10:dd7111d4279f 1020 hdr->frag = short_be(PICO_IPV4_DONTFRAG);
daniele 10:dd7111d4279f 1021 #ifdef PICO_SUPPORT_IPFRAG
daniele 10:dd7111d4279f 1022 # ifdef PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 1023 if (proto == PICO_PROTO_UDP) {
daniele 10:dd7111d4279f 1024 /* first fragment, can not use transport_len to calculate IP length */
daniele 10:dd7111d4279f 1025 if (f->transport_hdr != f->payload)
daniele 10:dd7111d4279f 1026 hdr->len = short_be(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len);
daniele 10:dd7111d4279f 1027 /* set fragmentation flags and offset calculated in socket layer */
daniele 10:dd7111d4279f 1028 hdr->frag = f->frag;
daniele 10:dd7111d4279f 1029 }
daniele 10:dd7111d4279f 1030 # endif /* PICO_SUPPORT_UDP */
daniele 10:dd7111d4279f 1031 #endif /* PICO_SUPPORT_IPFRAG */
daniele 10:dd7111d4279f 1032 pico_ipv4_checksum(f);
daniele 10:dd7111d4279f 1033
daniele 10:dd7111d4279f 1034 if (f->sock && f->sock->dev){
daniele 10:dd7111d4279f 1035 //if the socket has its device set, use that (currently used for DHCP)
daniele 10:dd7111d4279f 1036 f->dev = f->sock->dev;
daniele 10:dd7111d4279f 1037 } else {
daniele 10:dd7111d4279f 1038 f->dev = link->dev;
daniele 10:dd7111d4279f 1039 }
daniele 10:dd7111d4279f 1040
daniele 10:dd7111d4279f 1041 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1042 if (pico_ipv4_is_multicast(hdr->dst.addr)) {
daniele 10:dd7111d4279f 1043 struct pico_frame *cpy;
daniele 10:dd7111d4279f 1044 /* Sending UDP multicast datagram, am I member? If so, loopback copy */
daniele 10:dd7111d4279f 1045 if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) {
daniele 10:dd7111d4279f 1046 ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n");
daniele 10:dd7111d4279f 1047 cpy = pico_frame_copy(f);
daniele 10:dd7111d4279f 1048 pico_enqueue(&in, cpy);
daniele 10:dd7111d4279f 1049 }
daniele 10:dd7111d4279f 1050 }
daniele 10:dd7111d4279f 1051 #endif
daniele 10:dd7111d4279f 1052
daniele 10:dd7111d4279f 1053 if(pico_ipv4_link_get(&hdr->dst)){
daniele 10:dd7111d4279f 1054 //it's our own IP
daniele 10:dd7111d4279f 1055 return pico_enqueue(&in, f);
daniele 10:dd7111d4279f 1056 }else{
daniele 10:dd7111d4279f 1057 /* TODO: Check if there are members subscribed here */
daniele 10:dd7111d4279f 1058 return pico_enqueue(&out, f);
daniele 10:dd7111d4279f 1059 }
daniele 10:dd7111d4279f 1060
daniele 10:dd7111d4279f 1061 drop:
daniele 10:dd7111d4279f 1062 pico_frame_discard(f);
daniele 10:dd7111d4279f 1063 return -1;
daniele 10:dd7111d4279f 1064 }
daniele 10:dd7111d4279f 1065
daniele 10:dd7111d4279f 1066
daniele 10:dd7111d4279f 1067 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f)
daniele 10:dd7111d4279f 1068 {
daniele 10:dd7111d4279f 1069 struct pico_ip4 *dst;
daniele 10:dd7111d4279f 1070 struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
daniele 10:dd7111d4279f 1071 if (!f->sock) {
daniele 10:dd7111d4279f 1072 pico_frame_discard(f);
daniele 10:dd7111d4279f 1073 return -1;
daniele 10:dd7111d4279f 1074 }
daniele 10:dd7111d4279f 1075
daniele 10:dd7111d4279f 1076 if (remote_duple) {
daniele 10:dd7111d4279f 1077 dst = &remote_duple->remote_addr.ip4;
daniele 10:dd7111d4279f 1078 } else {
daniele 10:dd7111d4279f 1079 dst = &f->sock->remote_addr.ip4;
daniele 10:dd7111d4279f 1080 }
daniele 10:dd7111d4279f 1081
daniele 10:dd7111d4279f 1082 return pico_ipv4_frame_push(f, dst, f->sock->proto->proto_number);
daniele 10:dd7111d4279f 1083 }
daniele 10:dd7111d4279f 1084
daniele 10:dd7111d4279f 1085
daniele 10:dd7111d4279f 1086 #ifdef DEBUG_ROUTE
daniele 10:dd7111d4279f 1087 static void dbg_route(void)
daniele 10:dd7111d4279f 1088 {
daniele 10:dd7111d4279f 1089 struct pico_ipv4_route *r;
daniele 10:dd7111d4279f 1090 struct pico_tree_node * index;
daniele 10:dd7111d4279f 1091 pico_tree_foreach(index,&Routes){
daniele 10:dd7111d4279f 1092 r = index->keyValue;
daniele 10:dd7111d4279f 1093 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 1094 }
daniele 10:dd7111d4279f 1095 }
daniele 10:dd7111d4279f 1096 #else
daniele 10:dd7111d4279f 1097 #define dbg_route() do{ }while(0)
daniele 10:dd7111d4279f 1098 #endif
daniele 10:dd7111d4279f 1099
daniele 10:dd7111d4279f 1100 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 1101 {
daniele 10:dd7111d4279f 1102 struct pico_ipv4_route test, *new;
daniele 10:dd7111d4279f 1103 test.dest.addr = address.addr;
daniele 10:dd7111d4279f 1104 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1105 test.metric = metric;
daniele 10:dd7111d4279f 1106
daniele 10:dd7111d4279f 1107 if(pico_tree_findKey(&Routes,&test)){
daniele 10:dd7111d4279f 1108 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1109 return -1;
daniele 10:dd7111d4279f 1110 }
daniele 10:dd7111d4279f 1111
daniele 10:dd7111d4279f 1112 new = pico_zalloc(sizeof(struct pico_ipv4_route));
daniele 10:dd7111d4279f 1113 if (!new) {
daniele 10:dd7111d4279f 1114 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1115 return -1;
daniele 10:dd7111d4279f 1116 }
daniele 10:dd7111d4279f 1117 new->dest.addr = address.addr;
daniele 10:dd7111d4279f 1118 new->netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1119 new->gateway.addr = gateway.addr;
daniele 10:dd7111d4279f 1120 new->metric = metric;
daniele 10:dd7111d4279f 1121 if (gateway.addr == 0) {
daniele 10:dd7111d4279f 1122 /* No gateway provided, use the link */
daniele 10:dd7111d4279f 1123 new->link = link;
daniele 10:dd7111d4279f 1124 } else {
daniele 10:dd7111d4279f 1125 struct pico_ipv4_route *r = route_find(&gateway);
daniele 10:dd7111d4279f 1126 if (!r ) { /* Specified Gateway is unreachable */
daniele 10:dd7111d4279f 1127 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 1128 pico_free(new);
daniele 10:dd7111d4279f 1129 return -1;
daniele 10:dd7111d4279f 1130 }
daniele 10:dd7111d4279f 1131 if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
daniele 10:dd7111d4279f 1132 pico_err = PICO_ERR_ENETUNREACH;
daniele 10:dd7111d4279f 1133 pico_free(new);
daniele 10:dd7111d4279f 1134 return -1;
daniele 10:dd7111d4279f 1135 }
daniele 10:dd7111d4279f 1136 new->link = r->link;
daniele 10:dd7111d4279f 1137 }
daniele 10:dd7111d4279f 1138 if (!new->link) {
daniele 10:dd7111d4279f 1139 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1140 pico_free(new);
daniele 10:dd7111d4279f 1141 return -1;
daniele 10:dd7111d4279f 1142 }
daniele 10:dd7111d4279f 1143
daniele 10:dd7111d4279f 1144 pico_tree_insert(&Routes,new);
daniele 10:dd7111d4279f 1145 dbg_route();
daniele 10:dd7111d4279f 1146 return 0;
daniele 10:dd7111d4279f 1147 }
daniele 10:dd7111d4279f 1148
daniele 10:dd7111d4279f 1149 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 1150 {
daniele 10:dd7111d4279f 1151 struct pico_ipv4_route test, *found;
daniele 10:dd7111d4279f 1152 if (!link) {
daniele 10:dd7111d4279f 1153 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1154 return -1;
daniele 10:dd7111d4279f 1155 }
daniele 10:dd7111d4279f 1156 test.dest.addr = address.addr;
daniele 10:dd7111d4279f 1157 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1158 test.metric = metric;
daniele 10:dd7111d4279f 1159
daniele 10:dd7111d4279f 1160 found = pico_tree_findKey(&Routes,&test);
daniele 10:dd7111d4279f 1161 if (found) {
daniele 10:dd7111d4279f 1162
daniele 10:dd7111d4279f 1163 pico_tree_delete(&Routes,found);
daniele 10:dd7111d4279f 1164 pico_free(found);
daniele 10:dd7111d4279f 1165
daniele 10:dd7111d4279f 1166 dbg_route();
daniele 10:dd7111d4279f 1167 return 0;
daniele 10:dd7111d4279f 1168 }
daniele 10:dd7111d4279f 1169 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1170 return -1;
daniele 10:dd7111d4279f 1171 }
daniele 10:dd7111d4279f 1172
daniele 10:dd7111d4279f 1173
daniele 10:dd7111d4279f 1174 int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
daniele 10:dd7111d4279f 1175 {
daniele 10:dd7111d4279f 1176 struct pico_ipv4_link test, *new;
daniele 10:dd7111d4279f 1177 struct pico_ip4 network, gateway;
daniele 10:dd7111d4279f 1178 char ipstr[30];
daniele 10:dd7111d4279f 1179
daniele 10:dd7111d4279f 1180 if(!dev) {
daniele 10:dd7111d4279f 1181 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1182 return -1;
daniele 10:dd7111d4279f 1183 }
daniele 10:dd7111d4279f 1184 test.address.addr = address.addr;
daniele 10:dd7111d4279f 1185 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1186 test.dev = dev;
daniele 10:dd7111d4279f 1187 /** XXX: Valid netmask / unicast address test **/
daniele 10:dd7111d4279f 1188
daniele 10:dd7111d4279f 1189 if(pico_tree_findKey(&Tree_dev_link, &test)) {
daniele 10:dd7111d4279f 1190 dbg("IPv4: Trying to assign an invalid address (in use)\n");
daniele 10:dd7111d4279f 1191 pico_err = PICO_ERR_EADDRINUSE;
daniele 10:dd7111d4279f 1192 return -1;
daniele 10:dd7111d4279f 1193 }
daniele 10:dd7111d4279f 1194
daniele 10:dd7111d4279f 1195 /** 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 1196 new = pico_zalloc(sizeof(struct pico_ipv4_link));
daniele 10:dd7111d4279f 1197 if (!new) {
daniele 10:dd7111d4279f 1198 dbg("IPv4: Out of memory!\n");
daniele 10:dd7111d4279f 1199 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1200 return -1;
daniele 10:dd7111d4279f 1201 }
daniele 10:dd7111d4279f 1202 new->address.addr = address.addr;
daniele 10:dd7111d4279f 1203 new->netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1204 new->dev = dev;
daniele 10:dd7111d4279f 1205 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1206 new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree));
daniele 10:dd7111d4279f 1207 if (!new->MCASTGroups) {
daniele 10:dd7111d4279f 1208 pico_free(new);
daniele 10:dd7111d4279f 1209 dbg("IPv4: Out of memory!\n");
daniele 10:dd7111d4279f 1210 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1211 return -1;
daniele 10:dd7111d4279f 1212 }
daniele 10:dd7111d4279f 1213
daniele 10:dd7111d4279f 1214 new->MCASTGroups->root = &LEAF;
daniele 10:dd7111d4279f 1215 new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
daniele 10:dd7111d4279f 1216 new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
daniele 10:dd7111d4279f 1217 new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
daniele 10:dd7111d4279f 1218 #endif
daniele 10:dd7111d4279f 1219
daniele 10:dd7111d4279f 1220 pico_tree_insert(&Tree_dev_link, new);
daniele 10:dd7111d4279f 1221 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1222 do {
daniele 10:dd7111d4279f 1223 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
daniele 10:dd7111d4279f 1224 if (!mcast_default_link) {
daniele 10:dd7111d4279f 1225 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
daniele 10:dd7111d4279f 1226 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
daniele 10:dd7111d4279f 1227 mcast_gw.addr = long_be(0x00000000);
daniele 10:dd7111d4279f 1228 mcast_default_link = new;
daniele 10:dd7111d4279f 1229 pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
daniele 10:dd7111d4279f 1230 }
daniele 10:dd7111d4279f 1231 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
daniele 10:dd7111d4279f 1232 pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
daniele 10:dd7111d4279f 1233 } while(0);
daniele 10:dd7111d4279f 1234 #endif
daniele 10:dd7111d4279f 1235
daniele 10:dd7111d4279f 1236 network.addr = address.addr & netmask.addr;
daniele 10:dd7111d4279f 1237 gateway.addr = 0U;
daniele 10:dd7111d4279f 1238 pico_ipv4_route_add(network, netmask, gateway, 1, new);
daniele 10:dd7111d4279f 1239 pico_ipv4_to_string(ipstr, new->address.addr);
daniele 10:dd7111d4279f 1240 dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
daniele 10:dd7111d4279f 1241 return 0;
daniele 10:dd7111d4279f 1242 }
daniele 10:dd7111d4279f 1243
daniele 10:dd7111d4279f 1244
daniele 10:dd7111d4279f 1245 int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
daniele 10:dd7111d4279f 1246 {
daniele 10:dd7111d4279f 1247 struct pico_ipv4_link test, *found;
daniele 10:dd7111d4279f 1248 struct pico_ip4 network;
daniele 10:dd7111d4279f 1249
daniele 10:dd7111d4279f 1250 if(!dev) {
daniele 10:dd7111d4279f 1251 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1252 return -1;
daniele 10:dd7111d4279f 1253 }
daniele 10:dd7111d4279f 1254 test.address.addr = address.addr;
daniele 10:dd7111d4279f 1255 test.dev = dev;
daniele 10:dd7111d4279f 1256 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1257 if (!found) {
daniele 10:dd7111d4279f 1258 pico_err = PICO_ERR_ENXIO;
daniele 10:dd7111d4279f 1259 return -1;
daniele 10:dd7111d4279f 1260 }
daniele 10:dd7111d4279f 1261
daniele 10:dd7111d4279f 1262 network.addr = found->address.addr & found->netmask.addr;
daniele 10:dd7111d4279f 1263 pico_ipv4_route_del(network, found->netmask,pico_ipv4_route_get_gateway(&found->address), 1, found);
daniele 10:dd7111d4279f 1264 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1265 do {
daniele 10:dd7111d4279f 1266 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
daniele 10:dd7111d4279f 1267 struct pico_mcast_group *g = NULL;
daniele 10:dd7111d4279f 1268 struct pico_tree_node * index, * _tmp;
daniele 10:dd7111d4279f 1269 if (found == mcast_default_link) {
daniele 10:dd7111d4279f 1270 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
daniele 10:dd7111d4279f 1271 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
daniele 10:dd7111d4279f 1272 mcast_gw.addr = long_be(0x00000000);
daniele 10:dd7111d4279f 1273 mcast_default_link = NULL;
daniele 10:dd7111d4279f 1274 pico_ipv4_route_del(mcast_addr, mcast_nm, mcast_gw, 1, found);
daniele 10:dd7111d4279f 1275 }
daniele 10:dd7111d4279f 1276 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
daniele 10:dd7111d4279f 1277 pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
daniele 10:dd7111d4279f 1278 pico_tree_foreach_safe(index,found->MCASTGroups, _tmp)
daniele 10:dd7111d4279f 1279 {
daniele 10:dd7111d4279f 1280 g = index->keyValue;
daniele 10:dd7111d4279f 1281 pico_tree_delete(found->MCASTGroups, g);
daniele 10:dd7111d4279f 1282 pico_free(g);
daniele 10:dd7111d4279f 1283 }
daniele 10:dd7111d4279f 1284 } while(0);
daniele 10:dd7111d4279f 1285 #endif
daniele 10:dd7111d4279f 1286
daniele 10:dd7111d4279f 1287 pico_tree_delete(&Tree_dev_link, found);
daniele 10:dd7111d4279f 1288 /* XXX: pico_free(found); */
daniele 10:dd7111d4279f 1289 /* XXX: cleanup all routes containing the removed link */
daniele 10:dd7111d4279f 1290 return 0;
daniele 10:dd7111d4279f 1291 }
daniele 10:dd7111d4279f 1292
daniele 10:dd7111d4279f 1293
daniele 10:dd7111d4279f 1294 struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address)
daniele 10:dd7111d4279f 1295 {
daniele 10:dd7111d4279f 1296 struct pico_ipv4_link test = {0}, *found = NULL;
daniele 10:dd7111d4279f 1297 test.address.addr = address->addr;
daniele 10:dd7111d4279f 1298
daniele 10:dd7111d4279f 1299 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1300 if (!found)
daniele 10:dd7111d4279f 1301 return NULL;
daniele 10:dd7111d4279f 1302 else
daniele 10:dd7111d4279f 1303 return found;
daniele 10:dd7111d4279f 1304 }
daniele 10:dd7111d4279f 1305
daniele 10:dd7111d4279f 1306 struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev)
daniele 10:dd7111d4279f 1307 {
daniele 10:dd7111d4279f 1308 struct pico_tree_node *index = NULL;
daniele 10:dd7111d4279f 1309 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 1310
daniele 10:dd7111d4279f 1311 pico_tree_foreach(index, &Tree_dev_link)
daniele 10:dd7111d4279f 1312 {
daniele 10:dd7111d4279f 1313 link = index->keyValue;
daniele 10:dd7111d4279f 1314 if (link->dev == dev)
daniele 10:dd7111d4279f 1315 return link;
daniele 10:dd7111d4279f 1316 }
daniele 10:dd7111d4279f 1317 return NULL;
daniele 10:dd7111d4279f 1318 }
daniele 10:dd7111d4279f 1319
daniele 10:dd7111d4279f 1320
daniele 10:dd7111d4279f 1321 struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
daniele 10:dd7111d4279f 1322 {
daniele 10:dd7111d4279f 1323 struct pico_ipv4_link test, *found;
daniele 10:dd7111d4279f 1324 if(!address) {
daniele 10:dd7111d4279f 1325 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1326 return NULL;
daniele 10:dd7111d4279f 1327 }
daniele 10:dd7111d4279f 1328 test.dev = NULL;
daniele 10:dd7111d4279f 1329 test.address.addr = address->addr;
daniele 10:dd7111d4279f 1330 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1331 if (!found) {
daniele 10:dd7111d4279f 1332 pico_err = PICO_ERR_ENXIO;
daniele 10:dd7111d4279f 1333 return NULL;
daniele 10:dd7111d4279f 1334 }
daniele 10:dd7111d4279f 1335 return found->dev;
daniele 10:dd7111d4279f 1336 }
daniele 10:dd7111d4279f 1337
daniele 10:dd7111d4279f 1338 int pico_ipv4_rebound(struct pico_frame *f)
daniele 10:dd7111d4279f 1339 {
daniele 10:dd7111d4279f 1340 struct pico_ip4 dst;
daniele 10:dd7111d4279f 1341 struct pico_ipv4_hdr *hdr;
daniele 10:dd7111d4279f 1342 if(!f) {
daniele 10:dd7111d4279f 1343 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1344 return -1;
daniele 10:dd7111d4279f 1345 }
daniele 10:dd7111d4279f 1346
daniele 10:dd7111d4279f 1347 hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 1348 if (!hdr) {
daniele 10:dd7111d4279f 1349 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1350 return -1;
daniele 10:dd7111d4279f 1351 }
daniele 10:dd7111d4279f 1352 dst.addr = hdr->src.addr;
daniele 10:dd7111d4279f 1353 return pico_ipv4_frame_push(f, &dst, hdr->proto);
daniele 10:dd7111d4279f 1354 }
daniele 10:dd7111d4279f 1355
daniele 10:dd7111d4279f 1356 static int pico_ipv4_forward(struct pico_frame *f)
daniele 10:dd7111d4279f 1357 {
daniele 10:dd7111d4279f 1358 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
daniele 10:dd7111d4279f 1359 struct pico_ipv4_route *rt;
daniele 10:dd7111d4279f 1360 if (!hdr) {
daniele 10:dd7111d4279f 1361 return -1;
daniele 10:dd7111d4279f 1362 }
daniele 10:dd7111d4279f 1363
daniele 10:dd7111d4279f 1364 //dbg("IP> FORWARDING.\n");
daniele 10:dd7111d4279f 1365 rt = route_find(&hdr->dst);
daniele 10:dd7111d4279f 1366 if (!rt) {
daniele 10:dd7111d4279f 1367 pico_notify_dest_unreachable(f);
daniele 10:dd7111d4279f 1368 return -1;
daniele 10:dd7111d4279f 1369 }
daniele 10:dd7111d4279f 1370 //dbg("ROUTE: valid..\n");
daniele 10:dd7111d4279f 1371 f->dev = rt->link->dev;
daniele 10:dd7111d4279f 1372 hdr->ttl-=1;
daniele 10:dd7111d4279f 1373 if (hdr->ttl < 1) {
daniele 10:dd7111d4279f 1374 pico_notify_ttl_expired(f);
daniele 10:dd7111d4279f 1375 return -1;
daniele 10:dd7111d4279f 1376 }
daniele 10:dd7111d4279f 1377 hdr->crc++;
daniele 10:dd7111d4279f 1378
daniele 10:dd7111d4279f 1379 /* check if NAT enbled on link and do NAT if so */
daniele 10:dd7111d4279f 1380 if (pico_ipv4_nat_isenabled_out(rt->link) == 0)
daniele 10:dd7111d4279f 1381 pico_ipv4_nat(f, rt->link->address);
daniele 10:dd7111d4279f 1382
daniele 10:dd7111d4279f 1383 //dbg("Routing towards %s\n", f->dev->name);
daniele 10:dd7111d4279f 1384 f->start = f->net_hdr;
daniele 10:dd7111d4279f 1385 if(f->dev->eth != NULL)
daniele 10:dd7111d4279f 1386 f->len -= PICO_SIZE_ETHHDR;
daniele 10:dd7111d4279f 1387 pico_sendto_dev(f);
daniele 10:dd7111d4279f 1388 return 0;
daniele 10:dd7111d4279f 1389
daniele 10:dd7111d4279f 1390 }
daniele 10:dd7111d4279f 1391
daniele 10:dd7111d4279f 1392 int pico_ipv4_is_broadcast(uint32_t addr)
daniele 10:dd7111d4279f 1393 {
daniele 10:dd7111d4279f 1394 struct pico_ipv4_link *link;
daniele 10:dd7111d4279f 1395 struct pico_tree_node * index;
daniele 10:dd7111d4279f 1396 if (addr == PICO_IP4_ANY)
daniele 10:dd7111d4279f 1397 return 1;
daniele 10:dd7111d4279f 1398 if (addr == PICO_IP4_BCAST)
daniele 10:dd7111d4279f 1399 return 1;
daniele 10:dd7111d4279f 1400
daniele 10:dd7111d4279f 1401 pico_tree_foreach(index,&Tree_dev_link) {
daniele 10:dd7111d4279f 1402 link = index->keyValue;
daniele 10:dd7111d4279f 1403 if ((link->address.addr | (~link->netmask.addr)) == addr)
daniele 10:dd7111d4279f 1404 return 1;
daniele 10:dd7111d4279f 1405 }
daniele 10:dd7111d4279f 1406 return 0;
daniele 10:dd7111d4279f 1407 }
daniele 10:dd7111d4279f 1408
daniele 10:dd7111d4279f 1409 void pico_ipv4_unreachable(struct pico_frame *f, int err)
daniele 10:dd7111d4279f 1410 {
daniele 10:dd7111d4279f 1411 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 1412 #if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 1413 f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 1414 pico_transport_error(f, hdr->proto, err);
daniele 10:dd7111d4279f 1415 #endif
daniele 10:dd7111d4279f 1416 }
daniele 10:dd7111d4279f 1417
daniele 10:dd7111d4279f 1418 #endif