LCOV - code coverage report
Current view: top level - firmware/hw_layer/drivers/sent - sent.cpp (source / functions) Hit Total Coverage
Test: coverage.info Lines: 154 217 71.0 %
Date: 2024-12-14 04:24:15 Functions: 13 16 81.2 %

          Line data    Source code
       1             : /*
       2             :  * sent.cpp
       3             :  *
       4             :  * SENT protocol decoder
       5             :  *
       6             :  * TODO support MAF sensors like 04E906051 see https://github.com/rusefi/rusefi-hardware/issues/146
       7             :  *
       8             :  * @date Oct 01, 2022
       9             :  * @author Andrey Gusakov <dron0gus@gmail.com>, (c) 2022
      10             :  */
      11             : 
      12             : #if EFI_PROD_CODE || EFI_UNIT_TEST
      13             : 
      14             : #include "pch.h"
      15             : 
      16             : #if EFI_SENT_SUPPORT
      17             : 
      18             : #include "sent.h"
      19             : #include "init.h"
      20             : #include "sent_logic.h"
      21             : #include "sent_constants.h"
      22             : 
      23             : /*==========================================================================*/
      24             : /* Protocol definitions.                                                                                                        */
      25             : /*==========================================================================*/
      26             : 
      27             : #define SENT_OFFSET_INTERVAL    12
      28             : #define SENT_SYNC_INTERVAL              (56 - SENT_OFFSET_INTERVAL) // 56 ticks - 12
      29             : 
      30             : #define SENT_MAX_INTERVAL               15
      31             : 
      32             : #define SENT_CRC_SEED           0x05
      33             : 
      34             : /* Sync + Status + Signals + CRC: 9 pulses */
      35             : #define SENT_MSG_TOTAL                  (1 + SENT_MSG_PAYLOAD_SIZE)
      36             : 
      37             : /* use 3 full frames + one additional pulse for unit time calibration */
      38             : #define SENT_CALIBRATION_PULSES (1 + 3 * SENT_MSG_PAYLOAD_SIZE)
      39             : 
      40             : /*==========================================================================*/
      41             : /* Decoder configuration                                                                                                        */
      42             : /*==========================================================================*/
      43             : 
      44             : /*==========================================================================*/
      45             : /* Decoder                                                                                                                                      */
      46             : /*==========================================================================*/
      47             : 
      48             : /* Helpers for Msg manipulations */
      49             : /* nibbles order: status, sig0_MSN, sig0_MidN, sig0_LSN, sig1_MSN, sig1_MidN, sig1_LSN, CRC */
      50             : /* we shift rxReg left for 4 bits on each nibble received and put newest nibble
      51             :  * in [3:0] bits of rxReg, so when full message is received:
      52             :  * CRC is [3:0] - nibble 7
      53             :  * status is [31:28] - nibble 0
      54             :  * sig0 is [27:16], sig1 is [15:4] */
      55             : #define MsgGetNibble(msg, n)    (((msg) >> (4 * (7 - (n)))) & 0xf)
      56             : #define MsgGetStat(msg)                 MsgGetNibble(msg, 0)
      57             : #define MsgGetSig0(msg)                 (((msg) >> (4 * 4)) & 0xfff)
      58             : #define MsgGetSig1(msg)                 (((msg) >> (1 * 4)) & 0xfff)
      59             : #define MsgGetCrc(msg)                  MsgGetNibble(msg, 7)
      60             : 
      61             : /* convert CPU ticks to float Us */
      62             : #define TicksToUs(ticks)                ((float)(ticks) * 1000.0 * 1000.0 / CORE_CLOCK)
      63             : 
      64           0 : void sent_channel::restart() {
      65           0 :         state = SENT_STATE_CALIB;
      66           0 :         pulseCounter = 0;
      67           0 :         currentStatePulseCounter = 0;
      68           0 :         pausePulseReceived = false;
      69           0 :         tickPerUnit = 0;
      70             : 
      71             :         /* reset slow channels */
      72           0 :         SlowChannelDecoderReset();
      73           0 :         scMsgFlags = 0;
      74             : 
      75             :         #if SENT_STATISTIC_COUNTERS
      76           0 :                 statistic.ShortIntervalErr = 0;
      77           0 :                 statistic.LongIntervalErr = 0;
      78           0 :                 statistic.SyncErr = 0;
      79           0 :                 statistic.CrcErrCnt = 0;
      80           0 :                 statistic.FrameCnt = 0;
      81           0 :                 statistic.PauseCnt = 0;
      82           0 :                 statistic.sc = 0;
      83           0 :                 statistic.scCrcErr = 0;
      84           0 :                 statistic.RestartCnt++;
      85             :         #endif
      86           0 : }
      87             : 
      88       13878 : void sent_channel::calcTickPerUnit(uint32_t clocks) {
      89             :         /* int division with rounding */
      90       13878 :         tickPerUnit =  (clocks + (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL) / 2) /
      91             :                         (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL);
      92       13878 : }
      93             : 
      94      126874 : float sent_channel::getTickTime() {
      95      126874 :         return tickPerUnit;
      96             : }
      97             : 
      98       28852 : bool sent_channel::isSyncPulse(uint32_t clocks)
      99             : {
     100             :         /* check if pulse looks like sync with allowed +/-20% deviation */
     101       28852 :         uint32_t syncClocks = (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL) * tickPerUnit;
     102             : 
     103       28852 :         if (((100 * clocks) >= (syncClocks * 80)) &&
     104       17371 :                 ((100 * clocks) <= (syncClocks * 120)))
     105       13830 :                 return 1;
     106             : 
     107       15022 :         return 0;
     108             : }
     109             : 
     110      126874 : int sent_channel::FastChannelDecoder(uint32_t clocks) {
     111      126874 :         pulseCounter++;
     112             : 
     113             :         /* special case - tick time calculation */
     114      126874 :         if (state == SENT_STATE_CALIB) {
     115         101 :                 if ((tickPerUnit == 0) || (currentStatePulseCounter == 0)) {
     116             :                         /* invalid or not yet calculated tickPerUnit */
     117           7 :                         calcTickPerUnit(clocks);
     118             :                         /* lets assume this is sync pulse... */
     119           7 :                         currentStatePulseCounter = 1;
     120             :                 } else {
     121             :                         /* some tickPerUnit calculated...
     122             :                          * Check next 1 + 6 + 1 pulses if they are valid with current tickPerUnit */
     123          94 :                         criticalAssert(tickPerUnit != 0, "zero tickPerUnit", 0);
     124          94 :                         int checkInterval = (clocks + tickPerUnit / 2) / tickPerUnit - SENT_OFFSET_INTERVAL;
     125          94 :                         if ((checkInterval >= 0) && (checkInterval <= SENT_MAX_INTERVAL)) {
     126          53 :                                 currentStatePulseCounter++;
     127             :                                 /* Should end up with CRC pulse */
     128          53 :                                 if (currentStatePulseCounter == (1 + SENT_MSG_PAYLOAD_SIZE)) {
     129           6 :                                         pulseCounter = 0;
     130           6 :                                         currentStatePulseCounter = 0;
     131           6 :                                         state = SENT_STATE_INIT;
     132             :                                 }
     133             :                         } else {
     134          41 :                                 currentStatePulseCounter = 1;
     135          41 :                                 calcTickPerUnit(clocks);
     136             :                         }
     137             :                 }
     138         101 :                 if (pulseCounter >= SENT_CALIBRATION_PULSES) {
     139             :                         /* failed to calculate valid tickPerUnit, restart */
     140           0 :                         restart();
     141             :                 }
     142         101 :                 return 0;
     143             :         }
     144             : 
     145             :         /* special case for out-of-sync state */
     146      126773 :         if (state == SENT_STATE_INIT) {
     147        1703 :                 if (isSyncPulse(clocks)) {
     148             :                         /* adjust unit time */
     149         309 :                         calcTickPerUnit(clocks);
     150             :                         /* we get here from calibration phase. calibration phase end with CRC nibble
     151             :                          * if we had to skip ONE pulse before we get sync - that means device may send pause
     152             :                          * pulse in between of messages */
     153         309 :                         pausePulseReceived = false;
     154         309 :                         if (currentStatePulseCounter == 1) {
     155          26 :                                 pausePulseReceived = true;
     156             :                         }
     157             :                         /* next state */
     158         309 :                         currentStatePulseCounter = 0;
     159         309 :                         state = SENT_STATE_STATUS;
     160             :                 } else {
     161        1394 :                         currentStatePulseCounter++;
     162             :                         /* 3 frames skipped, no SYNC detected - recalibrate */
     163        1394 :                         if (currentStatePulseCounter >= (SENT_MSG_TOTAL * 3)) {
     164           0 :                                 restart();
     165             :                         }
     166             :                 }
     167             :                 /* done for this pulse */
     168        1703 :                 return 0;
     169             :         }
     170             : 
     171      125070 :         int interval = (clocks + tickPerUnit / 2) / tickPerUnit - SENT_OFFSET_INTERVAL;
     172             : 
     173      125070 :         if (interval < 0) {
     174             :                 #if SENT_STATISTIC_COUNTERS
     175         279 :                         statistic.ShortIntervalErr++;
     176             :                 #endif //SENT_STATISTIC_COUNTERS
     177         279 :                 state = SENT_STATE_INIT;
     178         279 :                 return -1;
     179             :         }
     180             : 
     181      124791 :         switch(state)
     182             :         {
     183           0 :                 case SENT_STATE_CALIB:
     184             :                 case SENT_STATE_INIT:
     185             :                         /* handled above, should not get in here */
     186           0 :                         return -1;
     187             : 
     188       16617 :                 case SENT_STATE_SYNC:
     189       16617 :                         if (isSyncPulse(clocks))
     190             :                         {
     191             :                                 /* measured tick interval will be used until next sync pulse */
     192       13331 :                                 calcTickPerUnit(clocks);
     193       13331 :                                 rxReg = 0;
     194       13331 :                                 state = SENT_STATE_STATUS;
     195             :                         }
     196             :                         else
     197             :                         {
     198        3286 :                                 if (pausePulseReceived) {
     199             :                                         #if SENT_STATISTIC_COUNTERS
     200             :                                                 // Increment sync interval err count
     201           0 :                                                 statistic.SyncErr++;
     202           0 :                                                 if (interval > SENT_SYNC_INTERVAL)
     203             :                                                 {
     204           0 :                                                         statistic.LongIntervalErr++;
     205             :                                                 }
     206             :                                                 else
     207             :                                                 {
     208           0 :                                                         statistic.ShortIntervalErr++;
     209             :                                                 }
     210             :                                         #endif // SENT_STATISTIC_COUNTERS
     211             :                                         /* wait for next sync and recalibrate tickPerUnit */
     212           0 :                                         state = SENT_STATE_INIT;
     213           0 :                                         return -1;
     214             :                                 } else {
     215             :                                         /* This is possibly pause pulse */
     216             :                                         /* TODO: check:
     217             :                                          * Minimum Length 12 ticks (equivalent to a nibble with 0 value) - this is already checked
     218             :                                          * Maximum Length 768 ticks (3 * 256) */
     219             :                                         #if SENT_STATISTIC_COUNTERS
     220        3286 :                                                 statistic.PauseCnt++;
     221             :                                         #endif // SENT_STATISTIC_COUNTERS
     222        3286 :                                         pausePulseReceived = true;
     223             :                                 }
     224             :                         }
     225       16617 :                         return 0;
     226             : 
     227       13798 :                 case SENT_STATE_STATUS:
     228             :                         /* it is possible that pause pulse was threaded as sync and we are here with sync pulse */
     229       13798 :                         if ((pausePulseReceived == false) && isSyncPulse(clocks)) {
     230             :                                 #if SENT_STATISTIC_COUNTERS
     231         190 :                                         statistic.PauseCnt++;
     232             :                                 #endif // SENT_STATISTIC_COUNTERS
     233             :                                 /* measured tick interval will be used until next sync pulse */
     234         190 :                                 calcTickPerUnit(clocks);
     235         190 :                                 return 0;
     236             :                         }
     237             :                         // fallthrough
     238             :                 case SENT_STATE_SIG1_DATA1:
     239             :                 case SENT_STATE_SIG1_DATA2:
     240             :                 case SENT_STATE_SIG1_DATA3:
     241             :                 case SENT_STATE_SIG2_DATA1:
     242             :                 case SENT_STATE_SIG2_DATA2:
     243             :                 case SENT_STATE_SIG2_DATA3:
     244             :                 case SENT_STATE_CRC:
     245      107984 :                         if (interval > SENT_MAX_INTERVAL)
     246             :                         {
     247             :                                 #if SENT_STATISTIC_COUNTERS
     248          24 :                                         statistic.LongIntervalErr++;
     249             :                                 #endif
     250             : 
     251          24 :                                 state = SENT_STATE_INIT;
     252          24 :                                 return -1;
     253             :                         }
     254             : 
     255      107960 :                         rxReg = (rxReg << 4) | (uint32_t)interval;
     256             : 
     257      107960 :                         if (state != SENT_STATE_CRC)
     258             :                         {
     259             :                                 /* TODO: refactor */
     260       94571 :                                 state = (SENT_STATE_enum)((int)state + 1);
     261       94571 :                                 return 0;
     262             :                         }
     263             : 
     264             :                         #if SENT_STATISTIC_COUNTERS
     265       13389 :                                 statistic.FrameCnt++;
     266             :                         #endif // SENT_STATISTIC_COUNTERS
     267       13389 :                         pausePulseReceived = false;
     268       13389 :                         state = SENT_STATE_SYNC;
     269             :                         /* CRC check */
     270             :                         /* TODO: find correct way to calculate CRC */
     271       13389 :                         if ((MsgGetCrc(rxReg) == crc4(rxReg)) ||
     272       16866 :                                 (MsgGetCrc(rxReg) == crc4_gm(rxReg)) ||
     273        3477 :                                 (MsgGetCrc(rxReg) == crc4_gm_v2(rxReg)))
     274             :                         {
     275             :                                 /* Full packet with correct CRC has been received */
     276       13389 :                                 rxLast = rxReg;
     277       13389 :                                 hasValidFast = true;
     278             :                                 /* TODO: add timestamp? */
     279       13389 :                                 return 1;
     280             :                         }
     281             :                         else
     282             :                         {
     283             :                                 #if SENT_STATISTIC_COUNTERS
     284           0 :                                         statistic.CrcErrCnt++;
     285             :                                 #endif // SENT_STATISTIC_COUNTERS
     286           0 :                                 return -1;
     287             :                         }
     288             :                         return 0;
     289             :         }
     290             : 
     291           0 :         return 0;
     292             : }
     293             : 
     294      126874 : int sent_channel::Decoder(uint32_t clocks, uint8_t flags) {
     295             :         int ret;
     296             : 
     297             :         #if SENT_STATISTIC_COUNTERS
     298      126874 :                 if (flags & SENT_FLAG_HW_OVERFLOW) {
     299           0 :                         statistic.hwOverflowCnt++;
     300             :                 }
     301             :         #endif
     302             : 
     303             :         /* TODO: handle flags */
     304             :         (void)flags;
     305             : 
     306      126874 :         ret = FastChannelDecoder(clocks);
     307      126874 :         if (ret > 0) {
     308             :                 /* valid packet received, can process slow channels */
     309       13389 :                 SlowChannelDecoder();
     310      113485 :         } else if (ret < 0) {
     311             :                 /* packet is incorrect, reset slow channel state machine */
     312         303 :                 SlowChannelDecoderReset();
     313             :         }
     314             : 
     315      126874 :         return ret;
     316             : }
     317             : 
     318           6 : int sent_channel::GetMsg(uint32_t* rx) {
     319           6 :         if (rx) {
     320           0 :                 *rx = rxLast;
     321             :         }
     322             : 
     323           6 :     if (!hasValidFast) {
     324           1 :         return -1;
     325             :     }
     326             :         /* TODO: add check for time since last message received */
     327           5 :         return 0;
     328             : }
     329             : 
     330           0 : int sent_channel::GetSignals(uint8_t *pStat, uint16_t *pSig0, uint16_t *pSig1) {
     331           0 :         uint32_t rx;
     332           0 :         int ret = GetMsg(&rx);
     333             : 
     334           0 :         if (ret < 0) {
     335           0 :                 return ret;
     336             :         }
     337             : 
     338             :         /* NOTE different MSB packing for sig0 and sig1
     339             :          * is it protocol-defined or device-specific?
     340             :          * Also looks like some devices send 16 + 8 bit, not 12 + 12 */
     341           0 :         if (pStat) {
     342           0 :                 *pStat = MsgGetStat(rx);
     343             :         }
     344             : 
     345           0 :         if (pSig0) {
     346           0 :                 uint16_t tmp = MsgGetSig0(rx);
     347           0 :                 *pSig0 = tmp;
     348             :         }
     349             : 
     350           0 :         if (pSig1) {
     351           0 :                 uint16_t tmp = MsgGetSig1(rx);
     352             :                 /* swap */
     353           0 :                 tmp =   ((tmp >> 8) & 0x00f) |
     354           0 :                                 ((tmp << 8) & 0xf00) |
     355           0 :                                 (tmp & 0x0f0);
     356           0 :                 *pSig1 = tmp;
     357             :         }
     358             : 
     359           0 :         return 0;
     360             : }
     361             : 
     362         216 : int sent_channel::StoreSlowChannelValue(uint8_t id, uint16_t data)
     363             : {
     364             :         size_t i;
     365             : 
     366             :         /* Update already allocated messagebox? */
     367        1369 :         for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
     368        1317 :                 if ((scMsgFlags & BIT(i)) && (scMsg[i].id == id)) {
     369         164 :                         scMsg[i].data = data;
     370         164 :                         return 0;
     371             :                 }
     372             :         }
     373             : 
     374             :         /* New message? Allocate messagebox */
     375         620 :         for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
     376         593 :                 if (!(scMsgFlags & BIT(i)))
     377             :                  {
     378          25 :                         scMsg[i].data = data;
     379          25 :                         scMsg[i].id = id;
     380          25 :                         scMsgFlags |= (1 << i);
     381          25 :                         return 0;
     382             :                 }
     383             :         }
     384             : 
     385             :         /* No free mailboxes for new ID */
     386          27 :         return -1;
     387             : }
     388             : 
     389           0 : int sent_channel::GetSlowChannelValue(uint8_t id)
     390             : {
     391             :         size_t i;
     392             : 
     393           0 :         for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
     394           0 :                 if ((scMsgFlags & BIT(i)) && (scMsg[i].id == id)) {
     395           0 :                         return scMsg[i].data;
     396             :                 }
     397             :         }
     398             : 
     399             :         /* not found */
     400           0 :         return -1;
     401             : }
     402             : 
     403       13389 : int sent_channel::SlowChannelDecoder()
     404             : {
     405             :         /* bit 2 and bit 3 from status nibble are used to transfer short messages */
     406       13389 :         bool b2 = !!(MsgGetStat(rxLast) & BIT(2));
     407       13389 :         bool b3 = !!(MsgGetStat(rxLast) & BIT(3));
     408             : 
     409             :         /* shift in new data */
     410       13389 :         scShift2 = (scShift2 << 1) | b2;
     411       13389 :         scShift3 = (scShift3 << 1) | b3;
     412       13389 :         scCrcShift = (scCrcShift << 2) | ((uint32_t)b2 << 1) | b3;
     413             : 
     414             :         if (1) {
     415             :                 /* Short Serial Message format */
     416             : 
     417             :                 /* 0b1000.0000.0000.0000? */
     418       13389 :                 if ((scShift3 & 0xffff) == 0x8000) {
     419             :                         /* Done receiving */
     420             : 
     421             :                         /* TODO: add crc check */
     422             : 
     423           0 :                         uint8_t id = (scShift2 >> 12) & 0x0f;
     424           0 :                         uint16_t data = (scShift2 >> 4) & 0xff;
     425             : 
     426           0 :                         return StoreSlowChannelValue(id, data);
     427             :                 }
     428             :         }
     429             :         if (1) {
     430             :                 /* Enhanced Serial Message format */
     431             : 
     432             :                 /* 0b11.1111.0xxx.xx0x.xxx0 ? */
     433       13389 :                 if ((scShift3 & 0x3f821) == 0x3f000) {
     434             :                         uint8_t id;
     435             : 
     436         216 :                         uint8_t crc = (scShift2 >> 12) & 0x3f;
     437             :                         #if SENT_STATISTIC_COUNTERS
     438         216 :                                 statistic.sc++;
     439             :                         #endif
     440         216 :                         if (crc == crc6(scCrcShift)) {
     441             :                                 /* C-flag: configuration bit is used to indicate 16 bit format */
     442         216 :                                 bool sc16Bit = !!(scShift3 & (1 << 10));
     443         216 :                                 if (!sc16Bit) {
     444             :                                         /* 12 bit message, 8 bit ID */
     445         216 :                                         id = ((scShift3 >> 1) & 0x0f) |
     446         216 :                                                  ((scShift3 >> 2) & 0xf0);
     447         216 :                                         uint16_t data = scShift2 & 0x0fff; /* 12 bit */
     448             : 
     449             :                                         /* TODO: add crc check */
     450         216 :                                         return StoreSlowChannelValue(id, data);
     451             :                                 } else {
     452             :                                         /* 16 bit message, 4 bit ID */
     453           0 :                                         id = (scShift3 >> 6) & 0x0f;
     454           0 :                                         uint16_t data = (scShift2 & 0x0fff) |
     455           0 :                                                    (((scShift3 >> 1) & 0x0f) << 12);
     456             : 
     457           0 :                                         return StoreSlowChannelValue(id, data);
     458             :                                 }
     459             :                         } else {
     460             :                                 #if SENT_STATISTIC_COUNTERS
     461           0 :                                         statistic.scCrcErr++;
     462             :                                 #endif
     463             :                         }
     464             :                 }
     465             :         }
     466             : 
     467       13173 :         return 0;
     468             : }
     469             : 
     470         303 : void sent_channel::SlowChannelDecoderReset()
     471             : {
     472             :         /* packet is incorrect, reset slow channel state machine */
     473         303 :         scShift2 = 0;
     474         303 :         scShift3 = 0;
     475         303 : }
     476             : 
     477             : /* This is correct for Si7215 */
     478             : /* This CRC is calculated for WHOLE message expect last nibble (CRC) */
     479       13389 : uint8_t sent_channel::crc4(uint32_t data)
     480             : {
     481             :         size_t i;
     482       13389 :         uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101"
     483       13389 :         const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5};
     484             : 
     485      107112 :         for (i = 0; i < 7; i++) {
     486       93723 :                 crc = crc ^ MsgGetNibble(data, i);
     487       93723 :                 crc = CrcLookup[crc];
     488             :         }
     489             : 
     490       13389 :         return crc;
     491             : }
     492             : 
     493             : /* TODO: double check two following and use same CRC routine? */
     494             : 
     495             : /* This is correct for GM throttle body */
     496             : /* This CRC is calculated for message expect status nibble and minus CRC nibble */
     497       11154 : uint8_t sent_channel::crc4_gm(uint32_t data)
     498             : {
     499             :         size_t i;
     500       11154 :         uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101"
     501       11154 :         const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5};
     502             : 
     503       78078 :         for (i = 1; i < 7; i++) {
     504       66924 :                 crc = CrcLookup[crc];
     505       66924 :                 crc = (crc ^ MsgGetNibble(data, i)) & 0xf;
     506             :         }
     507             : 
     508       11154 :         return crc;
     509             : }
     510             : 
     511             : /* This is correct for GDI fuel pressure sensor */
     512             : /* This CRC is calculated for message expect status nibble and minus CRC nibble */
     513        3477 : uint8_t sent_channel::crc4_gm_v2(uint32_t data)
     514             : {
     515             :         size_t i;
     516        3477 :         uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101"
     517        3477 :         const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5};
     518             : 
     519       24339 :         for (i = 1; i < 7; i++) {
     520       20862 :                 crc = CrcLookup[crc];
     521       20862 :                 crc = (crc ^ MsgGetNibble(data, i)) & 0xf;
     522             :         }
     523             :         // One more round with 0 as input
     524        3477 :         crc = CrcLookup[crc];
     525             : 
     526        3477 :         return crc;
     527             : }
     528             : 
     529         216 : uint8_t sent_channel::crc6(uint32_t data)
     530             : {
     531             :         size_t i;
     532             :         /* Seed 0x15 (21) */
     533         216 :         uint8_t crc = 0x15;
     534             :         /* CRC table for poly = 0x59 (x^6 + x^4 + x^3 + 1) */
     535         216 :         const uint8_t crc6_table[64] = {
     536             :                  0, 25, 50, 43, 61, 36, 15, 22, 35, 58, 17,  8, 30,  7, 44, 53,
     537             :                 31,  6, 45, 52, 34, 59, 16,  9, 60, 37, 14, 23,  1, 24, 51, 42,
     538             :                 62, 39, 12, 21,  3, 26, 49, 40, 29,  4, 47, 54, 32, 57, 18, 11,
     539             :                 33, 56, 19, 10, 28,  5, 46, 55,  2, 27, 48, 41, 63, 38, 13, 20 };
     540             : 
     541        1080 :         for (i = 0; i < 4; i++) {
     542         864 :                 uint8_t tmp = (data >> (24 - 6 * (i + 1))) & 0x3f;
     543         864 :                 crc = tmp ^ crc6_table[crc];
     544             :         }
     545             :         // Extra round with 0 input
     546         216 :         crc = 0 ^ crc6_table[crc];
     547             : 
     548         216 :         return crc;
     549             : }
     550             : 
     551             : #endif /* EFI_SENT_SUPPORT */
     552             : #endif // EFI_PROD_CODE || EFI_UNIT_TEST
     553             : 
     554             : #if EFI_PROD_CODE
     555             : #if EFI_SENT_SUPPORT
     556             : 
     557             : static sent_channel channels[SENT_CHANNELS_NUM];
     558             : 
     559             : void sent_channel::Info() {
     560             :         uint8_t stat;
     561             :         uint16_t sig0, sig1;
     562             : 
     563             :         efiPrintf("Unit time %lu CPU ticks %f uS", tickPerUnit, TicksToUs(getTickTime()));
     564             :         efiPrintf("Pause pulse detected %s", pausePulseReceived ? "Yes" : "No");
     565             :         efiPrintf("Total pulses %lu", pulseCounter);
     566             : 
     567             :         if (GetSignals(&stat, &sig0, &sig1) == 0) {
     568             :                 efiPrintf("Last valid fast msg Status 0x%01x Sig0 0x%03x Sig1 0x%03x", stat, sig0, sig1);
     569             :         }
     570             : 
     571             :         if (scMsgFlags) {
     572             :                 efiPrintf("Slow channels:");
     573             :                 for (int i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
     574             :                         if (scMsgFlags & BIT(i)) {
     575             :                                 efiPrintf(" ID %d: %d", scMsg[i].id, scMsg[i].data);
     576             :                         }
     577             :                 }
     578             :         }
     579             : 
     580             :         #if SENT_STATISTIC_COUNTERS
     581             :                 efiPrintf("HW overflows %lu\n", statistic.hwOverflowCnt);
     582             : 
     583             :                 efiPrintf("Pause pulses %lu\n", statistic.PauseCnt);
     584             :                 efiPrintf("Restarts %lu", statistic.RestartCnt);
     585             :                 efiPrintf("Interval errors %lu short, %lu long", statistic.ShortIntervalErr, statistic.LongIntervalErr);
     586             :                 efiPrintf("Total frames %lu with CRC error %lu (%f%%)", statistic.FrameCnt, statistic.CrcErrCnt, statistic.CrcErrCnt * 100.0 / statistic.FrameCnt);
     587             :                 efiPrintf("Total slow channel messages %lu with crc6 errors %lu (%f%%)", statistic.sc, statistic.scCrcErr, statistic.scCrcErr * 100.0 / statistic.sc);
     588             :                 efiPrintf("Sync errors %lu", statistic.SyncErr);
     589             :         #endif
     590             : }
     591             : 
     592             : /*==========================================================================*/
     593             : /* Decoder thread settings.                                                                                                     */
     594             : /*==========================================================================*/
     595             : 
     596             : /* 4 per channel should be enough */
     597             : #define SENT_MB_SIZE            (4 * SENT_CHANNELS_NUM)
     598             : 
     599             : static msg_t sent_mb_buffer[SENT_MB_SIZE];
     600             : static MAILBOX_DECL(sent_mb, sent_mb_buffer, SENT_MB_SIZE);
     601             : 
     602             : static THD_WORKING_AREA(waSentDecoderThread, 256);
     603             : 
     604             : void SENT_ISR_Handler(uint8_t channel, uint16_t clocks, uint8_t flags) {
     605             :         /* encode to fit msg_t */
     606             :         msg_t msg = (flags << 24) | (channel << 16) | clocks;
     607             : 
     608             :         /* called from ISR */
     609             :         chSysLockFromISR();
     610             :         chMBPostI(&sent_mb, msg);
     611             :         chSysUnlockFromISR();
     612             : }
     613             : 
     614             : static void SentDecoderThread(void*) {
     615             :         while (true) {
     616             :                 msg_t ret;
     617             :                 msg_t msg;
     618             : 
     619             :                 ret = chMBFetchTimeout(&sent_mb, &msg, TIME_INFINITE);
     620             : 
     621             :                 if (ret == MSG_OK) {
     622             :                         uint16_t tick = msg & 0xffff;
     623             :                         uint8_t n = (msg >> 16) & 0xff;
     624             :                         uint8_t flags = (msg >> 24) & 0xff;
     625             : 
     626             :                         if (n < SENT_CHANNELS_NUM) {
     627             :                                 sent_channel &channel = channels[n];
     628             : 
     629             :                                 if (channel.Decoder(tick, flags) > 0) {
     630             :                                         /* report only for first channel */
     631             :                                         if (n == 0) {
     632             :                                                 uint16_t sig0, sig1;
     633             :                                                 channel.GetSignals(NULL, &sig0, &sig1);
     634             :                                                 engine->sent_state.value0 = sig0;
     635             :                                                 engine->sent_state.value1 = sig1;
     636             : 
     637             :                                                 #if SENT_STATISTIC_COUNTERS
     638             :                                                     engine->sent_state.errorRate = 100.0 * channel.statistic.getErrorRate();
     639             :                                                 #endif // SENT_STATISTIC_COUNTERS
     640             :                                         }
     641             : 
     642             :                                         SentInput input = static_cast<SentInput>((size_t)SentInput::INPUT1 + n);
     643             :                                         /* Call high level decoder from here */
     644             :                                         /* TODO: implemnet subscribers, like it is done for ADC */
     645             :                                         sentTpsDecode(input);
     646             :                                         sentPressureDecode(input);
     647             :                                 }
     648             :                         }
     649             :                 }
     650             :         }
     651             : }
     652             : 
     653             : static void printSentInfo() {
     654             :         for (int i = 0; i < SENT_CHANNELS_NUM; i++) {
     655             :                 sent_channel &channel = channels[i];
     656             : 
     657             :         const char * pinName = getBoardSpecificPinName(engineConfiguration->sentInputPins[i]);
     658             :                 efiPrintf("---- SENT input %d ---- on %s", i + 1, pinName);
     659             :                 channel.Info();
     660             :                 efiPrintf("--------------------");
     661             :         }
     662             : }
     663             : 
     664             : /* Don't be confused: this actually returns throttle body position */
     665             : /* TODO: remove, replace with getSentValues() */
     666             : float getSentValue(SentInput input) {
     667             :         size_t index = static_cast<size_t>(input) - static_cast<size_t>(SentInput::INPUT1);
     668             : 
     669             :         if (index < SENT_CHANNELS_NUM) {
     670             :                 uint16_t sig0, sig1;
     671             :                 sent_channel &channel = channels[index];
     672             : 
     673             :                 if (channel.GetSignals(NULL, &sig0, &sig1) == 0) {
     674             : 
     675             :                         // GM sig0 + sig1 == 0xfff but Ford does not
     676             :                         /* scale to 0.0 .. 1.0 */
     677             :                         return sig0;
     678             :                 }
     679             :         }
     680             : 
     681             :     return NAN;
     682             : }
     683             : 
     684             : int getSentValues(SentInput input, uint16_t *sig0, uint16_t *sig1) {
     685             :         size_t index = static_cast<size_t>(input) - static_cast<size_t>(SentInput::INPUT1);
     686             : 
     687             :         if (index < SENT_CHANNELS_NUM) {
     688             :                 sent_channel &channel = channels[index];
     689             : 
     690             :                 return channel.GetSignals(NULL, sig0, sig1);
     691             :         }
     692             : 
     693             :         /* invalid channel */
     694             :     return -1;
     695             : }
     696             : 
     697             : /* Should be called once */
     698             : void initSent(void) {
     699             :         /* init interval mailbox */
     700             :         chMBObjectInit(&sent_mb, sent_mb_buffer, SENT_MB_SIZE);
     701             : 
     702             :         chThdCreateStatic(waSentDecoderThread, sizeof(waSentDecoderThread), NORMALPRIO, SentDecoderThread, nullptr);
     703             : 
     704             :         /* Start HW layer */
     705             :         startSent();
     706             : 
     707             :         addConsoleAction("sentinfo", &printSentInfo);
     708             : }
     709             : 
     710             : #endif /* EFI_SENT_SUPPORT */
     711             : #endif /* EFI_PROD_CODE */

Generated by: LCOV version 1.14