sys_utils.h 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541
  1. /**
  2. * Copyright (c) 2016 - 2020 Nordic Semiconductor ASA and Luxoft Global Operations Gmbh.
  3. *
  4. * All Rights Reserved.
  5. *
  6. * Redistribution and use in source and binary forms, with or without modification,
  7. * are permitted provided that the following conditions are met:
  8. *
  9. *
  10. * 1. Redistributions of source code must retain the above copyright notice, this
  11. * list of conditions and the following disclaimer.
  12. *
  13. * 2. Redistributions in binary form, except as embedded into a Nordic
  14. * Semiconductor ASA integrated circuit in a product or a software update for
  15. * such product, must reproduce the above copyright notice, this list of
  16. * conditions and the following disclaimer in the documentation and/or other
  17. * materials provided with the distribution.
  18. *
  19. * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
  20. * contributors may be used to endorse or promote products derived from this
  21. * software without specific prior written permission.
  22. *
  23. * 4. This software, with or without modification, must only be used with a
  24. * Nordic Semiconductor ASA integrated circuit.
  25. *
  26. * 5. Any software provided in binary form under this license must not be reverse
  27. * engineered, decompiled, modified and/or disassembled.
  28. *
  29. *
  30. * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
  31. * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  32. * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
  33. * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
  34. * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  35. * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
  36. * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  37. * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  38. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
  39. * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  40. *
  41. */
  42. #ifndef SYS_UTILS_H_INCLUDED
  43. #define SYS_UTILS_H_INCLUDED
  44. #include <stdint.h>
  45. #include <stddef.h>
  46. #include <string.h>
  47. #if (defined(__GNUC__) && !defined(__SES_ARM))
  48. #include <strings.h>
  49. #endif
  50. #ifdef __MINGW32__
  51. #define ffs __builtin_ffs
  52. #endif
  53. /** @file
  54. * This file contains definitions of useful macros and types.
  55. *
  56. * @defgroup sys_utils System Utilities API
  57. * @ingroup sys_15_4
  58. * @{
  59. * @brief Module to declare System Utilities API.
  60. * @details The System Utilities module implements multiple useful macros and inlines for the whole stack. Including
  61. * this header you will get access to GET_PARENT_BY_FIELD(), FIELD_SIZE() to work with complex structures,
  62. * ARRAY_SIZE() for arrays, mathematics macros like IMP(), LL_MIN(), LL_MAX(), CEIL(), ROUND(), Bitmap helpers
  63. * and many others. The variable arguments support macros are also defined here. Some SWAP routines are implemented
  64. * by this module as well.
  65. */
  66. /**@brief Returns the pointer to the data structure
  67. *
  68. * @param[in] struct_type name of the parent structure
  69. * @param[in] field_name name of the structure field
  70. * @param[in] field_pointer pointer to the structure field
  71. *
  72. * @retval Pointer to the parent structure which includes the field.
  73. */
  74. #define GET_PARENT_BY_FIELD(struct_type, field_name, field_pointer) \
  75. ((struct_type*)(void*)(((uint8_t*)field_pointer) - offsetof(struct_type, field_name)))
  76. /**@brief Returns the implication of two given expressions x and y.
  77. * @details The implication means: if X==TRUE then Y==TRUE.
  78. * The formula is: (X imp Y) = ((not X) or Y)
  79. */
  80. #define IMP(x, y) ( !(x) || (y) )
  81. /**@brief Returns the minimum of two given expressions x and y.
  82. */
  83. #define LL_MIN(x, y) ( ((x) < (y)) ? (x) : (y) )
  84. /**@brief Returns the maximum of two given expressions x and y.
  85. */
  86. #define LL_MAX(x, y) ( ((x) > (y)) ? (x) : (y) )
  87. /**@brief Returns the quotient of a divided by b rounded upwards to the nearest
  88. * integer.
  89. */
  90. #define CEIL(a, b) ((a) ? (((a) - 1U) / (b) + 1U) : 0U)
  91. /**@brief Returns the quotient of a divided by b rounded to the nearest integer
  92. * according to the standard arithmetic rules: if the fractional part of (a/b) is greater
  93. * or equal to 0.5 then the result is rounded upwards; if the fractional part of (a/b) is
  94. * less then 0.5 the result is rounded downwards.
  95. *
  96. * @note Use this formula only for unsigned arguments. The formula is not compatible with
  97. * the signed arguments: when a and b have different signs it gives incorrect result.
  98. */
  99. #define ROUND(a, b) ( ((a) + ((b) >> 1)) / (b) )
  100. /**@brief Declares a long bitmap named name of size bits. The size is rounded
  101. * upwards to come a multiple of 8.
  102. */
  103. #define BITMAP_DECLARE(name, size) uint8_t name[CEIL(size, 8)]
  104. /**@brief Clears all bits in given bitmap.
  105. */
  106. #define BITMAP_RESET(name) memset((name), 0U, sizeof(name))
  107. /**@brief Returns the value of a bit at position bit in the long bitmap named name.
  108. */
  109. #define BITMAP_ISSET(name, bit) ( 0 != ((name)[(bit) >> 3] & (1 << ((bit) & 0x7))) )
  110. /**@brief Sets the bit at position bit in the long bitmap named name.
  111. */
  112. #define BITMAP_SET(name, bit) (name)[(bit) >> 3] |= (1 << ((bit) & 0x7))
  113. /**@brief Clears the bit at position bit in the long bitmap named name.
  114. */
  115. #define BITMAP_CLR(name, bit) (name)[(bit) >> 3] &= ~(1 << ((bit) & 0x7))
  116. /**@brief Assigns the given bitmap with the second bitmap.
  117. */
  118. #define BITMAP_ASSIGN(nameDst, nameSrc) memcpy((nameDst), (nameSrc), sizeof(nameDst))
  119. /**@brief Compares two bitmaps and returns zero if they are equal.
  120. */
  121. #define BITMAP_EQUAL(name1, name2) ((sizeof(name1) == sizeof(name2)) && \
  122. (memcmp((name1), (name2), sizeof(name1)) == 0))
  123. /**@brief Checks number. Return true if number is power of two.
  124. */
  125. #define LL_IS_POWER_OF_TWO(name) ((0 != (name)) && (0 == ((name)&(name - 1))))
  126. /**@brief Return True if mask is fully included into a given set and False otherwise
  127. */
  128. #define IS_SUBSET_OF(mask, set) ((mask) == ((set) & (mask)))
  129. /**@brief Creates a bit mask with single set bit on the specified position.
  130. */
  131. #define BIT(pos) (1UL << (pos))
  132. /**@brief Gets the given bit in the given value
  133. */
  134. #define BIT_GET(val, pos) ((((uint32_t)val) & BIT(pos)) != 0)
  135. /**@brief Sets or clears the given bit in the given value
  136. */
  137. #define BIT_SET(val, pos, bit) { \
  138. if (bit) \
  139. { \
  140. val |= BIT(pos); \
  141. } \
  142. else \
  143. { \
  144. val &= ~BIT(pos); \
  145. } \
  146. }
  147. /**@brief Returns two to the income power.*/
  148. #define POWER2(n) (1ULL << (n))
  149. /**@brief Creates a bit mask of specified length.
  150. */
  151. #define BIT_MASK(len) (BIT(len) - 1UL)
  152. /**@brief Creates a bit field mask of specified length and start position.
  153. */
  154. #define BIT_FIELD_MASK(start, len) (BIT_MASK(len) << (start))
  155. /**@brief Creates a bit field mask of specified length, start position and value.
  156. */
  157. #define BIT_FIELD_VALUE(value, start, len) (((value) & BIT_MASK(len)) << (start))
  158. /**@brief Extracts a bit field value of specified start position and length.
  159. */
  160. #define GET_BITFIELD_VALUE(bitmask, start, len) (((bitmask) >> (start)) & BIT_MASK(len))
  161. /**@brief Inserts a bit field value with specified start position and length.
  162. */
  163. #define SET_BITFIELD_VALUE(bitmask, start, len, value) \
  164. (bitmask = (bitmask & ~BIT_FIELD_MASK(start, len)) | BIT_FIELD_VALUE(value, start, len))
  165. /**@brief Extracts a mask from a BITMAP.
  166. * BITMAP MUST be aligned and mask length MUST be one of 2, 4, 8, 16, 32.
  167. */
  168. #define BITMAP_MASK_GET(bitmap, bit, len) \
  169. GET_BITFIELD_VALUE(((uint32_t*)(bitmap))[(bit) >> 5], (bit) & 0x1F, len)
  170. /**@brief Sets up a mask to a BITMAP.
  171. * BITMAP MUST be aligned and mask length MUST be one of 2, 4, 8, 16, 32.
  172. */
  173. #define BITMAP_MASK_SET(bitmap, bit, len, value) \
  174. SET_BITFIELD_VALUE(((uint32_t*)(bitmap))[(bit) >> 5], (bit) & 0x1F, len, value)
  175. /**@brief Gets amount of the arguments.
  176. */
  177. #define VA_NARGS(...) VA_NARGS_EVAL(__VA_ARGS__)
  178. #define VA_NARGS_EVAL(...) VA_NARGS_IMPL(__VA_ARGS__, \
  179. /* 255, 254, */ 253, 252, 251, 250, 249, 248, 247, 246, 245, 244, 243, 242, 241, 240, \
  180. 239, 238, 237, 236, 235, 234, 233, 232, 231, 230, 229, 228, 227, 226, 225, 224, \
  181. 223, 222, 221, 220, 219, 218, 217, 216, 215, 214, 213, 212, 211, 210, 209, 208, \
  182. 207, 206, 205, 204, 203, 202, 201, 200, 199, 198, 197, 196, 195, 194, 193, 192, \
  183. 191, 190, 189, 188, 187, 186, 185, 184, 183, 182, 181, 180, 179, 178, 177, 176, \
  184. 175, 174, 173, 172, 171, 170, 169, 168, 167, 166, 165, 164, 163, 162, 161, 160, \
  185. 159, 158, 157, 156, 155, 154, 153, 152, 151, 150, 149, 148, 147, 146, 145, 144, \
  186. 143, 142, 141, 140, 139, 138, 137, 136, 135, 134, 133, 132, 131, 130, 129, 128, \
  187. 127, 126, 125, 124, 123, 122, 121, 120, 119, 118, 117, 116, 115, 114, 113, 112, \
  188. 111, 110, 109, 108, 107, 106, 105, 104, 103, 102, 101, 100, 99, 98, 97, 96, \
  189. 095, 94, 93, 92, 91, 90, 89, 88, 87, 86, 85, 84, 83, 82, 81, 80, \
  190. 079, 78, 77, 76, 75, 74, 73, 72, 71, 70, 69, 68, 67, 66, 65, 64, \
  191. 063, 62, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52, 51, 50, 49, 48, \
  192. 047, 46, 45, 44, 43, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33, 32, \
  193. 031, 30, 29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, \
  194. 015, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0)
  195. /**@brief Helper macro. Gets amount of the arguments.
  196. */
  197. #define VA_NARGS_IMPL(_________1, _2, _3, _4, _5, _6, _7, \
  198. _8, _9, _10, _11, _12, _13, _14, _15, \
  199. __16, _17, _18, _19, _20, _21, _22, _23, \
  200. _24, _25, _26, _27, _28, _29, _30, _31, \
  201. __32, _33, _34, _35, _36, _37, _38, _39, \
  202. _40, _41, _42, _43, _44, _45, _46, _47, \
  203. __48, _49, _50, _51, _52, _53, _54, _55, \
  204. _56, _57, _58, _59, _60, _61, _62, _63, \
  205. __64, _65, _66, _67, _68, _69, _70, _71, \
  206. _72, _73, _74, _75, _76, _77, _78, _79, \
  207. __80, _81, _82, _83, _84, _85, _86, _87, \
  208. _88, _89, _90, _91, _92, _93, _94, _95, \
  209. __96, _97, _98, _99, _100, _101, _102, _103, \
  210. _104, _105, _106, _107, _108, _109, _110, _111, \
  211. _112, _113, _114, _115, _116, _117, _118, _119, \
  212. _120, _121, _122, _123, _124, _125, _126, _127, \
  213. _128, _129, _130, _131, _132, _133, _134, _135, \
  214. _136, _137, _138, _139, _140, _141, _142, _143, \
  215. _144, _145, _146, _147, _148, _149, _150, _151, \
  216. _152, _153, _154, _155, _156, _157, _158, _159, \
  217. _160, _161, _162, _163, _164, _165, _166, _167, \
  218. _168, _169, _170, _171, _172, _173, _174, _175, \
  219. _176, _177, _178, _179, _180, _181, _182, _183, \
  220. _184, _185, _186, _187, _188, _189, _190, _191, \
  221. _192, _193, _194, _195, _196, _197, _198, _199, \
  222. _200, _201, _202, _203, _204, _205, _206, _207, \
  223. _208, _209, _210, _211, _212, _213, _214, _215, \
  224. _216, _217, _218, _219, _220, _221, _222, _223, \
  225. _224, _225, _226, _227, _228, _229, _230, _231, \
  226. _232, _233, _234, _235, _236, _237, _238, _239, \
  227. _240, _241, _242, _243, _244, _245, _246, _247, \
  228. _248, _249, _250, _251, _252, _253, /* _254, _255, */\
  229. N, ...) N
  230. /**@brief Gets amount of the arguments. Execute by compiler.
  231. */
  232. #define VA_NARGS_COMPILE_TIME(...) ((uint8_t)(sizeof((uint8_t[]){ __VA_ARGS__ })/sizeof(uint8_t)))
  233. /**@brief Swaps values.
  234. */
  235. #define SWAP_XOR(a, b) \
  236. do \
  237. { \
  238. (((b) ^= (a) ^= (b), (a) ^= (b))); \
  239. } while(0);
  240. /**@brief Compare two number and take care of overflow threshold limit.
  241. */
  242. #define COMPARE_WITH_THRESHOLD(a, b, threshold) \
  243. (((LL_MAX((a), (b)) - LL_MIN((a), (b))) < (threshold)) ? ((a) >= (b) ? 1 : 0) : ((a) > (b) ? 0 : 1))
  244. #define ROUND_MASK(a) ((a) - 1)
  245. #define ROUND_UP(x, a) (((x) + ROUND_MASK(a)) & ~ROUND_MASK(a))
  246. #define ROUND_DOWN(x, a) ((x) & ~ROUND_MASK(a))
  247. /**@brief Dereferences input pointer \a y as a type \a x.
  248. *
  249. * @param[in] x type name.
  250. * @param[in] y pointer name.
  251. */
  252. #define DEREF_VOID_PTR_AS(x, y) (*(x *)y)
  253. /**@brief Extends some bit value to the left extending 2's complement value
  254. * to 8-bit length.
  255. *
  256. * @param[out] result variable, where result is store to.
  257. * @param[in] x input value.
  258. * @param[in] sign_pos an integer in range 2..6 specifying bit position of sign bit.
  259. */
  260. #define SIGN_EXTENSION(result, x, sign_pos) \
  261. do \
  262. { \
  263. result = x & (1 << sign_pos) ? \
  264. x | (~((1 << (sign_pos + 1)) - 1)) : \
  265. x & ((1 << (sign_pos + 1)) - 1); \
  266. } while (0)
  267. /**@brief Clears some most significant bits of integer value reducing it precision.
  268. * Name and interface of the macro emphasizes complementary action to #SIGN_EXTENSION.
  269. *
  270. * @param[out] result variable, where result is store to.
  271. * @param[in] x input value.
  272. * @param[in] sign_pos an integer in range 2..6 specifying bit position of sign bit.
  273. */
  274. #define SIGN_COMPRESSION(result, x, sign_pos) \
  275. do \
  276. { \
  277. result = x & ((1 << (sign_pos + 1)) - 1); \
  278. } while (0)
  279. /************************* PROTOTYPES **************************************************/
  280. /**@brief Swaps values of two bytes.
  281. *
  282. */
  283. static inline void SWAP8(uint8_t * const x, uint8_t * const y)
  284. {
  285. uint8_t _x = *x;
  286. *x = *y;
  287. *y = _x;
  288. }
  289. /**@brief Swaps values of two double words (DWORD).
  290. *
  291. */
  292. static inline void SWAP32(uint32_t * const x, uint32_t * const y)
  293. {
  294. uint32_t _x = *x;
  295. *x = *y;
  296. *y = _x;
  297. }
  298. /**@brief Swaps values of two arrays.
  299. *
  300. * @param[inout] x array pointer
  301. * @param[inout] y array pointer
  302. * @param[in] length amount of bytes to swap
  303. */
  304. static inline void SWAP_ARRAYS(void * x, void * y, uint32_t length)
  305. {
  306. uint8_t *_x = (uint8_t *)(void *)x;
  307. uint8_t *_y = (uint8_t *)(void *)y;
  308. if (0x0 == ((((size_t)_x) | ((size_t)_y)) & 0x3))
  309. {
  310. size_t len4 = length / sizeof(uint32_t);
  311. for (size_t i = 0; i < len4; i++)
  312. {
  313. SWAP32((uint32_t*)_x, (uint32_t*)_y);
  314. _x += sizeof(uint32_t);
  315. _y += sizeof(uint32_t);
  316. }
  317. length &= 0x3;
  318. }
  319. for (size_t i = 0; i < length; i++)
  320. {
  321. SWAP8(_x, _y);
  322. _x++;
  323. _y++;
  324. }
  325. }
  326. /**@brief Find the first bit of the bitmap with the given value
  327. * (one or zero, as specified).
  328. *
  329. * @param[in] p_bitmap Pointer to bitmap.
  330. * @param[in] bitmap_size Number of bits in the bitmap.
  331. * @param[in] bit_value The bit value to find (one or zero).
  332. *
  333. * @retval Bit position of the bit with specified value, or bitmap_size if no such bit
  334. * was found.
  335. */
  336. static inline size_t bitmap_find_bit(uint8_t * p_bitmap, size_t bitmap_size, uint8_t bit_value)
  337. {
  338. #if (defined(__GNUC__) && !defined(__SES_ARM))
  339. if (bitmap_size <= 32)
  340. {
  341. uint32_t bitmap;
  342. memcpy(&bitmap, p_bitmap, sizeof(uint32_t));
  343. if (!bit_value)
  344. {
  345. bitmap ^= 0xFFFFFFFF;
  346. }
  347. size_t result = ffs(bitmap);
  348. if (result == 0 || result > bitmap_size)
  349. {
  350. return bitmap_size;
  351. }
  352. // built-in ffs implementation gives ffs(1) = 1, not 0
  353. return result - 1;
  354. }
  355. else
  356. #endif
  357. {
  358. for (size_t i = 0; i < bitmap_size; i++)
  359. {
  360. if (BITMAP_ISSET(p_bitmap, i) == bit_value)
  361. {
  362. return i;
  363. }
  364. }
  365. return bitmap_size;
  366. }
  367. }
  368. /**@brief Reverse the elements of array
  369. *
  370. * @param[in] ptr Pointer to array.
  371. * @param[in] len Length of array.
  372. */
  373. static inline void array_reverse(uint8_t * ptr, size_t len)
  374. {
  375. for (size_t i = 0; i < len/2; i++)
  376. {
  377. SWAP_XOR(ptr[i], ptr[len - 1 - i]);
  378. }
  379. }
  380. /**@brief Returns least significant byte of word.
  381. */
  382. #define LSB_WORD(x) ((uint8_t)(x & 0xFF))
  383. /**@brief Returns least significant byte of halfword.
  384. */
  385. #define LSB_HWORD(x) LSB_WORD(x)
  386. /**@brief Returns most significant byte of halfword.
  387. */
  388. #define MSB_HWORD(x) ((uint8_t)(x >> 8))
  389. #define ALIGN_VALUE (sizeof(size_t))
  390. /**@brief Compiler-independent definitions.
  391. */
  392. #if defined ( __CC_ARM )
  393. #ifndef __WEAK
  394. #define __WEAK __weak
  395. #endif
  396. #ifndef PACK
  397. #define PACK __attribute__ ((packed))
  398. #endif
  399. #ifndef BEGIN_PACK
  400. #define BEGIN_PACK
  401. #endif
  402. #ifndef END_PACK
  403. #define END_PACK
  404. #endif
  405. #ifndef __ALIGN
  406. #define __ALIGN(n) __align(n)
  407. #endif
  408. #elif defined ( __ICCARM__ )
  409. #ifndef __WEAK
  410. #define __WEAK __weak
  411. #endif
  412. #ifndef PACK
  413. #define PACK
  414. #endif
  415. #ifndef BEGIN_PACK
  416. #define BEGIN_PACK _Pragma("pack(push, 1)")
  417. #endif
  418. #ifndef END_PACK
  419. #define END_PACK _Pragma("pack(pop)")
  420. #endif
  421. #ifndef __ALIGN
  422. #define __ALIGN(n)
  423. #endif
  424. #elif defined ( __GNUC__ )
  425. #ifndef __WEAK
  426. #define __WEAK __attribute__((weak))
  427. #endif
  428. #ifndef PACK
  429. #define PACK __attribute__ ((packed))
  430. #endif
  431. #ifndef BEGIN_PACK
  432. #define BEGIN_PACK _Pragma("pack(push,1)")
  433. #endif
  434. #ifndef END_PACK
  435. #define END_PACK _Pragma("pack(pop)")
  436. #endif
  437. #ifndef __ALIGN
  438. #define __ALIGN(n) __attribute__((aligned(n)))
  439. #endif
  440. #elif defined ( __TASKING__ )
  441. #ifndef __WEAK
  442. #define __WEAK __attribute__((weak))
  443. #endif
  444. #ifndef PACK
  445. #define PACK __attribute__ ((packed))
  446. #endif
  447. #ifndef BEGIN_PACK
  448. #define BEGIN_PACK
  449. #endif
  450. #ifndef END_PACK
  451. #define END_PACK
  452. #endif
  453. #ifndef __ALIGN
  454. #define __ALIGN(n) __align(n)
  455. #endif
  456. #endif
  457. /** @} */
  458. #endif /* SYS_UTILS_H_INCLUDED */