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@51:ab4529a384a6, 2013-09-02 (annotated)

Committer:
tass
Date:
Mon Sep 02 08:02:21 2013 +0000
Revision:
51:ab4529a384a6
Parent:
49:4b404dd2c97a
Child:
63:97f481e33cb2
Updated 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 */
tass 51:ab4529a384a6 36 static struct pico_queue in = {0};
tass 51:ab4529a384a6 37 static struct pico_queue out = {0};
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++){
tass 51:ab4529a384a6 128 if((mask_swap << i) & 0x80000000){
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 {
tass 51:ab4529a384a6 417 IGNORE_PARAMETER(f);
daniele 10:dd7111d4279f 418 return 1;
daniele 10:dd7111d4279f 419 }
daniele 10:dd7111d4279f 420 #endif /* PICO_SUPPORT_CRC */
daniele 10:dd7111d4279f 421
daniele 10:dd7111d4279f 422 static int pico_ipv4_forward(struct pico_frame *f);
daniele 10:dd7111d4279f 423 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 424 static int pico_ipv4_mcast_filter(struct pico_frame *f);
daniele 10:dd7111d4279f 425 #endif
daniele 10:dd7111d4279f 426
daniele 10:dd7111d4279f 427 static int ipv4_link_compare(void *ka, void *kb)
daniele 10:dd7111d4279f 428 {
daniele 10:dd7111d4279f 429 struct pico_ipv4_link *a = ka, *b =kb;
daniele 10:dd7111d4279f 430 if (a->address.addr < b->address.addr)
daniele 10:dd7111d4279f 431 return -1;
daniele 10:dd7111d4279f 432 if (a->address.addr > b->address.addr)
daniele 10:dd7111d4279f 433 return 1;
daniele 10:dd7111d4279f 434
daniele 10:dd7111d4279f 435 //zero can be assigned multiple times (e.g. for DHCP)
daniele 10:dd7111d4279f 436 if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY){
daniele 10:dd7111d4279f 437 if (a->dev < b->dev)
daniele 10:dd7111d4279f 438 return -1;
daniele 10:dd7111d4279f 439 if (a->dev > b->dev)
daniele 10:dd7111d4279f 440 return 1;
daniele 10:dd7111d4279f 441 }
daniele 10:dd7111d4279f 442 return 0;
daniele 10:dd7111d4279f 443 }
daniele 10:dd7111d4279f 444
daniele 10:dd7111d4279f 445 PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare);
daniele 10:dd7111d4279f 446
daniele 10:dd7111d4279f 447 static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f)
daniele 10:dd7111d4279f 448 {
daniele 10:dd7111d4279f 449 uint8_t option_len = 0;
daniele 10:dd7111d4279f 450 int ret = 0;
daniele 10:dd7111d4279f 451 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 452 struct pico_ipv4_link test = {.address = {.addr = PICO_IP4_ANY}, .dev = NULL};
daniele 10:dd7111d4279f 453
daniele 10:dd7111d4279f 454 /* NAT needs transport header information */
daniele 10:dd7111d4279f 455 if(((hdr->vhl) & 0x0F )> 5){
daniele 10:dd7111d4279f 456 option_len = 4*(((hdr->vhl) & 0x0F)-5);
daniele 10:dd7111d4279f 457 }
daniele 10:dd7111d4279f 458 f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len;
daniele 10:dd7111d4279f 459 f->transport_len = short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len;
daniele 10:dd7111d4279f 460 f->net_len = PICO_SIZE_IP4HDR + option_len;
daniele 10:dd7111d4279f 461
daniele 10:dd7111d4279f 462 #ifdef PICO_SUPPORT_IPFILTER
daniele 10:dd7111d4279f 463 if (ipfilter(f)) {
daniele 10:dd7111d4279f 464 /*pico_frame is discarded as result of the filtering*/
daniele 10:dd7111d4279f 465 return 0;
daniele 10:dd7111d4279f 466 }
daniele 10:dd7111d4279f 467 #endif
daniele 10:dd7111d4279f 468
daniele 10:dd7111d4279f 469 /* ret == 1 indicates to continue the function */
daniele 10:dd7111d4279f 470 ret = pico_ipv4_crc_check(f);
daniele 10:dd7111d4279f 471 if (ret < 1)
daniele 10:dd7111d4279f 472 return ret;
daniele 10:dd7111d4279f 473 ret = pico_ipv4_fragmented_check(self, &f);
daniele 10:dd7111d4279f 474 if (ret < 1)
daniele 10:dd7111d4279f 475 return ret;
daniele 10:dd7111d4279f 476
daniele 10:dd7111d4279f 477 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 478 /* Multicast address in source, discard quietly */
daniele 10:dd7111d4279f 479 if (pico_ipv4_is_multicast(hdr->src.addr)) {
daniele 10:dd7111d4279f 480 ip_mcast_dbg("MCAST: ERROR multicast address %08X in source address\n", hdr->src.addr);
daniele 10:dd7111d4279f 481 pico_frame_discard(f);
daniele 10:dd7111d4279f 482 return 0;
daniele 10:dd7111d4279f 483 }
daniele 10:dd7111d4279f 484 #endif
daniele 10:dd7111d4279f 485 if (hdr->frag & 0x80) {
daniele 10:dd7111d4279f 486 pico_frame_discard(f); //RFC 3514
daniele 10:dd7111d4279f 487 return 0;
daniele 10:dd7111d4279f 488 }
daniele 10:dd7111d4279f 489 if (0) {
daniele 10:dd7111d4279f 490 #ifdef PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 491 } else if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
daniele 10:dd7111d4279f 492 /* Receiving UDP broadcast datagram */
daniele 10:dd7111d4279f 493 f->flags |= PICO_FRAME_FLAG_BCAST;
daniele 10:dd7111d4279f 494 pico_enqueue(pico_proto_udp.q_in, f);
daniele 10:dd7111d4279f 495 #endif
daniele 10:dd7111d4279f 496 } else if (pico_ipv4_is_multicast(hdr->dst.addr)) {
daniele 10:dd7111d4279f 497 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 498 /* Receiving UDP multicast datagram TODO set f->flags? */
daniele 10:dd7111d4279f 499 if (hdr->proto == PICO_PROTO_IGMP) {
daniele 10:dd7111d4279f 500 ip_mcast_dbg("MCAST: received IGMP message\n");
daniele 10:dd7111d4279f 501 pico_transport_receive(f, PICO_PROTO_IGMP);
daniele 10:dd7111d4279f 502 } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
daniele 10:dd7111d4279f 503 pico_enqueue(pico_proto_udp.q_in, f);
daniele 10:dd7111d4279f 504 } else {
daniele 10:dd7111d4279f 505 pico_frame_discard(f);
daniele 10:dd7111d4279f 506 }
daniele 10:dd7111d4279f 507 #endif
daniele 10:dd7111d4279f 508 } else if (pico_ipv4_link_find(&hdr->dst)) {
tass 51:ab4529a384a6 509 if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0)
tass 51:ab4529a384a6 510 pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */
tass 51:ab4529a384a6 511 else
daniele 10:dd7111d4279f 512 pico_transport_receive(f, hdr->proto);
daniele 10:dd7111d4279f 513 } else if (pico_tree_findKey(&Tree_dev_link, &test)){
daniele 10:dd7111d4279f 514 #ifdef PICO_SUPPORT_UDP
daniele 10:dd7111d4279f 515 //address of this device is apparently 0.0.0.0; might be a DHCP packet
tass 51:ab4529a384a6 516 /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages.
tass 51:ab4529a384a6 517 * incomming DHCP messages are to be broadcasted. Our current DHCP server
tass 51:ab4529a384a6 518 * implementation does not take this flag into account yet though ... */
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 51:ab4529a384a6 529 pico_ipv4_forward(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 {
tass 51:ab4529a384a6 540 IGNORE_PARAMETER(self);
tass 51:ab4529a384a6 541 f->start = (uint8_t*) f->net_hdr;
daniele 10:dd7111d4279f 542 #ifdef PICO_SUPPORT_IPFILTER
daniele 10:dd7111d4279f 543 if (ipfilter(f)) {
daniele 10:dd7111d4279f 544 /*pico_frame is discarded as result of the filtering*/
daniele 10:dd7111d4279f 545 return 0;
daniele 10:dd7111d4279f 546 }
daniele 10:dd7111d4279f 547 #endif
daniele 10:dd7111d4279f 548 return pico_sendto_dev(f);
daniele 10:dd7111d4279f 549 }
daniele 10:dd7111d4279f 550
daniele 10:dd7111d4279f 551
daniele 10:dd7111d4279f 552 static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, int size)
daniele 10:dd7111d4279f 553 {
tass 51:ab4529a384a6 554 struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
tass 51:ab4529a384a6 555 IGNORE_PARAMETER(self);
tass 51:ab4529a384a6 556
tass 51:ab4529a384a6 557 if (!f)
daniele 10:dd7111d4279f 558 return NULL;
daniele 10:dd7111d4279f 559 f->datalink_hdr = f->buffer;
daniele 10:dd7111d4279f 560 f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
daniele 10:dd7111d4279f 561 f->net_len = PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 562 f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 563 f->transport_len = size;
daniele 10:dd7111d4279f 564 f->len = size + PICO_SIZE_IP4HDR;
daniele 10:dd7111d4279f 565 return f;
daniele 10:dd7111d4279f 566 }
daniele 10:dd7111d4279f 567
daniele 10:dd7111d4279f 568 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f);
daniele 10:dd7111d4279f 569
daniele 10:dd7111d4279f 570 /* Interface: protocol definition */
daniele 10:dd7111d4279f 571 struct pico_protocol pico_proto_ipv4 = {
daniele 10:dd7111d4279f 572 .name = "ipv4",
daniele 10:dd7111d4279f 573 .proto_number = PICO_PROTO_IPV4,
daniele 10:dd7111d4279f 574 .layer = PICO_LAYER_NETWORK,
daniele 10:dd7111d4279f 575 .alloc = pico_ipv4_alloc,
daniele 10:dd7111d4279f 576 .process_in = pico_ipv4_process_in,
daniele 10:dd7111d4279f 577 .process_out = pico_ipv4_process_out,
daniele 10:dd7111d4279f 578 .push = pico_ipv4_frame_sock_push,
daniele 10:dd7111d4279f 579 .q_in = &in,
daniele 10:dd7111d4279f 580 .q_out = &out,
daniele 10:dd7111d4279f 581 };
daniele 10:dd7111d4279f 582
daniele 10:dd7111d4279f 583 struct pico_ipv4_route
daniele 10:dd7111d4279f 584 {
daniele 10:dd7111d4279f 585 struct pico_ip4 dest;
daniele 10:dd7111d4279f 586 struct pico_ip4 netmask;
daniele 10:dd7111d4279f 587 struct pico_ip4 gateway;
daniele 10:dd7111d4279f 588 struct pico_ipv4_link *link;
daniele 10:dd7111d4279f 589 uint32_t metric;
daniele 10:dd7111d4279f 590 };
daniele 10:dd7111d4279f 591
daniele 10:dd7111d4279f 592
daniele 10:dd7111d4279f 593 static int ipv4_route_compare(void *ka, void * kb)
daniele 10:dd7111d4279f 594 {
daniele 10:dd7111d4279f 595 struct pico_ipv4_route *a = ka, *b = kb;
daniele 10:dd7111d4279f 596
daniele 10:dd7111d4279f 597 /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
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 (long_be(a->netmask.addr) > long_be(b->netmask.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->dest.addr > b->dest.addr)
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 if (a->metric > b->metric)
daniele 10:dd7111d4279f 614 return 1;
daniele 10:dd7111d4279f 615
daniele 10:dd7111d4279f 616 return 0;
daniele 10:dd7111d4279f 617 }
daniele 10:dd7111d4279f 618
daniele 10:dd7111d4279f 619 static struct pico_ipv4_route *route_find(struct pico_ip4 *addr)
daniele 10:dd7111d4279f 620 {
daniele 10:dd7111d4279f 621 struct pico_ipv4_route *r;
daniele 10:dd7111d4279f 622 struct pico_tree_node * index;
daniele 10:dd7111d4279f 623
daniele 10:dd7111d4279f 624 if(addr->addr != PICO_IP4_BCAST)
daniele 10:dd7111d4279f 625 {
daniele 10:dd7111d4279f 626 pico_tree_foreach_reverse(index, &Routes) {
daniele 10:dd7111d4279f 627 r = index->keyValue;
daniele 10:dd7111d4279f 628 if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
daniele 10:dd7111d4279f 629 return r;
daniele 10:dd7111d4279f 630 }
daniele 10:dd7111d4279f 631 }
daniele 10:dd7111d4279f 632 }
daniele 10:dd7111d4279f 633 else
daniele 10:dd7111d4279f 634 {
daniele 10:dd7111d4279f 635 r = pico_tree_first(&Routes);
daniele 10:dd7111d4279f 636 if(!r->netmask.addr)
daniele 10:dd7111d4279f 637 {
daniele 10:dd7111d4279f 638 return r;
daniele 10:dd7111d4279f 639 }
daniele 10:dd7111d4279f 640 else
daniele 10:dd7111d4279f 641 {
daniele 10:dd7111d4279f 642 dbg("WARNING: no default route for a global broadcast found\n");
daniele 10:dd7111d4279f 643 }
daniele 10:dd7111d4279f 644 }
daniele 10:dd7111d4279f 645
daniele 10:dd7111d4279f 646 return NULL;
daniele 10:dd7111d4279f 647 }
daniele 10:dd7111d4279f 648
daniele 10:dd7111d4279f 649 struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
daniele 10:dd7111d4279f 650 {
daniele 10:dd7111d4279f 651 struct pico_ip4 nullip;
daniele 10:dd7111d4279f 652 struct pico_ipv4_route *route;
daniele 10:dd7111d4279f 653 nullip.addr = 0U;
daniele 10:dd7111d4279f 654
daniele 10:dd7111d4279f 655 if(!addr) {
daniele 10:dd7111d4279f 656 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 657 return nullip;
daniele 10:dd7111d4279f 658 }
daniele 10:dd7111d4279f 659
daniele 10:dd7111d4279f 660 route = route_find(addr);
daniele 10:dd7111d4279f 661 if (!route) {
daniele 10:dd7111d4279f 662 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 663 return nullip;
daniele 10:dd7111d4279f 664 }
daniele 10:dd7111d4279f 665 else
daniele 10:dd7111d4279f 666 return route->gateway;
daniele 10:dd7111d4279f 667 }
daniele 10:dd7111d4279f 668
daniele 10:dd7111d4279f 669 struct pico_ip4 *pico_ipv4_source_find(struct pico_ip4 *dst)
daniele 10:dd7111d4279f 670 {
daniele 10:dd7111d4279f 671 struct pico_ip4 *myself = NULL;
daniele 10:dd7111d4279f 672 struct pico_ipv4_route *rt;
daniele 10:dd7111d4279f 673
daniele 10:dd7111d4279f 674 if(!dst) {
daniele 10:dd7111d4279f 675 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 676 return NULL;
daniele 10:dd7111d4279f 677 }
daniele 10:dd7111d4279f 678
daniele 10:dd7111d4279f 679 rt = route_find(dst);
daniele 10:dd7111d4279f 680 if (rt) {
daniele 10:dd7111d4279f 681 myself = &rt->link->address;
daniele 10:dd7111d4279f 682 } else
daniele 10:dd7111d4279f 683 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 684 return myself;
daniele 10:dd7111d4279f 685 }
daniele 10:dd7111d4279f 686
daniele 10:dd7111d4279f 687
daniele 10:dd7111d4279f 688 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 689 /* link
daniele 10:dd7111d4279f 690 * |
daniele 10:dd7111d4279f 691 * MCASTGroups
daniele 10:dd7111d4279f 692 * | | |
daniele 10:dd7111d4279f 693 * ------------ | ------------
daniele 10:dd7111d4279f 694 * | | |
daniele 10:dd7111d4279f 695 * MCASTSources MCASTSources MCASTSources
daniele 10:dd7111d4279f 696 * | | | | | | | | | | | |
daniele 10:dd7111d4279f 697 * S S S S S S S S S S S S
daniele 10:dd7111d4279f 698 *
daniele 10:dd7111d4279f 699 * MCASTGroups: RBTree(mcast_group)
daniele 10:dd7111d4279f 700 * MCASTSources: RBTree(source)
daniele 10:dd7111d4279f 701 */
daniele 10:dd7111d4279f 702 static int ipv4_mcast_groups_cmp(void * ka, void * kb)
daniele 10:dd7111d4279f 703 {
daniele 10:dd7111d4279f 704 struct pico_mcast_group *a = ka, *b = kb;
daniele 10:dd7111d4279f 705 if (a->mcast_addr.addr < b->mcast_addr.addr) {
daniele 10:dd7111d4279f 706 return -1;
daniele 10:dd7111d4279f 707 } else if (a->mcast_addr.addr > b->mcast_addr.addr) {
daniele 10:dd7111d4279f 708 return 1;
daniele 10:dd7111d4279f 709 } else {
daniele 10:dd7111d4279f 710 return 0;
daniele 10:dd7111d4279f 711 }
daniele 10:dd7111d4279f 712 }
daniele 10:dd7111d4279f 713
daniele 10:dd7111d4279f 714 static int ipv4_mcast_sources_cmp(void *ka, void *kb)
daniele 10:dd7111d4279f 715 {
daniele 10:dd7111d4279f 716 struct pico_ip4 *a = ka, *b = kb;
daniele 10:dd7111d4279f 717 if (a->addr < b->addr)
daniele 10:dd7111d4279f 718 return -1;
daniele 10:dd7111d4279f 719 if (a->addr > b->addr)
daniele 10:dd7111d4279f 720 return 1;
daniele 10:dd7111d4279f 721 return 0;
daniele 10:dd7111d4279f 722 }
daniele 10:dd7111d4279f 723
daniele 10:dd7111d4279f 724 static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
daniele 10:dd7111d4279f 725 {
daniele 10:dd7111d4279f 726 uint16_t i = 0;
daniele 10:dd7111d4279f 727 struct pico_mcast_group __attribute__ ((unused)) *g = NULL;
daniele 10:dd7111d4279f 728 struct pico_ip4 __attribute__ ((unused)) *source = NULL;
daniele 10:dd7111d4279f 729 struct pico_tree_node *index = NULL, *index2 = NULL;
daniele 10:dd7111d4279f 730
daniele 10:dd7111d4279f 731 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
daniele 10:dd7111d4279f 732 ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name);
daniele 10:dd7111d4279f 733 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
daniele 10:dd7111d4279f 734 ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n");
daniele 10:dd7111d4279f 735 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
daniele 10:dd7111d4279f 736
daniele 10:dd7111d4279f 737 pico_tree_foreach(index, mcast_link->MCASTGroups)
daniele 10:dd7111d4279f 738 {
daniele 10:dd7111d4279f 739 g = index->keyValue;
daniele 10:dd7111d4279f 740 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 741 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 742 {
daniele 10:dd7111d4279f 743 source = index2->keyValue;
daniele 10:dd7111d4279f 744 ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr);
daniele 10:dd7111d4279f 745 }
daniele 10:dd7111d4279f 746 i++;
daniele 10:dd7111d4279f 747 }
daniele 10:dd7111d4279f 748 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
daniele 10:dd7111d4279f 749 }
daniele 10:dd7111d4279f 750
daniele 10:dd7111d4279f 751 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 752 {
daniele 10:dd7111d4279f 753 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 754 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 755 struct pico_tree_node *index = NULL, *_tmp = NULL;
daniele 10:dd7111d4279f 756 struct pico_ip4 *source = NULL;
daniele 10:dd7111d4279f 757
daniele 10:dd7111d4279f 758 if (mcast_link)
daniele 10:dd7111d4279f 759 link = pico_ipv4_link_get(mcast_link);
daniele 10:dd7111d4279f 760 else
daniele 10:dd7111d4279f 761 link = mcast_default_link;
daniele 10:dd7111d4279f 762
daniele 10:dd7111d4279f 763 test.mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 764 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 765 if (g) {
daniele 10:dd7111d4279f 766 if (reference_count)
daniele 10:dd7111d4279f 767 g->reference_count++;
daniele 10:dd7111d4279f 768 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
daniele 10:dd7111d4279f 769 } else {
daniele 10:dd7111d4279f 770 g = pico_zalloc(sizeof(struct pico_mcast_group));
daniele 10:dd7111d4279f 771 if (!g) {
daniele 10:dd7111d4279f 772 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 773 return -1;
daniele 10:dd7111d4279f 774 }
daniele 10:dd7111d4279f 775 /* "non-existent" state of filter mode INCLUDE and empty source list */
daniele 10:dd7111d4279f 776 g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
daniele 10:dd7111d4279f 777 g->reference_count = 1;
daniele 10:dd7111d4279f 778 g->mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 779 g->MCASTSources.root = &LEAF;
daniele 10:dd7111d4279f 780 g->MCASTSources.compare = ipv4_mcast_sources_cmp;
daniele 10:dd7111d4279f 781 pico_tree_insert(link->MCASTGroups, g);
daniele 10:dd7111d4279f 782 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
daniele 10:dd7111d4279f 783 }
daniele 10:dd7111d4279f 784
daniele 10:dd7111d4279f 785 /* cleanup filter */
daniele 10:dd7111d4279f 786 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 787 {
daniele 10:dd7111d4279f 788 source = index->keyValue;
daniele 10:dd7111d4279f 789 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 790 pico_free(source);
daniele 10:dd7111d4279f 791 }
daniele 10:dd7111d4279f 792 /* insert new filter */
daniele 10:dd7111d4279f 793 if (MCASTFilter) {
daniele 10:dd7111d4279f 794 pico_tree_foreach(index, MCASTFilter)
daniele 10:dd7111d4279f 795 {
daniele 10:dd7111d4279f 796 source = pico_zalloc(sizeof(struct pico_ip4));
daniele 10:dd7111d4279f 797 if (!source) {
daniele 10:dd7111d4279f 798 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 799 return -1;
daniele 10:dd7111d4279f 800 }
daniele 10:dd7111d4279f 801 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
daniele 10:dd7111d4279f 802 pico_tree_insert(&g->MCASTSources, source);
daniele 10:dd7111d4279f 803 }
daniele 10:dd7111d4279f 804 }
daniele 10:dd7111d4279f 805 g->filter_mode = filter_mode;
daniele 10:dd7111d4279f 806
daniele 10:dd7111d4279f 807 pico_ipv4_mcast_print_groups(link);
daniele 10:dd7111d4279f 808 return 0;
daniele 10:dd7111d4279f 809 }
daniele 10:dd7111d4279f 810
daniele 10:dd7111d4279f 811 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 812 {
daniele 10:dd7111d4279f 813
daniele 10:dd7111d4279f 814 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 815 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 816 struct pico_tree_node *index = NULL, *_tmp = NULL;
daniele 10:dd7111d4279f 817 struct pico_ip4 *source = NULL;
daniele 10:dd7111d4279f 818
daniele 10:dd7111d4279f 819 if (mcast_link)
daniele 10:dd7111d4279f 820 link = pico_ipv4_link_get(mcast_link);
daniele 10:dd7111d4279f 821 else
daniele 10:dd7111d4279f 822 link = mcast_default_link;
daniele 10:dd7111d4279f 823
daniele 10:dd7111d4279f 824 test.mcast_addr = *mcast_group;
daniele 10:dd7111d4279f 825 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 826 if (!g) {
daniele 10:dd7111d4279f 827 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 828 return -1;
daniele 10:dd7111d4279f 829 } else {
daniele 10:dd7111d4279f 830 if (reference_count && (--(g->reference_count) < 1)) {
daniele 10:dd7111d4279f 831 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
daniele 10:dd7111d4279f 832 /* cleanup filter */
daniele 10:dd7111d4279f 833 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 834 {
daniele 10:dd7111d4279f 835 source = index->keyValue;
daniele 10:dd7111d4279f 836 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 837 pico_free(source);
daniele 10:dd7111d4279f 838 }
daniele 10:dd7111d4279f 839 pico_tree_delete(link->MCASTGroups, g);
daniele 10:dd7111d4279f 840 pico_free(g);
daniele 10:dd7111d4279f 841 } else {
daniele 10:dd7111d4279f 842 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
daniele 10:dd7111d4279f 843 /* cleanup filter */
daniele 10:dd7111d4279f 844 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
daniele 10:dd7111d4279f 845 {
daniele 10:dd7111d4279f 846 source = index->keyValue;
daniele 10:dd7111d4279f 847 pico_tree_delete(&g->MCASTSources, source);
daniele 10:dd7111d4279f 848 pico_free(source);
daniele 10:dd7111d4279f 849 }
daniele 10:dd7111d4279f 850 /* insert new filter */
daniele 10:dd7111d4279f 851 if (MCASTFilter) {
daniele 10:dd7111d4279f 852 pico_tree_foreach(index, MCASTFilter)
daniele 10:dd7111d4279f 853 {
daniele 10:dd7111d4279f 854 source = pico_zalloc(sizeof(struct pico_ip4));
daniele 10:dd7111d4279f 855 if (!source) {
daniele 10:dd7111d4279f 856 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 857 return -1;
daniele 10:dd7111d4279f 858 }
daniele 10:dd7111d4279f 859 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
daniele 10:dd7111d4279f 860 pico_tree_insert(&g->MCASTSources, source);
daniele 10:dd7111d4279f 861 }
daniele 10:dd7111d4279f 862 }
daniele 10:dd7111d4279f 863 g->filter_mode = filter_mode;
daniele 10:dd7111d4279f 864 }
daniele 10:dd7111d4279f 865 }
daniele 10:dd7111d4279f 866
daniele 10:dd7111d4279f 867 pico_ipv4_mcast_print_groups(link);
daniele 10:dd7111d4279f 868 return 0;
daniele 10:dd7111d4279f 869 }
daniele 10:dd7111d4279f 870
daniele 10:dd7111d4279f 871 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
daniele 10:dd7111d4279f 872 {
daniele 10:dd7111d4279f 873 return mcast_default_link;
daniele 10:dd7111d4279f 874 }
daniele 10:dd7111d4279f 875
daniele 10:dd7111d4279f 876 static int pico_ipv4_mcast_filter(struct pico_frame *f)
daniele 10:dd7111d4279f 877 {
daniele 10:dd7111d4279f 878 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 879 struct pico_tree_node *index = NULL, *index2 = NULL;
daniele 10:dd7111d4279f 880 struct pico_mcast_group *g = NULL, test = {0};
daniele 10:dd7111d4279f 881 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 882
daniele 10:dd7111d4279f 883 test.mcast_addr = hdr->dst;
daniele 10:dd7111d4279f 884
daniele 10:dd7111d4279f 885 pico_tree_foreach(index, &Tree_dev_link)
daniele 10:dd7111d4279f 886 {
daniele 10:dd7111d4279f 887 link = index->keyValue;
daniele 10:dd7111d4279f 888 g = pico_tree_findKey(link->MCASTGroups, &test);
daniele 10:dd7111d4279f 889 if (g) {
daniele 10:dd7111d4279f 890 if (f->dev == link->dev) {
daniele 10:dd7111d4279f 891 ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
daniele 10:dd7111d4279f 892 /* perform source filtering */
daniele 10:dd7111d4279f 893 switch (g->filter_mode)
daniele 10:dd7111d4279f 894 {
daniele 10:dd7111d4279f 895 case PICO_IP_MULTICAST_INCLUDE:
daniele 10:dd7111d4279f 896 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 897 {
daniele 10:dd7111d4279f 898 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
daniele 10:dd7111d4279f 899 ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 900 return 0;
daniele 10:dd7111d4279f 901 }
daniele 10:dd7111d4279f 902 }
daniele 10:dd7111d4279f 903 ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 904 return -1;
daniele 10:dd7111d4279f 905
daniele 10:dd7111d4279f 906 case PICO_IP_MULTICAST_EXCLUDE:
daniele 10:dd7111d4279f 907 pico_tree_foreach(index2, &g->MCASTSources)
daniele 10:dd7111d4279f 908 {
daniele 10:dd7111d4279f 909 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
daniele 10:dd7111d4279f 910 ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 911 return -1;
daniele 10:dd7111d4279f 912 }
daniele 10:dd7111d4279f 913 }
daniele 10:dd7111d4279f 914 ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
daniele 10:dd7111d4279f 915 return 0;
daniele 10:dd7111d4279f 916
daniele 10:dd7111d4279f 917 default:
daniele 10:dd7111d4279f 918 return -1;
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 {
tass 51:ab4529a384a6 1074 struct pico_ip4 *dst;
daniele 10:dd7111d4279f 1075 struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
tass 51:ab4529a384a6 1076 IGNORE_PARAMETER(self);
tass 51:ab4529a384a6 1077
daniele 10:dd7111d4279f 1078 if (!f->sock) {
daniele 10:dd7111d4279f 1079 pico_frame_discard(f);
daniele 10:dd7111d4279f 1080 return -1;
daniele 10:dd7111d4279f 1081 }
daniele 10:dd7111d4279f 1082
daniele 10:dd7111d4279f 1083 if (remote_duple) {
daniele 10:dd7111d4279f 1084 dst = &remote_duple->remote_addr.ip4;
daniele 10:dd7111d4279f 1085 } else {
daniele 10:dd7111d4279f 1086 dst = &f->sock->remote_addr.ip4;
daniele 10:dd7111d4279f 1087 }
daniele 10:dd7111d4279f 1088
daniele 10:dd7111d4279f 1089 return pico_ipv4_frame_push(f, dst, f->sock->proto->proto_number);
daniele 10:dd7111d4279f 1090 }
daniele 10:dd7111d4279f 1091
daniele 10:dd7111d4279f 1092
daniele 10:dd7111d4279f 1093 #ifdef DEBUG_ROUTE
daniele 10:dd7111d4279f 1094 static void dbg_route(void)
daniele 10:dd7111d4279f 1095 {
daniele 10:dd7111d4279f 1096 struct pico_ipv4_route *r;
daniele 10:dd7111d4279f 1097 struct pico_tree_node * index;
daniele 10:dd7111d4279f 1098 pico_tree_foreach(index,&Routes){
daniele 10:dd7111d4279f 1099 r = index->keyValue;
daniele 10:dd7111d4279f 1100 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 1101 }
daniele 10:dd7111d4279f 1102 }
daniele 10:dd7111d4279f 1103 #else
daniele 10:dd7111d4279f 1104 #define dbg_route() do{ }while(0)
daniele 10:dd7111d4279f 1105 #endif
daniele 10:dd7111d4279f 1106
daniele 10:dd7111d4279f 1107 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 1108 {
daniele 10:dd7111d4279f 1109 struct pico_ipv4_route test, *new;
daniele 10:dd7111d4279f 1110 test.dest.addr = address.addr;
daniele 10:dd7111d4279f 1111 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1112 test.metric = metric;
daniele 10:dd7111d4279f 1113
daniele 10:dd7111d4279f 1114 if(pico_tree_findKey(&Routes,&test)){
daniele 10:dd7111d4279f 1115 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1116 return -1;
daniele 10:dd7111d4279f 1117 }
daniele 10:dd7111d4279f 1118
daniele 10:dd7111d4279f 1119 new = pico_zalloc(sizeof(struct pico_ipv4_route));
daniele 10:dd7111d4279f 1120 if (!new) {
daniele 10:dd7111d4279f 1121 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1122 return -1;
daniele 10:dd7111d4279f 1123 }
daniele 10:dd7111d4279f 1124 new->dest.addr = address.addr;
daniele 10:dd7111d4279f 1125 new->netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1126 new->gateway.addr = gateway.addr;
daniele 10:dd7111d4279f 1127 new->metric = metric;
daniele 10:dd7111d4279f 1128 if (gateway.addr == 0) {
daniele 10:dd7111d4279f 1129 /* No gateway provided, use the link */
daniele 10:dd7111d4279f 1130 new->link = link;
daniele 10:dd7111d4279f 1131 } else {
daniele 10:dd7111d4279f 1132 struct pico_ipv4_route *r = route_find(&gateway);
daniele 10:dd7111d4279f 1133 if (!r ) { /* Specified Gateway is unreachable */
daniele 10:dd7111d4279f 1134 pico_err = PICO_ERR_EHOSTUNREACH;
daniele 10:dd7111d4279f 1135 pico_free(new);
daniele 10:dd7111d4279f 1136 return -1;
daniele 10:dd7111d4279f 1137 }
daniele 10:dd7111d4279f 1138 if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
daniele 10:dd7111d4279f 1139 pico_err = PICO_ERR_ENETUNREACH;
daniele 10:dd7111d4279f 1140 pico_free(new);
daniele 10:dd7111d4279f 1141 return -1;
daniele 10:dd7111d4279f 1142 }
daniele 10:dd7111d4279f 1143 new->link = r->link;
daniele 10:dd7111d4279f 1144 }
daniele 10:dd7111d4279f 1145 if (!new->link) {
daniele 10:dd7111d4279f 1146 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1147 pico_free(new);
daniele 10:dd7111d4279f 1148 return -1;
daniele 10:dd7111d4279f 1149 }
daniele 10:dd7111d4279f 1150
daniele 10:dd7111d4279f 1151 pico_tree_insert(&Routes,new);
daniele 10:dd7111d4279f 1152 dbg_route();
daniele 10:dd7111d4279f 1153 return 0;
daniele 10:dd7111d4279f 1154 }
daniele 10:dd7111d4279f 1155
daniele 10:dd7111d4279f 1156 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 1157 {
tass 51:ab4529a384a6 1158 struct pico_ipv4_route test, *found;
tass 51:ab4529a384a6 1159 IGNORE_PARAMETER(gateway);
tass 51:ab4529a384a6 1160
tass 51:ab4529a384a6 1161 if (!link) {
daniele 10:dd7111d4279f 1162 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1163 return -1;
daniele 10:dd7111d4279f 1164 }
daniele 10:dd7111d4279f 1165 test.dest.addr = address.addr;
daniele 10:dd7111d4279f 1166 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1167 test.metric = metric;
daniele 10:dd7111d4279f 1168
daniele 10:dd7111d4279f 1169 found = pico_tree_findKey(&Routes,&test);
daniele 10:dd7111d4279f 1170 if (found) {
daniele 10:dd7111d4279f 1171
daniele 10:dd7111d4279f 1172 pico_tree_delete(&Routes,found);
daniele 10:dd7111d4279f 1173 pico_free(found);
daniele 10:dd7111d4279f 1174
daniele 10:dd7111d4279f 1175 dbg_route();
daniele 10:dd7111d4279f 1176 return 0;
daniele 10:dd7111d4279f 1177 }
daniele 10:dd7111d4279f 1178 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1179 return -1;
daniele 10:dd7111d4279f 1180 }
daniele 10:dd7111d4279f 1181
daniele 10:dd7111d4279f 1182
daniele 10:dd7111d4279f 1183 int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
daniele 10:dd7111d4279f 1184 {
daniele 10:dd7111d4279f 1185 struct pico_ipv4_link test, *new;
daniele 10:dd7111d4279f 1186 struct pico_ip4 network, gateway;
daniele 10:dd7111d4279f 1187 char ipstr[30];
daniele 10:dd7111d4279f 1188
daniele 10:dd7111d4279f 1189 if(!dev) {
daniele 10:dd7111d4279f 1190 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1191 return -1;
daniele 10:dd7111d4279f 1192 }
daniele 10:dd7111d4279f 1193 test.address.addr = address.addr;
daniele 10:dd7111d4279f 1194 test.netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1195 test.dev = dev;
daniele 10:dd7111d4279f 1196 /** XXX: Valid netmask / unicast address test **/
daniele 10:dd7111d4279f 1197
daniele 10:dd7111d4279f 1198 if(pico_tree_findKey(&Tree_dev_link, &test)) {
daniele 10:dd7111d4279f 1199 dbg("IPv4: Trying to assign an invalid address (in use)\n");
daniele 10:dd7111d4279f 1200 pico_err = PICO_ERR_EADDRINUSE;
daniele 10:dd7111d4279f 1201 return -1;
daniele 10:dd7111d4279f 1202 }
daniele 10:dd7111d4279f 1203
daniele 10:dd7111d4279f 1204 /** 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 1205 new = pico_zalloc(sizeof(struct pico_ipv4_link));
daniele 10:dd7111d4279f 1206 if (!new) {
daniele 10:dd7111d4279f 1207 dbg("IPv4: Out of memory!\n");
daniele 10:dd7111d4279f 1208 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1209 return -1;
daniele 10:dd7111d4279f 1210 }
daniele 10:dd7111d4279f 1211 new->address.addr = address.addr;
daniele 10:dd7111d4279f 1212 new->netmask.addr = netmask.addr;
daniele 10:dd7111d4279f 1213 new->dev = dev;
daniele 10:dd7111d4279f 1214 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1215 new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree));
daniele 10:dd7111d4279f 1216 if (!new->MCASTGroups) {
daniele 10:dd7111d4279f 1217 pico_free(new);
daniele 10:dd7111d4279f 1218 dbg("IPv4: Out of memory!\n");
daniele 10:dd7111d4279f 1219 pico_err = PICO_ERR_ENOMEM;
daniele 10:dd7111d4279f 1220 return -1;
daniele 10:dd7111d4279f 1221 }
daniele 10:dd7111d4279f 1222
daniele 10:dd7111d4279f 1223 new->MCASTGroups->root = &LEAF;
daniele 10:dd7111d4279f 1224 new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
daniele 10:dd7111d4279f 1225 new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
daniele 10:dd7111d4279f 1226 new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
daniele 10:dd7111d4279f 1227 #endif
daniele 10:dd7111d4279f 1228
daniele 10:dd7111d4279f 1229 pico_tree_insert(&Tree_dev_link, new);
daniele 10:dd7111d4279f 1230 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1231 do {
daniele 10:dd7111d4279f 1232 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
daniele 10:dd7111d4279f 1233 if (!mcast_default_link) {
daniele 10:dd7111d4279f 1234 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
daniele 10:dd7111d4279f 1235 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
daniele 10:dd7111d4279f 1236 mcast_gw.addr = long_be(0x00000000);
daniele 10:dd7111d4279f 1237 mcast_default_link = new;
daniele 10:dd7111d4279f 1238 pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
daniele 10:dd7111d4279f 1239 }
daniele 10:dd7111d4279f 1240 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
daniele 10:dd7111d4279f 1241 pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
daniele 10:dd7111d4279f 1242 } while(0);
daniele 10:dd7111d4279f 1243 #endif
daniele 10:dd7111d4279f 1244
daniele 10:dd7111d4279f 1245 network.addr = address.addr & netmask.addr;
daniele 10:dd7111d4279f 1246 gateway.addr = 0U;
daniele 10:dd7111d4279f 1247 pico_ipv4_route_add(network, netmask, gateway, 1, new);
daniele 10:dd7111d4279f 1248 pico_ipv4_to_string(ipstr, new->address.addr);
daniele 10:dd7111d4279f 1249 dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
daniele 10:dd7111d4279f 1250 return 0;
daniele 10:dd7111d4279f 1251 }
daniele 10:dd7111d4279f 1252
daniele 10:dd7111d4279f 1253
daniele 10:dd7111d4279f 1254 int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
daniele 10:dd7111d4279f 1255 {
daniele 10:dd7111d4279f 1256 struct pico_ipv4_link test, *found;
daniele 10:dd7111d4279f 1257 struct pico_ip4 network;
daniele 10:dd7111d4279f 1258
daniele 10:dd7111d4279f 1259 if(!dev) {
daniele 10:dd7111d4279f 1260 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1261 return -1;
daniele 10:dd7111d4279f 1262 }
daniele 10:dd7111d4279f 1263 test.address.addr = address.addr;
daniele 10:dd7111d4279f 1264 test.dev = dev;
daniele 10:dd7111d4279f 1265 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1266 if (!found) {
daniele 10:dd7111d4279f 1267 pico_err = PICO_ERR_ENXIO;
daniele 10:dd7111d4279f 1268 return -1;
daniele 10:dd7111d4279f 1269 }
daniele 10:dd7111d4279f 1270
daniele 10:dd7111d4279f 1271 network.addr = found->address.addr & found->netmask.addr;
daniele 10:dd7111d4279f 1272 pico_ipv4_route_del(network, found->netmask,pico_ipv4_route_get_gateway(&found->address), 1, found);
daniele 10:dd7111d4279f 1273 #ifdef PICO_SUPPORT_MCAST
daniele 10:dd7111d4279f 1274 do {
daniele 10:dd7111d4279f 1275 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
daniele 10:dd7111d4279f 1276 struct pico_mcast_group *g = NULL;
daniele 10:dd7111d4279f 1277 struct pico_tree_node * index, * _tmp;
daniele 10:dd7111d4279f 1278 if (found == mcast_default_link) {
daniele 10:dd7111d4279f 1279 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
daniele 10:dd7111d4279f 1280 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
daniele 10:dd7111d4279f 1281 mcast_gw.addr = long_be(0x00000000);
daniele 10:dd7111d4279f 1282 mcast_default_link = NULL;
daniele 10:dd7111d4279f 1283 pico_ipv4_route_del(mcast_addr, mcast_nm, mcast_gw, 1, found);
daniele 10:dd7111d4279f 1284 }
daniele 10:dd7111d4279f 1285 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
daniele 10:dd7111d4279f 1286 pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
daniele 10:dd7111d4279f 1287 pico_tree_foreach_safe(index,found->MCASTGroups, _tmp)
daniele 10:dd7111d4279f 1288 {
daniele 10:dd7111d4279f 1289 g = index->keyValue;
daniele 10:dd7111d4279f 1290 pico_tree_delete(found->MCASTGroups, g);
daniele 10:dd7111d4279f 1291 pico_free(g);
daniele 10:dd7111d4279f 1292 }
daniele 10:dd7111d4279f 1293 } while(0);
daniele 10:dd7111d4279f 1294 #endif
daniele 10:dd7111d4279f 1295
daniele 10:dd7111d4279f 1296 pico_tree_delete(&Tree_dev_link, found);
daniele 10:dd7111d4279f 1297 /* XXX: pico_free(found); */
daniele 10:dd7111d4279f 1298 /* XXX: cleanup all routes containing the removed link */
daniele 10:dd7111d4279f 1299 return 0;
daniele 10:dd7111d4279f 1300 }
daniele 10:dd7111d4279f 1301
daniele 10:dd7111d4279f 1302
daniele 10:dd7111d4279f 1303 struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address)
daniele 10:dd7111d4279f 1304 {
daniele 10:dd7111d4279f 1305 struct pico_ipv4_link test = {0}, *found = NULL;
daniele 10:dd7111d4279f 1306 test.address.addr = address->addr;
daniele 10:dd7111d4279f 1307
daniele 10:dd7111d4279f 1308 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1309 if (!found)
daniele 10:dd7111d4279f 1310 return NULL;
daniele 10:dd7111d4279f 1311 else
daniele 10:dd7111d4279f 1312 return found;
daniele 10:dd7111d4279f 1313 }
daniele 10:dd7111d4279f 1314
daniele 10:dd7111d4279f 1315 struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev)
daniele 10:dd7111d4279f 1316 {
daniele 10:dd7111d4279f 1317 struct pico_tree_node *index = NULL;
daniele 10:dd7111d4279f 1318 struct pico_ipv4_link *link = NULL;
daniele 10:dd7111d4279f 1319
daniele 10:dd7111d4279f 1320 pico_tree_foreach(index, &Tree_dev_link)
daniele 10:dd7111d4279f 1321 {
daniele 10:dd7111d4279f 1322 link = index->keyValue;
daniele 10:dd7111d4279f 1323 if (link->dev == dev)
daniele 10:dd7111d4279f 1324 return link;
daniele 10:dd7111d4279f 1325 }
daniele 10:dd7111d4279f 1326 return NULL;
daniele 10:dd7111d4279f 1327 }
daniele 10:dd7111d4279f 1328
daniele 10:dd7111d4279f 1329
daniele 10:dd7111d4279f 1330 struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
daniele 10:dd7111d4279f 1331 {
daniele 10:dd7111d4279f 1332 struct pico_ipv4_link test, *found;
daniele 10:dd7111d4279f 1333 if(!address) {
daniele 10:dd7111d4279f 1334 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1335 return NULL;
daniele 10:dd7111d4279f 1336 }
daniele 10:dd7111d4279f 1337 test.dev = NULL;
daniele 10:dd7111d4279f 1338 test.address.addr = address->addr;
daniele 10:dd7111d4279f 1339 found = pico_tree_findKey(&Tree_dev_link, &test);
daniele 10:dd7111d4279f 1340 if (!found) {
daniele 10:dd7111d4279f 1341 pico_err = PICO_ERR_ENXIO;
daniele 10:dd7111d4279f 1342 return NULL;
daniele 10:dd7111d4279f 1343 }
daniele 10:dd7111d4279f 1344 return found->dev;
daniele 10:dd7111d4279f 1345 }
daniele 10:dd7111d4279f 1346
daniele 10:dd7111d4279f 1347 int pico_ipv4_rebound(struct pico_frame *f)
daniele 10:dd7111d4279f 1348 {
daniele 10:dd7111d4279f 1349 struct pico_ip4 dst;
daniele 10:dd7111d4279f 1350 struct pico_ipv4_hdr *hdr;
daniele 10:dd7111d4279f 1351 if(!f) {
daniele 10:dd7111d4279f 1352 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1353 return -1;
daniele 10:dd7111d4279f 1354 }
daniele 10:dd7111d4279f 1355
daniele 10:dd7111d4279f 1356 hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 10:dd7111d4279f 1357 if (!hdr) {
daniele 10:dd7111d4279f 1358 pico_err = PICO_ERR_EINVAL;
daniele 10:dd7111d4279f 1359 return -1;
daniele 10:dd7111d4279f 1360 }
daniele 10:dd7111d4279f 1361 dst.addr = hdr->src.addr;
daniele 10:dd7111d4279f 1362 return pico_ipv4_frame_push(f, &dst, hdr->proto);
daniele 10:dd7111d4279f 1363 }
daniele 10:dd7111d4279f 1364
daniele 10:dd7111d4279f 1365 static int pico_ipv4_forward(struct pico_frame *f)
daniele 10:dd7111d4279f 1366 {
daniele 10:dd7111d4279f 1367 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
daniele 10:dd7111d4279f 1368 struct pico_ipv4_route *rt;
daniele 10:dd7111d4279f 1369 if (!hdr) {
daniele 10:dd7111d4279f 1370 return -1;
daniele 10:dd7111d4279f 1371 }
daniele 10:dd7111d4279f 1372
daniele 10:dd7111d4279f 1373 rt = route_find(&hdr->dst);
daniele 10:dd7111d4279f 1374 if (!rt) {
daniele 10:dd7111d4279f 1375 pico_notify_dest_unreachable(f);
daniele 10:dd7111d4279f 1376 return -1;
daniele 10:dd7111d4279f 1377 }
tass 51:ab4529a384a6 1378
daniele 10:dd7111d4279f 1379 f->dev = rt->link->dev;
daniele 10:dd7111d4279f 1380 hdr->ttl-=1;
daniele 10:dd7111d4279f 1381 if (hdr->ttl < 1) {
daniele 10:dd7111d4279f 1382 pico_notify_ttl_expired(f);
daniele 10:dd7111d4279f 1383 return -1;
daniele 10:dd7111d4279f 1384 }
daniele 10:dd7111d4279f 1385 hdr->crc++;
daniele 10:dd7111d4279f 1386
tass 51:ab4529a384a6 1387 pico_ipv4_nat_outbound(f, &rt->link->address);
daniele 10:dd7111d4279f 1388
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