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@111:4003cf17bc15, 2013-11-04 (annotated)

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