Improved CAN, locking and general IRQ performance

Fixes issues with CAN stopping when overwhelmed by data RX\TX.
Keeps can in freezemode until explicitly activated.
CANINTF_TX is now a CAN TX in progress flag.
Runtime asserts are now disabled.

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

10 files changed, 302 insertions, 152 deletions

diff --git a/.cproject b/.cproject
index c40e73a..270066b 100644
--- a/.cproject
+++ b/.cproject
@@ -17,7 +17,7 @@
 				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="${cross_rm} -rf" description="" id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.475762548" name="Debug" parent="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug">
 					<folderInfo id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.475762548." name="/" resourcePath="">
 						<toolChain id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.elf.debug.1690074070" name="Cross ARM GCC" superClass="ilg.gnuarmeclipse.managedbuild.cross.toolchain.elf.debug">
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.1531534215" name="Optimization Level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level" value="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.most" valueType="enumerated"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.1531534215" name="Optimization Level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level" value="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.debug" valueType="enumerated"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength.1376026763" name="Message length (-fmessage-length=0)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar.322137639" name="'char' is signed (-fsigned-char)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections.85374014" name="Function sections (-ffunction-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections" value="true" valueType="boolean"/>
@@ -42,7 +42,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.allwarn.998714097" 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.warnings.pedantic.1394555272" name="Pedantic (-pedantic)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pedantic" value="false" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.extrawarn.1645809733" name="Enable extra warnings (-Wextra)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.extrawarn" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto.1022138696" name="Link-time optimizer (-flto)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto.1022138696" name="Link-time optimizer (-flto)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto" value="false" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform.1444192880" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
 							<builder buildPath="${workspace_loc:/k20_tester}/Debug" id="ilg.gnuarmeclipse.managedbuild.cross.builder.1236922579" 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.959743626" name="Cross ARM GNU Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
@@ -54,11 +54,15 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/k20_main/CMSIS/Include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/k20_main/CMSIS/Driver/Include}&quot;"/>
 								</option>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs.1505271632" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="NDEBUG"/>
+								</option>
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input.852998941" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input"/>
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler.195326104" name="Cross ARM C Compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs.2027427795" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="&quot;CPU_MK20DN512VLK10&quot;"/>
+									<listOptionValue builtIn="false" value="NDEBUG"/>
 								</option>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.std.1026465758" name="Language standard" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.std" useByScannerDiscovery="true" value="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.std.gnu99" valueType="enumerated"/>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths.59602908" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths" useByScannerDiscovery="false" valueType="includePath">
@@ -81,6 +85,7 @@
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.compiler.476692042" name="Cross ARM C++ Compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.compiler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs.406548015" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="&quot;CPU_MK20DN512VLK10&quot;"/>
+									<listOptionValue builtIn="false" value="NDEBUG"/>
 								</option>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths.506184444" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../freertos/Source/include"/>
@@ -142,7 +147,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="CMSIS/Driver/DriverTemplates|drivers/fsl_mpu.c|usb_1.1.0/osa/usb_osa_bm.c|usb_1.1.0/device|freertos/Source/portable/MemMang/heap_5.c|freertos/Source/portable/MemMang/heap_3.c|freertos/Source/portable/MemMang/heap_2.c|freertos/Source/portable/MemMang/heap_1.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="usb_1.1.0|drivers/fsl_pit.c|drivers/fsl_pdb.c|drivers/fsl_llwu.c|drivers/fsl_flexbus.c|drivers/fsl_uart_edma.c|drivers/fsl_smc.c|drivers/fsl_sim.c|drivers/fsl_sdhc.c|drivers/fsl_sai.c|drivers/fsl_sai_edma.c|drivers/fsl_rtc.c|drivers/fsl_rcm.c|drivers/fsl_pmc.c|drivers/fsl_i2c.c|drivers/fsl_i2c_freertos.c|drivers/fsl_i2c_edma.c|drivers/fsl_ftm.c|drivers/fsl_flash.c|drivers/fsl_ewm.c|drivers/fsl_dac.c|drivers/fsl_crc.c|drivers/fsl_cmt.c|drivers/fsl_cmp.c|CMSIS/Driver/DriverTemplates|drivers/fsl_mpu.c|usb_1.1.0/osa/usb_osa_bm.c|usb_1.1.0/device|freertos/Source/portable/MemMang/heap_5.c|freertos/Source/portable/MemMang/heap_3.c|freertos/Source/portable/MemMang/heap_2.c|freertos/Source/portable/MemMang/heap_1.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -170,7 +175,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar.1398453889" name="'char' is signed (-fsigned-char)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections.458714102" name="Function sections (-ffunction-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.datasections.1713635727" name="Data sections (-fdata-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.datasections" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level.616331956" name="Debug level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level.616331956" name="Debug level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level" value="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level.max" valueType="enumerated"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format.1062923169" name="Debug format" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name.2099258968" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name" value="GNU Tools for ARM Embedded Processors" valueType="string"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.1414960201" name="Architecture" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.architecture" value="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.arm" valueType="enumerated"/>
@@ -202,12 +207,15 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/k20_main/CMSIS/Include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/k20_main/CMSIS/Driver/Include}&quot;"/>
 								</option>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs.1001154321" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="NDEBUG"/>
+								</option>
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input.982936805" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input"/>
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler.867386499" name="Cross ARM C Compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs.1292028075" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs" useByScannerDiscovery="false" valueType="definedSymbols">
-									<listOptionValue builtIn="false" value="NDEBUG"/>
 									<listOptionValue builtIn="false" value="&quot;CPU_MK20DN512VLK10&quot;"/>
+									<listOptionValue builtIn="false" value="NDEBUG"/>
 								</option>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.std.1188085931" name="Language standard" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.std" useByScannerDiscovery="true" value="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.std.gnu99" valueType="enumerated"/>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths.1580315270" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths" useByScannerDiscovery="false" valueType="includePath">
@@ -229,8 +237,8 @@
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.compiler.658547674" name="Cross ARM C++ Compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.compiler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs.59523076" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs" useByScannerDiscovery="false" valueType="definedSymbols">
-									<listOptionValue builtIn="false" value="NDEBUG"/>
 									<listOptionValue builtIn="false" value="&quot;CPU_MK20DN512VLK10&quot;"/>
+									<listOptionValue builtIn="false" value="NDEBUG"/>
 								</option>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths.1916697474" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../freertos/Source/include"/>
@@ -293,11 +301,12 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="CMSIS/Driver/DriverTemplates|drivers/fsl_mpu.c|usb_1.1.0/osa/usb_osa_bm.c|usb_1.1.0/device|freertos/Source/portable/MemMang/heap_5.c|freertos/Source/portable/MemMang/heap_3.c|freertos/Source/portable/MemMang/heap_2.c|freertos/Source/portable/MemMang/heap_1.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="usb_1.1.0|drivers/fsl_pit.c|drivers/fsl_pdb.c|drivers/fsl_llwu.c|drivers/fsl_flexbus.c|drivers/fsl_uart_edma.c|drivers/fsl_smc.c|drivers/fsl_sim.c|drivers/fsl_sdhc.c|drivers/fsl_sai.c|drivers/fsl_sai_edma.c|drivers/fsl_rtc.c|drivers/fsl_rcm.c|drivers/fsl_pmc.c|drivers/fsl_i2c.c|drivers/fsl_i2c_freertos.c|drivers/fsl_i2c_edma.c|drivers/fsl_ftm.c|drivers/fsl_flash.c|drivers/fsl_ewm.c|drivers/fsl_dac.c|drivers/fsl_crc.c|drivers/fsl_cmt.c|drivers/fsl_cmp.c|CMSIS/Driver/DriverTemplates|drivers/fsl_mpu.c|usb_1.1.0/osa/usb_osa_bm.c|usb_1.1.0/device|freertos/Source/portable/MemMang/heap_5.c|freertos/Source/portable/MemMang/heap_3.c|freertos/Source/portable/MemMang/heap_2.c|freertos/Source/portable/MemMang/heap_1.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
 			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+			<storageModule moduleId="ilg.gnuarmeclipse.managedbuild.packs"/>
 		</cconfiguration>
 	</storageModule>
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 0acd25f..3469e49 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="-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">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1447393205515885021" 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="-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">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1432191262867276349" 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/drivers/fsl_dspi.c b/drivers/fsl_dspi.c
index 66f8974..1ec01b3 100644
--- a/drivers/fsl_dspi.c
+++ b/drivers/fsl_dspi.c
@@ -1445,7 +1445,7 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
     assert(handle);
 
     volatile uint32_t dataReceived;
-    uint32_t dataSend = 0;
+//    uint32_t dataSend = 0;
 
     /* Because SPI protocol is synchronous, the number of bytes that that slave received from the
     * master is the actual number of bytes that the slave transmitted to the master. So we only
@@ -1492,25 +1492,26 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
                                     		case APALIS_TK1_K20_IRQREG:
                                     			registers[APALIS_TK1_K20_IRQREG] = 0;
                                     			break;
-                                    		case APALIS_TK1_K20_CANREG:
-                                    			registers[APALIS_TK1_K20_CANREG] &= ~0x10;
-                                    			break;
-                                    		case APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_OFFSET:
-                                    			registers[APALIS_TK1_K20_CANREG
-								  + APALIS_TK1_K20_CAN_OFFSET] &= ~0x10;
-                                    			break;
+                                    		case APALIS_TK1_K20_CANERR:
+                                    			registers[APALIS_TK1_K20_CANERR] = 0x00;
+                                    		case APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_OFFSET:
+                                    			registers[APALIS_TK1_K20_CANERR
+								  + APALIS_TK1_K20_CAN_OFFSET] = 0x00;
                                     		}
                         		}
                         		else
                         		{
-                        			base->PUSHR_SLAVE = 0x55;
+                        			if ( *(handle->rxData - 1) ==  APALIS_TK1_K20_READ_INST)
+                        			{
+                        				base->PUSHR_SLAVE = 0x55;
+                        			}
                         		}
                         	}
                 }
 
                 /* Decrease remaining receive byte count */
                 --handle->remainingReceiveByteCount;
-
+#ifndef SPI_DMA
                 if (handle->remainingSendByteCount > 0)
                 {
                 		if (handle->txData)
@@ -1527,6 +1528,7 @@ void DSPI_SlaveTransferHandleIRQ(SPI_Type *base, dspi_slave_handle_t *handle)
                     /* Write the data to the DSPI data register */
                     base->PUSHR_SLAVE = dataSend;
                 }
+#endif
 #if 0
             }
             else /* If bits/frame is 2 bytes */
@@ -1617,7 +1619,6 @@ 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_edma.c b/drivers/fsl_dspi_edma.c
index fa26b2f..2b91cdc 100644
--- a/drivers/fsl_dspi_edma.c
+++ b/drivers/fsl_dspi_edma.c
@@ -890,9 +890,9 @@ status_t DSPI_SlaveTransferEDMA(SPI_Type *base, dspi_slave_edma_handle_t *handle
     handle->state = kDSPI_Busy;
 
     uint32_t instance = DSPI_GetInstance(base);
-    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;
 
     /* If using a shared RX/TX DMA request, then this limits the amount of data we can transfer
     * due to the linked channel. The max bytes is 511 if 8-bit/frame or 1022 if 16-bit/frame
@@ -1213,23 +1213,23 @@ static void EDMA_DspiSlaveCallback(edma_handle_t *edmaHandle,
 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)
+		void *g_dspiEdmaPrivateHandle,
+		bool transferDone,
+		uint32_t tcds)
 {
 
-    assert(edmaHandle);
+	assert(edmaHandle);
 
-    DSPI_DisableDMA(SPI2, kDSPI_RxDmaEnable | kDSPI_TxDmaEnable);
+	DSPI_DisableDMA(SPI2, kDSPI_RxDmaEnable | kDSPI_TxDmaEnable);
 
-    g_dspi_edma_s_handle.state = kDSPI_Idle;
+	g_dspi_edma_s_handle.state = kDSPI_Idle;
 
-    if (g_dspi_edma_s_handle.callback)
-    {
+	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);
-    }
+		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)
diff --git a/drivers/fsl_flexcan.c b/drivers/fsl_flexcan.c
index 09add9e..2a07dc5 100644
--- a/drivers/fsl_flexcan.c
+++ b/drivers/fsl_flexcan.c
@@ -88,24 +88,6 @@ typedef void (*flexcan_isr_t)(CAN_Type *base, flexcan_handle_t *handle);
  */
 uint32_t FLEXCAN_GetInstance(CAN_Type *base);
 
-/*!
- * @brief Enter FlexCAN Freeze Mode.
- *
- * This function makes the FlexCAN work under Freeze Mode.
- *
- * @param base FlexCAN peripheral base address.
- */
-static void FLEXCAN_EnterFreezeMode(CAN_Type *base);
-
-/*!
- * @brief Exit FlexCAN Freeze Mode.
- *
- * This function makes the FlexCAN leave Freeze Mode.
- *
- * @param base FlexCAN peripheral base address.
- */
-static void FLEXCAN_ExitFreezeMode(CAN_Type *base);
-
 #if !defined(NDEBUG)
 /*!
  * @brief Check if Message Buffer is occupied by Rx FIFO.
@@ -117,7 +99,7 @@ static void FLEXCAN_ExitFreezeMode(CAN_Type *base);
  */
 static bool FLEXCAN_IsMbOccupied(CAN_Type *base, uint8_t mbIdx);
 #endif
-
+#if 0
 #if (defined(FSL_FEATURE_FLEXCAN_HAS_ERRATA_5641) && FSL_FEATURE_FLEXCAN_HAS_ERRATA_5641)
 /*!
  * @brief Get the first valid Message buffer ID of give FlexCAN instance.
@@ -129,7 +111,7 @@ static bool FLEXCAN_IsMbOccupied(CAN_Type *base, uint8_t mbIdx);
  */
 static uint32_t FLEXCAN_GetFirstValidMb(CAN_Type *base);
 #endif
-
+#endif
 /*!
  * @brief Check if Message Buffer interrupt is enabled.
  *
@@ -214,7 +196,7 @@ uint32_t FLEXCAN_GetInstance(CAN_Type *base)
     return instance;
 }
 
-static void FLEXCAN_EnterFreezeMode(CAN_Type *base)
+void FLEXCAN_EnterFreezeMode(CAN_Type *base)
 {
     /* Set Freeze, Halt bits. */
     base->MCR |= CAN_MCR_HALT_MASK;
@@ -225,7 +207,7 @@ static void FLEXCAN_EnterFreezeMode(CAN_Type *base)
     }
 }
 
-static void FLEXCAN_ExitFreezeMode(CAN_Type *base)
+void FLEXCAN_ExitFreezeMode(CAN_Type *base)
 {
     /* Clear Freeze, Halt bits. */
     base->MCR &= ~CAN_MCR_HALT_MASK;
@@ -279,7 +261,7 @@ static bool FLEXCAN_IsMbOccupied(CAN_Type *base, uint8_t mbIdx)
     }
 }
 #endif
-
+#if 0
 #if (defined(FSL_FEATURE_FLEXCAN_HAS_ERRATA_5641) && FSL_FEATURE_FLEXCAN_HAS_ERRATA_5641)
 static uint32_t FLEXCAN_GetFirstValidMb(CAN_Type *base)
 {
@@ -298,6 +280,7 @@ static uint32_t FLEXCAN_GetFirstValidMb(CAN_Type *base)
     return firstValidMbNum;
 }
 #endif
+#endif
 
 static bool FLEXCAN_IsMbIntEnabled(CAN_Type *base, uint8_t mbIdx)
 {
@@ -487,6 +470,9 @@ void FLEXCAN_Init(CAN_Type *base, const flexcan_config_t *config, uint32_t sourc
     mcrTemp = (config->enableDoze) ? mcrTemp | CAN_MCR_DOZE_MASK : mcrTemp & ~CAN_MCR_DOZE_MASK;
 #endif
 
+    mcrTemp |= CAN_MCR_HALT_MASK;
+    mcrTemp |= CAN_MCR_FRZ_MASK;
+
     /* Save MCR Configuation. */
     base->MCR = mcrTemp;
 
@@ -539,6 +525,10 @@ void FLEXCAN_SetTimingConfig(CAN_Type *base, const flexcan_timing_config_t *conf
 {
     /* Assertion. */
     assert(config);
+    int keep_frozen = 0;
+
+    if (base->MCR & CAN_MCR_FRZACK_MASK)
+        keep_frozen = 1;
 
     /* Enter Freeze Mode. */
     FLEXCAN_EnterFreezeMode(base);
@@ -553,24 +543,65 @@ void FLEXCAN_SetTimingConfig(CAN_Type *base, const flexcan_timing_config_t *conf
          CAN_CTRL1_PSEG1(config->phaseSeg1) | CAN_CTRL1_PSEG2(config->phaseSeg2) | CAN_CTRL1_PROPSEG(config->propSeg));
 
     /* Exit Freeze Mode. */
-    FLEXCAN_ExitFreezeMode(base);
+    if (!keep_frozen)
+        FLEXCAN_ExitFreezeMode(base);
 }
 
 void FLEXCAN_SetBitRate(CAN_Type *base, uint32_t sourceClock_Hz, uint32_t baudRate_Bps)
 {
+    int keep_frozen = 0;
+
+    if (base->MCR & CAN_MCR_FRZACK_MASK)
+        keep_frozen = 1;
+
     /* Enter Freeze Mode. */
     FLEXCAN_EnterFreezeMode(base);
 
     FLEXCAN_SetBaudRate(base, sourceClock_Hz, baudRate_Bps);
 
     /* Exit Freeze Mode. */
-    FLEXCAN_ExitFreezeMode(base);
+    if (!keep_frozen)
+	    FLEXCAN_ExitFreezeMode(base);
+}
+
+void FLEXCAN_SetMode(CAN_Type *base, uint32_t mode)
+{
+	int keep_frozen = 0;
+
+	if (base->MCR & CAN_MCR_FRZACK_MASK)
+		keep_frozen = 1;
+	/* Enter Freeze Mode. */
+	FLEXCAN_EnterFreezeMode(base);
+	switch (mode){
+	case CAN_CTRLMODE_3_SAMPLES:
+		base->CTRL1 &= ~CAN_CTRL1_LPB_MASK;
+		base->CTRL1 &= ~CAN_CTRL1_LOM_MASK;
+		base->CTRL1 |= CAN_CTRL1_SMP_MASK;
+		break;
+	case CAN_CTRLMODE_LISTENONLY:
+		base->CTRL1 &= ~CAN_CTRL1_LPB_MASK;
+		base->CTRL1 &= ~CAN_CTRL1_SMP_MASK;
+		base->CTRL1 |= CAN_CTRL1_LOM_MASK;
+		break;
+	case CAN_CTRLMODE_LOOPBACK:
+		base->CTRL1 &= ~CAN_CTRL1_SMP_MASK;
+		base->CTRL1 &= ~CAN_CTRL1_LOM_MASK;
+		base->CTRL1 |= CAN_CTRL1_LPB_MASK;
+		break;
+	case CAN_CTRLMODE_NORMAL:
+		base->CTRL1 &= ~CAN_CTRL1_LPB_MASK;
+		base->CTRL1 &= ~CAN_CTRL1_LOM_MASK;
+		base->CTRL1 &= ~CAN_CTRL1_SMP_MASK;
+	}
+	/* Exit Freeze Mode. */
+	if (!keep_frozen)
+		FLEXCAN_ExitFreezeMode(base);
 }
 
 void FLEXCAN_SetRxMbGlobalMask(CAN_Type *base, uint32_t mask)
 {
     /* Enter Freeze Mode. */
-    FLEXCAN_EnterFreezeMode(base);
+    //FLEXCAN_EnterFreezeMode(base);
 
     /* Setting Rx Message Buffer Global Mask value. */
     base->RXMGMASK = mask;
@@ -578,19 +609,19 @@ void FLEXCAN_SetRxMbGlobalMask(CAN_Type *base, uint32_t mask)
     base->RX15MASK = mask;
 
     /* Exit Freeze Mode. */
-    FLEXCAN_ExitFreezeMode(base);
+    //FLEXCAN_ExitFreezeMode(base);
 }
 
 void FLEXCAN_SetRxFifoGlobalMask(CAN_Type *base, uint32_t mask)
 {
     /* Enter Freeze Mode. */
-    FLEXCAN_EnterFreezeMode(base);
+    //FLEXCAN_EnterFreezeMode(base);
 
     /* Setting Rx FIFO Global Mask value. */
     base->RXFGMASK = mask;
 
     /* Exit Freeze Mode. */
-    FLEXCAN_ExitFreezeMode(base);
+    //FLEXCAN_ExitFreezeMode(base);
 }
 
 void FLEXCAN_SetRxIndividualMask(CAN_Type *base, uint8_t maskIdx, uint32_t mask)
@@ -678,7 +709,7 @@ void FLEXCAN_SetRxFifoConfig(CAN_Type *base, const flexcan_rx_fifo_config_t *con
     uint8_t setup_mb, i, rffn = 0;
 
     /* Enter Freeze Mode. */
-    FLEXCAN_EnterFreezeMode(base);
+    //FLEXCAN_EnterFreezeMode(base);
 
     if (enable)
     {
@@ -771,7 +802,7 @@ void FLEXCAN_SetRxFifoConfig(CAN_Type *base, const flexcan_rx_fifo_config_t *con
     }
     base->MCR |= CAN_MCR_SRXDIS_MASK;
     /* Exit Freeze Mode. */
-    FLEXCAN_ExitFreezeMode(base);
+    //FLEXCAN_ExitFreezeMode(base);
 }
 
 #if (defined(FSL_FEATURE_FLEXCAN_HAS_RX_FIFO_DMA) && FSL_FEATURE_FLEXCAN_HAS_RX_FIFO_DMA)
@@ -843,8 +874,8 @@ status_t FLEXCAN_WriteTxMb(CAN_Type *base, uint8_t mbIdx, const flexcan_frame_t
         base->MB[mbIdx].CS = cs_temp;
 
 #if (defined(FSL_FEATURE_FLEXCAN_HAS_ERRATA_5641) && FSL_FEATURE_FLEXCAN_HAS_ERRATA_5641)
-        base->MB[FLEXCAN_GetFirstValidMb(base)].CS = CAN_CS_CODE(kFLEXCAN_TxMbInactive);
-        base->MB[FLEXCAN_GetFirstValidMb(base)].CS = CAN_CS_CODE(kFLEXCAN_TxMbInactive);
+        base->MB[8].CS = CAN_CS_CODE(kFLEXCAN_TxMbInactive);
+        base->MB[8].CS = CAN_CS_CODE(kFLEXCAN_TxMbInactive);
 #endif
 
         return kStatus_Success;
@@ -1162,7 +1193,7 @@ void FLEXCAN_TransferAbortSend(CAN_Type *base, flexcan_handle_t *handle, uint8_t
     assert(!FLEXCAN_IsMbOccupied(base, mbIdx));
 
     /* Disable Message Buffer Interrupt. */
-    FLEXCAN_DisableMbInterrupts(base, 1 << mbIdx);
+    //FLEXCAN_DisableMbInterrupts(base, 1 << mbIdx);
 
     /* Un-register handle. */
     handle->mbFrameBuf[mbIdx] = 0x0;
@@ -1214,6 +1245,7 @@ void FLEXCAN_TransferHandleIRQ(CAN_Type *base, flexcan_handle_t *handle)
 
     status_t status = kStatus_FLEXCAN_UnHandled;
     uint32_t result;
+    BaseType_t reschedule = pdFALSE;
 
     /* Store Current FlexCAN Module Error and Status. */
     result = base->ESR1;
@@ -1234,7 +1266,7 @@ void FLEXCAN_TransferHandleIRQ(CAN_Type *base, flexcan_handle_t *handle)
         else
         {
             /* For this implementation, we solve the Message with lowest MB index first. */
-            for (result = 0; result < FSL_FEATURE_FLEXCAN_HAS_MESSAGE_BUFFER_MAX_NUMBERn(base); result++)
+            for (result = 0; result < 10/* FSL_FEATURE_FLEXCAN_HAS_MESSAGE_BUFFER_MAX_NUMBERn(base) */; result++)
             {
                 /* Get the lowest unhandled Message Buffer */
                 if ((FLEXCAN_GetMbStatusFlags(base, 1 << result)) && (FLEXCAN_IsMbIntEnabled(base, result)))
@@ -1244,7 +1276,7 @@ void FLEXCAN_TransferHandleIRQ(CAN_Type *base, flexcan_handle_t *handle)
             }
 
             /* Does not find Message to deal with. */
-            if (result == FSL_FEATURE_FLEXCAN_HAS_MESSAGE_BUFFER_MAX_NUMBERn(base))
+            if (result == 10)
             {
                 break;
             }
@@ -1323,7 +1355,8 @@ void FLEXCAN_TransferHandleIRQ(CAN_Type *base, flexcan_handle_t *handle)
         /* Calling Callback Function if has one. */
         if (handle->callback != NULL)
         {
-            handle->callback(base, handle, status, result, handle->userData);
+            if (handle->callback(base, handle, status, result, handle->userData) == pdTRUE)
+        	    reschedule = pdTRUE;
         }
 
         /* Reset return status */
@@ -1341,6 +1374,7 @@ void FLEXCAN_TransferHandleIRQ(CAN_Type *base, flexcan_handle_t *handle)
             (0 != (result & (kFLEXCAN_TxWarningIntFlag | kFLEXCAN_RxWarningIntFlag | kFLEXCAN_BusOffIntFlag |
                             kFLEXCAN_ErrorIntFlag | kFLEXCAN_WakeUpIntFlag))));
 #endif
+    portYIELD_FROM_ISR(reschedule);
 }
 
 #if defined(CAN0)
diff --git a/drivers/fsl_flexcan.h b/drivers/fsl_flexcan.h
index e6baa93..40246fe 100644
--- a/drivers/fsl_flexcan.h
+++ b/drivers/fsl_flexcan.h
@@ -31,6 +31,8 @@
 #define _FSL_FLEXCAN_H_
 
 #include "fsl_common.h"
+#include "FreeRTOS.h"
+#include "task.h"
 
 /*!
  * @addtogroup flexcan_driver
@@ -41,6 +43,11 @@
  * Definitions
  *****************************************************************************/
 
+#define CAN_CTRLMODE_NORMAL		0x00	/* normal mode */
+#define CAN_CTRLMODE_LOOPBACK		0x01	/* Loopback mode */
+#define CAN_CTRLMODE_LISTENONLY		0x02 	/* Listen-only mode */
+#define CAN_CTRLMODE_3_SAMPLES		0x03	/* Triple sampling mode */
+
 /*! @name Driver version */
 /*@{*/
 /*! @brief FlexCAN driver version 2.2.0. */
@@ -400,7 +407,7 @@ typedef struct _flexcan_handle flexcan_handle_t;
  *  If the status equals to other FlexCAN Message Buffer transfer status, the result is meaningless and should be
  *  Ignored.
  */
-typedef void (*flexcan_transfer_callback_t)(
+typedef BaseType_t (*flexcan_transfer_callback_t)(
     CAN_Type *base, flexcan_handle_t *handle, status_t status, uint32_t result, void *userData);
 
 /*! @brief FlexCAN handle structure. */
@@ -487,6 +494,24 @@ void FLEXCAN_GetDefaultConfig(flexcan_config_t *config);
  */
 
 /*!
+ * @brief Enter FlexCAN Freeze Mode.
+ *
+ * This function makes the FlexCAN work under Freeze Mode.
+ *
+ * @param base FlexCAN peripheral base address.
+ */
+void FLEXCAN_EnterFreezeMode(CAN_Type *base);
+
+/*!
+ * @brief Exit FlexCAN Freeze Mode.
+ *
+ * This function makes the FlexCAN leave Freeze Mode.
+ *
+ * @param base FlexCAN peripheral base address.
+ */
+void FLEXCAN_ExitFreezeMode(CAN_Type *base);
+
+/*!
  * @brief Sets the FlexCAN protocol timing characteristic.
  *
  * This function gives user settings to CAN bus timing characteristic.
@@ -504,6 +529,8 @@ void FLEXCAN_SetTimingConfig(CAN_Type *base, const flexcan_timing_config_t *conf
 
 void FLEXCAN_SetBitRate(CAN_Type *base, uint32_t sourceClock_Hz, uint32_t baudRate_Bps);
 
+void FLEXCAN_SetMode(CAN_Type *base, uint32_t mode);
+
 /*!
  * @brief Sets the FlexCAN receive message buffer global mask.
  *
diff --git a/source/adc_task.c b/source/adc_task.c
index 2615943..daf4716 100644
--- a/source/adc_task.c
+++ b/source/adc_task.c
@@ -270,7 +270,7 @@ int tsc_registers(dspi_transfer_t *spi_transfer)
 	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_TSCREG:
-			return -ENOENT;
+			return 0;
 		default:
 			return -ENOENT;
 		}
@@ -320,7 +320,7 @@ int adc_registers(dspi_transfer_t *spi_transfer)
 	if (rx_buf[0] == APALIS_TK1_K20_WRITE_INST) {
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_ADCREG:
-			return -ENOENT;
+			return 0;
 		default:
 			return -ENOENT;
 		}
diff --git a/source/can_task.c b/source/can_task.c
index bab19ac..e34d35d 100644
--- a/source/can_task.c
+++ b/source/can_task.c
@@ -14,19 +14,13 @@
 #define CAN_HEADER_MAX_LEN	5
 #define CAN_TRANSFER_BUF_LEN	(CAN_HEADER_MAX_LEN + CAN_FRAME_MAX_LEN)
 
-#define CAN_CTRLMODE_NORMAL		0x00	/* normal mode */
-#define CAN_CTRLMODE_LOOPBACK		0x01	/* Loopback mode */
-#define CAN_CTRLMODE_LISTENONLY		0x02 	/* Listen-only mode */
-#define CAN_CTRLMODE_3_SAMPLES		0x04	/* Triple sampling mode */
-#define CAN_CTRLMODE_ONE_SHOT		0x08	/* One-Shot mode */
-#define CAN_CTRLMODE_BERR_REPORTING	0x10	/* Bus-error reporting */
-
-#define CANCTRL_MODMASK	0x03
-#define CANCTRL_INTMASK	0x38
-#define CANCTRL_INTEN	BIT(2)
-#define CANINTF_RX	BIT(3)
-#define CANINTF_TX	BIT(4)
-#define CANINTF_ERR	BIT(5)
+#define CANCTRL_MODMASK		(BIT(1) | BIT(0))
+#define CANCTRL_INTEN		BIT(2)
+#define CANINTF_RX		BIT(3)
+#define CANINTF_TX		BIT(4)
+#define CANINTF_ERR		BIT(5)
+#define CANCTRL_ENABLE		BIT(6)
+#define CANCTRL_INTMASK	(CANINTF_RX | CANINTF_TX | CANINTF_ERR)
 
 #define EFLG_EWARN	0x01
 #define EFLG_RXWAR	0x02
@@ -39,24 +33,26 @@
 struct can_registers {
 	CAN_Type *base;
 	flexcan_handle_t handle;
-	uint8_t can_status_reg;
+	uint8_t can_enable;
 	uint8_t can_err_reg;
 	uint8_t can_mode;
 	uint8_t frames_in_buf;
 	uint8_t rx_buf_top;
 	uint8_t rx_buf_bottom;
+	uint8_t tx_counter;
 };
 
 static uint8_t data_buffer[2][CAN_RX_BUF_SIZE][CAN_TRANSFER_BUF_LEN];
 
 static struct can_registers can_regs[2];
 
-static void generate_can_irq(uint8_t id)
+static inline void generate_can_irq(uint8_t id)
 {
-	if (id == 0)
+	if (id == 0) {
 		GPIO_TogglePinsOutput(GPIOB, 1u << 8u);
-	else
+	} else {
 		GPIO_TogglePinsOutput(GPIOE, 1u << 26u);
+	}
 }
 
 void can_tx_notify_task(void *pvParameters)
@@ -66,17 +62,25 @@ void can_tx_notify_task(void *pvParameters)
 	while(1){
 		xTaskNotifyWait( 0x00, 0xFFFFFFFF, &ulInterruptStatus, portMAX_DELAY);
 		if (ulInterruptStatus & 0x01) {
-			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= CANINTF_TX;
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] &= ~CANINTF_TX;
 			generate_can_irq(0);
 		}
 		if (ulInterruptStatus & 0x02) {
-			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= CANINTF_TX;
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] &= ~CANINTF_TX;
+			generate_can_irq(1);
+		}
+		if (ulInterruptStatus & 0x04) {
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= CANINTF_ERR;
+			generate_can_irq(0);
+		}
+		if (ulInterruptStatus & 0x08) {
+			registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= CANINTF_ERR;
 			generate_can_irq(1);
 		}
 	}
 }
 
-static void flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t status, uint32_t result, void *userData)
+static BaseType_t flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t status, uint32_t result, void *userData)
 {
 	callback_message_t * cb = (callback_message_t*) userData;
 	BaseType_t reschedule = pdFALSE;
@@ -94,6 +98,7 @@ static void flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t
 			switch ((int)base)
 			{
 			case (int)CAN0:
+
 				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x01, eSetBits, &reschedule);
 				break;
 			case (int)CAN1:
@@ -104,10 +109,40 @@ static void flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t
 		}
 		break;
 
+	case kStatus_FLEXCAN_ErrorStatus:
+		if ((result & (kFLEXCAN_TxWarningIntFlag)) != 0) {
+			switch ((int)base)
+			{
+			case (int)CAN0:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= EFLG_TXWAR;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x04, eSetBits, &reschedule);
+				break;
+			case (int)CAN1:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= EFLG_TXWAR;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x08, eSetBits, &reschedule);
+				break;
+			}
+		}
+
+		if ((result & (kFLEXCAN_BusOffIntFlag)) != 0) {
+			switch ((int)base)
+			{
+			case (int)CAN0:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(0)] |= EFLG_TXBO;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x04, eSetBits, &reschedule);
+				break;
+			case (int)CAN1:
+				registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(1)] |= EFLG_TXBO;
+				xTaskNotifyFromISR(can_tx_notify_task_handle, 0x08, eSetBits, &reschedule);
+				break;
+			}
+		}
+		break;
 	default:
 		break;
 	}
-	portYIELD_FROM_ISR(reschedule);
+
+	return reschedule;
 }
 
 static void CAN_Init(uint8_t id)
@@ -139,7 +174,11 @@ static void CAN_Init(uint8_t id)
 	FLEXCAN_SetRxFifoConfig(can_regs[id].base, &fifoConfig, true);
 
 	/* errata #5641 */
-	FLEXCAN_SetTxMbConfig(can_regs[id].base, TX_MESSAGE_BUFFER_NUM0 - 1, true);
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].ID = 0x0;
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].WORD0 = 0x0;
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].WORD1 = 0x0;
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].CS = CAN_CS_CODE(0x8);
+	can_regs[id].base->MB[TX_MESSAGE_BUFFER_NUM0 - 1].CS = CAN_CS_CODE(0x8);
 
 	/* Setup Tx Message Buffer. */
 	FLEXCAN_SetTxMbConfig(can_regs[id].base, TX_MESSAGE_BUFFER_NUM0, true);
@@ -148,6 +187,7 @@ static void CAN_Init(uint8_t id)
 uint8_t available_data[2];
 
 static void can_calculate_available_data(uint8_t id) {
+	taskENTER_CRITICAL();
 	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
@@ -155,13 +195,17 @@ static void can_calculate_available_data(uint8_t id) {
 
 	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;
+	else
+		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] |= CANINTF_RX;
+	taskEXIT_CRITICAL();
 }
 
 void can_spi_read_complete(uint8_t id)
 {
-
+	can_calculate_available_data(id);
 }
 
 static void frame_to_buffer(flexcan_frame_t *frame, uint8_t id) {
@@ -195,24 +239,26 @@ static void frame_to_buffer(flexcan_frame_t *frame, uint8_t id) {
 	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 %= CAN_RX_BUF_SIZE;
+
 	can_calculate_available_data(id);
 }
 
-static void can_fifo_rx(uint8_t id, flexcan_fifo_transfer_t * rxXfer)
+static inline void can_fifo_rx(uint8_t id, flexcan_fifo_transfer_t * rxXfer)
 {
 	callback_message_t *can_msg = (callback_message_t *) can_regs[id].handle.userData;
-	taskENTER_CRITICAL();
+
 	FLEXCAN_TransferReceiveFifoNonBlocking(can_regs[id].base, &can_regs[id].handle, rxXfer);
-	taskEXIT_CRITICAL();
 	xSemaphoreTake(can_msg->sem, portMAX_DELAY);
-	frame_to_buffer(rxXfer->frame, id);
-	can_regs[id].frames_in_buf++;
-	registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] |= CANINTF_RX;
-	generate_can_irq(id);
-	if (can_regs[id].frames_in_buf >= CAN_RX_BUF_SIZE)
-		vTaskSuspend(NULL);
+	if (can_msg->async_status == pdTRUE) {
+		frame_to_buffer(rxXfer->frame, id);
+		can_regs[id].frames_in_buf++;
+		generate_can_irq(id);
+		if (can_regs[id].frames_in_buf >= CAN_RX_BUF_SIZE)
+			vTaskSuspend(NULL);
+	}
 }
 
 void can0_task(void *pvParameters) {
@@ -263,16 +309,25 @@ void can1_task(void *pvParameters) {
 
 static void can_change_mode(int id, uint8_t new_mode)
 {
-	CAN_Type *base = id ? CAN1:CAN0;
-
 	can_regs[id].can_mode = new_mode;
 
-	switch (new_mode){
-	case CAN_CTRLMODE_LOOPBACK:
-		base->CTRL1 = base->CTRL1 | CAN_CTRL1_LPB_MASK;
-		break;
-	case CAN_CTRLMODE_NORMAL:
-		base->CTRL1 = base->CTRL1 & ~CAN_CTRL1_LPB_MASK;
+	FLEXCAN_SetMode(can_regs[id].base, new_mode);
+
+}
+
+static void can_enable(int id, uint8_t enable)
+{
+	can_regs[id].can_mode = enable;
+
+	if (enable) {
+		callback_message_t *can_msg = (callback_message_t *) can_regs[id].handle.userData;
+		FLEXCAN_ExitFreezeMode(can_regs[id].base);
+		can_msg->async_status = pdFALSE;
+		xSemaphoreGive(can_msg->sem);
+		generate_can_irq(id);
+	} else {
+		FLEXCAN_TransferAbortReceiveFifo(can_regs[id].base, &can_regs[id].handle);
+		FLEXCAN_EnterFreezeMode(can_regs[id].base);
 	}
 
 }
@@ -282,18 +337,21 @@ static uint8_t set_canreg (int id, uint8_t 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));
+	if (can_regs[id].can_enable != (value & CANCTRL_ENABLE))
+		can_enable(id, (value & CANCTRL_ENABLE));
 	return 0;
 }
 
 static uint8_t clr_canreg (int id, uint8_t mask)
 {
-	mask &= (CANINTF_TX | CANINTF_ERR);
+	mask &= (CANINTF_RX | CANINTF_TX | CANINTF_ERR);
 	registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] &= ~mask;
 	return 0;
 }
 
 static uint8_t set_canerr (int id, uint8_t value)
 {
+	registers[APALIS_TK1_K20_CANERR + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] = value;
 	return 0;
 }
 
@@ -304,9 +362,9 @@ static uint8_t set_canbadreg (int id, uint8_t value)
 	return 0;
 }
 
-static uint8_t set_canbittimig (int id, uint16_t value, int16_t mask)
+static uint8_t set_canbittiming (int id, uint16_t value, int16_t mask)
 {
-	/* According to NXP we should use default setting */
+	/* According to NXP we should use default settings */
 	return 0;
 }
 
@@ -339,12 +397,16 @@ uint8_t can_sendframe(uint8_t id, uint8_t *data, uint8_t len)
 
 	txXfer.frame = &tx_frame;
 	txXfer.mbIdx = TX_MESSAGE_BUFFER_NUM0;
-
-	taskENTER_CRITICAL();
-	FLEXCAN_TransferSendNonBlocking(can_regs[id].base , &can_regs[id].handle, &txXfer);
-	taskEXIT_CRITICAL();
-
-	return 0;
+	if( tx_frame.length <= 8 ) {
+		FLEXCAN_TransferSendNonBlocking(can_regs[id].base , &can_regs[id].handle, &txXfer);
+		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] |= CANINTF_TX;
+		return 0;
+	}
+	else {
+		registers[APALIS_TK1_K20_CANREG + APALIS_TK1_K20_CAN_DEV_OFFSET(id)] &= ~CANINTF_TX;
+		generate_can_irq(id);
+		return -EIO;
+	}
 }
 
 uint16_t can_readframe(uint8_t id, dspi_transfer_t *spi_transfer)
@@ -354,11 +416,11 @@ uint16_t can_readframe(uint8_t id, dspi_transfer_t *spi_transfer)
 	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 %= CAN_RX_BUF_SIZE;
 
 	can_regs[id].frames_in_buf -= rx_size;
-	can_calculate_available_data(id);
 
 	return rx_size * CAN_TRANSFER_BUF_LEN;
 }
@@ -378,9 +440,9 @@ int canx_registers(dspi_transfer_t *spi_transfer, int id)
 		case APALIS_TK1_K20_CAN_BAUD_REG:
 			return set_canbadreg(id, rx_buf[2]);
 		case APALIS_TK1_K20_CAN_BIT_1:
-			return set_canbittimig(id, rx_buf[2], 0x00FF);
+			return set_canbittiming(id, rx_buf[2], 0x00FF);
 		case APALIS_TK1_K20_CAN_BIT_2:
-			return set_canbittimig(id, (rx_buf[2] << 8), 0xFF00);
+			return set_canbittiming(id, (rx_buf[2] << 8), 0xFF00);
 		default:
 			return -EIO;
 		}
@@ -398,7 +460,7 @@ int canx_registers(dspi_transfer_t *spi_transfer, int id)
 		switch (rx_buf[1]) {
 		case APALIS_TK1_K20_CAN_OUT_BUF:
 			can_sendframe(id, &rx_buf[3], rx_buf[2]);
-			break;
+			return 0;
 		default:
 			return -EIO;
 		}
diff --git a/source/com_task.c b/source/com_task.c
index 56a4ce2..d6bc97d 100644
--- a/source/com_task.c
+++ b/source/com_task.c
@@ -7,7 +7,7 @@
 volatile uint8_t registers[APALIS_TK1_K20_LAST_REG];
 volatile uint8_t regRxHandled;
 
-/* Put FW version at known address in a binary. Make it 32-bit to have room for the future*/
+/* Put FW version at known address in a binary. Make it 32-bit to have room for the future */
 #ifndef TESTER_BUILD
 const uint32_t __attribute__((section(".FwVersion"), used)) fw_version = APALIS_TK1_K20_FW_VER;
 #else
@@ -18,17 +18,20 @@ static dspi_slave_handle_t spi_handle;
 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) {
-	registers[APALIS_TK1_K20_IRQREG] |= BIT(irq);
-	/* Toggle INT1 pin */
-	GPIO_TogglePinsOutput(GPIOA, 1u << 16u);
+	if (!(registers[APALIS_TK1_K20_MSQREG] & BIT(irq))) {
+		taskENTER_CRITICAL();
+		registers[APALIS_TK1_K20_IRQREG] |= BIT(irq);
+		/* Toggle INT1 pin */
+		GPIO_TogglePinsOutput(GPIOA, 1u << 16u);
+		taskEXIT_CRITICAL();
+	}
 }
 
 void clear_irq_flag(uint8_t irq) {
@@ -110,7 +113,8 @@ inline int general_registers(dspi_transfer_t * spi_transfer)
 	return -ENXIO;
 }
 
-static void SPI_callback(SPI_Type *base, dspi_slave_handle_t *handle, status_t status, void *userData)
+
+inline void SPI_main_callback(status_t status, void *userData)
 {
 	callback_message_t * cb = (callback_message_t*) userData;
 	BaseType_t reschedule = pdFALSE;
@@ -119,14 +123,25 @@ static void SPI_callback(SPI_Type *base, dspi_slave_handle_t *handle, status_t s
 	{
 		xSemaphoreGiveFromISR(cb->sem, &reschedule);
 	}
-
+#if 0
 	if (status == kStatus_DSPI_Error)
 	{
 		__NOP();
 	}
+#endif
 	portYIELD_FROM_ISR(reschedule);
 }
 
+static void SPI_callback(SPI_Type *base, dspi_slave_handle_t *handle, status_t status, void *userData)
+{
+	SPI_main_callback(status, userData);
+}
+
+static void SPI_EDMA_callback(SPI_Type *base, dspi_slave_edma_handle_t *handle, status_t status, void *userData)
+{
+	SPI_main_callback(status, userData);
+}
+
 static dspi_slave_config_t spi2_slaveConfig;
 
 static void SPI_init() {
@@ -184,9 +199,10 @@ void spi_task(void *pvParameters) {
 	EDMA_CreateHandle(&dspiEdmaSlaveTxHandle, DMA0, slaveTxChannel);
 
 	g_dspi_edma_s_handle.userData = &spi_msg;
-	DSPI_SlaveTransferCreateHandleEDMA(SPI2, &g_dspi_edma_s_handle, SPI_callback, &spi_msg, &dspiEdmaSlaveRxHandle, &dspiEdmaSlaveTxHandle);
+	DSPI_SlaveTransferCreateHandleEDMA(SPI2, &g_dspi_edma_s_handle, SPI_EDMA_callback,
+			&spi_msg, &dspiEdmaSlaveRxHandle, &dspiEdmaSlaveTxHandle);
 #endif
-	memset(registers, 0x00, APALIS_TK1_K20_LAST_REG);
+	memset((uint8_t *)registers, 0x00, APALIS_TK1_K20_LAST_REG);
 	registers[APALIS_TK1_K20_REVREG] = APALIS_TK1_K20_FW_VER;
 	GPIO_SetPinsOutput(GPIOA, 1u << 29u); /* INT2 idle */
 	slaveXfer.configFlags = kDSPI_SlaveCtar0;
@@ -198,12 +214,12 @@ void spi_task(void *pvParameters) {
 		/* Wait for instructions from SoC */
 		ret = DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
 		if ( ret == kStatus_Success) {
-
 			xSemaphoreTake(spi_msg.sem, portMAX_DELAY);
 
-			slaveXfer.txData = slaveTxData;
-			slaveXfer.rxData = slaveRxData;
 			if (slaveRxData[0] != APALIS_TK1_K20_READ_INST) {
+				taskENTER_CRITICAL();
+				slaveXfer.txData = slaveTxData;
+				slaveXfer.rxData = slaveRxData;
 				if (slaveRxData[1] <= 0x05) {
 					ret = general_registers(&slaveXfer);
 #ifndef TESTER_BUILD
@@ -231,34 +247,33 @@ void spi_task(void *pvParameters) {
 					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;
+					PRINTF("Invalid read/write ret = %d rx[0] = 0x%x, rx[1] = 0x%x, rx[2] = 0x%x\r\n",
+							ret, slaveRxData[0], slaveRxData[1], slaveRxData[2]);
 				}
 
-				if (slaveRxData[0] == APALIS_TK1_K20_BULK_READ_INST)
-				{
+				if (slaveRxData[0] == APALIS_TK1_K20_BULK_READ_INST) {
 					slaveXfer.dataSize = ret;
-					slaveXfer.rxData = NULL; /* We're not expecting any MOSI traffic, but NULL messes up stuff */
+					slaveXfer.rxData = NULL;
 #ifdef SPI_DMA
 					DSPI_SlaveTransferEDMA(SPI2, &g_dspi_edma_s_handle, &slaveXfer);
 #else
 					DSPI_SlaveTransferNonBlocking(SPI2, &spi_handle, &slaveXfer);
 #endif
+					taskEXIT_CRITICAL();
 					xSemaphoreTake(spi_msg.sem, portMAX_DELAY);
+
 					if (can_read >= 0) {
 						can_spi_read_complete(can_read);
 						can_read = -1;
 					}
+				} else {
+					taskEXIT_CRITICAL();
 				}
 			}
-
+		} else {
+			/* Something went wrong, retry */
+			DSPI_SlaveTransferAbort(SPI2, &spi_handle);
 		}
 	}
 }
diff --git a/source/com_task.h b/source/com_task.h
index 4a9557d..a541f88 100644
--- a/source/com_task.h
+++ b/source/com_task.h
@@ -22,6 +22,8 @@
 #include "semphr.h"
 #include "errno.h"
 
+#define SPI_DMA
+
 typedef struct _callback_message_t
 {
 	status_t async_status;