thread_network_data_storage.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 /*
00031  * \file thread_network_data_storage.c
00032  * \brief Add short description about this file!!!
00033  *
00034  */
00035 
00036 #include "nsconfig.h"
00037 #ifdef HAVE_THREAD
00038 #include <string.h>
00039 #include <ns_types.h>
00040 #include <nsdynmemLIB.h>
00041 #include "NWK_INTERFACE/Include/protocol.h"
00042 #include "common_functions.h"
00043 #include "ns_trace.h"
00044 #include "ip6string.h"
00045 #include "6LoWPAN/Thread/thread_common.h"
00046 #include "6LoWPAN/Thread/thread_config.h"
00047 #include "6LoWPAN/Thread/thread_extension.h"
00048 #include "6LoWPAN/Thread/thread_joiner_application.h"
00049 #include "6LoWPAN/Thread/thread_network_data_lib.h"
00050 #include "6LoWPAN/Thread/thread_network_data_storage.h"
00051 #include "6LoWPAN/Thread/thread_dhcpv6_client.h"
00052 #include "6LoWPAN/MAC/mac_helper.h"
00053 #include "thread_management_if.h"
00054 #include "thread_meshcop_lib.h"
00055 #include "libDHCPv6/libDHCPv6_server.h"
00056 #include "DHCPv6_Server/DHCPv6_server_service.h"
00057 
00058 #define TRACE_GROUP "thrd"
00059 
00060 
00061 static void network_server_print(const thread_network_server_data_entry_t *route)
00062 {
00063 #ifndef HAVE_DEBUG
00064     (void) route;
00065 #endif
00066 
00067     tr_info("%s Info: routerId:%04x",
00068             (route->stableData ? "Stable" : "Temporary"),
00069             route->routerID);
00070 }
00071 
00072 //@TODO seems that this function is called from application,
00073 //       is there better way than print data inside stack ?
00074 void thread_nd_network_data_print(thread_network_data_cache_entry_t *networkData, uint16_t routerId)
00075 {
00076     // Route prefix is variable-length, so need to zero pad for ip6tos
00077     uint8_t addr[16];
00078     tr_info("Network Data:");
00079 
00080     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkData->localPrefixList ) {
00081         memset(addr, 0, 16);
00082         bitcopy(addr, cur->servicesPrefix, cur->servicesPrefixLen);
00083         tr_info("Prefix: %40s/%-3u", trace_ipv6(addr), cur->servicesPrefixLen);
00084         ns_list_foreach(thread_network_server_data_entry_t, curRoute, &cur->routeList) {
00085             tr_info("Has Route:");
00086             if (curRoute->routerID != routerId) {
00087                 network_server_print(curRoute);
00088             } else {
00089                 tr_info("This Interface");
00090             }
00091         }
00092 
00093         ns_list_foreach(thread_network_server_data_entry_t, curRoute, &cur->borderRouterList) {
00094             if (curRoute->P_dhcp) {
00095                 tr_info("DHCPv6:");
00096             }
00097             if (curRoute->P_slaac || curRoute->P_preferred) {
00098                 tr_info("SLAAC:");
00099             }
00100             if (curRoute->routerID != routerId) {
00101                 network_server_print(curRoute);
00102             } else {
00103                 tr_info("I'm Hosting");
00104             }
00105         }
00106     }
00107 }
00108 
00109 uint8_t thread_nd_server_external_route_list_size(thread_network_server_data_list_t *list, bool stable)
00110 {
00111 
00112     uint8_t listSize = 0;
00113 
00114     ns_list_foreach(thread_network_server_data_entry_t, cur, list) {
00115         if (cur->stableData == stable) {
00116             listSize += 3;
00117         }
00118     }
00119 
00120     if (listSize) {
00121         listSize += 2;
00122     }
00123 
00124     return listSize;
00125 }
00126 
00127 uint8_t thread_nd_border_routerserver_list_size(thread_network_server_data_list_t *list, bool stable)
00128 {
00129     uint8_t listSize = 0;
00130 
00131     ns_list_foreach(thread_network_server_data_entry_t, cur, list) {
00132         if (cur->stableData == stable) {
00133             listSize += 4;
00134         }
00135     }
00136     if (listSize) {
00137         listSize += 2;
00138     }
00139 
00140     return listSize;
00141 }
00142 
00143 uint8_t thread_nd_context_list_size(thread_data_context_list_t *list, bool stable)
00144 {
00145     uint8_t listSize = 0;
00146 
00147     ns_list_foreach(thread_network_data_context_entry_t, cur, list) {
00148         if (cur->stableData == stable) {
00149             listSize += 2;
00150         }
00151     }
00152 
00153     if (listSize) {
00154         listSize += 2;
00155     }
00156     return listSize;
00157 }
00158 
00159 uint8_t *thread_nd_context_list_write(thread_data_context_list_t *list, uint8_t *dataPtr, bool stableData)
00160 {
00161     uint8_t tlv_size = 0;
00162     uint8_t *ptr = (dataPtr + 2); //Leave allways Type & Length free first
00163 
00164     ns_list_foreach(thread_network_data_context_entry_t, cur, list) {
00165         if (cur->stableData == stableData) {
00166             tlv_size += 2;
00167             *ptr++ = cur->cid | (cur->compression ? THREAD_NWK_CONTEXT_COMPRESS_ENABLED : 0);
00168             *ptr++ = cur->contextPrefixLength;
00169         }
00170     }
00171     if (tlv_size) {
00172         uint8_t type = THREAD_NWK_DATA_TYPE_6LOWPAN_ID;
00173         if (stableData) {
00174             type |= THREAD_NWK_STABLE_DATA;
00175         }
00176 
00177         *dataPtr++ = type;
00178         *dataPtr = tlv_size;
00179         //SET Pointer back to end of data
00180         dataPtr = ptr;
00181     }
00182 
00183     return dataPtr;
00184 }
00185 
00186 static uint16_t thread_nd_service_border_router_flags_read(thread_network_server_data_entry_t *cur)
00187 {
00188     uint16_t flags = (cur->Prf << THREAD_PRF_BIT_MOVE);
00189     flags |= (cur->P_preferred << THREAD_P_PREFERRED_BIT_MOVE);
00190     flags |= (cur->P_slaac << THREAD_P_SLAAC_BIT_MOVE);
00191     flags |= (cur->P_dhcp << THREAD_P_DHCP_BIT_MOVE);
00192     flags |= (cur->P_configure << THREAD_P_CONFIGURE_BIT_MOVE);
00193     flags |= (cur->P_default_route << THREAD_P_DEF_ROUTE_BIT_MOVE);
00194     flags |= (cur->P_on_mesh << THREAD_P_ON_MESH_BIT_MOVE);
00195     flags |= (cur->P_nd_dns << THREAD_P_ND_DNS_BIT_MOVE);
00196     return flags;
00197 }
00198 
00199 uint8_t *thread_nd_server_list_write(thread_network_server_data_list_t *list, uint8_t *dataPtr, uint8_t type, bool stable)
00200 {
00201     uint8_t tlv_size = 0, tlv_length;
00202     uint16_t flags;
00203     uint8_t *ptr = (dataPtr + 2); //Leave allways Type & Length free first
00204     uint16_t routerId;
00205 
00206     if (type == THREAD_NWK_DATA_TYPE_BORDER_ROUTER) {
00207         tlv_length = THREAD_BORDER_ROUTER_TLV_LENGTH;
00208     } else {
00209         tlv_length = THREAD_HAS_ROUTE_TLV_LENGTH;
00210     }
00211 
00212     ns_list_foreach(thread_network_server_data_entry_t, cur, list) {
00213         if (cur->stableData == stable) {
00214             tlv_size += tlv_length;
00215             routerId = cur->routerID;
00216             if (type == THREAD_NWK_DATA_TYPE_BORDER_ROUTER) {
00217                 flags = thread_nd_service_border_router_flags_read(cur);
00218                 ptr = thread_nd_network_data_border_router_tlv_write(ptr, routerId, flags);
00219             } else {
00220                 uint8_t pref = (cur->Prf << THREAD_HAS_ROUTE_PRF_BIT_MOVE);
00221                 ptr = thread_nd_network_data_has_route_tlv_write(ptr, routerId, pref);
00222             }
00223         }
00224     }
00225 
00226     if (tlv_size) {
00227         if (stable) {
00228             type |= THREAD_NWK_STABLE_DATA;
00229         }
00230         *dataPtr++ = type;
00231         *dataPtr = tlv_size;
00232         //SET Pointer back to end of data
00233         dataPtr = ptr;
00234     }
00235 
00236     return dataPtr;
00237 }
00238 
00239 uint8_t *thread_nd_service_server_list_write(thread_network_data_service_server_list_t *list, uint8_t *ptr, bool full_list)
00240 {
00241     ns_list_foreach(thread_network_data_service_server_entry_t, cur, list) {
00242         if (!full_list && !cur->stable) {
00243             continue;
00244         }
00245 
00246         if (cur->stable) {
00247             *ptr++ = THREAD_NWK_DATA_TYPE_SERVER_DATA | THREAD_NWK_STABLE_DATA;
00248         } else {
00249             *ptr++ = THREAD_NWK_DATA_TYPE_SERVER_DATA;
00250         }
00251 
00252         *ptr++ = 2 + cur->server_data_length;
00253         ptr = common_write_16_bit(cur->router_id, ptr);
00254 
00255         memcpy(ptr, cur->server_data, cur->server_data_length);
00256         ptr += cur->server_data_length;
00257     }
00258 
00259     return ptr;
00260 }
00261 
00262 uint8_t thread_nd_service_based_on_list_entry_size(thread_network_data_service_cache_entry_t *entry, bool fullList)
00263 {
00264     uint8_t tempTlvSize = 0;
00265 
00266     ns_list_foreach(thread_network_data_service_server_entry_t, cur, &entry->server_list) {
00267         if (!fullList && !cur->stable) {
00268             continue;
00269         }
00270         tempTlvSize += 4; // Type, Length, and S_server_16
00271         tempTlvSize += cur->server_data_length;
00272     }
00273 
00274     if (tempTlvSize) {
00275         tempTlvSize += 2; // T + S_id + service data length
00276 
00277         if (!entry->T) {
00278             tempTlvSize += 4;
00279         }
00280 
00281         tempTlvSize += entry->S_service_data_length;
00282     }
00283 
00284     return tempTlvSize;
00285 }
00286 
00287 uint8_t thread_nd_prefix_based_on_list_entry_size(thread_network_data_prefix_cache_entry_t *entry, bool fullList)
00288 {
00289     uint8_t tempTlvSize = 0;
00290 
00291     tempTlvSize += thread_nd_border_routerserver_list_size(&entry->borderRouterList, true);
00292     tempTlvSize += thread_nd_server_external_route_list_size(&entry->routeList, true);
00293     tempTlvSize += thread_nd_context_list_size(&entry->contextList, true);
00294     if (fullList) {
00295         tempTlvSize += thread_nd_border_routerserver_list_size(&entry->borderRouterList, false);
00296         tempTlvSize += thread_nd_server_external_route_list_size(&entry->routeList, false);
00297         tempTlvSize += thread_nd_context_list_size(&entry->contextList, false);
00298     }
00299 
00300     if (tempTlvSize) {
00301         tempTlvSize += prefixBits_to_bytes(entry->servicesPrefixLen );
00302         tempTlvSize += 2; //Domain ID & Length
00303     }
00304     return tempTlvSize;
00305 }
00306 
00307 int thread_nd_local_data_length_updated(thread_network_data_cache_entry_t *networkDataList)
00308 {
00309     int ret_val = -1;
00310     uint16_t localDataLength = 0;
00311     localDataLength = thread_network_data_prefix_set_size(networkDataList, true)
00312         + thread_network_data_service_set_size(networkDataList, true);
00313 
00314     if (localDataLength != networkDataList->networkDataTlvSize ) {
00315         networkDataList->networkDataTlvSize  = localDataLength;
00316         ret_val = 0;
00317     }
00318     return ret_val;
00319 }
00320 
00321 
00322 thread_network_local_data_entry_t *thread_local_service_list_allocate(thread_prefix_tlv_t *prefixTlv)
00323 {
00324     thread_network_local_data_entry_t *newEntry = ns_dyn_mem_alloc(sizeof(thread_network_local_data_entry_t));
00325     if (newEntry) {
00326         memset(newEntry->servicesPrefix, 0, 16);
00327         bitcopy(newEntry->servicesPrefix, prefixTlv->Prefix,  prefixTlv->PrefixLen);
00328         newEntry->servicesPrefixLen = prefixTlv->PrefixLen;
00329         newEntry->domainId = prefixTlv->domainId;
00330         newEntry->dhcpv6ServerActive = false;
00331         newEntry->dhcpv6ServerDataStable = false;
00332         newEntry->slaacServerActive = false;
00333         newEntry->slaacServerDataStable = false;
00334         newEntry->slaacPreferred = false;
00335         newEntry->routeActive = false;
00336         newEntry->routeDataStable = false;
00337         newEntry->preference = 0;
00338         newEntry->configure = false;
00339         newEntry->defaultRoute = false;
00340         newEntry->onMesh = false;
00341         newEntry->ndDns = false;
00342     }
00343     return newEntry;
00344 }
00345 
00346 thread_network_data_prefix_cache_entry_t *thread_prefix_service_list_allocate(thread_prefix_tlv_t *prefixTlv)
00347 {
00348     thread_network_data_prefix_cache_entry_t *newEntry = ns_dyn_mem_alloc(sizeof(thread_network_data_prefix_cache_entry_t));
00349     if (newEntry) {
00350         memset(newEntry->servicesPrefix , 0, 16);
00351         bitcopy(newEntry->servicesPrefix , prefixTlv->Prefix,  prefixTlv->PrefixLen);
00352         newEntry->servicesPrefixLen  = prefixTlv->PrefixLen;
00353         newEntry->domainId = prefixTlv->domainId;
00354         ns_list_init(&newEntry->contextList);
00355         ns_list_init(&newEntry->borderRouterList);
00356         ns_list_init(&newEntry->routeList);
00357     }
00358     return newEntry;
00359 }
00360 
00361 thread_network_server_data_entry_t *thread_server_entry_allocate(thread_border_router_tlv_entry_t *service)
00362 {
00363     thread_network_server_data_entry_t *newEntry = ns_dyn_mem_alloc(sizeof(thread_network_server_data_entry_t));
00364     if (newEntry) {
00365         tr_debug("Create new server entry addr: %04x",service->routerID);
00366         newEntry->routerID = service->routerID;
00367         newEntry->stableData = service->stableData;
00368         newEntry->P_configure = service->P_configure;
00369         newEntry->P_default_route = service->P_default_route;
00370         newEntry->P_dhcp = service->P_dhcp;
00371         newEntry->P_preferred = service->P_preferred;
00372         newEntry->P_slaac = service->P_slaac;
00373         newEntry->Prf = service->Prf;
00374         newEntry->P_on_mesh = service->P_on_mesh;
00375         newEntry->P_nd_dns = service->P_nd_dns;
00376         newEntry->canDelete = false;
00377 
00378     }
00379     return newEntry;
00380 }
00381 
00382 thread_network_data_context_entry_t *thread_context_service_list_allocate(uint8_t contextLength, uint8_t cid, bool compersioEnabled)
00383 {
00384     thread_network_data_context_entry_t *newEntry = ns_dyn_mem_alloc(sizeof(thread_network_data_context_entry_t));
00385     if (newEntry) {
00386         newEntry->cid = cid;
00387         newEntry->compression = compersioEnabled;
00388         newEntry->contextPrefixLength = contextLength;
00389         newEntry->canDelete = false;
00390         newEntry->stableData = false;
00391     }
00392     return newEntry;
00393 }
00394 
00395 int  thread_nd_verify_contex_id_is_free(thread_network_data_cache_entry_t *list, uint8_t *prefixPtr, thread_network_local_data_context_entry_t *context)
00396 {
00397     int retVal = 0;
00398     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &list->localPrefixList ) {
00399 
00400         ns_list_foreach(thread_network_data_context_entry_t, curContetx, &cur->contextList) {
00401             if (curContetx->cid == context->cid) {
00402                 if (context->contextPrefixLength == cur->servicesPrefixLen) {
00403                     if (!bitsequal(cur->servicesPrefix, prefixPtr, context->contextPrefixLength)) {
00404                         retVal = -1; //Do not accept same context anymore again if prefix is different
00405                     }
00406                 } else {
00407                     retVal = -1;
00408                 }
00409 
00410                 return retVal;
00411             }
00412         }
00413 
00414     }
00415     return retVal;
00416 }
00417 
00418 thread_network_data_service_cache_entry_t *thread_network_data_service_entry_find(thread_network_data_service_cache_list_t *list, thread_network_data_service_entry_t *service)
00419 {
00420     ns_list_foreach(thread_network_data_service_cache_entry_t, cur, list) {
00421         if (service->S_enterprise_number == cur->S_enterprise_number) {
00422             if (service->S_service_data_length == cur->S_service_data_length) {
00423                 if (memcmp(service->S_service_data, cur->S_service_data, service->S_service_data_length) == 0) {
00424                     return cur;
00425                 }
00426             }
00427         }
00428     }
00429 
00430     return NULL;
00431 }
00432 
00433 thread_network_data_service_entry_t *thread_local_service_entry_find(thread_network_data_service_list_t *list, thread_network_data_service_entry_t *service)
00434 {
00435     ns_list_foreach(thread_network_data_service_entry_t, cur, list) {
00436         if (service->S_enterprise_number == cur->S_enterprise_number) {
00437             if (service->S_service_data_length == cur->S_service_data_length) {
00438                 if (memcmp(service->S_service_data, cur->S_service_data, service->S_service_data_length) == 0) {
00439                     return cur;
00440                 }
00441             }
00442         }
00443     }
00444 
00445     return NULL;
00446 }
00447 
00448 thread_network_data_service_entry_t *thread_local_service_entry_save(thread_network_data_service_list_t *list, thread_network_data_service_entry_t *service)
00449 {
00450     thread_network_data_service_entry_t *service_entry;
00451     service_entry = thread_local_service_entry_find(list, service);
00452 
00453     if (service_entry) {
00454         service_entry->S_id = service->S_id;
00455         service_entry->S_stable = service->S_stable;
00456 
00457         if (service_entry->S_server_data) {
00458             ns_dyn_mem_free(service_entry->S_server_data);
00459             service_entry->S_server_data = NULL;
00460         }
00461 
00462         if (service->S_server_data_length) {
00463             service_entry->S_server_data = ns_dyn_mem_alloc(service->S_server_data_length);
00464         }
00465 
00466         if (service_entry->S_server_data) {
00467             service_entry->S_server_data_length = service->S_server_data_length;
00468             memcpy(service_entry->S_server_data, service->S_server_data, service->S_server_data_length);
00469         }
00470 
00471         return service_entry;
00472     }
00473 
00474     service_entry = ns_dyn_mem_alloc(sizeof(thread_network_data_service_entry_t));
00475 
00476     if (service_entry) {
00477         memset(service_entry, 0, sizeof(thread_network_data_service_entry_t));
00478 
00479         service_entry->T = service->T;
00480         service_entry->S_id = service->S_id;
00481         service_entry->S_stable = service->S_stable;
00482         service_entry->S_enterprise_number = service->S_enterprise_number;
00483         service_entry->S_service_data = ns_dyn_mem_alloc(service->S_service_data_length);
00484 
00485         if (!service_entry->S_service_data) {
00486             tr_error("Failed to allocate memory for service data!");
00487             ns_dyn_mem_free(service_entry);
00488             return NULL;
00489         }
00490 
00491         service_entry->S_service_data_length = service->S_service_data_length;
00492         memcpy(service_entry->S_service_data, service->S_service_data, service->S_service_data_length);
00493 
00494         if (service->S_server_data_length) {
00495             service_entry->S_server_data = ns_dyn_mem_alloc(service->S_server_data_length);
00496 
00497             if (!service_entry->S_server_data) {
00498                 tr_error("Failed to allocate memory for server data!");
00499                 ns_dyn_mem_free(service_entry->S_service_data);
00500                 ns_dyn_mem_free(service_entry);
00501                 return NULL;
00502             }
00503 
00504             service_entry->S_server_data_length = service->S_server_data_length;
00505             memcpy(service_entry->S_server_data, service->S_server_data, service->S_server_data_length);
00506         }
00507 
00508         ns_list_add_to_end(list, service_entry);
00509 
00510         tr_debug("Added service; enterprise number: %"PRIu32", service: %s", service_entry->S_enterprise_number,
00511                  trace_array(service_entry->S_service_data, service_entry->S_service_data_length));
00512     }
00513 
00514     return service_entry;
00515 }
00516 
00517 thread_network_local_data_entry_t *thread_local_prefix_entry_find(thread_network_data_prefix_list_t *list, thread_prefix_tlv_t *prefixTlv)
00518 {
00519     ns_list_foreach(thread_network_local_data_entry_t, cur, list) {
00520         if (prefixTlv->domainId == cur->domainId) {
00521             if (prefixTlv->PrefixLen == cur->servicesPrefixLen) {
00522                 if (bitsequal(cur->servicesPrefix, prefixTlv->Prefix, prefixTlv->PrefixLen)) {
00523                     return cur;
00524                 }
00525             }
00526         }
00527     }
00528     return NULL;
00529 }
00530 
00531 thread_network_local_data_entry_t *thread_local_prefix_entry_get(thread_network_data_prefix_list_t *list, thread_prefix_tlv_t *prefixTlv)
00532 {
00533     thread_network_local_data_entry_t *prefix_entry;
00534 
00535     //Check current Prefix List
00536     prefix_entry = thread_local_prefix_entry_find(list, prefixTlv);
00537     if (prefix_entry) {
00538         return prefix_entry;
00539     }
00540 
00541     prefix_entry = thread_local_service_list_allocate(prefixTlv);
00542     if (prefix_entry) {
00543         bool inserted = false;
00544         ns_list_foreach(thread_network_local_data_entry_t, cur, list) {
00545             if (prefixTlv->PrefixLen >= cur->servicesPrefixLen) { //Add always longer or same length to before last one
00546                 //add before new
00547                 tr_debug("Add new Longer");
00548                 inserted = true;
00549                 ns_list_add_before(list, cur, prefix_entry);
00550                 break;
00551             }
00552         }
00553         if (!inserted) {
00554             tr_debug("Add to end");
00555             ns_list_add_to_end(list, prefix_entry);
00556         }
00557     }
00558     return prefix_entry;
00559 }
00560 
00561 uint8_t thread_service_next_free_service_type_id(thread_network_data_service_cache_list_t *list)
00562 {
00563     uint8_t sid;
00564 
00565     for (sid = 0; sid < 16; ++sid) {
00566         bool found = false;
00567 
00568         ns_list_foreach(thread_network_data_service_cache_entry_t, cur, list) {
00569             if (cur->S_id == sid) {
00570                 found = true;
00571                 break;
00572             }
00573         }
00574 
00575         if (!found) {
00576             return sid;
00577         }
00578     }
00579 
00580     return 0xff; // IDs exhausted
00581 }
00582 
00583 thread_network_data_prefix_cache_entry_t *thread_prefix_entry_find(thread_network_prefix_list_t *list, thread_prefix_tlv_t *prefixTlv)
00584 {
00585     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, list) {
00586         if (prefixTlv->domainId == cur->domainId) {
00587             if (prefixTlv->PrefixLen == cur->servicesPrefixLen) {
00588                 if (bitsequal(cur->servicesPrefix, prefixTlv->Prefix, prefixTlv->PrefixLen)) {
00589                     return cur;
00590                 }
00591             }
00592         }
00593     }
00594     return NULL;
00595 }
00596 thread_network_data_prefix_cache_entry_t *thread_prefix_entry_get(thread_network_prefix_list_t *list, thread_prefix_tlv_t *prefixTlv)
00597 {
00598     thread_network_data_prefix_cache_entry_t *main_list;
00599     //Check current Prefix List
00600     main_list = thread_prefix_entry_find(list, prefixTlv);
00601     if (main_list) {
00602         return main_list;
00603     }
00604 
00605     main_list = thread_prefix_service_list_allocate(prefixTlv);
00606     tr_info("New prefix added");
00607 
00608     if (main_list) {
00609         bool inserted = false;
00610         ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, list) {
00611             if (prefixTlv->PrefixLen >= cur->servicesPrefixLen) { //Add always longer or same length to before last one
00612                 //add before new
00613                 tr_debug("Add new Longer");
00614                 inserted = true;
00615                 ns_list_add_before(list, cur, main_list);
00616                 break;
00617             }
00618         }
00619         if (!inserted) {
00620             tr_debug("Add to end");
00621             ns_list_add_to_end(list, main_list);
00622         }
00623     }
00624 
00625     return main_list;
00626 }
00627 
00628 thread_network_data_service_cache_entry_t *thread_service_entry_get(thread_network_data_service_cache_list_t *list, thread_network_data_service_entry_t *service)
00629 {
00630     thread_network_data_service_cache_entry_t *service_entry;
00631 
00632     service_entry = thread_network_data_service_entry_find(list, service);
00633 
00634     if (service_entry) {
00635         return service_entry;
00636     }
00637 
00638     service_entry = ns_dyn_mem_alloc(sizeof(thread_network_data_service_cache_entry_t));
00639 
00640     if (service_entry) {
00641         memset(service_entry, 0, sizeof(thread_network_data_service_entry_t));
00642 
00643         service_entry->T = service->T;
00644         service_entry->S_stable = service->S_stable;
00645         service_entry->S_enterprise_number = service->S_enterprise_number;
00646 
00647         uint8_t sid = thread_service_next_free_service_type_id(list);
00648 
00649         if (sid == 0xff) {
00650             tr_error("All Service Type ID (S_id) values used!");
00651             ns_dyn_mem_free(service_entry);
00652             return NULL;
00653         }
00654 
00655         service_entry->S_id = sid;
00656         service_entry->S_service_data = ns_dyn_mem_alloc(service->S_service_data_length);
00657 
00658         if (!service_entry->S_service_data) {
00659             tr_error("Failed to allocate memory for service data!");
00660             ns_dyn_mem_free(service_entry);
00661             return NULL;
00662         }
00663 
00664         service_entry->S_service_data_length = service->S_service_data_length;
00665         memcpy(service_entry->S_service_data, service->S_service_data, service->S_service_data_length);
00666         ns_list_init(&service_entry->server_list);
00667         ns_list_add_to_end(list, service_entry);
00668     }
00669 
00670     return service_entry;
00671 }
00672 
00673 thread_network_server_data_entry_t *thread_server_entry_search(thread_network_server_data_list_t *list, uint16_t routerId)
00674 {
00675     ns_list_foreach(thread_network_server_data_entry_t, cur, list) {
00676         if (cur->routerID == routerId) {
00677             cur->canDelete = false;
00678             return cur;
00679         }
00680     }
00681     return NULL;
00682 }
00683 
00684 thread_network_data_service_server_entry_t *thread_service_server_entry_search(thread_network_data_service_server_list_t *list, uint16_t routerId)
00685 {
00686     ns_list_foreach(thread_network_data_service_server_entry_t, cur, list) {
00687         if (cur->router_id == routerId) {
00688             cur->can_delete = false;
00689             return cur;
00690         }
00691     }
00692     return NULL;
00693 }
00694 
00695 thread_network_server_data_entry_t *thread_server_entry_get(thread_network_server_data_list_t *list, thread_border_router_tlv_entry_t *service)
00696 {
00697     thread_network_server_data_entry_t *server_entry;
00698     bool inserted = false;
00699 
00700     //Check current Stable List
00701     server_entry = thread_server_entry_search(list, service->routerID);
00702     if (server_entry) {
00703         return server_entry;
00704     }
00705     server_entry = thread_server_entry_allocate(service);
00706     if (!server_entry) {
00707         return NULL;
00708     }
00709     tr_info("New server entry made");
00710     ns_list_foreach(thread_network_server_data_entry_t, cur, list)
00711     {
00712         if (service->routerID < cur->routerID) { //Add always longer or same length to before last one
00713             //add before new
00714             tr_debug("Add smaller id");
00715             inserted = true;
00716             ns_list_add_before(list, cur, server_entry);
00717             break;
00718         }
00719     }
00720     if (!inserted) {
00721         tr_debug("Add new Last");
00722         ns_list_add_to_end(list, server_entry);
00723     }
00724 
00725     return server_entry;
00726 }
00727 
00728 thread_network_data_service_server_entry_t *thread_service_server_entry_get(thread_network_data_service_server_list_t *list, thread_network_data_service_server_entry_t *server)
00729 {
00730     thread_network_data_service_server_entry_t *server_entry;
00731 
00732     server_entry = thread_service_server_entry_search(list, server->router_id);
00733 
00734     if (server_entry) {
00735         return server_entry;
00736     }
00737 
00738     server_entry = ns_dyn_mem_alloc(sizeof(thread_network_data_service_server_entry_t));
00739 
00740     if (server_entry) {
00741         memset(server_entry, 0, sizeof(thread_network_data_service_server_entry_t));
00742 
00743         server_entry->can_delete = false;
00744         server_entry->router_id = server->router_id;
00745         server_entry->stable = server->stable;
00746         server_entry->server_data = ns_dyn_mem_alloc(server->server_data_length);
00747 
00748         if (!server_entry->server_data) {
00749             tr_error("Failed to allocate memory for server data!");
00750             ns_dyn_mem_free(server_entry);
00751             return NULL;
00752         }
00753 
00754         server_entry->server_data_length = server->server_data_length;
00755         memcpy(server_entry->server_data, server->server_data, server->server_data_length);
00756         ns_list_add_to_end(list, server_entry);
00757     }
00758 
00759     return server_entry;
00760 }
00761 
00762 uint8_t thread_get_context_id_by_length(thread_data_context_list_t *list, uint8_t contextLength)
00763 {
00764     //Check current Stable List
00765     ns_list_foreach(thread_network_data_context_entry_t, cur, list) {
00766         if (cur->contextPrefixLength == contextLength) {
00767             return cur->cid;
00768         }
00769     }
00770     return 16;
00771 }
00772 
00773 thread_network_data_context_entry_t *thread_get_context_by_id(thread_data_context_list_t *list, uint8_t cid)
00774 {
00775     //Check current Stable List
00776     ns_list_foreach(thread_network_data_context_entry_t, cur, list) {
00777         if (cur->cid == cid) {
00778             return cur;
00779         }
00780     }
00781     return NULL;
00782 }
00783 
00784 thread_network_server_data_entry_t *thread_get_dhcp_server_from_list(thread_network_server_data_list_t *list)
00785 {
00786     //Check current Stable List
00787     ns_list_foreach(thread_network_server_data_entry_t, cur, list) {
00788         if (cur->P_dhcp) {
00789             return cur;
00790         }
00791     }
00792     return NULL;
00793 }
00794 
00795 thread_network_data_context_entry_t *thread_get_main_context_list(thread_data_context_list_t *list, thread_network_local_data_context_entry_t *context)
00796 {
00797     thread_network_data_context_entry_t *main_list;
00798 
00799     //Check current Stable List
00800     ns_list_foreach(thread_network_data_context_entry_t, cur, list) {
00801         if (cur->cid == context->cid) {
00802             return cur;
00803         }
00804     }
00805 
00806     main_list = thread_context_service_list_allocate(context->contextPrefixLength, context->cid, context->compression);
00807     if (main_list) {
00808         tr_debug("Add New");
00809         ns_list_add_to_end(list, main_list);
00810     }
00811 
00812     return main_list;
00813 }
00814 
00815 void thread_server_list_free(thread_network_server_data_list_t *listPtr)
00816 {
00817     ns_list_foreach_safe(thread_network_server_data_entry_t, cur, listPtr) {
00818         ns_list_remove(listPtr, cur);
00819         ns_dyn_mem_free(cur);
00820     }
00821 }
00822 
00823 void thread_service_server_list_free(thread_network_data_service_server_list_t *listPtr)
00824 {
00825     ns_list_foreach_safe(thread_network_data_service_server_entry_t, cur, listPtr) {
00826         if (cur->server_data) {
00827             ns_dyn_mem_free(cur->server_data);
00828         }
00829         ns_list_remove(listPtr, cur);
00830         ns_dyn_mem_free(cur);
00831     }
00832 }
00833 
00834 static int thread_server_context_delete_mark(thread_data_context_list_t *listPtr)
00835 {
00836     int retVal = -1;
00837     ns_list_foreach_safe(thread_network_data_context_entry_t, cur, listPtr) {
00838         cur->canDelete = true;
00839     }
00840     return retVal;
00841 }
00842 
00843 static int thread_server_data_delete_mark_by_router_id(thread_network_server_data_list_t *listPtr, uint16_t routerID, bool subSet)
00844 {
00845     int retVal = -1;
00846     ns_list_foreach_safe(thread_network_server_data_entry_t, cur, listPtr) {
00847         if (!subSet) {
00848             cur->canDelete = true;
00849         }
00850         if (cur->routerID == routerID) {
00851             cur->canDelete = true;
00852         }
00853     }
00854     return retVal;
00855 }
00856 
00857 static int thread_service_data_delete_mark_by_router_id(thread_network_data_service_server_list_t *listPtr, uint16_t routerID, bool subSet)
00858 {
00859     int retVal = -1;
00860     ns_list_foreach_safe(thread_network_data_service_server_entry_t, cur, listPtr) {
00861         if (!subSet) {
00862             cur->can_delete = true;
00863         }
00864         if (cur->router_id == routerID) {
00865             cur->can_delete = true;
00866         }
00867     }
00868     return retVal;
00869 }
00870 
00871 static int thread_server_context_clean(thread_network_data_cache_entry_t *cachePtr, thread_data_context_list_t *listPtr, thread_network_data_prefix_cache_entry_t *prefixEntry, lowpan_context_list_t *context_list)
00872 {
00873     int retVal = -1;
00874     (void) prefixEntry;
00875     ns_list_foreach_safe(thread_network_data_context_entry_t, cur, listPtr) {
00876         if (cur->canDelete) {
00877             cachePtr->temporaryUpdatePushed = true;
00878             if (cur->stableData) {
00879                 cachePtr->stableUpdatePushed = true;
00880             }
00881             // Set context lifetime to 0 to delete
00882             lowpan_context_update(context_list, cur->cid, 0, NULL, 0, true);
00883             ns_list_remove(listPtr, cur);
00884             ns_dyn_mem_free(cur);
00885             retVal = 0;
00886         }
00887     }
00888     return retVal;
00889 }
00890 
00891 
00892 static bool thread_server_data_clean_by_router_id(thread_network_data_cache_entry_t *cachePtr, thread_network_server_data_list_t *listPtr, thread_network_data_prefix_cache_entry_t *prefixEntry, bool routeInfo, protocol_interface_info_entry_t *curInterface)
00893 {
00894     uint8_t addr[16];
00895     uint8_t *nextHop;
00896     bool address_removed = false;
00897 
00898     uint16_t mac16 = mac_helper_mac16_address_get(curInterface);
00899     ns_list_foreach_safe(thread_network_server_data_entry_t, cur, listPtr) {
00900         if (cur->canDelete) {
00901             tr_debug("Delete BR %s", trace_array(prefixEntry->servicesPrefix , 8));
00902             cachePtr->temporaryUpdatePushed = true;
00903             if (cur->stableData) {
00904                 cachePtr->stableUpdatePushed = true;
00905             }
00906             //Call Route and DHCPV6 Updates here
00907             memcpy(addr, curInterface->thread_info->threadPrivatePrefixInfo.ulaPrefix, 8);
00908 
00909             memcpy(&addr[8], ADDR_SHORT_ADR_SUFFIC, 6);
00910             common_write_16_bit(cur->routerID, &addr[14]);
00911 
00912 
00913             if (routeInfo) {
00914                 if (cur->routerID == mac16) {
00915                     nextHop = NULL;
00916                 } else {
00917                     nextHop = addr;
00918                 }
00919                 if (prefixEntry) {
00920                     ipv6_route_delete(prefixEntry->servicesPrefix , prefixEntry->servicesPrefixLen , curInterface->id, nextHop, ROUTE_THREAD);
00921                 } else {
00922                     ipv6_route_delete(NULL, 0, curInterface->id, nextHop, ROUTE_THREAD);
00923                 }
00924             } else {
00925                 address_removed = true;
00926 
00927                 if (cur->P_dhcp) {
00928                     tr_debug("Delete DHCPv6 given address");
00929                     thread_dhcp_client_global_address_delete(curInterface->id, addr, prefixEntry->servicesPrefix );
00930                 } else {
00931                     tr_debug("Delete SLAAC address");
00932                     addr_delete_matching(curInterface, prefixEntry->servicesPrefix , 64, ADDR_SOURCE_SLAAC);
00933                 }
00934             }
00935 
00936             ns_list_remove(listPtr, cur);
00937             ns_dyn_mem_free(cur);
00938         }
00939     }
00940     return address_removed;
00941 }
00942 
00943 static void thread_service_data_clean_by_router_id(thread_network_data_cache_entry_t *cachePtr, thread_network_data_service_server_list_t *listPtr)
00944 {
00945     ns_list_foreach_safe(thread_network_data_service_server_entry_t, cur, listPtr) {
00946         if (cur->can_delete) {
00947             tr_debug("Deleting server %04x", cur->router_id);
00948             cachePtr->temporaryUpdatePushed = true;
00949             if (cur->stable) {
00950                 cachePtr->stableUpdatePushed = true;
00951             }
00952             if (cur->server_data) {
00953                 ns_dyn_mem_free(cur->server_data);
00954             }
00955             ns_list_remove(listPtr, cur);
00956             ns_dyn_mem_free(cur);
00957         }
00958     }
00959 }
00960 
00961 void thread_context_service_list_free(thread_data_context_list_t *listPtr)
00962 {
00963     ns_list_foreach_safe(thread_network_data_context_entry_t, cur, listPtr) {
00964         ns_list_remove(listPtr, cur);
00965         ns_dyn_mem_free(cur);
00966     }
00967 }
00968 
00969 
00970 /**
00971  * Initialise Thread Network Data cache
00972  *
00973  * \param cachePtr  Pointer to Network Data Structure which will be initialized
00974  *
00975  */
00976 void thread_network_data_base_init(thread_network_data_cache_entry_t *cachePtr)
00977 {
00978     ns_list_init(&cachePtr->localPrefixList );
00979     ns_list_init(&cachePtr->service_list);
00980     cachePtr->contex_id_reuse_timeout = THREAD_CONTEXT_ID_REUSE_TIMEOUT;
00981     cachePtr->networkDataTlvSize  = 0;
00982     cachePtr->stableUpdatePushed = false;
00983     cachePtr->temporaryUpdatePushed = false;
00984     cachePtr->network_data_update_delay = 0;
00985     cachePtr->network_data_len = 0;
00986 }
00987 
00988 void thread_network_local_server_data_base_init(thread_network_local_data_cache_entry_t *cachePtr)
00989 {
00990     ns_list_init(&cachePtr->prefix_list);
00991     ns_list_init(&cachePtr->service_list);
00992     cachePtr->registered_rloc16 = 0xffff;
00993     cachePtr->release_old_address = false;
00994     cachePtr->publish_active = false;
00995     cachePtr->publish_pending = false;
00996 }
00997 
00998 void thread_network_data_router_id_mark_delete(thread_network_data_cache_entry_t *cachePtr, uint16_t routerID, bool subSet)
00999 {
01000     ns_list_foreach_safe(thread_network_data_prefix_cache_entry_t, cur, &cachePtr->localPrefixList ) {
01001         thread_server_data_delete_mark_by_router_id(&cur->borderRouterList, routerID, subSet);
01002         thread_server_data_delete_mark_by_router_id(&cur->routeList, routerID, subSet);
01003         if (!subSet) {
01004             thread_server_context_delete_mark(&cur->contextList);
01005         }
01006     }
01007 
01008     ns_list_foreach_safe(thread_network_data_service_cache_entry_t, cur, &cachePtr->service_list) {
01009         thread_service_data_delete_mark_by_router_id(&cur->server_list, routerID, subSet);
01010     }
01011 }
01012 bool thread_network_data_router_id_free(thread_network_data_cache_entry_t *cachePtr, bool is_leader, protocol_interface_info_entry_t *curInterface)
01013 {
01014     bool address_removed = false;
01015     ns_list_foreach_safe(thread_network_data_prefix_cache_entry_t, cur, &cachePtr->localPrefixList ) {
01016         //GET Context
01017         if (thread_server_data_clean_by_router_id(cachePtr, &cur->borderRouterList, cur, false, curInterface) ) {
01018             address_removed = true;
01019         }
01020         thread_server_data_clean_by_router_id(cachePtr, &cur->routeList, cur, true, curInterface);
01021         if (!is_leader) {
01022             thread_server_context_clean(cachePtr, &cur->contextList, cur, &curInterface->lowpan_contexts);
01023         }
01024 
01025         if (ns_list_is_empty(&cur->borderRouterList)) {
01026             //Delete On-Mesh Prefix
01027             ipv6_route_delete(cur->servicesPrefix, cur->servicesPrefixLen, curInterface->id, NULL, ROUTE_THREAD);
01028         }
01029 
01030         if (!ns_list_is_empty(&cur->borderRouterList)) {
01031 
01032         } else if (!ns_list_is_empty(&cur->routeList)) {
01033 
01034         } else if (!ns_list_is_empty(&cur->contextList)) {
01035             if (is_leader) {
01036                 ns_list_foreach_safe(thread_network_data_context_entry_t, curContext, &cur->contextList) {
01037                     cachePtr->temporaryUpdatePushed = true;
01038                     if (curContext->stableData) {
01039                         cachePtr->stableUpdatePushed = true;
01040                     }
01041                     if (curContext->compression) {
01042                         curContext->compression = false;
01043                         tr_debug("disable Cur Context and start timer");
01044                         curContext->context_reuse_delay = cachePtr->contex_id_reuse_timeout;
01045                     }
01046                 }
01047             }
01048         } else {
01049             //ALL Empty
01050             ns_list_remove(&cachePtr->localPrefixList , cur);
01051             ns_dyn_mem_free(cur);
01052         }
01053     }
01054 
01055     ns_list_foreach_safe(thread_network_data_service_cache_entry_t, cur, &cachePtr->service_list) {
01056         thread_service_data_clean_by_router_id(cachePtr, &cur->server_list);
01057         if (ns_list_is_empty(&cur->server_list)) {
01058             if (cur->S_service_data) {
01059                 ns_dyn_mem_free(cur->S_service_data);
01060             }
01061             ns_list_remove(&cachePtr->service_list, cur);
01062             ns_dyn_mem_free(cur);
01063         }
01064     }
01065 
01066     if (cachePtr->temporaryUpdatePushed || cachePtr->stableUpdatePushed) {
01067         //Validate that Length will be always updated
01068         cachePtr->networkDataTlvSize   = thread_network_data_prefix_set_size(cachePtr, true)
01069             + thread_network_data_service_set_size(cachePtr, true);
01070     }
01071     return address_removed;
01072 }
01073 
01074 void thread_network_data_context_re_use_timer_update(thread_network_data_cache_entry_t *cachePtr, uint32_t ticks, lowpan_context_list_t *context_list)
01075 {
01076     ns_list_foreach_safe(thread_network_data_prefix_cache_entry_t, cur, &cachePtr->localPrefixList ) {
01077         ns_list_foreach_safe(thread_network_data_context_entry_t, curContext, &cur->contextList) {
01078             if (!curContext->compression) {
01079                 if (curContext->context_reuse_delay > ticks) {
01080                     curContext->context_reuse_delay -= ticks;
01081                 } else {
01082                     ns_list_remove(&cur->contextList, curContext);
01083                     if (curContext->stableData) {
01084                         cachePtr->stableUpdatePushed = true;
01085                     } else {
01086                         cachePtr->temporaryUpdatePushed = true;
01087                     }
01088                     // Set context lifetime to 0 to delete
01089                     lowpan_context_update(context_list, curContext->cid, 0, NULL, 0, true);
01090                     ns_dyn_mem_free(curContext);
01091                 }
01092             }
01093         }
01094         if (ns_list_is_empty(&cur->borderRouterList) &&
01095         ns_list_is_empty(&cur->routeList) &&
01096         ns_list_is_empty(&cur->contextList)) {
01097             ns_list_remove(&cachePtr->localPrefixList , cur);
01098             ns_dyn_mem_free(cur);
01099         }
01100     }
01101 }
01102 
01103 
01104 void thread_network_data_free_and_clean(thread_network_data_cache_entry_t *cachePtr)
01105 {
01106     if (!ns_list_is_empty(&cachePtr->localPrefixList )) {
01107         ns_list_foreach_safe(thread_network_data_prefix_cache_entry_t, cur, &cachePtr->localPrefixList ) {
01108             thread_context_service_list_free(&cur->contextList);
01109             thread_server_list_free(&cur->borderRouterList);
01110             thread_server_list_free(&cur->routeList);
01111             ns_list_remove(&cachePtr->localPrefixList , cur);
01112             ns_dyn_mem_free(cur);
01113         }
01114         ns_list_init(&cachePtr->localPrefixList );
01115     }
01116 
01117     if (!ns_list_is_empty(&cachePtr->service_list)) {
01118         ns_list_foreach_safe(thread_network_data_service_cache_entry_t, cur, &cachePtr->service_list) {
01119             thread_service_server_list_free(&cur->server_list);
01120             if (cur->S_service_data) {
01121                 ns_dyn_mem_free(cur->S_service_data);
01122             }
01123             ns_list_remove(&cachePtr->service_list, cur);
01124             ns_dyn_mem_free(cur);
01125         }
01126         ns_list_init(&cachePtr->service_list);
01127     }
01128 
01129     cachePtr->networkDataTlvSize  = 0;
01130     cachePtr->stableUpdatePushed = false;
01131     cachePtr->temporaryUpdatePushed = false;
01132     cachePtr->network_data_update_delay = 0;
01133     cachePtr->network_data_len = 0;
01134 }
01135 
01136 void thread_network_local_data_free_and_clean(thread_network_local_data_cache_entry_t *cachePtr, int8_t interface_id)
01137 {
01138     ns_list_foreach_safe(thread_network_local_data_entry_t, cur, &cachePtr->prefix_list) {
01139         if (cur->dhcpv6ServerActive) {
01140             DHCPv6_server_service_delete(interface_id, cur->servicesPrefix, true);
01141         }
01142 
01143         ns_list_remove(&cachePtr->prefix_list, cur);
01144         ns_dyn_mem_free(cur);
01145     }
01146 
01147     ns_list_foreach_safe(thread_network_data_service_entry_t, cur, &cachePtr->service_list) {
01148         ns_list_remove(&cachePtr->service_list, cur);
01149         if (cur->S_service_data) {
01150             ns_dyn_mem_free(cur->S_service_data);
01151         }
01152         if (cur->S_server_data) {
01153             ns_dyn_mem_free(cur->S_server_data);
01154         }
01155         ns_dyn_mem_free(cur);
01156     }
01157 
01158     cachePtr->publish_pending = false;
01159     cachePtr->publish_active = false;
01160     cachePtr->release_old_address = false;
01161 }
01162 
01163 static bool thread_network_data_has_subtlv(uint8_t *network_data_ptr, uint16_t network_data_length, uint8_t *prefix, uint8_t prefix_length, uint8_t lookup_type)
01164 {
01165     uint8_t *dptr;
01166     uint8_t length;
01167     uint8_t type;
01168     dptr = network_data_ptr;
01169     while (network_data_length) {
01170         if (network_data_length >= 2) {
01171             type  = *dptr++;
01172             type &= THREAD_NWK_DATA_TYPE_MASK;
01173             length = *dptr++;
01174 
01175             if (length == 0) {
01176                 return false;
01177             }
01178             network_data_length -= 2;
01179             if (network_data_length >= length) {
01180                 // Set length ready for next check
01181                 network_data_length -= length;
01182 
01183                 if (type == THREAD_NWK_DATA_TYPE_PREFIX) {
01184                     thread_prefix_tlv_t prefix_tlv;
01185                     prefix_tlv.domainId = *dptr++;
01186                     prefix_tlv.PrefixLen = *dptr++;
01187                     prefix_tlv.Prefix = dptr;
01188                     length -= 2;
01189 
01190                     uint8_t prefix_bytes_len = prefixBits_to_bytes(prefix_tlv.PrefixLen);
01191                     uint8_t prefix_bytes_length = prefixBits_to_bytes(prefix_length);
01192 
01193                     if (prefix_bytes_len > length) {
01194                         return false;
01195                     }
01196 
01197                     if (prefix_bytes_len != prefix_bytes_length ||
01198                         memcmp(prefix, prefix_tlv.Prefix, prefix_bytes_len) != 0) {
01199                         dptr += length;
01200                         continue;
01201                     }
01202 
01203                     length -= prefix_bytes_len;
01204                     dptr += prefix_bytes_len;
01205 
01206                     while (length > 2) {
01207                         type = *dptr++;
01208                         type &= THREAD_NWK_DATA_TYPE_MASK;
01209                         uint8_t subLength = *dptr++;
01210                         length -= 2;
01211 
01212                         if (subLength <= length) {
01213                             length -= subLength;
01214                             dptr += subLength;
01215 
01216                             if (lookup_type == type) {
01217                                 return true;
01218                             }
01219                         } else {
01220                             tr_error("Length fail");
01221                             return false;
01222                         }
01223                     }
01224                 } else {
01225                     dptr += length;
01226                 }
01227             } else {
01228                 tr_error("Length fail");
01229                 return false;
01230             }
01231         } else {
01232             return false;
01233         }
01234     }
01235 
01236     return false;
01237 }
01238 
01239 static uint16_t thread_network_data_prefix_shrink_size(thread_network_data_cache_entry_t *networkDataStorage, uint8_t *network_data_ptr, uint16_t network_data_length, uint16_t router_id)
01240 {
01241     uint16_t network_data_len = 0;
01242 
01243     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkDataStorage->localPrefixList ) {
01244         ns_list_foreach(thread_network_server_data_entry_t, br_cur, &cur->borderRouterList) {
01245             if (br_cur->routerID == router_id) {
01246                 tr_debug("Checking prefix: %s", trace_ipv6_prefix(cur->servicesPrefix, cur->servicesPrefixLen));
01247                 if (!thread_network_data_has_subtlv(network_data_ptr, network_data_length, cur->servicesPrefix, cur->servicesPrefixLen, THREAD_NWK_DATA_TYPE_BORDER_ROUTER)) {
01248                     if (ns_list_count(&cur->borderRouterList) == 1) {
01249                         network_data_len += 2 + THREAD_BORDER_ROUTER_TLV_LENGTH;
01250                     } else {
01251                         network_data_len += THREAD_BORDER_ROUTER_TLV_LENGTH;
01252                     }
01253                 }
01254             }
01255         }
01256 
01257         ns_list_foreach(thread_network_server_data_entry_t, route_cur, &cur->routeList) {
01258             if (route_cur->routerID == router_id) {
01259                 if (!thread_network_data_has_subtlv(network_data_ptr, network_data_length, cur->servicesPrefix, cur->servicesPrefixLen, THREAD_NWK_DATA_TYPE_ROUTE)) {
01260                     if (ns_list_count(&cur->routeList) == 1) {
01261                         network_data_len += 2 + THREAD_HAS_ROUTE_TLV_LENGTH;
01262                         if (ns_list_count(&cur->borderRouterList) == 0) {
01263                             uint8_t prefix_bytes_len = prefixBits_to_bytes(cur->servicesPrefixLen);
01264                             network_data_len += 2 + 2 + prefix_bytes_len;
01265                         }
01266                     } else {
01267                         network_data_len += THREAD_HAS_ROUTE_TLV_LENGTH;
01268                     }
01269                 }
01270             }
01271         }
01272     }
01273 
01274     tr_debug("Shrink size: %d", network_data_len);
01275     return network_data_len;
01276 }
01277 
01278 /**
01279  * Calculates what the resulting size of prefixes for Network Data TLV would be.
01280  */
01281 static int thread_network_data_resulting_prefix_set_size(thread_network_data_cache_entry_t *networkDataStorage, uint8_t *network_data_ptr, uint16_t network_data_length, uint16_t rid)
01282 {
01283     uint8_t *dptr;
01284     uint8_t length;
01285     uint8_t type;
01286 
01287     if (!networkDataStorage || !network_data_ptr) {
01288         return -1;
01289     }
01290 
01291     uint16_t network_data_len = thread_network_data_prefix_set_size(networkDataStorage, true);
01292     tr_debug("Old network data length: %d", network_data_len);
01293 
01294     /* Calculate how much removed network data would reduce the total size */
01295     network_data_len -= thread_network_data_prefix_shrink_size(networkDataStorage, network_data_ptr, network_data_length, rid);
01296 
01297     dptr = network_data_ptr;
01298     while (network_data_length) {
01299         if (network_data_length >= 2) {
01300             type = *dptr++;
01301             type &= THREAD_NWK_DATA_TYPE_MASK;
01302             length = *dptr++;
01303 
01304             if (length == 0) {
01305                 // 0 is not valid length for TLV
01306                 return -1;
01307             }
01308 
01309             network_data_length -= 2;
01310 
01311             if (network_data_length >= length) {
01312                 // Set length ready for next check
01313                 network_data_length -= length;
01314 
01315                 if (type == THREAD_NWK_DATA_TYPE_PREFIX) {
01316                     thread_prefix_tlv_t prefix_tlv;
01317                     prefix_tlv.domainId = *dptr++;
01318                     prefix_tlv.PrefixLen = *dptr++;
01319                     prefix_tlv.Prefix = dptr;
01320                     length -= 2;
01321 
01322                     uint8_t prefix_bytes_len = prefixBits_to_bytes(prefix_tlv.PrefixLen);
01323 
01324                     if (prefix_bytes_len > length) {
01325                         return -1;
01326                     }
01327 
01328                     length -= prefix_bytes_len;
01329                     dptr += prefix_bytes_len;
01330 
01331                     tr_debug("Prefix: %s", trace_ipv6_prefix(prefix_tlv.Prefix, prefix_tlv.PrefixLen));
01332                     thread_network_data_prefix_cache_entry_t *prefix = thread_prefix_entry_find(&networkDataStorage->localPrefixList , &prefix_tlv);
01333 
01334                     if (!prefix) {
01335                         tr_debug("Adding new prefix!");
01336                         // This is a new prefix; add header length
01337                         network_data_len += 2 + 2 + prefix_bytes_len;
01338                     }
01339 
01340                     while (length > 2) {
01341                         type = *dptr++;
01342                         type &= THREAD_NWK_DATA_TYPE_MASK;
01343                         uint8_t subLength = *dptr++;
01344                         length -= 2;
01345 
01346                         tr_debug("SubType: %02x, %s", type, trace_array(dptr, subLength));
01347 
01348                         if (subLength <= length) {
01349                             length -= subLength;
01350 
01351                             if (type == THREAD_NWK_DATA_TYPE_BORDER_ROUTER) {
01352                                 if (!prefix) {
01353                                     // Add 6LoWPAN ID length (plus type & length)
01354                                     network_data_len += 2 + THREAD_6LOWPAN_ID_TLV_LENGTH;
01355                                 }
01356 
01357                                 while (subLength) {
01358                                     thread_network_server_data_entry_t *server_entry = NULL;
01359                                     uint16_t router_id = common_read_16_bit(dptr);
01360                                     dptr += 4;
01361 
01362                                     if (prefix) {
01363                                         server_entry = thread_server_entry_search(&prefix->borderRouterList, router_id);
01364                                     }
01365 
01366                                     if (!prefix || !server_entry) {
01367                                         if (!prefix || ns_list_count(&prefix->borderRouterList) == 0) {
01368                                             // Add Border Router type and length
01369                                             network_data_len += 2;
01370                                         }
01371                                         // Add Border Router TLV length
01372                                         network_data_len += THREAD_BORDER_ROUTER_TLV_LENGTH;
01373                                     }
01374 
01375                                     subLength -= THREAD_BORDER_ROUTER_TLV_LENGTH;
01376                                 }
01377 
01378                                 tr_debug("Type is BORDER ROUTER");
01379                             } else if (type == THREAD_NWK_DATA_TYPE_ROUTE) {
01380                                 while (subLength) {
01381                                     thread_network_server_data_entry_t *server_entry = NULL;
01382                                     uint16_t router_id  = common_read_16_bit(dptr);
01383                                     dptr += 3;
01384 
01385                                     if (prefix) {
01386                                         server_entry = thread_server_entry_search(&prefix->routeList, router_id);
01387                                     }
01388 
01389                                     if (!prefix || !server_entry) {
01390                                         if (!prefix || ns_list_count(&prefix->routeList) == 0) {
01391                                             // Add Has Route type and length
01392                                             network_data_len += 2;
01393                                         }
01394                                         // Add Has Route TLV length
01395                                         network_data_len += THREAD_HAS_ROUTE_TLV_LENGTH;
01396                                     }
01397 
01398                                     subLength -= THREAD_HAS_ROUTE_TLV_LENGTH;
01399                                 }
01400 
01401                                 tr_debug("Type is HAS ROUTE");
01402                             } else {
01403                                 dptr += subLength;
01404                             }
01405                         } else {
01406                             tr_error("Length fail");
01407                             return -1;
01408                         }
01409                     }
01410                 } else {
01411                     dptr += length;
01412                 }
01413             } else {
01414                 tr_error("Length fail");
01415                 return -1;
01416             }
01417         } else {
01418             return -1;
01419         }
01420     }
01421 
01422     return network_data_len;
01423 }
01424 
01425 /**
01426  * Calculates what the future size of Network Data TLV would be if new data was
01427  * incorporated to the old data. NOTE: Use of this function should be replaced
01428  * by some sophisticated logic to analyze if new data or part of it should be
01429  * given priority. Now we simply limit by the total size.
01430  */
01431 int thread_network_data_resulting_tlv_size(thread_network_data_cache_entry_t *networkDataStorage, uint8_t *network_data_ptr, uint16_t network_data_length, uint16_t router_id)
01432 {
01433     int network_data_len = thread_network_data_resulting_prefix_set_size(networkDataStorage, network_data_ptr, network_data_length, router_id);
01434 
01435     // TODO: Add support for other TLVs than Prefix here
01436 
01437     tr_debug("Size of the prefix set: %d", network_data_len);
01438 
01439     if (network_data_len >= 0) {
01440         // Add maximum Commission TLV size
01441         network_data_len += THREAD_MAX_COMM_DATA_TLV_LEN;
01442     }
01443 
01444     return network_data_len;
01445 }
01446 
01447 /**
01448  * Add new route information to route List
01449  *
01450  * \param networkDataList Pointer main network data structure
01451  * \param prefixTlv Prefix TLV (domainID, Prefix, PrefixLen)
01452  * \param service Route TLV
01453  *
01454  * return 0, ADD OK
01455  * return <0 Add Not OK
01456  */
01457 int thread_nd_local_list_add_route(thread_network_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv, thread_border_router_tlv_entry_t *route)
01458 {
01459     thread_network_server_data_entry_t *entry;
01460     tr_debug("Add Route: %s", trace_ipv6_prefix(prefixTlv->Prefix, prefixTlv->PrefixLen));
01461 
01462     route->P_configure = false;
01463     route->P_default_route = false;
01464     route->P_dhcp = false;
01465     route->P_preferred = false;
01466     route->P_slaac = false;
01467 
01468     if (!networkDataList) {
01469         return -1;
01470     }
01471 
01472     thread_network_data_prefix_cache_entry_t *prefix_entry = thread_prefix_entry_get(&networkDataList->localPrefixList , prefixTlv);
01473     if (!prefix_entry) {
01474         return -1;
01475     }
01476 
01477     entry = thread_server_entry_get(&prefix_entry->routeList, route);
01478     if (!entry) {
01479         return -1;
01480     }
01481 
01482     entry->canDelete = false;
01483     if ((thread_nd_local_data_length_updated(networkDataList) == 0)) {
01484         if (route->stableData) {
01485             networkDataList->stableUpdatePushed = true;
01486         } else {
01487             networkDataList->temporaryUpdatePushed = true;
01488         }
01489     } else if (entry->Prf != route->Prf) {
01490         entry->Prf = route->Prf;
01491         if (route->stableData) {
01492             networkDataList->stableUpdatePushed = true;
01493         } else {
01494             networkDataList->temporaryUpdatePushed = true;
01495         }
01496     }
01497     return 0;
01498 }
01499 
01500 int thread_nd_local_list_add_service(thread_network_data_cache_entry_t *networkDataList, thread_network_data_service_entry_t *service, thread_network_data_service_server_entry_t *server)
01501 {
01502     bool changed = false;
01503     tr_debug("Add Service: %s", trace_array(service->S_service_data, service->S_service_data_length));
01504 
01505     if (!networkDataList) {
01506         return -1;
01507     }
01508 
01509     thread_network_data_service_cache_entry_t *service_entry = thread_service_entry_get(&networkDataList->service_list, service);
01510     if (!service_entry) {
01511         return -1;
01512     }
01513 
01514     thread_network_data_service_server_entry_t *entry = thread_service_server_entry_get(&service_entry->server_list, server);
01515     if (!entry) {
01516         return -1;
01517     }
01518 
01519     if (entry->server_data_length != server->server_data_length || memcmp(entry->server_data,server->server_data,server->server_data_length) != 0) {
01520         // Server data changed
01521         if (entry->server_data_length != server->server_data_length ) {
01522             ns_dyn_mem_free(entry->server_data);
01523             entry->server_data = ns_dyn_mem_alloc(server->server_data_length);
01524             if (!entry->server_data) {
01525                 return -1;
01526             }
01527         }
01528         memcpy(entry->server_data,server->server_data,server->server_data_length);
01529         entry->server_data_length = server->server_data_length;
01530         changed = true;
01531     }
01532     entry->can_delete = false;
01533 
01534     if (changed || (thread_nd_local_data_length_updated(networkDataList) == 0)) {
01535         if (server->stable) {
01536             networkDataList->stableUpdatePushed = true;
01537         } else {
01538             networkDataList->temporaryUpdatePushed = true;
01539         }
01540     }
01541 
01542     return 0;
01543 }
01544 
01545 /**
01546  * Add new BorderRouter Server information to route List
01547  *
01548  * \param networkDataList Pointer main network data structure
01549  * \param prefixTlv Prefix TLv
01550  * \param service Border Router TLV
01551  *
01552  * return 0, ADD OK
01553  * return <0 Add Not OK
01554  */
01555 int thread_nd_local_list_add_on_mesh_prefix(thread_network_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv, thread_border_router_tlv_entry_t *service)
01556 {
01557     bool trigDataPropagate = false;
01558     thread_network_data_prefix_cache_entry_t *prefix_entry;
01559     thread_network_server_data_entry_t *server_entry;
01560     if (service->P_dhcp) {
01561         tr_debug("Add DHCPv6 prefix:%s server: %04x", trace_ipv6_prefix(prefixTlv->Prefix, prefixTlv->PrefixLen),service->routerID);
01562     } else {
01563         tr_debug("Add SLAAC prefix:%s server: %04x", trace_ipv6_prefix(prefixTlv->Prefix, prefixTlv->PrefixLen),service->routerID);
01564     }
01565 
01566     if (!networkDataList) {
01567         return -1;
01568     }
01569 
01570     if (!prefixTlv->PrefixLen) {
01571         return -1;
01572     }
01573 
01574     prefix_entry = thread_prefix_entry_get(&networkDataList->localPrefixList , prefixTlv);
01575     if (!prefix_entry) {
01576         return -1;
01577     }
01578 
01579     server_entry = thread_server_entry_get(&prefix_entry->borderRouterList, service);
01580 
01581     if (!server_entry) {
01582         return -1;
01583     }
01584     tr_debug("update current BR");
01585 
01586     server_entry->canDelete = false;
01587     if (thread_nd_local_data_length_updated(networkDataList) == 0) {
01588         trigDataPropagate = true;
01589     } else {
01590         if (server_entry->P_configure != service->P_configure) {
01591             server_entry->P_configure = service->P_configure;
01592             trigDataPropagate = true;
01593         }
01594 
01595         if (server_entry->P_default_route != service->P_default_route) {
01596             server_entry->P_default_route = service->P_default_route;
01597             trigDataPropagate = true;
01598         }
01599 
01600         if (server_entry->Prf != service->Prf) {
01601             server_entry->Prf = service->Prf;
01602             trigDataPropagate = true;
01603         }
01604 
01605         if (server_entry->P_preferred != service->P_preferred || server_entry->P_slaac != service->P_slaac) {
01606             server_entry->P_preferred = service->P_preferred;
01607             server_entry->P_slaac = service->P_slaac;
01608             trigDataPropagate = true;
01609         }
01610 
01611         if (server_entry->P_dhcp != service->P_dhcp) {
01612             server_entry->P_dhcp = service->P_dhcp;
01613             trigDataPropagate = true;
01614         }
01615 
01616         if (server_entry->P_on_mesh != service->P_on_mesh) {
01617             server_entry->P_on_mesh = service->P_on_mesh;
01618             trigDataPropagate = true;
01619         }
01620 
01621         if (server_entry->P_nd_dns != service->P_nd_dns) {
01622             server_entry->P_nd_dns = service->P_nd_dns;
01623             trigDataPropagate = true;
01624         }
01625     }
01626 
01627     if (trigDataPropagate) {
01628         if (service->stableData) {
01629             networkDataList->stableUpdatePushed = true;
01630         } else {
01631             networkDataList->temporaryUpdatePushed = true;
01632         }
01633     }
01634 
01635     return 0;
01636 }
01637 
01638 
01639 /**
01640  * Del DHCPv6 Server information to route List
01641  *
01642  * \param networkDataList Pointer main network data structure
01643  * \param prefixTlv Prefix TLV (domainID, Prefix, PrefixLen)
01644  * \param service On Mesh prefix TLV
01645  *
01646  * return 0, Del OK
01647  * return <0 Del Not OK
01648  */
01649 int thread_nd_local_list_del_on_mesh_server(thread_network_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv, thread_border_router_tlv_entry_t *service)
01650 {
01651     thread_network_data_prefix_cache_entry_t *main_list;
01652     thread_network_server_data_entry_t *entry;
01653 
01654     if (!networkDataList) {
01655         return -1;
01656     }
01657 
01658     if (prefixTlv->PrefixLen) {
01659         return -1;
01660     }
01661 
01662     if (service->P_dhcp) {
01663         tr_debug("Del DHCPv6 server: %s",
01664                  trace_array(prefixTlv->Prefix, prefixBits_to_bytes(prefixTlv->PrefixLen)));
01665     } else {
01666         tr_debug("Del SLAAC server: %s",
01667                  trace_array(prefixTlv->Prefix, prefixBits_to_bytes(prefixTlv->PrefixLen)));
01668     }
01669 
01670     main_list = thread_prefix_entry_get(&networkDataList->localPrefixList , prefixTlv);
01671     if (!main_list) {
01672         return -1;
01673     }
01674 
01675     entry = thread_server_entry_search(&main_list->borderRouterList, service->routerID);
01676 
01677     if (!entry) {
01678         return -1;
01679     }
01680 
01681     entry->canDelete = true;
01682     if (entry->stableData) {
01683         networkDataList->stableUpdatePushed = true;
01684     } else {
01685         networkDataList->temporaryUpdatePushed = true;
01686     }
01687 
01688     return 0;
01689 }
01690 
01691 /**
01692  * Add new Local DHCPv6 Server information to route List
01693  *
01694  * \param networkDataList Pointer main network data structure
01695  * \param prefixTlv Prefix TLV (domainID, Prefix, PrefixLen)
01696  * \param service On Mesh prefix TLV
01697  *
01698  * return 0, ADD OK
01699  * return <0 Add Not OK
01700  */
01701 int thread_local_server_list_add_on_mesh_server(thread_network_local_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv, thread_border_router_tlv_entry_t *service)
01702 {
01703     int retVal = -1;
01704     tr_debug("Add prefix: %s prf:%d %s%s%s%s%s", trace_ipv6_prefix(prefixTlv->Prefix, prefixTlv->PrefixLen), service->Prf,
01705              service->P_default_route?"Default Route ":"",service->P_dhcp?"DHCPv6 Server ":"", service->P_configure?"DHCPv6 Configuration ":"",
01706              service->P_slaac?"SLAAC ":"",service->P_preferred?"Preferred ":"");
01707 
01708     if (networkDataList) {
01709         thread_network_local_data_entry_t *prefix_entry = thread_local_prefix_entry_get(&networkDataList->prefix_list, prefixTlv);
01710         if (prefix_entry) {
01711             prefix_entry->preference = service->Prf;
01712             prefix_entry->configure = service->P_configure;
01713             prefix_entry->defaultRoute = service->P_default_route;
01714             prefix_entry->onMesh = service->P_on_mesh;
01715             prefix_entry->ndDns = service->P_nd_dns;
01716 
01717             if (service->P_dhcp) {
01718                 prefix_entry->dhcpv6ServerActive = true;
01719                 prefix_entry->dhcpv6ServerDataStable = service->stableData;
01720             }
01721 
01722             if (service->P_slaac) {
01723                 prefix_entry->slaacServerActive = true;
01724                 prefix_entry->slaacServerDataStable = service->stableData;
01725                 prefix_entry->slaacPreferred = service->P_preferred;
01726             }
01727 
01728             if (prefixTlv->PrefixLen == 0) {
01729                 // Adding as default route ::/0
01730                 prefix_entry->routeActive = true;
01731                 prefix_entry->routeDataStable = service->stableData;
01732             }
01733             retVal = 0;
01734         }
01735     }
01736     return retVal;
01737 }
01738 
01739 /**
01740  * Del Local DHCPv6 Server information to route List
01741  *
01742  * \param networkDataList Pointer main network data structure
01743  * \param prefixTlv Prefix TLV (domainID, Prefix, PrefixLen)
01744  *
01745  * return 0, Del OK
01746  * return <0 Del Not OK
01747  */
01748 int thread_local_server_list_del_on_mesh_server(thread_network_local_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv)
01749 {
01750     thread_network_local_data_entry_t *prefix_entry;
01751 
01752     if (!networkDataList) {
01753         return -1;
01754     }
01755 
01756     tr_debug("Delete prefix %s", trace_array(prefixTlv->Prefix, prefixBits_to_bytes(prefixTlv->PrefixLen)));
01757 
01758     prefix_entry = thread_local_prefix_entry_find(&networkDataList->prefix_list, prefixTlv);
01759     if (!prefix_entry) {
01760         return -1;
01761     }
01762 
01763     tr_debug("Prefix deleted");
01764     //Delete Entry
01765     ns_list_remove(&networkDataList->prefix_list, prefix_entry);
01766     ns_dyn_mem_free(prefix_entry);
01767     return 0;
01768 }
01769 
01770 /**
01771  * Modify or add new service information to the local service list.
01772  *
01773  * \param networkDataList Pointer main network data structure.
01774  * \param service Service information to store to the list.
01775  *
01776  * return 0, Operation OK
01777  * return <0 Operation not OK
01778  */
01779 int thread_local_service_list_add(thread_network_local_data_cache_entry_t *networkDataList, thread_network_data_service_entry_t *service)
01780 {
01781     if (!networkDataList) {
01782         return -1;
01783     }
01784 
01785     tr_debug("Add service %"PRIu32": %s", service->S_enterprise_number, trace_array(service->S_service_data, service->S_service_data_length));
01786 
01787     thread_network_data_service_entry_t *service_entry = thread_local_service_entry_save(&networkDataList->service_list, service);
01788 
01789     if (!service_entry) {
01790         return -1;
01791     }
01792 
01793     tr_debug("Service added");
01794 
01795     return 0;
01796 }
01797 
01798 /**
01799  * Delete service information from the local service list.
01800  *
01801  * \param networkDataList Pointer main network data structure
01802  * \param service Service information to use in deletion.
01803  *
01804  * return 0, Delete OK
01805  * return <0 Delete not OK
01806  */
01807 int thread_local_service_list_del(thread_network_local_data_cache_entry_t *networkDataList, thread_network_data_service_entry_t *service)
01808 {
01809     thread_network_data_service_entry_t *service_entry;
01810 
01811     if (!networkDataList || !service) {
01812         return -1;
01813     }
01814 
01815     tr_debug("Delete service %"PRIu32": %s", service->S_enterprise_number, trace_array(service->S_service_data, service->S_service_data_length));
01816 
01817     service_entry = thread_local_service_entry_find(&networkDataList->service_list, service);
01818 
01819     if (!service_entry) {
01820         return -1;
01821     }
01822 
01823     tr_debug("Service deleted");
01824 
01825     ns_list_remove(&networkDataList->service_list, service_entry);
01826 
01827     ns_dyn_mem_free(service_entry->S_service_data);
01828 
01829     if (service_entry->S_server_data) {
01830         ns_dyn_mem_free(service_entry->S_server_data);
01831     }
01832 
01833     ns_dyn_mem_free(service_entry);
01834     return 0;
01835 }
01836 
01837 /**
01838  * Add new local route information to route List
01839  *
01840  * \param networkDataList Pointer main network data structure
01841  * \param routePrefixPtr pointer to route prefix
01842  * \param prefixLength indicate prefix pointer valid information in bits
01843  * \param stableService Boolean true generate stable service, false temporary
01844  *
01845  * return 0, ADD OK
01846  * return <0 Add Not OK
01847  */
01848 int thread_local_server_add_route(thread_network_local_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv, thread_border_router_tlv_entry_t *route)
01849 {
01850     tr_debug("Add Route: %s", trace_ipv6_prefix(prefixTlv->Prefix, prefixTlv->PrefixLen));
01851 
01852     if (!networkDataList) {
01853         return -1;
01854     }
01855 
01856     thread_network_local_data_entry_t *prefix_entry = thread_local_prefix_entry_get(&networkDataList->prefix_list, prefixTlv);
01857     if (!prefix_entry) {
01858         return -1;
01859     }
01860 
01861     prefix_entry->routeActive = true;
01862     prefix_entry->routeDataStable = route->stableData;
01863     prefix_entry->preference = route->Prf;
01864 
01865     return 0;
01866 }
01867 
01868 
01869 /**
01870  * Del local route information to route List
01871  *
01872  * \param networkDataList Pointer main network data structure
01873  * \param prefixTlv Prefix TLV (domainID, Prefix, PrefixLen)
01874  *
01875  * return 0, Del OK
01876  * return <0 Del Not OK
01877  */
01878 int thread_local_server_del_route(thread_network_local_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv)
01879 {
01880     thread_network_local_data_entry_t *prefix_entry;
01881     tr_debug("Del Route: %s",
01882              trace_array(prefixTlv->Prefix, prefixBits_to_bytes(prefixTlv->PrefixLen)));
01883 
01884     if (!networkDataList) {
01885         return -1;
01886     }
01887 
01888     prefix_entry = thread_local_prefix_entry_find(&networkDataList->prefix_list, prefixTlv);
01889     if (!prefix_entry) {
01890         return -1;
01891     }
01892 
01893     prefix_entry->routeActive = false;
01894     if (prefix_entry->dhcpv6ServerActive) {
01895         return 0;
01896     } else if (prefix_entry->slaacServerActive) {
01897         return 0;
01898     }
01899 
01900     tr_debug("Free Entry");
01901     //Delete Entry
01902     ns_list_remove(&networkDataList->prefix_list, prefix_entry);
01903     ns_dyn_mem_free(prefix_entry);
01904 
01905     return 0;
01906 }
01907 
01908 static bool thread_nd_public_contex_id_allocated(thread_network_data_cache_entry_t *networkDataList, uint8_t tempId)
01909 {
01910     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkDataList->localPrefixList ) {
01911         if (thread_get_context_by_id(&cur->contextList, tempId)) {
01912             return true;
01913         }
01914     }
01915     return false;
01916 }
01917 
01918 /**
01919  * Add new 6LoWPAN contexts information to Network Data list
01920  *
01921  * \param networkDataList Network Interface
01922  * \param prefixPtr pointer 6LoWPAN Contexts
01923  * \param prefixLength indicate prefix pointer valid information in bits
01924  *
01925  * return 0, ADD OK
01926  * return <0 Add Not OK
01927  */
01928 uint8_t thread_nd_context_id_allocate(thread_network_data_cache_entry_t *networkDataList, thread_network_local_data_cache_entry_t *localDataList, uint8_t *prefixPtr, uint8_t prefixLength)
01929 {
01930     uint8_t cid = 16;
01931     uint8_t tempId;
01932 
01933     tr_debug("Disvover Context id for: %s",
01934              trace_ipv6_prefix(prefixPtr, prefixLength));
01935 
01936     if (!networkDataList || !localDataList) {
01937         return cid;
01938     } else if (!prefixLength) {
01939         return cid;
01940     }
01941 
01942     //Check first is context already allocated
01943 
01944     //Check Fisrt Public list
01945     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkDataList->localPrefixList ) {
01946         if (cur->servicesPrefixLen >= prefixLength) {
01947             if (bitsequal(cur->servicesPrefix, prefixPtr, prefixLength)) {
01948 
01949                 //Check Prefix context List
01950                 tempId = thread_get_context_id_by_length(&cur->contextList, prefixLength);
01951                 if (tempId != 16) {
01952                     return tempId;
01953                 }
01954             }
01955         }
01956     }
01957 
01958     //Allocate Free context id
01959     if (thread_extension_version_check(thread_version)) {
01960         tempId = 2;
01961     } else {
01962         tempId = 1;
01963     }
01964     for (; tempId < 16; tempId++) {
01965         if (thread_nd_public_contex_id_allocated(networkDataList, tempId)) {
01966             //Allocated
01967         } else {
01968             cid  = tempId;
01969             break;
01970         }
01971     }
01972 
01973     return cid;
01974 }
01975 
01976 
01977 /**
01978  * Add new 6LoWPAN contexts information to Network Data list
01979  *
01980  * \param networkDataList Network Data structure pointer
01981  * \param prefixTlv Prefix TLV (domainID, Prefix, PrefixLen)
01982  * \param context Context TLV
01983  *
01984  * return 0, ADD OK
01985  * return <0 Add Not OK
01986  */
01987 int thread_nd_local_list_add_contexts(thread_network_data_cache_entry_t *networkDataList, thread_prefix_tlv_t *prefixTlv, thread_network_local_data_context_entry_t *context)
01988 {
01989     thread_network_data_prefix_cache_entry_t *main_list;
01990     thread_network_data_context_entry_t *cur;
01991     tr_debug("Add Public Context: %s", trace_ipv6_prefix(prefixTlv->Prefix, prefixTlv->PrefixLen));
01992 
01993     if (!networkDataList) {
01994         return -1;
01995     }
01996 
01997     if (!prefixTlv->PrefixLen) {
01998         return -1;
01999     }
02000 
02001     main_list = thread_prefix_entry_get(&networkDataList->localPrefixList , prefixTlv);
02002     if (!main_list) {
02003         return -1;
02004     }
02005 
02006     cur = thread_get_main_context_list(&main_list->contextList, context);
02007     if (!cur) {
02008         return -1;
02009     }
02010     cur->canDelete = false;
02011     if (context->stableData == true) {
02012         cur->stableData = true;
02013     }
02014     cur->compression = context->compression;
02015     cur->contextPrefixLength = context->contextPrefixLength;
02016 
02017     return 0;
02018 }
02019 
02020 uint8_t thread_server_prefix_length(thread_network_local_data_entry_t *cur)
02021 {
02022     uint8_t tempLength = 0;
02023 
02024 
02025     if (cur->routeActive) {
02026         if (!((cur->slaacServerActive || cur->dhcpv6ServerActive) && cur->defaultRoute)) {
02027             // HasRoute is added if BorderRouter TLV does not have default route bit
02028             tempLength += 5;
02029         }
02030     }
02031 
02032     if (cur->dhcpv6ServerActive) {
02033         tempLength += 6;
02034     }
02035 
02036     if (cur->slaacServerActive) {
02037         tempLength += 6;
02038     }
02039 
02040     if (tempLength) {
02041         tempLength += prefixBits_to_bytes(cur->servicesPrefixLen);
02042         tempLength += 2; //Length &domainId
02043     }
02044 
02045     return tempLength;
02046 }
02047 
02048 static uint8_t thread_service_length(thread_network_data_service_entry_t *cur)
02049 {
02050     uint8_t length = 0;
02051 
02052     length += 1; // T + S_id
02053 
02054     if (!cur->T) {
02055         length += 4; // S_enterprise_number
02056     }
02057 
02058     // Service data length + service data
02059     length += 1 + cur->S_service_data_length;
02060 
02061     if (cur->S_server_data) {
02062         length += 2; // Sub-TLV's type & length
02063         length += 2; // S_server_16
02064         length += cur->S_server_data_length;
02065     }
02066 
02067     return length;
02068 }
02069 
02070 uint16_t thread_nd_own_service_list_data_size(thread_network_local_data_cache_entry_t *serverDataList)
02071 {
02072     uint16_t localDataLength = 0;
02073     uint16_t tempLength;
02074 
02075     ns_list_foreach(thread_network_data_service_entry_t, cur, &serverDataList->service_list) {
02076         tempLength = thread_service_length(cur);
02077         if (tempLength) {
02078             localDataLength += tempLength + 2; //Type & Length for service
02079         }
02080     }
02081 
02082     ns_list_foreach(thread_network_local_data_entry_t, cur, &serverDataList->prefix_list) {
02083         tempLength = thread_server_prefix_length(cur);
02084         if (tempLength) {
02085             localDataLength += tempLength + 2; //Type & Length for prefix
02086         }
02087     }
02088 
02089     return localDataLength;
02090 }
02091 
02092 static bool thread_check_local_data_prefix_stable_boolean(thread_network_local_data_entry_t *dataList)
02093 {
02094     if (dataList->dhcpv6ServerActive && dataList->dhcpv6ServerDataStable) {
02095         return true;
02096     }
02097 
02098     if (dataList->slaacServerActive && dataList->slaacServerDataStable) {
02099         return true;
02100     }
02101 
02102     return false;
02103 }
02104 
02105 static uint8_t *thread_nd_hosted_prefix_header_write(uint8_t *ptr, uint8_t length,thread_network_local_data_entry_t *cur)
02106 {
02107     uint8_t prefixBytesLen = prefixBits_to_bytes(cur->servicesPrefixLen);
02108     if (thread_check_local_data_prefix_stable_boolean(cur)) {
02109         *ptr++ = THREAD_NWK_DATA_TYPE_PREFIX | THREAD_NWK_STABLE_DATA;
02110     } else {
02111         *ptr++ = THREAD_NWK_DATA_TYPE_PREFIX;
02112     }
02113     *ptr++ = length;
02114     *ptr++ = cur->domainId;
02115     *ptr++ = cur->servicesPrefixLen;
02116     if (prefixBytesLen) {
02117         memset(ptr, 0, prefixBytesLen);
02118         bitcopy(ptr, cur->servicesPrefix, cur->servicesPrefixLen);
02119         ptr += prefixBytesLen;
02120     }
02121     return ptr;
02122 }
02123 
02124 static uint8_t *thread_nd_hosted_service_header_write(uint8_t *ptr, uint8_t length, thread_network_data_service_entry_t *cur)
02125 {
02126     if (cur->S_stable) {
02127         *ptr++ = THREAD_NWK_DATA_TYPE_SERVICE_DATA | THREAD_NWK_STABLE_DATA;
02128     } else {
02129         *ptr++ = THREAD_NWK_DATA_TYPE_SERVICE_DATA;
02130     }
02131 
02132     *ptr++ = length;
02133     *ptr++ = (cur->T << 7) | cur->S_id;
02134 
02135     if (!cur->T) {
02136         ptr = common_write_32_bit(cur->S_enterprise_number, ptr);
02137     }
02138 
02139     *ptr++ = cur->S_service_data_length;
02140 
02141     memcpy(ptr, cur->S_service_data, cur->S_service_data_length);
02142     ptr += cur->S_service_data_length;
02143 
02144     return ptr;
02145 }
02146 
02147 static bool thread_nd_network_data_prefix_stable(thread_network_data_prefix_cache_entry_t *dataList)
02148 {
02149     ns_list_foreach(thread_network_server_data_entry_t, cur, &dataList->borderRouterList) {
02150         if (cur->stableData) {
02151             return true;
02152         }
02153     }
02154     ns_list_foreach(thread_network_server_data_entry_t, cur, &dataList->routeList) {
02155         if (cur->stableData) {
02156             return true;
02157         }
02158     }
02159     ns_list_foreach(thread_network_data_context_entry_t, cur, &dataList->contextList) {
02160         if (cur->stableData) {
02161             return true;
02162         }
02163     }
02164     return false;
02165 }
02166 
02167 static uint8_t *thread_nd_prefix_header_write(uint8_t *ptr, thread_network_data_prefix_cache_entry_t *cur, uint8_t tlvLength)
02168 {
02169     uint8_t prefixBytesLen = prefixBits_to_bytes(cur->servicesPrefixLen );
02170     if (thread_nd_network_data_prefix_stable(cur)) {
02171         *ptr++ = THREAD_NWK_DATA_TYPE_PREFIX | THREAD_NWK_STABLE_DATA;
02172     } else {
02173         *ptr++ = THREAD_NWK_DATA_TYPE_PREFIX;
02174     }
02175     *ptr++ = tlvLength;
02176     *ptr++ = cur->domainId;
02177     *ptr++ = cur->servicesPrefixLen ;
02178     if (prefixBytesLen) {
02179         memcpy(ptr, cur->servicesPrefix ,  prefixBytesLen);
02180         ptr += prefixBytesLen;
02181     }
02182 
02183     return ptr;
02184 }
02185 
02186 static bool thread_nd_network_data_service_stable(thread_network_data_service_cache_entry_t *dataList)
02187 {
02188     ns_list_foreach(thread_network_data_service_server_entry_t, cur, &dataList->server_list) {
02189         if (cur->stable) {
02190             return true;
02191         }
02192     }
02193     return false;
02194 }
02195 
02196 static uint8_t *thread_nd_service_header_write(uint8_t *ptr, thread_network_data_service_cache_entry_t *cur, uint8_t tlvLength)
02197 {
02198     if (thread_nd_network_data_service_stable(cur)) {
02199         *ptr++ = THREAD_NWK_DATA_TYPE_SERVICE_DATA | THREAD_NWK_STABLE_DATA;
02200     } else {
02201         *ptr++ = THREAD_NWK_DATA_TYPE_SERVICE_DATA;
02202     }
02203 
02204     *ptr++ = tlvLength;
02205     *ptr++ = (cur->T << 7) | cur->S_id;
02206 
02207     if (!cur->T) {
02208         ptr = common_write_32_bit(cur->S_enterprise_number, ptr);
02209     }
02210 
02211     *ptr++ = cur->S_service_data_length;
02212 
02213     memcpy(ptr, cur->S_service_data, cur->S_service_data_length);
02214     ptr += cur->S_service_data_length;
02215 
02216     return ptr;
02217 }
02218 
02219 static uint8_t *thread_nd_hosted_service_server_write(uint8_t *ptr, uint16_t router_id, thread_network_data_service_entry_t *cur)
02220 {
02221     if (cur->S_stable) {
02222         *ptr++ = THREAD_NWK_DATA_TYPE_SERVER_DATA | THREAD_NWK_STABLE_DATA;
02223     } else {
02224         *ptr++ = THREAD_NWK_DATA_TYPE_SERVER_DATA;
02225     }
02226 
02227     *ptr++ = 2 + cur->S_server_data_length;
02228     ptr = common_write_16_bit(router_id, ptr);
02229 
02230     memcpy(ptr, cur->S_server_data, cur->S_server_data_length);
02231     ptr += cur->S_server_data_length;
02232 
02233     return ptr;
02234 }
02235 
02236 static uint8_t *thread_service_border_router_tlv_write(uint8_t *ptr, uint8_t tlvType, uint16_t routerId, uint16_t flags)
02237 {
02238     *ptr++ = tlvType;
02239     *ptr++ = THREAD_BORDER_ROUTER_TLV_LENGTH;
02240     ptr = thread_nd_network_data_border_router_tlv_write(ptr, routerId, flags);
02241     return ptr;
02242 }
02243 
02244 static uint8_t *thread_service_has_route_tlv_write(uint8_t *ptr, uint8_t tlvType, uint16_t routerId, uint8_t preference)
02245 {
02246     *ptr++ = tlvType;
02247     *ptr++ = THREAD_HAS_ROUTE_TLV_LENGTH;
02248      ptr = thread_nd_network_data_has_route_tlv_write(ptr, routerId, preference);
02249     return ptr;
02250 }
02251 
02252 uint8_t * thread_nd_own_service_list_data_write(thread_network_local_data_cache_entry_t *serverDataList, uint8_t *ptr, uint16_t routerID)
02253 {
02254     uint8_t tlvType;
02255     uint8_t servicesLen;
02256 
02257     ns_list_foreach(thread_network_data_service_entry_t, cur, &serverDataList->service_list) {
02258         servicesLen = thread_service_length(cur);
02259         if (servicesLen) {
02260             ptr = thread_nd_hosted_service_header_write(ptr, servicesLen, cur);
02261             if (cur->S_server_data) {
02262                 ptr = thread_nd_hosted_service_server_write(ptr, routerID, cur);
02263             }
02264         }
02265     }
02266 
02267     ns_list_foreach(thread_network_local_data_entry_t, cur, &serverDataList->prefix_list) {
02268         servicesLen = thread_server_prefix_length(cur);
02269         if (servicesLen) {
02270             ptr = thread_nd_hosted_prefix_header_write(ptr,servicesLen, cur);
02271             if (cur->routeActive) {
02272                 if (!((cur->slaacServerActive || cur->dhcpv6ServerActive) && cur->defaultRoute)) {
02273                     // HasRoute is added if BorderRouter TLV does not have default route bit
02274                     uint8_t preference = 0;
02275                     tlvType = THREAD_NWK_DATA_TYPE_ROUTE;
02276                     if (cur->routeDataStable) {
02277                         tlvType |= THREAD_NWK_STABLE_DATA;
02278                     }
02279                     if (cur->preference) {
02280                         preference |= cur->preference << THREAD_HAS_ROUTE_PRF_BIT_MOVE;
02281                     }
02282                     ptr = thread_service_has_route_tlv_write(ptr, tlvType, routerID, preference);
02283                 }
02284             }
02285 
02286             if (cur->slaacServerActive || cur->dhcpv6ServerActive) {
02287                 uint16_t flags = 0;
02288                 tlvType = THREAD_NWK_DATA_TYPE_BORDER_ROUTER;
02289                 if (cur->slaacServerDataStable || cur->dhcpv6ServerDataStable) {
02290                     tlvType |= THREAD_NWK_STABLE_DATA;
02291                 }
02292                 if (cur->slaacServerActive) {
02293                     flags |= 1 << THREAD_P_SLAAC_BIT_MOVE;
02294                 }
02295                 if (cur->dhcpv6ServerActive) {
02296                     flags |= 1 << THREAD_P_DHCP_BIT_MOVE;
02297                 }
02298                 if (cur->slaacPreferred) {
02299                     flags |= 1 << THREAD_P_PREFERRED_BIT_MOVE;
02300                 }
02301                 if (cur->preference) {
02302                     flags |= cur->preference << THREAD_PRF_BIT_MOVE;
02303                 }
02304                 if (cur->configure) {
02305                     flags |= 1 << THREAD_P_CONFIGURE_BIT_MOVE;
02306                 }
02307                 if (cur->defaultRoute) {
02308                     flags |= 1 << THREAD_P_DEF_ROUTE_BIT_MOVE;
02309                 }
02310                 if (cur->onMesh) {
02311                     flags |= 1 << THREAD_P_ON_MESH_BIT_MOVE;
02312                 }
02313                 if (cur->ndDns) {
02314                     flags |= 1 << THREAD_P_ND_DNS_BIT_MOVE;
02315                 }
02316                 ptr = thread_service_border_router_tlv_write(ptr, tlvType, routerID, flags);
02317             } // slaac or dhcp
02318         }
02319     }
02320     return ptr;
02321 }
02322 
02323 bool thread_nd_dhcp_anycast_address_mapping_from_network_data(thread_network_data_cache_entry_t *networkDataList, uint16_t *rlocAddress, uint8_t contexId)
02324 {
02325     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkDataList->localPrefixList ) {
02326         if (thread_get_context_by_id(&cur->contextList, contexId)) {
02327             //Check current Stable List
02328             thread_network_server_data_entry_t *server = thread_get_dhcp_server_from_list(&cur->borderRouterList);
02329             if (server) {
02330                 *rlocAddress = server->routerID;
02331                 return true;
02332             }
02333             return false;
02334         }
02335     }
02336     return false;
02337 }
02338 
02339 bool thread_nd_service_anycast_address_mapping_from_network_data(thread_network_data_cache_entry_t *networkDataList, uint16_t *rlocAddress, uint8_t S_id)
02340 {
02341     ns_list_foreach(thread_network_data_service_cache_entry_t, curService, &networkDataList->service_list) {
02342         // Go through all services
02343         if (curService->S_id != S_id) {
02344             continue;
02345         }
02346         ns_list_foreach(thread_network_data_service_server_entry_t, curServiceServer, &curService->server_list) {
02347             *rlocAddress = curServiceServer->router_id;
02348             return true;
02349         }
02350     }
02351     return false;
02352 }
02353 
02354 bool thread_nd_on_mesh_address_valid(thread_network_server_data_entry_t *curRoute)
02355 {
02356     bool onMeshActive = false;
02357     if (curRoute->P_dhcp || curRoute->P_slaac || curRoute->P_preferred) {
02358         onMeshActive = true;
02359     }
02360 
02361     return onMeshActive;
02362 }
02363 
02364 thread_network_server_data_entry_t *thread_nd_hosted_by_this_routerid(uint16_t routerId, thread_network_server_data_list_t *list)
02365 {
02366     ns_list_foreach(thread_network_server_data_entry_t, curInfo, list) {
02367         if (curInfo->routerID == routerId) {
02368             return curInfo;
02369         }
02370     }
02371     return NULL;
02372 }
02373 bool thread_network_data_services_registered(thread_network_data_cache_entry_t *cachePtr, uint16_t routerID)
02374 {
02375     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &cachePtr->localPrefixList ) {
02376         if (thread_nd_hosted_by_this_routerid(routerID, &cur->borderRouterList)) {
02377             return true;
02378         }
02379         if (thread_nd_hosted_by_this_routerid(routerID, &cur->routeList)) {
02380             return true;
02381         }
02382     }
02383     return false;
02384 }
02385 
02386 uint16_t thread_network_data_service_set_size(thread_network_data_cache_entry_t *networkDataList, bool fullList)
02387 {
02388     uint16_t localDataLength = 0;
02389     uint16_t tempLength;
02390 
02391     ns_list_foreach(thread_network_data_service_cache_entry_t, cur, &networkDataList->service_list) {
02392         tempLength = thread_nd_service_based_on_list_entry_size(cur, fullList);
02393         if (tempLength) {
02394             localDataLength += tempLength + 2; //Type & Length for service
02395         }
02396     }
02397 
02398     return localDataLength;
02399 }
02400 
02401 uint16_t thread_network_data_prefix_set_size(thread_network_data_cache_entry_t *networkDataList, bool fullList)
02402 {
02403     uint16_t localDataLength = 0;
02404     uint16_t tempLength;
02405 
02406     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkDataList->localPrefixList ) {
02407         tempLength = thread_nd_prefix_based_on_list_entry_size(cur, fullList);
02408         if (tempLength) {
02409             localDataLength += tempLength + 2; //Type & Length for prefix
02410         }
02411     }
02412 
02413     return localDataLength;
02414 }
02415 
02416 uint8_t *thread_network_data_prefix_set_write(thread_network_data_cache_entry_t *networkDataList, uint8_t *ptr)
02417 {
02418     uint8_t tlvLength;
02419 
02420     //Add always Prefix based data first and Border Router List after That
02421     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkDataList->localPrefixList ) {
02422         tlvLength = thread_nd_prefix_based_on_list_entry_size(cur, true);
02423         if (tlvLength) {
02424             ptr = thread_nd_prefix_header_write(ptr, cur, tlvLength);
02425             tr_debug("Prefix: %s", trace_ipv6_prefix(cur->servicesPrefix, cur->servicesPrefixLen));
02426 
02427             //Follow canonical order first temporary data and then stable
02428             //Stable & Temporary subTLVS need to be smallest first also
02429             //SET Route always to 1. THREAD_NWK_DATA_TYPE_ROUTE
02430             //DHCPv6 Services 2. THREAD_NWK_DATA_TYPE_BORDER_ROUTER
02431             //SET Context always to 3. THREAD_NWK_DATA_TYPE_6LOWPAN_ID
02432 
02433             //write Temporary List this includes the stable and unstable servers with correct address.
02434             ptr = thread_nd_server_list_write(&cur->routeList, ptr, THREAD_NWK_DATA_TYPE_ROUTE, false);
02435             ptr = thread_nd_server_list_write(&cur->borderRouterList, ptr, THREAD_NWK_DATA_TYPE_BORDER_ROUTER, false);
02436             ptr = thread_nd_context_list_write(&cur->contextList, ptr, false);
02437 
02438             ptr = thread_nd_server_list_write(&cur->routeList, ptr, THREAD_NWK_DATA_TYPE_ROUTE, true);
02439             ptr = thread_nd_server_list_write(&cur->borderRouterList, ptr, THREAD_NWK_DATA_TYPE_BORDER_ROUTER, true);
02440             ptr = thread_nd_context_list_write(&cur->contextList, ptr, true);
02441         }
02442     }
02443     return ptr;
02444 }
02445 
02446 uint8_t *thread_network_data_service_set_write(thread_network_data_cache_entry_t *networkDataList, uint8_t *ptr)
02447 {
02448     uint8_t tlvLength;
02449 
02450     ns_list_foreach(thread_network_data_service_cache_entry_t, cur, &networkDataList->service_list) {
02451         tlvLength = thread_nd_service_based_on_list_entry_size(cur, true);
02452         if (tlvLength) {
02453             ptr = thread_nd_service_header_write(ptr, cur, tlvLength);
02454             tr_debug("Service: sid:%d e:%d data:%s",cur->S_id, cur->S_enterprise_number, trace_array(cur->S_service_data, cur->S_service_data_length));
02455             ptr = thread_nd_service_server_list_write(&cur->server_list, ptr, true);
02456         }
02457     }
02458 
02459     return ptr;
02460 }
02461 
02462 bool thread_network_data_service_hosted_by_this_router_id(thread_network_data_service_cache_entry_t *dataList, uint16_t router_id)
02463 {
02464     ns_list_foreach(thread_network_data_service_server_entry_t, cur, &dataList->server_list) {
02465         if (cur->router_id == router_id) {
02466             return true;
02467         }
02468     }
02469     return false;
02470 }
02471 
02472 uint16_t thread_network_data_service_child_id_from_networkdata_get(thread_network_data_cache_entry_t *networkDataList, uint16_t router_short_addr)
02473 {
02474     ns_list_foreach(thread_network_data_service_cache_entry_t, service, &networkDataList->service_list) {
02475         ns_list_foreach(thread_network_data_service_server_entry_t, server, &service->server_list) {
02476             if (thread_addr_is_child(router_short_addr, server->router_id)) {
02477                 return server->router_id;
02478             }
02479         }
02480     }
02481 
02482     ns_list_foreach(thread_network_data_prefix_cache_entry_t, cur, &networkDataList->localPrefixList ) {
02483         ns_list_foreach(thread_network_server_data_entry_t, curRoute, &cur->routeList) {
02484             if (thread_addr_is_child(router_short_addr, curRoute->routerID)) {
02485                 return curRoute->routerID;
02486             }
02487         }
02488 
02489         ns_list_foreach(thread_network_server_data_entry_t, curBR, &cur->borderRouterList) {
02490             if (thread_addr_is_child(router_short_addr, curBR->routerID)) {
02491                 return curBR->routerID;
02492             }
02493         }
02494     }
02495 
02496     return 0;
02497 }
02498 #endif