spi, can and general improvements

SPI:
-move to single transfer read
-clear interrupt register on read
-perform multibyte read transfers with DMA
-make frame format consistant

CAN:
-move from mailbox to CAN FIFO
-implement buffering
-support for reading multiple frames per transfer

General:
-use PLL at 100MHz
-remove debug task
-do not compile release with debug console enabled
-bumped version to 0.13

Signed-off-by: Dominik Sliwa <dominik.sliwa@toradex.com>

16 files changed, 394 insertions, 539 deletions

diff --git a/.cproject b/.cproject
index 53f1863..c40e73a 100644
--- a/.cproject
+++ b/.cproject
@@ -188,6 +188,8 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash.1723171499" name="Create flash image" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize.293528197" name="Print size" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.allwarn.1588021732" name="Enable all common warnings (-Wall)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.allwarn" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto.1302385297" name="Link-time optimizer (-flto)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pedantic.301387096" name="Pedantic (-pedantic)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pedantic" value="false" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform.883219432" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
 							<builder buildPath="${workspace_loc:/k20_tester}/Release" id="ilg.gnuarmeclipse.managedbuild.cross.builder.1075071058" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="ilg.gnuarmeclipse.managedbuild.cross.builder"/>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.1436178854" name="Cross ARM GNU Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index d48dd4b..0acd25f 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1090621813304243453" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-655446207629780334" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1056822376545845571" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-648854177427414568" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/board/clock_config.c b/board/clock_config.c
index 16bfe39..4da9ef5 100644
--- a/board/clock_config.c
+++ b/board/clock_config.c
@@ -54,6 +54,7 @@
 #define OSC_ER_CLK_DISABLE                                0U  /*!< Disable external reference clock */
 #define SIM_OSC32KSEL_OSC32KCLK_CLK                       0U  /*!< OSC32KSEL select: OSC32KCLK clock */
 #define SIM_PLLFLLSEL_MCGFLLCLK_CLK                       0U  /*!< PLLFLL select: MCGFLLCLK clock */
+#define SIM_PLLFLLSEL_MCGPLLCLK_CLK                       1U  /*!< PLLFLL select: MCGPLLCLK clock */
 #define BOARD_BOOTCLOCKRUN_CORE_CLOCK             100000000U  /*!< Core clock frequency: 100000000Hz */
 
 /*******************************************************************************
@@ -139,9 +140,9 @@ const mcg_config_t mcgConfig_BOARD_BootClockRUN =
     };
 const sim_clock_config_t simConfig_BOARD_BootClockRUN =
     {
-        .pllFllSel = SIM_PLLFLLSEL_MCGFLLCLK_CLK, /* PLLFLL select: MCGFLLCLK clock */
+        .pllFllSel = SIM_PLLFLLSEL_MCGPLLCLK_CLK, /* PLLFLL select: MCGPLLCLK clock */
         .er32kSrc = SIM_OSC32KSEL_OSC32KCLK_CLK,  /* OSC32KSEL select: OSC32KCLK clock */
-        .clkdiv1 = 0x1130000U,                    /* SIM_CLKDIV1 - OUTDIV1: /1, OUTDIV2: /2, OUTDIV3: /2, OUTDIV4: /4 */
+        .clkdiv1 = 0x01130000U,                    /* SIM_CLKDIV1 - OUTDIV1: /1, OUTDIV2: /2, OUTDIV3: /2, OUTDIV4: /4 */
     };
 const osc_config_t oscConfig_BOARD_BootClockRUN =
     {
diff --git a/drivers/fsl_dspi.c b/drivers/fsl_dspi.c
index dcd93aa..0da640a 100644
--- a/drivers/fsl_dspi.c
+++ b/drivers/fsl_dspi.c
@@ -1248,9 +1248,9 @@ status_t DSPI_SlaveTransferNonBlocking(SPI_Type *base, dspi_slave_handle_t *hand
 
     handle->errorCount = 0;
 
-    uint8_t whichCtar = (transfer->configFlags & DSPI_SLAVE_CTAR_MASK) >> DSPI_SLAVE_CTAR_SHIFT;
-    handle->bitsPerFrame =
-        (((base->CTAR_SLAVE[whichCtar]) & SPI_CTAR_SLAVE_FMSZ_MASK) >> SPI_CTAR_SLAVE_FMSZ_SHIFT) + 1;
+    //uint8_t whichCtar = (transfer->configFlags & DSPI_SLAVE_CTAR_MASK) >> DSPI_SLAVE_CTAR_SHIFT;
+    handle->bitsPerFrame = 8;
+        //(((base->CTAR_SLAVE[whichCtar]) & SPI_CTAR_SLAVE_FMSZ_MASK) >> SPI_CTAR_SLAVE_FMSZ_SHIFT) + 1;
 
     DSPI_StopTransfer(base);
 
@@ -1316,9 +1316,11 @@ static void DSPI_SlaveTransferFillUpTxFifo(SPI_Type *base, dspi_slave_handle_t *
         /* Transmit data */
         if (handle->remainingSendByteCount > 0)
         {
+#if 0
             /* Have data to transmit, update the transmit data and push to FIFO */
             if (handle->bitsPerFrame <= 8)
             {
+#endif
                 /* bits/frame is 1 byte */
                 if (handle->txData)
                 {
@@ -1333,6 +1335,7 @@ static void DSPI_SlaveTransferFillUpTxFifo(SPI_Type *base, dspi_slave_handle_t *
 
                 /* Decrease remaining dataSize */
                 --handle->remainingSendByteCount;
+#if 0
             }
             /* bits/frame is 2 bytes */
             else
@@ -1375,6 +1378,7 @@ static void DSPI_SlaveTransferFillUpTxFifo(SPI_Type *base, dspi_slave_handle_t *
                     transmitData = (uint16_t)((uint16_t)(dummyPattern) << 8) | dummyPattern;
                 }
             }
+#endif
         }
         else
         {
@@ -1440,8 +1444,7 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
 {
     assert(handle);
 
-    uint8_t dummyPattern = DSPI_DUMMY_DATA;
-    uint32_t dataReceived;
+    volatile uint32_t dataReceived;
     uint32_t dataSend = 0;
 
     /* Because SPI protocol is synchronous, the number of bytes that that slave received from the
@@ -1462,12 +1465,15 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
             DSPI_ClearStatusFlags(base, kDSPI_RxFifoDrainRequestFlag);
 
             /* If bits/frame is one byte */
+#if 0
             if (handle->bitsPerFrame <= 8)
             {
+#endif
                 if (handle->rxData)
                 {
-                    if ((handle->totalByteCount - handle->remainingReceiveByteCount) == 1){
-                    	if ( *(handle->rxData - 1) ==  APALIS_TK1_K20_BULK_WRITE_INST) {
+                    if ((handle->totalByteCount - handle->remainingReceiveByteCount) == 2){
+
+                    	if ( *(handle->rxData - 2) ==  APALIS_TK1_K20_BULK_WRITE_INST) {
                     		handle->remainingReceiveByteCount += dataReceived;
                     		handle->totalByteCount += dataReceived;
                     		handle->remainingSendByteCount += dataReceived;
@@ -1478,25 +1484,35 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
                     ++handle->rxData;
                 }
 
-                /* Descrease remaining receive byte count */
+                /* Decrease remaining receive byte count */
                 --handle->remainingReceiveByteCount;
-
+        	if (handle->remainingSendByteCount == 0){
+                        		if ( *(handle->rxData - 2) ==  APALIS_TK1_K20_READ_INST)
+                        		{
+                                    		base->PUSHR_SLAVE = registers[dataReceived];
+                        		}
+                        		else
+                        		{
+                        			base->PUSHR_SLAVE = 0x55;
+                        		}
+                        	}
                 if (handle->remainingSendByteCount > 0)
                 {
-                    if (handle->txData)
-                    {
-                        dataSend = *handle->txData;
-                        ++handle->txData;
-                    }
-                    else
-                    {
-                        dataSend = dummyPattern;
-                    }
+                		if (handle->txData)
+                		{
+                			dataSend = *handle->txData;
+                			++handle->txData;
+                		}
+                		else
+                		{
+                			dataSend = dataReceived;
+                		}
 
                     --handle->remainingSendByteCount;
                     /* Write the data to the DSPI data register */
                     base->PUSHR_SLAVE = dataSend;
                 }
+#if 0
             }
             else /* If bits/frame is 2 bytes */
             {
@@ -1572,6 +1588,7 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
                     base->PUSHR_SLAVE = dataSend;
                 }
             }
+#endif
             /* Try to clear TFFF by writing a one to it; it will not clear if TX FIFO not full */
             DSPI_ClearStatusFlags(base, kDSPI_TxFifoFillRequestFlag);
 
@@ -1585,6 +1602,7 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
     if ((handle->remainingReceiveByteCount == 0) || (handle->state == kDSPI_Error))
     {
         /* Other cases, stop the transfer. */
+	dataReceived = base->POPR;
         DSPI_SlaveTransferComplete(base, handle);
         return;
     }
diff --git a/drivers/fsl_dspi.h b/drivers/fsl_dspi.h
index 5dd96af..ae89c6d 100644
--- a/drivers/fsl_dspi.h
+++ b/drivers/fsl_dspi.h
@@ -50,7 +50,7 @@
 
 #ifndef DSPI_DUMMY_DATA
 /*! @brief DSPI dummy data if there is no Tx data.*/
-#define DSPI_DUMMY_DATA (0x00U) /*!< Dummy data used for Tx if there is no txData. */
+#define DSPI_DUMMY_DATA (0xBBU) /*!< Dummy data used for Tx if there is no txData. */
 #endif
 
 /*! @brief Status for the DSPI driver.*/
diff --git a/drivers/fsl_dspi_edma.c b/drivers/fsl_dspi_edma.c
index ef0d151..fa26b2f 100644
--- a/drivers/fsl_dspi_edma.c
+++ b/drivers/fsl_dspi_edma.c
@@ -1183,30 +1183,55 @@ status_t DSPI_SlaveTransferEDMA(SPI_Type *base, dspi_slave_edma_handle_t *handle
 
     return kStatus_Success;
 }
-
+#if 0
 static void EDMA_DspiSlaveCallback(edma_handle_t *edmaHandle,
                                    void *g_dspiEdmaPrivateHandle,
                                    bool transferDone,
                                    uint32_t tcds)
 {
+
     assert(edmaHandle);
     assert(g_dspiEdmaPrivateHandle);
 
     dspi_slave_edma_private_handle_t *dspiEdmaPrivateHandle;
 
+
     dspiEdmaPrivateHandle = (dspi_slave_edma_private_handle_t *)g_dspiEdmaPrivateHandle;
 
     DSPI_DisableDMA((dspiEdmaPrivateHandle->base), kDSPI_RxDmaEnable | kDSPI_TxDmaEnable);
-
+    GPIO_ClearPinsOutput(GPIOE, 1u << 5u);
     dspiEdmaPrivateHandle->handle->state = kDSPI_Idle;
 
     if (dspiEdmaPrivateHandle->handle->callback)
     {
+
         dspiEdmaPrivateHandle->handle->callback(dspiEdmaPrivateHandle->base, dspiEdmaPrivateHandle->handle,
                                                 kStatus_Success, dspiEdmaPrivateHandle->handle->userData);
     }
 }
+#else
+extern dspi_slave_edma_handle_t g_dspi_edma_s_handle;
 
+static void EDMA_DspiSlaveCallback(edma_handle_t *edmaHandle,
+                                   void *g_dspiEdmaPrivateHandle,
+                                   bool transferDone,
+                                   uint32_t tcds)
+{
+
+    assert(edmaHandle);
+
+    DSPI_DisableDMA(SPI2, kDSPI_RxDmaEnable | kDSPI_TxDmaEnable);
+
+    g_dspi_edma_s_handle.state = kDSPI_Idle;
+
+    if (g_dspi_edma_s_handle.callback)
+    {
+
+	    g_dspi_edma_s_handle.callback(SPI2, &g_dspi_edma_s_handle,
+                                                kStatus_Success, g_dspi_edma_s_handle.userData);
+    }
+}
+#endif
 void DSPI_SlaveTransferAbortEDMA(SPI_Type *base, dspi_slave_edma_handle_t *handle)
 {
     assert(handle);
diff --git a/drivers/fsl_flexcan.c b/drivers/fsl_flexcan.c
index 8fd3399..6c7e0bd 100644
--- a/drivers/fsl_flexcan.c
+++ b/drivers/fsl_flexcan.c
@@ -402,19 +402,20 @@ void FLEXCAN_SetBaudRate(CAN_Type *base, uint32_t sourceClock_Hz, uint32_t baudR
     /* Assertion: Source clock should greater than baud rate * FLEXCAN_TIME_QUANTA_NUM. */
     assert(priDiv <= sourceClock_Hz);
 
+
     if (0 == priDiv)
     {
         priDiv = 1;
     }
 
     priDiv = (sourceClock_Hz / priDiv) - 1;
-
     /* Desired baud rate is too low. */
     if (priDiv > 0xFF)
     {
         priDiv = 0xFF;
     }
 
+
     /* FlexCAN timing setting formula:
      * FLEXCAN_TIME_QUANTA_NUM = 1 + (PSEG1 + 1) + (PSEG2 + 1) + (PROPSEG + 1);
      */
@@ -768,7 +769,7 @@ void FLEXCAN_SetRxFifoConfig(CAN_Type *base, const flexcan_rx_fifo_config_t *con
         FLEXCAN_SetRxMbConfig(base, 4, NULL, false);
         FLEXCAN_SetRxMbConfig(base, 5, NULL, false);
     }
-
+    base->MCR |= CAN_MCR_SRXDIS_MASK;
     /* Exit Freeze Mode. */
     FLEXCAN_ExitFreezeMode(base);
 }
diff --git a/source/adc_task.c b/source/adc_task.c
index 8adb356..77d07af 100644
--- a/source/adc_task.c
+++ b/source/adc_task.c
@@ -213,40 +213,9 @@ void tsc_task(void *pvParameters)
 int tsc_registers(dspi_transfer_t *spi_transfer)
 {
 	uint8_t *rx_buf = spi_transfer->rxData;
-	uint8_t *tx_buf = &spi_transfer->txData[1];
-	if (rx_buf[0] == APALIS_TK1_K20_READ_INST) {
-		switch (rx_buf[2]) {
-		case APALIS_TK1_K20_TSCREG:
-			tx_buf[0] = 0x00;
-			return 1;
-		case APALIS_TK1_K20_TSC_XML:
-			tx_buf[0] = gen_regs.tsc_xm & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_TSC_XMH:
-			tx_buf[0] = (gen_regs.tsc_xm >> 8) & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_TSC_XPL:
-			tx_buf[0] = gen_regs.tsc_xp & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_TSC_XPH:
-			tx_buf[0] = (gen_regs.tsc_xp >> 8) & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_TSC_YML:
-			tx_buf[0] = gen_regs.tsc_ym & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_TSC_YMH:
-			tx_buf[0] = (gen_regs.tsc_ym >> 8) & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_TSC_YPL:
-			tx_buf[0] = gen_regs.tsc_yp & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_TSC_YPH:
-			tx_buf[0] = (gen_regs.tsc_yp >> 8) & 0xFF;
-			return 1;
-		default:
-			return -ENOENT;
-		}
-	} else if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
+	uint8_t *tx_buf = &spi_transfer->txData[0];
+
+	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_TSCREG:
 			return -ENOENT;
@@ -254,7 +223,7 @@ int tsc_registers(dspi_transfer_t *spi_transfer)
 			return -ENOENT;
 		}
 	} else if (rx_buf[0] == APALIS_TK1_K20_BULK_READ_INST) {
-		if (rx_buf[2] == APALIS_TK1_K20_TSC_XML) {
+		if (rx_buf[1] == APALIS_TK1_K20_TSC_XML) {
 			if (rx_buf[1] == 2) {
 				tx_buf[0] = gen_regs.tsc_xm & 0xFF;
 				tx_buf[1] = (gen_regs.tsc_xm >> 8) & 0xFF;
@@ -271,7 +240,7 @@ int tsc_registers(dspi_transfer_t *spi_transfer)
 				return 8;
 			}
 		}
-		switch (rx_buf[2]) {
+		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_TSC_XPL:
 			tx_buf[0] = gen_regs.tsc_xp & 0xFF;
 			tx_buf[1] = (gen_regs.tsc_xp >> 8) & 0xFF;
@@ -294,41 +263,9 @@ int tsc_registers(dspi_transfer_t *spi_transfer)
 int adc_registers(dspi_transfer_t *spi_transfer)
 {
 	uint8_t *rx_buf = spi_transfer->rxData;
-	uint8_t *tx_buf = &spi_transfer->txData[1];
+	uint8_t *tx_buf = &spi_transfer->txData[0];
 
-	if (rx_buf[0] == APALIS_TK1_K20_READ_INST) {
-		switch (rx_buf[2]) {
-		case APALIS_TK1_K20_ADCREG:
-			tx_buf[0] = 0x00;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH0L:
-			tx_buf[0] = gen_regs.adc[0] & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH0H:
-			tx_buf[0] = (gen_regs.adc[0] >> 8) & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH1L:
-			tx_buf[0] = gen_regs.adc[1] & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH1H:
-			tx_buf[0] = (gen_regs.adc[1] >> 8) & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH2L:
-			tx_buf[0] = gen_regs.adc[2] & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH2H:
-			tx_buf[0] = (gen_regs.adc[2] >> 8) & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH3L:
-			tx_buf[0] = gen_regs.adc[3] & 0xFF;
-			return 1;
-		case APALIS_TK1_K20_ADC_CH3H:
-			tx_buf[0] = (gen_regs.adc[3] >> 8) & 0xFF;
-			return 1;
-		default:
-			return -ENOENT;
-		}
-	} else if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
+	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_ADCREG:
 			return -ENOENT;
@@ -336,12 +273,12 @@ int adc_registers(dspi_transfer_t *spi_transfer)
 			return -ENOENT;
 		}
 	} else if (rx_buf[0] == APALIS_TK1_K20_BULK_READ_INST) {
-		if (rx_buf[2] == APALIS_TK1_K20_ADC_CH0L) {
-			if (rx_buf[1] == 2) {
+		if (rx_buf[1] == APALIS_TK1_K20_ADC_CH0L) {
+			if (rx_buf[2] == 2) {
 				tx_buf[0] = gen_regs.adc[0] & 0xFF;
 				tx_buf[1] = (gen_regs.adc[0] >> 8) & 0xFF;
 				return 2;
-			} else if (rx_buf[1] == 8) {
+			} else if (rx_buf[2] == 8) {
 				tx_buf[0] = gen_regs.adc[0] & 0xFF;
 				tx_buf[1] = (gen_regs.adc[0] >> 8) & 0xFF;
 				tx_buf[2] = gen_regs.adc[1] & 0xFF;
@@ -353,7 +290,7 @@ int adc_registers(dspi_transfer_t *spi_transfer)
 				return 8;
 			}
 		}
-		switch (rx_buf[2]){
+		switch (rx_buf[1]){
 		case APALIS_TK1_K20_ADC_CH1L:
 			tx_buf[0] = gen_regs.adc[1] & 0xFF;
 			tx_buf[1] = (gen_regs.adc[1] >> 8) & 0xFF;
diff --git a/source/can_task.c b/source/can_task.c
index e335d93..82b0dda 100644
--- a/source/can_task.c
+++ b/source/can_task.c
@@ -8,8 +8,7 @@
 #define CAN_RTR_FLAG 0x40000000U /* remote transmission request */
 #define CAN_ERR_FLAG 0x20000000U /* error message frame */
 
-#define RX_MESSAGE_BUFFER_NUM0 (0)
-#define TX_MESSAGE_BUFFER_NUM0 (1)
+#define TX_MESSAGE_BUFFER_NUM0 (9)
 
 #define CAN_FRAME_MAX_LEN	8
 #define CAN_HEADER_MAX_LEN	5
@@ -51,12 +50,22 @@ struct can_registers {
 	uint8_t can_err_reg;
 	uint8_t can_mode;
 	uint8_t recived_frames_cnt;
+	uint8_t rx_buf_top;
+	uint8_t rx_buf_bottom;
 };
 
 static flexcan_frame_t tx_frame[2];
-static flexcan_frame_t rx_buffer[2][APALIS_TK1_CAN_RX_BUF_SIZE];
+static uint8_t data_buffer[2][APALIS_TK1_CAN_RX_BUF_SIZE][CAN_TRANSFER_BUF_LEN];
 
 static struct can_registers can_regs[2];
+uint8_t resume_can;
+
+void generate_can_irq(uint8_t id) {
+	if (id == 0)
+		GPIO_TogglePinsOutput(GPIOB, 1u << 8u);
+	else
+		GPIO_TogglePinsOutput(GPIOE, 1u << 26u);
+}
 
 static void flexcan_callback0(CAN_Type *base, flexcan_handle_t *handle, status_t status, uint32_t result, void *userData)
 {
@@ -65,22 +74,17 @@ static void flexcan_callback0(CAN_Type *base, flexcan_handle_t *handle, status_t
 
 	switch (status)
 	{
-	case kStatus_FLEXCAN_RxIdle:
-		if (RX_MESSAGE_BUFFER_NUM0 == result)
-		{
-			cb->async_status = pdTRUE;
-			xSemaphoreGiveFromISR(cb->sem, &reschedule);
-		}
+	case kStatus_FLEXCAN_RxFifoIdle:
+		cb->async_status = pdTRUE;
+		xSemaphoreGiveFromISR(cb->sem, &reschedule);
 		break;
 
 	case kStatus_FLEXCAN_TxIdle:
 		if (TX_MESSAGE_BUFFER_NUM0 == result)
 		{
 			/* write status and gen irq */
-			can_regs[0].can_status_reg |= CANINTF_TX;
-			generate_irq(APALIS_TK1_K20_CAN0_IRQ);
-			cb->async_status = pdFALSE;
-			xSemaphoreGiveFromISR(cb->sem, &reschedule);
+			registers[APALIS_TK1_K20_CANREG] |= CANINTF_TX;
+			generate_can_irq(0);
 		}
 		break;
 
@@ -97,22 +101,17 @@ static void flexcan_callback1(CAN_Type *base, flexcan_handle_t *handle, status_t
 
 	switch (status)
 	{
-	case kStatus_FLEXCAN_RxIdle:
-		if (RX_MESSAGE_BUFFER_NUM0 == result)
-		{
-			cb->async_status = pdTRUE;
-			xSemaphoreGiveFromISR(cb->sem, &reschedule);
-		}
+	case kStatus_FLEXCAN_RxFifoIdle:
+		cb->async_status = pdTRUE;
+		xSemaphoreGiveFromISR(cb->sem, &reschedule);
 		break;
 
 	case kStatus_FLEXCAN_TxIdle:
 		if (TX_MESSAGE_BUFFER_NUM0 == result)
 		{
 			/* write status and gen irq */
-			can_regs[1].can_status_reg |= CANINTF_TX;
-			generate_irq(APALIS_TK1_K20_CAN1_IRQ);
-			cb->async_status = pdFALSE;
-			xSemaphoreGiveFromISR(cb->sem, &reschedule);
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_OFFSET] |= CANINTF_TX;
+			generate_can_irq(1);
 		}
 		break;
 
@@ -125,12 +124,12 @@ static void flexcan_callback1(CAN_Type *base, flexcan_handle_t *handle, status_t
 static void CAN0_Init()
 {
 	flexcan_config_t flexcanConfig;
-	flexcan_rx_mb_config_t mbConfig;
+	flexcan_rx_fifo_config_t fifoConfig;
+	uint32_t fifoFilter = 0xFFFFFFFF;
 
 	FLEXCAN_GetDefaultConfig(&flexcanConfig);
 
 	flexcanConfig.baudRate = 1000000U; /* set default to 1Mbit/s*/
-	flexcanConfig.maxMbNum = 2; /* Use only 2 mailboxes, 1 RX 1TX */
 
 	/* Init FlexCAN module. */
 	flexcanConfig.clkSrc = kFLEXCAN_ClkSrcPeri;
@@ -141,27 +140,29 @@ static void CAN0_Init()
 
 	/* Set Rx Mask to don't care on all bits. */
 	FLEXCAN_SetRxMbGlobalMask(CAN0, FLEXCAN_RX_MB_EXT_MASK(0x00, 0, 0));
-
-	/* Setup Rx Message Buffer. */
-	mbConfig.format = kFLEXCAN_FrameFormatExtend;
-	mbConfig.type = kFLEXCAN_FrameTypeData;
-	mbConfig.id = FLEXCAN_ID_EXT(0x1FFFFFFF);
-	FLEXCAN_SetRxMbConfig(CAN0, RX_MESSAGE_BUFFER_NUM0, &mbConfig, true);
+	FLEXCAN_SetRxFifoGlobalMask(CAN0, FLEXCAN_RX_MB_EXT_MASK(0x00, 0, 0));
 
 	/* Setup Tx Message Buffer. */
 	FLEXCAN_SetTxMbConfig(CAN0, TX_MESSAGE_BUFFER_NUM0, true);
+
+	fifoConfig.idFilterNum = 1;
+	fifoConfig.idFilterTable = &fifoFilter;
+	fifoConfig.idFilterType = kFLEXCAN_RxFifoFilterTypeC;
+	fifoConfig.priority = kFLEXCAN_RxFifoPrioHigh;
+	FLEXCAN_SetRxFifoConfig(CAN0, &fifoConfig, true);
+
 	PRINTF("CAN0 init done \r\n");
 }
 
 static void CAN1_Init()
 {
 	flexcan_config_t flexcanConfig;
-	flexcan_rx_mb_config_t mbConfig;
+	flexcan_rx_fifo_config_t fifoConfig;
+	uint32_t fifoFilter = 0xFFFFFFFF;
 
 	FLEXCAN_GetDefaultConfig(&flexcanConfig);
 
 	flexcanConfig.baudRate = 1000000U; /* set default to 1Mbit/s */
-	flexcanConfig.maxMbNum = 2; /* Use only 2 mailboxes, 1 RX 1TX */
 
 	/* Init FlexCAN module. */
 	flexcanConfig.clkSrc = kFLEXCAN_ClkSrcPeri;
@@ -172,135 +173,124 @@ static void CAN1_Init()
 
 	/* Set Rx Mask to don't care on all bits. */
 	FLEXCAN_SetRxMbGlobalMask(CAN1, FLEXCAN_RX_MB_EXT_MASK(0x00, 0, 0));
-
-	/* Setup Rx Message Buffer. */
-	mbConfig.format = kFLEXCAN_FrameFormatExtend;
-	mbConfig.type = kFLEXCAN_FrameTypeData;
-	mbConfig.id = FLEXCAN_ID_EXT(0x1FFFFFFF);
-	FLEXCAN_SetRxMbConfig(CAN1, RX_MESSAGE_BUFFER_NUM0, &mbConfig, true);
+	FLEXCAN_SetRxFifoGlobalMask(CAN1, FLEXCAN_RX_MB_EXT_MASK(0x00, 0, 0));
 
 	/* Setup Tx Message Buffer. */
 	FLEXCAN_SetTxMbConfig(CAN1, TX_MESSAGE_BUFFER_NUM0, true);
+
+	fifoConfig.idFilterNum = 1;
+	fifoConfig.idFilterTable = &fifoFilter;
+	fifoConfig.idFilterType = kFLEXCAN_RxFifoFilterTypeC;
+	fifoConfig.priority = kFLEXCAN_RxFifoPrioHigh;
+	FLEXCAN_SetRxFifoConfig(CAN1, &fifoConfig, true);
+
 	PRINTF("CAN1 init done \r\n");
 }
+uint8_t available_data[2];
+
+void can_calculate_available_data(uint8_t id) {
+	if ( can_regs[id].rx_buf_bottom <= can_regs[id].rx_buf_top)
+		available_data[id] = can_regs[id].rx_buf_top - can_regs[id].rx_buf_bottom;
+	else
+		available_data[id] = APALIS_TK1_CAN_RX_BUF_SIZE - can_regs[id].rx_buf_bottom;
+
+	available_data[id] = (available_data[id] > APALIS_TK1_MAX_CAN_DMA_XREF) ? APALIS_TK1_MAX_CAN_DMA_XREF:available_data[id];
+	registers[APALIS_TK1_K20_CAN_IN_BUF_CNT + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] = available_data[id];
+	if (can_regs[id].rx_buf_bottom == can_regs[id].rx_buf_top)
+		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] &= ~CANINTF_RX;
+}
+
+void frame_to_buffer(flexcan_frame_t *frame, uint8_t id) {
+	uint8_t top_frame = can_regs[id].rx_buf_top;
+	if (frame->format ==  kFLEXCAN_FrameFormatExtend) {
+		data_buffer[id][top_frame][0] = frame->id & 0xFF;
+		data_buffer[id][top_frame][1] = (frame->id >> 8 ) & 0xFF;
+		data_buffer[id][top_frame][2] = (frame->id >> 16 ) & 0xFF;
+		data_buffer[id][top_frame][3] = (frame->id >> 24 ) & 0x1F;
+		data_buffer[id][top_frame][3] |= CAN_EFF_FLAG >> 24;
+	} else {
+		data_buffer[id][top_frame][0] = (frame->id >> 18) & 0xFF;
+		data_buffer[id][top_frame][1] = ((frame->id >> 18) >> 8 ) & 0x7F;
+		data_buffer[id][top_frame][2] = 0x00;
+		data_buffer[id][top_frame][3] = 0x00;
+	}
+	data_buffer[id][top_frame][3] |= frame->type ?  (CAN_RTR_FLAG >> 24):0x00;
+	data_buffer[id][top_frame][4] = frame->length;
+
+	switch (frame->length) {
+	case 8:data_buffer[id][top_frame][5 + 7] = frame->dataByte7;
+	case 7:data_buffer[id][top_frame][5 + 6] = frame->dataByte6;
+	case 6:data_buffer[id][top_frame][5 + 5] = frame->dataByte5;
+	case 5:data_buffer[id][top_frame][5 + 4] = frame->dataByte4;
+	case 4:data_buffer[id][top_frame][5 + 3] = frame->dataByte3;
+	case 3:data_buffer[id][top_frame][5 + 2] = frame->dataByte2;
+	case 2:data_buffer[id][top_frame][5 + 1] = frame->dataByte1;
+	case 1:data_buffer[id][top_frame][5] = frame->dataByte0;
+	}
+	can_regs[id].rx_buf_top++;
+	can_regs[id].rx_buf_top %= APALIS_TK1_CAN_RX_BUF_SIZE;
+	can_calculate_available_data(id);
+}
 
 void can0_task(void *pvParameters) {
 	flexcan_frame_t  rxFrame;
-	flexcan_mb_transfer_t rxXfer;
-	flexcan_rx_mb_config_t mbConfig;
+	flexcan_fifo_transfer_t rxXfer;
 	callback_message_t cb_msg;
-	status_t ret;
-
-
 	cb_msg.sem = xSemaphoreCreateBinary();
+	cb_msg.async_status = pdFALSE;
 	flexcanHandle[0].userData = (void *) &cb_msg;
 	CAN0_Init();
-	mbConfig.format = kFLEXCAN_FrameFormatExtend;
-	mbConfig.type = kFLEXCAN_FrameTypeData;
-	mbConfig.id = FLEXCAN_ID_EXT(0x1FFFFFFF);
+	rxXfer.frame = &rxFrame;
 
 	while(1)
 	{
-		rxXfer.frame = &rxFrame;
-		rxXfer.mbIdx = RX_MESSAGE_BUFFER_NUM0;
-
-		FLEXCAN_SetRxMbConfig(CAN0, RX_MESSAGE_BUFFER_NUM0, &mbConfig, true);
-		ret = FLEXCAN_TransferReceiveNonBlocking(CAN0, &flexcanHandle[0], &rxXfer);
-		if (ret == kStatus_Success){
-			if (xSemaphoreTake(cb_msg.sem, portMAX_DELAY) == pdTRUE)
-			{
-				if (cb_msg.async_status == pdTRUE)
-				{
-					if (can_regs[0].recived_frames_cnt < APALIS_TK1_CAN_RX_BUF_SIZE){
-
-						memcpy(&rx_buffer[0][can_regs[0].recived_frames_cnt],
-								rxXfer.frame, sizeof(flexcan_frame_t));
-						can_regs[0].recived_frames_cnt++;
-						can_regs[0].can_status_reg |= CANINTF_RX;
-						generate_irq(APALIS_TK1_K20_CAN0_IRQ);
-					}
-					if (can_regs[0].recived_frames_cnt >= APALIS_TK1_CAN_RX_BUF_SIZE)
-						vTaskSuspend(NULL);
-				}
+		FLEXCAN_TransferReceiveFifoNonBlocking(CAN0, &flexcanHandle[0], &rxXfer);
+		if (xSemaphoreTake(cb_msg.sem, portMAX_DELAY) == pdTRUE) {
+			if (cb_msg.async_status == pdTRUE) {
+				cb_msg.async_status = pdFALSE;
+				frame_to_buffer(&rxFrame, 0);
+				can_regs[0].recived_frames_cnt++;
+				registers[APALIS_TK1_K20_CANREG] |= CANINTF_RX;
+				generate_can_irq(0);
 			}
-		}else {
-			FLEXCAN_TransferAbortReceive(CAN0, &flexcanHandle[0], RX_MESSAGE_BUFFER_NUM0);
-			FLEXCAN_TransferAbortSend(CAN0, &flexcanHandle[0], TX_MESSAGE_BUFFER_NUM0);
+
+			if (can_regs[0].recived_frames_cnt >= APALIS_TK1_CAN_RX_BUF_SIZE)
+				vTaskSuspend(NULL);
 		}
+
 	}
 	vSemaphoreDelete(cb_msg.sem);
-
 }
 
 void can1_task(void *pvParameters) {
 	flexcan_frame_t  rxFrame;
-	flexcan_mb_transfer_t rxXfer;
-	flexcan_rx_mb_config_t mbConfig;
+	flexcan_fifo_transfer_t rxXfer;
 	callback_message_t cb_msg;
-	status_t ret;
 
 	cb_msg.sem = xSemaphoreCreateBinary();
 	flexcanHandle[1].userData = (void *) &cb_msg;
 	CAN1_Init();
+	rxXfer.frame = &rxFrame;
 
 	while(1)
 	{
-		rxXfer.frame = &rxFrame;
-		rxXfer.mbIdx = RX_MESSAGE_BUFFER_NUM0;
+		FLEXCAN_TransferReceiveFifoNonBlocking(CAN1, &flexcanHandle[1], &rxXfer);
+		if (xSemaphoreTake(cb_msg.sem, portMAX_DELAY) == pdTRUE) {
+			if (cb_msg.async_status == pdTRUE) {
+				cb_msg.async_status = pdFALSE;
+				frame_to_buffer(&rxFrame, 1);
+				can_regs[1].recived_frames_cnt++;
+				registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_OFFSET] |= CANINTF_RX;
+				generate_can_irq(1);
+			}
 
-		FLEXCAN_SetRxMbConfig(CAN1, RX_MESSAGE_BUFFER_NUM0, &mbConfig, true);
-		ret = FLEXCAN_TransferReceiveNonBlocking(CAN1, &flexcanHandle[1], &rxXfer);
-		if (ret == kStatus_Success){
-		if (xSemaphoreTake(cb_msg.sem, portMAX_DELAY) == pdTRUE)
-		{
-			if (cb_msg.async_status == pdTRUE)
-			{
-				if (can_regs[1].recived_frames_cnt < APALIS_TK1_CAN_RX_BUF_SIZE){
-
-					memcpy(&rx_buffer[1][can_regs[1].recived_frames_cnt],
-							rxXfer.frame, sizeof(flexcan_frame_t));
-					can_regs[1].recived_frames_cnt++;
-					can_regs[1].can_status_reg |= CANINTF_RX;
-					generate_irq(APALIS_TK1_K20_CAN1_IRQ);
-				}
-				if (can_regs[1].recived_frames_cnt >= APALIS_TK1_CAN_RX_BUF_SIZE)
-					vTaskSuspend(NULL);
+			if (can_regs[1].recived_frames_cnt >= APALIS_TK1_CAN_RX_BUF_SIZE) {
+				vTaskSuspend(NULL);
 			}
 		}
-		}else {
-			FLEXCAN_TransferAbortReceive(CAN1, &flexcanHandle[1], RX_MESSAGE_BUFFER_NUM0);
-			FLEXCAN_TransferAbortSend(CAN1, &flexcanHandle[1], TX_MESSAGE_BUFFER_NUM0);
-		}
 	}
 	vSemaphoreDelete(cb_msg.sem);
-}
 
-uint8_t get_canreg (int id)
-{
-	return can_regs[id].can_status_reg;
-}
-
-uint8_t get_canerr (int id)
-{
-	uint8_t temp;
-	temp = can_regs[id].can_err_reg;
-	can_regs[id].can_err_reg = 0;
-	return temp;
-}
-
-uint8_t get_canbadreg (int id)
-{
-	return 0;
-}
-
-uint16_t get_canbittimig (int id)
-{
-	return 0;
-}
-
-uint8_t get_can_rxcnt(int id)
-{
-	return can_regs[id].recived_frames_cnt;
 }
 
 static void can_change_mode(int id, uint8_t new_mode)
@@ -321,12 +311,19 @@ static void can_change_mode(int id, uint8_t new_mode)
 
 uint8_t set_canreg (int id, uint8_t value)
 {
-	can_regs[id].can_status_reg = value;
+	registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] = value;
 	if ( can_regs[id].can_mode != (value & CANCTRL_MODMASK) )
 		can_change_mode(id, (value & CANCTRL_MODMASK));
 	return 1;
 }
 
+uint8_t clr_canreg (int id, uint8_t mask)
+{
+	mask &= (CANINTF_TX | CANINTF_ERR);
+	registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] &= ~mask;
+	return 1;
+}
+
 uint8_t set_canerr (int id, uint8_t value)
 {
 	return 1;
@@ -360,121 +357,34 @@ uint8_t set_canbittimig (int id, uint16_t value, int16_t mask)
 	return 1;
 }
 
-static int can0_inframe;
-
 uint8_t can0_transmit(){
 	flexcan_mb_transfer_t txXfer;
 
 	txXfer.frame = &tx_frame[0];
 	txXfer.mbIdx = TX_MESSAGE_BUFFER_NUM0;
-	can0_inframe = 0;
 
-	FLEXCAN_TransferAbortReceive(CAN0, &flexcanHandle[0], RX_MESSAGE_BUFFER_NUM0);
 	FLEXCAN_TransferSendNonBlocking(CAN0, &flexcanHandle[0], &txXfer);
 	return 0;
 }
 
-uint8_t can0_sendframe_single_rw(uint8_t address, uint8_t data)
-{
-
-	if (address == 0){
-		tx_frame[0].length = data;
-		can0_inframe = 1;
-	}
-
-	if ((address > 0) && (address < 5) && can0_inframe){
-		tx_frame[0].id = data << (8 * (address - 1));
-	}
-
-	if ((address > 4) && can0_inframe){
-		switch (address){
-		case 5: tx_frame[0].dataByte0 = data;
-			break;
-		case 6: tx_frame[0].dataByte1 = data;
-			break;
-		case 7: tx_frame[0].dataByte2 = data;
-			break;
-		case 8: tx_frame[0].dataByte3 = data;
-			break;
-		case 9: tx_frame[0].dataByte4 = data;
-			break;
-		case 10: tx_frame[0].dataByte5 = data;
-			break;
-		case 11: tx_frame[0].dataByte6 = data;
-			break;
-		case 12: tx_frame[0].dataByte7 = data;
-			break;
-		}
-		if ((address - 4) == tx_frame[0].length)
-			return can0_transmit();
-	}
-	return 0;
-}
-
-uint8_t can1_sendframe(uint8_t *data, uint8_t len)
-{
-	return 1;
-}
-
-static int can1_inframe;
-
 uint8_t can1_transmit(){
 	flexcan_mb_transfer_t txXfer;
 
 	txXfer.frame = &tx_frame[1];
 	txXfer.mbIdx = TX_MESSAGE_BUFFER_NUM0;
-	can1_inframe = 0;
 
-	FLEXCAN_TransferAbortReceive(CAN1, &flexcanHandle[1], RX_MESSAGE_BUFFER_NUM0);
 	FLEXCAN_TransferSendNonBlocking(CAN1, &flexcanHandle[1], &txXfer);
 	return 0;
 }
 
-uint8_t can1_sendframe_single_rw(uint8_t address, uint8_t data)
-{
-
-	if (address == 0){
-		tx_frame[1].length = data;
-		can1_inframe = 1;
-	}
-
-	if ((address > 0) && (address < 5) && can1_inframe){
-		tx_frame[1].id = data << (8 * (address - 1));
-	}
-
-	if ((address > 4) && can1_inframe){
-		switch (address){
-		case 5: tx_frame[1].dataByte0 = data;
-			break;
-		case 6: tx_frame[1].dataByte1 = data;
-			break;
-		case 7: tx_frame[1].dataByte2 = data;
-			break;
-		case 8: tx_frame[1].dataByte3 = data;
-			break;
-		case 9: tx_frame[1].dataByte4 = data;
-			break;
-		case 10: tx_frame[1].dataByte5 = data;
-			break;
-		case 11: tx_frame[1].dataByte6 = data;
-			break;
-		case 12: tx_frame[1].dataByte7 = data;
-			break;
-		}
-		if ((address - 4) == tx_frame[1].length)
-			return can1_transmit();
-	}
-	return 0;
-}
-
 uint8_t can_sendframe(uint8_t id, uint8_t *data, uint8_t len)
 {
-	tx_frame[id].length = data[0];
-	tx_frame[id].id = (data[4] << 24) + (data[3] << 16) + (data[2] << 8) + data[1];
+	tx_frame[id].length = data[4];
+	tx_frame[id].id = (data[3] << 24) + (data[2] << 16) + (data[1] << 8) + data[0];
 
-	tx_frame[id].format = (data[4] << 24 & CAN_EFF_FLAG) ?
+	tx_frame[id].format = (data[3] << 24 & CAN_EFF_FLAG) ?
 				kFLEXCAN_FrameFormatExtend:kFLEXCAN_FrameFormatStandard;
-	tx_frame[id].type = (data[4] << 24 & CAN_RTR_FLAG) ?
+	tx_frame[id].type = (data[3] << 24 & CAN_RTR_FLAG) ?
 				kFLEXCAN_FrameTypeRemote:kFLEXCAN_FrameTypeData;
 
 	if (tx_frame[id].format == kFLEXCAN_FrameFormatExtend)
@@ -497,112 +407,34 @@ uint8_t can_sendframe(uint8_t id, uint8_t *data, uint8_t len)
 	return 0;
 }
 
-uint8_t can_readframe(uint8_t id, uint8_t *data)
+uint16_t can_readframe(uint8_t id, dspi_transfer_t *spi_transfer)
 {
-	data[0] = rx_buffer[id][0].length ;
-	if (rx_buffer[id][0].format ==  kFLEXCAN_FrameFormatExtend) {
-		data[1] = rx_buffer[id][0].id & 0xFF;
-		data[2] = (rx_buffer[id][0].id >> 8 ) & 0xFF;
-		data[3] = (rx_buffer[id][0].id >> 16 ) & 0xFF;
-		data[4] = (rx_buffer[id][0].id >> 24 ) & 0x1F;
-		data[4] |= CAN_EFF_FLAG >> 24;
-	} else {
-		data[1] = (rx_buffer[id][0].id >> 18) & 0xFF;
-		data[2] = ((rx_buffer[id][0].id >> 18) >> 8 ) & 0x7F;
-		data[3] = 0x00;
-		data[4] = 0x00;
-	}
-	data[4] |= rx_buffer[id][0].type ?  (CAN_RTR_FLAG >> 24):0x00;
-
-	data[5] = rx_buffer[id][0].dataByte0;
-	data[5 + 1] = rx_buffer[id][0].dataByte1;
-	data[5 + 2] = rx_buffer[id][0].dataByte2;
-	data[5 + 3] = rx_buffer[id][0].dataByte3;
-	data[5 + 4] = rx_buffer[id][0].dataByte4;
-	data[5 + 5] = rx_buffer[id][0].dataByte5;
-	data[5 + 6] = rx_buffer[id][0].dataByte6;
-	data[5 + 7] = rx_buffer[id][0].dataByte7;
-
-	can_regs[id].recived_frames_cnt = 0;
-	can_regs[id].can_status_reg &= ~CANINTF_RX;
-	if (id == 1)
-		vTaskResume(can1_task_handle);
-	else
-		vTaskResume(can0_task_handle);
-
-	return CAN_TRANSFER_BUF_LEN;
-}
+	uint8_t rx_size;
+	spi_transfer->txData = data_buffer[id][can_regs[id].rx_buf_bottom];
+	rx_size = spi_transfer->rxData[2] / CAN_TRANSFER_BUF_LEN; /* size in frames, not bytes */
+	if (rx_size > available_data[id])
+		rx_size = available_data[id];
+	can_regs[id].rx_buf_bottom = can_regs[id].rx_buf_bottom + rx_size;
+	can_regs[id].rx_buf_bottom %= APALIS_TK1_CAN_RX_BUF_SIZE;
 
-uint8_t can_readframe_single_rw(int id, uint8_t address)
-{
-
-	if (address == 0){
-		return rx_buffer[id][0].length;
-	}
+	resume_can = id + 1;
 
-	if ((address > 0) && (address < 5) && can1_inframe){
-		return  rx_buffer[id][0].id >> (8 * (address - 1));
-	}
+	can_regs[id].recived_frames_cnt -= rx_size;
+	can_calculate_available_data(id);
 
-	if ((address > 4) && can1_inframe){
-		switch (address){
-		case 5: return rx_buffer[id][0].dataByte0;
-		case 6: return rx_buffer[id][0].dataByte1;
-		case 7: return rx_buffer[id][0].dataByte2;
-		case 8: return rx_buffer[id][0].dataByte3;
-		case 9: return rx_buffer[id][0].dataByte4;
-		case 10: return rx_buffer[id][0].dataByte5;
-		case 11: return rx_buffer[id][0].dataByte6;
-		case 12:
-			vTaskResume(id ? can1_task:can0_task);
-			can_regs[id].recived_frames_cnt = 0;
-			return rx_buffer[id][0].dataByte7;
-		}
-		if ((address - 4) == tx_frame[1].length)
-			return can1_transmit();
-	}
-	return 0;
+	return rx_size * CAN_TRANSFER_BUF_LEN;
 }
 
 int canx_registers(dspi_transfer_t *spi_transfer, int id)
 {
 	uint8_t *rx_buf = spi_transfer->rxData;
-	uint8_t *tx_buf = &spi_transfer->txData[1];
-
-	if (rx_buf[0] == APALIS_TK1_K20_READ_INST) {
-		rx_buf[2] -= APALIS_TK1_K20_CAN_DEV_OFFSET(id);
-		switch (rx_buf[2]) {
-		case APALIS_TK1_K20_CANREG:
-			tx_buf[0] = get_canreg(id);
-			break;
-		case APALIS_TK1_K20_CANERR:
-			tx_buf[0] = get_canerr(id);
-			break;
-		case APALIS_TK1_K20_CAN_BAUD_REG:
-			tx_buf[0] = get_canbadreg(id);
-			break;
-		case APALIS_TK1_K20_CAN_BIT_1:
-			tx_buf[0] = get_canbittimig(id) && 0xFF;
-			break;
-		case APALIS_TK1_K20_CAN_BIT_2:
-			tx_buf[0] = (get_canbittimig(id) >> 8) && 0xFF;
-			break;
-		case APALIS_TK1_K20_CAN_IN_BUF_CNT:
-			tx_buf[0] = get_can_rxcnt(id);
-			break;
-		default:
-			if((rx_buf[1] >=APALIS_TK1_K20_CAN_IN_BUF) &&
-					(rx_buf[1] <= APALIS_TK1_K20_CAN_IN_BUF_END)) {
-				tx_buf[0] = can_readframe_single_rw(id, rx_buf[2]);
-			} else
-				return -EIO;
-		}
-		return 1;
-	} else if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
+	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		rx_buf[1] -= APALIS_TK1_K20_CAN_DEV_OFFSET(id);
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_CANREG:
 			return set_canreg(id, rx_buf[2]);
+		case APALIS_TK1_K20_CANREG_CLR:
+			return clr_canreg(id, rx_buf[2]);
 		case APALIS_TK1_K20_CANERR:
 			return set_canerr(id, rx_buf[2]);
 		case APALIS_TK1_K20_CAN_BAUD_REG:
@@ -612,29 +444,23 @@ int canx_registers(dspi_transfer_t *spi_transfer, int id)
 		case APALIS_TK1_K20_CAN_BIT_2:
 			return set_canbittimig(id, (rx_buf[2] << 8), 0xFF00);
 		default:
-			if((rx_buf[1] >=APALIS_TK1_K20_CAN_OUT_BUF) &&
-					(rx_buf[1] <= APALIS_TK1_K20_CAN_OUT_BUF_END) &&
-					(id == 0)) {
-				return can0_sendframe_single_rw(rx_buf[1] - APALIS_TK1_K20_CAN_OUT_BUF, rx_buf[2]);
-			} else
-				return -EIO;
+			return -EIO;
 		}
 
 	} else if (rx_buf[0] == APALIS_TK1_K20_BULK_READ_INST) {
-		rx_buf[2] -= APALIS_TK1_K20_CAN_DEV_OFFSET(id);
-		switch (rx_buf[2]) {
+		rx_buf[1] -= APALIS_TK1_K20_CAN_DEV_OFFSET(id);
+		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_CAN_IN_BUF:
-			return can_readframe(id, tx_buf);
+			return can_readframe(id, spi_transfer);
 		default:
 			return -EIO;
 		}
 	} else if (rx_buf[0] == APALIS_TK1_K20_BULK_WRITE_INST) {
-		rx_buf[2] -= APALIS_TK1_K20_CAN_DEV_OFFSET(id);
-		switch (rx_buf[2]) {
+		rx_buf[1] -= APALIS_TK1_K20_CAN_DEV_OFFSET(id);
+		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_CAN_OUT_BUF:
-			can_sendframe(id, &rx_buf[3], rx_buf[1]);
+			can_sendframe(id, &rx_buf[3], rx_buf[2]);
 			break;
-		case APALIS_TK1_K20_CAN_BIT_1:
 		default:
 			return -EIO;
 		}
diff --git a/source/can_task.h b/source/can_task.h
index b2113bf..564f073 100644
--- a/source/can_task.h
+++ b/source/can_task.h
@@ -15,5 +15,6 @@ TaskHandle_t can1_task_handle;
 void can0_task(void *pvParameters);
 void can1_task(void *pvParameters);
 int canx_registers(dspi_transfer_t *spi_transfer, int id);
+void can_calculate_available_data(uint8_t id);
 
 #endif /* SOURCE_CAN_TASK_H_ */
diff --git a/source/com_task.c b/source/com_task.c
index 9edc906..1c814df 100644
--- a/source/com_task.c
+++ b/source/com_task.c
@@ -4,59 +4,68 @@
 #include "gpio_ext.h"
 #include "adc_task.h"
 
+uint8_t  registers[APALIS_TK1_K20_LAST_REG];
+
 /* Put FW version at known address in binary. Make it 32-bit to have room for the future*/
 const uint32_t __attribute__((section(".FwVersion"), used)) fw_version = APALIS_TK1_K20_FW_VER;
 
 static dspi_slave_handle_t spi_handle;
-static uint8_t slaveRxData[APALIS_TK1_K20_MAX_BULK + APALIS_TK1_K20_HEADER] = {0U};
-static uint8_t slaveTxData[APALIS_TK1_K20_MAX_BULK + APALIS_TK1_K20_HEADER] = {0U};
+static uint8_t slaveRxData[APALIS_TK1_K20_MAX_BULK + APALIS_TK1_K20_HEADER];
+static uint8_t slaveTxData[APALIS_TK1_K20_MAX_BULK + APALIS_TK1_K20_HEADER];
+
+#define SPI_DMA
 
+#ifdef SPI_DMA
+dspi_slave_edma_handle_t g_dspi_edma_s_handle;
+edma_handle_t dspiEdmaSlaveRxHandle;
+edma_handle_t dspiEdmaSlaveTxHandle;
+#endif
 void generate_irq(uint8_t irq) {
-	gen_regs.irq = gen_regs.irq | BIT(irq);
+	registers[APALIS_TK1_K20_IRQREG] |= BIT(irq);
 	/* Toggle INT1 pin */
 	GPIO_TogglePinsOutput(GPIOA, 1u << 16u);
 }
 
 void clear_irq_flag(uint8_t irq) {
-	gen_regs.irq = ~irq & gen_regs.irq;
+	registers[APALIS_TK1_K20_IRQREG] &= ~irq;
 }
 
 uint8_t get_control_reg()
 {
-	return gen_regs.control;
+	return registers[APALIS_TK1_K20_CTRREG];
 
 }
 
 void set_control_reg(uint8_t value)
 {
-	gen_regs.control = value;
+	registers[APALIS_TK1_K20_CTRREG] = value;
 }
 
 uint8_t get_status_reg()
 {
-	return gen_regs.status;
+	return registers[APALIS_TK1_K20_STAREG];
 
 }
 
 void set_status_reg(uint8_t value)
 {
-	gen_regs.status = value;
+	registers[APALIS_TK1_K20_STAREG] = value;
 }
 
 uint8_t get_mask_reg()
 {
-	return gen_regs.irq_mask;
+	return registers[APALIS_TK1_K20_MSQREG];
 
 }
 
 void set_mask_reg(uint8_t value)
 {
-	gen_regs.irq_mask = value;
+	registers[APALIS_TK1_K20_MSQREG] = value;
 }
 
 uint8_t get_irq_reg()
 {
-	return gen_regs.irq;
+	return registers[APALIS_TK1_K20_IRQREG];
 
 }
 
@@ -70,29 +79,8 @@ void set_irq_reg(uint8_t value)
 inline int general_registers(dspi_transfer_t * spi_transfer)
 {
 	uint8_t *rx_buf = spi_transfer->rxData;
-	uint8_t *tx_buf = &spi_transfer->txData[1];
 
-	if (rx_buf[0] == APALIS_TK1_K20_READ_INST) {
-		switch (rx_buf[2]) {
-		case APALIS_TK1_K20_STAREG:
-			tx_buf[0] = get_status_reg();
-			return 1;
-		case APALIS_TK1_K20_REVREG:
-			tx_buf[0] = APALIS_TK1_K20_FW_VER;
-			return 1;
-		case APALIS_TK1_K20_IRQREG:
-			tx_buf[0] = get_irq_reg();
-			return 1;
-		case APALIS_TK1_K20_CTRREG:
-			tx_buf[0] = get_control_reg();
-			return 1;
-		case APALIS_TK1_K20_MSQREG:
-			tx_buf[0] = get_mask_reg();
-			return 1;
-		default:
-			return -ENOENT;
-		}
-	} else if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
+	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_STAREG:
 			set_status_reg(rx_buf[2]);
@@ -109,12 +97,12 @@ inline int general_registers(dspi_transfer_t * spi_transfer)
 			set_mask_reg(rx_buf[2]);
 			return 1;
 		default:
-			return -ENOENT;
+			return -ESRCH;
 		}
 	}
 
 
-	return -ENOENT;
+	return -ENXIO;
 }
 
 static void SPI_callback(SPI_Type *base, dspi_slave_handle_t *handle, status_t status, void *userData)
@@ -164,76 +152,120 @@ static void SPI_init() {
 void spi_task(void *pvParameters) {
 	callback_message_t cb_msg;
 	dspi_transfer_t slaveXfer;
-	int ret, retry_size = 0;
-	uint8_t req_register = 0xFF;
+	int ret = 0;
+#ifdef SPI_DMA
+	uint32_t slaveRxChannel, slaveTxChannel;
+	edma_config_t userConfig;
+#endif
 
 	cb_msg.sem = xSemaphoreCreateBinary();
 	spi_handle.userData = &cb_msg;
 	SPI_init();
+#ifdef SPI_DMA
+	slaveRxChannel = 0U;
+	slaveTxChannel = 1U;
+
+	DMAMUX_Init(DMAMUX);
+	DMAMUX_SetSource(DMAMUX, slaveRxChannel, kDmaRequestMux0SPI2Rx);
+	DMAMUX_EnableChannel(DMAMUX, slaveRxChannel);
+	DMAMUX_SetSource(DMAMUX, slaveTxChannel, kDmaRequestMux0SPI2Tx);
+	DMAMUX_EnableChannel(DMAMUX, slaveTxChannel);
+
+	EDMA_GetDefaultConfig(&userConfig);
+	EDMA_Init(DMA0, &userConfig);
+
+	EDMA_CreateHandle(&dspiEdmaSlaveRxHandle, DMA0, slaveRxChannel);
+	EDMA_CreateHandle(&dspiEdmaSlaveTxHandle, DMA0, slaveTxChannel);
+
+	g_dspi_edma_s_handle.userData = &cb_msg;
+	DSPI_SlaveTransferCreateHandleEDMA(SPI2, &g_dspi_edma_s_handle, SPI_callback, &cb_msg, &dspiEdmaSlaveRxHandle, &dspiEdmaSlaveTxHandle);
+#endif
+	registers[APALIS_TK1_K20_REVREG] = APALIS_TK1_K20_FW_VER;
 	GPIO_SetPinsOutput(GPIOA, 1u << 29u); /* INT2 idle */
+	slaveXfer.configFlags = kDSPI_SlaveCtar0;
+
+while(1){
+	slaveXfer.txData = NULL;/* no MISO traffic*/
+	slaveXfer.rxData = slaveRxData;
+	slaveXfer.dataSize = 3;
+	/* Wait for instructions from SoC */
+	ret = DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
+	if ( ret == kStatus_Success) {
 
-	while(1){
-		slaveXfer.txData = NULL;/* We're not expecting any MISO traffic */
-		slaveXfer.rxData = slaveRxData;
-		slaveXfer.dataSize = 3;
-		slaveXfer.configFlags = kDSPI_SlaveCtar0;
-		/* Wait for instructions from SoC */
-		DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
 		xSemaphoreTake(cb_msg.sem, portMAX_DELAY);
-		GPIO_ClearPinsOutput(GPIOD, 1u << 11u);
 		slaveXfer.txData = slaveTxData;
 		slaveXfer.rxData = slaveRxData;
 
-		if (slaveRxData[0] == APALIS_TK1_K20_WRITE_INST)
-			req_register = slaveRxData[1];
+		if (slaveRxData[0] == APALIS_TK1_K20_READ_INST)
+		{
+			/* clear interrupt flags on read; */
+			if ( slaveRxData[1] == APALIS_TK1_K20_IRQREG)
+			{
+				registers[APALIS_TK1_K20_IRQREG] = 0;
+			}
+		}
 		else
-			req_register = slaveRxData[2];
-
-		if (req_register <= 0x05) {
-			ret = general_registers(&slaveXfer);
-		} else if ((req_register >= APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0))
-			&& (req_register <= APALIS_TK1_K20_CAN_OUT_BUF_END + APALIS_TK1_K20_CAN_DEV_OFFSET(0))) {
-			ret = canx_registers(&slaveXfer, 0);
-
-		} else if ((req_register >= APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1))
-			&& (req_register <= APALIS_TK1_K20_CAN_OUT_BUF_END + APALIS_TK1_K20_CAN_DEV_OFFSET(1))) {
-			ret = canx_registers(&slaveXfer, 1);
+		{
+			if (slaveRxData[1] <= 0x05) {
+				ret = general_registers(&slaveXfer);
+			} else if ((slaveRxData[1] >= APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0))
+					&& (slaveRxData[1] <= APALIS_TK1_K20_CAN_OUT_BUF_END + APALIS_TK1_K20_CAN_DEV_OFFSET(0))) {
+				ret = canx_registers(&slaveXfer, 0);
+
+			} else if ((slaveRxData[1] >= APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1))
+					&& (slaveRxData[1] <= APALIS_TK1_K20_CAN_OUT_BUF_END + APALIS_TK1_K20_CAN_DEV_OFFSET(1))) {
+				ret = canx_registers(&slaveXfer, 1);
 #ifdef BOARD_USES_ADC
-		} else if ((req_register >= APALIS_TK1_K20_ADCREG) && (req_register <= APALIS_TK1_K20_ADC_CH3H)) {
-			ret = adc_registers(&slaveXfer);
+			} else if ((slaveRxData[1] >= APALIS_TK1_K20_ADCREG) && (slaveRxData[1] <= APALIS_TK1_K20_ADC_CH3H)) {
+				ret = adc_registers(&slaveXfer);
 
-		} else if ((req_register >= APALIS_TK1_K20_TSCREG) && (req_register <= APALIS_TK1_K20_TSC_YPH)) {
-			ret = tsc_registers(&slaveXfer);
+			} else if ((slaveRxData[1] >= APALIS_TK1_K20_TSCREG) && (slaveRxData[1] <= APALIS_TK1_K20_TSC_YPH)) {
+				ret = tsc_registers(&slaveXfer);
 #endif
-		} else if ((req_register >= APALIS_TK1_K20_GPIOREG) && (req_register <= APALIS_TK1_K20_GPIO_STA)) {
-			ret = gpio_registers(&slaveXfer);
-
-		} else if (req_register == APALIS_TK1_K20_RET_REQ) {
-			/* something wrong with the SPI peripheral, try resetting it */
-			DSPI_StopTransfer(SPI2);
-			DSPI_Enable(SPI2, false);
-			DSPI_SlaveInit(SPI2, &spi2_slaveConfig);
-			ret = retry_size - 1;
-		} else {
-			/* Register not defined */
-			ret = -EINVAL;
-		}
-
-		if (ret <= 0) {
-			slaveTxData[0] = TK1_K20_INVAL;
-			slaveTxData[1] = TK1_K20_INVAL;
-		} else {
-			slaveTxData[0] = TK1_K20_SENTINEL;
-		}
-
-		if (slaveRxData[0] == APALIS_TK1_K20_READ_INST || slaveRxData[0] == APALIS_TK1_K20_BULK_READ_INST)
-		{
-			slaveXfer.dataSize = (ret > 0) ? (ret + 1):2; /* Extra byte is for sentinel*/
-			retry_size = slaveXfer.dataSize;
-			slaveXfer.rxData = NULL; /* We're not expecting any MOSI traffic */
-			DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
-			GPIO_SetPinsOutput(GPIOD, 1u << 11u);
-			xSemaphoreTake(cb_msg.sem, portMAX_DELAY);
+			} else if ((slaveRxData[1] >= APALIS_TK1_K20_GPIOREG) && (slaveRxData[1] <= APALIS_TK1_K20_GPIO_STA)) {
+				ret = gpio_registers(&slaveXfer);
+			} else {
+				/* Register not defined */
+				ret = -EINVAL;
+			}
+
+
+			if (ret <= 0) {
+				slaveTxData[0] = TK1_K20_INVAL;
+				slaveTxData[1] = ret;
+				slaveTxData[2] = slaveRxData[0];
+				slaveTxData[3] = slaveRxData[1];
+				slaveTxData[4] = slaveRxData[2];
+				slaveXfer.txData = slaveTxData;
+				ret = 5;
+			}
+
+			if (slaveRxData[0] == APALIS_TK1_K20_BULK_READ_INST)
+			{
+				slaveXfer.dataSize = ret;
+				//retry_size = slaveXfer.dataSize;
+				slaveXfer.rxData = slaveRxData; /* We're not expecting any MOSI traffic, but NULL messes up stuff */
+#ifdef SPI_DMA
+				__disable_irq(); // just to make sure that high can irq rate will not interfere
+				DSPI_SlaveTransferEDMA(SPI2, &g_dspi_edma_s_handle, &slaveXfer);
+				__enable_irq();
+#else
+				DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
+#endif
+				xSemaphoreTake(cb_msg.sem, portMAX_DELAY);
+				if (resume_can != 0) {
+					if (resume_can == 1) {
+						vTaskResume(can0_task_handle);
+						can_calculate_available_data(0);
+					}
+					else {
+						vTaskResume(can1_task_handle);
+						can_calculate_available_data(1);
+					}
+					resume_can = 0;
+				}
+			}
 		}
 	}
 }
+}
diff --git a/source/com_task.h b/source/com_task.h
index a5db8c4..72ca858 100644
--- a/source/com_task.h
+++ b/source/com_task.h
@@ -9,6 +9,9 @@
 #include "fsl_debug_console.h"
 #include "fsl_dspi.h"
 #include "fsl_gpio.h"
+#include "fsl_edma.h"
+#include "fsl_dspi_edma.h"
+#include "fsl_dmamux.h"
 
 /* FreeRTOS kernel includes. */
 #include "FreeRTOS.h"
@@ -40,12 +43,12 @@ void spi_task(void *pvParameters);
 #define APALIS_TK1_K20_BULK_WRITE_INST		0x3C
 #define APALIS_TK1_K20_BULK_READ_INST		0xC3
 
-#define APALIS_TK1_K20_MAX_BULK			(32)
+#define APALIS_TK1_K20_MAX_BULK			(250)
 
 /* General registers*/
 #define APALIS_TK1_K20_STAREG			0x00 /* general status register RO */
 #define APALIS_TK1_K20_REVREG			0x01 /* FW revision register RO*/
-#define APALIS_TK1_K20_IRQREG			0x02 /* IRQ status RW(write of 1 will reset the bit) */
+#define APALIS_TK1_K20_IRQREG			0x02 /* IRQ status RO(reset of read) */
 #define APALIS_TK1_K20_CTRREG			0x03 /* general control register RW */
 #define APALIS_TK1_K20_MSQREG			0x04 /* IRQ mask register RW */
 
@@ -53,21 +56,22 @@ void spi_task(void *pvParameters);
 
 /* CAN Registers */
 #define APALIS_TK1_K20_CANREG			0x10 /* CAN0 control & status register RW */
-#define APALIS_TK1_K20_CANERR			0x11 /* CAN0 error register RW */
-#define APALIS_TK1_K20_CAN_BAUD_REG		0x12 /* CAN0 baud set register RW */
-#define APALIS_TK1_K20_CAN_BIT_1		0x13 /* CAN0 bit timing register 1 RW */
-#define APALIS_TK1_K20_CAN_BIT_2		0x14 /* CAN0 bit timing register 2 RW */
-#define APALIS_TK1_K20_CAN_IN_BUF_CNT		0x15 /* CAN0 IN received data count RO */
-#define APALIS_TK1_K20_CAN_IN_BUF		0x16 /* CAN0 IN RO */
+#define APALIS_TK1_K20_CANREG_CLR		0x11 /* CAN0 CANREG clear register WO */
+#define APALIS_TK1_K20_CANERR			0x12 /* CAN0 error register RW */
+#define APALIS_TK1_K20_CAN_BAUD_REG		0x13 /* CAN0 baud set register RW */
+#define APALIS_TK1_K20_CAN_BIT_1		0x14 /* CAN0 bit timing register 1 RW */
+#define APALIS_TK1_K20_CAN_BIT_2		0x15 /* CAN0 bit timing register 2 RW */
+#define APALIS_TK1_K20_CAN_IN_BUF_CNT		0x16 /* CAN0 IN received data count RO */
+#define APALIS_TK1_K20_CAN_IN_BUF		0x17 /* CAN0 IN RO */
 /* buffer size is 13 bytes */
-#define APALIS_TK1_K20_CAN_IN_BUF_END		0x22 /* CAN0 IN RO */
-#define APALIS_TK1_K20_CAN_OUT_BUF_CNT		0x23 /* CAN0 OUT data Count WO */
-#define APALIS_TK1_K20_CAN_OUT_BUF		0x26 /* CAN0 OUT WO */
+#define APALIS_TK1_K20_CAN_IN_BUF_END		0x23 /* CAN0 IN RO */
+#define APALIS_TK1_K20_CAN_OUT_BUF		0x24 /* CAN0 OUT WO */
 /* buffer size is 13 bytes */
 #define APALIS_TK1_K20_CAN_OUT_BUF_END		(APALIS_TK1_K20_CAN_OUT_BUF + 13 - 1)/* CAN OUT BUF END */
-#define APALIS_TK1_K20_CAN_DEV_OFFSET(x)	(x ? 0x30 : 0)
+#define APALIS_TK1_K20_CAN_OFFSET		0x30
+#define APALIS_TK1_K20_CAN_DEV_OFFSET(x)	(x ? APALIS_TK1_K20_CAN_OFFSET : 0)
 
-/* 0x33-0x3F Reserved */
+/* 0x30-0x3F Reserved */
 /* 0x40-0x62 CAN1 registers same layout as CAN0*/
 /* 0x63-0x6F Reserved */
 
@@ -111,7 +115,8 @@ void spi_task(void *pvParameters);
 #define APALIS_TK1_K20_GPIO_STA_OE		BIT(0)
 #define APALIS_TK1_K20_GPIO_STA_VAL		BIT(1)
 
-/* 0x93-0xFD Reserved */
+/* 0x93-0xFC Reserved */
+#define APALIS_TK1_K20_LAST_REG			0xFD
 #define APALIS_TK1_K20_RET_REQ			0xFE
 /* 0xFF Reserved */
 
@@ -123,7 +128,7 @@ void spi_task(void *pvParameters);
 #define APALIS_TK1_K20_TSC_IRQ			4
 #define APALIS_TK1_K20_GPIO_IRQ			5
 
-#define APALIS_TK1_K20_FW_VER			0x0B
+#define APALIS_TK1_K20_FW_VER			0x0D
 
 #define FW_MINOR (APALIS_TK1_K20_FW_VER & 0x0F)
 #define FW_MAJOR ((APALIS_TK1_K20_FW_VER & 0xF0) >> 4)
@@ -137,7 +142,8 @@ void spi_task(void *pvParameters);
 
 #define APALIS_TK1_CAN_CLK_UNIT			6250
 
-#define APALIS_TK1_CAN_RX_BUF_SIZE		1
+#define APALIS_TK1_CAN_RX_BUF_SIZE		256
+#define APALIS_TK1_MAX_CAN_DMA_XREF		19
 
 #define APALIS_TK1_K20_HEADER			4
 
@@ -159,4 +165,7 @@ struct register_struct {
 	uint16_t tsc_yp;
 } gen_regs;
 
+extern uint8_t  registers[APALIS_TK1_K20_LAST_REG];
+extern uint8_t resume_can;
+
 #endif /* COM_TASK_H_ */
diff --git a/source/gpio_ext.c b/source/gpio_ext.c
index 6d4d3a8..839cd58 100644
--- a/source/gpio_ext.c
+++ b/source/gpio_ext.c
@@ -95,10 +95,10 @@ uint8_t get_gpio_status(uint8_t pin)
 int gpio_registers(dspi_transfer_t *spi_transfer)
 {
 	uint8_t *rx_buf = spi_transfer->rxData;
-	uint8_t *tx_buf = &spi_transfer->txData[1];
+	uint8_t *tx_buf = &spi_transfer->txData[0];
 
 	if (rx_buf[0] == APALIS_TK1_K20_READ_INST) {
-		switch (rx_buf[2]) {
+	/*	switch (rx_buf[1]) {
 		case APALIS_TK1_K20_GPIOREG:
 			return -ENOENT;
 			break;
@@ -121,7 +121,7 @@ int gpio_registers(dspi_transfer_t *spi_transfer)
 			break;
 		default:
 			return -ENOENT;
-		}
+		} */
 	} else if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_GPIOREG:
diff --git a/source/gpio_ext.h b/source/gpio_ext.h
index a5a14da..4df1788 100644
--- a/source/gpio_ext.h
+++ b/source/gpio_ext.h
@@ -22,6 +22,7 @@ struct gpio_id gpio_list [] = {
 		{PORTA, GPIOA, 5},
 		{PORTA, GPIOA, 17},
 #ifndef BOARD_USES_ADC
+		{PORTB, GPIOB, 0},
 		{PORTB, GPIOB, 1},
 		{PORTB, GPIOB, 2},
 		{PORTB, GPIOB, 3},
diff --git a/source/main.c b/source/main.c
index d910ab1..ad4e82b 100644
--- a/source/main.c
+++ b/source/main.c
@@ -128,6 +128,8 @@ int main(void) {
 
 	NVIC_SetPriority(CAN0_ORed_Message_buffer_IRQn, 5u);
 	NVIC_SetPriority(CAN1_ORed_Message_buffer_IRQn, 5u);
+	NVIC_SetPriority(SPI2_IRQn, 5u);
+	NVIC_SetPriority(DMA0_IRQn, 5u);
 	NVIC_SetPriorityGrouping( 0 );
 	vTaskStartScheduler();
 
diff --git a/utilities/fsl_debug_console.h b/utilities/fsl_debug_console.h
index 6c50de5..a3d3790 100644
--- a/utilities/fsl_debug_console.h
+++ b/utilities/fsl_debug_console.h
@@ -57,7 +57,7 @@
 
 /*! @brief Definition to select sdk or toolchain printf, scanf. */
 #ifndef SDK_DEBUGCONSOLE
-#define SDK_DEBUGCONSOLE 1U
+//#define SDK_DEBUGCONSOLE 1U
 #endif
 
 #if defined(SDK_DEBUGCONSOLE) && !(SDK_DEBUGCONSOLE)
@@ -90,10 +90,10 @@
 #define PUTCHAR DbgConsole_Putchar
 #define GETCHAR DbgConsole_Getchar
 #else /* Select printf, scanf, putchar, getchar of toolchain. */
-#define PRINTF printf
-#define SCANF scanf
-#define PUTCHAR putchar
-#define GETCHAR getchar
+#define PRINTF(x...)
+#define SCANF(x...)
+#define PUTCHAR
+#define GETCHAR
 #endif /* SDK_DEBUGCONSOLE */
 
 /*******************************************************************************