AP_Common: unify bitmask out of bounds checking

Ensures out-of-bounds reads and writes are never performed for setting,
clearing, and checking. Fixes test failure when the number of bits
evenly divides the word size.
This commit is contained in:
Thomas Watson 2024-07-07 14:57:02 -05:00 committed by Andrew Tridgell
parent 9a89223531
commit 0b320fe303

View File

@ -53,10 +53,8 @@ public:
// set given bitnumber // set given bitnumber
void set(uint16_t bit) { void set(uint16_t bit) {
// ignore an invalid bit number if (!validate(bit)) {
if (bit >= numbits) { return; // ignore access of invalid bit
INTERNAL_ERROR(AP_InternalError::error_t::bitmask_range);
return;
} }
uint16_t word = bit/32; uint16_t word = bit/32;
uint8_t ofs = bit & 0x1f; uint8_t ofs = bit & 0x1f;
@ -78,6 +76,9 @@ public:
// clear given bitnumber // clear given bitnumber
void clear(uint16_t bit) { void clear(uint16_t bit) {
if (!validate(bit)) {
return; // ignore access of invalid bit
}
uint16_t word = bit/32; uint16_t word = bit/32;
uint8_t ofs = bit & 0x1f; uint8_t ofs = bit & 0x1f;
bits[word] &= ~(1U << ofs); bits[word] &= ~(1U << ofs);
@ -99,14 +100,11 @@ public:
// return true if given bitnumber is set // return true if given bitnumber is set
bool get(uint16_t bit) const { bool get(uint16_t bit) const {
if (!validate(bit)) {
return false; // pretend invalid bit is not set
}
uint16_t word = bit/32; uint16_t word = bit/32;
uint8_t ofs = bit & 0x1f; uint8_t ofs = bit & 0x1f;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (bit >= numbits) {
INTERNAL_ERROR(AP_InternalError::error_t::bitmask_range);
return false;
}
#endif
return (bits[word] & (1U << ofs)) != 0; return (bits[word] & (1U << ofs)) != 0;
} }
@ -160,6 +158,14 @@ public:
} }
private: 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 numbits;
uint16_t numwords; uint16_t numwords;
uint32_t bits[(num_bits+31)/32]; uint32_t bits[(num_bits+31)/32];