src/main/common/bitarray.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19 #include <stdbool.h>
  20 #include <string.h>
  21
  22 #include "bitarray.h"
  23
  24 #define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op (1 << ((bit) % (sizeof((array)[0]) * 8))))
  25
  26 bool bitArrayGet(const bitarrayElement_t *array, unsigned bit)
  27 {
  28     return BITARRAY_BIT_OP((uint32_t*)array, bit, &);
  29 }
  30
  31 void bitArraySet(bitarrayElement_t *array, unsigned bit)
  32 {
  33     BITARRAY_BIT_OP((uint32_t*)array, bit, |=);
  34 }
  35
  36 void bitArrayClr(bitarrayElement_t *array, unsigned bit)
  37 {
  38     BITARRAY_BIT_OP((uint32_t*)array, bit, &=~);
  39 }
  40
  41 void bitArraySetAll(bitarrayElement_t *array, size_t size)
  42 {
  43     memset(array, 0xFF, size);
  44 }
  45
  46 void bitArrayClrAll(bitarrayElement_t *array, size_t size)
  47 {
  48     memset(array, 0, size);
  49 }
  50
  51 __attribute__((always_inline)) static inline uint8_t __CTZ(uint32_t val)
  52 {
  53     // __builtin_ctz is not defined for zero, since it's arch
  54     // dependant. However, in ARM it gets translated to a
  55     // rbit and then a clz, making it return 32 for zero on ARM.
  56     // For other architectures, explicitely implement the same
  57     // semantics.
  58 #if defined(__arm__) && !defined(SITL_BUILD)
  59     uint8_t zc;
  60     __asm__ volatile ("rbit %1, %1\n\t"
  61                       "clz %0, %1"
  62                     : "=r" (zc)
  63                     : "r" (val) );
  64     return zc;
  65 #else
  66     // __builtin_clz is not defined for zero, since it's arch
  67     // dependant. Make it return 32 like ARM's CLZ.
  68     return val ? __builtin_ctz(val) : 32;
  69 #endif
  70 }
  71
  72 int bitArrayFindFirstSet(const bitarrayElement_t *array, unsigned start, size_t size)
  73 {
  74     const uint32_t *ptr = (uint32_t*)array;
  75     const uint32_t *end = ptr + (size / 4);
  76     const uint32_t *p = ptr + start / (8 * 4);
  77     int ret;
  78     if (p < end) {
  79         // First iteration might need to mask some bits
  80         uint32_t mask = 0xFFFFFFFF << (start % (8 * 4));
  81         if ((ret = __CTZ(*p & mask)) != 32) {
  82             return (((char *)p) - ((char *)ptr)) * 8 + ret;
  83         }
  84         p++;
  85         while (p < end) {
  86             if ((ret = __CTZ(*p)) != 32) {
  87                 return (((char *)p) - ((char *)ptr)) * 8 + ret;
  88             }
  89             p++;
  90         }
  91     }
  92     return -1;
  93 }