diff --git a/src/core/CanMessage.cpp b/src/core/CanMessage.cpp index 386cfbfc..948f1647 100644 --- a/src/core/CanMessage.cpp +++ b/src/core/CanMessage.cpp @@ -184,33 +184,31 @@ void CanMessage::setByte(const uint8_t index, const uint8_t value) { uint64_t CanMessage::extractRawSignal(uint8_t start_bit, const uint8_t length, const bool isBigEndian) const { -// if ((start_bit+length) > (getLength()*8)) { -// return 0; -// } + if ((length == 0) || (length > 64)) { + return 0; + } // FIXME: This only gives access to data bytes 0-8. Need to rework for CANFD. - uint64_t data = le64toh(_u64[0]); - - data >>= start_bit; - - uint64_t mask = 0xFFFFFFFFFFFFFFFF; - mask <<= length; - mask = ~mask; - - data &= mask; - - // If the length is greater than 8, we need to byteswap to preserve endianness - if (isBigEndian && (length > 8)) - { - - // Swap bytes - data = __builtin_bswap64(data); + const uint8_t avail_bits = 8 * ((getLength() > 8) ? 8 : getLength()); + if (start_bit + length > avail_bits) { + return 0; + } - // Shift out unused bits - data >>= 64 - length; + const uint64_t mask = (length == 64) ? ~(uint64_t)0 : (((uint64_t)1 << length) - 1); + + if (isBigEndian) { + // DbcParser has already normalized start_bit into an MSB-first bit index + // counted from the first transmitted bit of the payload. The signal + // therefore occupies bits [start_bit, start_bit+length) of the payload + // read as one big-endian 64 bit value. + uint64_t be = 0; + for (int i = 0; i < 8; i++) { + be = (be << 8) | _u8[i]; + } + return (be >> (64 - start_bit - length)) & mask; } - return data; + return (le64toh(_u64[0]) >> start_bit) & mask; } void CanMessage::setDataAt(uint8_t position, uint8_t data)