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@105:47c74c5d3d15, 2013-10-24 (annotated)

Committer:
tass
Date:
Thu Oct 24 06:18:38 2013 +0000
Revision:
105:47c74c5d3d15
Parent:
88:0e827d0d8017
Child:
110:0ece1bbbd36e
Issue #50 fixed.

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 105:47c74c5d3d15 532 pico_frame_discard(f);
tass 88:0e827d0d8017 533 //dbg("Forward failed.\n");
tass 68:0847e35d08a6 534 }
tass 68:0847e35d08a6 535 }
tass 68:0847e35d08a6 536 return 0;
tass 68:0847e35d08a6 537 }
tass 68:0847e35d08a6 538
tass 68:0847e35d08a6 539 PICO_TREE_DECLARE(Routes, ipv4_route_compare);
tass 68:0847e35d08a6 540
tass 68:0847e35d08a6 541
tass 68:0847e35d08a6 542 static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f)
tass 68:0847e35d08a6 543 {
tass 68:0847e35d08a6 544 IGNORE_PARAMETER(self);
tass 68:0847e35d08a6 545 f->start = (uint8_t*) f->net_hdr;
tass 68:0847e35d08a6 546 #ifdef PICO_SUPPORT_IPFILTER
tass 68:0847e35d08a6 547 if (ipfilter(f)) {
tass 68:0847e35d08a6 548 /*pico_frame is discarded as result of the filtering*/
tass 68:0847e35d08a6 549 return 0;
tass 68:0847e35d08a6 550 }
tass 68:0847e35d08a6 551 #endif
tass 68:0847e35d08a6 552 return pico_sendto_dev(f);
tass 68:0847e35d08a6 553 }
tass 68:0847e35d08a6 554
tass 68:0847e35d08a6 555
tass 70:cd218dd180e5 556 static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, uint16_t size)
tass 68:0847e35d08a6 557 {
tass 68:0847e35d08a6 558 struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
tass 68:0847e35d08a6 559 IGNORE_PARAMETER(self);
tass 68:0847e35d08a6 560
tass 68:0847e35d08a6 561 if (!f)
tass 68:0847e35d08a6 562 return NULL;
tass 68:0847e35d08a6 563 f->datalink_hdr = f->buffer;
tass 68:0847e35d08a6 564 f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
tass 68:0847e35d08a6 565 f->net_len = PICO_SIZE_IP4HDR;
tass 68:0847e35d08a6 566 f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
tass 68:0847e35d08a6 567 f->transport_len = size;
tass 68:0847e35d08a6 568 f->len = size + PICO_SIZE_IP4HDR;
tass 68:0847e35d08a6 569 return f;
tass 68:0847e35d08a6 570 }
tass 68:0847e35d08a6 571
tass 68:0847e35d08a6 572 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f);
tass 68:0847e35d08a6 573
tass 68:0847e35d08a6 574 /* Interface: protocol definition */
tass 68:0847e35d08a6 575 struct pico_protocol pico_proto_ipv4 = {
tass 68:0847e35d08a6 576 .name = "ipv4",
tass 68:0847e35d08a6 577 .proto_number = PICO_PROTO_IPV4,
tass 68:0847e35d08a6 578 .layer = PICO_LAYER_NETWORK,
tass 68:0847e35d08a6 579 .alloc = pico_ipv4_alloc,
tass 68:0847e35d08a6 580 .process_in = pico_ipv4_process_in,
tass 68:0847e35d08a6 581 .process_out = pico_ipv4_process_out,
tass 68:0847e35d08a6 582 .push = pico_ipv4_frame_sock_push,
tass 68:0847e35d08a6 583 .q_in = &in,
tass 68:0847e35d08a6 584 .q_out = &out,
tass 68:0847e35d08a6 585 };
tass 68:0847e35d08a6 586
tass 68:0847e35d08a6 587 struct pico_ipv4_route
tass 68:0847e35d08a6 588 {
tass 68:0847e35d08a6 589 struct pico_ip4 dest;
tass 68:0847e35d08a6 590 struct pico_ip4 netmask;
tass 68:0847e35d08a6 591 struct pico_ip4 gateway;
tass 68:0847e35d08a6 592 struct pico_ipv4_link *link;
tass 68:0847e35d08a6 593 uint32_t metric;
tass 68:0847e35d08a6 594 };
tass 68:0847e35d08a6 595
tass 68:0847e35d08a6 596
tass 68:0847e35d08a6 597 static int ipv4_route_compare(void *ka, void * kb)
tass 68:0847e35d08a6 598 {
tass 68:0847e35d08a6 599 struct pico_ipv4_route *a = ka, *b = kb;
tass 68:0847e35d08a6 600
tass 68:0847e35d08a6 601 /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
tass 68:0847e35d08a6 602 if (long_be(a->netmask.addr) < long_be(b->netmask.addr))
tass 68:0847e35d08a6 603 return -1;
tass 68:0847e35d08a6 604
tass 68:0847e35d08a6 605 if (long_be(a->netmask.addr) > long_be(b->netmask.addr))
tass 68:0847e35d08a6 606 return 1;
tass 68:0847e35d08a6 607
tass 68:0847e35d08a6 608 if (a->dest.addr < b->dest.addr)
tass 68:0847e35d08a6 609 return -1;
tass 68:0847e35d08a6 610
tass 68:0847e35d08a6 611 if (a->dest.addr > b->dest.addr)
tass 68:0847e35d08a6 612 return 1;
tass 68:0847e35d08a6 613
tass 68:0847e35d08a6 614 if (a->metric < b->metric)
tass 68:0847e35d08a6 615 return -1;
tass 68:0847e35d08a6 616
tass 68:0847e35d08a6 617 if (a->metric > b->metric)
tass 68:0847e35d08a6 618 return 1;
tass 68:0847e35d08a6 619
tass 68:0847e35d08a6 620 return 0;
tass 68:0847e35d08a6 621 }
tass 68:0847e35d08a6 622
tass 68:0847e35d08a6 623 static struct pico_ipv4_route *route_find(const struct pico_ip4 *addr)
tass 68:0847e35d08a6 624 {
tass 68:0847e35d08a6 625 struct pico_ipv4_route *r;
tass 68:0847e35d08a6 626 struct pico_tree_node * index;
tass 68:0847e35d08a6 627
tass 68:0847e35d08a6 628 if(addr->addr != PICO_IP4_BCAST)
tass 68:0847e35d08a6 629 {
tass 68:0847e35d08a6 630 pico_tree_foreach_reverse(index, &Routes) {
tass 68:0847e35d08a6 631 r = index->keyValue;
tass 68:0847e35d08a6 632 if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
tass 68:0847e35d08a6 633 return r;
tass 68:0847e35d08a6 634 }
tass 68:0847e35d08a6 635 }
tass 68:0847e35d08a6 636 }
tass 68:0847e35d08a6 637 else
tass 68:0847e35d08a6 638 {
tass 68:0847e35d08a6 639 r = pico_tree_first(&Routes);
tass 68:0847e35d08a6 640 if(!r->netmask.addr)
tass 68:0847e35d08a6 641 {
tass 68:0847e35d08a6 642 return r;
tass 68:0847e35d08a6 643 }
tass 68:0847e35d08a6 644 else
tass 68:0847e35d08a6 645 {
tass 68:0847e35d08a6 646 dbg("WARNING: no default route for a global broadcast found\n");
tass 68:0847e35d08a6 647 }
tass 68:0847e35d08a6 648 }
tass 68:0847e35d08a6 649
tass 68:0847e35d08a6 650 return NULL;
tass 68:0847e35d08a6 651 }
tass 68:0847e35d08a6 652
tass 68:0847e35d08a6 653 struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
tass 68:0847e35d08a6 654 {
tass 68:0847e35d08a6 655 struct pico_ip4 nullip;
tass 68:0847e35d08a6 656 struct pico_ipv4_route *route;
tass 68:0847e35d08a6 657 nullip.addr = 0U;
tass 68:0847e35d08a6 658
tass 68:0847e35d08a6 659 if(!addr) {
tass 68:0847e35d08a6 660 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 661 return nullip;
tass 68:0847e35d08a6 662 }
tass 68:0847e35d08a6 663
tass 68:0847e35d08a6 664 route = route_find(addr);
tass 68:0847e35d08a6 665 if (!route) {
tass 68:0847e35d08a6 666 pico_err = PICO_ERR_EHOSTUNREACH;
tass 68:0847e35d08a6 667 return nullip;
tass 68:0847e35d08a6 668 }
tass 68:0847e35d08a6 669 else
tass 68:0847e35d08a6 670 return route->gateway;
tass 68:0847e35d08a6 671 }
tass 68:0847e35d08a6 672
tass 68:0847e35d08a6 673 struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst)
tass 68:0847e35d08a6 674 {
tass 68:0847e35d08a6 675 struct pico_ip4 *myself = NULL;
tass 68:0847e35d08a6 676 struct pico_ipv4_route *rt;
tass 68:0847e35d08a6 677
tass 68:0847e35d08a6 678 if(!dst) {
tass 68:0847e35d08a6 679 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 680 return NULL;
tass 68:0847e35d08a6 681 }
tass 68:0847e35d08a6 682
tass 68:0847e35d08a6 683 rt = route_find(dst);
tass 68:0847e35d08a6 684 if (rt) {
tass 68:0847e35d08a6 685 myself = &rt->link->address;
tass 68:0847e35d08a6 686 } else
tass 68:0847e35d08a6 687 pico_err = PICO_ERR_EHOSTUNREACH;
tass 68:0847e35d08a6 688 return myself;
tass 68:0847e35d08a6 689 }
tass 68:0847e35d08a6 690
tass 68:0847e35d08a6 691
tass 68:0847e35d08a6 692 #ifdef PICO_SUPPORT_MCAST
tass 68:0847e35d08a6 693 /* link
tass 68:0847e35d08a6 694 * |
tass 68:0847e35d08a6 695 * MCASTGroups
tass 68:0847e35d08a6 696 * | | |
tass 68:0847e35d08a6 697 * ------------ | ------------
tass 68:0847e35d08a6 698 * | | |
tass 68:0847e35d08a6 699 * MCASTSources MCASTSources MCASTSources
tass 68:0847e35d08a6 700 * | | | | | | | | | | | |
tass 68:0847e35d08a6 701 * S S S S S S S S S S S S
tass 68:0847e35d08a6 702 *
tass 68:0847e35d08a6 703 * MCASTGroups: RBTree(mcast_group)
tass 68:0847e35d08a6 704 * MCASTSources: RBTree(source)
tass 68:0847e35d08a6 705 */
tass 68:0847e35d08a6 706 static int ipv4_mcast_groups_cmp(void * ka, void * kb)
tass 68:0847e35d08a6 707 {
tass 68:0847e35d08a6 708 struct pico_mcast_group *a = ka, *b = kb;
tass 68:0847e35d08a6 709 if (a->mcast_addr.addr < b->mcast_addr.addr) {
tass 68:0847e35d08a6 710 return -1;
tass 68:0847e35d08a6 711 } else if (a->mcast_addr.addr > b->mcast_addr.addr) {
tass 68:0847e35d08a6 712 return 1;
tass 68:0847e35d08a6 713 } else {
tass 68:0847e35d08a6 714 return 0;
tass 68:0847e35d08a6 715 }
tass 68:0847e35d08a6 716 }
tass 68:0847e35d08a6 717
tass 68:0847e35d08a6 718 static int ipv4_mcast_sources_cmp(void *ka, void *kb)
tass 68:0847e35d08a6 719 {
tass 68:0847e35d08a6 720 struct pico_ip4 *a = ka, *b = kb;
tass 68:0847e35d08a6 721 if (a->addr < b->addr)
tass 68:0847e35d08a6 722 return -1;
tass 68:0847e35d08a6 723 if (a->addr > b->addr)
tass 68:0847e35d08a6 724 return 1;
tass 68:0847e35d08a6 725 return 0;
tass 68:0847e35d08a6 726 }
tass 68:0847e35d08a6 727
tass 68:0847e35d08a6 728 static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
tass 68:0847e35d08a6 729 {
tass 68:0847e35d08a6 730 uint16_t i = 0;
tass 68:0847e35d08a6 731 struct pico_mcast_group __attribute__ ((unused)) *g = NULL;
tass 68:0847e35d08a6 732 struct pico_ip4 __attribute__ ((unused)) *source = NULL;
tass 68:0847e35d08a6 733 struct pico_tree_node *index = NULL, *index2 = NULL;
tass 68:0847e35d08a6 734
tass 68:0847e35d08a6 735 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
tass 68:0847e35d08a6 736 ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name);
tass 68:0847e35d08a6 737 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
tass 68:0847e35d08a6 738 ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n");
tass 68:0847e35d08a6 739 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
tass 68:0847e35d08a6 740
tass 68:0847e35d08a6 741 pico_tree_foreach(index, mcast_link->MCASTGroups)
tass 68:0847e35d08a6 742 {
tass 68:0847e35d08a6 743 g = index->keyValue;
tass 68:0847e35d08a6 744 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 745 pico_tree_foreach(index2, &g->MCASTSources)
tass 68:0847e35d08a6 746 {
tass 68:0847e35d08a6 747 source = index2->keyValue;
tass 68:0847e35d08a6 748 ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr);
tass 68:0847e35d08a6 749 }
tass 68:0847e35d08a6 750 i++;
tass 68:0847e35d08a6 751 }
tass 68:0847e35d08a6 752 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
tass 68:0847e35d08a6 753 }
tass 68:0847e35d08a6 754
tass 70:cd218dd180e5 755 static int mcast_group_update(struct pico_mcast_group *g, struct pico_tree *MCASTFilter, uint8_t filter_mode)
tass 70:cd218dd180e5 756 {
tass 70:cd218dd180e5 757 struct pico_tree_node *index = NULL, *_tmp = NULL;
tass 70:cd218dd180e5 758 struct pico_ip4 *source = NULL;
tass 70:cd218dd180e5 759 /* cleanup filter */
tass 70:cd218dd180e5 760 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
tass 70:cd218dd180e5 761 {
tass 70:cd218dd180e5 762 source = index->keyValue;
tass 70:cd218dd180e5 763 pico_tree_delete(&g->MCASTSources, source);
tass 70:cd218dd180e5 764 pico_free(source);
tass 70:cd218dd180e5 765 }
tass 70:cd218dd180e5 766 /* insert new filter */
tass 70:cd218dd180e5 767 if (MCASTFilter) {
tass 70:cd218dd180e5 768 pico_tree_foreach(index, MCASTFilter)
tass 70:cd218dd180e5 769 {
tass 70:cd218dd180e5 770 source = pico_zalloc(sizeof(struct pico_ip4));
tass 70:cd218dd180e5 771 if (!source) {
tass 70:cd218dd180e5 772 pico_err = PICO_ERR_ENOMEM;
tass 70:cd218dd180e5 773 return -1;
tass 70:cd218dd180e5 774 }
tass 70:cd218dd180e5 775 source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
tass 70:cd218dd180e5 776 pico_tree_insert(&g->MCASTSources, source);
tass 70:cd218dd180e5 777 }
tass 70:cd218dd180e5 778 }
tass 70:cd218dd180e5 779 g->filter_mode = filter_mode;
tass 70:cd218dd180e5 780 return 0;
tass 70:cd218dd180e5 781 }
tass 70:cd218dd180e5 782
tass 68:0847e35d08a6 783 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 784 {
tass 68:0847e35d08a6 785 struct pico_mcast_group *g = NULL, test = {0};
tass 68:0847e35d08a6 786 struct pico_ipv4_link *link = NULL;
tass 68:0847e35d08a6 787
tass 68:0847e35d08a6 788 if (mcast_link)
tass 68:0847e35d08a6 789 link = pico_ipv4_link_get(mcast_link);
tass 68:0847e35d08a6 790 else
tass 68:0847e35d08a6 791 link = mcast_default_link;
tass 68:0847e35d08a6 792
tass 68:0847e35d08a6 793 test.mcast_addr = *mcast_group;
tass 68:0847e35d08a6 794 g = pico_tree_findKey(link->MCASTGroups, &test);
tass 68:0847e35d08a6 795 if (g) {
tass 68:0847e35d08a6 796 if (reference_count)
tass 68:0847e35d08a6 797 g->reference_count++;
tass 68:0847e35d08a6 798 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
tass 68:0847e35d08a6 799 } else {
tass 68:0847e35d08a6 800 g = pico_zalloc(sizeof(struct pico_mcast_group));
tass 68:0847e35d08a6 801 if (!g) {
tass 68:0847e35d08a6 802 pico_err = PICO_ERR_ENOMEM;
tass 68:0847e35d08a6 803 return -1;
tass 68:0847e35d08a6 804 }
tass 68:0847e35d08a6 805 /* "non-existent" state of filter mode INCLUDE and empty source list */
tass 68:0847e35d08a6 806 g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
tass 68:0847e35d08a6 807 g->reference_count = 1;
tass 68:0847e35d08a6 808 g->mcast_addr = *mcast_group;
tass 68:0847e35d08a6 809 g->MCASTSources.root = &LEAF;
tass 68:0847e35d08a6 810 g->MCASTSources.compare = ipv4_mcast_sources_cmp;
tass 68:0847e35d08a6 811 pico_tree_insert(link->MCASTGroups, g);
tass 68:0847e35d08a6 812 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
tass 68:0847e35d08a6 813 }
tass 70:cd218dd180e5 814 if (mcast_group_update(g, MCASTFilter, filter_mode) < 0)
tass 70:cd218dd180e5 815 return -1;
tass 68:0847e35d08a6 816 pico_ipv4_mcast_print_groups(link);
tass 68:0847e35d08a6 817 return 0;
tass 68:0847e35d08a6 818 }
tass 68:0847e35d08a6 819
tass 68:0847e35d08a6 820 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 821 {
tass 68:0847e35d08a6 822
tass 68:0847e35d08a6 823 struct pico_mcast_group *g = NULL, test = {0};
tass 68:0847e35d08a6 824 struct pico_ipv4_link *link = NULL;
tass 68:0847e35d08a6 825 struct pico_tree_node *index = NULL, *_tmp = NULL;
tass 68:0847e35d08a6 826 struct pico_ip4 *source = NULL;
tass 68:0847e35d08a6 827
tass 68:0847e35d08a6 828 if (mcast_link)
tass 68:0847e35d08a6 829 link = pico_ipv4_link_get(mcast_link);
tass 68:0847e35d08a6 830 else
tass 68:0847e35d08a6 831 link = mcast_default_link;
tass 68:0847e35d08a6 832
tass 68:0847e35d08a6 833 test.mcast_addr = *mcast_group;
tass 68:0847e35d08a6 834 g = pico_tree_findKey(link->MCASTGroups, &test);
tass 68:0847e35d08a6 835 if (!g) {
tass 68:0847e35d08a6 836 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 837 return -1;
tass 68:0847e35d08a6 838 } else {
tass 68:0847e35d08a6 839 if (reference_count && (--(g->reference_count) < 1)) {
tass 68:0847e35d08a6 840 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
tass 68:0847e35d08a6 841 /* cleanup filter */
tass 68:0847e35d08a6 842 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
tass 68:0847e35d08a6 843 {
tass 68:0847e35d08a6 844 source = index->keyValue;
tass 68:0847e35d08a6 845 pico_tree_delete(&g->MCASTSources, source);
tass 68:0847e35d08a6 846 pico_free(source);
tass 68:0847e35d08a6 847 }
tass 68:0847e35d08a6 848 pico_tree_delete(link->MCASTGroups, g);
tass 68:0847e35d08a6 849 pico_free(g);
tass 68:0847e35d08a6 850 } else {
tass 68:0847e35d08a6 851 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
tass 70:cd218dd180e5 852 if (mcast_group_update(g, MCASTFilter, filter_mode) < 0)
tass 70:cd218dd180e5 853 return -1;
tass 68:0847e35d08a6 854 }
tass 68:0847e35d08a6 855 }
tass 68:0847e35d08a6 856
tass 68:0847e35d08a6 857 pico_ipv4_mcast_print_groups(link);
tass 68:0847e35d08a6 858 return 0;
tass 68:0847e35d08a6 859 }
tass 68:0847e35d08a6 860
tass 68:0847e35d08a6 861 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
tass 68:0847e35d08a6 862 {
tass 68:0847e35d08a6 863 return mcast_default_link;
tass 68:0847e35d08a6 864 }
tass 68:0847e35d08a6 865
tass 68:0847e35d08a6 866 static int pico_ipv4_mcast_filter(struct pico_frame *f)
tass 68:0847e35d08a6 867 {
tass 68:0847e35d08a6 868 struct pico_ipv4_link *link = NULL;
tass 68:0847e35d08a6 869 struct pico_tree_node *index = NULL, *index2 = NULL;
tass 68:0847e35d08a6 870 struct pico_mcast_group *g = NULL, test = {0};
tass 68:0847e35d08a6 871 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
tass 68:0847e35d08a6 872
tass 68:0847e35d08a6 873 test.mcast_addr = hdr->dst;
tass 68:0847e35d08a6 874
tass 68:0847e35d08a6 875 pico_tree_foreach(index, &Tree_dev_link)
tass 68:0847e35d08a6 876 {
tass 68:0847e35d08a6 877 link = index->keyValue;
tass 68:0847e35d08a6 878 g = pico_tree_findKey(link->MCASTGroups, &test);
tass 68:0847e35d08a6 879 if (g) {
tass 68:0847e35d08a6 880 if (f->dev == link->dev) {
tass 68:0847e35d08a6 881 ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
tass 68:0847e35d08a6 882 /* perform source filtering */
tass 68:0847e35d08a6 883 switch (g->filter_mode)
tass 68:0847e35d08a6 884 {
tass 68:0847e35d08a6 885 case PICO_IP_MULTICAST_INCLUDE:
tass 68:0847e35d08a6 886 pico_tree_foreach(index2, &g->MCASTSources)
tass 68:0847e35d08a6 887 {
tass 68:0847e35d08a6 888 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
tass 68:0847e35d08a6 889 ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
tass 68:0847e35d08a6 890 return 0;
tass 68:0847e35d08a6 891 }
tass 68:0847e35d08a6 892 }
tass 68:0847e35d08a6 893 ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
tass 68:0847e35d08a6 894 return -1;
tass 68:0847e35d08a6 895
tass 68:0847e35d08a6 896 case PICO_IP_MULTICAST_EXCLUDE:
tass 68:0847e35d08a6 897 pico_tree_foreach(index2, &g->MCASTSources)
tass 68:0847e35d08a6 898 {
tass 68:0847e35d08a6 899 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
tass 68:0847e35d08a6 900 ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
tass 68:0847e35d08a6 901 return -1;
tass 68:0847e35d08a6 902 }
tass 68:0847e35d08a6 903 }
tass 68:0847e35d08a6 904 ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
tass 68:0847e35d08a6 905 return 0;
tass 68:0847e35d08a6 906
tass 68:0847e35d08a6 907 default:
tass 68:0847e35d08a6 908 return -1;
tass 68:0847e35d08a6 909 }
tass 68:0847e35d08a6 910 } else {
tass 68:0847e35d08a6 911 ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name);
tass 68:0847e35d08a6 912 }
tass 68:0847e35d08a6 913 } else {
tass 68:0847e35d08a6 914 ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name);
tass 68:0847e35d08a6 915 }
tass 68:0847e35d08a6 916 }
tass 68:0847e35d08a6 917 return -1;
tass 68:0847e35d08a6 918 }
tass 68:0847e35d08a6 919
tass 68:0847e35d08a6 920 #else
tass 68:0847e35d08a6 921
tass 68:0847e35d08a6 922 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 923 {
tass 68:0847e35d08a6 924 pico_err = PICO_ERR_EPROTONOSUPPORT;
tass 68:0847e35d08a6 925 return -1;
tass 68:0847e35d08a6 926 }
tass 68:0847e35d08a6 927 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 928 {
tass 68:0847e35d08a6 929 pico_err = PICO_ERR_EPROTONOSUPPORT;
tass 68:0847e35d08a6 930 return -1;
tass 68:0847e35d08a6 931 }
tass 68:0847e35d08a6 932 struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
tass 68:0847e35d08a6 933 {
tass 68:0847e35d08a6 934 pico_err = PICO_ERR_EPROTONOSUPPORT;
tass 68:0847e35d08a6 935 return NULL;
tass 68:0847e35d08a6 936 }
tass 68:0847e35d08a6 937 #endif /* PICO_SUPPORT_MCAST */
tass 68:0847e35d08a6 938
tass 88:0e827d0d8017 939 #ifdef DEBUG_ROUTE
tass 88:0e827d0d8017 940 static void dbg_route(void)
tass 88:0e827d0d8017 941 {
tass 88:0e827d0d8017 942 struct pico_ipv4_route *r;
tass 88:0e827d0d8017 943 struct pico_tree_node * index;
tass 88:0e827d0d8017 944 pico_tree_foreach(index,&Routes){
tass 88:0e827d0d8017 945 r = index->keyValue;
tass 88:0e827d0d8017 946 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 947 }
tass 88:0e827d0d8017 948 }
tass 88:0e827d0d8017 949 #else
tass 88:0e827d0d8017 950 #define dbg_route() do{ }while(0)
tass 88:0e827d0d8017 951 #endif
tass 88:0e827d0d8017 952
tass 68:0847e35d08a6 953 int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto)
tass 68:0847e35d08a6 954 {
tass 68:0847e35d08a6 955
tass 68:0847e35d08a6 956 struct pico_ipv4_route *route;
tass 68:0847e35d08a6 957 struct pico_ipv4_link *link;
tass 68:0847e35d08a6 958 struct pico_ipv4_hdr *hdr;
tass 68:0847e35d08a6 959 uint8_t ttl = PICO_IPV4_DEFAULT_TTL;
tass 68:0847e35d08a6 960 uint8_t vhl = 0x45; /* version 4, header length 20 */
tass 68:0847e35d08a6 961 static uint16_t ipv4_progressive_id = 0x91c0;
tass 68:0847e35d08a6 962 #ifdef PICO_SUPPORT_MCAST
tass 68:0847e35d08a6 963 struct pico_tree_node *index;
tass 68:0847e35d08a6 964 #endif
tass 68:0847e35d08a6 965
tass 68:0847e35d08a6 966 if(!f || !dst) {
tass 68:0847e35d08a6 967 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 968 return -1;
tass 68:0847e35d08a6 969 }
tass 68:0847e35d08a6 970 hdr = (struct pico_ipv4_hdr *) f->net_hdr;
tass 68:0847e35d08a6 971 if (!hdr) {
tass 68:0847e35d08a6 972 dbg("IP header error\n");
tass 68:0847e35d08a6 973 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 974 goto drop;
tass 68:0847e35d08a6 975 }
tass 68:0847e35d08a6 976
tass 68:0847e35d08a6 977 if (dst->addr == 0) {
tass 68:0847e35d08a6 978 dbg("IP destination addr error\n");
tass 68:0847e35d08a6 979 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 980 goto drop;
tass 68:0847e35d08a6 981 }
tass 68:0847e35d08a6 982
tass 68:0847e35d08a6 983 route = route_find(dst);
tass 68:0847e35d08a6 984 if (!route) {
tass 88:0e827d0d8017 985 //dbg("Route to %08x not found.\n", long_be(dst->addr));
tass 88:0e827d0d8017 986
tass 88:0e827d0d8017 987
tass 68:0847e35d08a6 988 pico_err = PICO_ERR_EHOSTUNREACH;
tass 68:0847e35d08a6 989 goto drop;
tass 68:0847e35d08a6 990 } else {
tass 68:0847e35d08a6 991 link = route->link;
tass 68:0847e35d08a6 992 #ifdef PICO_SUPPORT_MCAST
tass 68:0847e35d08a6 993 if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */
tass 68:0847e35d08a6 994 switch (proto) {
tass 68:0847e35d08a6 995 case PICO_PROTO_UDP:
tass 68:0847e35d08a6 996 if(pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
tass 68:0847e35d08a6 997 ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
tass 68:0847e35d08a6 998 break;
tass 68:0847e35d08a6 999 case PICO_PROTO_IGMP:
tass 68:0847e35d08a6 1000 vhl = 0x46; /* header length 24 */
tass 68:0847e35d08a6 1001 ttl = 1;
tass 68:0847e35d08a6 1002 /* router alert (RFC 2113) */
tass 68:0847e35d08a6 1003 hdr->options[0] = 0x94;
tass 68:0847e35d08a6 1004 hdr->options[1] = 0x04;
tass 68:0847e35d08a6 1005 hdr->options[2] = 0x00;
tass 68:0847e35d08a6 1006 hdr->options[3] = 0x00;
tass 68:0847e35d08a6 1007 if (f->dev && link->dev != f->dev) { /* default link is not requested link */
tass 68:0847e35d08a6 1008 pico_tree_foreach(index, &Tree_dev_link) {
tass 68:0847e35d08a6 1009 link = index->keyValue;
tass 68:0847e35d08a6 1010 if (link->dev == f->dev)
tass 68:0847e35d08a6 1011 break;
tass 68:0847e35d08a6 1012 }
tass 68:0847e35d08a6 1013 }
tass 68:0847e35d08a6 1014 break;
tass 68:0847e35d08a6 1015 default:
tass 68:0847e35d08a6 1016 ttl = PICO_IPV4_DEFAULT_TTL;
tass 68:0847e35d08a6 1017 }
tass 68:0847e35d08a6 1018 }
tass 68:0847e35d08a6 1019 #endif
tass 68:0847e35d08a6 1020 }
tass 68:0847e35d08a6 1021
tass 68:0847e35d08a6 1022 hdr->vhl = vhl;
tass 70:cd218dd180e5 1023 hdr->len = short_be((uint16_t)(f->transport_len + f->net_len));
tass 68:0847e35d08a6 1024 if (f->transport_hdr != f->payload)
tass 68:0847e35d08a6 1025 ipv4_progressive_id++;
tass 68:0847e35d08a6 1026 hdr->id = short_be(ipv4_progressive_id);
tass 68:0847e35d08a6 1027 hdr->dst.addr = dst->addr;
tass 68:0847e35d08a6 1028 hdr->src.addr = link->address.addr;
tass 68:0847e35d08a6 1029 hdr->ttl = ttl;
tass 68:0847e35d08a6 1030 hdr->proto = proto;
tass 68:0847e35d08a6 1031 hdr->frag = short_be(PICO_IPV4_DONTFRAG);
tass 68:0847e35d08a6 1032 #ifdef PICO_SUPPORT_IPFRAG
tass 68:0847e35d08a6 1033 # ifdef PICO_SUPPORT_UDP
tass 68:0847e35d08a6 1034 if (proto == PICO_PROTO_UDP) {
tass 68:0847e35d08a6 1035 /* first fragment, can not use transport_len to calculate IP length */
tass 68:0847e35d08a6 1036 if (f->transport_hdr != f->payload)
tass 70:cd218dd180e5 1037 hdr->len = short_be((uint16_t)(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len));
tass 68:0847e35d08a6 1038 /* set fragmentation flags and offset calculated in socket layer */
tass 68:0847e35d08a6 1039 hdr->frag = f->frag;
tass 68:0847e35d08a6 1040 }
tass 68:0847e35d08a6 1041 # endif /* PICO_SUPPORT_UDP */
tass 68:0847e35d08a6 1042 #endif /* PICO_SUPPORT_IPFRAG */
tass 68:0847e35d08a6 1043 pico_ipv4_checksum(f);
tass 68:0847e35d08a6 1044
tass 68:0847e35d08a6 1045 if (f->sock && f->sock->dev){
tass 68:0847e35d08a6 1046 //if the socket has its device set, use that (currently used for DHCP)
tass 68:0847e35d08a6 1047 f->dev = f->sock->dev;
tass 68:0847e35d08a6 1048 } else {
tass 68:0847e35d08a6 1049 f->dev = link->dev;
tass 68:0847e35d08a6 1050 }
tass 68:0847e35d08a6 1051
tass 68:0847e35d08a6 1052 #ifdef PICO_SUPPORT_MCAST
tass 68:0847e35d08a6 1053 if (pico_ipv4_is_multicast(hdr->dst.addr)) {
tass 68:0847e35d08a6 1054 struct pico_frame *cpy;
tass 68:0847e35d08a6 1055 /* Sending UDP multicast datagram, am I member? If so, loopback copy */
tass 68:0847e35d08a6 1056 if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) {
tass 68:0847e35d08a6 1057 ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n");
tass 68:0847e35d08a6 1058 cpy = pico_frame_copy(f);
tass 68:0847e35d08a6 1059 pico_enqueue(&in, cpy);
tass 68:0847e35d08a6 1060 }
tass 68:0847e35d08a6 1061 }
tass 68:0847e35d08a6 1062 #endif
tass 68:0847e35d08a6 1063
tass 68:0847e35d08a6 1064 if(pico_ipv4_link_get(&hdr->dst)){
tass 68:0847e35d08a6 1065 //it's our own IP
tass 68:0847e35d08a6 1066 return pico_enqueue(&in, f);
tass 68:0847e35d08a6 1067 }else{
tass 68:0847e35d08a6 1068 /* TODO: Check if there are members subscribed here */
tass 68:0847e35d08a6 1069 return pico_enqueue(&out, f);
tass 68:0847e35d08a6 1070 }
tass 68:0847e35d08a6 1071
tass 68:0847e35d08a6 1072 drop:
tass 68:0847e35d08a6 1073 pico_frame_discard(f);
tass 68:0847e35d08a6 1074 return -1;
tass 68:0847e35d08a6 1075 }
tass 68:0847e35d08a6 1076
tass 68:0847e35d08a6 1077
tass 68:0847e35d08a6 1078 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f)
tass 68:0847e35d08a6 1079 {
tass 68:0847e35d08a6 1080 struct pico_ip4 *dst;
tass 68:0847e35d08a6 1081 struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
tass 68:0847e35d08a6 1082 IGNORE_PARAMETER(self);
tass 68:0847e35d08a6 1083
tass 68:0847e35d08a6 1084 if (!f->sock) {
tass 68:0847e35d08a6 1085 pico_frame_discard(f);
tass 68:0847e35d08a6 1086 return -1;
tass 68:0847e35d08a6 1087 }
tass 68:0847e35d08a6 1088
tass 68:0847e35d08a6 1089 if (remote_duple) {
tass 68:0847e35d08a6 1090 dst = &remote_duple->remote_addr.ip4;
tass 68:0847e35d08a6 1091 } else {
tass 68:0847e35d08a6 1092 dst = &f->sock->remote_addr.ip4;
tass 68:0847e35d08a6 1093 }
tass 68:0847e35d08a6 1094
tass 70:cd218dd180e5 1095 return pico_ipv4_frame_push(f, dst, (uint8_t)f->sock->proto->proto_number);
tass 68:0847e35d08a6 1096 }
tass 68:0847e35d08a6 1097
tass 68:0847e35d08a6 1098
tass 68:0847e35d08a6 1099 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 1100 {
tass 68:0847e35d08a6 1101 struct pico_ipv4_route test, *new;
tass 68:0847e35d08a6 1102 test.dest.addr = address.addr;
tass 68:0847e35d08a6 1103 test.netmask.addr = netmask.addr;
tass 70:cd218dd180e5 1104 test.metric = (uint32_t)metric;
tass 68:0847e35d08a6 1105
tass 68:0847e35d08a6 1106 if(pico_tree_findKey(&Routes,&test)){
tass 68:0847e35d08a6 1107 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1108 return -1;
tass 68:0847e35d08a6 1109 }
tass 68:0847e35d08a6 1110
tass 68:0847e35d08a6 1111 new = pico_zalloc(sizeof(struct pico_ipv4_route));
tass 68:0847e35d08a6 1112 if (!new) {
tass 68:0847e35d08a6 1113 pico_err = PICO_ERR_ENOMEM;
tass 68:0847e35d08a6 1114 return -1;
tass 68:0847e35d08a6 1115 }
tass 68:0847e35d08a6 1116 new->dest.addr = address.addr;
tass 68:0847e35d08a6 1117 new->netmask.addr = netmask.addr;
tass 68:0847e35d08a6 1118 new->gateway.addr = gateway.addr;
tass 70:cd218dd180e5 1119 new->metric = (uint32_t)metric;
tass 68:0847e35d08a6 1120 if (gateway.addr == 0) {
tass 68:0847e35d08a6 1121 /* No gateway provided, use the link */
tass 68:0847e35d08a6 1122 new->link = link;
tass 68:0847e35d08a6 1123 } else {
tass 68:0847e35d08a6 1124 struct pico_ipv4_route *r = route_find(&gateway);
tass 68:0847e35d08a6 1125 if (!r ) { /* Specified Gateway is unreachable */
tass 68:0847e35d08a6 1126 pico_err = PICO_ERR_EHOSTUNREACH;
tass 68:0847e35d08a6 1127 pico_free(new);
tass 68:0847e35d08a6 1128 return -1;
tass 68:0847e35d08a6 1129 }
tass 68:0847e35d08a6 1130 if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
tass 68:0847e35d08a6 1131 pico_err = PICO_ERR_ENETUNREACH;
tass 68:0847e35d08a6 1132 pico_free(new);
tass 68:0847e35d08a6 1133 return -1;
tass 68:0847e35d08a6 1134 }
tass 68:0847e35d08a6 1135 new->link = r->link;
tass 68:0847e35d08a6 1136 }
tass 68:0847e35d08a6 1137 if (!new->link) {
tass 68:0847e35d08a6 1138 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1139 pico_free(new);
tass 68:0847e35d08a6 1140 return -1;
tass 68:0847e35d08a6 1141 }
tass 68:0847e35d08a6 1142
tass 68:0847e35d08a6 1143 pico_tree_insert(&Routes,new);
tass 88:0e827d0d8017 1144 //dbg_route();
tass 68:0847e35d08a6 1145 return 0;
tass 68:0847e35d08a6 1146 }
tass 68:0847e35d08a6 1147
tass 72:887bc44746ff 1148 int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, int metric)
tass 68:0847e35d08a6 1149 {
tass 72:887bc44746ff 1150 struct pico_ipv4_route test, *found;
tass 68:0847e35d08a6 1151
tass 68:0847e35d08a6 1152 test.dest.addr = address.addr;
tass 68:0847e35d08a6 1153 test.netmask.addr = netmask.addr;
tass 70:cd218dd180e5 1154 test.metric = (uint32_t)metric;
tass 68:0847e35d08a6 1155
tass 68:0847e35d08a6 1156 found = pico_tree_findKey(&Routes,&test);
tass 68:0847e35d08a6 1157 if (found) {
tass 68:0847e35d08a6 1158
tass 68:0847e35d08a6 1159 pico_tree_delete(&Routes,found);
tass 68:0847e35d08a6 1160 pico_free(found);
tass 68:0847e35d08a6 1161
tass 88:0e827d0d8017 1162 //dbg_route();
tass 68:0847e35d08a6 1163 return 0;
tass 68:0847e35d08a6 1164 }
tass 68:0847e35d08a6 1165 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1166 return -1;
tass 68:0847e35d08a6 1167 }
tass 68:0847e35d08a6 1168
tass 68:0847e35d08a6 1169
tass 68:0847e35d08a6 1170 int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
tass 68:0847e35d08a6 1171 {
tass 68:0847e35d08a6 1172 struct pico_ipv4_link test, *new;
tass 68:0847e35d08a6 1173 struct pico_ip4 network, gateway;
tass 68:0847e35d08a6 1174 char ipstr[30];
tass 68:0847e35d08a6 1175
tass 68:0847e35d08a6 1176 if(!dev) {
tass 68:0847e35d08a6 1177 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1178 return -1;
tass 68:0847e35d08a6 1179 }
tass 68:0847e35d08a6 1180 test.address.addr = address.addr;
tass 68:0847e35d08a6 1181 test.netmask.addr = netmask.addr;
tass 68:0847e35d08a6 1182 test.dev = dev;
tass 68:0847e35d08a6 1183 /** XXX: Valid netmask / unicast address test **/
tass 68:0847e35d08a6 1184
tass 68:0847e35d08a6 1185 if(pico_tree_findKey(&Tree_dev_link, &test)) {
tass 68:0847e35d08a6 1186 dbg("IPv4: Trying to assign an invalid address (in use)\n");
tass 68:0847e35d08a6 1187 pico_err = PICO_ERR_EADDRINUSE;
tass 68:0847e35d08a6 1188 return -1;
tass 68:0847e35d08a6 1189 }
tass 68:0847e35d08a6 1190
tass 68:0847e35d08a6 1191 /** 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 1192 new = pico_zalloc(sizeof(struct pico_ipv4_link));
tass 68:0847e35d08a6 1193 if (!new) {
tass 68:0847e35d08a6 1194 dbg("IPv4: Out of memory!\n");
tass 68:0847e35d08a6 1195 pico_err = PICO_ERR_ENOMEM;
tass 68:0847e35d08a6 1196 return -1;
tass 68:0847e35d08a6 1197 }
tass 68:0847e35d08a6 1198 new->address.addr = address.addr;
tass 68:0847e35d08a6 1199 new->netmask.addr = netmask.addr;
tass 68:0847e35d08a6 1200 new->dev = dev;
tass 68:0847e35d08a6 1201 #ifdef PICO_SUPPORT_MCAST
tass 68:0847e35d08a6 1202 new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree));
tass 68:0847e35d08a6 1203 if (!new->MCASTGroups) {
tass 68:0847e35d08a6 1204 pico_free(new);
tass 68:0847e35d08a6 1205 dbg("IPv4: Out of memory!\n");
tass 68:0847e35d08a6 1206 pico_err = PICO_ERR_ENOMEM;
tass 68:0847e35d08a6 1207 return -1;
tass 68:0847e35d08a6 1208 }
tass 68:0847e35d08a6 1209
tass 68:0847e35d08a6 1210 new->MCASTGroups->root = &LEAF;
tass 68:0847e35d08a6 1211 new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
tass 68:0847e35d08a6 1212 new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
tass 68:0847e35d08a6 1213 new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
tass 68:0847e35d08a6 1214 #endif
tass 68:0847e35d08a6 1215
tass 68:0847e35d08a6 1216 pico_tree_insert(&Tree_dev_link, new);
tass 68:0847e35d08a6 1217 #ifdef PICO_SUPPORT_MCAST
tass 68:0847e35d08a6 1218 do {
tass 68:0847e35d08a6 1219 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
tass 68:0847e35d08a6 1220 if (!mcast_default_link) {
tass 68:0847e35d08a6 1221 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
tass 68:0847e35d08a6 1222 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
tass 68:0847e35d08a6 1223 mcast_gw.addr = long_be(0x00000000);
tass 68:0847e35d08a6 1224 mcast_default_link = new;
tass 68:0847e35d08a6 1225 pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
tass 68:0847e35d08a6 1226 }
tass 68:0847e35d08a6 1227 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
tass 68:0847e35d08a6 1228 pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
tass 68:0847e35d08a6 1229 } while(0);
tass 68:0847e35d08a6 1230 #endif
tass 68:0847e35d08a6 1231
tass 68:0847e35d08a6 1232 network.addr = address.addr & netmask.addr;
tass 68:0847e35d08a6 1233 gateway.addr = 0U;
tass 68:0847e35d08a6 1234 pico_ipv4_route_add(network, netmask, gateway, 1, new);
tass 68:0847e35d08a6 1235 pico_ipv4_to_string(ipstr, new->address.addr);
tass 68:0847e35d08a6 1236 dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
tass 68:0847e35d08a6 1237 return 0;
tass 68:0847e35d08a6 1238 }
tass 68:0847e35d08a6 1239
tass 72:887bc44746ff 1240 static int pico_ipv4_cleanup_routes(struct pico_ipv4_link *link)
tass 72:887bc44746ff 1241 {
tass 72:887bc44746ff 1242 struct pico_tree_node *index = NULL, *tmp = NULL;
tass 72:887bc44746ff 1243 struct pico_ipv4_route *route = NULL;
tass 72:887bc44746ff 1244
tass 72:887bc44746ff 1245 pico_tree_foreach_safe(index, &Routes, tmp)
tass 72:887bc44746ff 1246 {
tass 72:887bc44746ff 1247 route = index->keyValue;
tass 72:887bc44746ff 1248 if (link == route->link)
tass 73:dfb737147f6e 1249 pico_ipv4_route_del(route->dest, route->netmask, (int)route->metric);
tass 72:887bc44746ff 1250 }
tass 72:887bc44746ff 1251 return 0;
tass 72:887bc44746ff 1252 }
tass 68:0847e35d08a6 1253
tass 68:0847e35d08a6 1254 int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
tass 68:0847e35d08a6 1255 {
tass 68:0847e35d08a6 1256 struct pico_ipv4_link test, *found;
tass 68:0847e35d08a6 1257
tass 68:0847e35d08a6 1258 if(!dev) {
tass 68:0847e35d08a6 1259 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1260 return -1;
tass 68:0847e35d08a6 1261 }
tass 68:0847e35d08a6 1262 test.address.addr = address.addr;
tass 68:0847e35d08a6 1263 test.dev = dev;
tass 68:0847e35d08a6 1264 found = pico_tree_findKey(&Tree_dev_link, &test);
tass 68:0847e35d08a6 1265 if (!found) {
tass 68:0847e35d08a6 1266 pico_err = PICO_ERR_ENXIO;
tass 68:0847e35d08a6 1267 return -1;
tass 68:0847e35d08a6 1268 }
tass 68:0847e35d08a6 1269
tass 68:0847e35d08a6 1270 #ifdef PICO_SUPPORT_MCAST
tass 68:0847e35d08a6 1271 do {
tass 72:887bc44746ff 1272 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm;
tass 68:0847e35d08a6 1273 struct pico_mcast_group *g = NULL;
tass 68:0847e35d08a6 1274 struct pico_tree_node * index, * _tmp;
tass 68:0847e35d08a6 1275 if (found == mcast_default_link) {
tass 68:0847e35d08a6 1276 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
tass 68:0847e35d08a6 1277 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
tass 68:0847e35d08a6 1278 mcast_default_link = NULL;
tass 72:887bc44746ff 1279 pico_ipv4_route_del(mcast_addr, mcast_nm, 1);
tass 68:0847e35d08a6 1280 }
tass 68:0847e35d08a6 1281 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
tass 68:0847e35d08a6 1282 pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
tass 68:0847e35d08a6 1283 pico_tree_foreach_safe(index,found->MCASTGroups, _tmp)
tass 68:0847e35d08a6 1284 {
tass 68:0847e35d08a6 1285 g = index->keyValue;
tass 68:0847e35d08a6 1286 pico_tree_delete(found->MCASTGroups, g);
tass 68:0847e35d08a6 1287 pico_free(g);
tass 68:0847e35d08a6 1288 }
tass 68:0847e35d08a6 1289 } while(0);
tass 68:0847e35d08a6 1290 #endif
tass 68:0847e35d08a6 1291
tass 72:887bc44746ff 1292 pico_ipv4_cleanup_routes(found);
tass 68:0847e35d08a6 1293 pico_tree_delete(&Tree_dev_link, found);
tass 72:887bc44746ff 1294 pico_free(found);
tass 72:887bc44746ff 1295
tass 68:0847e35d08a6 1296 return 0;
tass 68:0847e35d08a6 1297 }
tass 68:0847e35d08a6 1298
tass 68:0847e35d08a6 1299
tass 68:0847e35d08a6 1300 struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address)
tass 68:0847e35d08a6 1301 {
tass 68:0847e35d08a6 1302 struct pico_ipv4_link test = {0}, *found = NULL;
tass 68:0847e35d08a6 1303 test.address.addr = address->addr;
tass 68:0847e35d08a6 1304
tass 68:0847e35d08a6 1305 found = pico_tree_findKey(&Tree_dev_link, &test);
tass 68:0847e35d08a6 1306 if (!found)
tass 68:0847e35d08a6 1307 return NULL;
tass 68:0847e35d08a6 1308 else
tass 68:0847e35d08a6 1309 return found;
tass 68:0847e35d08a6 1310 }
tass 68:0847e35d08a6 1311
tass 68:0847e35d08a6 1312 struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev)
tass 68:0847e35d08a6 1313 {
tass 68:0847e35d08a6 1314 struct pico_tree_node *index = NULL;
tass 68:0847e35d08a6 1315 struct pico_ipv4_link *link = NULL;
tass 68:0847e35d08a6 1316
tass 68:0847e35d08a6 1317 pico_tree_foreach(index, &Tree_dev_link)
tass 68:0847e35d08a6 1318 {
tass 68:0847e35d08a6 1319 link = index->keyValue;
tass 68:0847e35d08a6 1320 if (link->dev == dev)
tass 68:0847e35d08a6 1321 return link;
tass 68:0847e35d08a6 1322 }
tass 68:0847e35d08a6 1323 return NULL;
tass 68:0847e35d08a6 1324 }
tass 68:0847e35d08a6 1325
tass 74:c146c4e346c4 1326 struct pico_ipv4_link *pico_ipv4_link_by_dev_next(struct pico_device *dev, struct pico_ipv4_link *last)
tass 74:c146c4e346c4 1327 {
tass 74:c146c4e346c4 1328 struct pico_tree_node *index = NULL;
tass 74:c146c4e346c4 1329 struct pico_ipv4_link *link = NULL;
tass 74:c146c4e346c4 1330 int valid = 0;
tass 74:c146c4e346c4 1331
tass 74:c146c4e346c4 1332 if (last == NULL)
tass 74:c146c4e346c4 1333 valid = 1;
tass 74:c146c4e346c4 1334
tass 74:c146c4e346c4 1335 pico_tree_foreach(index, &Tree_dev_link)
tass 74:c146c4e346c4 1336 {
tass 74:c146c4e346c4 1337 link = index->keyValue;
tass 74:c146c4e346c4 1338 if (link->dev == dev) {
tass 74:c146c4e346c4 1339 if (last == link)
tass 74:c146c4e346c4 1340 valid = 1;
tass 74:c146c4e346c4 1341 else if (valid > 0)
tass 74:c146c4e346c4 1342 return link;
tass 74:c146c4e346c4 1343 }
tass 74:c146c4e346c4 1344 }
tass 74:c146c4e346c4 1345 return NULL;
tass 74:c146c4e346c4 1346 }
tass 68:0847e35d08a6 1347
tass 68:0847e35d08a6 1348 struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
tass 68:0847e35d08a6 1349 {
tass 68:0847e35d08a6 1350 struct pico_ipv4_link test, *found;
tass 68:0847e35d08a6 1351 if(!address) {
tass 68:0847e35d08a6 1352 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1353 return NULL;
tass 68:0847e35d08a6 1354 }
tass 68:0847e35d08a6 1355 test.dev = NULL;
tass 68:0847e35d08a6 1356 test.address.addr = address->addr;
tass 68:0847e35d08a6 1357 found = pico_tree_findKey(&Tree_dev_link, &test);
tass 68:0847e35d08a6 1358 if (!found) {
tass 68:0847e35d08a6 1359 pico_err = PICO_ERR_ENXIO;
tass 68:0847e35d08a6 1360 return NULL;
tass 68:0847e35d08a6 1361 }
tass 68:0847e35d08a6 1362 return found->dev;
tass 68:0847e35d08a6 1363 }
tass 68:0847e35d08a6 1364
tass 68:0847e35d08a6 1365 int pico_ipv4_rebound(struct pico_frame *f)
tass 68:0847e35d08a6 1366 {
tass 68:0847e35d08a6 1367 struct pico_ip4 dst;
tass 68:0847e35d08a6 1368 struct pico_ipv4_hdr *hdr;
tass 68:0847e35d08a6 1369 if(!f) {
tass 68:0847e35d08a6 1370 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1371 return -1;
tass 68:0847e35d08a6 1372 }
tass 68:0847e35d08a6 1373
tass 68:0847e35d08a6 1374 hdr = (struct pico_ipv4_hdr *) f->net_hdr;
tass 68:0847e35d08a6 1375 if (!hdr) {
tass 68:0847e35d08a6 1376 pico_err = PICO_ERR_EINVAL;
tass 68:0847e35d08a6 1377 return -1;
tass 68:0847e35d08a6 1378 }
tass 68:0847e35d08a6 1379 dst.addr = hdr->src.addr;
tass 68:0847e35d08a6 1380 return pico_ipv4_frame_push(f, &dst, hdr->proto);
tass 68:0847e35d08a6 1381 }
tass 68:0847e35d08a6 1382
tass 68:0847e35d08a6 1383 static int pico_ipv4_forward(struct pico_frame *f)
tass 68:0847e35d08a6 1384 {
tass 68:0847e35d08a6 1385 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
tass 68:0847e35d08a6 1386 struct pico_ipv4_route *rt;
tass 68:0847e35d08a6 1387 if (!hdr) {
tass 68:0847e35d08a6 1388 return -1;
tass 68:0847e35d08a6 1389 }
tass 68:0847e35d08a6 1390
tass 68:0847e35d08a6 1391 rt = route_find(&hdr->dst);
tass 68:0847e35d08a6 1392 if (!rt) {
tass 68:0847e35d08a6 1393 pico_notify_dest_unreachable(f);
tass 68:0847e35d08a6 1394 return -1;
tass 68:0847e35d08a6 1395 }
tass 105:47c74c5d3d15 1396
tass 105:47c74c5d3d15 1397 if (f->dev == rt->link->dev)
tass 105:47c74c5d3d15 1398 return -1;
tass 105:47c74c5d3d15 1399
tass 68:0847e35d08a6 1400 f->dev = rt->link->dev;
tass 73:dfb737147f6e 1401 hdr->ttl= (uint8_t)(hdr->ttl - 1);
tass 68:0847e35d08a6 1402 if (hdr->ttl < 1) {
tass 68:0847e35d08a6 1403 pico_notify_ttl_expired(f);
tass 88:0e827d0d8017 1404 dbg(" ------------------- TTL EXPIRED\n");
tass 68:0847e35d08a6 1405 return -1;
tass 68:0847e35d08a6 1406 }
tass 68:0847e35d08a6 1407 hdr->crc++;
tass 68:0847e35d08a6 1408
tass 68:0847e35d08a6 1409 pico_ipv4_nat_outbound(f, &rt->link->address);
tass 68:0847e35d08a6 1410
tass 68:0847e35d08a6 1411 f->start = f->net_hdr;
tass 68:0847e35d08a6 1412 if(f->dev->eth != NULL)
tass 68:0847e35d08a6 1413 f->len -= PICO_SIZE_ETHHDR;
tass 68:0847e35d08a6 1414 pico_sendto_dev(f);
tass 68:0847e35d08a6 1415 return 0;
tass 68:0847e35d08a6 1416
tass 68:0847e35d08a6 1417 }
tass 68:0847e35d08a6 1418
tass 68:0847e35d08a6 1419 int pico_ipv4_is_broadcast(uint32_t addr)
tass 68:0847e35d08a6 1420 {
tass 68:0847e35d08a6 1421 struct pico_ipv4_link *link;
tass 68:0847e35d08a6 1422 struct pico_tree_node * index;
tass 68:0847e35d08a6 1423 if (addr == PICO_IP4_ANY)
tass 68:0847e35d08a6 1424 return 1;
tass 68:0847e35d08a6 1425 if (addr == PICO_IP4_BCAST)
tass 68:0847e35d08a6 1426 return 1;
tass 68:0847e35d08a6 1427
tass 68:0847e35d08a6 1428 pico_tree_foreach(index,&Tree_dev_link) {
tass 68:0847e35d08a6 1429 link = index->keyValue;
tass 68:0847e35d08a6 1430 if ((link->address.addr | (~link->netmask.addr)) == addr)
tass 68:0847e35d08a6 1431 return 1;
tass 68:0847e35d08a6 1432 }
tass 68:0847e35d08a6 1433 return 0;
tass 68:0847e35d08a6 1434 }
tass 68:0847e35d08a6 1435
tass 68:0847e35d08a6 1436 void pico_ipv4_unreachable(struct pico_frame *f, int err)
tass 68:0847e35d08a6 1437 {
tass 68:0847e35d08a6 1438 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
tass 68:0847e35d08a6 1439 #if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP
tass 68:0847e35d08a6 1440 f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR;
tass 68:0847e35d08a6 1441 pico_transport_error(f, hdr->proto, err);
tass 68:0847e35d08a6 1442 #endif
tass 68:0847e35d08a6 1443 }
tass 68:0847e35d08a6 1444
tass 68:0847e35d08a6 1445 #endif