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@88:0e827d0d8017, 2013-10-07 (annotated)

Committer:
tass
Date:
Mon Oct 07 06:51:23 2013 +0000
Revision:
88:0e827d0d8017
Parent:
74:c146c4e346c4
Child:
105:47c74c5d3d15
Update from masterbranch.

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