common_functions.h

00001 /*
00002  * Copyright (c) 2014-2015 ARM Limited. All rights reserved.
00003  * SPDX-License-Identifier: Apache-2.0
00004  * Licensed under the Apache License, Version 2.0 (the License); you may
00005  * not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00012  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 #ifndef COMMON_FUNCTIONS_H_
00017 #define COMMON_FUNCTIONS_H_
00018 
00019 #include "ns_types.h"
00020 
00021 #ifdef __cplusplus
00022 extern "C" {
00023 #endif
00024 
00025 /*
00026  * Common write 64-bit variable to 8-bit pointer.
00027  *
00028  * Write 64 bits in big-endian (network) byte order.
00029  *
00030  * \param value 64-bit variable
00031  * \param ptr pointer where data to be written
00032  *
00033  * \return updated pointer
00034  */
00035 NS_INLINE uint8_t *common_write_64_bit(uint64_t value, uint8_t ptr[__static 8]);
00036 
00037 /*
00038  * Common read 64-bit variable from 8-bit pointer.
00039  *
00040  * Read 64 bits in big-endian (network) byte order.
00041  *
00042  * \param data_buf pointer where data to be read
00043  *
00044  * \return 64-bit variable
00045  */
00046 NS_INLINE uint64_t common_read_64_bit(const uint8_t data_buf[__static 8]);
00047 
00048 /*
00049  * Common write 32-bit variable to 8-bit pointer.
00050  *
00051  * Write 32 bits in big-endian (network) byte order.
00052  *
00053  * \param value 32-bit variable
00054  * \param ptr pointer where data to be written
00055  *
00056  * \return updated pointer
00057  */
00058 NS_INLINE uint8_t *common_write_32_bit(uint32_t value, uint8_t ptr[__static 4]);
00059 
00060 /*
00061  * Common read 32-bit variable from 8-bit pointer.
00062  *
00063  * Read 32 bits in big-endian (network) byte order.
00064  *
00065  * \param data_buf pointer where data to be read
00066  *
00067  * \return 32-bit variable
00068  */
00069 NS_INLINE uint32_t common_read_32_bit(const uint8_t data_buf[__static 4]);
00070 
00071 /*
00072  * Common write 32-bit variable to 8-bit pointer.
00073  *
00074  * Write 32 bits in little-endian byte order.
00075  *
00076  * \param value 32-bit variable
00077  * \param ptr pointer where data to be written
00078  *
00079  * \return updated pointer
00080  */
00081 NS_INLINE uint8_t *common_write_32_bit_inverse(uint32_t value, uint8_t ptr[__static 4]);
00082 
00083 /*
00084  * Common read 32-bit variable from 8-bit pointer.
00085  *
00086  * Read 32 bits in little-endian byte order.
00087  *
00088  * \param data_buf pointer where data to be read
00089  *
00090  * \return 32-bit variable
00091  */
00092 NS_INLINE uint32_t common_read_32_bit_inverse(const uint8_t data_buf[__static 4]);
00093 
00094 /*
00095  * Common write 24-bit variable to 8-bit pointer.
00096  *
00097  * Write 24 bits in big-endian (network) byte order.
00098  *
00099  * \param value 24-bit variable
00100  * \param ptr pointer where data to be written
00101  *
00102  * \return updated pointer
00103  */
00104 NS_INLINE uint8_t *common_write_24_bit(uint_fast24_t value, uint8_t ptr[__static 3]);
00105 
00106 /*
00107  * Common read 24-bit variable from 8-bit pointer.
00108  *
00109  * Read 24 bits in big-endian (network) byte order.
00110  *
00111  * \param data_buf pointer where data to be read
00112  *
00113  * \return 24-bit variable
00114  */
00115 NS_INLINE uint_fast24_t common_read_24_bit(const uint8_t data_buf[__static 3]);
00116 
00117 /*
00118  * Common write 24-bit variable to 8-bit pointer.
00119  *
00120  * Write 24 bits in little-endian byte order.
00121  *
00122  * \param value 24-bit variable
00123  * \param ptr pointer where data to be written
00124  *
00125  * \return updated pointer
00126  */
00127 NS_INLINE uint8_t *common_write_24_bit_inverse(uint_fast24_t value, uint8_t ptr[__static 3]);
00128 
00129 /*
00130  * Common read 24-bit variable from 8-bit pointer.
00131  *
00132  * Read 24 bits in little-endian byte order.
00133  *
00134  * \param data_buf pointer where data to be read
00135  *
00136  * \return 24-bit variable
00137  */
00138 NS_INLINE uint_fast24_t common_read_24_bit_inverse(const uint8_t data_buf[__static 3]);
00139 
00140 /*
00141  * Common write 16-bit variable to 8-bit pointer.
00142  *
00143  * Write 16 bits in big-endian (network) byte order.
00144  *
00145  * \param value 16-bit variable
00146  * \param ptr pointer where data to be written
00147  *
00148  * \return updated pointer
00149  */
00150 NS_INLINE uint8_t *common_write_16_bit(uint16_t value, uint8_t ptr[__static 2]);
00151 
00152 /*
00153  * Common read 16-bit variable from 8-bit pointer.
00154  *
00155  * Read 16 bits in big-endian (network) byte order.
00156  *
00157  * \param data_buf pointer where data to be read
00158  *
00159  * \return 16-bit variable
00160  */
00161 NS_INLINE uint16_t common_read_16_bit(const uint8_t data_buf[__static 2]);
00162 
00163 /*
00164  * Common write 16-bit variable to 8-bit pointer.
00165  *
00166  * Write 16 bits in little-endian byte order.
00167  *
00168  * \param value 16-bit variable
00169  * \param ptr pointer where data to be written
00170  *
00171  * \return updated pointer
00172  */
00173 NS_INLINE uint8_t *common_write_16_bit_inverse(uint16_t value, uint8_t ptr[__static 2]);
00174 
00175 /*
00176  * Common read 16-bit variable from 8-bit pointer.
00177  *
00178  * Read 16 bits in little-endian byte order.
00179  *
00180  * \param data_buf pointer where data to be read
00181  *
00182  * \return 16-bit variable
00183  */
00184 NS_INLINE uint16_t common_read_16_bit_inverse(const uint8_t data_buf[__static 2]);
00185 
00186 /*
00187  * Count bits in a byte
00188  *
00189  * \param value byte to inspect
00190  *
00191  * \return number of 1-bits in byte
00192  */
00193 NS_INLINE uint_fast8_t common_count_bits(uint8_t value);
00194 
00195 /*
00196  * Count leading zeros in a byte
00197  *
00198  * \deprecated Use common_count_leading_zeros_8
00199  *
00200  * \param value byte to inspect
00201  *
00202  * \return number of leading zeros in byte (0-8)
00203  */
00204 NS_INLINE uint_fast8_t common_count_leading_zeros(uint8_t value);
00205 
00206 /*
00207  * Count leading zeros in a byte
00208  *
00209  * \param value byte to inspect
00210  *
00211  * \return number of leading zeros in byte (0-8)
00212  */
00213 NS_INLINE uint_fast8_t common_count_leading_zeros_8(uint8_t value);
00214 
00215 /*
00216  * Count leading zeros in a 16-bit value
00217  *
00218  * \param value value to inspect
00219  *
00220  * \return number of leading zeros in byte (0-16)
00221  */
00222 NS_INLINE uint_fast8_t common_count_leading_zeros_16(uint16_t value);
00223 
00224 /*
00225  * Count leading zeros in a 32-bit value
00226  *
00227  * \param value value to inspect
00228  *
00229  * \return number of leading zeros in byte (0-32)
00230  */
00231 NS_INLINE uint_fast8_t common_count_leading_zeros_32(uint32_t value);
00232 
00233 /*
00234  * Compare 8-bit serial numbers
00235  *
00236  * Compare two 8-bit serial numbers, according to RFC 1982 Serial Number
00237  * Arithmetic.
00238  *
00239  * \param s1 first serial number
00240  * \param s2 second serial number
00241  *
00242  * \return true if s1 > s2
00243  * \return false if s1 <= s2, or the comparison is undefined
00244  */
00245 NS_INLINE bool common_serial_number_greater_8(uint8_t s1, uint8_t s2);
00246 
00247 /*
00248  * Compare 16-bit serial numbers
00249  *
00250  * Compare two 16-bit serial numbers, according to RFC 1982 Serial Number
00251  * Arithmetic.
00252  *
00253  * \param s1 first serial number
00254  * \param s2 second serial number
00255  *
00256  * \return true if s1 > s2
00257  * \return false if s1 <= s2, or the comparison is undefined
00258  */
00259 NS_INLINE bool common_serial_number_greater_16(uint16_t s1, uint16_t s2);
00260 
00261 /*
00262  * Compare 32-bit serial numbers
00263  *
00264  * Compare two 32-bit serial numbers, according to RFC 1982 Serial Number
00265  * Arithmetic.
00266  *
00267  * \param s1 first serial number
00268  * \param s2 second serial number
00269  *
00270  * \return true if s1 > s2
00271  * \return false if s1 <= s2, or the comparison is undefined
00272  */
00273 NS_INLINE bool common_serial_number_greater_32(uint32_t s1, uint32_t s2);
00274 
00275 /*
00276  * Test a bit in an bit array.
00277  *
00278  * Check whether a particular bit is set in a bit string. The bit array
00279  * is in big-endian (network) bit order.
00280  *
00281  * \param bitset pointer to bit array
00282  * \param bit index of bit - 0 is the most significant bit of the first byte
00283  *
00284  * \return true if the bit is set
00285  */
00286 NS_INLINE bool bit_test(const uint8_t *bitset, uint_fast8_t bit);
00287 
00288 /*
00289  * Set a bit in an bit array.
00290  *
00291  * Set a bit in a bit array. The array is in big-endian (network) bit order.
00292  *
00293  * \param bitset pointer to bit array
00294  * \param bit index of bit - 0 is the most significant bit of the first byte
00295  */
00296 NS_INLINE void bit_set(uint8_t *bitset, uint_fast8_t bit);
00297 
00298 /*
00299  * Clear a bit in an bit array.
00300  *
00301  * Clear a bit in a bit array. The bit array is in big-endian (network) bit order.
00302  *
00303  * \param bitset pointer to bit array
00304  * \param bit index of bit - 0 is the most significant bit of the first byte
00305  */
00306 NS_INLINE void bit_clear(uint8_t *bitset, uint_fast8_t bit);
00307 
00308 /*
00309  * Compare two bitstrings.
00310  *
00311  * Compare two bitstrings of specified length. The bit strings are in
00312  * big-endian (network) bit order.
00313  *
00314  * \param a pointer to first string
00315  * \param b pointer to second string
00316  * \param bits number of bits to compare
00317  *
00318  * \return true if the strings compare equal
00319  */
00320 bool bitsequal(const uint8_t *a, const uint8_t *b, uint_fast8_t bits);
00321 
00322 /*
00323  * Copy a bitstring
00324  *
00325  * Copy a bitstring of specified length. The bit string is in big-endian
00326  * (network) bit order. Bits beyond the bitlength at the destination are not
00327  * modified.
00328  *
00329  * For example, copying 4 bits sets the first 4 bits of dst[0] from src[0],
00330  * the lower 4 bits of dst[0] are unmodified.
00331  *
00332  * \param dst destination pointer
00333  * \param src source pointer
00334  * \param bits number of bits to copy
00335  *
00336  * \return the value of dst
00337  */
00338 uint8_t *bitcopy(uint8_t *restrict dst, const uint8_t *restrict src, uint_fast8_t bits);
00339 
00340 /*
00341  * Copy a bitstring and pad last byte with zeros
00342  *
00343  * Copy a bitstring of specified length. The bit string is in big-endian
00344  * (network) bit order. Bits beyond the bitlength in the last destination byte are
00345  * zeroed.
00346  *
00347  * For example, copying 4 bits sets the first 4 bits of dst[0] from src[0], and
00348  * the lower 4 bits of dst[0] are set to 0.
00349  *
00350  * \param dst destination pointer
00351  * \param src source pointer
00352  * \param bits number of bits to copy
00353  *
00354  * \return the value of dst
00355  */
00356 uint8_t *bitcopy0(uint8_t *restrict dst, const uint8_t *restrict src, uint_fast8_t bits);
00357 
00358 /* Provide definitions, either for inlining, or for common_functions.c */
00359 #if defined NS_ALLOW_INLINING || defined COMMON_FUNCTIONS_FN
00360 #ifndef COMMON_FUNCTIONS_FN
00361 #define COMMON_FUNCTIONS_FN NS_INLINE
00362 #endif
00363 
00364 COMMON_FUNCTIONS_FN uint8_t *common_write_64_bit(uint64_t value, uint8_t ptr[__static 8])
00365 {
00366     *ptr++ = value >> 56;
00367     *ptr++ = value >> 48;
00368     *ptr++ = value >> 40;
00369     *ptr++ = value >> 32;
00370     *ptr++ = value >> 24;
00371     *ptr++ = value >> 16;
00372     *ptr++ = value >> 8;
00373     *ptr++ = value;
00374     return ptr;
00375 }
00376 
00377 COMMON_FUNCTIONS_FN uint64_t common_read_64_bit(const uint8_t data_buf[__static 8])
00378 {
00379     uint64_t temp_64;
00380     temp_64 = (uint64_t)(*data_buf++) << 56;
00381     temp_64 += (uint64_t)(*data_buf++) << 48;
00382     temp_64 += (uint64_t)(*data_buf++) << 40;
00383     temp_64 += (uint64_t)(*data_buf++) << 32;
00384     temp_64 += (uint64_t)(*data_buf++) << 24;
00385     temp_64 += (uint64_t)(*data_buf++) << 16;
00386     temp_64 += (uint64_t)(*data_buf++) << 8;
00387     temp_64 += *data_buf++;
00388     return temp_64;
00389 }
00390 
00391 COMMON_FUNCTIONS_FN uint8_t *common_write_32_bit(uint32_t value, uint8_t ptr[__static 4])
00392 {
00393     *ptr++ = value >> 24;
00394     *ptr++ = value >> 16;
00395     *ptr++ = value >> 8;
00396     *ptr++ = value;
00397     return ptr;
00398 }
00399 
00400 COMMON_FUNCTIONS_FN uint32_t common_read_32_bit(const uint8_t data_buf[__static 4])
00401 {
00402     uint32_t temp_32;
00403     temp_32 = (uint32_t)(*data_buf++) << 24;
00404     temp_32 += (uint32_t)(*data_buf++) << 16;
00405     temp_32 += (uint32_t)(*data_buf++) << 8;
00406     temp_32 += *data_buf++;
00407     return temp_32;
00408 }
00409 
00410 COMMON_FUNCTIONS_FN uint8_t *common_write_32_bit_inverse(uint32_t value, uint8_t ptr[__static 4])
00411 {
00412     *ptr++ = value;
00413     *ptr++ = value >> 8;
00414     *ptr++ = value >> 16;
00415     *ptr++ = value >> 24;
00416     return ptr;
00417 }
00418 
00419 COMMON_FUNCTIONS_FN uint32_t common_read_32_bit_inverse(const uint8_t data_buf[__static 4])
00420 {
00421     uint32_t temp_32;
00422     temp_32 =  *data_buf++;
00423     temp_32 += (uint32_t)(*data_buf++) << 8;
00424     temp_32 += (uint32_t)(*data_buf++) << 16;
00425     temp_32 += (uint32_t)(*data_buf++) << 24;
00426     return temp_32;
00427 }
00428 
00429 COMMON_FUNCTIONS_FN uint8_t *common_write_24_bit(uint_fast24_t value, uint8_t ptr[__static 3])
00430 {
00431     *ptr++ = value >> 16;
00432     *ptr++ = value >> 8;
00433     *ptr++ = value;
00434     return ptr;
00435 }
00436 
00437 COMMON_FUNCTIONS_FN uint_fast24_t common_read_24_bit(const uint8_t data_buf[__static 3])
00438 {
00439     uint_fast24_t temp_24;
00440     temp_24 = (uint_fast24_t)(*data_buf++) << 16;
00441     temp_24 += (uint_fast24_t)(*data_buf++) << 8;
00442     temp_24 += *data_buf++;
00443     return temp_24;
00444 }
00445 
00446 COMMON_FUNCTIONS_FN uint8_t *common_write_24_bit_inverse(uint_fast24_t value, uint8_t ptr[__static 3])
00447 {
00448     *ptr++ = value;
00449     *ptr++ = value >> 8;
00450     *ptr++ = value >> 16;
00451     return ptr;
00452 }
00453 
00454 COMMON_FUNCTIONS_FN uint_fast24_t common_read_24_bit_inverse(const uint8_t data_buf[__static 3])
00455 {
00456     uint_fast24_t temp_24;
00457     temp_24 =  *data_buf++;
00458     temp_24 += (uint_fast24_t)(*data_buf++) << 8;
00459     temp_24 += (uint_fast24_t)(*data_buf++) << 16;
00460     return temp_24;
00461 }
00462 
00463 COMMON_FUNCTIONS_FN uint8_t *common_write_16_bit(uint16_t value, uint8_t ptr[__static 2])
00464 {
00465     *ptr++ = value >> 8;
00466     *ptr++ = value;
00467     return ptr;
00468 }
00469 
00470 COMMON_FUNCTIONS_FN uint16_t common_read_16_bit(const uint8_t data_buf[__static 2])
00471 {
00472     uint16_t temp_16;
00473     temp_16 = (uint16_t)(*data_buf++) << 8;
00474     temp_16 += *data_buf++;
00475     return temp_16;
00476 }
00477 
00478 COMMON_FUNCTIONS_FN uint8_t *common_write_16_bit_inverse(uint16_t value, uint8_t ptr[__static 2])
00479 {
00480     *ptr++ = value;
00481     *ptr++ = value >> 8;
00482     return ptr;
00483 }
00484 
00485 COMMON_FUNCTIONS_FN uint16_t common_read_16_bit_inverse(const uint8_t data_buf[__static 2])
00486 {
00487     uint16_t temp_16;
00488     temp_16 = *data_buf++;
00489     temp_16 += (uint16_t)(*data_buf++) << 8;
00490     return temp_16;
00491 }
00492 
00493 COMMON_FUNCTIONS_FN uint_fast8_t common_count_bits(uint8_t value)
00494 {
00495     /* First step sets each bit pair to be count of bits (00,01,10) */
00496     /* [00-00 = 00, 01-00 = 01, 10-01 = 01, 11-01 = 10] */
00497     uint_fast8_t count = value - ((value >> 1) & 0x55);
00498     /* Add bit pairs to make each nibble contain count of bits (0-4) */
00499     count = (count & 0x33) + ((count >> 2) & 0x33);
00500     /* Final result is sum of nibbles (0-8) */
00501     count = (count >> 4) + (count & 0x0F);
00502     return count;
00503 }
00504 
00505 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros(uint8_t value)
00506 {
00507     return common_count_leading_zeros_8(value);
00508 }
00509 
00510 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros_8(uint8_t value)
00511 {
00512 #ifdef  __CC_ARM
00513     return value ? __clz((unsigned int) value << 24) : 8;
00514 #elif defined __GNUC__
00515     return value ? __builtin_clz((unsigned int) value << 24) : 8;
00516 #else
00517     uint_fast8_t cnt = 0;
00518     if (value == 0) {
00519         return 8;
00520     }
00521     if ((value & 0xF0) == 0) {
00522         value <<= 4;
00523         cnt += 4;
00524     }
00525     if ((value & 0xC0) == 0) {
00526         value <<= 2;
00527         cnt += 2;
00528     }
00529     if ((value & 0x80) == 0) {
00530         cnt += 1;
00531     }
00532 
00533     return cnt;
00534 #endif
00535 }
00536 
00537 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros_16(uint16_t value)
00538 {
00539 #ifdef  __CC_ARM
00540     return value ? __clz((unsigned int) value << 16) : 16;
00541 #elif defined __GNUC__
00542     return value ? __builtin_clz((unsigned int) value << 16) : 16;
00543 #else
00544     uint_fast8_t cnt = 0;
00545     if (value == 0) {
00546         return 16;
00547     }
00548     if ((value & 0xFF00) == 0) {
00549         value <<= 8;
00550         cnt += 8;
00551     }
00552     if ((value & 0xF000) == 0) {
00553         value <<= 4;
00554         cnt += 4;
00555     }
00556     if ((value & 0xC000) == 0) {
00557         value <<= 2;
00558         cnt += 2;
00559     }
00560     if ((value & 0x8000) == 0) {
00561         cnt += 1;
00562     }
00563 
00564     return cnt;
00565 #endif
00566 }
00567 
00568 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros_32(uint32_t value)
00569 {
00570 #ifdef  __CC_ARM
00571     return __clz(value);
00572 #elif defined __GNUC__
00573     return value ? __builtin_clz(value) : 32;
00574 #else
00575     uint_fast8_t cnt = 0;
00576     if (value == 0) {
00577         return 32;
00578     }
00579     if ((value & 0xFFFF0000) == 0) {
00580         value <<= 16;
00581         cnt += 16;
00582     }
00583     if ((value & 0xFF000000) == 0) {
00584         value <<= 8;
00585         cnt += 8;
00586     }
00587     if ((value & 0xF0000000) == 0) {
00588         value <<= 4;
00589         cnt += 4;
00590     }
00591     if ((value & 0xC0000000) == 0) {
00592         value <<= 2;
00593         cnt += 2;
00594     }
00595     if ((value & 0x80000000) == 0) {
00596         cnt += 1;
00597     }
00598 
00599     return cnt;
00600 #endif
00601 }
00602 
00603 COMMON_FUNCTIONS_FN bool common_serial_number_greater_8(uint8_t s1, uint8_t s2)
00604 {
00605     return (s1 > s2 && s1 - s2 < UINT8_C(0x80)) || (s1 < s2 && s2 - s1 > UINT8_C(0x80));
00606 }
00607 
00608 COMMON_FUNCTIONS_FN bool common_serial_number_greater_16(uint16_t s1, uint16_t s2)
00609 {
00610     return (s1 > s2 && s1 - s2 < UINT16_C(0x8000)) || (s1 < s2 && s2 - s1 > UINT16_C(0x8000));
00611 }
00612 
00613 COMMON_FUNCTIONS_FN bool common_serial_number_greater_32(uint32_t s1, uint32_t s2)
00614 {
00615     return (s1 > s2 && s1 - s2 < UINT32_C(0x80000000)) || (s1 < s2 && s2 - s1 > UINT32_C(0x80000000));
00616 }
00617 
00618 COMMON_FUNCTIONS_FN bool bit_test(const uint8_t *bitset, uint_fast8_t bit)
00619 {
00620     return bitset[bit >> 3] & (0x80 >> (bit & 7));
00621 }
00622 
00623 COMMON_FUNCTIONS_FN void bit_set(uint8_t *bitset, uint_fast8_t bit)
00624 {
00625     bitset[bit >> 3] |= (0x80 >> (bit & 7));
00626 }
00627 
00628 COMMON_FUNCTIONS_FN void bit_clear(uint8_t *bitset, uint_fast8_t bit)
00629 {
00630     bitset[bit >> 3] &= ~(0x80 >> (bit & 7));
00631 }
00632 
00633 #endif /* defined NS_ALLOW_INLINING || defined COMMON_FUNCTIONS_FN */
00634 
00635 #ifdef __cplusplus
00636 }
00637 #endif
00638 #endif /*__COMMON_FUNCTIONS_H_*/