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@72:887bc44746ff, 2013-09-27 (annotated)

Committer:
tass
Date:
Fri Sep 27 06:27:23 2013 +0000
Revision:
72:887bc44746ff
Parent:
70:cd218dd180e5
Child:
73:dfb737147f6e
Fix Issue #32

Who changed what in which revision?

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