diff --git a/firmware/config/boards/microrusefi/board_configuration.cpp b/firmware/config/boards/microrusefi/board_configuration.cpp
index 515a239f87..77b74f4910 100644
--- a/firmware/config/boards/microrusefi/board_configuration.cpp
+++ b/firmware/config/boards/microrusefi/board_configuration.cpp
@@ -37,10 +37,10 @@ const ConfigOverrides& getConfigOverrides() {
 }
 
 static void setInjectorPins() {
-	engineConfiguration->injectionPins[0] = GPIOE_14;
-	engineConfiguration->injectionPins[1] = GPIOE_13;
-	engineConfiguration->injectionPins[2] = GPIOE_12;
-	engineConfiguration->injectionPins[3] = GPIOE_11;
+	engineConfiguration->injectionPins[0] = TLE8888_PIN_1;
+	engineConfiguration->injectionPins[1] = TLE8888_PIN_2;
+	engineConfiguration->injectionPins[2] = TLE8888_PIN_3;
+	engineConfiguration->injectionPins[3] = TLE8888_PIN_4;
 
 	// Disable remainder
 	for (int i = 4; i < INJECTION_PIN_COUNT;i++) {
@@ -211,8 +211,8 @@ void setBoardConfigurationOverrides(void) {
 	// rusEfi firmware is totally not involved with main relay control on microRusEfi board
 	// todo: maybe even set EFI_MAIN_RELAY_CONTROL to FALSE for MRE configuration
 	// TLE8888 half bridges (pushpull, lowside, or high-low)  TLE8888_IN11 / TLE8888_OUT21
-	// GPIOE_8: "35 - GP Out 1"
-	engineConfiguration->fuelPumpPin = GPIOE_8;
+	// TLE8888_PIN_21: "35 - GP Out 1"
+	engineConfiguration->fuelPumpPin = TLE8888_PIN_21;
 
 	engineConfiguration->sdCardSpiDevice = SPI_DEVICE_3;
 	engineConfiguration->spi3mosiPin = GPIOC_12;
@@ -225,7 +225,7 @@ void setBoardConfigurationOverrides(void) {
 
 	// TLE8888 high current low side: VVT2 IN9 / OUT5
 	// GPIOE_10: "3 - Lowside 2"
-	engineConfiguration->idle.solenoidPin = GPIOE_10;
+	engineConfiguration->idle.solenoidPin = TLE8888_PIN_5;
 
 
 	// TLE8888_PIN_22: "34 - GP Out 2"
diff --git a/firmware/config/boards/microrusefi/mapping.yaml b/firmware/config/boards/microrusefi/mapping.yaml
index d031bf515e..b99cbf38d2 100644
--- a/firmware/config/boards/microrusefi/mapping.yaml
+++ b/firmware/config/boards/microrusefi/mapping.yaml
@@ -5,10 +5,10 @@
 
 outputs:
   # TLE8888 injector channels
-  GPIOE_14: "37 - Injector 1"
-  GPIOE_13: "38 - Injector 2"
-  GPIOE_12: "41 - Injector 3"
-  GPIOE_11: "42 - Injector 4"
+  TLE8888_PIN_1: "37 - Injector 1"
+  TLE8888_PIN_2: "38 - Injector 2"
+  TLE8888_PIN_3: "41 - Injector 3"
+  TLE8888_PIN_4: "42 - Injector 4"
   # TC4427 ignition outputs (5v)
   GPIOD_4: "9 - Ignition 1"
   GPIOD_3: "10 - Ignition 2"
@@ -18,17 +18,18 @@ outputs:
   GPIOD_7: "14 - GP Out 5"
   GPIOD_6: "13 - GP Out 6"
 
-  # default VVT TLE8888 high current low side: VVT1 TLE8888_IN10 / TLE8888_OUT6
-  GPIOE_9:  "7 - Lowside 1"
-  # default Idle Air Control TLE8888 high current low side: VVT2 TLE8888_IN9 / TLE8888_OUT5
-  GPIOE_10: "3 - Lowside 2"
+  # default VVT TLE8888 high current low side: VVT1 TLE8888_OUT6
+  TLE8888_PIN_6: "7 - Lowside 1"
+  # default Idle Air Control TLE8888 high current low side: VVT2 TLE8888_OUT5
+  TLE8888_PIN_5: "3 - Lowside 2"
 
-  # TLE8888 half bridges (pushpull, lowside, or high-low)  TLE8888_IN11 / TLE8888_OUT21#91
-  GPIOE_8: "35 - GP Out 1"
-  # TLE8888 half bridges (pushpull, lowside, or high-low)  IN? / TLE8888_OUT22#89
-  # this should work but it does not GPIOE_7: "34 - GP Out 2"
+  # TLE8888 half bridges (pushpull, lowside, or high-low) TLE8888_OUT21#91
+  TLE8888_PIN_21: "35 - GP Out 1"
+  # TLE8888 half bridges (pushpull, lowside, or high-low) TLE8888_OUT22#89
   TLE8888_PIN_22: "34 - GP Out 2"
+  # TLE8888 half bridges (pushpull, lowside, or high-low) TLE8888_OUT23
   TLE8888_PIN_23: "33 - GP Out 3"
+  # TLE8888 half bridges (pushpull, lowside, or high-low) TLE8888_OUT24
   TLE8888_PIN_24: "43 - GP Out 4"
 
   # J2
diff --git a/firmware/config/boards/skeleton/board_configuration.cpp b/firmware/config/boards/skeleton/board_configuration.cpp
index 1b477fe181..50ff2cb7c0 100644
--- a/firmware/config/boards/skeleton/board_configuration.cpp
+++ b/firmware/config/boards/skeleton/board_configuration.cpp
@@ -195,13 +195,13 @@ void setBoardConfigurationOverrides(void) {
 
 	// Configure the TLE8888 half bridges (pushpull, lowside, or high-low)
 	// TLE8888_IN11 -> TLE8888_OUT21
-	// GPIOE_8: "35 - GP Out 1"
-	engineConfiguration->fuelPumpPin = GPIOE_8;
+	// TLE8888_PIN_21: "35 - GP Out 1"
+	engineConfiguration->fuelPumpPin = TLE8888_PIN_21;
 
 
 	// TLE8888 high current low side: VVT2 IN9 / OUT5
-	// GPIOE_10: "3 - Lowside 2"
-	engineConfiguration->idle.solenoidPin = GPIOE_10;
+	// TLE8888_PIN_4: "3 - Lowside 2"
+	engineConfiguration->idle.solenoidPin = TLE8888_PIN_5;
 
 
 	// TLE8888_PIN_22: "34 - GP Out 2"
diff --git a/firmware/config/engines/custom_engine.cpp b/firmware/config/engines/custom_engine.cpp
index 13d5636d66..23e24ee8a9 100644
--- a/firmware/config/engines/custom_engine.cpp
+++ b/firmware/config/engines/custom_engine.cpp
@@ -469,24 +469,24 @@ void mreBoardOldTest(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 
 
 	// LED #1
-    // GPIOE_7: "34 - GP Out 2"
-	engineConfiguration->injectionPins[1 - 1] = TLE8888_PIN_22;//GPIOE_7;
+	// TLE8888_PIN_22: "34 - GP Out 2"
+	engineConfiguration->injectionPins[1 - 1] = TLE8888_PIN_22;
 
 	// LED #2
 	// TLE8888_PIN_23: "33 - GP Out 3"
 	engineConfiguration->injectionPins[10 - 1] = TLE8888_PIN_23;
 
-	// LED #3 - INJ#2
-	engineConfiguration->injectionPins[9 - 1] = GPIOE_13;
+	// TLE8888_PIN_1: LED #3 - INJ#2
+	engineConfiguration->injectionPins[9 - 1] = TLE8888_PIN_1;
 
-	// LED #4 - INJ#1
-	engineConfiguration->injectionPins[4 - 1] = GPIOE_14;
+	// TLE8888_PIN_2: LED #4 - INJ#1
+	engineConfiguration->injectionPins[4 - 1] = TLE8888_PIN_2;
 
-	// LED #5 - INJ#3
-	engineConfiguration->injectionPins[3 - 1] = GPIOE_12;
+	// TLE8888_PIN_3: LED #5 - INJ#3
+	engineConfiguration->injectionPins[3 - 1] = TLE8888_PIN_3;
 
-	// LED #6 - INJ#4
-	engineConfiguration->injectionPins[6 - 1] = GPIOE_11;
+	// TLE8888_PIN_4: LED #6 - INJ#4
+	engineConfiguration->injectionPins[6 - 1] = TLE8888_PIN_4;
 
 	// LED #7
 	// TLE8888_PIN_24: "43 - GP Out 4"
@@ -494,18 +494,18 @@ void mreBoardOldTest(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 
 	// LED #8
 	// TLE8888 half bridges (pushpull, lowside, or high-low)  IN12
-	// GPIOE_8: "35 - GP Out 1"
-	engineConfiguration->injectionPins[8 - 1] = GPIOE_8;
+	// TLE8888_PIN_21: "35 - GP Out 1"
+	engineConfiguration->injectionPins[8 - 1] = TLE8888_PIN_21;
 
 	// LED #9
 	// TLE8888 high current low side: IN10
-	// GPIOE_9:  "7 - Lowside 1"
-	engineConfiguration->injectionPins[7 - 1] = GPIOE_9;
+	// TLE8888_PIN_6: "7 - Lowside 1"
+	engineConfiguration->injectionPins[7 - 1] = TLE8888_PIN_6;
 
 	// LED #10
 	// TLE8888 high current low side: VVT2 IN9 / OUT5
-	// GPIOE_10: "3 - Lowside 2"
-	engineConfiguration->injectionPins[2 - 1] = GPIOE_10;
+	// TLE8888_PIN_5: "3 - Lowside 2"
+	engineConfiguration->injectionPins[2 - 1] = TLE8888_PIN_5;
 #endif /* BOARD_TLE8888_COUNT */
 }
 
@@ -530,17 +530,16 @@ void mreBCM(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 
 
 #if (BOARD_TLE8888_COUNT > 0)
-	engineConfiguration->fsioOutputPins[0] = GPIOE_14;// "37 - Injector 1"
-	engineConfiguration->fsioOutputPins[1] = GPIOE_13;// "38 - Injector 2"
-	engineConfiguration->fsioOutputPins[2] = GPIOE_12;// "41 - Injector 3"
-	engineConfiguration->fsioOutputPins[3] = GPIOE_11;// "42 - Injector 4"
-// 	engineConfiguration->fsioOutputPins[4] = LS1
-// 			engineConfiguration->fsioOutputPins[5] = LS2
-
-				// engineConfiguration->fsioOutputPins[6] = GP1
-	engineConfiguration->fsioOutputPins[7] = TLE8888_PIN_22;// "34 - GP Out 2"
-	engineConfiguration->fsioOutputPins[8] = TLE8888_PIN_23;// "33 - GP Out 3"
-	engineConfiguration->fsioOutputPins[9] = TLE8888_PIN_24;// "43 - GP Out 4"
+	engineConfiguration->fsioOutputPins[0] = TLE8888_PIN_1; // "37 - Injector 1"
+	engineConfiguration->fsioOutputPins[1] = TLE8888_PIN_2; // "38 - Injector 2"
+	engineConfiguration->fsioOutputPins[2] = TLE8888_PIN_3; // "41 - Injector 3"
+	engineConfiguration->fsioOutputPins[3] = TLE8888_PIN_4; // "42 - Injector 4"
+// 	engineConfiguration->fsioOutputPins[4] = TLE8888_PIN_5;
+// 	engineConfiguration->fsioOutputPins[5] = TLE8888_PIN_6;
+//	engineConfiguration->fsioOutputPins[6] = TLE8888_PIN_21;
+	engineConfiguration->fsioOutputPins[7] = TLE8888_PIN_22; // "34 - GP Out 2"
+	engineConfiguration->fsioOutputPins[8] = TLE8888_PIN_23; // "33 - GP Out 3"
+	engineConfiguration->fsioOutputPins[9] = TLE8888_PIN_24; // "43 - GP Out 4"
 #endif /* BOARD_TLE8888_COUNT */
 
 }
@@ -555,9 +554,6 @@ void mreBoardNewTest(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 	engineConfiguration->specs.cylindersCount = 12;
 	engineConfiguration->specs.firingOrder = FO_1_2_3_4_5_6_7_8_9_10_11_12;
 
-
-
-
 #if (BOARD_TLE8888_COUNT > 0)
 	engineConfiguration->ignitionPins[1 - 1] = GPIOD_6;
 	engineConfiguration->ignitionPins[2 - 1] = GPIOD_7;
@@ -571,12 +567,12 @@ void mreBoardNewTest(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 
 	// LED #8
 	// TLE8888 half bridges (pushpull, lowside, or high-low)  IN12
-	// GPIOE_8: "35 - GP Out 1"
-	engineConfiguration->ignitionPins[9 - 1] = GPIOE_8;
+	// TLE8888_PIN_21: "35 - GP Out 1"
+	engineConfiguration->ignitionPins[9 - 1] = TLE8888_PIN_21;
 
 	// LED #1
-    // GPIOE_7: "34 - GP Out 2"
-	engineConfiguration->ignitionPins[10- 1] = TLE8888_PIN_22;//GPIOE_7;
+	// TLE8888_PIN_22: "34 - GP Out 2"
+	engineConfiguration->ignitionPins[10- 1] = TLE8888_PIN_22;
 
 	// LED #2
 	// TLE8888_PIN_23: "33 - GP Out 3"
@@ -589,27 +585,22 @@ void mreBoardNewTest(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 	engineConfiguration->afr.hwChannel = EFI_ADC_6;
 	engineConfiguration->throttlePedalPositionAdcChannel = EFI_ADC_NONE;
 
-
-
-
-
 	// TLE8888 high current low side: IN10
-	// GPIOE_9:  "7 - Lowside 1"
-	engineConfiguration->injectionPins[1 - 1] = GPIOE_9;
+	// TLE8888_PIN_6:  "7 - Lowside 1"
+	engineConfiguration->injectionPins[1 - 1] = TLE8888_PIN_6;
 
 	// TLE8888 high current low side: VVT2 IN9 / OUT5
-	// GPIOE_10: "3 - Lowside 2"
-	engineConfiguration->injectionPins[2 - 1] = GPIOE_10;
+	// TLE8888_PIN_5: "3 - Lowside 2"
+	engineConfiguration->injectionPins[2 - 1] = TLE8888_PIN_5;
 
-	//          INJ#4
-	engineConfiguration->injectionPins[3 - 1] = GPIOE_11;
-	//          INJ#3
-	engineConfiguration->injectionPins[4 - 1] = GPIOE_12;
-	//          INJ#2
-	engineConfiguration->injectionPins[5 - 1] = GPIOE_13;
-
-	// LED #3 - INJ#1
-	engineConfiguration->injectionPins[6 - 1] = GPIOE_14;
+	// TLE8888_PIN_4: INJ#4
+	engineConfiguration->injectionPins[3 - 1] = TLE8888_PIN_4;
+	// TLE8888_PIN_3: INJ#3
+	engineConfiguration->injectionPins[4 - 1] = TLE8888_PIN_3;
+	// TLE8888_PIN_2: INJ#2
+	engineConfiguration->injectionPins[5 - 1] = TLE8888_PIN_2;
+	// TLE8888_PIN_1: LED #3 - INJ#1
+	engineConfiguration->injectionPins[6 - 1] = TLE8888_PIN_1;
 
 
 	engineConfiguration->injectionPins[7 - 1] = GPIOA_4; // AV10
@@ -620,11 +611,6 @@ void mreBoardNewTest(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 	engineConfiguration->injectionPins[11- 1] = TLE8888_PIN_13;
 
 	engineConfiguration->injectionPins[12- 1] = TLE8888_PIN_10;
-
-
-
-
-
 #endif /* BOARD_TLE8888_COUNT */
 
 }
diff --git a/firmware/config/engines/lada_kalina.cpp b/firmware/config/engines/lada_kalina.cpp
index 8ed46ebaf4..44152f5f0d 100644
--- a/firmware/config/engines/lada_kalina.cpp
+++ b/firmware/config/engines/lada_kalina.cpp
@@ -13,6 +13,25 @@
 
 EXTERN_CONFIG;
 
+#if (BOARD_TLE8888_COUNT > 0)
+void setLadaKalina(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
+	/* MRE uses TLE8888_PIN_21 for fuel pump */
+	engineConfiguration->fuelPumpPin = GPIO_UNASSIGNED;
+	/* PRE uses TLE8888_PIN_22 for fan */
+	engineConfiguration->fanPin = GPIO_UNASSIGNED;
+	// TLE8888 two bridge drivers for stepper
+	engineConfiguration->etbIo2[0].directionPin1 = TLE8888_PIN_21;
+	engineConfiguration->etbIo2[0].directionPin2 = TLE8888_PIN_22;
+	engineConfiguration->etbIo2[1].directionPin1 = TLE8888_PIN_23;
+	engineConfiguration->etbIo2[1].directionPin2 = TLE8888_PIN_24;
+	/* IDLE configuration */
+	engineConfiguration->useStepperIdle = true;
+	engineConfiguration->useHbridges = true;
+	engineConfiguration->idleMode = IM_AUTO;
+}
+
+#else
+
 void setLadaKalina(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 	setFrankensoConfiguration(PASS_CONFIG_PARAMETER_SIGNATURE);
 	disableLCD(engineConfiguration);
@@ -59,3 +78,4 @@ void setLadaKalina(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 	setFsioExt(0, GPIOE_3, RPM_BELOW_USER_SETTING_1, 0 PASS_CONFIG_PARAMETER_SUFFIX);
 #endif /* EFI_FSIO */
 }
+#endif
diff --git a/firmware/config/engines/vw_b6.cpp b/firmware/config/engines/vw_b6.cpp
index 77d4aa0c30..187633eff7 100644
--- a/firmware/config/engines/vw_b6.cpp
+++ b/firmware/config/engines/vw_b6.cpp
@@ -99,7 +99,7 @@ void setVwPassatB6(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 		}
 	}
 */
-	coolantControl->pin = GPIOE_10; // "3 - Lowside 2"
+	coolantControl->pin = TLE8888_PIN_5; // "3 - Lowside 2"
 
 
 	engineConfiguration->idle.solenoidPin = GPIO_UNASSIGNED;
diff --git a/firmware/controllers/algo/engine.cpp b/firmware/controllers/algo/engine.cpp
index 0b7b3bf326..258e401a7a 100644
--- a/firmware/controllers/algo/engine.cpp
+++ b/firmware/controllers/algo/engine.cpp
@@ -199,7 +199,7 @@ void Engine::periodicSlowCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	if (CONFIG(useTLE8888_cranking_hack) && ENGINE(rpmCalculator).isCranking()) {
 		efitick_t nowNt = getTimeNowNt();
 		if (nowNt - tle8888CrankingResetTime > MS2NT(300)) {
-			requestTLE8888initialization();
+			tle8888_req_init();
 			// let's reset TLE8888 every 300ms while cranking since that's the best we can do to deal with undervoltage reset
 			// PS: oh yes, it's a horrible design! Please suggest something better!
 			tle8888CrankingResetTime = nowNt;
diff --git a/firmware/controllers/bench_test.cpp b/firmware/controllers/bench_test.cpp
index fff7997742..dbe2186bf8 100644
--- a/firmware/controllers/bench_test.cpp
+++ b/firmware/controllers/bench_test.cpp
@@ -286,7 +286,7 @@ static void handleCommandX14(uint16_t index) {
 		return;
 	case 8:
 #if (BOARD_TLE8888_COUNT > 0)
-		requestTLE8888initialization();
+		tle8888_req_init();
 #endif
 		return;
 	case 0xA:
diff --git a/firmware/hw_layer/drivers/gpio/tle8888.c b/firmware/hw_layer/drivers/gpio/tle8888.c
index d0d5ca7e4d..e4c3b44a7d 100644
--- a/firmware/hw_layer/drivers/gpio/tle8888.c
+++ b/firmware/hw_layer/drivers/gpio/tle8888.c
@@ -54,8 +54,6 @@ EXTERN_ENGINE_CONFIGURATION;
 
 #define DRIVER_NAME				"tle8888"
 
-static bool drv_task_ready = false;
-
 typedef enum {
 	TLE8888_DISABLED = 0,
 	TLE8888_WAIT_INIT,
@@ -63,6 +61,7 @@ typedef enum {
 	TLE8888_FAILED
 } tle8888_drv_state;
 
+/* SPI communication helpers */
 /* C0 */
 #define CMD_READ			(0 << 0)
 #define CMD_WRITE			(1 << 0)
@@ -71,38 +70,28 @@ typedef enum {
 /* CD7:0 */
 #define CMD_REG_DATA(d)		(((d) & 0xff) << 8)
 
-#define CMD_WR(a, d)		(CMD_WRITE | CMD_REG_ADDR(a) | CMD_REG_DATA(d))
+#define CMD_W(a, d)			(CMD_WRITE | CMD_REG_ADDR(a) | CMD_REG_DATA(d))
 #define CMD_R(a)			(CMD_READ | CMD_REG_ADDR(a))
 
+#define REG_INVALID			0x00
+
+/* Command registers */
+#define CMD_CMD0(d)			CMD_W(0x01, d)
 /* Window watchdog open WWDOWT window time = 12.8 mS - fixed value for TLE8888QK */
-#define CMD_WWDServiceCmd   CMD_WR(0x15, 0x03)
-#define FWDRespCmd(d)       CMD_WR(0x16, d)
-#define FWDRespSyncCmd(d)   CMD_WR(0x17, d)
+#define CMD_WWDSERVICECMD	CMD_W(0x15, 0x03)
+#define CMD_FWDRESPCMD(d)	CMD_W(0x16, d)
+#define CMD_FWDRESPSYNCCMD(d)	CMD_W(0x17, d)
 
 #define CMD_SR_CODE			0x1a
-#define CMD_SR				CMD_WR(CMD_SR_CODE, 0x03)
-// 0x238 = 568
-#define CMD_OE_SET			CMD_WR(0x1c, 0x02)
-/* not used
-#define CMD_OE_CLR			CMD_WR(0x1c, 0x01)
-#define CMD_LOCK			CMD_WR(0x1e, 0x02)
-*/
-#define CMD_UNLOCK			CMD_WR(0x1e, 0x01)
+#define CMD_SR				CMD_W(CMD_SR_CODE, 0x03)
+#define CMD_OE_SET			CMD_W(0x1c, 0x02)
+#define CMD_OE_CLR			CMD_W(0x1c, 0x01)
+#define CMD_UNLOCK			CMD_W(0x1e, 0x01)
+#define CMD_LOCK			CMD_W(0x1e, 0x02)
 
-#define WWDStat             0x36
-#define FWDStat0            0x37
-#define FWDStat1            0x38
-
-/* Status registers */
-#define CMD_OPSTAT(n)		CMD_R(0x34 + ((n) & 0x01))
-#define CMD_WWDSTAT			CMD_R(WWDStat)
-#define CMD_FWDSTAT0  		CMD_R(FWDStat0)
-#define CMD_FWDSTAT1        CMD_R(FWDStat1)
-#define CMD_TECSTAT			CMD_R(0x39)
-#define CMD_WdDiag			CMD_R(0x2e)
-
-/* Diagnostic */
-#define CMD_DIAG(n)			CMD_R(0x20 + ((n) & 0x01))
+/* Diagnostic registers */
+#define REG_DIAG(n)			(0x20 + ((n) & 0x01))
+#define CMD_DIAG(n)			CMD_R(REG_DIAG(n))
 #define CMD_VRSDIAG(n)		CMD_R(0x22 + ((n) & 0x01))
 #define CMD_COMDIAG			CMD_R(0x24)
 #define CMD_OUTDIAG(n)		CMD_R(0x25 + ((n) & 0x07))
@@ -111,34 +100,58 @@ typedef enum {
 #define CMD_IGNDIAG			CMD_R(0x2d)
 #define CMD_WDDIAG			CMD_R(0x2e)
 
-#define CMD_OUTCONFIG(n, d)	CMD_WR(0x40 + (n), d)
-#define CMD_BRICONFIG(n, d)	CMD_WR(0x46 + ((n) & 0x01), d)
-//#define CMD_VRSCONFIG0(d)	CMD_WR(0x49, d)
-#define CMD_VRSCONFIG1(d)	CMD_WR(0x4a, d)
-#define CMD_INCONFIG(n, d)	CMD_WR(0x53 + ((n) & 0x03), d)
-#define CMD_DDCONFIG(n, d)	CMD_WR(0x57 + ((n) & 0x03), d)
-#define CMD_OECONFIG(n, d)	CMD_WR(0x5b + ((n) & 0x03), d)
-#define CMD_CONT(n, d)		CMD_WR(0x7b + ((n) & 0x03), d)
+/* Status registers */
+#define REG_OPSTAT(n)		(0x34 + ((n) & 0x01))
+#define CMD_OPSTAT(n)		CMD_R(REG_OPSTAT(n))
+#define REG_WWDSTAT			0x36
+#define CMD_WWDSTAT			CMD_R(REG_WWDSTAT)
+#define REG_FWDSTAT(n)		(0x37 + ((n) & 0x01))
+#define CMD_FWDSTAT(n)		CMD_R(REG_FWDSTAT(n))
+#define REG_TECSTAT			0x39
+#define CMD_TECSTAT			CMD_R(REG_TECSTAT)
 
-const uint8_t watchDogResponses[16][4] = {
-/* Reverse order:
- * RESP3,RESP2,RESP1,REST0 */
-{0xFF, 0x0F, 0xF0, 0x00},
-{0xB0, 0x40, 0xBF, 0x4F},
-{0xE9, 0x19, 0xE6, 0x16},
-{0xA6, 0x56, 0xA9, 0x59},
-{0x75, 0x85, 0x7A, 0x8A},
-{0x3A, 0xCA, 0x35, 0xC5},
-{0x63, 0x93, 0x6C, 0x9C},
-{0x2C, 0xDC, 0x23, 0xD3},
-{0xD2, 0x22, 0xDD, 0x2D},
-{0x9D, 0x6D, 0x92, 0x62},
-{0xC4, 0x34, 0xCB, 0x3B},
-{0x8B, 0x7B, 0x84, 0x74},
-{0x58, 0xA8, 0x57, 0xA7},
-{0x17, 0xE7, 0x18, 0xE8},
-{0x4E, 0xBE, 0x41, 0xB1},
-{0x01, 0xF1, 0x0E, 0xFE}
+/* Configuration registers */
+#define CMD_OUTCONFIG(n, d)	CMD_W(0x40 + (n), d)
+#define CMD_BRICONFIG(n, d)	CMD_W(0x46 + ((n) & 0x01), d)
+#define CMD_IGNCONFIG(d)	CMD_W(0x48, d)
+#define CMD_VRSCONFIG(n, d)	CMD_W(0x49 + ((n) & 0x03), d)
+#define CMD_OPCONFIG0(d)	CMD_W(0x4e, d)
+#define CMD_INCONFIG(n, d)	CMD_W(0x53 + ((n) & 0x03), d)
+#define CMD_DDCONFIG(n, d)	CMD_W(0x57 + ((n) & 0x03), d)
+#define CMD_OECONFIG(n, d)	CMD_W(0x5b + ((n) & 0x03), d)
+
+/* Control registers */
+#define CMD_CONT(n, d)		CMD_W(0x7b + ((n) & 0x03), d)
+
+/* Looks like reset value is 113.6ms? 1.6ms * 0x47 */
+#define FWD_PERIOD_MS		(20)
+
+/* Default closed window time is 0b0011.1111 * 1.6 = 100.8mS
+ * Default open window time is 0b0011 * 3.2 = 12.8 mS */
+#define WWD_PERIOD_MS		(100.8 + (12.8 / 2))
+
+/* DOTO: add irq support */
+#define DIAG_PERIOD_MS		(7)
+
+const uint8_t tle8888_fwd_responses[16][4] = {
+	/* Reverse order:
+	 * RESP3,RESP2,RESP1,RESP0 */
+	{0xFF, 0x0F, 0xF0, 0x00},
+	{0xB0, 0x40, 0xBF, 0x4F},
+	{0xE9, 0x19, 0xE6, 0x16},
+	{0xA6, 0x56, 0xA9, 0x59},
+	{0x75, 0x85, 0x7A, 0x8A},
+	{0x3A, 0xCA, 0x35, 0xC5},
+	{0x63, 0x93, 0x6C, 0x9C},
+	{0x2C, 0xDC, 0x23, 0xD3},
+	{0xD2, 0x22, 0xDD, 0x2D},
+	{0x9D, 0x6D, 0x92, 0x62},
+	{0xC4, 0x34, 0xCB, 0x3B},
+	{0x8B, 0x7B, 0x84, 0x74},
+	{0x58, 0xA8, 0x57, 0xA7},
+	{0x17, 0xE7, 0x18, 0xE8},
+	{0x4E, 0xBE, 0x41, 0xB1},
+	{0x01, 0xF1, 0x0E, 0xFE}
 };
 
 /*==========================================================================*/
@@ -149,54 +162,22 @@ const uint8_t watchDogResponses[16][4] = {
 /* Driver local variables and types.										*/
 /*==========================================================================*/
 
-/* OS */
-SEMAPHORE_DECL(tle8888_wake, 10 /* or BOARD_TLE8888_COUNT ? */);
-static THD_WORKING_AREA(tle8888_thread_1_wa, 256);
-
 // todo: much of state is currently global while technically it should be per-chip. but we
 // are lazy and in reality it's usually one chip per board
 
-
-/**
- * For the timing check the microcontroller has to send periodically the window watchdog service command
- * WWDServiceCmd. The window watchdog is triggered correctly if the command is received inside the open
- * window of the window watchdog sequence.
- */
-static efitick_t lastWindowWatchdogTimeNt = 0;
-
-static efitick_t lastFunctionWatchdogTimeNt = 0;
-
-static uint16_t WindowWatchdogErrorCounterValue;
-static uint16_t FunctionalWatchdogPassCounterValue;
-static uint16_t TotalErrorCounterValue;
-
-static uint16_t maybeFirstResponse = 0;
-static uint16_t functionWDrx = 0;
-static uint16_t wdDiagResponse = 0;
-
-//static_assert(TLE8888_POLL_INTERVAL_MS < Window_watchdog_open_window_time_ms)
-
-static bool needInitialSpi = true;
-static bool isWatchdogHappy = false;
-static bool wasWatchdogHappy = false;
-
-static int selfResetCounter = 0;
 static int lowVoltageResetCounter = 0;
-static int requestedResetCounter = 0;
-int tle8888reinitializationCounter = 0;
 
 float vBattForTle8888 = 0;
 
-// set debug_mode 31
-static int tle8888SpiCounter = 0;
-static uint16_t spiRxb = 0, spiTxb = 0;
-
-
 /* Driver private data */
 struct tle8888_priv {
 	const struct tle8888_config	*cfg;
-	/* cached output state - state last send to chip */
-	uint32_t					o_state_cached;
+
+	/* thread stuff */
+	thread_t 					*thread;
+	THD_WORKING_AREA(thread_wa, 256);
+	semaphore_t					wake;
+
 	/* state to be sent to chip */
 	uint32_t					o_state;
 	/* direct driven output mask */
@@ -207,18 +188,49 @@ struct tle8888_priv {
 	/* this is overhead to store 4 bits in uint32_t
 	 * but I don't want any magic shift math */
 	uint32_t					o_pp_mask;
+	/* cached CONT registers state - value last send to chip */
+	uint32_t					cont_data_cached;
 
 	tle8888_drv_state			drv_state;
 
+	/* direct drive mapping registers */
+	uint8_t						InConfig[TLE8888_DIRECT_MISC];
+
+	/* last accessed register, for validation on next SPI access */
+	uint8_t						last_reg;
+
 	/* diagnostic registers */
 	uint8_t						OutDiag[5];
+	uint8_t						PPOVDiag;
 	uint8_t						BriDiag[2];
 	uint8_t						IgnDiag;
 	/* status registers */
 	uint8_t						OpStat[2];
 
-	/* last diagnostick was read */
-	systime_t					ts_diag;
+	/* last diagnostic was read */
+	systime_t					diag_ts;
+
+	/* WD stuff */
+	uint8_t						wwd_err_cnt;
+	uint8_t						fwd_err_cnt;
+	uint8_t						tot_err_cnt;
+	uint8_t						wd_diag;
+	bool						wd_happy;
+	systime_t					wwd_ts;
+	systime_t					fwd_ts;
+
+	/* chip needs reintialization due to some critical issue */
+	bool						need_init;
+
+	/* statistic */
+	int							por_cnt;
+	int							wdr_cnt;
+	int							comfe_cnt;
+	int							init_cnt;
+	int							init_req_cnt;
+	int							spi_cnt;
+	uint16_t					tx;
+	uint16_t					rx;
 };
 
 static struct tle8888_priv chips[BOARD_TLE8888_COUNT];
@@ -233,26 +245,23 @@ static const char* tle8888_pin_names[TLE8888_OUTPUTS] = {
 	"TLE8888.IGN1",		"TLE8888.IGN2",		"TLE8888.IGN3",		"TLE8888.IGN4"
 };
 
-#define getWindowWatchdog() ((WindowWatchdogErrorCounterValue >> 8) & 0x3f)
-#define getFunctionalWatchdog() ((FunctionalWatchdogPassCounterValue >> 8) & 0x3f)
-#define getTotalErrorCounter() ((TotalErrorCounterValue >> 8) & 0x3f)
-
 #if EFI_TUNER_STUDIO
 // set debug_mode 31
 void tle8888PostState(TsDebugChannels *debugChannels) {
+	struct tle8888_priv *chip = &chips[0];
 
-	debugChannels->debugIntField1 = getWindowWatchdog();
-	debugChannels->debugIntField2 = getFunctionalWatchdog();
-	debugChannels->debugIntField3 = getTotalErrorCounter();
-	//debugChannels->debugIntField1 = tle8888SpiCounter;
-	//debugChannels->debugIntField2 = spiTxb;
-	//debugChannels->debugIntField3 = spiRxb;
-	debugChannels->debugIntField5 = tle8888reinitializationCounter;
+	debugChannels->debugIntField1 = chip->wwd_err_cnt;
+	debugChannels->debugIntField2 = chip->fwd_err_cnt;
+	debugChannels->debugIntField3 = chip->tot_err_cnt;
+	//debugChannels->debugIntField1 = chip->spi_cnt;
+	//debugChannels->debugIntField2 = chip->tx;
+	//debugChannels->debugIntField3 = chip->rx;
+	debugChannels->debugIntField5 = chip->init_cnt;
 
-	debugChannels->debugFloatField3 = chips[0].OpStat[1];
-	debugChannels->debugFloatField4 = selfResetCounter * 1000000 + requestedResetCounter * 10000 + lowVoltageResetCounter;
-	debugChannels->debugFloatField5 = functionWDrx;
-	debugChannels->debugFloatField6 = wdDiagResponse;
+	debugChannels->debugFloatField3 = chip->OpStat[1];
+	debugChannels->debugFloatField4 = chip->por_cnt * 1000000 + chip->init_req_cnt * 10000 + lowVoltageResetCounter;
+	debugChannels->debugFloatField5 = 0;
+	debugChannels->debugFloatField6 = 0;
 }
 #endif /* EFI_TUNER_STUDIO */
 
@@ -266,11 +275,48 @@ static SPIDriver *get_bus(struct tle8888_priv *chip)
 	return chip->cfg->spi_bus;
 }
 
-/**
- * @return always return 0 for now
- */
-static int tle8888_spi_rw(struct tle8888_priv *chip, uint16_t tx, uint16_t *rx)
+static int tle8888_spi_validate(struct tle8888_priv *chip, uint16_t rx)
 {
+	uint8_t reg = getRegisterFromResponse(rx);
+
+	if ((chip->last_reg != REG_INVALID) && (chip->last_reg != reg)) {
+		/* unexpected SPI answers */
+		if (reg == REG_OPSTAT(0)) {
+			/* after power on reset: the address and the content of the
+			 * status register OpStat0 is transmitted with the next SPI
+			 * transmission */
+			chip->por_cnt++;
+		} else if (reg == REG_FWDSTAT(1)) {
+			/* after watchdog reset: the address and the content of the
+			 * diagnosis register FWDStat1 is transmitted with the first
+			 * SPI transmission after the low to high transition of RST */
+			chip->wdr_cnt++;
+		} else if (reg == REG_DIAG(0)) {
+			/* after an invalid communication frame: the address and the
+			 * content of the diagnosis register Diag0 is transmitted
+			 * with the next SPI transmission and the bit COMFE in
+			 * diagnosis register ComDiag is set to "1" */
+			chip->comfe_cnt++;
+		}
+		/* during power on reset: SPI commands are ignored, SDO is always
+		 * tristate */
+		/* during watchdog reset: SPI commands are ignored, SDO has the
+		 * value of the status flag */
+		chip->need_init = true;
+
+		return -1;
+	}
+
+	return 0;
+}
+
+/**
+ * @returns -1 in case of communication error
+ */
+static int tle8888_spi_rw(struct tle8888_priv *chip, uint16_t tx, uint16_t *rx_ptr)
+{
+	int ret;
+	uint16_t rx;
 	SPIDriver *spi = get_bus(chip);
 
 	/**
@@ -288,21 +334,79 @@ static int tle8888_spi_rw(struct tle8888_priv *chip, uint16_t tx, uint16_t *rx)
 	/* Slave Select assertion. */
 	spiSelect(spi);
 	/* Atomic transfer operations. */
-	spiRxb = spiPolledExchange(spi, tx);
+	rx = spiPolledExchange(spi, tx);
 	//spiExchange(spi, 2, &tx, &rxb); 8 bit version just in case?
 	/* Slave Select de-assertion. */
 	spiUnselect(spi);
 	/* Ownership release. */
 	spiReleaseBus(spi);
 
-	spiTxb = tx;
-	tle8888SpiCounter++;
+	/* statisctic and debug */
+	chip->tx = tx;
+	chip->rx = rx;
+	chip->spi_cnt++;
 
-	if (rx)
-		*rx = spiRxb;
+	if (rx_ptr)
+		*rx_ptr = rx;
+
+	/* validate reply and save last accessed register */
+	ret = tle8888_spi_validate(chip, rx);
+	chip->last_reg = getRegisterFromResponse(tx);
 
 	/* no errors for now */
-	return 0;
+	return ret;
+}
+
+/**
+ * @return -1 in case of communication error
+ */
+static int tle8888_spi_rw_array(struct tle8888_priv *chip, const uint16_t *tx, uint16_t *rx, int n)
+{
+	int ret;
+	uint16_t rxdata;
+	SPIDriver *spi = get_bus(chip);
+
+	/**
+	 * 15.1 SPI Protocol
+	 *
+	 * after a read or write command: the address and content of the selected register
+	 * is transmitted with the next SPI transmission (for not existing addresses or
+	 * wrong access mode the data is always 0)
+	 */
+
+	/* Acquire ownership of the bus. */
+	spiAcquireBus(spi);
+	/* Setup transfer parameters. */
+	spiStart(spi, &chip->cfg->spi_config);
+
+	for (int i = 0; i < n; i++) {
+		/* Slave Select assertion. */
+		spiSelect(spi);
+		/* data transfer */
+		rxdata = spiPolledExchange(spi, tx[i]);
+		//spiExchange(spi, 2, &tx, &rxb); 8 bit version just in case?
+		if (rx)
+			rx[i] = rxdata;
+		/* Slave Select de-assertion. */
+		spiUnselect(spi);
+
+		/* statistic and debug */
+		chip->tx = tx[i];
+		chip->rx = rxdata;
+		chip->spi_cnt++;
+
+		/* validate reply and save last accessed register */
+		ret = tle8888_spi_validate(chip, rxdata);
+		chip->last_reg = getRegisterFromResponse(tx[i]);
+
+		if (ret < 0)
+			break;
+	}
+	/* Ownership release. */
+	spiReleaseBus(spi);
+
+	/* no errors for now */
+	return ret;
 }
 
 /**
@@ -312,142 +416,141 @@ static int tle8888_spi_rw(struct tle8888_priv *chip, uint16_t tx, uint16_t *rx)
 
 static int tle8888_update_output(struct tle8888_priv *chip)
 {
-	int ret = 0;
+	int i;
+	int ret;
 
 	/* TODO: lock? */
 
-	/* set value only for non-direct driven pins */
-	uint32_t out_data = chip->o_state & (~chip->o_direct_mask);
-	for (int i = 0; i < 4; i++) {
-		uint8_t od;
+	uint8_t briconfig0 = 0;
 
-		od = (out_data >> (8 * i)) & 0xff;
-		ret |= tle8888_spi_rw(chip, CMD_CONT(i, od), NULL);
+	/* calculate briconfig0 */
+	for (i = 20; i < 24; i++) {
+		if (chip->o_pp_mask & BIT(i)) {
+			if (chip->o_state & BIT(i)) {
+				/* low-side switch mode */
+			} else {
+				/* else enable high-side switch mode */
+				briconfig0 |= BIT((i - 20) * 2);
+			}
+		}
 	}
+	/* TODO: set freewheeling bits in briconfig0? */
+	/* TODO: apply hi-Z mask when support will be added */
+
+	/* set value only for non-direct driven pins */
+	uint32_t cont_data = chip->o_state & ~chip->o_direct_mask;
+
+	/* output for push-pull pins is allways enabled
+	 * (at least until we start supporting hi-Z state) */
+	cont_data |= chip->o_pp_mask;
+
+	uint16_t tx[] = {
+		/* bridge config */
+		CMD_BRICONFIG(0, briconfig0),
+		/* output enables */
+		CMD_CONT(0, cont_data >>  0),
+		CMD_CONT(1, cont_data >>  8),
+		CMD_CONT(2, cont_data >> 16),
+		CMD_CONT(3, cont_data >> 24)
+	};
+	ret = tle8888_spi_rw_array(chip, tx, NULL, ARRAY_SIZE(tx));
+
+	/* TODO: unlock? */
 
 	if (ret == 0) {
 		/* atomic */
-		chip->o_state_cached = out_data;
+		chip->cont_data_cached = cont_data;
 	}
 
-	/* TODO: unlock? */
-
 	return ret;
 }
 
 /**
- * @brief read TLE8888 OpStat1 and diagnostic registers data.
+ * @brief read TLE8888 diagnostic registers data.
  * @details Chained read of several registers
  */
 static int tle8888_update_status_and_diag(struct tle8888_priv *chip)
 {
 	int ret = 0;
-	uint16_t rx = 0;
+	const uint16_t tx[] = {
+		CMD_OUTDIAG(0),
+		CMD_OUTDIAG(1),
+		CMD_OUTDIAG(2),
+		CMD_OUTDIAG(3),
+		CMD_OUTDIAG(4),
+		CMD_PPOVDIAG,
+		CMD_BRIDIAG(0),
+		CMD_BRIDIAG(1),
+		CMD_IGNDIAG,
+		CMD_OPSTAT(0),
+		CMD_OPSTAT(1),
+		CMD_OPSTAT(1)
+	};
+	uint16_t rx[ARRAY_SIZE(tx)];
 
 	/* TODO: lock? */
-
-	// todo: extract helper method?
-	/* the address and content of the selected register is transmitted with the
-	 * next SPI transmission (for not existing addresses or wrong access mode
-	 * the data is always '0' */
-	/* this is quite expensive to call tle8888_spi_rw on each register read
-	 * TODO: implement tle8888_spi_rw_array ? */
-
-//	/* request OutDiad0, ignore received */
-//	if ((ret = tle8888_spi_rw(chip, CMD_OUTDIAG(0), NULL)))
-//		return ret;
-//
-//	/* request OutDiad1, receive OutDiag0 */
-//	if ((ret = tle8888_spi_rw(chip, CMD_OUTDIAG(1), &rx)))
-//		return ret;
-//	chip->OutDiag[0] = getDataFromResponse(rx);
-//
-//	/* request OutDiad2, receive OutDiag1 */
-//	if ((ret = tle8888_spi_rw(chip, CMD_OUTDIAG(2), &rx)))
-//		return ret;
-//	chip->OutDiag[1] = getDataFromResponse(rx);
-//
-//	/* request OutDiad3, receive OutDiag2 */
-//	if ((ret = tle8888_spi_rw(chip, CMD_OUTDIAG(3), &rx)))
-//		return ret;
-//	chip->OutDiag[2] = getDataFromResponse(rx);
-//
-//	/* request OutDiad4, receive OutDiag3 */
-//	if ((ret = tle8888_spi_rw(chip, CMD_OUTDIAG(4), &rx)))
-//		return ret;
-//	chip->OutDiag[3] = getDataFromResponse(rx);
-//
-//	/* request BriDiag0, receive OutDiag4 */
-//	if ((ret = tle8888_spi_rw(chip, CMD_BRIDIAG(0), &rx)))
-//		return ret;
-//	chip->OutDiag[4] = getDataFromResponse(rx);
-//
-//	/* request BriDiag1, receive BriDiag0 */
-//	if ((ret = tle8888_spi_rw(chip, CMD_BRIDIAG(1), &rx)))
-//		return ret;
-//	chip->BriDiag[0] = getDataFromResponse(rx);
-//
-//	/* request IgnDiag, receive BriDiag1 */
-//	if ((ret = tle8888_spi_rw(chip, CMD_IGNDIAG, &rx)))
-//		return ret;
-//	chip->BriDiag[1] = getDataFromResponse(rx);
-
-	/* request OpStat0, receive IgnDiag */
-	if ((ret = tle8888_spi_rw(chip, CMD_OPSTAT(0), &rx)))
-		return ret;
-//	chip->IgnDiag = getDataFromResponse(rx);
-
-	/* request OpStat1, receive OpStat0 */
-	if ((ret = tle8888_spi_rw(chip, CMD_OPSTAT(1), &rx)))
-		return ret;
-	chip->OpStat[0] = getDataFromResponse(rx);
-
-	/* request OpStat1, receive OpStat1 */
-	if ((ret = tle8888_spi_rw(chip, CMD_OPSTAT(1), &rx)))
-		return ret;
-	chip->OpStat[1] = getDataFromResponse(rx);
-
+	ret = tle8888_spi_rw_array(chip, tx, rx, ARRAY_SIZE(tx));
 	/* TODO: unlock? */
 
+	if (ret == 0) {
+		/* the address and content of the selected register is transmitted with the
+		 * next SPI transmission */
+		chip->OutDiag[0] = getDataFromResponse(rx[0 + 1]);
+		chip->OutDiag[1] = getDataFromResponse(rx[1 + 1]);
+		chip->OutDiag[2] = getDataFromResponse(rx[2 + 1]);
+		chip->OutDiag[3] = getDataFromResponse(rx[3 + 1]);
+		chip->OutDiag[4] = getDataFromResponse(rx[4 + 1]);
+		chip->PPOVDiag   = getDataFromResponse(rx[5 + 1]);
+		chip->BriDiag[0] = getDataFromResponse(rx[6 + 1]);
+		chip->BriDiag[1] = getDataFromResponse(rx[7 + 1]);
+		chip->IgnDiag    = getDataFromResponse(rx[8 + 1]);
+		chip->OpStat[0]  = getDataFromResponse(rx[9 + 1]);
+		chip->OpStat[1]  = getDataFromResponse(rx[10 + 1]);
+	}
+
 	return ret;
 }
 
+/**
+ * @brief Drives natve MCU pins connected to TLE8888 inputs
+ * @details This is faster than updating Cont registers over SPI
+ */
+
 static int tle8888_update_direct_output(struct tle8888_priv *chip, int pin, int value)
 {
+	int index = -1;
 	const struct tle8888_config	*cfg = chip->cfg;
 
-	/* find direct drive gpio */
-	for (int i = 0; i < TLE8888_DIRECT_MISC; i++) {
-		/* again: outputs in cfg counted starting from 1 - hate this */
-		if (cfg->direct_io[i].output == pin + 1) {
-			if (value)
-				palSetPort(cfg->direct_io[i].port,
-						   PAL_PORT_BIT(cfg->direct_io[i].pad));
-			else
-				palClearPort(cfg->direct_io[i].port,
-						   PAL_PORT_BIT(cfg->direct_io[i].pad));
-			return 0;
+	if (pin < 4) {
+		/* OUT1..4 */
+		index = pin;
+	} else if (pin > 24) {
+		/* IGN1..4 */
+		index = (pin - 24) + 4;
+	} else {
+		/* find remapable direct drive gpio */
+		for (int i = 0; i < TLE8888_DIRECT_MISC; i++) {
+			/* again: outputs in cfg counted starting from 1 - hate this */
+			if (cfg->direct_maps[i].output == pin + 1) {
+				index = 8 + i;
+				break;
+			}
 		}
 	}
 
 	/* direct gpio not found */
-	return -1;
+	if (index < 0)
+		return -1;
+
+	if (value)
+		palSetPort(cfg->direct_gpio[index].port,
+				   PAL_PORT_BIT(cfg->direct_gpio[index].pad));
+	else
+		palClearPort(cfg->direct_gpio[index].port,
+				   PAL_PORT_BIT(cfg->direct_gpio[index].pad));
+	return 0;
 }
 
-// ChibiOS does not offer this function so that's a copy-paste of 'chSemSignal' without locking
-void chSemSignalS(semaphore_t *sp) {
-
-  chDbgCheck(sp != NULL);
-
-  chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) ||
-              ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)),
-              "inconsistent semaphore");
-  if (++sp->cnt <= (cnt_t)0) {
-    chSchWakeupS(queue_fifo_remove(&sp->queue), MSG_OK);
-  }
-}
-
-
 /**
  * @brief TLE8888 chip driver wakeup.
  * @details Wake up driver. Will cause output register update
@@ -455,11 +558,9 @@ void chSemSignalS(semaphore_t *sp) {
 
 static int tle8888_wake_driver(struct tle8888_priv *chip)
 {
-	(void)chip;
-
     /* Entering a reentrant critical zone.*/
     syssts_t sts = chSysGetStatusAndLockX();
-	chSemSignalI(&tle8888_wake);
+	chSemSignalI(&chip->wake);
 	if (!port_is_isr_context()) {
 		/**
 		 * chSemSignalI above requires rescheduling
@@ -473,157 +574,238 @@ static int tle8888_wake_driver(struct tle8888_priv *chip)
 	return 0;
 }
 
+static brain_pin_diag_e tle8888_2b_to_diag_no_temp(unsigned int bits)
+{
+	if (bits == 0x01)
+		return PIN_SHORT_TO_BAT;
+	if (bits == 0x02)
+		return PIN_OPEN;
+	if (bits == 0x03)
+		return PIN_SHORT_TO_GND;
+	return PIN_OK;
+}
+
+static brain_pin_diag_e tle8888_2b_to_diag_with_temp(unsigned int bits)
+{
+	brain_pin_diag_e diag = tle8888_2b_to_diag_no_temp(bits);
+
+	if (diag == PIN_SHORT_TO_BAT)
+		diag |= PIN_DRIVER_OVERTEMP;
+
+	return diag;
+}
+
+static int tle8888_chip_reset(struct tle8888_priv *chip) {
+	int ret;
+
+	ret = tle8888_spi_rw(chip, CMD_SR, NULL);
+	/**
+	 * Table 8. Reset Times. All reset times not more than 20uS
+	 */
+	chThdSleepMilliseconds(3);
+
+	chip->last_reg = REG_INVALID;
+
+	return ret;
+}
+
+static int tle8888_chip_init(struct tle8888_priv *chip)
+{
+	int ret;
+
+	/* statistic */
+	chip->init_cnt++;
+
+	uint16_t tx[] = {
+		/* unlock */
+		CMD_UNLOCK,
+		/* set INCONFIG - aux input mapping */
+		CMD_INCONFIG(0, chip->InConfig[0]),
+		CMD_INCONFIG(1, chip->InConfig[1]),
+		CMD_INCONFIG(2, chip->InConfig[2]),
+		CMD_INCONFIG(3, chip->InConfig[3]),
+		/* Diagnnostic settings */
+		/* Enable open load detection and disable switch off
+		 * in case of overcurrent for OUTPUT1..4 */
+		CMD_OUTCONFIG(0, BIT(7) | BIT(5) | BIT(3) | BIT(1)),
+		/* Enable open load detection and disable switch off
+		 * in case of overcurrent for OUTPUT5..7 */
+		CMD_OUTCONFIG(1, BIT(5) | BIT(3) | BIT(1)),
+		/* Enable open load detection and set short to bat
+		 * thresholt to 125 mV (default) for OUTPUT8..13 */
+		CMD_OUTCONFIG(2, (0x0 << 6) | BIT(5) | BIT(4) | BIT(3) | BIT(2) | BIT(1) | BIT(0)),
+		/* Enable open load detection and disable switch off
+		 * in case of overcurrent for OUTPUT14
+		 * Set short to bat threshold to 125mV (default) for
+		 * OUTPUT12..13 and OUTPUT10..11 */
+		CMD_OUTCONFIG(3, BIT(5) | (0x0 << 2) | (0x0 << 0)),
+		/* No delayed off function for OUTPUT17
+		 * Enable open load detection and disable switch off
+		 * in case of overcurrent for OUTPUT15..17 */
+		CMD_OUTCONFIG(4, BIT(5) | BIT(3) | BIT(1)),
+		/* Enable open load detection and disable switch off
+		 * in case of overcurrent for OUTPUT18..20 */
+		CMD_OUTCONFIG(5, BIT(5) | BIT(3) | BIT(1)),
+		/* set OE and DD registers */
+		CMD_OECONFIG(0, chip->o_oe_mask >>  0),
+		CMD_DDCONFIG(0, chip->o_direct_mask >> 0),
+		CMD_OECONFIG(1, chip->o_oe_mask >>  8),
+		CMD_DDCONFIG(1, chip->o_direct_mask >> 8),
+		CMD_OECONFIG(2, chip->o_oe_mask >>  16),
+		CMD_DDCONFIG(2, chip->o_direct_mask >> 16),
+		CMD_OECONFIG(3, chip->o_oe_mask >>  24),
+		CMD_DDCONFIG(3, chip->o_direct_mask >> 24),
+		/* set VR mode: VRS/Hall */
+		CMD_VRSCONFIG(1, (0 << 4) |
+						 (chip->cfg->mode << 2) |
+						 (0 << 0)),
+		/* enable outputs */
+		CMD_OE_SET
+	};
+	/* TODO: lock? */
+	ret = tle8888_spi_rw_array(chip, tx, NULL, ARRAY_SIZE(tx));
+	/* TODO: unlock? */
+
+	if (ret == 0) {
+		const struct tle8888_config	*cfg = chip->cfg;
+
+		/* enable pins */
+		if (cfg->ign_en.port)
+			palSetPort(cfg->ign_en.port, PAL_PORT_BIT(cfg->ign_en.pad));
+		if (cfg->inj_en.port)
+			palSetPort(cfg->inj_en.port, PAL_PORT_BIT(cfg->inj_en.pad));
+	}
+
+	if (CONFIG(verboseTLE8888)) {
+		tle8888_dump_regs();
+	}
+
+	return ret;
+}
+
+static int tle8888_wwd_feed(struct tle8888_priv *chip) {
+	tle8888_spi_rw(chip, CMD_WWDSERVICECMD, NULL);
+
+	return 0;
+}
+
+static int tle8888_fwd_feed(struct tle8888_priv *chip) {
+	uint16_t reg;
+
+	tle8888_spi_rw(chip, CMD_FWDSTAT(1), NULL);
+	/* here we get response of the 'FWDStat1' above */
+	tle8888_spi_rw(chip, CMD_WDDIAG, &reg);
+
+	uint8_t data = getDataFromResponse(reg);
+	uint8_t fwdquest = data & 0xF;
+	uint8_t fwdrespc = (data >> 4) & 3;
+	/* Table lines are filled in reverse order (like in DS) */
+	uint8_t response = tle8888_fwd_responses[fwdquest][3 - fwdrespc];
+	if (fwdrespc != 0) {
+		tle8888_spi_rw(chip, CMD_FWDRESPCMD(response), NULL);
+	} else {
+		/* to restart heartbeat timer, sync command should be used for response 0 */
+		tle8888_spi_rw(chip, CMD_FWDRESPSYNCCMD(response), NULL);
+	}
+
+	return 0;
+}
+
+static int tle8888_wd_get_status(struct tle8888_priv *chip) {
+	uint16_t reg;
+
+	tle8888_spi_rw(chip, CMD_WDDIAG, NULL);
+	tle8888_spi_rw(chip, CMD_WDDIAG, &reg);
+
+	chip->wd_diag = getDataFromResponse(reg);
+
+	if (chip->wd_diag & 0x70) {
+		/* Reset caused by TEC
+		 * Reset caused by FWD
+		 * Reset caused by WWD */
+		 return -1;
+	}
+	if (chip->wd_diag & 0x0f) {
+		/* Some error in WD handling */
+		return 1;
+	}
+
+	return 0;
+}
+
+static int tle8888_wd_feed(struct tle8888_priv *chip) {
+	bool update_status;
+
+	if (chip->wwd_ts <= chVTGetSystemTimeX()) {
+		update_status = true;
+		if (tle8888_wwd_feed(chip) == 0) {
+			chip->wwd_ts = chTimeAddX(chVTGetSystemTimeX(), TIME_MS2I(WWD_PERIOD_MS));
+		}
+	}
+
+	if (chip->fwd_ts <= chVTGetSystemTimeX()) {
+		update_status = true;
+		if (tle8888_fwd_feed(chip) == 0) {
+			chip->fwd_ts = chTimeAddX(chVTGetSystemTimeX(), TIME_MS2I(FWD_PERIOD_MS));
+		}
+	}
+
+	if (update_status) {
+		uint16_t wwd_reg, fwd_reg, tec_reg;
+
+		tle8888_spi_rw(chip, CMD_WWDSTAT, NULL);
+		tle8888_spi_rw(chip, CMD_FWDSTAT(0), &wwd_reg);
+		tle8888_spi_rw(chip, CMD_TECSTAT, &fwd_reg);
+		tle8888_spi_rw(chip, CMD_TECSTAT, &tec_reg);
+
+		chip->wwd_err_cnt = getDataFromResponse(wwd_reg) & 0x7f;
+		chip->fwd_err_cnt = getDataFromResponse(fwd_reg) & 0x7f;
+		chip->tot_err_cnt = getDataFromResponse(tec_reg) & 0x7f;
+
+		chip->wd_happy = ((chip->wwd_err_cnt == 0) &&
+						  (chip->fwd_err_cnt == 0));
+
+		return tle8888_wd_get_status(chip);
+	} else {
+		return 0;
+	}
+}
+
+static int tle8888_calc_sleep_interval(struct tle8888_priv *chip) {
+	systime_t now = chVTGetSystemTimeX();
+
+	sysinterval_t wwd_delay = chTimeDiffX(now, chip->wwd_ts);
+	sysinterval_t fwd_delay = chTimeDiffX(now, chip->fwd_ts);
+	sysinterval_t diag_delay = chTimeDiffX(now, chip->diag_ts);
+
+	if ((diag_delay <= wwd_delay) && (diag_delay <= fwd_delay))
+		return diag_delay;
+	if (fwd_delay <= wwd_delay)
+		return fwd_delay;
+	return wwd_delay;
+}
+
 /*==========================================================================*/
 /* Driver thread.															*/
 /*==========================================================================*/
 
-static void handleFWDStat1(struct tle8888_priv *chip, int registerNum, int data) {
-	if (registerNum != FWDStat1)
-		return;
-	uint8_t FWDQUEST = data & 0xF;
-	uint8_t FWDRESPC = (data >> 4) & 3;
-	/* Table lines are filled in reverse order (like in DS) */
-	uint8_t response = watchDogResponses[FWDQUEST][3 - FWDRESPC];
-	if (FWDRESPC) {
-		tle8888_spi_rw(chip, FWDRespCmd(response), NULL);
-	} else {
-		/* to restart heartbeat timer, sync command should be used for response 0 */
-		tle8888_spi_rw(chip, FWDRespSyncCmd(response), NULL);
-	}
-	tle8888_spi_rw(chip, CMD_WdDiag, NULL);
-	tle8888_spi_rw(chip, CMD_WdDiag, &wdDiagResponse);
-}
-
-static int startupConfiguration(struct tle8888_priv *chip) {
-	const struct tle8888_config	*cfg = chip->cfg;
-	uint16_t response = 0;
-	/* Set LOCK bit to 0 */
-	// second 0x13D=317 => 0x35=53
-	tle8888_spi_rw(chip, CMD_UNLOCK, &response);
-
-	chip->o_direct_mask = 0;
-	chip->o_oe_mask		= 0;
-	/* HACK HERE if you want to enable PP for OUT21..OUT24
-	 * without approprirate call to setPinMode */
-	chip->o_pp_mask		= 0; /* = BIT(20) | BIT(21) | BIT(22) | BIT(23); */
-	/* enable direct drive of OUTPUT4..1
-	 * ...still need INJEN signal */
-	chip->o_direct_mask	|= 0x0000000f;
-	chip->o_oe_mask		|= 0x0000000f;
-	/* enable direct drive of IGN4..1
-	 * ...still need IGNEN signal */
-	chip->o_direct_mask |= 0x0f000000;
-	chip->o_oe_mask		|= 0x0f000000;
-
-	/* map and enable outputs for direct driven channels */
-	for (int i = 0; i < TLE8888_DIRECT_MISC; i++) {
-		int out = cfg->direct_io[i].output;
-
-		/* not used? */
-		if (out == 0)
-			continue;
-
-		/* OUT1..4 driven direct only through dedicated pins */
-		if (out < 5)
-			return -1;
-
-		/* in config counted from 1 */
-		uint32_t mask = (1 << (out - 1));
-
-		/* check if output already occupied */
-		if (chip->o_direct_mask & mask) {
-			/* incorrect config? */
-			return -1;
-		}
-
-		/* enable direct drive and output enable */
-		chip->o_direct_mask	|= mask;
-		chip->o_oe_mask		|= mask;
-
-		/* set INCONFIG - aux input mapping */
-		tle8888_spi_rw(chip, CMD_INCONFIG(i, out - 5), NULL);
-	}
-
-	/* enable all ouputs
-	 * TODO: add API to enable/disable? */
-	chip->o_oe_mask		|= 0x0ffffff0;
-
-	/* set OE and DD registers */
-	for (int i = 0; i < 4; i++) {
-		uint8_t oe, dd;
-
-		oe = (chip->o_oe_mask >> (8 * i)) & 0xff;
-		dd = (chip->o_direct_mask >> (8 * i)) & 0xff;
-		tle8888_spi_rw(chip, CMD_OECONFIG(i, oe), NULL);
-		tle8888_spi_rw(chip, CMD_DDCONFIG(i, dd), NULL);
-	}
-
-	/* Debug: disable diagnostic */
-	for (int i = 0; i <= 5; i++) {
-		tle8888_spi_rw(chip, CMD_OUTCONFIG(i, 0), NULL);
-	}
-
-	/* enable outputs */
-	tle8888_spi_rw(chip, CMD_OE_SET, NULL);
-
-	if (cfg->mode > 0) {
-		tle8888_spi_rw(chip, CMD_VRSCONFIG1(cfg->mode << 2), NULL);
-	}
-	return 0;
-}
-
-void watchdogLogic(struct tle8888_priv *chip) {
-	efitick_t nowNt = getTimeNowNt();
-	if (nowNt - lastWindowWatchdogTimeNt > MS2NT(Window_watchdog_close_window_time_ms)) {
-		tle8888_spi_rw(chip, CMD_WWDServiceCmd, &maybeFirstResponse);
-		lastWindowWatchdogTimeNt = nowNt;
-	}
-
-	if (nowNt - lastFunctionWatchdogTimeNt > MS2NT(Functional_Watchdog_PERIOD_MS)) {
-		// todo: extract helper method?
-		/* the address and content of the selected register is transmitted with the
-		 * next SPI transmission (for not existing addresses or wrong access mode
-		 * the data is always '0' */
-		tle8888_spi_rw(chip, CMD_FWDSTAT1, &maybeFirstResponse);
-		// here we get response of the 'FWDStat1' above
-		tle8888_spi_rw(chip, CMD_WdDiag, &functionWDrx);
-		handleFWDStat1(chip, getRegisterFromResponse(functionWDrx), getDataFromResponse(functionWDrx));
-		lastFunctionWatchdogTimeNt = nowNt;
-	}
-
-	tle8888_spi_rw(chip, CMD_WWDSTAT, NULL);
-	tle8888_spi_rw(chip, CMD_FWDSTAT0, &WindowWatchdogErrorCounterValue);
-	tle8888_spi_rw(chip, CMD_TECSTAT, &FunctionalWatchdogPassCounterValue);
-	tle8888_spi_rw(chip, CMD_TECSTAT, &TotalErrorCounterValue);
-
-
-	// sanity checking that we are looking at the right responses
-	if (getRegisterFromResponse(WindowWatchdogErrorCounterValue) == WWDStat &&
-			getRegisterFromResponse(FunctionalWatchdogPassCounterValue) == FWDStat0
-			) {
-
-		wasWatchdogHappy = isWatchdogHappy;
-		// reset state for error counters has us start in Safe Mode
-		isWatchdogHappy = (getWindowWatchdog() == 0 && getFunctionalWatchdog() == 0);
-		if (!wasWatchdogHappy && isWatchdogHappy) {
-			startupConfiguration(chip);
-		}
-	}
-}
-
-int tle8888SpiStartupExchange(struct tle8888_priv *chip);
-
 static THD_FUNCTION(tle8888_driver_thread, p) {
-	(void)p;
+	struct tle8888_priv *chip = p;
+	sysinterval_t poll_interval = 0;
 
 	chRegSetThreadName(DRIVER_NAME);
 
 	while (1) {
-		msg_t msg = chSemWaitTimeout(&tle8888_wake, TIME_MS2I(TLE8888_POLL_INTERVAL_MS));
+		int ret;
+		msg_t msg = chSemWaitTimeout(&chip->wake, poll_interval);
 
 		/* should we care about msg == MSG_TIMEOUT? */
 		(void)msg;
 
+		/* default polling interval */
+		poll_interval = TIME_MS2I(DIAG_PERIOD_MS);
+
+#if 0
 		if (vBattForTle8888 < LOW_VBATT) {
 			// we assume TLE8888 is down and we should not bother with SPI communication
 			if (!needInitialSpi) {
@@ -632,53 +814,64 @@ static THD_FUNCTION(tle8888_driver_thread, p) {
 			}
 			continue; // we should not bother communicating with TLE8888 until we have +12
 		}
-
-		if (needInitialSpi) {
-			wasWatchdogHappy = isWatchdogHappy = needInitialSpi = false;
-
-			for (int i = 0; i < BOARD_TLE8888_COUNT; i++) {
-				struct tle8888_priv *chip = &chips[i];
-				tle8888SpiStartupExchange(chip);
-			}
-		}
-
+#endif
 		// todo: super-lazy implementation with only first chip!
-		watchdogLogic(&chips[0]);
+		//watchdogLogic(&chips[0]);
 
-		for (int i = 0; i < BOARD_TLE8888_COUNT; i++) {
-			struct tle8888_priv *chip = &chips[i];
-			if ((chip->cfg == NULL) ||
-				(chip->drv_state == TLE8888_DISABLED) ||
-				(chip->drv_state == TLE8888_FAILED))
-				continue;
+		if ((chip->cfg == NULL) ||
+			(chip->drv_state == TLE8888_DISABLED) ||
+			(chip->drv_state == TLE8888_FAILED))
+			continue;
 
-			int ret = tle8888_update_output(chip);
+		bool wd_happy = chip->wd_happy;
+
+		/* update outputs only if WD is happy */
+		if (wd_happy) {
+			ret = tle8888_update_output(chip);
 			if (ret) {
 				/* set state to TLE8888_FAILED? */
 			}
-
-			if (chVTTimeElapsedSinceX(chip->ts_diag) >= TIME_MS2I(TLE8888_POLL_INTERVAL_MS)) {
-				/* this is expensive call, will do a lot of spi transfers... */
-				ret = tle8888_update_status_and_diag(chip);
-				if (ret) {
-					/* set state to TLE8888_FAILED or force reinit? */
-				}
-
-				chip->ts_diag = chVTGetSystemTimeX();
-			}
-
-			/* if bit OE is cleared - reset happened */
-			if (!(chip->OpStat[1] & (1 << 6))) {
-				needInitialSpi = true;
-				selfResetCounter++;
-			}
 		}
-	}
-}
 
-void requestTLE8888initialization(void) {
-	needInitialSpi = true;
-	requestedResetCounter++;
+		ret = tle8888_wd_feed(chip);
+		if (ret < 0) {
+			/* WD is not happy */
+			continue;
+		}
+		/* happiness state has changed! */
+		if ((chip->wd_happy != wd_happy) && (chip->wd_happy)) {
+			chip->need_init = true;
+		}
+
+		if (chip->need_init) {
+			/* clear first, as flag can be raised again during init */
+			chip->need_init = false;
+			/* re-init chip! */
+			tle8888_chip_init(chip);
+			/* sync pins state */
+			tle8888_update_output(chip);
+		}
+
+		if (chip->diag_ts <= chVTGetSystemTimeX()) {
+			/* this is expensive call, will do a lot of spi transfers... */
+			ret = tle8888_update_status_and_diag(chip);
+			if (ret) {
+				/* set state to TLE8888_FAILED or force reinit? */
+			}
+			/* TODO:
+			 * Procedure to switch on after failure condition occurred:
+			 *  - Read out of diagnosis bits
+			 *  - Second read out to verify that the failure conditions are not
+			 *    remaining
+			 *  - Set of the dedicated output enable bit of the affected channel
+			 *    if the diagnosis bit is not active anymore
+			 *  - Switch on of the channel */
+
+			chip->diag_ts = chTimeAddX(chVTGetSystemTimeX(), TIME_MS2I(DIAG_PERIOD_MS));
+		}
+
+		poll_interval = tle8888_calc_sleep_interval(chip);
+	}
 }
 
 /*==========================================================================*/
@@ -710,6 +903,8 @@ static int tle8888_setPadMode(void *data, unsigned int pin, iomode_t mode) {
 	} else {
 		chip->o_pp_mask |=  BIT(pin);
 	}
+#else
+	(void)mode;
 #endif
 
 	return 0;
@@ -738,27 +933,6 @@ static int tle8888_writePad(void *data, unsigned int pin, int value) {
 	return 0;
 }
 
-static brain_pin_diag_e tle8888_2b_to_diag_no_temp(unsigned int bits)
-{
-	if (bits == 0x01)
-		return PIN_SHORT_TO_BAT;
-	if (bits == 0x02)
-		return PIN_OPEN;
-	if (bits == 0x03)
-		return PIN_SHORT_TO_GND;
-	return PIN_OK;
-}
-
-static brain_pin_diag_e tle8888_2b_to_diag_with_temp(unsigned int bits)
-{
-	brain_pin_diag_e diag = tle8888_2b_to_diag_no_temp(bits);
-
-	if (diag == PIN_SHORT_TO_BAT)
-		diag |= PIN_DRIVER_OVERTEMP;
-
-	return diag;
-}
-
 static brain_pin_diag_e tle8888_getDiag(void *data, unsigned int pin)
 {
 	if ((pin >= TLE8888_OUTPUTS) || (data == NULL))
@@ -766,24 +940,40 @@ static brain_pin_diag_e tle8888_getDiag(void *data, unsigned int pin)
 
 	struct tle8888_priv *chip = (struct tle8888_priv *)data;
 
+	/* OUT1..OUT4, indexes 0..3 */
 	if (pin < 4)
 		return tle8888_2b_to_diag_with_temp((chip->OutDiag[0] >> ((pin - 0) * 2)) & 0x03);
-	if (pin < 8) {
+	/* OUT5..OUT7, indexes 4..6 */
+	if (pin < 7) {
+		return tle8888_2b_to_diag_with_temp((chip->OutDiag[1] >> ((pin - 4) * 2)) & 0x03);
+	}
+	/* OUT8 to OUT13, indexes 7..12 */
+	if (pin < 13) {
+		brain_pin_diag_e ret;
+
+		/* OUT8 */
 		if (pin == 7)
-			return tle8888_2b_to_diag_no_temp((chip->OutDiag[1] >> ((pin - 4) * 2)) & 0x03);
-		else
-			return tle8888_2b_to_diag_with_temp((chip->OutDiag[1] >> ((pin - 4) * 2)) & 0x03);
-	}
-	if (pin < 12)
-		return tle8888_2b_to_diag_with_temp((chip->OutDiag[2] >> ((pin - 8) * 2)) & 0x03);
-	if (pin < 16) {
-		if (pin == 12)
-			return tle8888_2b_to_diag_no_temp((chip->OutDiag[3] >> ((pin - 12) * 2)) & 0x03);
-		else
-			return tle8888_2b_to_diag_with_temp((chip->OutDiag[3] >> ((pin - 12) * 2)) & 0x03);
+			ret = tle8888_2b_to_diag_no_temp((chip->OutDiag[1] >> 6) & 0x03);
+		/* OUT9..OUT12 */
+		else if (pin < 12)
+			ret = tle8888_2b_to_diag_no_temp((chip->OutDiag[2] >> ((pin - 8) * 2)) & 0x03);
+		/* OUT13 */
+		else /* if (pin == 12) */
+			ret = tle8888_2b_to_diag_no_temp((chip->OutDiag[3] >> 0) & 0x03);
+
+		/* overvoltage bit */
+		if (chip->PPOVDiag & BIT(pin - 7))
+			ret |= PIN_SHORT_TO_BAT;
+
+		return ret;
 	}
+	/* OUT14 to OUT16, indexes 13..15 */
+	if (pin < 16)
+		return tle8888_2b_to_diag_with_temp((chip->OutDiag[3] >> ((pin - 13 + 1) * 2)) & 0x03);
+	/* OUT17 to OUT20, indexes 16..19 */
 	if (pin < 20)
 		return tle8888_2b_to_diag_with_temp((chip->OutDiag[4] >> ((pin - 16) * 2)) & 0x03);
+	/* OUT21..OUT24, indexes 20..23 */
 	if (pin < 24) {
 		/* half bridges */
 		brain_pin_diag_e diag;
@@ -806,59 +996,18 @@ static brain_pin_diag_e tle8888_getDiag(void *data, unsigned int pin)
 	return PIN_OK;
 }
 
-/**
- * @return 0 for valid configuration, -1 for invalid configuration
- */
-int tle8888SpiStartupExchange(struct tle8888_priv *chip) {
-	const struct tle8888_config	*cfg = chip->cfg;
-
-	tle8888reinitializationCounter++;
-
-	/**
-	 * We need around 50ms to get reliable TLE8888 start if MCU is powered externally but +12 goes gown and then goes up
-	 * again
-	 */
-	chThdSleepMilliseconds(50);
-
-	watchdogLogic(chip);
-
-	/* Software reset */
-	uint16_t response = 0;
-	tle8888_spi_rw(chip, CMD_SR, NULL);
-	tle8888_spi_rw(chip, CMD_UNLOCK, &response);
-	if (response != (CMD_WRITE | CMD_REG_ADDR(CMD_SR_CODE))) {
-		firmwareError(CUSTOM_ERR_TLE8888_RESPONSE, "please check your VBatt wiring and TLE8888 soldering r=%x while vbatt=%f", response, vBattForTle8888);
-	}
-
-	/**
-	 * Table 8. Reset Times. All reset times not more than 20uS
-	 *
-	 */
-	chThdSleepMilliseconds(3);
-
-	startupConfiguration(chip);
-
-	/* enable pins */
-	if (cfg->ign_en.port)
-		palSetPort(cfg->ign_en.port, PAL_PORT_BIT(cfg->ign_en.pad));
-	if (cfg->inj_en.port)
-		palSetPort(cfg->inj_en.port, PAL_PORT_BIT(cfg->inj_en.pad));
-
-	if (CONFIG(verboseTLE8888)) {
-		tle8888_dump_regs();
-	}
-
-	return 0;
-}
-
-static int tle8888_chip_init(void * data) {
+static int tle8888_chip_init_data(void * data) {
+	int i;
 	struct tle8888_priv *chip = (struct tle8888_priv *)data;
 	const struct tle8888_config	*cfg = chip->cfg;
 
 	int ret = 0;
+
+	chip->o_direct_mask = 0;
+	chip->o_oe_mask		= 0;
+	chip->o_pp_mask		= 0;
+
 	/* mark pins used */
-	// we do not initialize CS pin so we should not be marking it used - i'm sad
-	//ret = gpio_pin_markUsed(cfg->spi_config.ssport, cfg->spi_config.sspad, DRIVER_NAME " CS");
 	if (cfg->reset.port != NULL) {
 		ret |= gpio_pin_markUsed(cfg->reset.port, cfg->reset.pad, DRIVER_NAME " RST");
 		palSetPadMode(cfg->reset.port, cfg->reset.pad, PAL_MODE_OUTPUT_PUSHPULL);
@@ -874,50 +1023,88 @@ static int tle8888_chip_init(void * data) {
 		palSetPadMode(cfg->inj_en.port, cfg->inj_en.pad, PAL_MODE_OUTPUT_PUSHPULL);
 		palClearPort(cfg->inj_en.port, PAL_PORT_BIT(cfg->inj_en.pad));
 	}
-	for (int i = 0; i < TLE8888_DIRECT_MISC; i++) {
-		if (cfg->direct_io[i].port) {
-// TODO: we need this but that's incompatible configuration change
-//			ret |= gpio_pin_markUsed(cfg->direct_io[i].port, cfg->direct_io[i].pad, DRIVER_NAME " DIRECT IO");
-//			palSetPadMode(cfg->direct_io[i].port, cfg->direct_io[i].pad, PAL_MODE_OUTPUT_PUSHPULL);
-//			palClearPort(cfg->direct_io[i].port, PAL_PORT_BIT(cfg->direct_io[i].pad));
+
+	for (i = 0; i < TLE8888_DIRECT_OUTPUTS; i++) {
+		int out = -1;
+		uint32_t mask;
+
+		if (i < 4) {
+			/* injector */
+			out = i;
+		} else if (i < 8) {
+			/* ignition */
+			out = (i - 4) + 24;
+		} else {
+			/* remappable */
+			/* in config counted from 1 */
+			out = cfg->direct_maps[i - 8].output - 1;
+		}
+
+		if ((out < 0) || (cfg->direct_gpio[i].port == NULL))
+			continue;
+
+		/* calculate mask */
+		mask = BIT(out);
+
+		/* check if output already occupied */
+		if (chip->o_direct_mask & mask) {
+			/* incorrect config? */
+			ret = -1;
+			goto err_gpios;
+		}
+
+		/* configure source gpio */
+		ret = gpio_pin_markUsed(cfg->direct_gpio[i].port, cfg->direct_gpio[i].pad, DRIVER_NAME " DIRECT IO");
+		if (ret) {
+			ret = -1;
+			goto err_gpios;
+		}
+		palSetPadMode(cfg->direct_gpio[i].port, cfg->direct_gpio[i].pad, PAL_MODE_OUTPUT_PUSHPULL);
+		palClearPort(cfg->direct_gpio[i].port, PAL_PORT_BIT(cfg->direct_gpio[i].pad));
+
+		/* enable direct drive */
+		chip->o_direct_mask	|= mask;
+
+		/* calculate INCONFIG - aux input mapping for IN9..IN12 */
+		if (i >= 8) {
+			if ((out < 4) || (out >= 24)) {
+				ret = -1;
+				goto err_gpios;
+			}
+			chip->InConfig[i - 8] = out - 4;
 		}
 	}
 
+	/* HACK HERE if you want to enable PP for OUT21..OUT24
+	 * without approprirate call to setPinMode */
+	//chip->o_pp_mask		|= BIT(20) | BIT(21) | BIT(22) | BIT(23);
 
-	if (ret) {
-		ret = -1;
-		goto err_gpios;
-	}
+	/* enable all direct driven */
+	chip->o_oe_mask		|= chip->o_direct_mask;
+
+	/* enable all ouputs
+	 * TODO: add API to enable/disable? */
+	chip->o_oe_mask		|= 0x0ffffff0;
 
 	return 0;
 
 err_gpios:
 	/* unmark pins */
-	//gpio_pin_markUnused(cfg->spi_config.ssport, cfg->spi_config.sspad);
 	if (cfg->inj_en.port != NULL)
 		gpio_pin_markUnused(cfg->inj_en.port, cfg->inj_en.pad);
 	if (cfg->ign_en.port != NULL)
 		gpio_pin_markUnused(cfg->ign_en.port, cfg->ign_en.pad);
 	if (cfg->reset.port != NULL)
 		gpio_pin_markUnused(cfg->reset.port, cfg->reset.pad);
-	for (int i = 0; i < TLE8888_DIRECT_MISC; i++) {
-		if (cfg->direct_io[i].port) {
-// TODO: we need this but that's incompatible configuration change
-//			gpio_pin_markUnused(cfg->direct_io[i].port, cfg->direct_io[i].pad);
+	for (int i = 0; i < TLE8888_DIRECT_OUTPUTS; i++) {
+		if (cfg->direct_gpio[i].port) {
+			gpio_pin_markUnused(cfg->direct_gpio[i].port, cfg->direct_gpio[i].pad);
 		}
 	}
 
 	return ret;
 }
 
-/* DEBUG */
-void tle8888_read_reg(uint16_t reg, uint16_t *val)
-{
-	struct tle8888_priv *chip = &chips[0];
-
-	tle8888_spi_rw(chip, CMD_R(reg), val);
-}
-
 static int tle8888_init(void * data)
 {
 	int ret;
@@ -929,18 +1116,26 @@ static int tle8888_init(void * data)
 	if (chip->drv_state != TLE8888_WAIT_INIT)
 		return -1;
 
-	ret = tle8888_chip_init(chip);
+	ret = tle8888_chip_reset(chip);
 	if (ret)
 		return ret;
 
+	ret = tle8888_chip_init_data(chip);
+	if (ret)
+		return ret;
+
+	/* force init from driver thread */
+	chip->need_init = true;
+
+	/* instance is ready */
 	chip->drv_state = TLE8888_READY;
 
-	/* one task for all TLE8888 instances, so create only once */
-	if (!drv_task_ready) {
-		chThdCreateStatic(tle8888_thread_1_wa, sizeof(tle8888_thread_1_wa),
-						  NORMALPRIO + 1, tle8888_driver_thread, NULL);
-		drv_task_ready = true;
-	}
+	/* init semaphore */
+	chSemObjectInit(&chip->wake, 10);
+
+	/* start thread */
+	chip->thread = chThdCreateStatic(chip->thread_wa, sizeof(chip->thread_wa),
+									 NORMALPRIO + 1, tle8888_driver_thread, chip);
 
 	return 0;
 }
@@ -956,7 +1151,9 @@ static int tle8888_deinit(void *data)
 	if (cfg->inj_en.port)
 		palClearPort(cfg->inj_en.port, PAL_PORT_BIT(cfg->inj_en.pad));
 
-	/* TODO: stop task? */
+	/* stop thread */
+	chThdTerminate(chip->thread);
+
 	return 0;
 }
 
@@ -996,8 +1193,8 @@ int tle8888_add(unsigned int index, const struct tle8888_config *cfg) {
 
 	chip->cfg = cfg;
 	chip->o_state = 0;
-	chip->o_state_cached = 0;
 	chip->o_direct_mask = 0;
+	chip->cont_data_cached = 0;
 	chip->drv_state = TLE8888_WAIT_INIT;
 
 	/* register, return gpio chip base */
@@ -1009,6 +1206,24 @@ int tle8888_add(unsigned int index, const struct tle8888_config *cfg) {
 	return ret;
 }
 
+/*==========================================================================*/
+/* Driver exported debug functions.												*/
+/*==========================================================================*/
+void tle8888_read_reg(uint16_t reg, uint16_t *val)
+{
+	struct tle8888_priv *chip = &chips[0];
+
+	tle8888_spi_rw(chip, CMD_R(reg), val);
+}
+
+void tle8888_req_init(void)
+{
+	struct tle8888_priv *chip = &chips[0];
+
+	chip->need_init = true;
+	chip->init_req_cnt++;
+}
+
 #else /* BOARD_TLE8888_COUNT > 0 */
 
 int tle8888_add(unsigned int index, const struct tle8888_config *cfg)
diff --git a/firmware/hw_layer/drivers/gpio/tle8888.h b/firmware/hw_layer/drivers/gpio/tle8888.h
index fcb5361317..21cfd67eaf 100644
--- a/firmware/hw_layer/drivers/gpio/tle8888.h
+++ b/firmware/hw_layer/drivers/gpio/tle8888.h
@@ -19,21 +19,9 @@
  * 4 INJ channels - OUT1..4 - IN5..8 */
 #define TLE8888_DIRECT_OUTPUTS		(4 + 4 + TLE8888_DIRECT_MISC)
 
-// Looks like reset value is 113.6ms? 1.6ms * 0x47
-#define Functional_Watchdog_PERIOD_MS 20
-
-// we can change this value on TLE8888QK but we probably do not have a reason to change
-#define Window_watchdog_close_window_time_ms 100.8
-
 #define getRegisterFromResponse(x) (((x) >> 1) & 0x7f)
 #define getDataFromResponse(x) (((x) >> 8) & 0xff)
 
-// unchangeable value for TLE8888QK
-// unused for now
-//#define Window_watchdog_open_window_time_ms 12.8
-
-/* DOTO: add irq support */
-#define TLE8888_POLL_INTERVAL_MS	7
 
 /* note that spi transfer should be LSB first */
 struct tle8888_config {
@@ -45,12 +33,15 @@ struct tle8888_config {
 		uint_fast8_t	pad;
 	} reset;
 	struct {
-		/* MCU port-pin routed to IN9..12 */
+		/* MCU port-pin routed to IN1..12 */
 		ioportid_t		port;
 		uint_fast8_t	pad;
+	} direct_gpio[TLE8888_DIRECT_OUTPUTS];
+	/* IN9..IN12 to output mapping */
+	struct {
 		/* ...used to drive output (starts from 1, as in DS, coders gonna hate) */
 		int 			output;
-	} direct_io[TLE8888_DIRECT_MISC];
+	} direct_maps[TLE8888_DIRECT_MISC];
 	struct {
 		ioportid_t		port;
 		uint_fast8_t	pad;
@@ -71,7 +62,10 @@ extern "C"
  * @return return gpio chip base
  */
 int tle8888_add(unsigned int index, const struct tle8888_config *cfg);
-void requestTLE8888initialization(void);
+
+/* debug */
+void tle8888_read_reg(uint16_t reg, uint16_t *val);
+void tle8888_req_init(void);
 
 #if EFI_TUNER_STUDIO
 #include "tunerstudio_debug_struct.h"
diff --git a/firmware/hw_layer/pin_repository.cpp b/firmware/hw_layer/pin_repository.cpp
index 4f4c44302a..33625883a1 100644
--- a/firmware/hw_layer/pin_repository.cpp
+++ b/firmware/hw_layer/pin_repository.cpp
@@ -95,19 +95,15 @@ PinRepository::PinRepository() {
 }
 
 #if (BOARD_TLE8888_COUNT > 0)
-/* DEBUG */
-extern "C" {
-	extern void tle8888_read_reg(uint16_t reg, uint16_t *val);
-}
 void tle8888_dump_regs(void)
 {
 	// since responses are always in the NEXT transmission we will have this one first
 	tle8888_read_reg(0, NULL);
 
 	scheduleMsg(&logger, "register: data");
-	for (int request = 0; request < 0x7e + 1; request++) {
+	for (int request = 0; request <= 0x7e + 1; request++) {
 		uint16_t tmp;
-		tle8888_read_reg(request, &tmp);
+		tle8888_read_reg(request < (0x7e + 1) ? request : 0x7e, &tmp);
 		uint8_t reg = getRegisterFromResponse(tmp);
 		uint8_t data = getDataFromResponse(tmp);
 
@@ -227,7 +223,7 @@ void initPinRepository(void) {
 
 #if (BOARD_TLE8888_COUNT > 0)
 	addConsoleAction("tle8888", tle8888_dump_regs);
-	addConsoleAction("tle8888init", requestTLE8888initialization);
+	addConsoleAction("tle8888init", tle8888_req_init);
 #endif
 }
 
diff --git a/firmware/hw_layer/smart_gpio.cpp b/firmware/hw_layer/smart_gpio.cpp
index 24229bbab5..e253299450 100644
--- a/firmware/hw_layer/smart_gpio.cpp
+++ b/firmware/hw_layer/smart_gpio.cpp
@@ -122,20 +122,29 @@ struct tle8888_config tle8888_cfg = {
 		.cr2 = SPI_CR2_16BIT_MODE
 	},
 	.reset =  {.port = NULL,	.pad = 0},
-	.direct_io = {
-		[0] = {.port = GPIOE,	.pad = 10,	.output =  5},
-		[1] = {.port = GPIOE,	.pad = 9,	.output =  6},
-		[2] = {.port = GPIOE,	.pad = 8,	.output = 21},
-		[3] = {.port = NULL,	.pad = 0,	.output = 15},
-//		[3] = {.port = GPIOE,	.pad = 7,	.output = 22},
-
-
-/*
-		[0] = {.port = NULL,	.pad = 0,	.output = 9},
-		[1] = {.port = NULL,	.pad = 0,	.output = 10},
-		[2] = {.port = NULL,	.pad = 0,	.output = 11},
-		[3] = {.port = NULL,	.pad = 0,	.output = 12},
-*/
+	.direct_gpio = {
+		/* IN1..4 -> OUT1..OUT4 (Injectors) */
+		[0] = {.port = GPIOE,	.pad = 14},
+		[1] = {.port = GPIOE,	.pad = 13},
+		[2] = {.port = GPIOE,	.pad = 12},
+		[3] = {.port = GPIOE,	.pad = 11},
+		/* IN5..8 -> IGN1..IGN4 (Ignotors) */
+		/* Not used */
+		[4] = {.port = NULL,	.pad = 0},
+		[5] = {.port = NULL,	.pad = 0},
+		[6] = {.port = NULL,	.pad = 0},
+		[7] = {.port = NULL,	.pad = 0},
+		/* Remapable IN9..12 */
+		[8] = {.port = GPIOE,	.pad = 10},
+		[9] = {.port = GPIOE,	.pad = 9},
+		[10] = {.port = GPIOE,	.pad = 8},
+		[11] = {.port = GPIOE,	.pad = 7},
+	},
+	.direct_maps = {
+		[0] = {.output =  5},
+		[1] = {.output =  6},
+		[2] = {.output = 21},
+		[3] = {.output = 22},
 	},
 	.ign_en =  {.port = GPIOD,	.pad = 10},
 	.inj_en =  {.port = GPIOD,	.pad = 11},