Free (GPLv2) TCP/IP stack developed by TASS Belgium
Fork of PicoTCP by
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?
User | Revision | Line number | New 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 |