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@3:b4047e8a0123, 2013-05-24 (annotated)

Committer:
daniele
Date:
Fri May 24 15:25:25 2013 +0000
Revision:
3:b4047e8a0123
Updated from main repo + fixed Mutexes;

Who changed what in which revision?

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