thread_meshcop_lib.c

00001 /*
00002  * Copyright (c) 2015-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 #include "nsconfig.h"
00031 
00032 #include "thread_meshcop_lib.h"
00033 #include "common_functions.h"
00034 #include <string.h>
00035 #ifdef HAVE_THREAD
00036 uint8_t *thread_meshcop_tlv_data_write(uint8_t *ptr, uint8_t type, uint16_t length, const uint8_t *data)
00037 {
00038     if (!ptr) {
00039         return ptr;    // Check parameters
00040     }
00041     *ptr++ = type;
00042     if (length > 254) {
00043         *ptr++ = 0Xff;
00044         ptr = common_write_16_bit(length, ptr);
00045     } else {
00046         *ptr++ = length;
00047     }
00048     if ( length > 0  && data) {
00049         memcpy(ptr, data, length);
00050     }
00051     return ptr + length;
00052 }
00053 uint8_t *thread_meshcop_tlv_data_write_header(uint8_t *ptr, uint8_t type, uint16_t length)
00054 {
00055     if (!ptr) {
00056         return ptr;    // Check parameters
00057     }
00058     *ptr++ = type;
00059     if (length > 254) {
00060         *ptr++ = 0Xff;
00061         ptr = common_write_16_bit(length, ptr);
00062     } else {
00063         *ptr++ = length;
00064     }
00065     return ptr;
00066 }
00067 
00068 uint8_t *thread_meshcop_tlv_data_write_uint8(uint8_t *ptr, uint8_t type, uint8_t data)
00069 {
00070     *ptr++ = type;
00071     *ptr++ = 1;
00072     *ptr++ = data;
00073     return ptr;
00074 }
00075 
00076 uint8_t *thread_meshcop_tlv_data_write_uint16(uint8_t *ptr, uint8_t type, uint16_t data)
00077 {
00078     *ptr++ = type;
00079     *ptr++ = 2;
00080     ptr = common_write_16_bit(data, ptr);
00081     return ptr;
00082 }
00083 uint8_t *thread_meshcop_tlv_data_write_uint32(uint8_t *ptr, uint8_t type, uint32_t data)
00084 {
00085     *ptr++ = type;
00086     *ptr++ = 4;
00087     ptr = common_write_32_bit(data, ptr);
00088     return ptr;
00089 }
00090 
00091 uint8_t *thread_meshcop_tlv_data_write_uint64(uint8_t *ptr, uint8_t type, uint64_t data)
00092 {
00093     *ptr++ = type;
00094     *ptr++ = 8;
00095     ptr = common_write_64_bit(data, ptr);
00096     return ptr;
00097 }
00098 
00099 bool thread_meshcop_tlv_exist(const uint8_t *ptr, const uint16_t length, const uint8_t type)
00100 {
00101     const uint8_t *p;
00102     if (!ptr || length < 2) {
00103         return false;
00104     }
00105 
00106     p = ptr;
00107     while (p != NULL) {
00108         const uint8_t *tlv_data_ptr;
00109         uint16_t tlv_data_length;
00110         // check if we have enough length for normal length tlv
00111         if (p + 2 > ptr + length) {
00112             break;    //must have at least type and short length
00113         }
00114 
00115         if (p[1] == 0xff) {
00116             // Long length format
00117             if (p + 4 > ptr + length) {
00118                 break;    // check if enough length for long length
00119             }
00120 
00121             tlv_data_length = common_read_16_bit(&p[2]);
00122             tlv_data_ptr = p + 4;
00123         } else {
00124             tlv_data_length = p[1];
00125             tlv_data_ptr = p + 2;
00126         }
00127 
00128         // check if length of tlv is correct
00129         if (tlv_data_ptr + tlv_data_length > ptr + length) {
00130             break;    //length goes past the data block
00131         }
00132 
00133         if (*p == type) {
00134             // Correct TLV found
00135             return true;
00136         }
00137 
00138         p = tlv_data_ptr + tlv_data_length;
00139     }
00140     return false;
00141 }
00142 
00143 uint16_t thread_meshcop_tlv_find(const uint8_t *ptr, uint16_t length, uint8_t type, uint8_t **result_ptr)
00144 {
00145     const uint8_t *p;
00146     if (!ptr || length < 2) {
00147         return 0;
00148     }
00149     //tr_info("tlv_find length: %d, type: %d", length, type);
00150 
00151     p = ptr;
00152     while (p != NULL) {
00153         const uint8_t *tlv_data_ptr;
00154         uint16_t tlv_data_length;
00155         //tr_info("tlv_find first check");
00156         // check if we have enough length for normal length tlv
00157         if (p + 2 > ptr + length) {
00158             break;    //must have at least type and short length
00159         }
00160 
00161         if (p[1] == 0xff) {
00162             // Long length format
00163             if (p + 4 > ptr + length) {
00164                 break;    // check if enough length for long length
00165             }
00166 
00167             tlv_data_length = common_read_16_bit(&p[2]);
00168             tlv_data_ptr = p + 4;
00169         } else {
00170             tlv_data_length = p[1];
00171             tlv_data_ptr = p + 2;
00172         }
00173         //tr_info("tlv_find check: %d, type: %d", tlv_data_length, *p);
00174 
00175         // check if length of tlv is correct
00176         if (tlv_data_ptr + tlv_data_length > ptr + length) {
00177             break;    //length goes past the data block
00178         }
00179 
00180         if (*p == type) {
00181             // Correct TLV found
00182             //tr_info("tlv_find Found: %d, type: %d", tlv_data_length, *p);
00183             if (result_ptr != NULL) {
00184                 *result_ptr = (uint8_t *)tlv_data_ptr;
00185             }
00186             // return the correct tlv data
00187             return tlv_data_length;
00188         }
00189 
00190         p = tlv_data_ptr + tlv_data_length;
00191     }
00192     return 0;
00193 }
00194 
00195 static const uint8_t *thread_meshcop_next_tlv(const uint8_t *ptr, uint16_t length)
00196 {
00197     // This fails if returned pointer would go past the length and it must have 2 bytes room
00198     if (length < 4) {
00199         return NULL;
00200     }
00201     if (ptr[1] == 0xff) {
00202         // Long length format
00203         if (length < 6) {
00204             return NULL;
00205         }
00206         return ptr + 4 + common_read_16_bit(&ptr[2]);
00207     }
00208     return ptr + 2 + ptr[1];
00209 }
00210 int16_t thread_meshcop_tlv_length(const uint8_t *ptr, uint16_t length)
00211 {
00212     if (length < 2) {
00213         return -1;
00214     }
00215     if (ptr[1] == 0xff) {
00216         // Long length format
00217         if (length < 4) {
00218             return -1;
00219         }
00220         return common_read_16_bit(&ptr[2]);
00221     }
00222     return ptr[1];
00223 }
00224 
00225 int16_t thread_meshcop_tlv_length_required(const uint8_t *ptr, uint16_t length)
00226 {
00227     if (length < 2) {
00228         return -1;
00229     }
00230     if (ptr[1] == 0xff) {
00231         // Long length format
00232         if (length < 4) {
00233             return -1;
00234         }
00235         return 4 + common_read_16_bit(&ptr[2]);
00236     }
00237     return 2 + ptr[1];
00238 }
00239 
00240 const uint8_t *thread_meshcop_tlv_get_next(const uint8_t *ptr, uint16_t *length)
00241 {
00242     if (!ptr || !length) {
00243         return NULL;
00244     }
00245 
00246     const uint8_t *next_ptr = thread_meshcop_next_tlv(ptr,*length);
00247 
00248     if (!next_ptr) {
00249         // No next TLV present
00250         *length = 0;
00251         return NULL;
00252     }
00253     *length -= next_ptr - ptr;
00254     return next_ptr;
00255 }
00256 
00257 bool thread_meshcop_tlv_list_present(const uint8_t *ptr, uint16_t length, const uint8_t *required_tlv_ptr, uint8_t required_tlv_len)
00258 {
00259     if (!ptr || !required_tlv_ptr) {
00260         return false;
00261     }
00262 
00263     for(uint8_t n = 0;n < sizeof(required_tlv_len);n++) {
00264         if(0 == thread_meshcop_tlv_find(ptr, length, required_tlv_ptr[n], NULL)){
00265             return false;
00266         }
00267     }
00268     return true;
00269 }
00270 uint16_t thread_meshcop_tlv_list_generate(const uint8_t *ptr, uint16_t length,uint8_t *result_ptr, uint16_t *result_len)
00271 {
00272     if (!ptr || length < 2 || !result_len) {
00273         return 0;
00274     }
00275 
00276     *result_len = 0;
00277     while(ptr && length) {
00278         if (result_ptr) {
00279             *result_ptr++ = *ptr; // write the TLV name
00280         }
00281         (*result_len)++; // Count the amount of TLVs
00282         ptr = thread_meshcop_tlv_get_next(ptr,&length);
00283     }
00284     return *result_len;
00285 }
00286 uint16_t thread_meshcop_tlv_list_remove(uint8_t *tlv_ptr, uint16_t tlv_len, uint8_t tlv_type)
00287 {
00288     uint8_t *start_ptr = tlv_ptr;
00289 
00290     if((!tlv_ptr) || (!tlv_len)){
00291         return 0;
00292     }
00293 
00294     /* Go through TLV's */
00295     while((tlv_ptr - start_ptr) < tlv_len){
00296         /* If match, remove it from list */
00297         if(*tlv_ptr == tlv_type){
00298             /* If not last TLV in list, shift */
00299             if(((tlv_ptr - start_ptr) + 1) < tlv_len){
00300                 memmove(tlv_ptr, tlv_ptr + 1, (tlv_len - ((tlv_ptr - start_ptr) + 1)));
00301             }
00302             /* Update length */
00303             tlv_len--;
00304         /* No match, go to next... */
00305         } else{
00306             tlv_ptr++;
00307         }
00308     }
00309 
00310     return tlv_len;
00311 }
00312 
00313 bool thread_meshcop_tlv_list_type_available(const uint8_t *list_ptr, uint16_t list_len, uint8_t tlv_type)
00314 {
00315     for (uint16_t i = 0 ;i < list_len; i++) {
00316         if (*(list_ptr + i) == tlv_type) {
00317             return true;
00318         }
00319     }
00320     return false;
00321 }
00322 
00323 uint16_t thread_meshcop_tlv_find_next(uint8_t* tlv_ba, uint16_t tlv_ba_length, uint8_t tlv_id, uint8_t** found_tlv)
00324 {
00325     //tr_debug("thread_meshcop_tlv_find_next TLV ID=%d ", tlv_id);
00326     uint8_t* prev_tlv = *found_tlv;
00327     uint16_t tlv_length = thread_meshcop_tlv_find((const uint8_t*)tlv_ba, tlv_ba_length, tlv_id, found_tlv);
00328 
00329     if (!prev_tlv || tlv_length == 0) {
00330         // first TLV requested or TLV not found
00331         return tlv_length;
00332     }
00333     
00334     while (tlv_length > 0) {
00335         tlv_ba_length = tlv_ba_length - tlv_length - (*found_tlv - tlv_ba); // adjust length
00336         tlv_ba = *found_tlv + tlv_length; //+ 2; // skip already found TLV
00337 
00338         if (*found_tlv == prev_tlv) {
00339             // current TLV is matching previous one, return next TLV
00340             return thread_meshcop_tlv_find((const uint8_t*)tlv_ba, tlv_ba_length, tlv_id, found_tlv);
00341         }
00342         tlv_length = thread_meshcop_tlv_find((const uint8_t*)tlv_ba, tlv_ba_length, tlv_id, found_tlv);
00343     }
00344 
00345     return tlv_length;
00346 }
00347 
00348 uint8_t thread_meshcop_tlv_data_get_uint8(const uint8_t *ptr, uint16_t length, uint8_t type, uint8_t *data_ptr)
00349 {
00350     uint8_t result_len;
00351     uint8_t *result_ptr;
00352     result_len = thread_meshcop_tlv_find(ptr, length, type, &result_ptr);
00353 
00354     //tr_info("tlv_find Found: %s",trace_array(result_ptr,result_len));
00355     if (result_len >= 1 && data_ptr) {
00356         *data_ptr = *result_ptr;
00357     }
00358     return result_len;
00359 }
00360 
00361 uint8_t thread_meshcop_tlv_data_get_uint16(const uint8_t *ptr, uint16_t length, uint8_t type, uint16_t *data_ptr)
00362 {
00363     uint8_t result_len;
00364     uint8_t *result_ptr;
00365 
00366     result_len = thread_meshcop_tlv_find(ptr, length, type, &result_ptr);
00367     if (result_len >= 2 && data_ptr) {
00368         *data_ptr = common_read_16_bit(result_ptr);
00369     }
00370     return result_len;
00371 }
00372 
00373 uint8_t thread_meshcop_tlv_data_get_uint32(const uint8_t *ptr, uint16_t length, uint8_t type, uint32_t *data_ptr)
00374 {
00375     uint8_t result_len;
00376     uint8_t *result_ptr;
00377 
00378     result_len = thread_meshcop_tlv_find(ptr, length, type, &result_ptr);
00379     if (result_len >= 4 && data_ptr) {
00380         *data_ptr = common_read_32_bit(result_ptr);
00381     }
00382     return result_len;
00383 }
00384 
00385 uint8_t thread_meshcop_tlv_data_get_uint64(const uint8_t *ptr, uint16_t length, uint8_t type, uint64_t *data_ptr)
00386 {
00387     uint8_t result_len;
00388     uint8_t *result_ptr;
00389 
00390     result_len = thread_meshcop_tlv_find(ptr, length, type, &result_ptr);
00391     if (result_len >= 8 && data_ptr) {
00392         *data_ptr = common_read_64_bit(result_ptr);
00393     }
00394     return result_len;
00395 }
00396 
00397 #else
00398 uint16_t thread_meshcop_tlv_find(const uint8_t *ptr, uint16_t length, uint8_t type, uint8_t **result_ptr) {
00399     (void)ptr;
00400     (void)length;
00401     (void)type;
00402     (void)result_ptr;
00403     return 0;
00404 }
00405 
00406 uint8_t *thread_meshcop_tlv_data_write(uint8_t *ptr, uint8_t type, uint16_t length, const uint8_t *data) {
00407     (void)ptr;
00408     (void)type;
00409     (void)length;
00410     (void)data;
00411     return NULL;
00412 }
00413 
00414 uint8_t *thread_meshcop_tlv_data_write_uint8(uint8_t *ptr, uint8_t type, uint8_t data)
00415 {
00416     (void) ptr;
00417     (void)type;
00418     (void)data;
00419     return NULL;
00420 }
00421 
00422 uint8_t *thread_meshcop_tlv_data_write_uint16(uint8_t *ptr, uint8_t type, uint16_t data)
00423 {
00424     (void) ptr;
00425     (void)type;
00426     (void)data;
00427     return NULL;
00428 }
00429 
00430 uint8_t *thread_meshcop_tlv_data_write_uint32(uint8_t *ptr, uint8_t type, uint32_t data)
00431 {
00432     (void) ptr;
00433     (void)type;
00434     (void)data;
00435     return NULL;
00436 }
00437 
00438 uint8_t *thread_meshcop_tlv_data_write_uint64(uint8_t *ptr, uint8_t type, uint64_t data) {
00439     (void) ptr;
00440     (void)type;
00441     (void)data;
00442     return NULL;
00443 }
00444 bool thread_meshcop_tlv_exist(const uint8_t *ptr, const uint16_t length, const uint8_t type)
00445 {
00446     (void)ptr;
00447     (void)length;
00448     (void)type;
00449     return false;
00450 }
00451 
00452 int16_t thread_meshcop_tlv_length(const uint8_t *ptr, uint16_t length)
00453 {
00454     (void)ptr;
00455     (void)length;
00456     return 0;
00457 }
00458 
00459 int16_t thread_meshcop_tlv_length_required(const uint8_t *ptr, uint16_t length)
00460 {
00461     (void)ptr;
00462     (void)length;
00463     return 0;
00464 }
00465 
00466 const uint8_t *thread_meshcop_tlv_get_next(const uint8_t *ptr, uint16_t *length)
00467 {
00468     (void)ptr;
00469     (void)length;
00470     return NULL;
00471 }
00472 
00473 bool thread_meshcop_tlv_list_present(const uint8_t *ptr, uint16_t length, const uint8_t *required_tlv_ptr, uint8_t required_tlv_len)
00474 {
00475     (void)ptr;
00476     (void)length;
00477     (void)required_tlv_ptr;
00478     (void)required_tlv_len;
00479     return false;
00480 }
00481 uint16_t thread_meshcop_tlv_list_generate(const uint8_t *ptr, uint16_t length,uint8_t *result_ptr, uint16_t *result_len)
00482 {
00483     (void)ptr;
00484     (void)length;
00485     (void)result_ptr;
00486     (void)result_len;
00487     return 0;
00488 }
00489 uint16_t thread_meshcop_tlv_list_remove(uint8_t *tlv_ptr, uint16_t tlv_len, uint8_t tlv_type)
00490 {
00491     (void)tlv_ptr;
00492     (void)tlv_len;
00493     (void)tlv_type;
00494     return 0;
00495 }
00496 
00497 bool thread_meshcop_tlv_list_type_available(const uint8_t *list_ptr, uint16_t list_len, uint8_t tlv_type)
00498 {
00499     (void)list_ptr;
00500     (void)list_len;
00501     (void)tlv_type;
00502     return false;
00503 }
00504 
00505 uint16_t thread_meshcop_tlv_find_next(uint8_t* tlv_ba, uint16_t tlv_ba_length, uint8_t tlv_id, uint8_t** found_tlv)
00506 {
00507     (void)tlv_ba;
00508     (void)tlv_ba_length;
00509     (void)tlv_id;
00510     (void)found_tlv;
00511     return 0;
00512 }
00513 
00514 uint8_t thread_meshcop_tlv_data_get_uint8(const uint8_t *ptr, uint16_t length, uint8_t type, uint8_t *data_ptr)
00515 {
00516     (void)ptr;
00517     (void)length;
00518     (void)type;
00519     (void)data_ptr;
00520     return 0;
00521 }
00522 
00523 uint8_t thread_meshcop_tlv_data_get_uint16(const uint8_t *ptr, uint16_t length, uint8_t type, uint16_t *data_ptr)
00524 {
00525     (void) ptr;
00526     (void)length;
00527     (void)type;
00528     (void)data_ptr;
00529     return 0;
00530 }
00531 
00532 uint8_t thread_meshcop_tlv_data_get_uint32(const uint8_t *ptr, uint16_t length, uint8_t type, uint32_t *data_ptr)
00533 {
00534     (void) ptr;
00535     (void)length;
00536     (void)type;
00537     (void)data_ptr;
00538     return 0;
00539 }
00540 
00541 uint8_t thread_meshcop_tlv_data_get_uint64(const uint8_t *ptr, uint16_t length, uint8_t type, uint64_t *data_ptr)
00542 {
00543     (void) ptr;
00544     (void)length;
00545     (void)type;
00546     (void)data_ptr;
00547     return 0;
00548 }
00549 
00550 #endif