thread_routing.c

00001 /*
00002  * Copyright (c) 2014-2017, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: BSD-3-Clause
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * 1. Redistributions of source code must retain the above copyright
00009  *    notice, this list of conditions and the following disclaimer.
00010  * 2. Redistributions in binary form must reproduce the above copyright
00011  *    notice, this list of conditions and the following disclaimer in the
00012  *    documentation and/or other materials provided with the distribution.
00013  * 3. Neither the name of the copyright holder nor the
00014  *    names of its contributors may be used to endorse or promote products
00015  *    derived from this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 /*
00030  * Thread-specific routing functionality
00031  *
00032  * draft-kelsey-thread-routing-00
00033  */
00034 
00035 #include "nsconfig.h"
00036 #include <string.h>
00037 #include <ns_types.h>
00038 #include <ns_list.h>
00039 #include <randLIB.h>
00040 #include <nsdynmemLIB.h>
00041 
00042 #define THREAD_ROUTING_FN extern
00043 
00044 #include  <net_thread_test.h>
00045 #include "ns_trace.h"
00046 #include "common_functions.h"
00047 #include "NWK_INTERFACE/Include/protocol.h"
00048 #include "MLE/mle.h"
00049 #include "6LoWPAN/Mesh/mesh.h"
00050 #include "6LoWPAN/Thread/thread_common.h"
00051 #include "6LoWPAN/Thread/thread_nd.h"
00052 #include "6LoWPAN/Thread/thread_routing.h"
00053 #include "6LoWPAN/Thread/thread_leader_service.h"
00054 #include "6LoWPAN/MAC/mac_helper.h"
00055 
00056 #define TRACE_GROUP "trou"
00057 
00058 /* MLE Route Data bit assignments (draft-kelsey-thread-routing-00) */
00059 #define ROUTE_DATA_OUT_MASK     0xC0
00060 #define ROUTE_DATA_OUT_SHIFT    6
00061 #define ROUTE_DATA_IN_MASK      0x30
00062 #define ROUTE_DATA_IN_SHIFT     4
00063 #define ROUTE_DATA_COST_MASK    0x0F
00064 #define ROUTE_DATA_OURSELF      0x01
00065 
00066 /*
00067  * MAX_LINK_AGE must be > 1.5 * trickle Imax, as that's the maximum spacing
00068  * between advert transmissions (assuming all peers have same Imax, as they
00069  * should)
00070  *
00071  * |---Imax---|---Imax---|  (Two Imax intervals, transmitting at Imax/2 in first
00072  *       t               t   and Imax-1 in second, so 1.5*Imax - 1 apart).
00073  */
00074 #define MAX_LINK_AGE 100*10     /* 100 seconds */
00075 
00076 #define LINK_AGE_STATIC 0xFFF  /* Magic number to indicate "never expire" */
00077 
00078 #ifdef HAVE_THREAD_ROUTER
00079 
00080 static trickle_params_t thread_mle_advert_trickle_params = {
00081     .Imin = 1 * 10,     /* 1 second; ticks are 100ms */
00082     .Imax = 32 * 10,    /* 32 seconds */
00083     .k = 0,             /* infinity - no consistency checking */
00084     .TimerExpirations = TRICKLE_EXPIRATIONS_INFINITE
00085 };
00086 
00087 static bool thread_update_fast_route(thread_info_t *thread, thread_router_id_t dest);
00088 static void thread_update_fast_route_table(thread_info_t *thread);
00089 
00090 bool router_id_sequence_is_greater(const thread_routing_info_t *routing, uint8_t seq )
00091 {
00092     return !routing->router_id_sequence_valid || common_serial_number_greater_8(seq, routing->router_id_sequence);
00093 }
00094 
00095 /*
00096  * Hysteresis for quality changes in dB. Note that in this implementation, given
00097  * the thresholds, this must be less than 2dB. At 1dB, quality will switch from
00098  * "2dB" to "BAD" only when link margin drops below 1dB.
00099  */
00100 #define LINK_QUALITY_HYSTERESIS (1 << THREAD_LINK_MARGIN_SCALING)   /* 1dB */
00101 
00102 static thread_link_margin_t link_quality_to_margin_lower_bound(thread_link_quality_e quality)
00103 {
00104     switch (quality) {
00105         case QUALITY_20dB:
00106             return 20 << THREAD_LINK_MARGIN_SCALING;
00107         case QUALITY_10dB:
00108             return 10 << THREAD_LINK_MARGIN_SCALING;
00109         case QUALITY_2dB:
00110             return 2 << THREAD_LINK_MARGIN_SCALING;
00111         default:
00112             return 0;
00113     }
00114 }
00115 
00116 static thread_link_margin_t link_quality_to_margin_upper_bound(thread_link_quality_e quality)
00117 {
00118     switch (quality) {
00119         case QUALITY_10dB:
00120             return 20 << THREAD_LINK_MARGIN_SCALING;
00121         case QUALITY_2dB:
00122             return 10 << THREAD_LINK_MARGIN_SCALING;
00123         case QUALITY_BAD:
00124             return 2 << THREAD_LINK_MARGIN_SCALING;
00125         default:
00126             return THREAD_LINK_MARGIN_MAX;
00127     }
00128 }
00129 static thread_link_quality_e link_margin_to_quality_with_hysteresis(thread_link_margin_t margin, thread_link_quality_e old_quality)
00130 {
00131     thread_link_quality_e new_quality = thread_link_margin_to_quality(margin);
00132 
00133     if ((new_quality > old_quality && margin > link_quality_to_margin_upper_bound(old_quality) + LINK_QUALITY_HYSTERESIS) ||
00134             (new_quality < old_quality && margin < link_quality_to_margin_lower_bound(old_quality) - LINK_QUALITY_HYSTERESIS)) {
00135         return new_quality;
00136     } else {
00137         return old_quality;
00138     }
00139 }
00140 
00141 static bool cost_less(thread_route_cost_t a, thread_route_cost_t b)
00142 {
00143     if (a == THREAD_COST_INFINITE) {
00144         return false;
00145     } else if (b == THREAD_COST_INFINITE) {
00146         return true;
00147     } else {
00148         return a < b;
00149     }
00150 }
00151 
00152 bool thread_i_am_router(const protocol_interface_info_entry_t *cur)
00153 {
00154     return cur->thread_info && cur->mac_parameters && thread_is_router_addr(mac_helper_mac16_address_get(cur));
00155 }
00156 
00157 
00158 /* Look up a router in our list of neighbour routers - return NULL if not a neighbour */
00159 static thread_router_link_t *thread_get_neighbour_router_by_id(thread_routing_info_t *routing, thread_router_id_t id)
00160 {
00161     ns_list_foreach(thread_router_link_t, neighbour, &routing->link_set) {
00162         if (neighbour->router_id == id) {
00163             return neighbour;
00164         }
00165     }
00166 
00167     return NULL;
00168 }
00169 
00170 static inline thread_link_quality_e thread_quality_combine(thread_link_quality_e in, thread_link_quality_e out)
00171 {
00172     thread_link_quality_e q;
00173 
00174     if (out < in) {
00175         q = out;
00176     } else {
00177         q = in;
00178     }
00179 
00180     return q;
00181 }
00182 
00183 /* Return the quality (worse of incoming and outgoing quality) for a neighbour router */
00184 static inline thread_link_quality_e thread_neighbour_router_quality(const thread_router_link_t *neighbour)
00185 {
00186     return thread_quality_combine(neighbour->incoming_quality, neighbour->outgoing_quality);
00187 }
00188 
00189 
00190 /* Return the quality for a neighbour router; return QUALITY_BAD if not a neighbour */
00191 static thread_link_quality_e thread_get_neighbour_router_quality(thread_routing_info_t *routing, thread_router_id_t id)
00192 {
00193     thread_router_link_t *neighbour = thread_get_neighbour_router_by_id(routing, id);
00194     if (!neighbour) {
00195         return QUALITY_BAD;
00196     }
00197 
00198     return thread_neighbour_router_quality(neighbour);
00199 }
00200 
00201 /* Return the routing cost for a neighbour router */
00202 static thread_route_cost_t thread_neighbour_router_cost(const thread_router_link_t *neighbour)
00203 {
00204     return thread_link_quality_to_cost(thread_neighbour_router_quality(neighbour));
00205 }
00206 
00207 /* Return the routing cost for a neighbour router; return THREAD_COST_INFINITE if
00208  * not a neighbour.
00209  */
00210 static thread_route_cost_t thread_get_neighbour_router_cost(thread_routing_info_t *routing, thread_router_id_t id)
00211 {
00212     return thread_link_quality_to_cost(thread_get_neighbour_router_quality(routing, id));
00213 }
00214 
00215 static thread_route_t *thread_get_route_entry_by_id(thread_routing_info_t *routing, thread_router_id_t id)
00216 {
00217     ns_list_foreach(thread_route_t, r, &routing->route_set) {
00218         if (r->destination == id) {
00219             return r;
00220         }
00221     }
00222 
00223     return NULL;
00224 }
00225 
00226 static thread_route_t *thread_delete_route_entry_by_id(thread_info_t *thread, thread_router_id_t id)
00227 {
00228     thread_route_t *r = thread_get_route_entry_by_id(&thread->routing, id);
00229     if (r) {
00230         ns_list_remove(&thread->routing.route_set, r);
00231         ns_dyn_mem_free(r);
00232         if (thread_update_fast_route(thread, id)) {
00233             trickle_inconsistent_heard(&thread->routing.mle_advert_timer, &thread_mle_advert_trickle_params);
00234         }
00235     }
00236 
00237     return NULL;
00238 }
00239 
00240 /* Routing function for Mesh layer */
00241 static int_fast8_t thread_route_fn(
00242     protocol_interface_info_entry_t *cur,
00243     uint_fast8_t last_hop_addr_len, const uint8_t last_hop_addr[last_hop_addr_len],
00244     uint_fast8_t addr_len, const uint8_t dest_addr[addr_len],
00245     mesh_routing_route_response_t *resp)
00246 {
00247     if (addr_len != 2) {
00248         return -1;
00249     }
00250     uint16_t mac16 = mac_helper_mac16_address_get(cur);
00251     thread_info_t *thread = cur->thread_info;
00252     if (!thread  || mac16 >= 0xfffe) {
00253         return -1;
00254     }
00255 
00256 
00257     if (!thread_is_router_addr(mac16)) {
00258         /* We're just a leaf - always send to our parent router */
00259         resp->addr_len = 2;
00260         common_write_16_bit(thread_router_addr_from_addr(mac16), resp->address);
00261         return 0;
00262     }
00263 
00264     uint16_t dest = common_read_16_bit(dest_addr);
00265     uint16_t dest_router_addr = thread_router_addr_from_addr(dest);
00266     if (dest_router_addr == mac16) {
00267         /* We're this device's parent - transmit direct to it */
00268         mle_neigh_table_entry_t *entry = mle_class_get_by_link_address(cur->id, dest_addr, ADDR_802_15_4_SHORT );
00269         if (!entry || (entry->mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
00270             /* To cover some of draft-kelsey-thread-network-data-00, we send the
00271              * packet up to our own IP layer in the case where it's addressed to
00272              * an unrecognised child. The special IP forwarding rules can then
00273              * generate an ICMP message. Also catching reduced function device here.
00274              */
00275             resp->intercept = true;
00276             return 0;
00277         }
00278         resp->addr_len = 2;
00279         common_write_16_bit(dest, resp->address);
00280         return 0;
00281     }
00282 
00283     thread_router_id_t dest_router_id = thread_router_id_from_addr(dest_router_addr);
00284     if (thread->routing.fast_route_table[dest_router_id] >= N_THREAD_ROUTERS) {
00285         return -1;
00286     }
00287 
00288     thread_router_id_t next_hop_router_id = thread->routing.fast_route_table[dest_router_id];
00289     uint16_t next_hop_router_addr = thread_router_addr_from_id(next_hop_router_id);
00290 
00291     /* 2-hop loop detection required by Thread spec */
00292     if (last_hop_addr_len == 2 && next_hop_router_addr == common_read_16_bit(last_hop_addr)) {
00293         tr_debug("%d: two-hop loop detected", dest_router_id);
00294         thread_delete_route_entry_by_id(thread, dest_router_id);
00295         // Don't drop packet - we still forward, which gives other hop
00296         // a chance to detect the loop. If it comes back again, then we
00297         // won't have a route that time. (Spec isn't explicit here -
00298         // found this works better than dropping).
00299     }
00300 
00301     resp->addr_len = 2;
00302     common_write_16_bit(next_hop_router_addr, resp->address);
00303     return 0;
00304 }
00305 
00306 /* First hop function for Mesh layer */
00307 static int_fast8_t thread_first_hop_fn(
00308     protocol_interface_info_entry_t *cur,
00309     uint_fast8_t addr_len, const uint8_t dest_addr[addr_len],
00310     mesh_routing_route_response_t *resp)
00311 {
00312     /* Re-use the routing function */
00313     return thread_route_fn(cur, 0, NULL, addr_len, dest_addr, resp);
00314 }
00315 
00316 static thread_route_cost_t thread_compute_route_cost(thread_routing_info_t *routing, thread_router_id_t dest, thread_router_id_t *next_hop)
00317 {
00318     /* Never attempt to route if router ID is known invalid */
00319     if (routing->fast_route_table[dest] == FAST_ROUTE_INVALID_ID) {
00320         return THREAD_COST_INFINITE;
00321     }
00322 
00323     /* Check cost for direct transmission */
00324     thread_route_cost_t cost_direct = thread_get_neighbour_router_cost(routing, dest);
00325 
00326     /* Then cost for multihop transmission */
00327     thread_route_cost_t cost_multihop = THREAD_COST_INFINITE;
00328     thread_route_t *route_entry = thread_get_route_entry_by_id(routing, dest);
00329     if (route_entry) {
00330         thread_route_cost_t next_hop_cost = thread_get_neighbour_router_cost(routing, route_entry->next_hop);
00331         cost_multihop = thread_link_cost_sum(route_entry->route_cost, next_hop_cost);
00332     }
00333 
00334     if (cost_direct == THREAD_COST_INFINITE && cost_multihop == THREAD_COST_INFINITE) {
00335         return THREAD_COST_INFINITE;
00336     } else {
00337         if (route_entry && cost_less(cost_multihop, cost_direct)) {
00338             if (next_hop) {
00339                 *next_hop = route_entry->next_hop;
00340             }
00341             return cost_multihop;
00342         } else {
00343             if (next_hop) {
00344                 *next_hop = dest;
00345             }
00346             return cost_direct;
00347         }
00348     }
00349 }
00350 
00351 /* Called by thread_routing.c as a result of updates to routing table - allows
00352  * leader to monitor a router being available (having a finite route cost).
00353  * Possible values for cost are 1-15, or 0 meaning infinite.
00354  */
00355 static void thread_router_link_changed(thread_info_t *thread_info, uint8_t router_id, uint8_t cost, int8_t interface_id)
00356 {
00357     /* Convert cost to scale to 0 (= cost 1) to 15 (= cost infinite), and place in bottom 4 bits of metric */
00358     uint8_t metric = ((unsigned) cost - 1) & 0xf;
00359 
00360     uint16_t router_addr_16 = thread_router_addr_from_id(router_id);
00361     uint8_t router_ip_addr[16];
00362     thread_addr_write_mesh_local_16(router_ip_addr, router_addr_16, thread_info);
00363 
00364     /* Leave upper (preference) bits of metric untouched */
00365     ipv6_route_table_modify_router_metric(interface_id, router_ip_addr, ROUTE_THREAD_BORDER_ROUTER, 0xf0, metric);
00366 
00367     /* Also tell the leader, who's monitoring which are available */
00368     thread_leader_service_router_state_changed(thread_info, router_id, cost != 0, interface_id);
00369 
00370 }
00371 
00372 static void set_fast_route_entry(thread_info_t *thread, thread_router_id_t dest, uint8_t value, thread_route_cost_t cost)
00373 {
00374     thread->routing.fast_route_table[dest] = value;
00375     thread_router_link_changed(thread, dest, cost, thread->interface_id);
00376 }
00377 
00378 /* Returns true if a change relevant to MLE Trickle has occurred */
00379 static bool thread_update_fast_route(thread_info_t *thread, thread_router_id_t dest)
00380 {
00381     thread_routing_info_t *routing = &thread->routing;
00382 
00383     if (routing->fast_route_table[dest] == FAST_ROUTE_INVALID_ID) {
00384         return false;
00385     }
00386 
00387     /* Trickle only cares if routes come or go, not if they change */
00388     bool change = false;
00389     thread_router_id_t next_hop;
00390     thread_route_cost_t cost = thread_compute_route_cost(routing, dest, &next_hop);
00391     if (cost == THREAD_COST_INFINITE) {
00392         if (routing->fast_route_table[dest] != FAST_ROUTE_NO_ROUTE) {
00393             change = true;
00394         }
00395 
00396         set_fast_route_entry(thread, dest, FAST_ROUTE_NO_ROUTE, THREAD_COST_INFINITE);
00397     } else {
00398         if (routing->fast_route_table[dest] == FAST_ROUTE_NO_ROUTE) {
00399             change = true;
00400         }
00401 
00402         set_fast_route_entry(thread, dest, next_hop, cost);
00403     }
00404 
00405     return change;
00406 }
00407 
00408 /* Rescan the neighbour list to generate the overall routing table. "id_valid"
00409  * fields are always live - this updates the other fields.
00410  */
00411 static void thread_update_fast_route_table(thread_info_t *thread)
00412 {
00413     bool change = false;
00414 
00415     for (thread_router_id_t id = 0; id < N_THREAD_ROUTERS; id++) {
00416         change |= thread_update_fast_route(thread, id);
00417     }
00418 
00419     if (change) {
00420         trickle_inconsistent_heard(&thread->routing.mle_advert_timer, &thread_mle_advert_trickle_params);
00421     }
00422 }
00423 
00424 void thread_routing_update_id_set(protocol_interface_info_entry_t *cur, uint8_t seq , const uint8_t *id_mask)
00425 {
00426     thread_info_t *thread = cur->thread_info;
00427     thread_routing_info_t *routing = &thread->routing;
00428     bool change = false;
00429 
00430     if (!router_id_sequence_is_greater(routing, seq)) {
00431         return;
00432     }
00433     routing->router_id_sequence = seq ;
00434     routing->router_id_sequence_valid = true;
00435 
00436     for (thread_router_id_t i = 0; i < N_THREAD_ROUTERS; i++) {
00437         if (bit_test(id_mask, i)) {
00438             if (routing->fast_route_table[i] == FAST_ROUTE_INVALID_ID) {
00439                 /* Won't have any route info for this new router ID, but may have
00440                  * some existing link quality data from a handshake. Set
00441                  * initial route to direct, if we think we can hear it.
00442                  */
00443                 thread_route_cost_t cost = thread_get_neighbour_router_cost(routing, i);
00444                 if (cost == THREAD_COST_INFINITE) {
00445                     set_fast_route_entry(thread, i, FAST_ROUTE_NO_ROUTE, THREAD_COST_INFINITE);
00446                 } else {
00447                     set_fast_route_entry(thread, i, i, cost);
00448                     change = true;
00449                 }
00450                 tr_info("Add router (ID: %d)", i);
00451             }
00452         } else {
00453             if (routing->fast_route_table[i] != FAST_ROUTE_INVALID_ID) {
00454                 if (routing->fast_route_table[i] != FAST_ROUTE_NO_ROUTE) {
00455                     change = true;
00456                 }
00457                 set_fast_route_entry(thread, i, FAST_ROUTE_INVALID_ID, THREAD_COST_INFINITE);
00458                 tr_info("Remove router (ID: %d)", i);
00459                 thread_nd_flush_neighbour_cache_for_short_addr(cur, thread_router_addr_from_id(i), true);
00460             }
00461         }
00462     }
00463 
00464     /* Transitions from invalid->finite or finite->invalid must kick timer */
00465     if (change) {
00466         trickle_inconsistent_heard(&routing->mle_advert_timer, &thread_mle_advert_trickle_params);
00467     }
00468 }
00469 
00470 void thread_routing_force_next_hop(protocol_interface_info_entry_t *cur, uint8_t id_seq, const uint8_t *id_mask, thread_router_id_t next_hop_id)
00471 {
00472     /*
00473      * Called when we become a router, to make our original parent be
00474      * the initial path to all routers.
00475      *
00476      * Do this by creating and processing a fake advertisement from the
00477      * next hop. (We assume we have no existing routing information, so
00478      * it wins and ends up being our route to everywhere). Real routing
00479      * information, either from other routers or the one we're faking,
00480      * will then take over later.
00481      */
00482     uint8_t route_data[64];
00483     uint8_t *ptr = route_data;
00484     thread_router_id_t my_router_id = thread_router_id_from_addr(mac_helper_mac16_address_get(cur));
00485 
00486     for (thread_router_id_t r = 0; r < N_THREAD_ROUTERS; r++) {
00487         if (!bit_test(id_mask, r)) {
00488             continue;
00489         }
00490 
00491         if (r == my_router_id) {
00492             // It has a bad link to us
00493             *ptr++ = (QUALITY_2dB << ROUTE_DATA_OUT_SHIFT) | (QUALITY_2dB << ROUTE_DATA_IN_SHIFT) | thread_link_quality_to_cost(QUALITY_2dB);
00494         } else if (r == next_hop_id) {
00495             // It reports itself correctly
00496             *ptr++ = ROUTE_DATA_OURSELF;
00497         } else {
00498             // It has a maximum-length route to every other router (and no direct link)
00499             *ptr++ = (QUALITY_BAD << ROUTE_DATA_OUT_SHIFT) | (QUALITY_BAD << ROUTE_DATA_IN_SHIFT) | (THREAD_MAX_ROUTE_COST - thread_link_quality_to_cost(QUALITY_2dB));
00500         }
00501     }
00502 
00503     if (thread_routing_add_link(cur, thread_router_addr_from_id(next_hop_id), 4 /* dB (bad) */,
00504                                 id_seq, id_mask, route_data, false)) {
00505         tr_warn("Function thread_routing_force_next_hop() failed");
00506     }
00507 }
00508 
00509 /* We leave it to the caller to give us the dB link margin Thread wants.
00510  *
00511  * Getting that will be hardware dependent - new RF API needed...
00512  */
00513 
00514 static thread_router_link_t *thread_routing_update_link_margin_internal(thread_info_t *thread,
00515         uint16_t sender,
00516         uint8_t link_margin_db)
00517 {
00518     thread_router_id_t sender_id = thread_router_id_from_addr(sender);
00519 
00520     /* Find the sender in our Link Set - creating an entry if necessary */
00521     thread_router_link_t *neighbour = thread_get_neighbour_router_by_id(&thread->routing, sender_id);
00522     if (!neighbour) {
00523         neighbour = ns_dyn_mem_alloc(sizeof * neighbour);
00524         if (!neighbour) {
00525             return NULL;
00526         }
00527         neighbour->router_id = sender_id;
00528         neighbour->link_margin = link_margin_db << THREAD_LINK_MARGIN_SCALING;
00529         neighbour->incoming_quality = thread_link_margin_to_quality(neighbour->link_margin);
00530         neighbour->outgoing_quality = QUALITY_BAD;
00531         neighbour->outgoing_quality_known = false;
00532         neighbour->as_good = false;
00533         ns_list_add_to_end(&thread->routing.link_set, neighbour);
00534         trickle_inconsistent_heard(&thread->routing.mle_advert_timer, &thread_mle_advert_trickle_params); //Reset Trigle when learn new link
00535     } else {
00536         /* Exponentially weighted moving average, weighted by THREAD_LINK_MARGIN_SCALING */
00537         neighbour->link_margin = neighbour->link_margin + link_margin_db - (neighbour->link_margin >> THREAD_LINK_MARGIN_SCALING);
00538         neighbour->incoming_quality = link_margin_to_quality_with_hysteresis(neighbour->link_margin, (thread_link_quality_e) neighbour->incoming_quality);
00539     }
00540     neighbour->age = 0;
00541 
00542     return neighbour;
00543 }
00544 
00545 int_fast8_t thread_routing_update_link_margin(protocol_interface_info_entry_t *cur,
00546         uint16_t sender,
00547         uint8_t link_margin_db,
00548         uint8_t outgoing_link_margin_db)
00549 {
00550     thread_info_t *thread = cur->thread_info;
00551     /* Sanity check that the source is a Thread router */
00552     if (!thread || !thread_is_router_addr(sender)) {
00553         return -2;
00554     }
00555 
00556     tr_debug("New margin info for %04x: in=%d, out=%d", sender, link_margin_db, outgoing_link_margin_db);
00557 
00558     /* We record link quality info even if we're not currently a router - we can
00559      * use the info when we become one, and as part of the decision about
00560      * whether to become one.
00561      */
00562 
00563     thread_router_link_t *neighbour = thread_routing_update_link_margin_internal(thread, sender, link_margin_db);
00564     if (!neighbour) {
00565         return -3;
00566     }
00567 
00568     /* We use the outgoing link margin provided here only if we haven't ever
00569      * heard an actual Route TLV quality report from the neighbour. This is
00570      * intended for "secondary" info, derived from RSSI TLVs to bootstrap.
00571      */
00572     if (!neighbour->outgoing_quality_known) {
00573         neighbour->outgoing_quality = thread_link_margin_to_quality(outgoing_link_margin_db);
00574     }
00575 
00576     thread_update_fast_route_table(thread);
00577 
00578     return 0;
00579 }
00580 
00581 int_fast8_t thread_routing_force_link_margin(protocol_interface_info_entry_t *cur,
00582         uint16_t addr,
00583         uint8_t link_margin_db)
00584 {
00585     thread_info_t *thread = cur->thread_info;
00586     if (!thread || !cur->mac_parameters || !thread_is_router_addr(addr)) {
00587         return -2;
00588     }
00589 
00590     thread_router_id_t sender_id = thread_router_id_from_addr(addr);
00591     thread_router_link_t *neighbour = thread_get_neighbour_router_by_id(&thread->routing, sender_id);
00592     if (!neighbour) {
00593         return -3;
00594     }
00595 
00596     tr_debug("Forcing link margin for %04x: in=%d", addr, link_margin_db);
00597 
00598     neighbour->link_margin = link_margin_db << THREAD_LINK_MARGIN_SCALING;
00599     neighbour->incoming_quality = thread_link_margin_to_quality(neighbour->link_margin);
00600 
00601     thread_update_fast_route_table(thread);
00602 
00603     return 0;
00604 }
00605 
00606 int_fast8_t thread_routing_add_link(protocol_interface_info_entry_t *cur,
00607                                     uint16_t sender, uint8_t link_margin_db,
00608                                     uint8_t id_seq,
00609                                     const uint8_t *id_mask,
00610                                     const uint8_t *route_data,
00611                                     bool is_static)
00612 {
00613     thread_info_t *thread = cur->thread_info;
00614     if (!thread) {
00615         return -2;
00616     }
00617 
00618     /* Update master Router ID Set */
00619     thread_routing_update_id_set(cur, id_seq, id_mask);
00620 
00621     /* Sanity check that the source is a Thread router */
00622     if (!thread_is_router_addr(sender)) {
00623         return -2;
00624     }
00625 
00626     /* Even if not currently a router, worth tracking incoming link margin in case we become one */
00627     thread_router_link_t *neighbour = thread_routing_update_link_margin_internal(thread, sender, link_margin_db);
00628     if (!neighbour) {
00629         return -3;
00630     }
00631 
00632     /* Now check that we're a router - if not, we don't bother with anything further */
00633     if (!thread_is_router_addr(mac_helper_mac16_address_get(cur))) {
00634         return 0;
00635     }
00636 
00637     thread_router_id_t sender_id = thread_router_id_from_addr(sender);
00638     thread_router_id_t my_router_id = thread_router_id_from_addr(mac_helper_mac16_address_get(cur));
00639 
00640     neighbour->age = is_static ? LINK_AGE_STATIC : 0;
00641 
00642     /* We have to check its quality report for us first - need to have
00643      * this information before making routing decisions.
00644      */
00645     const uint8_t *ptr = route_data;
00646     for (thread_router_id_t r = 0; r < N_THREAD_ROUTERS; r++) {
00647         uint8_t byte;
00648         bool router_in_mask = bit_test(id_mask, r);
00649 
00650         if (router_in_mask) {
00651             byte = *ptr++;
00652         } else {
00653             byte = (QUALITY_BAD << ROUTE_DATA_OUT_SHIFT) | (QUALITY_BAD << ROUTE_DATA_IN_SHIFT) | THREAD_COST_INFINITE;
00654         }
00655 
00656         if (r == my_router_id) {
00657             neighbour->outgoing_quality_known = router_in_mask;
00658             neighbour->outgoing_quality = (byte & ROUTE_DATA_IN_MASK) >> ROUTE_DATA_IN_SHIFT;
00659             break;
00660         }
00661     }
00662 
00663     /* Compute Thread REED downgrade condition - does this router have as good or better quality links
00664      * to all Routers for which we have two-way link quality 2 (10dB) or better?
00665      */
00666     neighbour->as_good = true;
00667     ptr = route_data;
00668     for (thread_router_id_t r = 0; r < N_THREAD_ROUTERS; r++) {
00669         uint8_t byte;
00670         bool router_in_mask = bit_test(id_mask, r);
00671 
00672         if (router_in_mask) {
00673             byte = *ptr++;
00674         } else {
00675             byte = (QUALITY_BAD << ROUTE_DATA_OUT_SHIFT) | (QUALITY_BAD << ROUTE_DATA_IN_SHIFT) | THREAD_COST_INFINITE;
00676         }
00677 
00678         if (r == sender_id) {
00679             continue;
00680         }
00681 
00682         thread_router_link_t *other_neighbour = thread_get_neighbour_router_by_id(&thread->routing, r);
00683         if (!other_neighbour) {
00684             continue;
00685         }
00686 
00687         thread_link_quality_e our_quality_to_other_neighbour = thread_neighbour_router_quality(other_neighbour);
00688         if (our_quality_to_other_neighbour < QUALITY_10dB) {
00689             continue;
00690         }
00691         thread_link_quality_e neighbours_incoming_quality_to_other_neighbour = (byte & ROUTE_DATA_IN_MASK) >> ROUTE_DATA_IN_SHIFT;
00692         thread_link_quality_e neighbours_outgoing_quality_to_other_neighbour = (byte & ROUTE_DATA_OUT_MASK) >> ROUTE_DATA_OUT_SHIFT;
00693         thread_link_quality_e neighbours_quality_to_other_neighbour = thread_quality_combine(neighbours_incoming_quality_to_other_neighbour,
00694                                                                                              neighbours_outgoing_quality_to_other_neighbour);
00695         if (neighbours_quality_to_other_neighbour < our_quality_to_other_neighbour) {
00696             neighbour->as_good = false;
00697             break;
00698         }
00699     }
00700 
00701     /* Now go through and update routes based on its data */
00702     for (thread_router_id_t r = 0; r < N_THREAD_ROUTERS; r++) {
00703         uint8_t byte;
00704 
00705         /* If a router isn't in its ID set (but remains in ours), we treat
00706          * it as if it's saying "no route"+"quality bad".
00707          */
00708         if (bit_test(id_mask, r)) {
00709             byte = *route_data++;
00710         } else {
00711             byte = (QUALITY_BAD << ROUTE_DATA_OUT_SHIFT) | (QUALITY_BAD << ROUTE_DATA_IN_SHIFT) | THREAD_COST_INFINITE;
00712         }
00713 
00714         /* Only _after_ consuming route data do we skip invalid IDs */
00715         if (thread->routing.fast_route_table[r] == FAST_ROUTE_INVALID_ID) {
00716             continue;
00717         }
00718 
00719         if (r == sender_id || r == my_router_id) {
00720             continue;
00721         }
00722 
00723         thread_route_t *route_entry = thread_get_route_entry_by_id(&thread->routing, r);
00724         thread_route_cost_t cost_reported = byte & ROUTE_DATA_COST_MASK;
00725         if (cost_reported == 0) {
00726             /* If sender was our next hop to r, but it no longer has a route */
00727             if (route_entry && route_entry->next_hop == sender_id) {
00728                 /* Really delete the route? I guess the model in the spec
00729                  * doesn't leave us with the state info to choose an alternative
00730                  * immediately, so we do have to wait for more adverts...
00731                  */
00732                 tr_debug("Delete Route by Cost Report: NH=%d dest=%d", route_entry->next_hop, route_entry->destination);
00733                 ns_list_remove(&thread->routing.route_set, route_entry);
00734                 ns_dyn_mem_free(route_entry);
00735             }
00736         } else {
00737             /* If we have no existing multihop route, or if this is our
00738              * existing multihop route, or it's better than our existing one */
00739             if (route_entry == NULL ||
00740                     route_entry->next_hop == sender_id ||
00741                     cost_less(thread_link_cost_sum(thread_neighbour_router_cost(neighbour), cost_reported),
00742                               thread_link_cost_sum(thread_get_neighbour_router_cost(&thread->routing, route_entry->next_hop), route_entry->route_cost))) {
00743                 if (!route_entry) {
00744                     route_entry = ns_dyn_mem_alloc(sizeof * route_entry);
00745                     if (!route_entry) {
00746                         continue;
00747                     }
00748                     route_entry->destination = r;
00749                     ns_list_add_to_end(&thread->routing.route_set, route_entry);
00750                 } else {
00751                     tr_debug("Update Old %d D, %d NH, %d C", route_entry->destination, route_entry->next_hop, route_entry->route_cost);
00752                 }
00753                 route_entry->next_hop = sender_id;
00754                 route_entry->route_cost = cost_reported;
00755             }
00756         }
00757     }
00758 
00759     thread_update_fast_route_table(thread);
00760 
00761     return 0;
00762 }
00763 
00764 static void delete_link(thread_info_t *thread, thread_router_link_t *link)
00765 {
00766     thread_router_id_t id = link->router_id;
00767     thread_routing_info_t *routing = &thread->routing;
00768 
00769     /* Remove entry from the link set */
00770     ns_list_remove(&routing->link_set, link);
00771     ns_dyn_mem_free(link);
00772 
00773     /* Remove all routing entries for which that link was the next hop */
00774     ns_list_foreach_safe(thread_route_t, route_entry, &routing->route_set) {
00775         if (route_entry->next_hop == id) {
00776             ns_list_remove(&routing->route_set, route_entry);
00777             ns_dyn_mem_free(route_entry);
00778         }
00779     }
00780 
00781     thread_update_fast_route_table(thread);
00782 }
00783 
00784 int_fast8_t thread_routing_remove_link(protocol_interface_info_entry_t *cur,
00785                                        uint16_t sender)
00786 {
00787     thread_info_t *thread = cur->thread_info;
00788     /* Sanity check that the source is a Thread router */
00789     if (!thread || !thread_is_router_addr(sender)) {
00790         return -2;
00791     }
00792 
00793     thread_router_id_t sender_id = thread_router_id_from_addr(sender);
00794     thread_router_link_t *neighbour = thread_get_neighbour_router_by_id(&thread->routing, sender_id);
00795     if (!neighbour) {
00796         return -1;
00797     }
00798 
00799     delete_link(thread, neighbour);
00800 
00801     return 0;
00802 }
00803 
00804 uint8_t thread_routing_get_route_data_size(protocol_interface_info_entry_t *cur)
00805 {
00806     uint_fast8_t len = 0;
00807     thread_info_t *thread = cur->thread_info;
00808     if (!thread || !thread->routing.router_id_sequence_valid) {
00809         return 0;
00810     }
00811 
00812     for (thread_router_id_t r = 0; r < N_THREAD_ROUTERS; r++) {
00813         if (thread->routing.fast_route_table[r] != FAST_ROUTE_INVALID_ID) {
00814             len++;
00815         }
00816     }
00817 
00818     return len;
00819 }
00820 
00821 uint_fast8_t thread_routing_cost_get_by_router_id(thread_routing_info_t *routing , uint8_t routerId)
00822 {
00823     return thread_compute_route_cost(routing, routerId, NULL);
00824 }
00825 
00826 int_fast8_t thread_routing_get_route_data(protocol_interface_info_entry_t *cur,
00827         uint8_t *id_seq,
00828         uint8_t *id_mask,
00829         uint8_t *data,
00830         uint8_t *len_out)
00831 {
00832     uint8_t len = 0;
00833     thread_info_t *thread = cur->thread_info;
00834     if (!thread || !thread->routing.router_id_sequence_valid) {
00835         return -1;
00836     }
00837 
00838     uint16_t mac16 = mac_helper_mac16_address_get(cur);
00839 
00840     *id_seq = thread->routing.router_id_sequence;
00841     memset(id_mask, 0, (N_THREAD_ROUTERS + 7) / 8);
00842 
00843     for (thread_router_id_t r = 0; r < N_THREAD_ROUTERS; r++) {
00844         uint8_t val;
00845 
00846         if (thread->routing.fast_route_table[r] == FAST_ROUTE_INVALID_ID) {
00847             continue;
00848         }
00849 
00850         bit_set(id_mask, r);
00851 
00852         if (thread_router_addr_from_id(r) == mac16) {
00853             val = ROUTE_DATA_OURSELF;
00854         } else {
00855             thread_router_link_t *link = thread_get_neighbour_router_by_id(&thread->routing, r);
00856             if (link) {
00857                 val = (link->outgoing_quality << ROUTE_DATA_OUT_SHIFT)
00858                       | (link->incoming_quality << ROUTE_DATA_IN_SHIFT);
00859             } else {
00860                 val = (QUALITY_BAD << ROUTE_DATA_OUT_SHIFT)
00861                       | (QUALITY_BAD << ROUTE_DATA_IN_SHIFT);
00862             }
00863 
00864             val |= thread_compute_route_cost(&thread->routing, r, NULL);
00865         }
00866 
00867         *data++ = val;
00868         len++;
00869     }
00870 
00871     *len_out = len;
00872 
00873     return 0;
00874 }
00875 
00876 void thread_routing_init(thread_routing_info_t *routing)
00877 {
00878     ns_list_init(&routing->route_set);
00879     ns_list_init(&routing->link_set);
00880     routing->networkIdTimeout = NETWORK_ID_TIMEOUT;
00881     thread_routing_reset(routing);
00882 }
00883 
00884 void thread_routing_reset(thread_routing_info_t *routing)
00885 {
00886     thread_routing_free(routing);
00887     routing->router_id_sequence_valid = false;
00888     memset(routing->fast_route_table, FAST_ROUTE_INVALID_ID, sizeof routing->fast_route_table);
00889     trickle_start(&routing->mle_advert_timer, &thread_mle_advert_trickle_params);
00890 }
00891 
00892 void thread_routing_free(thread_routing_info_t *routing)
00893 {
00894     ns_list_foreach_safe(thread_route_t, route_entry, &routing->route_set) {
00895         ns_list_remove(&routing->route_set, route_entry);
00896         ns_dyn_mem_free(route_entry);
00897     }
00898 
00899     ns_list_foreach_safe(thread_router_link_t, link_entry, &routing->link_set) {
00900         ns_list_remove(&routing->link_set, link_entry);
00901         ns_dyn_mem_free(link_entry);
00902     }
00903 }
00904 
00905 void thread_routing_activate(thread_routing_info_t *routing)
00906 {
00907     trickle_inconsistent_heard(&routing->mle_advert_timer, &thread_mle_advert_trickle_params);
00908     routing->activated = true;
00909     routing->networkFragmentationTimer = 0;
00910 }
00911 
00912 void thread_routing_deactivate(thread_routing_info_t *routing)
00913 {
00914     thread_routing_reset(routing);
00915     routing->activated = false;
00916 }
00917 
00918 /* ticks is in 100ms units, I think */
00919 /* Return true if we want to send an MLE advertisement */
00920 bool thread_routing_timer(thread_info_t *thread, uint8_t ticks)
00921 {
00922     thread_routing_info_t *routing = &thread->routing;
00923 
00924     if (!routing->activated) {
00925         return false;
00926     }
00927 
00928     ns_list_foreach_safe(thread_router_link_t, link, &routing->link_set) {
00929         if (link->age == LINK_AGE_STATIC) {
00930             continue;
00931         }
00932 
00933         link->age += ticks;
00934         if (link->age > MAX_LINK_AGE) {
00935             delete_link(thread, link);
00936         }
00937     }
00938 
00939     return trickle_timer(&routing->mle_advert_timer, &thread_mle_advert_trickle_params, ticks);
00940 }
00941 
00942 /* This is experimental and is testing if we can improve long topology networks to be more stable without
00943  * resetting the trickle every time we hear inconsistency.
00944  * This speeds up next advertisement when connection restored
00945  *
00946  * Solution proposal 2:
00947  * turn on consistency checking, treat leader data changes as inconsistent, and matching leader data
00948  * as consistent if we've sent an advertisement recently.
00949  * Needs specification update. Does cause more resetting of trickle.
00950  *
00951  */
00952 
00953 //
00954 //
00955 
00956 static void thread_trickle_accelerate(trickle_t *t, const trickle_params_t *params, uint16_t ticks)
00957 {
00958     trickle_time_t new_time = t->now + ticks;
00959 
00960     /* Catch overflow */
00961     if (new_time < t->now) {
00962         new_time = TRICKLE_TIME_MAX;
00963     }
00964 
00965     if (new_time < t->t) {
00966         // We must speed up this so that next trigger happens before this time
00967         t->now = t->t - new_time;
00968     }
00969     if (t->now > t->t) {
00970         // if now is larger than t move t to trigger event again during this period
00971         t->t = t->now + randLIB_get_random_in_range(0, params->Imin/2);
00972     }
00973 }
00974 
00975 // This functions speeds up next advertisement depending on the disconnect period to leader
00976 void thread_routing_leader_connection_validate(thread_info_t *thread, uint16_t disconnect_period)
00977 {
00978     thread_routing_info_t *routing = &thread->routing;
00979 
00980     if (!routing->activated) {
00981         return;
00982     }
00983     //If disconnect period is about 60 seconds make sure next advertisement happens before 70 second mark
00984     if (disconnect_period < NETWORK_ID_SPEEDUP) {
00985         return;
00986     }
00987     if (disconnect_period > NETWORK_ID_SPEEDUP_MAX ) {
00988         tr_debug("Leader restored:accelerate reset: %d",disconnect_period);
00989         trickle_inconsistent_heard(&routing->mle_advert_timer, &thread_mle_advert_trickle_params);
00990     } else {
00991         tr_debug("Leader restored:accelerate: %d",disconnect_period);
00992         thread_trickle_accelerate(&routing->mle_advert_timer, &thread_mle_advert_trickle_params, 100);// 10 second with 100ms tics
00993     }
00994 }
00995 
00996 static bool thread_forwardable_address_fn(const protocol_interface_info_entry_t *cur, addrtype_t addr_type, const uint8_t *address)
00997 {
00998     (void) cur;
00999     if (addr_type != ADDR_802_15_4_SHORT ) {
01000         return false;
01001     }
01002 
01003     if (addr_check_broadcast(address, addr_type) == 0) {
01004         return false;
01005     }
01006 
01007     return true;
01008 }
01009 
01010 static bool thread_address_map_fn(protocol_interface_info_entry_t *cur, addrtype_t *addr_type, uint8_t *address)
01011 {
01012     /* Don't touch addresses other than short */
01013     if (*addr_type != ADDR_802_15_4_SHORT ) {
01014         return true;
01015     }
01016 
01017     /* Anything with maximum router ID is anycast - others are normal unmapped unicast */
01018     uint16_t addr16 = common_read_16_bit(address + 2);
01019     if (thread_router_id_from_addr(addr16) != N_THREAD_ROUTERS - 1) {
01020         return true;
01021     }
01022 
01023     /* If the ND mapping function fails, drop the packet */
01024     if (thread_nd_map_anycast_address(cur, &addr16) < 0) {
01025         return false;
01026     }
01027 
01028     /* Update the address */
01029     common_write_16_bit(addr16, address + 2);
01030     return true;
01031 }
01032 
01033 static bool thread_header_needed_fn(const protocol_interface_info_entry_t *cur, const buffer_t *buf )
01034 {
01035     /* Never mesh headers on broadcasts/multicasts */
01036     if (addr_check_broadcast(buf->dst_sa .address , buf->dst_sa .addr_type ) == 0) {
01037         return false;
01038     }
01039     /* We have no use for long addresses in mesh headers */
01040     if (buf->dst_sa .addr_type  != ADDR_802_15_4_SHORT  || buf->src_sa .addr_type  != ADDR_802_15_4_SHORT ) {
01041         return false;
01042     }
01043 
01044     /* Don't need a mesh header if travelling between a router and its child */
01045     uint16_t src = common_read_16_bit(buf->src_sa .address  + 2);
01046     uint16_t dst = common_read_16_bit(buf->dst_sa .address  + 2);
01047     if (src == thread_router_addr_from_addr(dst) || dst == thread_router_addr_from_addr(src)) {
01048         return false;
01049     }
01050 
01051     /* Don't need a mesh header if we are routing directly to a neighbor router
01052        Possibly unsafe if routing info changes before it reaches the transmit stage? */
01053     if (thread_i_am_router(cur) && thread_is_router_addr(dst)) {
01054         thread_router_id_t id = thread_router_id_from_addr(dst);
01055         /* Direct routing is indicated by the next hop
01056            in fast_route_table being the router itself */
01057         if (cur->thread_info->routing.fast_route_table[id] == id) {
01058             return false;
01059         }
01060     }
01061 
01062     return true;
01063 }
01064 
01065 
01066 static mesh_callbacks_t thread_routing_mesh_callbacks = {
01067     .first_hop = thread_first_hop_fn,
01068     .route = thread_route_fn,
01069     .header_needed = thread_header_needed_fn,
01070     .forwardable_address = thread_forwardable_address_fn,
01071     .address_map = thread_address_map_fn,
01072 };
01073 
01074 void thread_routing_set_mesh_callbacks(protocol_interface_info_entry_t *cur)
01075 {
01076     cur->mesh_callbacks = &thread_routing_mesh_callbacks;
01077     mesh_all_addresses_unicast(true);
01078 }
01079 
01080 /* Return value = number of neighbours with quality 10dB or better ("set M")
01081  * *as_good     = number of neighbours with as good or better quality links to all routers in set M
01082  *
01083  * (Note, these numbers are not calculated at exactly the same time, so could be a slight mismatch...)
01084  */
01085 uint_fast8_t thread_routing_count_neighbours_for_downgrade(thread_routing_info_t *routing, uint_fast8_t *as_good)
01086 {
01087     uint_fast8_t linkCnt = 0;
01088     *as_good = 0;
01089     thread_link_quality_e linkQuality;
01090     ns_list_foreach_safe(thread_router_link_t, link, &routing->link_set) {
01091         linkQuality = thread_neighbour_router_quality(link);
01092         if (linkQuality >= QUALITY_10dB) {
01093             linkCnt++;
01094         }
01095         if (link->as_good) {
01096             (*as_good)++;
01097         }
01098     }
01099 
01100     return linkCnt;
01101 }
01102 
01103 uint_fast8_t thread_routing_count_active_routers(thread_routing_info_t *routing)
01104 {
01105     uint_fast8_t activeRouterCnt = 0;
01106     for (thread_router_id_t i = 0; i < N_THREAD_ROUTERS; i++) {
01107 
01108         if (routing->fast_route_table[i] != FAST_ROUTE_INVALID_ID) {
01109             activeRouterCnt++;
01110         }
01111     }
01112 
01113     return activeRouterCnt;
01114 }
01115 
01116 uint_fast8_t thread_routing_count_active_routers_from_mask(const uint8_t *id_mask)
01117 {
01118     uint_fast8_t activeRouterCnt = 0;
01119     for (uint_fast8_t i = 0; i < 8; i++) {
01120         activeRouterCnt += common_count_bits(id_mask[i]);
01121     }
01122     return activeRouterCnt;
01123 }
01124 
01125 #endif //HAVE_THREAD_ROUTER