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_olsr.c@91:8da42f2c4820, 2013-10-10 (annotated)

Committer:
tass
Date:
Thu Oct 10 12:16:48 2013 +0000
Revision:
91:8da42f2c4820
Parent:
88:0e827d0d8017
Child:
123:dd26752a4538
Fix Issue #37

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tass 74:c146c4e346c4 1 /*********************************************************************
tass 74:c146c4e346c4 2 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
tass 74:c146c4e346c4 3 See LICENSE and COPYING for usage.
tass 74:c146c4e346c4 4
tass 74:c146c4e346c4 5 Authors: Daniele Lacamera
tass 74:c146c4e346c4 6 *********************************************************************/
tass 74:c146c4e346c4 7
tass 74:c146c4e346c4 8 #include "pico_stack.h"
tass 74:c146c4e346c4 9 #include "pico_config.h"
tass 74:c146c4e346c4 10 #include "pico_device.h"
tass 74:c146c4e346c4 11 #include "pico_ipv4.h"
tass 74:c146c4e346c4 12 #include "pico_arp.h"
tass 74:c146c4e346c4 13 #include "pico_socket.h"
tass 74:c146c4e346c4 14 #ifdef PICO_SUPPORT_OLSR
tass 88:0e827d0d8017 15 #define DGRAM_MAX_SIZE (576)
tass 88:0e827d0d8017 16 #define MAX_OLSR_MEM (4 * DGRAM_MAX_SIZE)
tass 74:c146c4e346c4 17
tass 74:c146c4e346c4 18
tass 88:0e827d0d8017 19 #define OLSR_HELLO_INTERVAL ((uint32_t)2000)
tass 88:0e827d0d8017 20 #define OLSR_TC_INTERVAL ((uint32_t)5000)
tass 88:0e827d0d8017 21 #define OLSR_MAXJITTER ((uint32_t)(OLSR_HELLO_INTERVAL >> 2))
tass 74:c146c4e346c4 22 static const struct pico_ip4 HOST_NETMASK = { 0xFFFFFFFF };
tass 74:c146c4e346c4 23 #ifndef MIN
tass 74:c146c4e346c4 24 # define MIN(a,b) (a<b?a:b)
tass 74:c146c4e346c4 25 #endif
tass 74:c146c4e346c4 26
tass 74:c146c4e346c4 27 #define fresher(a,b) ((a>b) || ((b - a) > 32768))
tass 74:c146c4e346c4 28
tass 88:0e827d0d8017 29
tass 74:c146c4e346c4 30 /* Objects */
tass 74:c146c4e346c4 31 struct olsr_route_entry
tass 74:c146c4e346c4 32 {
tass 88:0e827d0d8017 33 struct olsr_route_entry *next;
tass 88:0e827d0d8017 34 uint32_t time_left;
tass 88:0e827d0d8017 35 struct pico_ip4 destination;
tass 88:0e827d0d8017 36 struct olsr_route_entry *gateway;
tass 88:0e827d0d8017 37 struct pico_device *iface;
tass 88:0e827d0d8017 38 uint16_t metric;
tass 88:0e827d0d8017 39 uint8_t link_type;
tass 88:0e827d0d8017 40 struct olsr_route_entry *children;
tass 88:0e827d0d8017 41 uint16_t ansn;
tass 88:0e827d0d8017 42 uint16_t seq;
tass 88:0e827d0d8017 43 uint8_t lq, nlq;
tass 88:0e827d0d8017 44 uint8_t *advertised_tc;
tass 74:c146c4e346c4 45 };
tass 74:c146c4e346c4 46
tass 74:c146c4e346c4 47 struct olsr_dev_entry
tass 74:c146c4e346c4 48 {
tass 74:c146c4e346c4 49 struct olsr_dev_entry *next;
tass 74:c146c4e346c4 50 struct pico_device *dev;
tass 88:0e827d0d8017 51 uint16_t pkt_counter;
tass 74:c146c4e346c4 52 };
tass 74:c146c4e346c4 53
tass 74:c146c4e346c4 54
tass 74:c146c4e346c4 55 /* OLSR Protocol */
tass 88:0e827d0d8017 56 #define OLSRMSG_HELLO 0xc9
tass 88:0e827d0d8017 57 #define OLSRMSG_MID 0x03
tass 88:0e827d0d8017 58 #define OLSRMSG_TC 0xca
tass 74:c146c4e346c4 59
tass 74:c146c4e346c4 60 #define OLSRLINK_SYMMETRIC 0x06
tass 74:c146c4e346c4 61 #define OLSRLINK_UNKNOWN 0x08
tass 88:0e827d0d8017 62 #define OLSRLINK_MPR 0x0a
tass 74:c146c4e346c4 63
tass 74:c146c4e346c4 64
tass 74:c146c4e346c4 65 #define OLSR_PORT (short_be((uint16_t)698))
tass 74:c146c4e346c4 66
tass 88:0e827d0d8017 67
tass 88:0e827d0d8017 68 /* Headers */
tass 88:0e827d0d8017 69
tass 74:c146c4e346c4 70 struct __attribute__((packed)) olsr_link
tass 74:c146c4e346c4 71 {
tass 88:0e827d0d8017 72 uint8_t link_code;
tass 88:0e827d0d8017 73 uint8_t reserved;
tass 88:0e827d0d8017 74 uint16_t link_msg_size;
tass 74:c146c4e346c4 75 };
tass 74:c146c4e346c4 76
tass 74:c146c4e346c4 77 struct __attribute__((packed)) olsr_neighbor
tass 74:c146c4e346c4 78 {
tass 88:0e827d0d8017 79 uint32_t addr;
tass 88:0e827d0d8017 80 uint8_t lq;
tass 88:0e827d0d8017 81 uint8_t nlq;
tass 88:0e827d0d8017 82 uint16_t reserved;
tass 74:c146c4e346c4 83 };
tass 74:c146c4e346c4 84
tass 74:c146c4e346c4 85 struct __attribute__((packed)) olsr_hmsg_hello
tass 74:c146c4e346c4 86 {
tass 88:0e827d0d8017 87 uint16_t reserved;
tass 88:0e827d0d8017 88 uint8_t htime;
tass 88:0e827d0d8017 89 uint8_t willingness;
tass 74:c146c4e346c4 90 };
tass 74:c146c4e346c4 91
tass 74:c146c4e346c4 92 struct __attribute__((packed)) olsr_hmsg_tc
tass 74:c146c4e346c4 93 {
tass 88:0e827d0d8017 94 uint16_t ansn;
tass 88:0e827d0d8017 95 uint16_t reserved;
tass 74:c146c4e346c4 96 };
tass 74:c146c4e346c4 97
tass 74:c146c4e346c4 98
tass 74:c146c4e346c4 99 struct __attribute__((packed)) olsrmsg
tass 74:c146c4e346c4 100 {
tass 88:0e827d0d8017 101 uint8_t type;
tass 88:0e827d0d8017 102 uint8_t vtime;
tass 88:0e827d0d8017 103 uint16_t size;
tass 88:0e827d0d8017 104 struct pico_ip4 orig;
tass 88:0e827d0d8017 105 uint8_t ttl;
tass 88:0e827d0d8017 106 uint8_t hop;
tass 88:0e827d0d8017 107 uint16_t seq;
tass 74:c146c4e346c4 108 };
tass 74:c146c4e346c4 109
tass 74:c146c4e346c4 110 struct __attribute__((packed)) olsrhdr
tass 74:c146c4e346c4 111 {
tass 88:0e827d0d8017 112 uint16_t len;
tass 88:0e827d0d8017 113 uint16_t seq;
tass 74:c146c4e346c4 114 };
tass 74:c146c4e346c4 115
tass 74:c146c4e346c4 116
tass 74:c146c4e346c4 117
tass 74:c146c4e346c4 118 /* Globals */
tass 74:c146c4e346c4 119 static struct pico_socket *udpsock = NULL;
tass 74:c146c4e346c4 120 uint16_t my_ansn = 0;
tass 74:c146c4e346c4 121 static struct olsr_route_entry *Local_interfaces = NULL;
tass 74:c146c4e346c4 122 static struct olsr_dev_entry *Local_devices = NULL;
tass 74:c146c4e346c4 123
tass 88:0e827d0d8017 124 static struct olsr_dev_entry *olsr_get_deventry(struct pico_device *dev)
tass 88:0e827d0d8017 125 {
tass 88:0e827d0d8017 126 struct olsr_dev_entry *cur = Local_devices;
tass 88:0e827d0d8017 127 while(cur) {
tass 88:0e827d0d8017 128 if (cur->dev == dev)
tass 88:0e827d0d8017 129 return cur;
tass 88:0e827d0d8017 130 cur = cur->next;
tass 88:0e827d0d8017 131 }
tass 88:0e827d0d8017 132 return NULL;
tass 88:0e827d0d8017 133 }
tass 88:0e827d0d8017 134
tass 88:0e827d0d8017 135 static struct olsr_route_entry *olsr_get_ethentry(struct pico_device *vif)
tass 74:c146c4e346c4 136 {
tass 74:c146c4e346c4 137 struct olsr_route_entry *cur = Local_interfaces;
tass 74:c146c4e346c4 138 while(cur) {
tass 74:c146c4e346c4 139 if (cur->iface == vif)
tass 74:c146c4e346c4 140 return cur;
tass 74:c146c4e346c4 141 cur = cur->next;
tass 74:c146c4e346c4 142 }
tass 74:c146c4e346c4 143 return NULL;
tass 74:c146c4e346c4 144 }
tass 74:c146c4e346c4 145
tass 74:c146c4e346c4 146 static struct olsr_route_entry *get_next_hop(struct olsr_route_entry *dst)
tass 74:c146c4e346c4 147 {
tass 88:0e827d0d8017 148 struct olsr_route_entry *hop = dst;
tass 88:0e827d0d8017 149 while(hop) {
tass 88:0e827d0d8017 150 if(hop->metric <= 1)
tass 88:0e827d0d8017 151 return hop;
tass 88:0e827d0d8017 152 hop = hop->gateway;
tass 88:0e827d0d8017 153 }
tass 88:0e827d0d8017 154 return NULL;
tass 74:c146c4e346c4 155 }
tass 74:c146c4e346c4 156
tass 74:c146c4e346c4 157 static inline void olsr_route_add(struct olsr_route_entry *el)
tass 74:c146c4e346c4 158 {
tass 88:0e827d0d8017 159 struct olsr_route_entry *nexthop;
tass 74:c146c4e346c4 160
tass 88:0e827d0d8017 161 my_ansn++;
tass 74:c146c4e346c4 162
tass 88:0e827d0d8017 163 if (el->gateway) {
tass 88:0e827d0d8017 164 nexthop = get_next_hop(el);
tass 88:0e827d0d8017 165 /* 2-hops route or more */
tass 88:0e827d0d8017 166 el->next = el->gateway->children;
tass 88:0e827d0d8017 167 el->gateway->children = el;
tass 88:0e827d0d8017 168 el->link_type = OLSRLINK_MPR;
tass 88:0e827d0d8017 169 if (nexthop->destination.addr != el->destination.addr) {
tass 88:0e827d0d8017 170 //dbg("[OLSR] Adding route to %08x via %08x metric %d..................", el->destination.addr, nexthop->destination.addr, el->metric);
tass 91:8da42f2c4820 171 pico_ipv4_route_add(el->destination, HOST_NETMASK, nexthop->destination, el->metric, NULL);
tass 88:0e827d0d8017 172 //dbg("route added: %d err: %s\n", ret, strerror(pico_err));
tass 88:0e827d0d8017 173 }
tass 88:0e827d0d8017 174 } else if (el->iface) {
tass 88:0e827d0d8017 175 /* neighbor */
tass 88:0e827d0d8017 176 struct olsr_route_entry *ei = olsr_get_ethentry(el->iface);
tass 88:0e827d0d8017 177 if (el->link_type == OLSRLINK_UNKNOWN)
tass 88:0e827d0d8017 178 el->link_type = OLSRLINK_SYMMETRIC;
tass 88:0e827d0d8017 179 if (ei) {
tass 88:0e827d0d8017 180 el->next = ei->children;
tass 88:0e827d0d8017 181 ei->children = el;
tass 88:0e827d0d8017 182 }
tass 88:0e827d0d8017 183 }
tass 74:c146c4e346c4 184 }
tass 74:c146c4e346c4 185
tass 74:c146c4e346c4 186 static inline void olsr_route_del(struct olsr_route_entry *r)
tass 74:c146c4e346c4 187 {
tass 88:0e827d0d8017 188 struct olsr_route_entry *cur, *prev = NULL, *lst;
tass 88:0e827d0d8017 189 //dbg("[OLSR] DELETING route..................\n");
tass 88:0e827d0d8017 190 my_ansn++;
tass 88:0e827d0d8017 191 if (r->gateway) {
tass 88:0e827d0d8017 192 lst = r->gateway->children;
tass 88:0e827d0d8017 193 } else if (r->iface) {
tass 88:0e827d0d8017 194 lst = olsr_get_ethentry(r->iface);
tass 88:0e827d0d8017 195 } else {
tass 88:0e827d0d8017 196 lst = Local_interfaces;
tass 88:0e827d0d8017 197 }
tass 88:0e827d0d8017 198 cur = lst, prev = NULL;
tass 88:0e827d0d8017 199 while(cur) {
tass 88:0e827d0d8017 200 if (cur == r) {
tass 88:0e827d0d8017 201 /* found */
tass 88:0e827d0d8017 202 if (r->gateway) {
tass 88:0e827d0d8017 203 pico_ipv4_route_del(r->destination, HOST_NETMASK, r->metric);
tass 88:0e827d0d8017 204 if (!prev)
tass 88:0e827d0d8017 205 r->gateway->children = r->next;
tass 88:0e827d0d8017 206 else
tass 88:0e827d0d8017 207 prev->next = r->next;
tass 88:0e827d0d8017 208 }
tass 74:c146c4e346c4 209
tass 88:0e827d0d8017 210 while (r->children) {
tass 88:0e827d0d8017 211 olsr_route_del(r->children);
tass 88:0e827d0d8017 212 /* Orphans must die. */
tass 88:0e827d0d8017 213 pico_free(r->children);
tass 88:0e827d0d8017 214 }
tass 88:0e827d0d8017 215 return;
tass 88:0e827d0d8017 216 }
tass 88:0e827d0d8017 217 prev = cur;
tass 88:0e827d0d8017 218 cur = cur->next;
tass 88:0e827d0d8017 219 }
tass 74:c146c4e346c4 220 }
tass 74:c146c4e346c4 221
tass 74:c146c4e346c4 222 static struct olsr_route_entry *get_route_by_address(struct olsr_route_entry *lst, uint32_t ip)
tass 74:c146c4e346c4 223 {
tass 88:0e827d0d8017 224 struct olsr_route_entry *found;
tass 88:0e827d0d8017 225 if(lst) {
tass 88:0e827d0d8017 226 if (lst->destination.addr == ip) {
tass 88:0e827d0d8017 227 return lst;
tass 88:0e827d0d8017 228 }
tass 88:0e827d0d8017 229 found = get_route_by_address(lst->children, ip);
tass 88:0e827d0d8017 230 if (found)
tass 88:0e827d0d8017 231 return found;
tass 88:0e827d0d8017 232 found = get_route_by_address(lst->next, ip);
tass 88:0e827d0d8017 233 if (found)
tass 88:0e827d0d8017 234 return found;
tass 88:0e827d0d8017 235 }
tass 88:0e827d0d8017 236 return NULL;
tass 74:c146c4e346c4 237 }
tass 74:c146c4e346c4 238
tass 74:c146c4e346c4 239 #define OLSR_C_SHIFT (uint32_t)4 /* 1/16 */
tass 74:c146c4e346c4 240 #define DEFAULT_VTIME 288UL
tass 74:c146c4e346c4 241
tass 74:c146c4e346c4 242 uint8_t seconds2olsr(uint32_t seconds)
tass 74:c146c4e346c4 243 {
tass 88:0e827d0d8017 244 uint16_t a, b;
tass 88:0e827d0d8017 245 //dbg("seconds=%u\n", (uint16_t)seconds);
tass 88:0e827d0d8017 246
tass 88:0e827d0d8017 247 if (seconds > 32767)
tass 88:0e827d0d8017 248 seconds = 32767;
tass 74:c146c4e346c4 249
tass 74:c146c4e346c4 250 /* find largest b such as seconds/C >= 2^b */
tass 88:0e827d0d8017 251 for (b = 1; b <= 0x0fu; b++) {
tass 88:0e827d0d8017 252 if ((uint16_t)(seconds * 16u) < (1u << b)){
tass 74:c146c4e346c4 253 b--;
tass 74:c146c4e346c4 254 break;
tass 74:c146c4e346c4 255 }
tass 74:c146c4e346c4 256 }
tass 88:0e827d0d8017 257 //dbg("b=%u", b);
tass 74:c146c4e346c4 258 /* compute the expression 16*(T/(C*(2^b))-1), which may not be a
tass 74:c146c4e346c4 259 integer, and round it up. This results in the value for 'a' */
tass 88:0e827d0d8017 260 //a = (T / ( C * (1u << b) ) ) - 1u;
tass 88:0e827d0d8017 261 {
tass 88:0e827d0d8017 262 uint16_t den = ((uint16_t)(1u << b) >> 4u );
tass 88:0e827d0d8017 263 //dbg(" den=%u ", den);
tass 88:0e827d0d8017 264 if (den == 0)
tass 88:0e827d0d8017 265 {
tass 88:0e827d0d8017 266 //dbg("div by 0!\n");
tass 88:0e827d0d8017 267 den = 1u;
tass 88:0e827d0d8017 268 }
tass 88:0e827d0d8017 269 a = (uint16_t)(((uint16_t)seconds / den ) - (uint16_t)1);
tass 88:0e827d0d8017 270 }
tass 88:0e827d0d8017 271 //a = a & 0x0Fu;
tass 74:c146c4e346c4 272
tass 88:0e827d0d8017 273 //dbg(" a=%u\n", a);
tass 88:0e827d0d8017 274
tass 74:c146c4e346c4 275 /* if 'a' is equal to 16: increment 'b' by one, and set 'a' to 0 */
tass 74:c146c4e346c4 276 if (16u == a) {
tass 74:c146c4e346c4 277 b++;
tass 74:c146c4e346c4 278 a = 0u;
tass 74:c146c4e346c4 279 }
tass 74:c146c4e346c4 280 return (uint8_t)((a << 4u) + b);
tass 74:c146c4e346c4 281 }
tass 74:c146c4e346c4 282
tass 74:c146c4e346c4 283 uint32_t olsr2seconds(uint8_t olsr)
tass 74:c146c4e346c4 284 {
tass 74:c146c4e346c4 285 uint8_t a, b;
tass 88:0e827d0d8017 286 uint16_t seconds;
tass 88:0e827d0d8017 287 //dbg("olsr format: %u -- ", olsr);
tass 88:0e827d0d8017 288 a = (olsr >> 4) & 0xFu;
tass 74:c146c4e346c4 289 b = olsr & 0x0f;
tass 88:0e827d0d8017 290 //dbg("o2s: a=%u, b=%u\n", a,b);
tass 88:0e827d0d8017 291 if (b<4)
tass 88:0e827d0d8017 292 seconds = (uint16_t)(( (1u << b) + (uint16_t)(((uint16_t)(a << b) >> 4u) & 0xFu) ) >> OLSR_C_SHIFT);
tass 88:0e827d0d8017 293 else
tass 88:0e827d0d8017 294 seconds = (uint16_t)(( (1u << b) + (uint16_t)(((uint16_t)(a << (b-4))) & 0xFu) ) >> OLSR_C_SHIFT);
tass 88:0e827d0d8017 295 //dbg("o2s: seconds: %u\n", seconds);
tass 88:0e827d0d8017 296 return seconds;
tass 74:c146c4e346c4 297 }
tass 74:c146c4e346c4 298
tass 74:c146c4e346c4 299
tass 74:c146c4e346c4 300 static void refresh_neighbors(struct pico_device *iface)
tass 74:c146c4e346c4 301 {
tass 88:0e827d0d8017 302 struct pico_ip4 neighbors[256];
tass 88:0e827d0d8017 303 int i;
tass 88:0e827d0d8017 304 struct olsr_route_entry *found = NULL, *ancestor = NULL;
tass 88:0e827d0d8017 305 int n_vec_size;
tass 88:0e827d0d8017 306
tass 88:0e827d0d8017 307 n_vec_size = pico_arp_get_neighbors(iface, neighbors, 256);
tass 74:c146c4e346c4 308
tass 88:0e827d0d8017 309 ancestor = olsr_get_ethentry(iface);
tass 88:0e827d0d8017 310 if (!ancestor)
tass 88:0e827d0d8017 311 return;
tass 74:c146c4e346c4 312
tass 88:0e827d0d8017 313 for (i = 0; i < n_vec_size; i++) {
tass 88:0e827d0d8017 314 found = get_route_by_address(Local_interfaces, neighbors[i].addr);
tass 88:0e827d0d8017 315 if (found) {
tass 88:0e827d0d8017 316 if (found->metric > 1) { /* Reposition among neighbors */
tass 88:0e827d0d8017 317 olsr_route_del(found);
tass 88:0e827d0d8017 318 found->gateway = olsr_get_ethentry(iface);
tass 88:0e827d0d8017 319 found->iface = iface;
tass 88:0e827d0d8017 320 found->metric = 1;
tass 88:0e827d0d8017 321 found->lq = 0xFF;
tass 88:0e827d0d8017 322 found->nlq = 0xFF;
tass 88:0e827d0d8017 323 olsr_route_add(found);
tass 88:0e827d0d8017 324 }
tass 88:0e827d0d8017 325 if (found->link_type == OLSRLINK_UNKNOWN)
tass 88:0e827d0d8017 326 found->link_type = OLSRLINK_SYMMETRIC;
tass 88:0e827d0d8017 327 found->time_left = (OLSR_HELLO_INTERVAL << 2);
tass 88:0e827d0d8017 328 } else {
tass 88:0e827d0d8017 329 struct olsr_route_entry *e = pico_zalloc(sizeof (struct olsr_route_entry));
tass 88:0e827d0d8017 330 if (!e) {
tass 88:0e827d0d8017 331 dbg("olsr: adding local route entry\n");
tass 88:0e827d0d8017 332 return;
tass 88:0e827d0d8017 333 }
tass 88:0e827d0d8017 334 e->destination.addr = neighbors[i].addr;
tass 88:0e827d0d8017 335 e->link_type = OLSRLINK_SYMMETRIC;
tass 88:0e827d0d8017 336 e->time_left = (OLSR_HELLO_INTERVAL << 2);
tass 88:0e827d0d8017 337 e->gateway = olsr_get_ethentry(iface);
tass 88:0e827d0d8017 338 e->iface = iface;
tass 88:0e827d0d8017 339 e->metric = 1;
tass 88:0e827d0d8017 340 e->lq = 0xFF;
tass 88:0e827d0d8017 341 e->nlq = 0xFF;
tass 88:0e827d0d8017 342 olsr_route_add(e);
tass 88:0e827d0d8017 343 }
tass 88:0e827d0d8017 344 }
tass 74:c146c4e346c4 345 }
tass 74:c146c4e346c4 346
tass 74:c146c4e346c4 347 static void olsr_garbage_collector(struct olsr_route_entry *sublist)
tass 74:c146c4e346c4 348 {
tass 88:0e827d0d8017 349 if(!sublist)
tass 88:0e827d0d8017 350 return;
tass 88:0e827d0d8017 351 if (sublist->time_left <= 0) {
tass 88:0e827d0d8017 352 olsr_route_del(sublist);
tass 88:0e827d0d8017 353 pico_free(sublist);
tass 88:0e827d0d8017 354 return;
tass 88:0e827d0d8017 355 } else {
tass 88:0e827d0d8017 356 sublist->time_left -= 2u;
tass 88:0e827d0d8017 357 }
tass 88:0e827d0d8017 358 olsr_garbage_collector(sublist->children);
tass 88:0e827d0d8017 359 olsr_garbage_collector(sublist->next);
tass 88:0e827d0d8017 360 }
tass 88:0e827d0d8017 361
tass 88:0e827d0d8017 362 struct olsr_fwd_pkt
tass 88:0e827d0d8017 363 {
tass 88:0e827d0d8017 364 void *buf;
tass 88:0e827d0d8017 365 uint16_t len;
tass 88:0e827d0d8017 366 struct pico_device *pdev;
tass 88:0e827d0d8017 367 };
tass 88:0e827d0d8017 368
tass 88:0e827d0d8017 369 static uint32_t buffer_mem_used = 0U;
tass 88:0e827d0d8017 370
tass 88:0e827d0d8017 371 void olsr_process_out(uint32_t now, void *arg)
tass 88:0e827d0d8017 372 {
tass 88:0e827d0d8017 373 struct olsr_fwd_pkt *p = (struct olsr_fwd_pkt *)arg;
tass 88:0e827d0d8017 374 struct pico_ip4 bcast;
tass 88:0e827d0d8017 375 struct pico_ipv4_link *addr;
tass 88:0e827d0d8017 376 struct olsr_dev_entry *pdev = Local_devices;
tass 88:0e827d0d8017 377 struct olsrhdr *ohdr;
tass 88:0e827d0d8017 378 (void)now;
tass 88:0e827d0d8017 379
tass 88:0e827d0d8017 380 /* Send the thing out */
tass 88:0e827d0d8017 381 ohdr = (struct olsrhdr *)p->buf;
tass 88:0e827d0d8017 382 ohdr->len = short_be((uint16_t)p->len);
tass 88:0e827d0d8017 383
tass 88:0e827d0d8017 384 if (p->pdev) {
tass 88:0e827d0d8017 385 struct olsr_dev_entry *odev = olsr_get_deventry(p->pdev);
tass 88:0e827d0d8017 386 if (!odev) {
tass 88:0e827d0d8017 387 goto out_free;
tass 88:0e827d0d8017 388 }
tass 88:0e827d0d8017 389 addr = pico_ipv4_link_by_dev(p->pdev);
tass 88:0e827d0d8017 390 if (!addr)
tass 88:0e827d0d8017 391 goto out_free;
tass 88:0e827d0d8017 392 ohdr->seq = short_be((uint16_t)(odev->pkt_counter)++);
tass 88:0e827d0d8017 393 bcast.addr = (addr->netmask.addr & addr->address.addr) | (~addr->netmask.addr);
tass 88:0e827d0d8017 394 if ( 0 > pico_socket_sendto(udpsock, p->buf, p->len, &bcast, OLSR_PORT) ) {
tass 88:0e827d0d8017 395 dbg("olsr send\n");
tass 88:0e827d0d8017 396 }
tass 88:0e827d0d8017 397 } else {
tass 88:0e827d0d8017 398 while(pdev) {
tass 88:0e827d0d8017 399 ohdr->seq = short_be((uint16_t)(pdev->pkt_counter++));
tass 88:0e827d0d8017 400 addr = pico_ipv4_link_by_dev(pdev->dev);
tass 88:0e827d0d8017 401 if (!addr)
tass 88:0e827d0d8017 402 continue;
tass 88:0e827d0d8017 403 bcast.addr = (addr->netmask.addr & addr->address.addr) | (~addr->netmask.addr);
tass 88:0e827d0d8017 404 if ( 0 > pico_socket_sendto(udpsock, p->buf, p->len, &bcast, OLSR_PORT) ) {
tass 88:0e827d0d8017 405 dbg("olsr send\n");
tass 88:0e827d0d8017 406 }
tass 88:0e827d0d8017 407 pdev = pdev->next;
tass 88:0e827d0d8017 408 }
tass 88:0e827d0d8017 409 }
tass 88:0e827d0d8017 410
tass 88:0e827d0d8017 411 out_free:
tass 88:0e827d0d8017 412 pico_free(p->buf);
tass 88:0e827d0d8017 413 buffer_mem_used -= DGRAM_MAX_SIZE;
tass 88:0e827d0d8017 414 pico_free(p);
tass 88:0e827d0d8017 415 }
tass 88:0e827d0d8017 416
tass 88:0e827d0d8017 417 static void olsr_scheduled_output(uint32_t when, void *buffer, uint16_t size, struct pico_device *pdev)
tass 88:0e827d0d8017 418 {
tass 88:0e827d0d8017 419 struct olsr_fwd_pkt *p;
tass 88:0e827d0d8017 420 if ((buffer_mem_used + DGRAM_MAX_SIZE) > MAX_OLSR_MEM)
tass 88:0e827d0d8017 421 return;
tass 88:0e827d0d8017 422
tass 88:0e827d0d8017 423 p = pico_zalloc(sizeof(struct olsr_fwd_pkt));
tass 88:0e827d0d8017 424 if (!p) {
tass 88:0e827d0d8017 425 pico_free(buffer);
tass 88:0e827d0d8017 426 return;
tass 88:0e827d0d8017 427 }
tass 88:0e827d0d8017 428 p->buf = buffer;
tass 88:0e827d0d8017 429 p->len = size;
tass 88:0e827d0d8017 430 p->pdev = pdev;
tass 88:0e827d0d8017 431 buffer_mem_used += DGRAM_MAX_SIZE;
tass 88:0e827d0d8017 432 pico_timer_add(1 + when - ((pico_rand() % OLSR_MAXJITTER)), &olsr_process_out, p);
tass 74:c146c4e346c4 433 }
tass 74:c146c4e346c4 434
tass 74:c146c4e346c4 435
tass 74:c146c4e346c4 436 static void refresh_routes(void)
tass 74:c146c4e346c4 437 {
tass 88:0e827d0d8017 438 struct olsr_route_entry *local, *neighbor = NULL;
tass 74:c146c4e346c4 439 struct olsr_dev_entry *icur = Local_devices;
tass 74:c146c4e346c4 440
tass 88:0e827d0d8017 441 /* Refresh local entries */
tass 74:c146c4e346c4 442
tass 88:0e827d0d8017 443 /* Step 1: set zero expire time for local addresses and neighbors*/
tass 88:0e827d0d8017 444 local = Local_interfaces;
tass 88:0e827d0d8017 445 while(local) {
tass 88:0e827d0d8017 446 local->time_left = 0;
tass 88:0e827d0d8017 447 neighbor = local->children;
tass 88:0e827d0d8017 448 while (neighbor && (neighbor->metric < 2)) {
tass 88:0e827d0d8017 449 //dbg("Setting to zero. Neigh: %08x metric %d\n", neighbor->destination, neighbor->metric);
tass 88:0e827d0d8017 450 neighbor->time_left = 0;
tass 88:0e827d0d8017 451 neighbor = neighbor->next;
tass 88:0e827d0d8017 452 }
tass 88:0e827d0d8017 453 local = local->next;
tass 88:0e827d0d8017 454 }
tass 74:c146c4e346c4 455
tass 88:0e827d0d8017 456 /* Step 2: refresh timer for entries that are still valid.
tass 88:0e827d0d8017 457 * Add new entries.
tass 88:0e827d0d8017 458 */
tass 74:c146c4e346c4 459 while(icur) {
tass 74:c146c4e346c4 460 struct pico_ipv4_link *lnk = NULL;
tass 74:c146c4e346c4 461 do {
tass 74:c146c4e346c4 462 lnk = pico_ipv4_link_by_dev_next(icur->dev, lnk);
tass 74:c146c4e346c4 463 if (!lnk) break;
tass 88:0e827d0d8017 464 local = olsr_get_ethentry(icur->dev);
tass 88:0e827d0d8017 465 if (local) {
tass 88:0e827d0d8017 466 local->time_left = (OLSR_HELLO_INTERVAL << 2);
tass 88:0e827d0d8017 467 } else if (lnk) {
tass 88:0e827d0d8017 468 struct olsr_route_entry *e = pico_zalloc(sizeof (struct olsr_route_entry));
tass 88:0e827d0d8017 469 if (!e) {
tass 88:0e827d0d8017 470 dbg("olsr: adding local route entry\n");
tass 88:0e827d0d8017 471 return;
tass 88:0e827d0d8017 472 }
tass 88:0e827d0d8017 473 e->destination.addr = lnk->address.addr; /* Always pick the first address */
tass 88:0e827d0d8017 474 e->time_left = (OLSR_HELLO_INTERVAL << 2);
tass 88:0e827d0d8017 475 e->iface = icur->dev;
tass 88:0e827d0d8017 476 e->metric = 0;
tass 88:0e827d0d8017 477 e->lq = 0xFF;
tass 88:0e827d0d8017 478 e->nlq = 0xFF;
tass 88:0e827d0d8017 479 e->next = Local_interfaces;
tass 88:0e827d0d8017 480 Local_interfaces = e;
tass 88:0e827d0d8017 481 }
tass 74:c146c4e346c4 482 } while (lnk);
tass 74:c146c4e346c4 483
tass 88:0e827d0d8017 484 refresh_neighbors(icur->dev);
tass 74:c146c4e346c4 485 icur = icur->next;
tass 88:0e827d0d8017 486 }
tass 74:c146c4e346c4 487 }
tass 74:c146c4e346c4 488
tass 74:c146c4e346c4 489 static uint32_t olsr_build_hello_neighbors(uint8_t *buf, uint32_t size)
tass 74:c146c4e346c4 490 {
tass 88:0e827d0d8017 491 uint32_t ret = 0;
tass 88:0e827d0d8017 492 struct olsr_route_entry *local, *neighbor;
tass 88:0e827d0d8017 493 struct olsr_neighbor *dst = (struct olsr_neighbor *) buf;
tass 88:0e827d0d8017 494 local = Local_interfaces;
tass 88:0e827d0d8017 495 while (local) {
tass 88:0e827d0d8017 496 neighbor = local->children;
tass 88:0e827d0d8017 497 while (neighbor) {
tass 88:0e827d0d8017 498 struct olsr_link *li = (struct olsr_link *) (buf + ret);
tass 88:0e827d0d8017 499 li->link_code = neighbor->link_type;
tass 88:0e827d0d8017 500 li->reserved = 0;
tass 88:0e827d0d8017 501 li->link_msg_size = short_be(sizeof(struct olsr_neighbor) + sizeof(struct olsr_link));
tass 88:0e827d0d8017 502 ret += (uint32_t)sizeof(struct olsr_link);
tass 88:0e827d0d8017 503 dst = (struct olsr_neighbor *) (buf+ret);
tass 88:0e827d0d8017 504 dst->addr = neighbor->destination.addr;
tass 88:0e827d0d8017 505 dst->nlq = neighbor->nlq;
tass 88:0e827d0d8017 506 dst->lq = neighbor->lq;
tass 88:0e827d0d8017 507 dst->reserved = 0;
tass 88:0e827d0d8017 508 ret += (uint32_t)sizeof(struct olsr_neighbor);
tass 88:0e827d0d8017 509 if (ret >= size)
tass 88:0e827d0d8017 510 return (uint32_t)((uint32_t)(ret - sizeof(struct olsr_neighbor)) - sizeof(struct olsr_link));
tass 88:0e827d0d8017 511 neighbor = neighbor->next;
tass 88:0e827d0d8017 512 }
tass 88:0e827d0d8017 513 local = local->next;
tass 88:0e827d0d8017 514 }
tass 88:0e827d0d8017 515 return ret;
tass 74:c146c4e346c4 516 }
tass 74:c146c4e346c4 517
tass 74:c146c4e346c4 518 static uint32_t olsr_build_tc_neighbors(uint8_t *buf, uint32_t size)
tass 74:c146c4e346c4 519 {
tass 88:0e827d0d8017 520 uint32_t ret = 0;
tass 88:0e827d0d8017 521 struct olsr_route_entry *local, *neighbor;
tass 88:0e827d0d8017 522 struct olsr_neighbor *dst = (struct olsr_neighbor *) buf;
tass 88:0e827d0d8017 523 local = Local_interfaces;
tass 88:0e827d0d8017 524 while (local) {
tass 88:0e827d0d8017 525 neighbor = local->children;
tass 88:0e827d0d8017 526 while (neighbor) {
tass 88:0e827d0d8017 527 dst->addr = neighbor->destination.addr;
tass 88:0e827d0d8017 528 dst->nlq = neighbor->nlq;
tass 88:0e827d0d8017 529 dst->lq = neighbor->lq;
tass 88:0e827d0d8017 530 dst->reserved = 0;
tass 88:0e827d0d8017 531 ret += (uint32_t)sizeof(struct olsr_neighbor);
tass 88:0e827d0d8017 532 dst = (struct olsr_neighbor *) (buf + ret);
tass 88:0e827d0d8017 533 if (ret >= size)
tass 88:0e827d0d8017 534 return (uint32_t)(ret - sizeof(struct olsr_neighbor));
tass 88:0e827d0d8017 535 neighbor = neighbor->next;
tass 88:0e827d0d8017 536 }
tass 88:0e827d0d8017 537 local = local->next;
tass 88:0e827d0d8017 538 }
tass 88:0e827d0d8017 539 return ret;
tass 74:c146c4e346c4 540 }
tass 74:c146c4e346c4 541
tass 74:c146c4e346c4 542 static uint32_t olsr_build_mid(uint8_t *buf, uint32_t size, struct pico_device *excluded)
tass 74:c146c4e346c4 543 {
tass 88:0e827d0d8017 544 uint32_t ret = 0;
tass 88:0e827d0d8017 545 struct olsr_route_entry *local;
tass 88:0e827d0d8017 546 struct pico_ip4 *dst = (struct pico_ip4 *) buf;
tass 88:0e827d0d8017 547 local = Local_interfaces;
tass 88:0e827d0d8017 548 while (local) {
tass 88:0e827d0d8017 549 if (local->iface != excluded) {
tass 88:0e827d0d8017 550 dst->addr = local->destination.addr;
tass 88:0e827d0d8017 551 ret += (uint32_t)sizeof(uint32_t);
tass 88:0e827d0d8017 552 dst = (struct pico_ip4 *) (buf + ret);
tass 88:0e827d0d8017 553 if (ret >= size)
tass 88:0e827d0d8017 554 return (uint32_t)(ret - sizeof(uint32_t));
tass 88:0e827d0d8017 555 }
tass 88:0e827d0d8017 556 local = local->next;
tass 88:0e827d0d8017 557 }
tass 88:0e827d0d8017 558 return ret;
tass 74:c146c4e346c4 559 }
tass 74:c146c4e346c4 560
tass 74:c146c4e346c4 561 static void olsr_make_dgram(struct pico_device *pdev, int full)
tass 74:c146c4e346c4 562 {
tass 88:0e827d0d8017 563 uint8_t * dgram;
tass 88:0e827d0d8017 564 uint32_t size = 0, r;
tass 88:0e827d0d8017 565 struct pico_ipv4_link *ep;
tass 88:0e827d0d8017 566 struct olsrmsg *msg_hello, *msg_mid, *msg_tc;
tass 88:0e827d0d8017 567 struct olsr_hmsg_hello *hello;
tass 88:0e827d0d8017 568 struct olsr_hmsg_tc *tc;
tass 88:0e827d0d8017 569 static uint16_t msg_counter; /* Global message sequence number */
tass 88:0e827d0d8017 570 uint32_t interval = OLSR_HELLO_INTERVAL;
tass 74:c146c4e346c4 571
tass 88:0e827d0d8017 572 dgram = pico_zalloc(DGRAM_MAX_SIZE);
tass 88:0e827d0d8017 573 if (!dgram)
tass 88:0e827d0d8017 574 return;
tass 88:0e827d0d8017 575
tass 88:0e827d0d8017 576 size += (uint32_t)sizeof(struct olsrhdr);
tass 74:c146c4e346c4 577 ep = pico_ipv4_link_by_dev(pdev);
tass 88:0e827d0d8017 578 if (!ep) {
tass 88:0e827d0d8017 579 pico_free(dgram);
tass 88:0e827d0d8017 580 return;
tass 88:0e827d0d8017 581 }
tass 74:c146c4e346c4 582
tass 88:0e827d0d8017 583 if (!full) {
tass 88:0e827d0d8017 584 /* HELLO Message */
tass 74:c146c4e346c4 585
tass 88:0e827d0d8017 586 msg_hello = (struct olsrmsg *) (dgram + size);
tass 88:0e827d0d8017 587 size += (uint32_t)sizeof(struct olsrmsg);
tass 88:0e827d0d8017 588 msg_hello->type = OLSRMSG_HELLO;
tass 88:0e827d0d8017 589 msg_hello->vtime = seconds2olsr(DEFAULT_VTIME);
tass 88:0e827d0d8017 590 msg_hello->orig.addr = ep->address.addr;
tass 88:0e827d0d8017 591 msg_hello->ttl = 1;
tass 88:0e827d0d8017 592 msg_hello->hop = 0;
tass 88:0e827d0d8017 593 msg_hello->seq = short_be(msg_counter++);
tass 88:0e827d0d8017 594 hello = (struct olsr_hmsg_hello *)(dgram + size);
tass 88:0e827d0d8017 595 size += (uint32_t)sizeof(struct olsr_hmsg_hello);
tass 88:0e827d0d8017 596 hello->reserved = 0;
tass 88:0e827d0d8017 597 hello->htime = seconds2olsr(OLSR_HELLO_INTERVAL);
tass 88:0e827d0d8017 598 hello->htime = 0x05; /* Todo: find and define values */
tass 88:0e827d0d8017 599 hello->willingness = 0x07;
tass 88:0e827d0d8017 600 r = olsr_build_hello_neighbors(dgram + size, DGRAM_MAX_SIZE - size);
tass 88:0e827d0d8017 601 if (r == 0) {
tass 88:0e827d0d8017 602 //dbg("Building hello message\n");
tass 88:0e827d0d8017 603 pico_free(dgram);
tass 88:0e827d0d8017 604 return;
tass 88:0e827d0d8017 605 }
tass 88:0e827d0d8017 606 size += r;
tass 88:0e827d0d8017 607 msg_hello->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_hello) + r));
tass 74:c146c4e346c4 608
tass 88:0e827d0d8017 609 } else {
tass 88:0e827d0d8017 610 /* MID Message */
tass 74:c146c4e346c4 611
tass 88:0e827d0d8017 612 msg_mid = (struct olsrmsg *)(dgram + size);
tass 88:0e827d0d8017 613 size += (uint32_t)sizeof(struct olsrmsg);
tass 88:0e827d0d8017 614 msg_mid->type = OLSRMSG_MID;
tass 88:0e827d0d8017 615 msg_mid->vtime = seconds2olsr(60);
tass 88:0e827d0d8017 616 msg_mid->orig.addr = ep->address.addr;
tass 88:0e827d0d8017 617 msg_mid->ttl = 0xFF;
tass 88:0e827d0d8017 618 msg_mid->hop = 0;
tass 88:0e827d0d8017 619 msg_mid->seq = short_be(msg_counter++);
tass 88:0e827d0d8017 620 r = olsr_build_mid(dgram + size, DGRAM_MAX_SIZE - size, pdev);
tass 88:0e827d0d8017 621 if (r == 0) {
tass 88:0e827d0d8017 622 size -= (uint32_t)sizeof(struct olsrmsg);
tass 88:0e827d0d8017 623 } else {
tass 88:0e827d0d8017 624 size += r;
tass 88:0e827d0d8017 625 msg_mid->size = short_be((uint16_t)(sizeof(struct olsrmsg) + r));
tass 88:0e827d0d8017 626 }
tass 74:c146c4e346c4 627
tass 88:0e827d0d8017 628 msg_tc = (struct olsrmsg *) (dgram + size);
tass 88:0e827d0d8017 629 size += (uint32_t)sizeof(struct olsrmsg);
tass 88:0e827d0d8017 630 msg_tc->type = OLSRMSG_TC;
tass 88:0e827d0d8017 631 msg_tc->vtime = seconds2olsr(DEFAULT_VTIME);
tass 88:0e827d0d8017 632 msg_tc->orig.addr = ep->address.addr;
tass 88:0e827d0d8017 633 msg_tc->ttl = 0xFF;
tass 88:0e827d0d8017 634 msg_tc->hop = 0;
tass 88:0e827d0d8017 635 msg_tc->seq = short_be(msg_counter++);
tass 88:0e827d0d8017 636 tc = (struct olsr_hmsg_tc *)(dgram + size);
tass 88:0e827d0d8017 637 size += (uint32_t)sizeof(struct olsr_hmsg_tc);
tass 88:0e827d0d8017 638 tc->ansn = short_be(my_ansn);
tass 88:0e827d0d8017 639 r = olsr_build_tc_neighbors(dgram + size, DGRAM_MAX_SIZE - size);
tass 88:0e827d0d8017 640 size += r;
tass 88:0e827d0d8017 641 msg_tc->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_tc) + r));
tass 88:0e827d0d8017 642 interval = OLSR_TC_INTERVAL;
tass 74:c146c4e346c4 643 } /*if full */
tass 88:0e827d0d8017 644
tass 74:c146c4e346c4 645
tass 88:0e827d0d8017 646 /* Send the thing out */
tass 88:0e827d0d8017 647 olsr_scheduled_output(interval, dgram, (uint16_t)size, pdev );
tass 74:c146c4e346c4 648 }
tass 74:c146c4e346c4 649
tass 74:c146c4e346c4 650 static inline void arp_storm(struct pico_ip4 *addr)
tass 74:c146c4e346c4 651 {
tass 74:c146c4e346c4 652 struct olsr_dev_entry *icur = Local_devices;
tass 74:c146c4e346c4 653 while(icur) {
tass 88:0e827d0d8017 654 pico_arp_query(icur->dev, addr);
tass 74:c146c4e346c4 655 icur = icur->next;
tass 88:0e827d0d8017 656 }
tass 74:c146c4e346c4 657 }
tass 74:c146c4e346c4 658
tass 74:c146c4e346c4 659 static void recv_mid(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin)
tass 74:c146c4e346c4 660 {
tass 88:0e827d0d8017 661 uint32_t parsed = 0;
tass 88:0e827d0d8017 662 uint32_t *address;
tass 88:0e827d0d8017 663 struct olsr_route_entry *e;
tass 74:c146c4e346c4 664
tass 88:0e827d0d8017 665 if (len % sizeof(uint32_t)) /*drop*/
tass 88:0e827d0d8017 666 return;
tass 74:c146c4e346c4 667
tass 88:0e827d0d8017 668 while (len > parsed) {
tass 88:0e827d0d8017 669 address = (uint32_t *)(buffer + parsed);
tass 88:0e827d0d8017 670 e = get_route_by_address(Local_interfaces, *address);
tass 88:0e827d0d8017 671 if (!e) {
tass 88:0e827d0d8017 672 e = pico_zalloc(sizeof(struct olsr_route_entry));
tass 88:0e827d0d8017 673 if (!e) {
tass 88:0e827d0d8017 674 dbg("olsr allocating route\n");
tass 88:0e827d0d8017 675 return;
tass 88:0e827d0d8017 676 }
tass 88:0e827d0d8017 677 e->time_left = (OLSR_HELLO_INTERVAL << 2);
tass 88:0e827d0d8017 678 e->destination.addr = *address;
tass 88:0e827d0d8017 679 e->gateway = origin;
tass 88:0e827d0d8017 680 e->iface = origin->iface;
tass 88:0e827d0d8017 681 e->metric = (uint16_t)(origin->metric + 1u);
tass 88:0e827d0d8017 682 e->lq = origin->lq;
tass 88:0e827d0d8017 683 e->nlq = origin->nlq;
tass 88:0e827d0d8017 684 olsr_route_add(e);
tass 88:0e827d0d8017 685 arp_storm(&e->destination);
tass 88:0e827d0d8017 686 } else if (e->metric > (origin->metric + 1)) {
tass 88:0e827d0d8017 687 olsr_route_del(e);
tass 88:0e827d0d8017 688 e->metric = origin->metric;
tass 88:0e827d0d8017 689 e->gateway = origin;
tass 88:0e827d0d8017 690 olsr_route_add(e);
tass 88:0e827d0d8017 691 }
tass 88:0e827d0d8017 692 parsed += (uint32_t)sizeof(uint32_t);
tass 88:0e827d0d8017 693 }
tass 74:c146c4e346c4 694 }
tass 74:c146c4e346c4 695
tass 74:c146c4e346c4 696 static void recv_hello(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin)
tass 74:c146c4e346c4 697 {
tass 88:0e827d0d8017 698 struct olsr_link *li;
tass 88:0e827d0d8017 699 struct olsr_route_entry *e;
tass 88:0e827d0d8017 700 uint32_t parsed = 0;
tass 88:0e827d0d8017 701 struct olsr_neighbor *neigh;
tass 74:c146c4e346c4 702
tass 88:0e827d0d8017 703 if (!origin)
tass 88:0e827d0d8017 704 return;
tass 74:c146c4e346c4 705
tass 88:0e827d0d8017 706 while (len > parsed) {
tass 88:0e827d0d8017 707 li = (struct olsr_link *) buffer;
tass 88:0e827d0d8017 708 neigh = (struct olsr_neighbor *)(buffer + parsed + sizeof(struct olsr_link));
tass 88:0e827d0d8017 709 parsed += short_be(li->link_msg_size);
tass 88:0e827d0d8017 710 e = get_route_by_address(Local_interfaces, neigh->addr);
tass 88:0e827d0d8017 711 if (!e) {
tass 88:0e827d0d8017 712 e = pico_zalloc(sizeof(struct olsr_route_entry));
tass 88:0e827d0d8017 713 if (!e) {
tass 88:0e827d0d8017 714 dbg("olsr allocating route\n");
tass 88:0e827d0d8017 715 return;
tass 88:0e827d0d8017 716 }
tass 88:0e827d0d8017 717 e->time_left = (OLSR_HELLO_INTERVAL << 2);
tass 88:0e827d0d8017 718 e->destination.addr = neigh->addr;
tass 88:0e827d0d8017 719 e->gateway = origin;
tass 88:0e827d0d8017 720 e->iface = origin->iface;
tass 88:0e827d0d8017 721 e->metric = (uint16_t)(origin->metric + 1u);
tass 88:0e827d0d8017 722 e->link_type = OLSRLINK_UNKNOWN;
tass 88:0e827d0d8017 723 e->lq = MIN(origin->lq, neigh->lq);
tass 88:0e827d0d8017 724 e->nlq = MIN(origin->nlq, neigh->nlq);
tass 88:0e827d0d8017 725 olsr_route_add(e);
tass 88:0e827d0d8017 726 arp_storm(&e->destination);
tass 88:0e827d0d8017 727 } else if ((e->gateway != origin) && (e->metric > (origin->metric + 1))) {
tass 88:0e827d0d8017 728 olsr_route_del(e);
tass 88:0e827d0d8017 729 e->metric = (uint16_t)(origin->metric + 1u);
tass 88:0e827d0d8017 730 e->gateway = origin;
tass 88:0e827d0d8017 731 olsr_route_add(e);
tass 88:0e827d0d8017 732 }
tass 88:0e827d0d8017 733 }
tass 74:c146c4e346c4 734 }
tass 74:c146c4e346c4 735
tass 74:c146c4e346c4 736 static uint32_t reconsider_topology(uint8_t *buf, uint32_t size, struct olsr_route_entry *e)
tass 74:c146c4e346c4 737 {
tass 88:0e827d0d8017 738 struct olsr_hmsg_tc *tc = (struct olsr_hmsg_tc *) buf;
tass 88:0e827d0d8017 739 uint16_t new_ansn = short_be(tc->ansn);
tass 88:0e827d0d8017 740 uint32_t parsed = sizeof(struct olsr_hmsg_tc);
tass 88:0e827d0d8017 741 struct olsr_route_entry *rt;
tass 88:0e827d0d8017 742 struct olsr_neighbor *n;
tass 88:0e827d0d8017 743 uint32_t retval = 0;
tass 74:c146c4e346c4 744
tass 88:0e827d0d8017 745 if (!e->advertised_tc)
tass 88:0e827d0d8017 746 retval = 1;
tass 74:c146c4e346c4 747
tass 88:0e827d0d8017 748 if (e->advertised_tc && fresher(new_ansn, e->ansn))
tass 88:0e827d0d8017 749 {
tass 88:0e827d0d8017 750 pico_free(e->advertised_tc);
tass 88:0e827d0d8017 751 e->advertised_tc = NULL;
tass 88:0e827d0d8017 752 retval = 1;
tass 88:0e827d0d8017 753 }
tass 74:c146c4e346c4 754
tass 88:0e827d0d8017 755 if (!e->advertised_tc) {
tass 88:0e827d0d8017 756 e->advertised_tc = pico_zalloc(size);
tass 88:0e827d0d8017 757 if (!e->advertised_tc) {
tass 88:0e827d0d8017 758 dbg("Allocating forward packet\n");
tass 88:0e827d0d8017 759 return 0;
tass 88:0e827d0d8017 760 }
tass 88:0e827d0d8017 761 memcpy(e->advertised_tc, buf, size);
tass 88:0e827d0d8017 762 e->ansn = new_ansn;
tass 88:0e827d0d8017 763 while (parsed < size) {
tass 88:0e827d0d8017 764 n = (struct olsr_neighbor *) (buf + parsed);
tass 88:0e827d0d8017 765 parsed += (uint32_t)sizeof(struct olsr_neighbor);
tass 88:0e827d0d8017 766 rt = get_route_by_address(Local_interfaces, n->addr);
tass 88:0e827d0d8017 767 if (rt && (rt->gateway == e)) {
tass 88:0e827d0d8017 768 /* Refresh existing node */
tass 88:0e827d0d8017 769 rt->time_left = e->time_left;
tass 88:0e827d0d8017 770 } else if (!rt || (rt->metric > (e->metric + 1)) || (rt->nlq < n->nlq)) {
tass 88:0e827d0d8017 771 if (!rt) {
tass 88:0e827d0d8017 772 rt = pico_zalloc(sizeof (struct olsr_route_entry));
tass 88:0e827d0d8017 773 rt->destination.addr = n->addr;
tass 88:0e827d0d8017 774 rt->link_type = OLSRLINK_UNKNOWN;
tass 88:0e827d0d8017 775 } else {
tass 88:0e827d0d8017 776 olsr_route_del(rt);
tass 88:0e827d0d8017 777 }
tass 88:0e827d0d8017 778 rt->iface = e->iface;
tass 88:0e827d0d8017 779 rt->gateway = e;
tass 88:0e827d0d8017 780 rt->metric = (uint16_t)(e->metric + 1);
tass 88:0e827d0d8017 781 rt->lq = n->lq;
tass 88:0e827d0d8017 782 rt->nlq = n->nlq;
tass 88:0e827d0d8017 783 rt->time_left = e->time_left;
tass 88:0e827d0d8017 784 olsr_route_add(rt);
tass 88:0e827d0d8017 785 }
tass 88:0e827d0d8017 786 }
tass 88:0e827d0d8017 787 //dbg("Routes changed...\n");
tass 88:0e827d0d8017 788 }
tass 88:0e827d0d8017 789 return retval;
tass 74:c146c4e346c4 790 }
tass 74:c146c4e346c4 791
tass 88:0e827d0d8017 792
tass 74:c146c4e346c4 793 static void olsr_recv(uint8_t *buffer, uint32_t len)
tass 74:c146c4e346c4 794 {
tass 88:0e827d0d8017 795 struct olsrmsg *msg;
tass 88:0e827d0d8017 796 struct olsrhdr *oh = (struct olsrhdr *) buffer;
tass 88:0e827d0d8017 797 struct olsr_route_entry *ancestor;
tass 88:0e827d0d8017 798 uint32_t parsed = 0;
tass 88:0e827d0d8017 799 uint16_t outsize = 0;
tass 88:0e827d0d8017 800 uint8_t *datagram;
tass 88:0e827d0d8017 801
tass 88:0e827d0d8017 802 if (len != short_be(oh->len)) {
tass 88:0e827d0d8017 803 return;
tass 88:0e827d0d8017 804 }
tass 88:0e827d0d8017 805
tass 88:0e827d0d8017 806 /* RFC 3626, section 3.4, if a packet is too small, it is silently discarded */
tass 88:0e827d0d8017 807 if (len < 16) {
tass 88:0e827d0d8017 808 return;
tass 88:0e827d0d8017 809 }
tass 88:0e827d0d8017 810
tass 88:0e827d0d8017 811 parsed += (uint32_t)sizeof(struct olsrhdr);
tass 88:0e827d0d8017 812
tass 88:0e827d0d8017 813 datagram = pico_zalloc(DGRAM_MAX_SIZE);
tass 88:0e827d0d8017 814 if (!datagram)
tass 88:0e827d0d8017 815 return;
tass 88:0e827d0d8017 816 outsize = (uint16_t) (outsize + (sizeof(struct olsrhdr)));
tass 74:c146c4e346c4 817
tass 88:0e827d0d8017 818 /* Section 1: parsing received messages. */
tass 88:0e827d0d8017 819
tass 88:0e827d0d8017 820 while (len > parsed) {
tass 88:0e827d0d8017 821 struct olsr_route_entry *origin;
tass 88:0e827d0d8017 822 msg = (struct olsrmsg *) (buffer + parsed);
tass 88:0e827d0d8017 823 origin = get_route_by_address(Local_interfaces, msg->orig.addr);
tass 88:0e827d0d8017 824
tass 88:0e827d0d8017 825 if(pico_ipv4_link_find(&msg->orig) != NULL) {
tass 88:0e827d0d8017 826 //dbg("rebound\n");
tass 88:0e827d0d8017 827 parsed += short_be(msg->size);
tass 88:0e827d0d8017 828 continue;
tass 88:0e827d0d8017 829 }
tass 88:0e827d0d8017 830
tass 88:0e827d0d8017 831 /* OLSR's TTL expired. */
tass 88:0e827d0d8017 832 if (msg->ttl < 1u) {
tass 88:0e827d0d8017 833 parsed += short_be(msg->size);
tass 88:0e827d0d8017 834 continue;
tass 88:0e827d0d8017 835 }
tass 88:0e827d0d8017 836
tass 88:0e827d0d8017 837 if (!origin) {
tass 88:0e827d0d8017 838 arp_storm(&msg->orig);
tass 88:0e827d0d8017 839 parsed += short_be(msg->size);
tass 88:0e827d0d8017 840 continue;
tass 88:0e827d0d8017 841 }
tass 74:c146c4e346c4 842
tass 88:0e827d0d8017 843 /* We know this is a Master host and a neighbor */
tass 88:0e827d0d8017 844 origin->link_type = OLSRLINK_MPR;
tass 88:0e827d0d8017 845 origin->time_left = olsr2seconds(msg->vtime);
tass 88:0e827d0d8017 846 switch(msg->type) {
tass 88:0e827d0d8017 847 case OLSRMSG_HELLO:
tass 88:0e827d0d8017 848 ancestor = olsr_get_ethentry(origin->iface);
tass 88:0e827d0d8017 849 if ((origin->metric > 1) && ancestor) {
tass 88:0e827d0d8017 850 olsr_route_del(origin);
tass 88:0e827d0d8017 851 origin->gateway = ancestor;
tass 88:0e827d0d8017 852 origin->metric = 1;
tass 88:0e827d0d8017 853 olsr_route_add(origin);
tass 88:0e827d0d8017 854 }
tass 88:0e827d0d8017 855 recv_hello(buffer + (uint32_t)parsed + (uint32_t)sizeof(struct olsrmsg) + (uint32_t)sizeof(struct olsr_hmsg_hello),
tass 88:0e827d0d8017 856 (uint32_t) ((short_be(msg->size) - (sizeof(struct olsrmsg))) - (uint32_t)sizeof(struct olsr_hmsg_hello)),
tass 88:0e827d0d8017 857 origin);
tass 88:0e827d0d8017 858 msg->ttl = 0;
tass 88:0e827d0d8017 859 break;
tass 88:0e827d0d8017 860 case OLSRMSG_MID:
tass 88:0e827d0d8017 861 if ((origin->seq != 0) && (!fresher(short_be(msg->seq), origin->seq))) {
tass 88:0e827d0d8017 862 msg->ttl = 0;
tass 88:0e827d0d8017 863 } else {
tass 88:0e827d0d8017 864 recv_mid(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin);
tass 88:0e827d0d8017 865 //dbg("MID forwarded from origin %08x (seq: %u)\n", long_be(msg->orig.addr), short_be(msg->seq));
tass 88:0e827d0d8017 866 origin->seq = short_be(msg->seq);
tass 88:0e827d0d8017 867 }
tass 88:0e827d0d8017 868 break;
tass 88:0e827d0d8017 869 case OLSRMSG_TC:
tass 88:0e827d0d8017 870 reconsider_topology(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin);
tass 88:0e827d0d8017 871 if ((origin->seq != 0) && (!fresher(short_be(msg->seq), origin->seq))) {
tass 88:0e827d0d8017 872 msg->ttl = 0;
tass 88:0e827d0d8017 873 } else {
tass 88:0e827d0d8017 874 //dbg("TC forwarded from origin %08x (seq: %u)\n", long_be(msg->orig.addr), short_be(msg->seq));
tass 88:0e827d0d8017 875 origin->seq = short_be(msg->seq);
tass 88:0e827d0d8017 876 }
tass 88:0e827d0d8017 877 break;
tass 88:0e827d0d8017 878 default:
tass 88:0e827d0d8017 879 pico_free(datagram);
tass 88:0e827d0d8017 880 return;
tass 88:0e827d0d8017 881 }
tass 88:0e827d0d8017 882 if (msg->ttl > 1) {
tass 88:0e827d0d8017 883 msg->ttl--;
tass 88:0e827d0d8017 884 msg->hop++;
tass 88:0e827d0d8017 885 memcpy(datagram + outsize, msg, short_be(msg->size));
tass 88:0e827d0d8017 886 outsize = (uint16_t)(outsize + short_be(msg->size));
tass 88:0e827d0d8017 887 }
tass 88:0e827d0d8017 888 parsed += short_be(msg->size);
tass 88:0e827d0d8017 889 }
tass 74:c146c4e346c4 890
tass 74:c146c4e346c4 891
tass 88:0e827d0d8017 892 /* Section 2: forwarding parsed messages that got past the filter. */
tass 88:0e827d0d8017 893 if ((outsize > sizeof(struct olsrhdr))) {
tass 88:0e827d0d8017 894 /* Finalize FWD packet */
tass 88:0e827d0d8017 895 olsr_scheduled_output(OLSR_MAXJITTER, datagram, outsize, NULL);
tass 88:0e827d0d8017 896 } else {
tass 88:0e827d0d8017 897 /* Nothing to forward. */
tass 88:0e827d0d8017 898 pico_free(datagram);
tass 88:0e827d0d8017 899 }
tass 74:c146c4e346c4 900 }
tass 74:c146c4e346c4 901
tass 74:c146c4e346c4 902 static void wakeup(uint16_t ev, struct pico_socket *s)
tass 74:c146c4e346c4 903 {
tass 88:0e827d0d8017 904 unsigned char *recvbuf;
tass 74:c146c4e346c4 905 int r = 0;
tass 74:c146c4e346c4 906 struct pico_ip4 ANY = {0};
tass 74:c146c4e346c4 907 uint16_t port = OLSR_PORT;
tass 88:0e827d0d8017 908 recvbuf = pico_zalloc(DGRAM_MAX_SIZE);
tass 88:0e827d0d8017 909 if (!recvbuf)
tass 88:0e827d0d8017 910 return;
tass 74:c146c4e346c4 911
tass 74:c146c4e346c4 912 if (ev & PICO_SOCK_EV_RD) {
tass 74:c146c4e346c4 913 r = pico_socket_recv(s, recvbuf, DGRAM_MAX_SIZE);
tass 74:c146c4e346c4 914 if (r > 0)
tass 74:c146c4e346c4 915 olsr_recv(recvbuf, (uint32_t)r);
tass 74:c146c4e346c4 916 }
tass 74:c146c4e346c4 917
tass 74:c146c4e346c4 918 if (ev == PICO_SOCK_EV_ERR) {
tass 74:c146c4e346c4 919 pico_socket_close(udpsock);
tass 74:c146c4e346c4 920 udpsock = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &wakeup);
tass 74:c146c4e346c4 921 if (udpsock)
tass 74:c146c4e346c4 922 pico_socket_bind(udpsock, &ANY, &port);
tass 74:c146c4e346c4 923 }
tass 88:0e827d0d8017 924 pico_free(recvbuf);
tass 74:c146c4e346c4 925 }
tass 74:c146c4e346c4 926
tass 88:0e827d0d8017 927 static void olsr_hello_tick(uint32_t when, void *unused)
tass 74:c146c4e346c4 928 {
tass 74:c146c4e346c4 929 struct olsr_dev_entry *d;
tass 74:c146c4e346c4 930 (void)when;
tass 74:c146c4e346c4 931 (void)unused;
tass 74:c146c4e346c4 932 olsr_garbage_collector(Local_interfaces);
tass 88:0e827d0d8017 933 refresh_routes();
tass 74:c146c4e346c4 934 d = Local_devices;
tass 74:c146c4e346c4 935 while(d) {
tass 88:0e827d0d8017 936 olsr_make_dgram(d->dev, 0);
tass 74:c146c4e346c4 937 d = d->next;
tass 74:c146c4e346c4 938 }
tass 88:0e827d0d8017 939 pico_timer_add(OLSR_HELLO_INTERVAL, &olsr_hello_tick, NULL);
tass 88:0e827d0d8017 940 }
tass 88:0e827d0d8017 941
tass 88:0e827d0d8017 942 static void olsr_tc_tick(uint32_t when, void *unused)
tass 88:0e827d0d8017 943 {
tass 88:0e827d0d8017 944 struct olsr_dev_entry *d;
tass 88:0e827d0d8017 945 (void)when;
tass 88:0e827d0d8017 946 (void)unused;
tass 88:0e827d0d8017 947 d = Local_devices;
tass 88:0e827d0d8017 948 while(d) {
tass 88:0e827d0d8017 949 olsr_make_dgram(d->dev, 1);
tass 88:0e827d0d8017 950 d = d->next;
tass 88:0e827d0d8017 951 }
tass 88:0e827d0d8017 952 pico_timer_add(OLSR_TC_INTERVAL, &olsr_tc_tick, NULL);
tass 74:c146c4e346c4 953 }
tass 74:c146c4e346c4 954
tass 74:c146c4e346c4 955
tass 74:c146c4e346c4 956 /* Public interface */
tass 74:c146c4e346c4 957
tass 74:c146c4e346c4 958 void pico_olsr_init(void)
tass 74:c146c4e346c4 959 {
tass 74:c146c4e346c4 960 struct pico_ip4 ANY = {0};
tass 74:c146c4e346c4 961 uint16_t port = OLSR_PORT;
tass 74:c146c4e346c4 962 dbg("OLSR initialized.\n");
tass 74:c146c4e346c4 963 if (!udpsock) {
tass 74:c146c4e346c4 964 udpsock = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &wakeup);
tass 74:c146c4e346c4 965 if (udpsock)
tass 74:c146c4e346c4 966 pico_socket_bind(udpsock, &ANY, &port);
tass 74:c146c4e346c4 967 }
tass 88:0e827d0d8017 968 pico_timer_add(100, &olsr_hello_tick, NULL);
tass 88:0e827d0d8017 969 pico_timer_add(1100, &olsr_tc_tick, NULL);
tass 74:c146c4e346c4 970 }
tass 74:c146c4e346c4 971
tass 74:c146c4e346c4 972 int pico_olsr_add(struct pico_device *dev)
tass 74:c146c4e346c4 973 {
tass 74:c146c4e346c4 974 struct pico_ipv4_link *lnk = NULL;
tass 74:c146c4e346c4 975 struct olsr_dev_entry *od;
tass 74:c146c4e346c4 976 if (!dev) {
tass 74:c146c4e346c4 977 pico_err = PICO_ERR_EINVAL;
tass 74:c146c4e346c4 978 return -1;
tass 74:c146c4e346c4 979 }
tass 74:c146c4e346c4 980 dbg("OLSR: Adding device %s\n", dev->name);
tass 74:c146c4e346c4 981 od = pico_zalloc(sizeof(struct olsr_dev_entry));
tass 74:c146c4e346c4 982 if (!od) {
tass 74:c146c4e346c4 983 pico_err = PICO_ERR_ENOMEM;
tass 74:c146c4e346c4 984 return -1;
tass 74:c146c4e346c4 985 }
tass 74:c146c4e346c4 986 od->dev = dev;
tass 74:c146c4e346c4 987 od->next = Local_devices;
tass 74:c146c4e346c4 988 Local_devices = od;
tass 74:c146c4e346c4 989
tass 74:c146c4e346c4 990 do {
tass 88:0e827d0d8017 991 char ipaddr[20];
tass 74:c146c4e346c4 992 lnk = pico_ipv4_link_by_dev_next(dev, lnk);
tass 74:c146c4e346c4 993 if (lnk) {
tass 74:c146c4e346c4 994 struct olsr_route_entry *e = pico_zalloc(sizeof(struct olsr_route_entry));
tass 88:0e827d0d8017 995 //dbg("OLSR: Found IP address %08x\n", long_be(lnk->address.addr));
tass 88:0e827d0d8017 996 pico_ipv4_to_string(ipaddr, (lnk->address.addr));
tass 88:0e827d0d8017 997 dbg("OLSR: Found IP address %s\n", ipaddr);
tass 74:c146c4e346c4 998 if (!e) {
tass 74:c146c4e346c4 999 pico_err = PICO_ERR_ENOMEM;
tass 74:c146c4e346c4 1000 return -1;
tass 74:c146c4e346c4 1001 }
tass 74:c146c4e346c4 1002 e->destination.addr = lnk->address.addr;
tass 74:c146c4e346c4 1003 e->link_type = OLSRLINK_SYMMETRIC;
tass 88:0e827d0d8017 1004 e->time_left = (OLSR_HELLO_INTERVAL << 2);
tass 74:c146c4e346c4 1005 e->gateway = NULL;
tass 74:c146c4e346c4 1006 e->iface = dev;
tass 88:0e827d0d8017 1007 e->metric = 0;
tass 88:0e827d0d8017 1008 e->lq = 0xFF;
tass 88:0e827d0d8017 1009 e->nlq = 0xFF;
tass 88:0e827d0d8017 1010 e->next = Local_interfaces;
tass 88:0e827d0d8017 1011 Local_interfaces = e;
tass 74:c146c4e346c4 1012
tass 74:c146c4e346c4 1013 }
tass 74:c146c4e346c4 1014 } while(lnk);
tass 74:c146c4e346c4 1015 return 0;
tass 74:c146c4e346c4 1016 }
tass 74:c146c4e346c4 1017
tass 74:c146c4e346c4 1018 #endif