Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of OmniWheels by
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 16-bit variable to 8-bit pointer. 00119 * 00120 * Write 16 bits in big-endian (network) byte order. 00121 * 00122 * \param value 16-bit variable 00123 * \param ptr pointer where data to be written 00124 * 00125 * \return updated pointer 00126 */ 00127 NS_INLINE uint8_t *common_write_16_bit(uint16_t value, uint8_t ptr[__static 2]); 00128 00129 /* 00130 * Common read 16-bit variable from 8-bit pointer. 00131 * 00132 * Read 16 bits in big-endian (network) byte order. 00133 * 00134 * \param data_buf pointer where data to be read 00135 * 00136 * \return 16-bit variable 00137 */ 00138 NS_INLINE uint16_t common_read_16_bit(const uint8_t data_buf[__static 2]); 00139 00140 /* 00141 * Common write 16-bit variable to 8-bit pointer. 00142 * 00143 * Write 16 bits in little-endian 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_inverse(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 little-endian 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_inverse(const uint8_t data_buf[__static 2]); 00162 00163 /* 00164 * Count bits in a byte 00165 * 00166 * \param byte byte to inspect 00167 * 00168 * \return number of 1-bits in byte 00169 */ 00170 NS_INLINE uint_fast8_t common_count_bits(uint8_t byte); 00171 00172 /* 00173 * Count leading zeros in a byte 00174 * 00175 * \deprecated Use common_count_leading_zeros_8 00176 * 00177 * \param byte byte to inspect 00178 * 00179 * \return number of leading zeros in byte (0-8) 00180 */ 00181 NS_INLINE uint_fast8_t common_count_leading_zeros(uint8_t byte); 00182 00183 /* 00184 * Count leading zeros in a byte 00185 * 00186 * \param byte byte to inspect 00187 * 00188 * \return number of leading zeros in byte (0-8) 00189 */ 00190 NS_INLINE uint_fast8_t common_count_leading_zeros_8(uint8_t byte); 00191 00192 /* 00193 * Count leading zeros in a 16-bit value 00194 * 00195 * \param value value to inspect 00196 * 00197 * \return number of leading zeros in byte (0-16) 00198 */ 00199 NS_INLINE uint_fast8_t common_count_leading_zeros_16(uint16_t value); 00200 00201 /* 00202 * Count leading zeros in a 32-bit value 00203 * 00204 * \param value value to inspect 00205 * 00206 * \return number of leading zeros in byte (0-32) 00207 */ 00208 NS_INLINE uint_fast8_t common_count_leading_zeros_32(uint32_t value); 00209 00210 /* 00211 * Compare 8-bit serial numbers 00212 * 00213 * Compare two 8-bit serial numbers, according to RFC 1982 Serial Number 00214 * Arithmetic. 00215 * 00216 * \param s1 first serial number 00217 * \param s2 second serial number 00218 * 00219 * \return true if s1 > s2 00220 * \return false if s1 <= s2, or the comparison is undefined 00221 */ 00222 NS_INLINE bool common_serial_number_greater_8(uint8_t s1, uint8_t s2); 00223 00224 /* 00225 * Compare 16-bit serial numbers 00226 * 00227 * Compare two 16-bit serial numbers, according to RFC 1982 Serial Number 00228 * Arithmetic. 00229 * 00230 * \param s1 first serial number 00231 * \param s2 second serial number 00232 * 00233 * \return true if s1 > s2 00234 * \return false if s1 <= s2, or the comparison is undefined 00235 */ 00236 NS_INLINE bool common_serial_number_greater_16(uint16_t s1, uint16_t s2); 00237 00238 /* 00239 * Compare 32-bit serial numbers 00240 * 00241 * Compare two 32-bit serial numbers, according to RFC 1982 Serial Number 00242 * Arithmetic. 00243 * 00244 * \param s1 first serial number 00245 * \param s2 second serial number 00246 * 00247 * \return true if s1 > s2 00248 * \return false if s1 <= s2, or the comparison is undefined 00249 */ 00250 NS_INLINE bool common_serial_number_greater_32(uint32_t s1, uint32_t s2); 00251 00252 /* 00253 * Test a bit in an bit array. 00254 * 00255 * Check whether a particular bit is set in a bit string. The bit array 00256 * is in big-endian (network) bit order. 00257 * 00258 * \param bitset pointer to bit array 00259 * \param bit index of bit - 0 is the most significant bit of the first byte 00260 * 00261 * \return true if the bit is set 00262 */ 00263 NS_INLINE bool bit_test(const uint8_t *bitset, uint_fast8_t bit); 00264 00265 /* 00266 * Set a bit in an bit array. 00267 * 00268 * Set a bit in a bit array. The array is in big-endian (network) bit order. 00269 * 00270 * \param bitset pointer to bit array 00271 * \param bit index of bit - 0 is the most significant bit of the first byte 00272 */ 00273 NS_INLINE void bit_set(uint8_t *bitset, uint_fast8_t bit); 00274 00275 /* 00276 * Clear a bit in an bit array. 00277 * 00278 * Clear a bit in a bit array. The bit array is in big-endian (network) bit order. 00279 * 00280 * \param bitset pointer to bit array 00281 * \param bit index of bit - 0 is the most significant bit of the first byte 00282 */ 00283 NS_INLINE void bit_clear(uint8_t *bitset, uint_fast8_t bit); 00284 00285 /* 00286 * Compare two bitstrings. 00287 * 00288 * Compare two bitstrings of specified length. The bit strings are in 00289 * big-endian (network) bit order. 00290 * 00291 * \param a pointer to first string 00292 * \param b pointer to second string 00293 * \param bits number of bits to compare 00294 * 00295 * \return true if the strings compare equal 00296 */ 00297 bool bitsequal(const uint8_t *a, const uint8_t *b, uint_fast8_t bits); 00298 00299 /* 00300 * Copy a bitstring 00301 * 00302 * Copy a bitstring of specified length. The bit string is in big-endian 00303 * (network) bit order. Bits beyond the bitlength at the destination are not 00304 * modified. 00305 * 00306 * For example, copying 4 bits sets the first 4 bits of dst[0] from src[0], 00307 * the lower 4 bits of dst[0] are unmodified. 00308 * 00309 * \param dst destination pointer 00310 * \param src source pointer 00311 * \param bits number of bits to copy 00312 * 00313 * \return the value of dst 00314 */ 00315 uint8_t *bitcopy(uint8_t *restrict dst, const uint8_t *restrict src, uint_fast8_t bits); 00316 00317 /* 00318 * Copy a bitstring and pad last byte with zeros 00319 * 00320 * Copy a bitstring of specified length. The bit string is in big-endian 00321 * (network) bit order. Bits beyond the bitlength in the last destination byte are 00322 * zeroed. 00323 * 00324 * For example, copying 4 bits sets the first 4 bits of dst[0] from src[0], and 00325 * the lower 4 bits of dst[0] are set to 0. 00326 * 00327 * \param dst destination pointer 00328 * \param src source pointer 00329 * \param bits number of bits to copy 00330 * 00331 * \return the value of dst 00332 */ 00333 uint8_t *bitcopy0(uint8_t *restrict dst, const uint8_t *restrict src, uint_fast8_t bits); 00334 00335 /* Provide definitions, either for inlining, or for common_functions.c */ 00336 #if defined NS_ALLOW_INLINING || defined COMMON_FUNCTIONS_FN 00337 #ifndef COMMON_FUNCTIONS_FN 00338 #define COMMON_FUNCTIONS_FN NS_INLINE 00339 #endif 00340 00341 COMMON_FUNCTIONS_FN uint8_t *common_write_64_bit(uint64_t value, uint8_t ptr[__static 8]) 00342 { 00343 *ptr++ = value >> 56; 00344 *ptr++ = value >> 48; 00345 *ptr++ = value >> 40; 00346 *ptr++ = value >> 32; 00347 *ptr++ = value >> 24; 00348 *ptr++ = value >> 16; 00349 *ptr++ = value >> 8; 00350 *ptr++ = value; 00351 return ptr; 00352 } 00353 00354 COMMON_FUNCTIONS_FN uint64_t common_read_64_bit(const uint8_t data_buf[__static 8]) 00355 { 00356 uint64_t temp_64; 00357 temp_64 = (uint64_t)(*data_buf++) << 56; 00358 temp_64 += (uint64_t)(*data_buf++) << 48; 00359 temp_64 += (uint64_t)(*data_buf++) << 40; 00360 temp_64 += (uint64_t)(*data_buf++) << 32; 00361 temp_64 += (uint64_t)(*data_buf++) << 24; 00362 temp_64 += (uint64_t)(*data_buf++) << 16; 00363 temp_64 += (uint64_t)(*data_buf++) << 8; 00364 temp_64 += *data_buf++; 00365 return temp_64; 00366 } 00367 00368 COMMON_FUNCTIONS_FN uint8_t *common_write_32_bit(uint32_t value, uint8_t ptr[__static 4]) 00369 { 00370 *ptr++ = value >> 24; 00371 *ptr++ = value >> 16; 00372 *ptr++ = value >> 8; 00373 *ptr++ = value; 00374 return ptr; 00375 } 00376 00377 COMMON_FUNCTIONS_FN uint32_t common_read_32_bit(const uint8_t data_buf[__static 4]) 00378 { 00379 uint32_t temp_32; 00380 temp_32 = (uint32_t)(*data_buf++) << 24; 00381 temp_32 += (uint32_t)(*data_buf++) << 16; 00382 temp_32 += (uint32_t)(*data_buf++) << 8; 00383 temp_32 += *data_buf++; 00384 return temp_32; 00385 } 00386 00387 COMMON_FUNCTIONS_FN uint8_t *common_write_32_bit_inverse(uint32_t value, uint8_t ptr[__static 4]) 00388 { 00389 *ptr++ = value; 00390 *ptr++ = value >> 8; 00391 *ptr++ = value >> 16; 00392 *ptr++ = value >> 24; 00393 return ptr; 00394 } 00395 00396 COMMON_FUNCTIONS_FN uint32_t common_read_32_bit_inverse(const uint8_t data_buf[__static 4]) 00397 { 00398 uint32_t temp_32; 00399 temp_32 = *data_buf++; 00400 temp_32 += (uint32_t)(*data_buf++) << 8; 00401 temp_32 += (uint32_t)(*data_buf++) << 16; 00402 temp_32 += (uint32_t)(*data_buf++) << 24; 00403 return temp_32; 00404 } 00405 00406 COMMON_FUNCTIONS_FN uint8_t *common_write_24_bit(uint_fast24_t value, uint8_t ptr[__static 3]) 00407 { 00408 *ptr++ = value >> 16; 00409 *ptr++ = value >> 8; 00410 *ptr++ = value; 00411 return ptr; 00412 } 00413 00414 COMMON_FUNCTIONS_FN uint_fast24_t common_read_24_bit(const uint8_t data_buf[__static 3]) 00415 { 00416 uint_fast24_t temp_24; 00417 temp_24 = (uint_fast24_t)(*data_buf++) << 16; 00418 temp_24 += (uint_fast24_t)(*data_buf++) << 8; 00419 temp_24 += *data_buf++; 00420 return temp_24; 00421 } 00422 00423 COMMON_FUNCTIONS_FN uint8_t *common_write_16_bit(uint16_t value, uint8_t ptr[__static 2]) 00424 { 00425 *ptr++ = value >> 8; 00426 *ptr++ = value; 00427 return ptr; 00428 } 00429 00430 COMMON_FUNCTIONS_FN uint16_t common_read_16_bit(const uint8_t data_buf[__static 2]) 00431 { 00432 uint16_t temp_16; 00433 temp_16 = (uint16_t)(*data_buf++) << 8; 00434 temp_16 += *data_buf++; 00435 return temp_16; 00436 } 00437 00438 COMMON_FUNCTIONS_FN uint8_t *common_write_16_bit_inverse(uint16_t value, uint8_t ptr[__static 2]) 00439 { 00440 *ptr++ = value; 00441 *ptr++ = value >> 8; 00442 return ptr; 00443 } 00444 00445 COMMON_FUNCTIONS_FN uint16_t common_read_16_bit_inverse(const uint8_t data_buf[__static 2]) 00446 { 00447 uint16_t temp_16; 00448 temp_16 = *data_buf++; 00449 temp_16 += (uint16_t)(*data_buf++) << 8; 00450 return temp_16; 00451 } 00452 00453 COMMON_FUNCTIONS_FN uint_fast8_t common_count_bits(uint8_t byte) 00454 { 00455 /* First step sets each bit pair to be count of bits (00,01,10) */ 00456 /* [00-00 = 00, 01-00 = 01, 10-01 = 01, 11-01 = 10] */ 00457 uint_fast8_t count = byte - ((byte >> 1) & 0x55); 00458 /* Add bit pairs to make each nibble contain count of bits (0-4) */ 00459 count = (count & 0x33) + ((count >> 2) & 0x33); 00460 /* Final result is sum of nibbles (0-8) */ 00461 count = (count >> 4) + (count & 0x0F); 00462 return count; 00463 } 00464 00465 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros(uint8_t byte) 00466 { 00467 return common_count_leading_zeros_8(byte); 00468 } 00469 00470 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros_8(uint8_t byte) 00471 { 00472 #ifdef __CC_ARM 00473 return byte ? __clz((unsigned int) byte << 24) : 8; 00474 #elif defined __GNUC__ 00475 return byte ? __builtin_clz((unsigned int) byte << 24) : 8; 00476 #else 00477 uint_fast8_t cnt = 0; 00478 if (byte == 0) { 00479 return 8; 00480 } 00481 if ((byte & 0xF0) == 0) { 00482 byte <<= 4; 00483 cnt += 4; 00484 } 00485 if ((byte & 0xC0) == 0) { 00486 byte <<= 2; 00487 cnt += 2; 00488 } 00489 if ((byte & 0x80) == 0) { 00490 cnt += 1; 00491 } 00492 00493 return cnt; 00494 #endif 00495 } 00496 00497 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros_16(uint16_t value) 00498 { 00499 #ifdef __CC_ARM 00500 return value ? __clz((unsigned int) value << 16) : 16; 00501 #elif defined __GNUC__ 00502 return value ? __builtin_clz((unsigned int) value << 16) : 16; 00503 #else 00504 uint_fast8_t cnt = 0; 00505 if (value == 0) { 00506 return 16; 00507 } 00508 if ((value & 0xFF00) == 0) { 00509 value <<= 8; 00510 cnt += 8; 00511 } 00512 if ((value & 0xF000) == 0) { 00513 value <<= 4; 00514 cnt += 4; 00515 } 00516 if ((value & 0xC000) == 0) { 00517 value <<= 2; 00518 cnt += 2; 00519 } 00520 if ((value & 0x8000) == 0) { 00521 cnt += 1; 00522 } 00523 00524 return cnt; 00525 #endif 00526 } 00527 00528 COMMON_FUNCTIONS_FN uint_fast8_t common_count_leading_zeros_32(uint32_t value) 00529 { 00530 #ifdef __CC_ARM 00531 return __clz(value); 00532 #elif defined __GNUC__ 00533 return value ? __builtin_clz(value) : 32; 00534 #else 00535 uint_fast8_t cnt = 0; 00536 if (value == 0) { 00537 return 32; 00538 } 00539 if ((value & 0xFFFF0000) == 0) { 00540 value <<= 16; 00541 cnt += 16; 00542 } 00543 if ((value & 0xFF000000) == 0) { 00544 value <<= 8; 00545 cnt += 8; 00546 } 00547 if ((value & 0xF0000000) == 0) { 00548 value <<= 4; 00549 cnt += 4; 00550 } 00551 if ((value & 0xC0000000) == 0) { 00552 value <<= 2; 00553 cnt += 2; 00554 } 00555 if ((value & 0x80000000) == 0) { 00556 cnt += 1; 00557 } 00558 00559 return cnt; 00560 #endif 00561 } 00562 00563 COMMON_FUNCTIONS_FN bool common_serial_number_greater_8(uint8_t s1, uint8_t s2) 00564 { 00565 return (s1 > s2 && s1 - s2 < UINT8_C(0x80)) || (s1 < s2 && s2 - s1 > UINT8_C(0x80)); 00566 } 00567 00568 COMMON_FUNCTIONS_FN bool common_serial_number_greater_16(uint16_t s1, uint16_t s2) 00569 { 00570 return (s1 > s2 && s1 - s2 < UINT16_C(0x8000)) || (s1 < s2 && s2 - s1 > UINT16_C(0x8000)); 00571 } 00572 00573 COMMON_FUNCTIONS_FN bool common_serial_number_greater_32(uint32_t s1, uint32_t s2) 00574 { 00575 return (s1 > s2 && s1 - s2 < UINT32_C(0x80000000)) || (s1 < s2 && s2 - s1 > UINT32_C(0x80000000)); 00576 } 00577 00578 COMMON_FUNCTIONS_FN bool bit_test(const uint8_t *bitset, uint_fast8_t bit) 00579 { 00580 return bitset[bit >> 3] & (0x80 >> (bit & 7)); 00581 } 00582 00583 COMMON_FUNCTIONS_FN void bit_set(uint8_t *bitset, uint_fast8_t bit) 00584 { 00585 bitset[bit >> 3] |= (0x80 >> (bit & 7)); 00586 } 00587 00588 COMMON_FUNCTIONS_FN void bit_clear(uint8_t *bitset, uint_fast8_t bit) 00589 { 00590 bitset[bit >> 3] &= ~(0x80 >> (bit & 7)); 00591 } 00592 00593 #endif /* defined NS_ALLOW_INLINING || defined COMMON_FUNCTIONS_FN */ 00594 00595 #ifdef __cplusplus 00596 } 00597 #endif 00598 #endif /*__COMMON_FUNCTIONS_H_*/
Generated on Fri Jul 22 2022 04:53:46 by
