From e5bdf0197cabba2f7b4c50fa9f949780f733221b Mon Sep 17 00:00:00 2001 From: Andrey Gusakov Date: Fri, 13 Dec 2024 20:09:27 +0300 Subject: [PATCH] SENT: move decoder to libfirmware --- Makefile | 1 + sent/include/sent_constants.h | 7 - sent/include/sent_decoder.h | 135 +++++++++ sent/sent.mk | 5 + sent/src/sent_decoder.cpp | 550 ++++++++++++++++++++++++++++++++++ 5 files changed, 691 insertions(+), 7 deletions(-) delete mode 100644 sent/include/sent_constants.h create mode 100644 sent/include/sent_decoder.h create mode 100644 sent/sent.mk create mode 100644 sent/src/sent_decoder.cpp diff --git a/Makefile b/Makefile index 5b5a4c5..db30c54 100644 --- a/Makefile +++ b/Makefile @@ -9,6 +9,7 @@ PROJECT_DIR = . RUSEFI_LIB = . include $(RUSEFI_LIB)/util/util.mk include $(RUSEFI_LIB)/pt2001/pt2001.mk +include $(RUSEFI_LIB)/sent/sent.mk # C sources that can be compiled in ARM or THUMB mode depending on the global # setting. diff --git a/sent/include/sent_constants.h b/sent/include/sent_constants.h deleted file mode 100644 index fd0aad5..0000000 --- a/sent/include/sent_constants.h +++ /dev/null @@ -1,7 +0,0 @@ - -#pragma once - - -#define SENT_MSG_DATA_SIZE 6 -/* Status + two 12-bit signals + CRC */ -#define SENT_MSG_PAYLOAD_SIZE (1 + SENT_MSG_DATA_SIZE + 1) // Size of payload diff --git a/sent/include/sent_decoder.h b/sent/include/sent_decoder.h new file mode 100644 index 0000000..eab7f59 --- /dev/null +++ b/sent/include/sent_decoder.h @@ -0,0 +1,135 @@ +/* + * sent_decoder.h + * + * SENT protocol decoder header + * + * @date Oct 01, 2022 + * @author Andrey Gusakov , (c) 2022-2024 + */ + +#pragma once + +#include + +/* Maximum slow shannel mailboxes, DO NOT CHANGE */ +#define SENT_SLOW_CHANNELS_MAX 16 + +/* collect statistic */ +#define SENT_STATISTIC_COUNTERS 1 + +typedef enum +{ + SENT_STATE_CALIB = 0, + SENT_STATE_INIT, + SENT_STATE_SYNC, + SENT_STATE_STATUS, + SENT_STATE_SIG1_DATA1, + SENT_STATE_SIG1_DATA2, + SENT_STATE_SIG1_DATA3, + SENT_STATE_SIG2_DATA1, + SENT_STATE_SIG2_DATA2, + SENT_STATE_SIG2_DATA3, + SENT_STATE_CRC, +} SENT_STATE_enum; + +struct sent_channel_stat { + uint32_t hwOverflowCnt; + + uint32_t ShortIntervalErr; + uint32_t LongIntervalErr; + uint32_t SyncErr; + uint32_t CrcErrCnt; + uint32_t FrameCnt; + uint32_t PauseCnt; + uint32_t RestartCnt; + + /* Slow channel */ + uint32_t sc; + uint32_t scCrcErr; + + uint32_t getTotalError() { + return ShortIntervalErr + LongIntervalErr + SyncErr + CrcErrCnt; + } + + float getErrorRate() { + return 1.0 * getTotalError() / (FrameCnt + getTotalError()); + } +}; + +#define SENT_FLAG_HW_OVERFLOW (1 << 0) + +class sent_channel { +private: + SENT_STATE_enum state = SENT_STATE_CALIB; + + /* Unit interval in timer clocks - adjusted on SYNC */ + uint32_t tickPerUnit = 0; + uint32_t pulseCounter = 0; + /* pulses skipped in init or calibration state while waiting for SYNC */ + uint32_t currentStatePulseCounter = 0; + bool pausePulseReceived = false; + + /* fast channel shift register*/ + uint32_t rxReg; + bool hasValidFast = false; + /* fast channel last received valid message */ + uint32_t rxLast; + + /* slow channel shift registers and flags */ + uint16_t scMsgFlags; + uint32_t scShift2; /* shift register for bit 2 from status nibble */ + uint32_t scShift3; /* shift register for bit 3 from status nibble */ + uint32_t scCrcShift; /* shift register for special order for CRC6 calculation */ + /* Slow channel decoder and helpers */ + int StoreSlowChannelValue(uint8_t id, uint16_t data); + int FastChannelDecoder(uint32_t clocks); + int SlowChannelDecoder(); + void SlowChannelDecoderReset(); + + /* CRC */ + uint8_t crc4(uint32_t data); + uint8_t crc4_gm(uint32_t data); + uint8_t crc4_gm_v2(uint32_t data); + /* Slow channel CRC6 */ + uint8_t crc6(uint32_t data); + + /* calc unit tick time from sync pulse */ + void calcTickPerUnit(uint32_t clocks); + + /* check if current pulse looks like sync pulse */ + bool isSyncPulse(uint32_t clocks); + + void restart(); + +public: + /* slow channel data */ + struct { + uint16_t data; + uint8_t id; + } scMsg[SENT_SLOW_CHANNELS_MAX]; + + /* Statistic counters */ +#if SENT_STATISTIC_COUNTERS + sent_channel_stat statistic; +#endif // SENT_STATISTIC_COUNTERS + + /* Decoder */ + int Decoder(uint32_t clocks, uint8_t flags = 0); + + /* Get last raw message */ + int GetMsg(uint32_t* rx); + /* Unpack last valid message to status, signal0 and signal1 + * Note: + * sig0 is nibbles 0 .. 2, where nibble 0 is MSB + * sig1 is nibbles 5 .. 3, where nibble 5 is MSB */ + int GetSignals(uint8_t *pStat, uint16_t *pSig0, uint16_t *pSig1); + + /* Get slow channel value for given ID 8*/ + int GetSlowChannelValue(uint8_t id); + + /* Current tick time in CPU/timer clocks */ + float getTickTime(); + + /* Show status */ + void Info(); +}; diff --git a/sent/sent.mk b/sent/sent.mk new file mode 100644 index 0000000..4f98597 --- /dev/null +++ b/sent/sent.mk @@ -0,0 +1,5 @@ +RUSEFI_LIB_INC += \ + $(RUSEFI_LIB)/sent/include + +RUSEFI_LIB_CPP += \ + $(RUSEFI_LIB)/sent/src/sent_decoder.cpp diff --git a/sent/src/sent_decoder.cpp b/sent/src/sent_decoder.cpp new file mode 100644 index 0000000..ffce4d0 --- /dev/null +++ b/sent/src/sent_decoder.cpp @@ -0,0 +1,550 @@ +/* + * sent_decoder.cpp + * + * SENT protocol decoder + * + * TODO support MAF sensors like 04E906051 see https://github.com/rusefi/rusefi-hardware/issues/146 + * + * @date Oct 01, 2022 + * @author Andrey Gusakov , (c) 2022-2024 + */ + +#include +#include "sent_decoder.h" + +/*==========================================================================*/ +/* Misc helpers. */ +/*==========================================================================*/ +#define BIT(n) (UINT32_C(1) << (n)) + +/*==========================================================================*/ +/* Protocol definitions. */ +/*==========================================================================*/ + +/* Signals only */ +#define SENT_MSG_DATA_SIZE 6 +/* Status + two 12-bit signals + CRC: 8 pulses */ +#define SENT_MSG_PAYLOAD_SIZE (1 + SENT_MSG_DATA_SIZE + 1) // Size of payload +/* Sync + Status + Signals + CRC: 9 pulses */ +#define SENT_MSG_TOTAL (1 + SENT_MSG_PAYLOAD_SIZE) + +#define SENT_OFFSET_INTERVAL 12 +#define SENT_SYNC_INTERVAL (56 - SENT_OFFSET_INTERVAL) // 56 ticks - 12 + +#define SENT_MIN_INTERVAL 12 +#define SENT_MAX_INTERVAL 15 + +#define SENT_CRC_SEED 0x05 + +/* use 3 full frames + one additional pulse for unit time calibration */ +#define SENT_CALIBRATION_PULSES (1 + 3 * SENT_MSG_PAYLOAD_SIZE) + +/*==========================================================================*/ +/* Decoder configuration */ +/*==========================================================================*/ + +/*==========================================================================*/ +/* Decoder */ +/*==========================================================================*/ + +/* Helpers for Msg manipulations */ +/* nibbles order: status, sig0_MSN, sig0_MidN, sig0_LSN, sig1_MSN, sig1_MidN, sig1_LSN, CRC */ +/* we shift rxReg left for 4 bits on each nibble received and put newest nibble + * in [3:0] bits of rxReg, so when full message is received: + * CRC is [3:0] - nibble 7 + * status is [31:28] - nibble 0 + * sig0 is [27:16], sig1 is [15:4] */ +#define MsgGetNibble(msg, n) (((msg) >> (4 * (7 - (n)))) & 0xf) +#define MsgGetStat(msg) MsgGetNibble(msg, 0) +#define MsgGetSig0(msg) (((msg) >> (4 * 4)) & 0xfff) +#define MsgGetSig1(msg) (((msg) >> (1 * 4)) & 0xfff) +#define MsgGetCrc(msg) MsgGetNibble(msg, 7) + +/* convert CPU ticks to float Us */ +#define TicksToUs(ticks) ((float)(ticks) * 1000.0 * 1000.0 / CORE_CLOCK) + +void sent_channel::restart() { + state = SENT_STATE_CALIB; + pulseCounter = 0; + currentStatePulseCounter = 0; + pausePulseReceived = false; + tickPerUnit = 0; + + /* reset slow channels */ + SlowChannelDecoderReset(); + scMsgFlags = 0; + + #if SENT_STATISTIC_COUNTERS + statistic.ShortIntervalErr = 0; + statistic.LongIntervalErr = 0; + statistic.SyncErr = 0; + statistic.CrcErrCnt = 0; + statistic.FrameCnt = 0; + statistic.PauseCnt = 0; + statistic.sc = 0; + statistic.scCrcErr = 0; + statistic.RestartCnt++; + #endif +} + +void sent_channel::calcTickPerUnit(uint32_t clocks) { + /* int division with rounding */ + tickPerUnit = (clocks + (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL) / 2) / + (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL); +} + +float sent_channel::getTickTime() { + return tickPerUnit; +} + +bool sent_channel::isSyncPulse(uint32_t clocks) +{ + /* check if pulse looks like sync with allowed +/-20% deviation */ + uint32_t syncClocks = (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL) * tickPerUnit; + + if (((100 * clocks) >= (syncClocks * 80)) && + ((100 * clocks) <= (syncClocks * 120))) + return 1; + + return 0; +} + +int sent_channel::FastChannelDecoder(uint32_t clocks) { + pulseCounter++; + + /* special case - tick time calculation */ + if (state == SENT_STATE_CALIB) { + if ((tickPerUnit == 0) || (currentStatePulseCounter == 0)) { + /* invalid or not yet calculated tickPerUnit */ + calcTickPerUnit(clocks); + /* lets assume this is sync pulse... */ + currentStatePulseCounter = 1; + } else { + /* some tickPerUnit calculated... + * Check next 1 + 6 + 1 pulses if they are valid with current tickPerUnit */ + int checkInterval = (clocks + tickPerUnit / 2) / tickPerUnit - SENT_OFFSET_INTERVAL; + if ((checkInterval >= 0) && (checkInterval <= SENT_MAX_INTERVAL)) { + currentStatePulseCounter++; + /* Should end up with CRC pulse */ + if (currentStatePulseCounter == (1 + SENT_MSG_PAYLOAD_SIZE)) { + pulseCounter = 0; + currentStatePulseCounter = 0; + state = SENT_STATE_INIT; + } + } else { + currentStatePulseCounter = 1; + calcTickPerUnit(clocks); + } + } + if (pulseCounter >= SENT_CALIBRATION_PULSES) { + /* failed to calculate valid tickPerUnit, restart */ + restart(); + } + return 0; + } + + /* special case for out-of-sync state */ + if (state == SENT_STATE_INIT) { + if (isSyncPulse(clocks)) { + /* adjust unit time */ + calcTickPerUnit(clocks); + /* we get here from calibration phase. calibration phase end with CRC nibble + * if we had to skip ONE pulse before we get sync - that means device may send pause + * pulse in between of messages */ + pausePulseReceived = false; + if (currentStatePulseCounter == 1) { + pausePulseReceived = true; + } + /* next state */ + currentStatePulseCounter = 0; + state = SENT_STATE_STATUS; + } else { + currentStatePulseCounter++; + /* 3 frames skipped, no SYNC detected - recalibrate */ + if (currentStatePulseCounter >= (SENT_MSG_TOTAL * 3)) { + restart(); + } + } + /* done for this pulse */ + return 0; + } + + int interval = (clocks + tickPerUnit / 2) / tickPerUnit - SENT_OFFSET_INTERVAL; + + if (interval < 0) { + #if SENT_STATISTIC_COUNTERS + statistic.ShortIntervalErr++; + #endif //SENT_STATISTIC_COUNTERS + state = SENT_STATE_INIT; + return -1; + } + + switch(state) + { + case SENT_STATE_CALIB: + case SENT_STATE_INIT: + /* handled above, should not get in here */ + return -1; + + case SENT_STATE_SYNC: + if (isSyncPulse(clocks)) + { + /* measured tick interval will be used until next sync pulse */ + calcTickPerUnit(clocks); + rxReg = 0; + state = SENT_STATE_STATUS; + } + else + { + if (pausePulseReceived) { + #if SENT_STATISTIC_COUNTERS + // Increment sync interval err count + statistic.SyncErr++; + if (interval > SENT_SYNC_INTERVAL) + { + statistic.LongIntervalErr++; + } + else + { + statistic.ShortIntervalErr++; + } + #endif // SENT_STATISTIC_COUNTERS + /* wait for next sync and recalibrate tickPerUnit */ + state = SENT_STATE_INIT; + return -1; + } else { + /* This is possibly pause pulse */ + /* TODO: check: + * Minimum Length 12 ticks (equivalent to a nibble with 0 value) - this is already checked + * Maximum Length 768 ticks (3 * 256) */ + #if SENT_STATISTIC_COUNTERS + statistic.PauseCnt++; + #endif // SENT_STATISTIC_COUNTERS + pausePulseReceived = true; + } + } + return 0; + + case SENT_STATE_STATUS: + /* it is possible that pause pulse was threaded as sync and we are here with sync pulse */ + if ((pausePulseReceived == false) && isSyncPulse(clocks)) { + #if SENT_STATISTIC_COUNTERS + statistic.PauseCnt++; + #endif // SENT_STATISTIC_COUNTERS + /* measured tick interval will be used until next sync pulse */ + calcTickPerUnit(clocks); + return 0; + } + // fallthrough + case SENT_STATE_SIG1_DATA1: + case SENT_STATE_SIG1_DATA2: + case SENT_STATE_SIG1_DATA3: + case SENT_STATE_SIG2_DATA1: + case SENT_STATE_SIG2_DATA2: + case SENT_STATE_SIG2_DATA3: + case SENT_STATE_CRC: + if (interval > SENT_MAX_INTERVAL) + { + #if SENT_STATISTIC_COUNTERS + statistic.LongIntervalErr++; + #endif + + state = SENT_STATE_INIT; + return -1; + } + + rxReg = (rxReg << 4) | (uint32_t)interval; + + if (state != SENT_STATE_CRC) + { + /* TODO: refactor */ + state = (SENT_STATE_enum)((int)state + 1); + return 0; + } + + #if SENT_STATISTIC_COUNTERS + statistic.FrameCnt++; + #endif // SENT_STATISTIC_COUNTERS + pausePulseReceived = false; + state = SENT_STATE_SYNC; + /* CRC check */ + /* TODO: find correct way to calculate CRC */ + if ((MsgGetCrc(rxReg) == crc4(rxReg)) || + (MsgGetCrc(rxReg) == crc4_gm(rxReg)) || + (MsgGetCrc(rxReg) == crc4_gm_v2(rxReg))) + { + /* Full packet with correct CRC has been received */ + rxLast = rxReg; + hasValidFast = true; + /* TODO: add timestamp? */ + return 1; + } + else + { + #if SENT_STATISTIC_COUNTERS + statistic.CrcErrCnt++; + #endif // SENT_STATISTIC_COUNTERS + return -1; + } + return 0; + } + + return 0; +} + +int sent_channel::Decoder(uint32_t clocks, uint8_t flags) { + int ret; + + #if SENT_STATISTIC_COUNTERS + if (flags & SENT_FLAG_HW_OVERFLOW) { + statistic.hwOverflowCnt++; + } + #endif + + /* TODO: handle flags */ + (void)flags; + + ret = FastChannelDecoder(clocks); + if (ret > 0) { + /* valid packet received, can process slow channels */ + SlowChannelDecoder(); + } else if (ret < 0) { + /* packet is incorrect, reset slow channel state machine */ + SlowChannelDecoderReset(); + } + + return ret; +} + +int sent_channel::GetMsg(uint32_t* rx) { + if (rx) { + *rx = rxLast; + } + + if (!hasValidFast) { + return -1; + } + /* TODO: add check for time since last message received */ + return 0; +} + +int sent_channel::GetSignals(uint8_t *pStat, uint16_t *pSig0, uint16_t *pSig1) { + uint32_t rx; + int ret = GetMsg(&rx); + + if (ret < 0) { + return ret; + } + + /* NOTE different MSB packing for sig0 and sig1 + * is it protocol-defined or device-specific? + * Also looks like some devices send 16 + 8 bit, not 12 + 12 */ + if (pStat) { + *pStat = MsgGetStat(rx); + } + + if (pSig0) { + uint16_t tmp = MsgGetSig0(rx); + *pSig0 = tmp; + } + + if (pSig1) { + uint16_t tmp = MsgGetSig1(rx); + /* swap */ + tmp = ((tmp >> 8) & 0x00f) | + ((tmp << 8) & 0xf00) | + (tmp & 0x0f0); + *pSig1 = tmp; + } + + return 0; +} + +int sent_channel::StoreSlowChannelValue(uint8_t id, uint16_t data) +{ + size_t i; + + /* Update already allocated messagebox? */ + for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) { + if ((scMsgFlags & BIT(i)) && (scMsg[i].id == id)) { + scMsg[i].data = data; + return 0; + } + } + + /* New message? Allocate messagebox */ + for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) { + if (!(scMsgFlags & BIT(i))) + { + scMsg[i].data = data; + scMsg[i].id = id; + scMsgFlags |= (1 << i); + return 0; + } + } + + /* No free mailboxes for new ID */ + return -1; +} + +int sent_channel::GetSlowChannelValue(uint8_t id) +{ + size_t i; + + for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) { + if ((scMsgFlags & BIT(i)) && (scMsg[i].id == id)) { + return scMsg[i].data; + } + } + + /* not found */ + return -1; +} + +int sent_channel::SlowChannelDecoder() +{ + /* bit 2 and bit 3 from status nibble are used to transfer short messages */ + bool b2 = !!(MsgGetStat(rxLast) & BIT(2)); + bool b3 = !!(MsgGetStat(rxLast) & BIT(3)); + + /* shift in new data */ + scShift2 = (scShift2 << 1) | b2; + scShift3 = (scShift3 << 1) | b3; + scCrcShift = (scCrcShift << 2) | ((uint32_t)b2 << 1) | b3; + + if (1) { + /* Short Serial Message format */ + + /* 0b1000.0000.0000.0000? */ + if ((scShift3 & 0xffff) == 0x8000) { + /* Done receiving */ + + /* TODO: add crc check */ + + uint8_t id = (scShift2 >> 12) & 0x0f; + uint16_t data = (scShift2 >> 4) & 0xff; + + return StoreSlowChannelValue(id, data); + } + } + if (1) { + /* Enhanced Serial Message format */ + + /* 0b11.1111.0xxx.xx0x.xxx0 ? */ + if ((scShift3 & 0x3f821) == 0x3f000) { + uint8_t id; + + uint8_t crc = (scShift2 >> 12) & 0x3f; + #if SENT_STATISTIC_COUNTERS + statistic.sc++; + #endif + if (crc == crc6(scCrcShift)) { + /* C-flag: configuration bit is used to indicate 16 bit format */ + bool sc16Bit = !!(scShift3 & (1 << 10)); + if (!sc16Bit) { + /* 12 bit message, 8 bit ID */ + id = ((scShift3 >> 1) & 0x0f) | + ((scShift3 >> 2) & 0xf0); + uint16_t data = scShift2 & 0x0fff; /* 12 bit */ + + /* TODO: add crc check */ + return StoreSlowChannelValue(id, data); + } else { + /* 16 bit message, 4 bit ID */ + id = (scShift3 >> 6) & 0x0f; + uint16_t data = (scShift2 & 0x0fff) | + (((scShift3 >> 1) & 0x0f) << 12); + + return StoreSlowChannelValue(id, data); + } + } else { + #if SENT_STATISTIC_COUNTERS + statistic.scCrcErr++; + #endif + } + } + } + + return 0; +} + +void sent_channel::SlowChannelDecoderReset() +{ + /* packet is incorrect, reset slow channel state machine */ + scShift2 = 0; + scShift3 = 0; +} + +/* This is correct for Si7215 */ +/* This CRC is calculated for WHOLE message expect last nibble (CRC) */ +uint8_t sent_channel::crc4(uint32_t data) +{ + size_t i; + uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101" + const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5}; + + for (i = 0; i < 7; i++) { + crc = crc ^ MsgGetNibble(data, i); + crc = CrcLookup[crc]; + } + + return crc; +} + +/* TODO: double check two following and use same CRC routine? */ + +/* This is correct for GM throttle body */ +/* This CRC is calculated for message expect status nibble and minus CRC nibble */ +uint8_t sent_channel::crc4_gm(uint32_t data) +{ + size_t i; + uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101" + const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5}; + + for (i = 1; i < 7; i++) { + crc = CrcLookup[crc]; + crc = (crc ^ MsgGetNibble(data, i)) & 0xf; + } + + return crc; +} + +/* This is correct for GDI fuel pressure sensor */ +/* This CRC is calculated for message expect status nibble and minus CRC nibble */ +uint8_t sent_channel::crc4_gm_v2(uint32_t data) +{ + size_t i; + uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101" + const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5}; + + for (i = 1; i < 7; i++) { + crc = CrcLookup[crc]; + crc = (crc ^ MsgGetNibble(data, i)) & 0xf; + } + // One more round with 0 as input + crc = CrcLookup[crc]; + + return crc; +} + +uint8_t sent_channel::crc6(uint32_t data) +{ + size_t i; + /* Seed 0x15 (21) */ + uint8_t crc = 0x15; + /* CRC table for poly = 0x59 (x^6 + x^4 + x^3 + 1) */ + const uint8_t crc6_table[64] = { + 0, 25, 50, 43, 61, 36, 15, 22, 35, 58, 17, 8, 30, 7, 44, 53, + 31, 6, 45, 52, 34, 59, 16, 9, 60, 37, 14, 23, 1, 24, 51, 42, + 62, 39, 12, 21, 3, 26, 49, 40, 29, 4, 47, 54, 32, 57, 18, 11, + 33, 56, 19, 10, 28, 5, 46, 55, 2, 27, 48, 41, 63, 38, 13, 20 }; + + for (i = 0; i < 4; i++) { + uint8_t tmp = (data >> (24 - 6 * (i + 1))) & 0x3f; + crc = tmp ^ crc6_table[crc]; + } + // Extra round with 0 input + crc = 0 ^ crc6_table[crc]; + + return crc; +}