Daniele Lacamera / PicoTCP-Experimental_CDC_ECM_Branch

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

Fork of PicoTCP by Daniele Lacamera

modules/pico_ipv4.c@51:18637a3d071f, 2013-08-03 (annotated)

Committer:
daniele
Date:
Sat Aug 03 08:50:27 2013 +0000
Revision:
51:18637a3d071f
Parent:
10:dd7111d4279f 
Branch for CDC-ECM: Work in progress

Who changed what in which revision?

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