Daniele Lacamera / PicoTCP Featured

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

Dependents:   lpc1768-picotcp-demo ZeroMQ_PicoTCP_Publisher_demo TCPSocket_HelloWorld_PicoTCP Pico_TCP_UDP_Test ... more

PicoTCP. Copyright (c) 2013 TASS Belgium NV.

Released under the GNU General Public License, version 2.

Different licensing models may exist, at the sole discretion of the Copyright holders.

Official homepage: http://www.picotcp.com

Bug tracker: https://github.com/tass-belgium/picotcp/issues

Development steps:

  • initial integration with mbed RTOS
  • generic mbed Ethernet driver
  • high performance NXP LPC1768 specific Ethernet driver
  • Multi-threading support for mbed RTOS
  • Berkeley sockets and integration with the New Socket API
  • Fork of the apps running on top of the New Socket API
  • Scheduling optimizations
  • Debugging/benchmarking/testing

Demo application (measuring TCP sender performance):

Import programlpc1768-picotcp-demo

A PicoTCP demo app testing the ethernet throughput on the lpc1768 mbed board.

Last commit 17 May 2013 by TASS Belgium

modules/pico_ipv4.c@1:cfe8984a32b4, 2013-05-17 (annotated)

Committer:
tass
Date:
Fri May 17 12:09:59 2013 +0000
Revision:
1:cfe8984a32b4
Parent:
libraries/picotcp/modules/pico_ipv4.c@0:d7f2341ab245 
Update for smaller SOCKETQ

Who changed what in which revision?

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