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@6:55b47464d5bc, 2013-05-31 (annotated)

Committer:
tass
Date:
Fri May 31 11:34:20 2013 +0000
Revision:
6:55b47464d5bc
Integrated mbed friendly sockets;

Who changed what in which revision?

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