ardupilot/libraries/AP_Common/Bitmask.h

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

173 lines
4.5 KiB
C
Raw Normal View History

2016-11-17 03:19:05 -04:00
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simple bitmask class
*/
2017-05-05 00:13:14 -03:00
#pragma once
#include <stdint.h>
#include <string.h>
#include <AP_InternalError/AP_InternalError.h>
template<uint16_t num_bits>
2016-11-17 03:19:05 -04:00
class Bitmask {
public:
Bitmask() :
numbits(num_bits),
numwords((num_bits+31)/32) {
clearall();
2016-11-17 03:19:05 -04:00
}
Bitmask &operator=(const Bitmask&other) {
memcpy(bits, other.bits, sizeof(bits[0])*other.numwords);
return *this;
2016-11-17 03:19:05 -04:00
}
bool operator==(const Bitmask&other) {
if (other.numbits != numbits) {
return false;
} else {
return memcmp(bits, other.bits, sizeof(bits[0])*numwords) == 0;
}
}
bool operator!=(const Bitmask&other) {
return !(*this == other);
}
Bitmask(const Bitmask &other) = delete;
2016-11-17 03:19:05 -04:00
// set given bitnumber
void set(uint16_t bit) {
if (!validate(bit)) {
return; // ignore access of invalid bit
}
2016-11-17 03:19:05 -04:00
uint16_t word = bit/32;
uint8_t ofs = bit & 0x1f;
bits[word] |= (1U << ofs);
}
// set all bits
void setall(void) {
// set all words to 111...
for (uint16_t i=0; i<numwords; i++) {
bits[i] = 0xffffffff;
}
// ensure out-of-range bits in the last word, if any exist, are 0
uint16_t num_valid_bits = numbits % 32;
if (num_valid_bits) { // word has out of range bits
bits[numwords-1] = (1U << num_valid_bits) - 1;
}
}
2016-11-17 03:19:05 -04:00
// clear given bitnumber
void clear(uint16_t bit) {
if (!validate(bit)) {
return; // ignore access of invalid bit
}
2016-11-17 03:19:05 -04:00
uint16_t word = bit/32;
uint8_t ofs = bit & 0x1f;
bits[word] &= ~(1U << ofs);
}
// set given bitnumber to on/off
void setonoff(uint16_t bit, bool onoff) {
if (onoff) {
set(bit);
} else {
clear(bit);
}
}
2016-11-17 03:19:05 -04:00
// clear all bits
void clearall(void) {
memset(bits, 0, numwords*sizeof(bits[0]));
2016-11-17 03:19:05 -04:00
}
2017-08-23 01:58:31 -03:00
2016-11-17 03:19:05 -04:00
// return true if given bitnumber is set
2017-08-31 02:59:16 -03:00
bool get(uint16_t bit) const {
if (!validate(bit)) {
return false; // pretend invalid bit is not set
}
2016-11-17 03:19:05 -04:00
uint16_t word = bit/32;
uint8_t ofs = bit & 0x1f;
return (bits[word] & (1U << ofs)) != 0;
}
// return true if all bits are clear
2017-08-31 02:59:16 -03:00
bool empty(void) const {
2016-11-17 03:19:05 -04:00
for (uint16_t i=0; i<numwords; i++) {
if (bits[i] != 0) {
return false;
}
}
return true;
}
// return number of bits set
2017-08-31 02:59:16 -03:00
uint16_t count() const {
uint16_t sum = 0;
for (uint16_t i=0; i<numwords; i++) {
if (sizeof(bits[i]) <= sizeof(int)) {
sum += __builtin_popcount(bits[i]);
} else if (sizeof(bits[i]) <= sizeof(long)) {
sum += __builtin_popcountl(bits[i]);
} else {
sum += __builtin_popcountll(bits[i]);
}
}
return sum;
}
// return first bit set, or -1 if none set
int16_t first_set() const {
for (uint16_t i=0; i<numwords; i++) {
if (bits[i] == 0) {
continue;
}
int fs;
if (sizeof(bits[i]) <= sizeof(int)) {
fs = __builtin_ffs(bits[i]);
} else if (sizeof(bits[i]) <= sizeof(long)) {
fs = __builtin_ffsl(bits[i]);
} else {
fs = __builtin_ffsll(bits[i]);
}
return i*32 + fs - 1;
}
return -1;
}
// return number of bits available
2017-08-31 02:59:16 -03:00
uint16_t size() const {
return numbits;
}
2016-11-17 03:19:05 -04:00
private:
bool validate(uint16_t bit) const {
if (bit >= numbits) {
INTERNAL_ERROR(AP_InternalError::error_t::bitmask_range);
return false;
}
return true;
}
uint16_t numbits;
uint16_t numwords;
uint32_t bits[(num_bits+31)/32];
2016-11-17 03:19:05 -04:00
};