Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   simple bitmask class
      17             :  */
      18             : 
      19             : #pragma once
      20             : 
      21             : #include <stdint.h>
      22             : #include <string.h>
      23             : 
      24             : #include <AP_InternalError/AP_InternalError.h>
      25             : 
      26             : template<uint16_t NUMBITS>
      27             : class Bitmask {
      28             :     static constexpr uint16_t NUMWORDS = ((NUMBITS+31)/32);
      29             : 
      30             :     static_assert(NUMBITS > 0, "must store something");
      31             :     // for first_set()'s return value
      32             :     static_assert(NUMBITS <= INT16_MAX, "must fit in int16_t");
      33             :     // so that 1U << bits is in range
      34             :     static_assert(sizeof(unsigned int) >= sizeof(uint32_t), "int too small");
      35             : 
      36             : public:
      37      213115 :     Bitmask() {
      38      213115 :         clearall();
      39      213115 :     }
      40             : 
      41     1257455 :     Bitmask &operator=(const Bitmask&other) {
      42     1257455 :         memcpy(bits, other.bits, sizeof(bits[0])*NUMWORDS);
      43     1257455 :         return *this;
      44             :     }
      45             : 
      46          61 :     bool operator==(const Bitmask&other) {
      47          61 :         return memcmp(bits, other.bits, sizeof(bits[0])*NUMWORDS) == 0;
      48             :     }
      49             : 
      50             :     bool operator!=(const Bitmask&other) {
      51             :         return !(*this == other);
      52             :     }
      53             : 
      54             :     Bitmask(const Bitmask &other) = delete;
      55             : 
      56             : 
      57             :     // Construct a bitmask with some bits enabled.
      58             :     template<size_t N>
      59           1 :     Bitmask(const uint16_t (&enabled_bits)[N]) {
      60           1 :         clearall();
      61           6 :         for (size_t i = 0; i < N; ++i) {
      62           5 :             if (enabled_bits[i] < NUMBITS) {
      63           5 :                 set(enabled_bits[i]);
      64             :             }
      65             :         }
      66           1 :     }
      67             : 
      68             :     // set given bitnumber
      69    46720579 :     void set(uint16_t bit) {
      70    46720579 :         if (!validate(bit)) {
      71           9 :             return; // ignore access of invalid bit
      72             :         }
      73    46720570 :         uint16_t word = bit/32;
      74    46720570 :         uint8_t ofs = bit & 0x1f;
      75    46720570 :         bits[word] |= (1U << ofs);
      76             :     }
      77             : 
      78             :     // set all bits
      79       17590 :     void setall(void) {
      80             :         // set all words to 111...
      81      298776 :         for (uint16_t i=0; i<NUMWORDS; i++) {
      82      281186 :             bits[i] = 0xffffffff;
      83             :         }
      84             :         // ensure out-of-range bits in the last word, if any exist, are 0
      85       17590 :         uint16_t num_valid_bits = NUMBITS % 32;
      86       17590 :         if (num_valid_bits) { // word has out of range bits
      87       17586 :             bits[NUMWORDS-1] = (1U << num_valid_bits) - 1;
      88             :         }
      89       17590 :     }
      90             : 
      91             :     // clear given bitnumber
      92   156131888 :     void clear(uint16_t bit) {
      93   156131888 :         if (!validate(bit)) {
      94           9 :             return; // ignore access of invalid bit
      95             :         }
      96   156131880 :         uint16_t word = bit/32;
      97   156131880 :         uint8_t ofs = bit & 0x1f;
      98   156131880 :         bits[word] &= ~(1U << ofs);
      99             :     }
     100             : 
     101             :     // set given bitnumber to on/off
     102        4274 :     void setonoff(uint16_t bit, bool onoff) {
     103        4274 :         if (onoff) {
     104        2433 :             set(bit);
     105             :         } else {
     106        1841 :             clear(bit);
     107             :         }
     108        4274 :     }
     109             : 
     110             :     // clear all bits
     111      371676 :     void clearall(void) {
     112      371676 :         memset(bits, 0, NUMWORDS*sizeof(bits[0]));
     113      371676 :     }
     114             : 
     115             :     // return true if given bitnumber is set
     116   383674490 :     bool get(uint16_t bit) const {
     117   383674490 :         if (!validate(bit)) {
     118          18 :             return false; // pretend invalid bit is not set
     119             :         }
     120   383674443 :         uint16_t word = bit/32;
     121   383674443 :         uint8_t ofs = bit & 0x1f;
     122   383674443 :         return (bits[word] & (1U << ofs)) != 0;
     123             :     }
     124             : 
     125             :     // return true if all bits are clear
     126   243357897 :     bool empty(void) const {
     127 15812444215 :         for (uint16_t i=0; i<NUMWORDS; i++) {
     128 15569182093 :             if (bits[i] != 0) {
     129       95775 :                 return false;
     130             :             }
     131             :         }
     132   243262122 :         return true;
     133             :     }
     134             : 
     135             :     // return number of bits set
     136    74121080 :     uint16_t count() const {
     137    74121080 :         uint16_t sum = 0;
     138   370604441 :         for (uint16_t i=0; i<NUMWORDS; i++) {
     139   296483361 :             sum += __builtin_popcount(bits[i]);
     140             :         }
     141    74121080 :         return sum;
     142             :     }
     143             : 
     144             :     // return first bit set, or -1 if none set
     145   159986924 :     int16_t first_set() const {
     146   611816792 :         for (uint16_t i=0; i<NUMWORDS; i++) {
     147   510004792 :             if (bits[i] != 0) {
     148    58174924 :                 return i*32 + __builtin_ffs(bits[i]) - 1;
     149             :             }
     150             :         }
     151   101812000 :         return -1;
     152             :     }
     153             : 
     154             :     // return number of bits available
     155        1130 :     uint16_t size() const {
     156        1130 :         return NUMBITS;
     157             :     }
     158             : 
     159             : private:
     160   586525997 :     bool validate(uint16_t bit) const {
     161   586525997 :         if (bit >= NUMBITS) {
     162          36 :             INTERNAL_ERROR(AP_InternalError::error_t::bitmask_range);
     163          36 :             return false;
     164             :         }
     165   586525961 :         return true;
     166             :     }
     167             : 
     168             :     uint32_t bits[NUMWORDS];
     169             : };