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