thread_meshcop_lib.c

00001 /*
00002  * Copyright (c) 2015-2019, 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 {
00400     (void)ptr;
00401     (void)length;
00402     (void)type;
00403     (void)result_ptr;
00404     return 0;
00405 }
00406 
00407 uint8_t *thread_meshcop_tlv_data_write(uint8_t *ptr, uint8_t type, uint16_t length, const uint8_t *data)
00408 {
00409     (void)ptr;
00410     (void)type;
00411     (void)length;
00412     (void)data;
00413     return NULL;
00414 }
00415 
00416 uint8_t *thread_meshcop_tlv_data_write_uint8(uint8_t *ptr, uint8_t type, uint8_t data)
00417 {
00418     (void) ptr;
00419     (void)type;
00420     (void)data;
00421     return NULL;
00422 }
00423 
00424 uint8_t *thread_meshcop_tlv_data_write_uint16(uint8_t *ptr, uint8_t type, uint16_t data)
00425 {
00426     (void) ptr;
00427     (void)type;
00428     (void)data;
00429     return NULL;
00430 }
00431 
00432 uint8_t *thread_meshcop_tlv_data_write_uint32(uint8_t *ptr, uint8_t type, uint32_t data)
00433 {
00434     (void) ptr;
00435     (void)type;
00436     (void)data;
00437     return NULL;
00438 }
00439 
00440 uint8_t *thread_meshcop_tlv_data_write_uint64(uint8_t *ptr, uint8_t type, uint64_t data)
00441 {
00442     (void) ptr;
00443     (void)type;
00444     (void)data;
00445     return NULL;
00446 }
00447 bool thread_meshcop_tlv_exist(const uint8_t *ptr, const uint16_t length, const uint8_t type)
00448 {
00449     (void)ptr;
00450     (void)length;
00451     (void)type;
00452     return false;
00453 }
00454 
00455 int16_t thread_meshcop_tlv_length(const uint8_t *ptr, uint16_t length)
00456 {
00457     (void)ptr;
00458     (void)length;
00459     return 0;
00460 }
00461 
00462 int16_t thread_meshcop_tlv_length_required(const uint8_t *ptr, uint16_t length)
00463 {
00464     (void)ptr;
00465     (void)length;
00466     return 0;
00467 }
00468 
00469 const uint8_t *thread_meshcop_tlv_get_next(const uint8_t *ptr, uint16_t *length)
00470 {
00471     (void)ptr;
00472     (void)length;
00473     return NULL;
00474 }
00475 
00476 bool thread_meshcop_tlv_list_present(const uint8_t *ptr, uint16_t length, const uint8_t *required_tlv_ptr, uint8_t required_tlv_len)
00477 {
00478     (void)ptr;
00479     (void)length;
00480     (void)required_tlv_ptr;
00481     (void)required_tlv_len;
00482     return false;
00483 }
00484 uint16_t thread_meshcop_tlv_list_generate(const uint8_t *ptr, uint16_t length, uint8_t *result_ptr, uint16_t *result_len)
00485 {
00486     (void)ptr;
00487     (void)length;
00488     (void)result_ptr;
00489     (void)result_len;
00490     return 0;
00491 }
00492 uint16_t thread_meshcop_tlv_list_remove(uint8_t *tlv_ptr, uint16_t tlv_len, uint8_t tlv_type)
00493 {
00494     (void)tlv_ptr;
00495     (void)tlv_len;
00496     (void)tlv_type;
00497     return 0;
00498 }
00499 
00500 bool thread_meshcop_tlv_list_type_available(const uint8_t *list_ptr, uint16_t list_len, uint8_t tlv_type)
00501 {
00502     (void)list_ptr;
00503     (void)list_len;
00504     (void)tlv_type;
00505     return false;
00506 }
00507 
00508 uint16_t thread_meshcop_tlv_find_next(uint8_t *tlv_ba, uint16_t tlv_ba_length, uint8_t tlv_id, uint8_t **found_tlv)
00509 {
00510     (void)tlv_ba;
00511     (void)tlv_ba_length;
00512     (void)tlv_id;
00513     (void)found_tlv;
00514     return 0;
00515 }
00516 
00517 uint8_t thread_meshcop_tlv_data_get_uint8(const uint8_t *ptr, uint16_t length, uint8_t type, uint8_t *data_ptr)
00518 {
00519     (void)ptr;
00520     (void)length;
00521     (void)type;
00522     (void)data_ptr;
00523     return 0;
00524 }
00525 
00526 uint8_t thread_meshcop_tlv_data_get_uint16(const uint8_t *ptr, uint16_t length, uint8_t type, uint16_t *data_ptr)
00527 {
00528     (void) ptr;
00529     (void)length;
00530     (void)type;
00531     (void)data_ptr;
00532     return 0;
00533 }
00534 
00535 uint8_t thread_meshcop_tlv_data_get_uint32(const uint8_t *ptr, uint16_t length, uint8_t type, uint32_t *data_ptr)
00536 {
00537     (void) ptr;
00538     (void)length;
00539     (void)type;
00540     (void)data_ptr;
00541     return 0;
00542 }
00543 
00544 uint8_t thread_meshcop_tlv_data_get_uint64(const uint8_t *ptr, uint16_t length, uint8_t type, uint64_t *data_ptr)
00545 {
00546     (void) ptr;
00547     (void)length;
00548     (void)type;
00549     (void)data_ptr;
00550     return 0;
00551 }
00552 
00553 #endif