diff --git a/src/core/util/bitset_extensions.cpp b/src/core/util/bitset_extensions.cpp index 00d11723cb..a555d7061e 100644 --- a/src/core/util/bitset_extensions.cpp +++ b/src/core/util/bitset_extensions.cpp @@ -32,20 +32,11 @@ size_t FindNextFixedWidth(std::bitset<kWidth> const& bs, size_t pos) { size_t bit_pos = pos % 8ul; for (size_t byte_idx{start_byte}; byte_idx < kNumBytes; ++byte_idx) { auto byte = GetByte(val, byte_idx); + if (byte_idx == start_byte) { + byte &= kFirstBits[bit_pos]; + } if (byte > 0ul) { - if (byte_idx > start_byte) { - return byte_idx * 8ul + std::countr_zero(byte); - } else { - size_t leading_zeros = std::countl_zero(byte); - if (leading_zeros < 7ul - bit_pos) { - std::bitset<8> bs{byte}; - for (size_t i{bit_pos + 1}; i < 8ul; ++i) { - if (bs[i]) { - return start_byte * 8ul + i; - } - } - } - } + return byte_idx * 8ul + std::countr_zero(byte); } } return kWidth; diff --git a/src/core/util/bitset_extensions.h b/src/core/util/bitset_extensions.h index 869989571d..f5a6ed5e17 100644 --- a/src/core/util/bitset_extensions.h +++ b/src/core/util/bitset_extensions.h @@ -23,6 +23,9 @@ static std::vector<unsigned long long> const kBytes{ 0x00'00'00'00'00'00'00'ff, 0x00'00'00'00'00'00'ff'00, 0x00'00'00'00'ff'ff'00'00, 0x00'00'00'00'ff'00'00'00, 0x00'00'00'ff'00'00'00'00, 0x00'00'ff'00'00'00'00'00, 0x00'ff'00'00'00'00'00'00, 0xff'00'00'00'00'00'00'00}; +static std::vector<unsigned char> const kFirstBits { + 0b11111110, 0b11111100, 0b11111000, 0b11110000, 0b11100000, 0b11000000, 0b10000000, 0b00000000 +}; constexpr static size_t kNumBytes = 8; constexpr static size_t kWidth = 64;