visus
/
framework-arduinoteensy-cxx20
關注 1
收藏 0
複製 0
程式碼 問題 0 合併請求 0 版本發佈 0 Wiki 活動
PlatformIO package of the Teensy core framework compatible with GCC 10 & C++20
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2 提交
1 分支
目錄樹: daa7cc5a30
分支列表 標籤
${ item.name }
建立分支 ${ searchTerm }
從 'daa7cc5a30'
${ noResults }
framework-arduinoteensy-cxx20/libraries/FlexCAN_T4/FlexCAN_T4FD.tpp

828 line
37KB
原始文件 Blame 歷史記錄 

  1. /*

  2.   MIT License

  3. 

  4.   Copyright (c) 2018 Antonio Alexander Brewer (tonton81) - https://github.com/tonton81

  5. 

  6.   Designed and tested for PJRC Teensy 4.0.

  7. 

  8.   Forum link : https://forum.pjrc.com/threads/56035-FlexCAN_T4-FlexCAN-for-Teensy-4?highlight=flexcan_t4

  9. 

  10.   Thanks goes to skpang, mjs513, and collin for tech/testing support

  11. 

  12.   Permission is hereby granted, free of charge, to any person obtaining a copy

  13.   of this software and associated documentation files (the "Software"), to deal

  14.   in the Software without restriction, including without limitation the rights

  15.   to use, copy, modify, merge, publish, distribute, sublicense, and / or sell

  16.   copies of the Software, and to permit persons to whom the Software is

  17.   furnished to do so, subject to the following conditions:

  18. 

  19.   The above copyright notice and this permission notice shall be included in all

  20.   copies or substantial portions of the Software.

  21. 

  22.   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

  23.   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

  24.   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

  25.   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

  26.   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

  27.   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE

  28.   SOFTWARE.

  29. */

  30. #include <FlexCAN_T4.h>

  31. #include "imxrt_flexcan.h"

  32. #include "Arduino.h"

  33. 

  34. static void flexcan_isr_can3fd();

  35. 

  36. FCTPFD_FUNC FCTPFD_OPT::FlexCAN_T4FD() {

  37.   if ( _bus == CAN3 ) _CAN3 = this;

  38. }

  39. 

  40. FCTPFD_FUNC void FCTPFD_OPT::setClock(FLEXCAN_CLOCK clock) {

  41.   if ( clock == CLK_OFF ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(3) | CCM_CSCMR2_CAN_CLK_PODF(0);

  42.   if ( clock == CLK_8MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(9);

  43.   if ( clock == CLK_16MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(4);

  44.   if ( clock == CLK_24MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(1) | CCM_CSCMR2_CAN_CLK_PODF(0);

  45.   if ( clock == CLK_20MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(3);

  46.   if ( clock == CLK_30MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(0) | CCM_CSCMR2_CAN_CLK_PODF(1);

  47.   if ( clock == CLK_40MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(1);

  48.   if ( clock == CLK_60MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(0) | CCM_CSCMR2_CAN_CLK_PODF(0);

  49.   if ( clock == CLK_80MHz ) CCM_CSCMR2 = (CCM_CSCMR2 & 0xFFFFFC03) | CCM_CSCMR2_CAN_CLK_SEL(2) | CCM_CSCMR2_CAN_CLK_PODF(0);

  50. }

  51. 

  52. FCTPFD_FUNC uint32_t FCTPFD_OPT::getClock() {

  53.   const uint8_t clocksrc[4] = {60, 24, 80, 0};

  54.   return clocksrc[(CCM_CSCMR2 & 0x300) >> 8];

  55. }

  56. 

  57. FCTPFD_FUNC void FCTPFD_OPT::begin() {

  58.   if ( !getClock() ) setClock(CLK_24MHz); /* no clock enabled, enable osc clock */

  59.   CCM_CCGR0 |= CCM_CCGR0_LPUART3(CCM_CCGR_ON); /* hardware bug, FD is unable to operate without an LPUART clock online */

  60.   if ( _bus == CAN3 ) {

  61.     nvicIrq = IRQ_CAN3;

  62.     _VectorsRam[16 + nvicIrq] = flexcan_isr_can3fd;

  63.     CCM_CCGR7 |= 0x3C0;

  64.     busNumber = 3;

  65.   }

  66. 

  67.   setTX(); setRX();

  68. 

  69.   FLEXCANb_MCR(_bus) &= ~FLEXCAN_MCR_MDIS; /* enable module */

  70.   FLEXCAN_EnterFreezeMode();

  71.   FLEXCANb_CTRL1(_bus) = FLEXCAN_CTRL_LOM /*| (1UL << 5)*/; /* listen only mode, TSYN */

  72.   FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_FRZ; /* enable freeze bit */

  73.   while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_LPM_ACK);

  74.   softReset(); /* reset bus */

  75.   while (!(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK));

  76.   FLEXCANb_MCR(_bus) &= ~0x8000; // disable DMA

  77.   FLEXCANb_MCR(_bus) |= (1UL << 16) | (1UL << 11) | (1UL << 17) | 0x3F; // IRMQ, FDEN, SRXDIS, 64MBs

  78.   FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_AEN; // TX ABORT FEATURE

  79.   FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_LPRIO_EN; // TX PRIORITY FEATURE

  80.   FLEXCANb_CTRL2(_bus) |= FLEXCAN_CTRL2_RRS | // store remote frames

  81.                                   FLEXCAN_CTRL2_EACEN | /* handles the way filtering works. Library adjusts to whether you use this or not */ 

  82.                                   FLEXCAN_CTRL2_MRP | // mailbox > FIFO priority.

  83.                                   FLEXCAN_CTRL2_ISOCANFDEN;

  84.   FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_WRN_EN;

  85.   FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_WAK_MSK;

  86.   FLEXCANb_FDCTRL(_bus) = 0x80008300; // MBDSR0-MBDSR1 = 0; 32MBs each as default, TDCEN, FDRATE(BRS)

  87.   disableFIFO();

  88.   FLEXCAN_ExitFreezeMode();

  89.   NVIC_ENABLE_IRQ(nvicIrq);

  90. }

  91. 

  92. FCTPFD_FUNC uint8_t FCTPFD_OPT::setRegions(uint8_t size) {

  93.   FLEXCAN_EnterFreezeMode();

  94.   FLEXCANb_FDCTRL(_bus) &= ~0x1B0000; /* clear both memory region bits */

  95.   if ( constrain(size, 8, 64) == 8 ) {

  96.     size = 64;

  97.   }

  98.   else if ( constrain(size, 16, 64) == 16 ) {

  99.     FLEXCANb_FDCTRL(_bus) |= 0x90000;

  100.     size = 42;

  101.   }

  102.   else if ( constrain(size, 32, 64) == 32 ) {

  103.     FLEXCANb_FDCTRL(_bus) |= 0x120000;

  104.     size = 24;

  105.   }

  106.   else {

  107.     FLEXCANb_FDCTRL(_bus) |= 0x1B0000;

  108.     size = 14;

  109.   }

  110.   disableFIFO();

  111.   FLEXCAN_ExitFreezeMode();

  112.   return size;

  113. }

  114. 

  115. FCTPFD_FUNC uint8_t FCTPFD_OPT::setRegions(uint8_t mbdsr0, uint8_t mbdsr1) {

  116.   FLEXCAN_EnterFreezeMode();

  117.   FLEXCANb_FDCTRL(_bus) &= ~0x1B0000; /* clear both memory region bits */

  118.   if ( constrain(mbdsr0, 8, 64) == 8 ) {

  119.     mbdsr0 = 32;

  120.   }

  121.   else if ( constrain(mbdsr0, 16, 64) == 16 ) {

  122.     FLEXCANb_FDCTRL(_bus) |= 0x10000;

  123.     mbdsr0 = 21;

  124.   }

  125.   else if ( constrain(mbdsr0, 32, 64) == 32 ) {

  126.     FLEXCANb_FDCTRL(_bus) |= 0x20000;

  127.     mbdsr0 = 12;

  128.   }

  129.   else {

  130.     FLEXCANb_FDCTRL(_bus) |= 0x30000;

  131.     mbdsr0 = 7;

  132.   }

  133.   if ( constrain(mbdsr1, 8, 64) == 8 ) {

  134.     mbdsr1 = 32;

  135.   }

  136.   else if ( constrain(mbdsr1, 16, 64) == 16 ) {

  137.     FLEXCANb_FDCTRL(_bus) |= 0x80000;

  138.     mbdsr1 = 21;

  139.   }

  140.   else if ( constrain(mbdsr1, 32, 64) == 32 ) {

  141.     FLEXCANb_FDCTRL(_bus) |= 0x100000;

  142.     mbdsr1 = 12;

  143.   }

  144.   else {

  145.     FLEXCANb_FDCTRL(_bus) |= 0x180000;

  146.     mbdsr1 = 7;

  147.   }

  148.   disableFIFO();

  149.   FLEXCAN_ExitFreezeMode();

  150.   return mbdsr0 + mbdsr1;

  151. }

  152. 

  153. FCTPFD_FUNC uint32_t FCTPFD_OPT::mailbox_offset(uint8_t mailbox, uint8_t &maxsize) {

  154.   const uint8_t data_size[4] = { 8, 16, 32, 64 };

  155.   const uint8_t mbx_total[4] = { 32, 21, 12, 7 };

  156.   const uint8_t mbx_shift[4] = { 0x10, 0x18, 0x28, 0x48 };

  157.   uint8_t region0 = (FLEXCANb_FDCTRL(_bus) & (3UL << 16)) >> 16;

  158.   uint8_t region1 = (FLEXCANb_FDCTRL(_bus) & (3UL << 19)) >> 19;

  159. 

  160.   if ( mailbox < mbx_total[region0] ) {

  161.     maxsize = data_size[region0];

  162.     return _bus + 0x80 + (mbx_shift[region0] * mailbox);

  163.   }

  164.   else if ( mailbox < (mbx_total[region0] + mbx_total[region1]) ) {

  165.     maxsize = data_size[region1];

  166.     return _bus + 0x280 + (mbx_shift[region1] * (mailbox - mbx_total[region0]));

  167.   }

  168.   return _bus + 0x80;

  169. }

  170. 

  171. FCTPFD_FUNC int FCTPFD_OPT::getFirstTxBox() {

  172.   for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) {

  173.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize)));

  174.     if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return i; // if TX

  175.   }

  176.   return -1;

  177. }

  178. 

  179. FCTPFD_FUNC uint8_t FCTPFD_OPT::getFirstTxBoxSize() {

  180.   uint8_t mbsize = 0;

  181.   mailbox_offset(getFirstTxBox(), mbsize);

  182.   return mbsize;

  183. }

  184. 

  185. FCTPFD_FUNC void FCTPFD_OPT::setBaudRate(FLEXCAN_FDRATES input, FLEXCAN_RXTX listen_only) {

  186.   bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK);

  187.   FLEXCAN_EnterFreezeMode();

  188.   FLEXCANb_FDCTRL(_bus) = (FLEXCANb_FDCTRL(_bus) & 0xFFFF60FF); /* clear TDC values */

  189.   FLEXCANb_CBT(_bus) &= ~(1UL << 31); /* clear BTE bit to edit CTRL1 register */

  190.   ( listen_only != LISTEN_ONLY ) ? FLEXCANb_CTRL1(_bus) &= ~FLEXCAN_CTRL_LOM : FLEXCANb_CTRL1(_bus) |= FLEXCAN_CTRL_LOM;

  191.   if ( input == CAN_1M_2M ) { /* based on 24MHz and 70% sample point */

  192.     setClock(CLK_24MHz);

  193.     FLEXCANb_FDCTRL(_bus) |= (0x801B8300 & 0x9F00);

  194.     FLEXCANb_FDCBT(_bus) = 0x31423;

  195.     FLEXCANb_CBT(_bus) = 0x800624A6;

  196.   }

  197.   if ( input == CAN_1M_4M ) { /* based on 24MHz and 70% sample point */

  198.     setClock(CLK_24MHz);

  199.     FLEXCANb_FDCTRL(_bus) |= (0x80008300 & 0x9F00);

  200.     FLEXCANb_FDCBT(_bus) = 0x10421;

  201.     FLEXCANb_CBT(_bus) = 0x800624A6;

  202.   }

  203.   if ( input == CAN_1M_6M ) { /* based on 30MHz and 70% sample point */

  204.     setClock(CLK_30MHz);

  205.     FLEXCANb_FDCTRL(_bus) |= (0x80008300 & 0x9F00);

  206.     FLEXCANb_FDCBT(_bus) = 0x401;

  207.     FLEXCANb_CBT(_bus) = 0x80082CE8;

  208.   }

  209.   if ( input == CAN_1M_8M ) { /* based on 40MHz and 70% sample point */

  210.     setClock(CLK_40MHz);

  211.     FLEXCANb_FDCTRL(_bus) |= (0x80008300 & 0x9F00);

  212.     FLEXCANb_FDCBT(_bus) = 0x401;

  213.     FLEXCANb_CBT(_bus) = 0x800B3D4B;

  214.   }

  215.   if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode();

  216. }

  217. 

  218. FCTPFD_FUNC uint8_t FCTPFD_OPT::max_mailboxes() {

  219.   uint8_t mb_count = 0;

  220.   uint8_t block0 = (FLEXCANb_FDCTRL(_bus) & (3UL << 16)) >> 16;

  221.   uint8_t block1 = (FLEXCANb_FDCTRL(_bus) & (3UL << 19)) >> 19;

  222. 

  223.   const uint8_t sizes[4] = {32, 21, 12, 7};

  224.   mb_count = sizes[block0] + sizes[block1];

  225. 

  226.   if ( mb_count > FLEXCANb_MAXMB_SIZE(_bus) ) return FLEXCANb_MAXMB_SIZE(_bus);

  227.   return mb_count;

  228. }

  229. 

  230. FCTPFD_FUNC int FCTPFD_OPT::write(FLEXCAN_MAILBOX mb_num, const CANFD_message_t &msg) {

  231.   uint8_t mbsize = 0;

  232.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  233. 

  234.   if ( !((FLEXCAN_get_code(mbxAddr[0])) >> 3) ) return 0; /* not a transmit mailbox */

  235.   if ( msg.seq && FLEXCAN_get_code(mbxAddr[0]) != FLEXCAN_MB_CODE_TX_INACTIVE ) return 0; /* non blocking resend sequential frames */

  236.   uint32_t timeout = millis();

  237.   while ( FLEXCAN_get_code(mbxAddr[0]) != FLEXCAN_MB_CODE_TX_INACTIVE ) {

  238.     if ( millis() - timeout > 100 ) return 0;

  239.   }

  240.   writeTxMailbox(mb_num, msg);

  241.   return 1; // transmit entry accepted //

  242. }

  243. 

  244. FCTPFD_FUNC void FCTPFD_OPT::writeTxMailbox(uint8_t mb_num, const CANFD_message_t &frame) {

  245.   CANFD_message_t msg = frame;

  246.   writeIFLAGBit(mb_num);

  247.   uint8_t mbsize = 0;

  248.   uint32_t code = 0;

  249.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  250.   mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE);

  251.   mbxAddr[1] = (( msg.flags.extended ) ? ( msg.id & FLEXCAN_MB_ID_EXT_MASK ) : FLEXCAN_MB_ID_IDSTD(msg.id));

  252.   if ( msg.flags.extended ) code |= (3UL << 21);

  253.   for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) mbxAddr[2 + i] = (msg.buf[0 + i * 4] << 24) | (msg.buf[1 + i * 4] << 16) | (msg.buf[2 + i * 4] << 8) | msg.buf[3 + i * 4];

  254.   if ( msg.len > mbsize ) msg.len = mbsize;

  255.   code |= len_to_dlc(msg.len) << 16;

  256.   if ( msg.brs ) code |= (1UL << 30); // BRS

  257.   if ( msg.edl ) code |= (1UL << 31); // EDL

  258.   mbxAddr[0] = code | FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_ONCE);

  259. }

  260. 

  261. FCTPFD_FUNC uint8_t FCTPFD_OPT::len_to_dlc(uint8_t val) {

  262.   if ( val <= 8 );

  263.   else if ( val <= 12 ) val = 9;

  264.   else if ( val <= 16 ) val = 10;

  265.   else if ( val <= 20 ) val = 11;

  266.   else if ( val <= 24 ) val = 12;

  267.   else if ( val <= 32 ) val = 13;

  268.   else if ( val <= 48 ) val = 14;

  269.   else if ( val <= 64 ) val = 15;

  270.   return val;

  271. }

  272. 

  273. FCTPFD_FUNC bool FCTPFD_OPT::setMB(const FLEXCAN_MAILBOX &mb_num, const FLEXCAN_RXTX &mb_rx_tx, const FLEXCAN_IDE &ide) {

  274.   if ( mb_num >= max_mailboxes() ) return 0;

  275.   writeIMASKBit(mb_num, 0); /* immediately disable mailbox interrupt */

  276.   FLEXCAN_EnterFreezeMode();

  277.   FLEXCANb_RXIMR(_bus, mb_num) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // CLEAR MAILBOX MASK

  278.   FLEXCAN_ExitFreezeMode();

  279.   uint8_t mbsize = 0;

  280.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num , mbsize)));

  281.   FLEXCAN_get_code(mbxAddr[0]); // Reading Control Status atomically locks mailbox (if it is RX mode).

  282.   for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) mbxAddr[2 + i] = 0x0; /* clear mailbox data */

  283.   if ( ide == INACTIVE ) mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_INACTIVE);

  284.   else if ( mb_rx_tx == RX ) mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((ide != EXT) ? 0 : FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE);

  285.   else if ( mb_rx_tx == TX ) mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE);

  286.   mbxAddr[1] = 0; // clear ID

  287.   FLEXCANb_TIMER(_bus); /* reading timer unlocks individual mailbox */

  288.   writeIFLAGBit(mb_num); /* clear mailbox flag */

  289.   mb_filter_table[mb_num][0] = ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */

  290.   return 1;

  291. }

  292. 

  293. FCTPFD_FUNC uint8_t FCTPFD_OPT::dlc_to_len(uint8_t val) {

  294.   const uint8_t dlcTolen[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64};

  295.   return dlcTolen[val];

  296. }

  297. 

  298. FCTPFD_FUNC void FCTPFD_OPT::FLEXCAN_ExitFreezeMode() {

  299.   FLEXCANb_MCR(_bus) &= ~FLEXCAN_MCR_HALT;

  300.   while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK);

  301. }

  302. 

  303. FCTPFD_FUNC void FCTPFD_OPT::FLEXCAN_EnterFreezeMode() {

  304.   FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_FRZ | FLEXCAN_MCR_HALT;

  305.   while (!(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK));

  306. }

  307. 

  308. FCTPFD_FUNC void FCTPFD_OPT::softReset() {

  309.   FLEXCANb_MCR(_bus) |= FLEXCAN_MCR_SOFT_RST;

  310.   while (FLEXCANb_MCR(_bus) & FLEXCAN_MCR_SOFT_RST);

  311. }

  312. 

  313. FCTPFD_FUNC void FCTPFD_OPT::setTX(FLEXCAN_PINS pin) {

  314.   if ( _bus == CAN3 ) {

  315.     if ( pin == DEF ) {

  316.       IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_36 = 0x19; // pin31 T3B2

  317.       IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_36 = 0x10B0; // pin31 T3B2

  318.     }

  319.   }

  320.   if ( _bus == CAN2 ) {

  321.     if ( pin == DEF ) {

  322.       IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_02 = 0x10; // pin 1 T4B1+B2

  323.       IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B0_02 = 0x10B0; // pin 1 T4B1+B2

  324.     }

  325.   }

  326.   if ( _bus == CAN1 ) {

  327.     if ( pin == DEF ) {

  328.       IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B1_08 = 0x12; // pin 22 T4B1+B2

  329.       IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_08 = 0x10B0; // pin 22 T4B1+B2

  330.     }

  331.   }

  332. }

  333. 

  334. FCTPFD_FUNC void FCTPFD_OPT::setRX(FLEXCAN_PINS pin) {

  335.   /* DAISY REGISTER CAN3

  336.     00 GPIO_EMC_37_ALT9 — Selecting Pad: GPIO_EMC_37 for Mode: ALT9

  337.     01 GPIO_AD_B0_15_ALT8 — Selecting Pad: GPIO_AD_B0_15 for Mode: ALT8

  338.     10 GPIO_AD_B0_11_ALT8 — Selecting Pad: GPIO_AD_B0_11 for Mode: ALT8

  339.   */

  340.   /* DAISY REGISTER CAN2

  341.     00 GPIO_EMC_10_ALT3 — Selecting Pad: GPIO_EMC_10 for Mode: ALT3

  342.     01 GPIO_AD_B0_03_ALT0 — Selecting Pad: GPIO_AD_B0_03 for Mode: ALT0

  343.     10 GPIO_AD_B0_15_ALT6 — Selecting Pad: GPIO_AD_B0_15 for Mode: ALT6

  344.     11 GPIO_B1_09_ALT6 — Selecting Pad: GPIO_B1_09 for Mode: ALT6

  345.   */

  346.   /* DAISY REGISTER CAN1

  347.     00 GPIO_SD_B1_03_ALT4 — Selecting Pad: GPIO_SD_B1_03 for Mode: ALT4

  348.     01 GPIO_EMC_18_ALT3 — Selecting Pad: GPIO_EMC_18 for Mode: ALT3

  349.     10 GPIO_AD_B1_09_ALT2 — Selecting Pad: GPIO_AD_B1_09 for Mode: ALT2

  350.     11 GPIO_B0_03_ALT2 — Selecting Pad: GPIO_B0_03 for Mode: ALT2

  351.   */

  352.   if ( _bus == CAN3 ) {

  353.     if ( pin == DEF ) {

  354.       IOMUXC_CANFD_IPP_IND_CANRX_SELECT_INPUT = 0x00;

  355.       IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_37 = 0x19; // pin30 T3B2

  356.       IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_37 = 0x10B0; // pin30 T3B2

  357.     }

  358.   }

  359.   if ( _bus == CAN2 ) {

  360.     if ( pin == DEF ) {

  361.       IOMUXC_FLEXCAN2_RX_SELECT_INPUT = 0x01;

  362.       IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B0_03 = 0x10; // pin 0 T4B1+B2

  363.       IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B0_03 = 0x10B0; // pin 0 T4B1+B2

  364.     }

  365.   }

  366.   if ( _bus == CAN1 ) {

  367.     if ( pin == DEF ) {

  368.       IOMUXC_FLEXCAN1_RX_SELECT_INPUT = 0x02;

  369.       IOMUXC_SW_MUX_CTL_PAD_GPIO_AD_B1_09 = 0x12; // pin 23 T4B1+B2

  370.       IOMUXC_SW_PAD_CTL_PAD_GPIO_AD_B1_09 = 0x10B0; // pin 23 T4B1+B2

  371.     }

  372.   }

  373. }

  374. 

  375. FCTPFD_FUNC void FCTPFD_OPT::enableDMA(bool state) { /* only CAN3 supports this on 1062, untested */

  376.   bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK);

  377.   FLEXCAN_EnterFreezeMode();

  378.   ( !state ) ? FLEXCANb_MCR(_bus) &= ~0x8000 : FLEXCANb_MCR(_bus) |= 0x8000;

  379.   if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode();

  380. }

  381. 

  382. FCTPFD_FUNC void FCTPFD_OPT::writeIFLAG(uint64_t value) {

  383.   FLEXCANb_IFLAG2(_bus) = value >> 32;

  384.   FLEXCANb_IFLAG1(_bus) = value;

  385. }

  386. 

  387. FCTPFD_FUNC void FCTPFD_OPT::writeIFLAGBit(uint8_t mb_num) {

  388.   if ( mb_num < 32 ) FLEXCANb_IFLAG1(_bus) |= (1UL << mb_num);

  389.   else FLEXCANb_IFLAG2(_bus) |= (1UL << (mb_num - 32));

  390. }

  391. 

  392. FCTPFD_FUNC void FCTPFD_OPT::writeIMASK(uint64_t value) {

  393.   FLEXCANb_IMASK2(_bus) = value >> 32;

  394.   FLEXCANb_IMASK1(_bus) = value;

  395. }

  396. 

  397. FCTPFD_FUNC void FCTPFD_OPT::writeIMASKBit(uint8_t mb_num, bool set) {

  398.   if ( mb_num < 32 ) (( set ) ? FLEXCANb_IMASK1(_bus) |= (1UL << mb_num) : FLEXCANb_IMASK1(_bus) &= ~(1UL << mb_num));

  399.   else (( set ) ? FLEXCANb_IMASK2(_bus) |= (1UL << (mb_num - 32)) : FLEXCANb_IMASK2(_bus) &= ~(1UL << (mb_num - 32)));

  400. }

  401. 

  402. static void flexcan_isr_can3fd() {

  403.   if ( _CAN3 ) _CAN3->flexcan_interrupt();

  404. }

  405. 

  406. FCTPFD_FUNC void FCTPFD_OPT::enableMBInterrupts(bool status) {

  407.   FLEXCAN_EnterFreezeMode();

  408.   for ( uint8_t mb_num = 0, mbsize = 0; mb_num < max_mailboxes(); mb_num++ ) {

  409.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  410.     if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) continue; // skip TX mailboxes

  411.     enableMBInterrupt((FLEXCAN_MAILBOX)mb_num, status);

  412.   }

  413.   FLEXCAN_ExitFreezeMode();

  414. }

  415. 

  416. FCTPFD_FUNC void FCTPFD_OPT::enableMBInterrupt(const FLEXCAN_MAILBOX &mb_num, bool status) {

  417.   FLEXCAN_EnterFreezeMode();

  418.   if ( status ) writeIMASKBit(mb_num); /* enable mailbox interrupt */

  419.   else writeIMASKBit(mb_num, 0); /* disable mailbox interrupt */

  420.   FLEXCAN_ExitFreezeMode();

  421. }

  422. 

  423. FCTPFD_FUNC void FCTPFD_OPT::onReceive(const FLEXCAN_MAILBOX &mb_num, _MBFD_ptr handler) {

  424.   if ( FIFO == mb_num ) {

  425.     _mbHandlers[0] = handler;

  426.     return;

  427.   }

  428.   _mbHandlers[mb_num] = handler;

  429. }

  430. 

  431. FCTPFD_FUNC void FCTPFD_OPT::onReceive(_MBFD_ptr handler) {

  432.   _mainHandler = handler;

  433. }

  434. 

  435. FCTPFD_FUNC void FCTPFD_OPT::mbCallbacks(const FLEXCAN_MAILBOX &mb_num, const CANFD_message_t &msg) {

  436.   if ( mb_num == FIFO ) {

  437.     if ( _mbHandlers[0] ) _mbHandlers[0](msg);

  438.     return;

  439.   }

  440.   if ( _mbHandlers[mb_num] ) _mbHandlers[mb_num](msg);

  441. }

  442. 

  443. FCTPFD_FUNC void FCTPFD_OPT::flexcan_interrupt() {

  444.   CANFD_message_t msg; // setup a temporary storage buffer

  445.   uint64_t imask = readIMASK(), iflag = readIFLAG();

  446. 

  447.   for ( uint8_t mb_num = 0, mbsize = 0; mb_num < max_mailboxes(); mb_num++ ) {

  448.     if (!(imask & (1ULL << mb_num))) continue; /* don't read non-interrupt mailboxes */

  449.     if (!(iflag & (1ULL << mb_num))) continue; /* don't read unflagged mailboxes */

  450.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  451.     uint32_t code = mbxAddr[0];

  452.     if ( ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_FULL ) ||

  453.          ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) ) {

  454.       msg.flags.extended = (bool)(code & (1UL << 21));

  455.       msg.edl = (bool)(code & (1UL << 31));

  456.       msg.brs = (bool)(code & (1UL << 30));

  457.       msg.esi = (bool)(code & (1UL << 29));

  458.       msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18);

  459.       if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1;

  460.       msg.len = dlc_to_len((code & 0xF0000) >> 16);

  461.       msg.mb = mb_num;

  462.       msg.timestamp = code & 0xFFFF;

  463.       msg.bus = busNumber;

  464.       for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d));

  465.       mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((msg.flags.extended) ? (FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE) : 0);

  466.       (void)FLEXCANb_TIMER(_bus);

  467.       writeIFLAGBit(mb_num);

  468.       if ( filter_match((FLEXCAN_MAILBOX)mb_num, msg.id) ) struct2queueRx(msg); /* store frame in queue */

  469.       frame_distribution(msg);

  470.       ext_outputFD1(msg);

  471.       ext_outputFD2(msg);

  472.       ext_outputFD3(msg);

  473.     }

  474.   }

  475.   FLEXCANb_ESR1(_bus) = FLEXCANb_ESR1(_bus);

  476. }

  477. 

  478. FCTPFD_FUNC void FCTPFD_OPT::struct2queueRx(const CANFD_message_t &msg) {

  479.   uint8_t buf[sizeof(CANFD_message_t)];

  480.   memmove(buf, &msg, sizeof(msg));

  481.   rxBuffer.push_back(buf, sizeof(CANFD_message_t));

  482. }

  483. 

  484. FCTPFD_FUNC void FCTPFD_OPT::struct2queueTx(const CANFD_message_t &msg) {

  485.   uint8_t buf[sizeof(CANFD_message_t)];

  486.   memmove(buf, &msg, sizeof(msg));

  487.   txBuffer.push_back(buf, sizeof(CANFD_message_t));

  488. }

  489. 

  490. FCTPFD_FUNC int FCTPFD_OPT::write(const CANFD_message_t &msg) {

  491.   if ( msg.seq ) {

  492.     if ( !write((FLEXCAN_MAILBOX)getFirstTxBox(), msg) ) struct2queueTx(msg);

  493.     return 1;

  494.   }

  495.   for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) {

  496.     if (readIMASK() & (1ULL << i)) continue; /* don't write interrupt enabled mailboxes */

  497.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize)));

  498.     if ( FLEXCAN_get_code(mbxAddr[0]) == FLEXCAN_MB_CODE_TX_INACTIVE ) {

  499.       writeTxMailbox(i, msg);

  500.       return 1; /* transmit entry accepted */

  501.     }

  502.   }

  503.   return 0; /* transmit entry failed, no mailboxes or queues available */

  504. }

  505. 

  506. FCTPFD_FUNC uint64_t FCTPFD_OPT::events() {

  507.   if ( rxBuffer.size() ) {

  508.     CANFD_message_t frame;

  509.     uint8_t buf[sizeof(CANFD_message_t)];

  510.     rxBuffer.pop_front(buf, sizeof(CANFD_message_t));

  511.     memmove(&frame, buf, sizeof(frame));

  512.     if ( _mbHandlers[frame.mb] ) _mbHandlers[frame.mb](frame);

  513.     if ( _mainHandler ) _mainHandler(frame);

  514.   }

  515.   if ( txBuffer.size() ) {

  516.     CANFD_message_t frame;

  517.     uint8_t buf[sizeof(CANFD_message_t)];

  518.     txBuffer.peek_front(buf, sizeof(CANFD_message_t));

  519.     memmove(&frame, buf, sizeof(frame));

  520.     if ( write((FLEXCAN_MAILBOX)getFirstTxBox(), frame) ) txBuffer.pop_front();

  521.   }

  522.   return (uint64_t)(rxBuffer.size() << 12) | txBuffer.size();

  523. }

  524. 

  525. FCTPFD_FUNC int FCTPFD_OPT::read(CANFD_message_t &msg) {

  526.   return readMB(msg);

  527. }

  528. 

  529. FCTPFD_FUNC int FCTPFD_OPT::readMB(CANFD_message_t &msg) {

  530.   uint8_t cycle_limit = 3;

  531.   for ( uint8_t mbsize = 0; mailbox_reader_increment <= max_mailboxes(); ++mailbox_reader_increment ) {

  532.     if ( mailbox_reader_increment >= max_mailboxes() ) {

  533.       mailbox_reader_increment = 0;

  534.       if ( !--cycle_limit ) return 0;

  535.     }

  536.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mailbox_reader_increment , mbsize)));

  537.     if ((readIMASK() & (1ULL << mailbox_reader_increment))) continue; /* don't read interrupt enabled mailboxes */

  538.     uint32_t code = mbxAddr[0];

  539.     if ( (FLEXCAN_get_code(code) >> 3) ) continue; /* skip TX mailboxes */

  540.     // if (!(code & 0x600000) && !(readIFLAG() & (1ULL << mailbox_reader_increment))) continue; /* don't read unflagged mailboxes, errata: extended mailboxes iflags do not work in poll mode, must check CS field */

  541.     if ( ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_FULL ) ||

  542.          ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) ) {

  543.       msg.flags.extended = (bool)(code & (1UL << 21));

  544.       msg.edl = (bool)(code & (1UL << 31));

  545.       msg.brs = (bool)(code & (1UL << 30));

  546.       msg.esi = (bool)(code & (1UL << 29));

  547.       msg.id = (mbxAddr[1] & 0x1FFFFFFF) >> ((msg.flags.extended) ? 0 : 18);

  548.       if ( FLEXCAN_get_code(code) == FLEXCAN_MB_CODE_RX_OVERRUN ) msg.flags.overrun = 1;

  549.       msg.len = dlc_to_len((code & 0xF0000) >> 16);

  550.       msg.mb = mailbox_reader_increment++;

  551.       msg.timestamp = code & 0xFFFF;

  552.       msg.bus = busNumber;

  553.       for ( uint8_t i = 0; i < (mbsize >> 2); i++ ) for ( int8_t d = 0; d < 4 ; d++ ) msg.buf[(4 * i) + 3 - d] = (uint8_t)(mbxAddr[2 + i] >> (8 * d));

  554.       mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((msg.flags.extended) ? (FLEXCAN_MB_CS_SRR | FLEXCAN_MB_CS_IDE) : 0);

  555.       (void)FLEXCANb_TIMER(_bus); /* Read free-running timer to unlock Rx Message Buffer. */

  556.       writeIFLAGBit(msg.mb);

  557.       frame_distribution(msg);

  558.       if ( filter_match((FLEXCAN_MAILBOX)msg.mb, msg.id) ) return 1;

  559.     }

  560.   } 

  561.   return 0; /* no messages available */

  562. }

  563. 

  564. FCTPFD_FUNC void FCTPFD_OPT::enableFIFO(bool status) {

  565.   bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK);

  566.   FLEXCAN_EnterFreezeMode();

  567.   writeIMASK(0ULL); // disable all FIFO/MB Interrupts

  568.   FLEXCANb_RXMGMASK(_bus) = FLEXCANb_RXFGMASK(_bus) = 0;

  569.   writeIFLAG(readIFLAG()); // (all bits reset when written back)

  570.   FLEXCANb_MCR(_bus) &= ~0x20008000; /* we disable DMA, Legacy FIFO (we never use this in FD mode!) */

  571.   for (uint8_t i = 0; i < max_mailboxes(); i++ ) FLEXCANb_RXIMR(_bus, i) = 0 | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // CLEAR MAILBOX MASKS (RXIMR)

  572.   for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) { /* clear valid MB codes from memory regions */

  573.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize)));

  574.     mbxAddr[0] = mbxAddr[1] = 0;

  575.   }

  576. 

  577.   if ( status ) { /* enable FIFO */

  578.     // TODO: FD FIFO, ### WARNING: NXP claims, after being tested to fail despite documented and advertised, that the 1062 does NOT support FD FIFO mode!!!

  579.   }

  580.   else { // FIFO disabled default setup of mailboxes, 0-7 RX, 8-15 TX

  581.     for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++ ) { // clear all mailboxes

  582.       volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize)));

  583.       if ( i < max_mailboxes() / 2 ) {

  584.         mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_RX_EMPTY) | ((i < (max_mailboxes() / 4)) ? 0 : FLEXCAN_MB_CS_IDE | FLEXCAN_MB_CS_SRR);

  585.         FLEXCANb_RXIMR(_bus, i) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR)

  586.       }

  587.       else {

  588.         mbxAddr[0] = FLEXCAN_MB_CS_CODE(FLEXCAN_MB_CODE_TX_INACTIVE);

  589.       }

  590.     }

  591.   }

  592.   if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode();

  593. }

  594. 

  595. FCTPFD_FUNC void FCTPFD_OPT::setMBFilterProcessing(FLEXCAN_MAILBOX mb_num, uint32_t filter_id, uint32_t calculated_mask) {

  596.   bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK);

  597.   FLEXCAN_EnterFreezeMode();

  598.   uint8_t mbsize = 0;

  599.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  600.   FLEXCANb_RXIMR(_bus, mb_num) = calculated_mask | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0);

  601.   mbxAddr[1] = ((!(mbxAddr[0] & FLEXCAN_MB_CS_IDE)) ? FLEXCAN_MB_ID_IDSTD(filter_id) : FLEXCAN_MB_ID_IDEXT(filter_id));

  602.   if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode();

  603. }

  604. 

  605. FCTPFD_FUNC bool FCTPFD_OPT::setMBFilterRange(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2) {

  606.   if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */

  607.   uint8_t mbsize = 0;

  608.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  609.   if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ 

  610.   if ( id1 > id2 || ((id2 > id1) && (id2-id1>1000)) || !id1 || !id2 ) return 0; /* don't play around... */

  611.   uint32_t stage1 = id1, stage2 = id1;

  612.   for ( uint32_t i = id1 + 1; i <= id2; i++ ) {

  613.     stage1 |= i; stage2 &= i;

  614.   }

  615.   uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD( (stage1 ^ stage2) ^ 0x1FFFFFFF ) : FLEXCAN_MB_ID_IDEXT( (stage1 ^ stage2) ^ 0x1FFFFFFF );

  616.   setMBFilterProcessing(mb_num,id1,mask);

  617.   filter_store(FLEXCAN_RANGE, mb_num, 2, id1, id2, 0, 0, 0, mask);

  618.   return 1;

  619. }

  620. 

  621. FCTPFD_FUNC void FCTPFD_OPT::filter_store(FLEXCAN_FILTER_TABLE type, FLEXCAN_MAILBOX mb_num, uint32_t id_count, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5, uint32_t mask) {

  622.   mb_filter_table[mb_num][0] = mb_num; // first 7 bits reserved for MB

  623.   mb_filter_table[mb_num][0] |= (id_count << 7); // we store the quantity of ids after the mailboxes 

  624.   /* bit 28: filter enabled */

  625.   mb_filter_table[mb_num][0] |= (type << 29); // we reserve 3 upper bits for type

  626.   uint8_t mbsize;

  627.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  628.   mb_filter_table[mb_num][0] |= ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */

  629.   mb_filter_table[mb_num][1] = id1; // id1

  630.   mb_filter_table[mb_num][2] = id2; // id2

  631.   mb_filter_table[mb_num][3] = id3; // id3

  632.   mb_filter_table[mb_num][4] = id4; // id4

  633.   mb_filter_table[mb_num][5] = id5; // id5

  634.   mb_filter_table[mb_num][6] = mask; // mask

  635. }

  636. 

  637. FCTPFD_FUNC void FCTPFD_OPT::enhanceFilter(FLEXCAN_MAILBOX mb_num) {

  638.   if ( !(mb_filter_table[mb_num][0] & 0xE0000000) ) return;

  639.   mb_filter_table[mb_num][0] |= (1UL << 28); /* enable enhancement */

  640. }

  641. 

  642. FCTPFD_FUNC bool FCTPFD_OPT::filter_match(FLEXCAN_MAILBOX mb_num, uint32_t id) {

  643.   if ( !(mb_filter_table[mb_num][0] & 0x10000000) ) return 1;

  644.   if ( (mb_filter_table[mb_num][0] >> 29) == FLEXCAN_MULTI ) {

  645.     for ( uint8_t i = 0; i < ((mb_filter_table[mb_num][0] & 0x380) >> 7); i++) if ( id == mb_filter_table[mb_num][i+1] ) return 1;

  646.   }

  647.   else if ( (mb_filter_table[mb_num][0] >> 29) == FLEXCAN_RANGE ) {

  648.     if ( id >= mb_filter_table[mb_num][1] && id <= mb_filter_table[mb_num][2] ) return 1;

  649.   }

  650.   return 0;

  651. }

  652. 

  653. FCTPFD_FUNC void FCTPFD_OPT::frame_distribution(CANFD_message_t &msg) {

  654.   if ( !distribution ) return; /* distribution not enabled */

  655.   CANFD_message_t frame = msg;

  656.   for ( uint8_t i = 0; i < max_mailboxes(); i++ ) {

  657.     if ( frame.mb == i ) continue; // don't distribute to same mailbox

  658.     if ( !(mb_filter_table[i][0] & 0xE0000000) ) continue; // skip unset filters

  659.     if ( (bool)(mb_filter_table[i][0] & (1UL << 27)) != msg.flags.extended ) continue; /* extended flag check */

  660.     if ( (mb_filter_table[i][0] >> 29) == FLEXCAN_MULTI ) {

  661.       for ( uint8_t p = 0; p < ((mb_filter_table[i][0] & 0x380) >> 7); p++) {

  662.         if ( frame.id == mb_filter_table[i][p+1] ) {

  663.           frame.mb = i;

  664.           struct2queueRx(frame);

  665.         }

  666.       }

  667.     }

  668.     else if ( (mb_filter_table[i][0] >> 29) == FLEXCAN_RANGE ) {

  669.       if ( frame.id >= mb_filter_table[i][1] && frame.id <= mb_filter_table[i][2] ) {

  670.         frame.mb = i;

  671.         struct2queueRx(frame);

  672.       }

  673.     }

  674.   }

  675. }

  676. 

  677. FCTPFD_FUNC void FCTPFD_OPT::setMBFilter(FLEXCAN_FLTEN input) {

  678.   bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK);

  679.   FLEXCAN_EnterFreezeMode();

  680.   for (uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++) {

  681.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize)));

  682.     if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) continue; /* skip TX mailboxes */

  683.     if ( input == ACCEPT_ALL ) FLEXCANb_RXIMR(_bus, i) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR)

  684.     if ( input == REJECT_ALL ) FLEXCANb_RXIMR(_bus, i) = ~0UL; // (RXIMR)

  685.     mbxAddr[1] = 0UL;

  686.     mb_filter_table[i][0] = ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */

  687.   }

  688.   if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode();

  689. }

  690. 

  691. FCTPFD_FUNC void FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, FLEXCAN_FLTEN input) {

  692.   if ( mb_num >= max_mailboxes() ) return; /* mailbox not available */

  693.   uint8_t mbsize = 0;

  694.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  695.   if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return; /* exit on TX mailbox */ 

  696.   bool frz_flag_negate = !(FLEXCANb_MCR(_bus) & FLEXCAN_MCR_FRZ_ACK);

  697.   FLEXCAN_EnterFreezeMode();

  698.   if ( input == ACCEPT_ALL ) FLEXCANb_RXIMR(_bus, mb_num) = 0UL | ((FLEXCANb_CTRL2(_bus) & FLEXCAN_CTRL2_EACEN) ? (1UL << 30) : 0); // (RXIMR)

  699.   if ( input == REJECT_ALL ) FLEXCANb_RXIMR(_bus, mb_num) = ~0UL; // (RXIMR)

  700.   mbxAddr[1] = 0UL;

  701.   mb_filter_table[mb_num][0] = ( ((mbxAddr[0] & 0x600000) ? 1UL : 0UL) << 27); /* extended flag check */

  702.   if ( frz_flag_negate ) FLEXCAN_ExitFreezeMode();

  703. }

  704. 

  705. FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1) {

  706.   if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */

  707.   uint8_t mbsize = 0;

  708.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  709.   if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ 

  710.   uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1) ^ (id1)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1) ^ (id1)) ^ 0x1FFFFFFF);

  711.   setMBFilterProcessing(mb_num,id1,mask);

  712.   filter_store(FLEXCAN_MULTI, mb_num, 1, id1, 0, 0, 0, 0, mask);

  713.   return 1;

  714. }

  715. 

  716. FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2) {

  717.   if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */

  718.   uint8_t mbsize = 0;

  719.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  720.   if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ 

  721.   uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2) ^ (id1 & id2)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2) ^ (id1 & id2)) ^ 0x1FFFFFFF);

  722.   setMBFilterProcessing(mb_num,id1,mask);

  723.   filter_store(FLEXCAN_MULTI, mb_num, 2, id1, id2, 0, 0, 0, mask);

  724.   return 1;

  725. }

  726. 

  727. FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3) {

  728.   if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */

  729.   uint8_t mbsize = 0;

  730.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  731.   if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ 

  732.   uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2 | id3) ^ (id1 & id2 & id3)) ^ 0x1FFFFFFF);

  733.   setMBFilterProcessing(mb_num,id1,mask);

  734.   filter_store(FLEXCAN_MULTI, mb_num, 3, id1, id2, id3, 0, 0, mask);

  735.   return 1;

  736. }

  737. 

  738. FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4) {

  739.   if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */

  740.   uint8_t mbsize = 0;

  741.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  742.   if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ 

  743.   uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2 | id3 | id4) ^ (id1 & id2 & id3 & id4)) ^ 0x1FFFFFFF);

  744.   setMBFilterProcessing(mb_num,id1,mask);

  745.   filter_store(FLEXCAN_MULTI, mb_num, 4, id1, id2, id3, id4, 0, mask);

  746.   return 1;

  747. }

  748. 

  749. FCTPFD_FUNC bool FCTPFD_OPT::setMBFilter(FLEXCAN_MAILBOX mb_num, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4, uint32_t id5) {

  750.   if ( mb_num >= max_mailboxes() ) return 0; /* mailbox not available */

  751.   uint8_t mbsize = 0;

  752.   volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(mb_num, mbsize)));

  753.   if ( (FLEXCAN_get_code(mbxAddr[0]) >> 3) ) return 0; /* exit on TX mailbox */ 

  754.   uint32_t mask = ( !(mbxAddr[0] & FLEXCAN_MB_CS_IDE) ) ? FLEXCAN_MB_ID_IDSTD(((id1 | id2 | id3 | id4 | id5) ^ (id1 & id2 & id3 & id4 & id5)) ^ 0x7FF) : FLEXCAN_MB_ID_IDEXT(((id1 | id2 | id3 | id4 | id5) ^ (id1 & id2 & id3 & id4 & id5)) ^ 0x1FFFFFFF);

  755.   setMBFilterProcessing(mb_num,id1,mask);

  756.   filter_store(FLEXCAN_MULTI, mb_num, 5, id1, id2, id3, id4, id5, mask);

  757.   return 1;

  758. }

  759. 

  760. FCTPFD_FUNC void FCTPFD_OPT::mailboxStatus() {

  761.   Serial.print("FIFO Disabled\n\tMailboxes:\n");

  762.   for ( uint8_t i = 0, mbsize = 0; i < max_mailboxes(); i++ ) {

  763.     volatile uint32_t *mbxAddr = &(*(volatile uint32_t*)(mailbox_offset(i, mbsize)));

  764.     switch ( FLEXCAN_get_code(mbxAddr[0]) ) {

  765.       case 0b0000: {

  766.           Serial.printf("\t\tMB%u%s\n", i, " code: RX_INACTIVE");

  767.           break;

  768.         }

  769.       case 0b0100: {

  770.           Serial.printf("\t\tMB%u%s%s\n", i, " code: RX_EMPTY",((mbxAddr[0] & FLEXCAN_MB_CS_IDE)?"\t(Extended Frame)":"\t(Standard Frame)"));

  771.           break;

  772.         }

  773.       case 0b0010: {

  774.           Serial.printf("\t\tMB%u%s\n", i, " code: RX_FULL");

  775.           break;

  776.         }

  777.       case 0b0110: {

  778.           Serial.printf("\t\tMB%u%s\n", i, " code: RX_OVERRUN");

  779.           break;

  780.         }

  781.       case 0b1010: {

  782.           Serial.printf("\t\tMB%u%s\n", i, " code: RX_ANSWER");

  783.           break;

  784.         }

  785.       case 0b0001: {

  786.           Serial.printf("\t\tMB%u%s\n", i, " code: RX_BUSY");

  787.           break;

  788.         }

  789.       case 0b1000: {

  790.           Serial.printf("\t\tMB%u%s\n", i, " code: TX_INACTIVE");

  791.           break;

  792.         }

  793.       case 0b1001: {

  794.           Serial.printf("\t\tMB%u%s\n", i, " code: TX_ABORT");

  795.           break;

  796.         }

  797.       case 0b1100: {

  798.           Serial.printf("\t\tMB%u%s", i, " code: TX_DATA (Transmitting)");

  799.           uint32_t extid = (mbxAddr[0] & FLEXCAN_MB_CS_IDE);

  800.           (extid) ? Serial.print("(Extended Frame)") : Serial.print("(Standard Frame)");

  801.           uint32_t dataIn = mbxAddr[2];

  802.           uint32_t id = (FLEXCANb_MBn_ID(_bus, i) & FLEXCAN_MB_ID_EXT_MASK);

  803.           if (!extid) id >>= FLEXCAN_MB_ID_STD_BIT_NO;

  804.           Serial.print("(ID: 0x"); Serial.print(id, HEX); Serial.print(")");

  805.           Serial.print("(Payload: "); Serial.print((uint8_t)(dataIn >> 24), HEX);

  806.           Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX);

  807.           Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX);

  808.           Serial.print(" "); Serial.print((uint8_t)dataIn, HEX);

  809.           dataIn = mbxAddr[3];

  810.           Serial.print(" "); Serial.print((uint8_t)(dataIn >> 24), HEX);

  811.           Serial.print(" "); Serial.print((uint8_t)(dataIn >> 16), HEX);

  812.           Serial.print(" "); Serial.print((uint8_t)(dataIn >> 8), HEX);

  813.           Serial.print(" "); Serial.print((uint8_t)dataIn, HEX);

  814.           Serial.println(")");

  815.           break;

  816.         }

  817.       case 0b1110: {

  818.           Serial.printf("\t\tMB%u%s\n", i, " code: TX_ANSWER");

  819.           break;

  820.         }

  821.     }

  822.   } // for loop

  823. }

  824. 

  825. extern void __attribute__((weak)) ext_outputFD1(const CANFD_message_t &msg);

  826. extern void __attribute__((weak)) ext_outputFD2(const CANFD_message_t &msg);

  827. extern void __attribute__((weak)) ext_outputFD3(const CANFD_message_t &msg);