diff --git a/bootloader/targets/f5/api-hal/api-hal-i2c.c b/bootloader/targets/f5/api-hal/api-hal-i2c.c
index d9aca753..e1548ee7 100644
--- a/bootloader/targets/f5/api-hal/api-hal-i2c.c
+++ b/bootloader/targets/f5/api-hal/api-hal-i2c.c
@@ -4,6 +4,7 @@
 #include <stm32wbxx_ll_i2c.h>
 #include <stm32wbxx_ll_rcc.h>
 #include <stm32wbxx_ll_gpio.h>
+#include <stm32wbxx_ll_cortex.h>
 
 void api_hal_i2c_init() {
     LL_I2C_InitTypeDef I2C_InitStruct = {0};
@@ -37,7 +38,15 @@ void api_hal_i2c_init() {
     LL_I2C_EnableClockStretching(I2C1);
 }
 
-void api_hal_i2c_tx(I2C_TypeDef* instance, uint8_t address, const uint8_t* data, uint8_t size) {
+bool api_hal_i2c_tx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    const uint8_t* data,
+    uint8_t size,
+    uint32_t timeout) {
+    uint32_t time_left = timeout;
+    bool ret = true;
+
     LL_I2C_HandleTransfer(
         instance,
         address,
@@ -49,13 +58,30 @@ void api_hal_i2c_tx(I2C_TypeDef* instance, uint8_t address, const uint8_t* data,
     while(!LL_I2C_IsActiveFlag_STOP(instance)) {
         if(LL_I2C_IsActiveFlag_TXIS(instance)) {
             LL_I2C_TransmitData8(instance, (*data++));
+            time_left = timeout;
+        }
+
+        if(LL_SYSTICK_IsActiveCounterFlag()) {
+            if(--time_left == 0) {
+                ret = false;
+                break;
+            }
         }
     }
 
     LL_I2C_ClearFlag_STOP(instance);
+    return ret;
 }
 
-void api_hal_i2c_rx(I2C_TypeDef* instance, uint8_t address, uint8_t* data, uint8_t size) {
+bool api_hal_i2c_rx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    uint8_t* data,
+    uint8_t size,
+    uint32_t timeout) {
+    uint32_t time_left = timeout;
+    bool ret = true;
+
     LL_I2C_HandleTransfer(
         instance,
         address,
@@ -67,19 +93,33 @@ void api_hal_i2c_rx(I2C_TypeDef* instance, uint8_t address, uint8_t* data, uint8
     while(!LL_I2C_IsActiveFlag_STOP(instance)) {
         if(LL_I2C_IsActiveFlag_RXNE(instance)) {
             *data++ = LL_I2C_ReceiveData8(instance);
+            time_left = timeout;
+        }
+
+        if(LL_SYSTICK_IsActiveCounterFlag()) {
+            if(--time_left == 0) {
+                ret = false;
+                break;
+            }
         }
     }
 
     LL_I2C_ClearFlag_STOP(instance);
+    return ret;
 }
 
-void api_hal_i2c_trx(
+bool api_hal_i2c_trx(
     I2C_TypeDef* instance,
     uint8_t address,
     const uint8_t* tx_data,
     uint8_t tx_size,
     uint8_t* rx_data,
-    uint8_t rx_size) {
-    api_hal_i2c_tx(instance, address, tx_data, tx_size);
-    api_hal_i2c_rx(instance, address, rx_data, rx_size);
+    uint8_t rx_size,
+    uint32_t timeout) {
+    if(api_hal_i2c_tx(instance, address, tx_data, tx_size, timeout) &&
+       api_hal_i2c_rx(instance, address, rx_data, rx_size, timeout)) {
+        return true;
+    } else {
+        return false;
+    }
 }
diff --git a/bootloader/targets/f5/api-hal/api-hal-i2c.h b/bootloader/targets/f5/api-hal/api-hal-i2c.h
index a3e14681..835693a6 100644
--- a/bootloader/targets/f5/api-hal/api-hal-i2c.h
+++ b/bootloader/targets/f5/api-hal/api-hal-i2c.h
@@ -10,25 +10,28 @@ extern "C" {
 
 void api_hal_i2c_init();
 
-void api_hal_i2c_tx(
+bool api_hal_i2c_tx(
     I2C_TypeDef* instance,
     const uint8_t address,
     const uint8_t* data,
-    const uint8_t size);
+    const uint8_t size,
+    uint32_t timeout);
 
-void api_hal_i2c_rx(
+bool api_hal_i2c_rx(
     I2C_TypeDef* instance,
     const uint8_t address,
     uint8_t* data,
-    const uint8_t size);
+    const uint8_t size,
+    uint32_t timeout);
 
-void api_hal_i2c_trx(
+bool api_hal_i2c_trx(
     I2C_TypeDef* instance,
     const uint8_t address,
     const uint8_t* tx_data,
     const uint8_t tx_size,
     uint8_t* rx_data,
-    const uint8_t rx_size);
+    const uint8_t rx_size,
+    uint32_t timeout);
 
 #define with_api_hal_i2c(type, pointer, function_body)        \
     {                                                         \
diff --git a/firmware/targets/api-hal-include/api-hal-i2c.h b/firmware/targets/api-hal-include/api-hal-i2c.h
index 755cf292..596fffd7 100644
--- a/firmware/targets/api-hal-include/api-hal-i2c.h
+++ b/firmware/targets/api-hal-include/api-hal-i2c.h
@@ -10,25 +10,38 @@ extern "C" {
 
 void api_hal_i2c_init();
 
-void api_hal_i2c_tx(I2C_TypeDef* instance, const uint8_t address, const uint8_t *data, const uint8_t size);
+bool api_hal_i2c_tx(
+    I2C_TypeDef* instance,
+    const uint8_t address,
+    const uint8_t* data,
+    const uint8_t size,
+    uint32_t timeout);
 
-void api_hal_i2c_rx(I2C_TypeDef* instance, const uint8_t address, uint8_t *data, const uint8_t size);
+bool api_hal_i2c_rx(
+    I2C_TypeDef* instance,
+    const uint8_t address,
+    uint8_t* data,
+    const uint8_t size,
+    uint32_t timeout);
 
-void api_hal_i2c_trx(
-    I2C_TypeDef* instance, const uint8_t address,
-    const uint8_t *tx_data, const uint8_t tx_size,
-    uint8_t *rx_data, const uint8_t rx_size
-);
+bool api_hal_i2c_trx(
+    I2C_TypeDef* instance,
+    const uint8_t address,
+    const uint8_t* tx_data,
+    const uint8_t tx_size,
+    uint8_t* rx_data,
+    const uint8_t rx_size,
+    uint32_t timeout);
 
 void api_hal_i2c_lock();
 
 void api_hal_i2c_unlock();
 
-#define with_api_hal_i2c(type, pointer, function_body)          \
-    {                                                           \
-        api_hal_i2c_lock();                                     \
-        *pointer = ({ type __fn__ function_body __fn__; })();   \
-        api_hal_i2c_unlock();                                   \
+#define with_api_hal_i2c(type, pointer, function_body)        \
+    {                                                         \
+        api_hal_i2c_lock();                                   \
+        *pointer = ({ type __fn__ function_body __fn__; })(); \
+        api_hal_i2c_unlock();                                 \
     }
 
 #ifdef __cplusplus
diff --git a/firmware/targets/f4/api-hal/api-hal-i2c.c b/firmware/targets/f4/api-hal/api-hal-i2c.c
index b56895a1..c6ef2c70 100644
--- a/firmware/targets/f4/api-hal/api-hal-i2c.c
+++ b/firmware/targets/f4/api-hal/api-hal-i2c.c
@@ -1,6 +1,7 @@
 #include <api-hal-i2c.h>
 #include <stm32wbxx_ll_i2c.h>
 #include <stm32wbxx_ll_gpio.h>
+#include <stm32wbxx_ll_cortex.h>
 #include <furi.h>
 
 osMutexId_t api_hal_i2c_mutex = NULL;
@@ -40,33 +41,90 @@ void api_hal_i2c_init() {
     LL_I2C_EnableClockStretching(I2C1);
 }
 
-void api_hal_i2c_tx(I2C_TypeDef* instance, uint8_t address, const uint8_t *data, uint8_t size) {
-    LL_I2C_HandleTransfer(instance, address, LL_I2C_ADDRSLAVE_7BIT, size, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_START_WRITE);
+bool api_hal_i2c_tx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    const uint8_t* data,
+    uint8_t size,
+    uint32_t timeout) {
+    uint32_t time_left = timeout;
+    bool ret = true;
 
-    while (!LL_I2C_IsActiveFlag_STOP(instance)) {
-        if (LL_I2C_IsActiveFlag_TXIS(instance)) {
+    LL_I2C_HandleTransfer(
+        instance,
+        address,
+        LL_I2C_ADDRSLAVE_7BIT,
+        size,
+        LL_I2C_MODE_AUTOEND,
+        LL_I2C_GENERATE_START_WRITE);
+
+    while(!LL_I2C_IsActiveFlag_STOP(instance)) {
+        if(LL_I2C_IsActiveFlag_TXIS(instance)) {
             LL_I2C_TransmitData8(instance, (*data++));
+            time_left = timeout;
+        }
+
+        if(LL_SYSTICK_IsActiveCounterFlag()) {
+            if(--time_left == 0) {
+                ret = false;
+                break;
+            }
         }
     }
 
     LL_I2C_ClearFlag_STOP(instance);
+    return ret;
 }
 
-void api_hal_i2c_rx(I2C_TypeDef* instance, uint8_t address, uint8_t *data, uint8_t size) {
-    LL_I2C_HandleTransfer(instance, address, LL_I2C_ADDRSLAVE_7BIT, size, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_START_READ);
+bool api_hal_i2c_rx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    uint8_t* data,
+    uint8_t size,
+    uint32_t timeout) {
+    uint32_t time_left = timeout;
+    bool ret = true;
 
-    while (!LL_I2C_IsActiveFlag_STOP(instance)) {
-        if (LL_I2C_IsActiveFlag_RXNE(instance)) {
+    LL_I2C_HandleTransfer(
+        instance,
+        address,
+        LL_I2C_ADDRSLAVE_7BIT,
+        size,
+        LL_I2C_MODE_AUTOEND,
+        LL_I2C_GENERATE_START_READ);
+
+    while(!LL_I2C_IsActiveFlag_STOP(instance)) {
+        if(LL_I2C_IsActiveFlag_RXNE(instance)) {
             *data++ = LL_I2C_ReceiveData8(instance);
+            time_left = timeout;
+        }
+
+        if(LL_SYSTICK_IsActiveCounterFlag()) {
+            if(--time_left == 0) {
+                ret = false;
+                break;
+            }
         }
     }
 
     LL_I2C_ClearFlag_STOP(instance);
+    return ret;
 }
 
-void api_hal_i2c_trx(I2C_TypeDef* instance, uint8_t address, const uint8_t *tx_data, uint8_t tx_size, uint8_t *rx_data, uint8_t rx_size) {
-    api_hal_i2c_tx(instance, address, tx_data, tx_size);
-    api_hal_i2c_rx(instance, address, rx_data, rx_size);
+bool api_hal_i2c_trx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    const uint8_t* tx_data,
+    uint8_t tx_size,
+    uint8_t* rx_data,
+    uint8_t rx_size,
+    uint32_t timeout) {
+    if(api_hal_i2c_tx(instance, address, tx_data, tx_size, timeout) &&
+       api_hal_i2c_rx(instance, address, rx_data, rx_size, timeout)) {
+        return true;
+    } else {
+        return false;
+    }
 }
 
 void api_hal_i2c_lock() {
diff --git a/firmware/targets/f4/api-hal/api-hal-vibro.c b/firmware/targets/f4/api-hal/api-hal-vibro.c
new file mode 100644
index 00000000..d418aacc
--- /dev/null
+++ b/firmware/targets/f4/api-hal/api-hal-vibro.c
@@ -0,0 +1,11 @@
+#include <api-hal-vibro.h>
+#include <api-hal-gpio.h>
+
+void api_hal_vibro_init() {
+    hal_gpio_init(&vibro_gpio, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
+    hal_gpio_write(&vibro_gpio, false);
+}
+
+void api_hal_vibro_on(bool value) {
+    hal_gpio_write(&vibro_gpio, value);
+}
diff --git a/firmware/targets/f4/api-hal/api-hal.c b/firmware/targets/f4/api-hal/api-hal.c
index 70fe3108..6d24afcf 100644
--- a/firmware/targets/f4/api-hal/api-hal.c
+++ b/firmware/targets/f4/api-hal/api-hal.c
@@ -7,4 +7,5 @@ void api_hal_init() {
     api_hal_i2c_init();
     api_hal_power_init();
     api_hal_light_init();
+    api_hal_vibro_init();
 }
\ No newline at end of file
diff --git a/firmware/targets/f5/api-hal/api-hal-i2c.c b/firmware/targets/f5/api-hal/api-hal-i2c.c
index b56895a1..c6ef2c70 100644
--- a/firmware/targets/f5/api-hal/api-hal-i2c.c
+++ b/firmware/targets/f5/api-hal/api-hal-i2c.c
@@ -1,6 +1,7 @@
 #include <api-hal-i2c.h>
 #include <stm32wbxx_ll_i2c.h>
 #include <stm32wbxx_ll_gpio.h>
+#include <stm32wbxx_ll_cortex.h>
 #include <furi.h>
 
 osMutexId_t api_hal_i2c_mutex = NULL;
@@ -40,33 +41,90 @@ void api_hal_i2c_init() {
     LL_I2C_EnableClockStretching(I2C1);
 }
 
-void api_hal_i2c_tx(I2C_TypeDef* instance, uint8_t address, const uint8_t *data, uint8_t size) {
-    LL_I2C_HandleTransfer(instance, address, LL_I2C_ADDRSLAVE_7BIT, size, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_START_WRITE);
+bool api_hal_i2c_tx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    const uint8_t* data,
+    uint8_t size,
+    uint32_t timeout) {
+    uint32_t time_left = timeout;
+    bool ret = true;
 
-    while (!LL_I2C_IsActiveFlag_STOP(instance)) {
-        if (LL_I2C_IsActiveFlag_TXIS(instance)) {
+    LL_I2C_HandleTransfer(
+        instance,
+        address,
+        LL_I2C_ADDRSLAVE_7BIT,
+        size,
+        LL_I2C_MODE_AUTOEND,
+        LL_I2C_GENERATE_START_WRITE);
+
+    while(!LL_I2C_IsActiveFlag_STOP(instance)) {
+        if(LL_I2C_IsActiveFlag_TXIS(instance)) {
             LL_I2C_TransmitData8(instance, (*data++));
+            time_left = timeout;
+        }
+
+        if(LL_SYSTICK_IsActiveCounterFlag()) {
+            if(--time_left == 0) {
+                ret = false;
+                break;
+            }
         }
     }
 
     LL_I2C_ClearFlag_STOP(instance);
+    return ret;
 }
 
-void api_hal_i2c_rx(I2C_TypeDef* instance, uint8_t address, uint8_t *data, uint8_t size) {
-    LL_I2C_HandleTransfer(instance, address, LL_I2C_ADDRSLAVE_7BIT, size, LL_I2C_MODE_AUTOEND, LL_I2C_GENERATE_START_READ);
+bool api_hal_i2c_rx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    uint8_t* data,
+    uint8_t size,
+    uint32_t timeout) {
+    uint32_t time_left = timeout;
+    bool ret = true;
 
-    while (!LL_I2C_IsActiveFlag_STOP(instance)) {
-        if (LL_I2C_IsActiveFlag_RXNE(instance)) {
+    LL_I2C_HandleTransfer(
+        instance,
+        address,
+        LL_I2C_ADDRSLAVE_7BIT,
+        size,
+        LL_I2C_MODE_AUTOEND,
+        LL_I2C_GENERATE_START_READ);
+
+    while(!LL_I2C_IsActiveFlag_STOP(instance)) {
+        if(LL_I2C_IsActiveFlag_RXNE(instance)) {
             *data++ = LL_I2C_ReceiveData8(instance);
+            time_left = timeout;
+        }
+
+        if(LL_SYSTICK_IsActiveCounterFlag()) {
+            if(--time_left == 0) {
+                ret = false;
+                break;
+            }
         }
     }
 
     LL_I2C_ClearFlag_STOP(instance);
+    return ret;
 }
 
-void api_hal_i2c_trx(I2C_TypeDef* instance, uint8_t address, const uint8_t *tx_data, uint8_t tx_size, uint8_t *rx_data, uint8_t rx_size) {
-    api_hal_i2c_tx(instance, address, tx_data, tx_size);
-    api_hal_i2c_rx(instance, address, rx_data, rx_size);
+bool api_hal_i2c_trx(
+    I2C_TypeDef* instance,
+    uint8_t address,
+    const uint8_t* tx_data,
+    uint8_t tx_size,
+    uint8_t* rx_data,
+    uint8_t rx_size,
+    uint32_t timeout) {
+    if(api_hal_i2c_tx(instance, address, tx_data, tx_size, timeout) &&
+       api_hal_i2c_rx(instance, address, rx_data, rx_size, timeout)) {
+        return true;
+    } else {
+        return false;
+    }
 }
 
 void api_hal_i2c_lock() {
diff --git a/lib/drivers/bq25896.c b/lib/drivers/bq25896.c
index 9aea4ce8..c07a710e 100644
--- a/lib/drivers/bq25896.c
+++ b/lib/drivers/bq25896.c
@@ -5,21 +5,19 @@
 #include <stddef.h>
 
 uint8_t bit_reverse(uint8_t b) {
-   b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
-   b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
-   b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
-   return b;
+    b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
+    b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
+    b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
+    return b;
 }
 
 bool bq25896_read(uint8_t address, uint8_t* data, size_t size) {
     bool ret;
-    with_api_hal_i2c(bool, &ret, (){
-        api_hal_i2c_trx(
-            POWER_I2C, BQ25896_ADDRESS,
-            &address, 1, data, size
-        );
-        return true;
-    });
+    with_api_hal_i2c(
+        bool, &ret, () {
+            return api_hal_i2c_trx(
+                POWER_I2C, BQ25896_ADDRESS, &address, 1, data, size, BQ25896_I2C_TIMEOUT);
+        });
     return ret;
 }
 
@@ -29,12 +27,12 @@ bool bq25896_read_reg(uint8_t address, uint8_t* data) {
 }
 
 bool bq25896_write_reg(uint8_t address, uint8_t* data) {
-    uint8_t buffer[2] = { address, *data };
+    uint8_t buffer[2] = {address, *data};
     bool ret;
-    with_api_hal_i2c(bool, &ret, (){
-        api_hal_i2c_tx(POWER_I2C, BQ25896_ADDRESS, buffer, 2);
-        return true;
-    });
+    with_api_hal_i2c(
+        bool, &ret, () {
+            return api_hal_i2c_tx(POWER_I2C, BQ25896_ADDRESS, buffer, 2, BQ25896_I2C_TIMEOUT);
+        });
     return ret;
 }
 
@@ -83,7 +81,7 @@ void bq25896_init() {
 }
 
 void bq25896_poweroff() {
-    bq25896_regs.r09.BATFET_DIS=1;
+    bq25896_regs.r09.BATFET_DIS = 1;
     bq25896_write_reg(0x09, (uint8_t*)&bq25896_regs.r09);
 }
 
@@ -105,7 +103,7 @@ void bq25896_disable_otg() {
 
 uint16_t bq25896_get_vbus_voltage() {
     bq25896_read_reg(0x11, (uint8_t*)&bq25896_regs.r11);
-    if (bq25896_regs.r11.VBUS_GD) {
+    if(bq25896_regs.r11.VBUS_GD) {
         return (uint16_t)bq25896_regs.r11.VBUSV * 100 + 2600;
     } else {
         return 0;
@@ -129,5 +127,5 @@ uint16_t bq25896_get_vbat_current() {
 
 uint32_t bq25896_get_ntc_mpct() {
     bq25896_read_reg(0x10, (uint8_t*)&bq25896_regs.r10);
-    return (uint32_t)bq25896_regs.r10.TSPCT * 465+21000;
+    return (uint32_t)bq25896_regs.r10.TSPCT * 465 + 21000;
 }
diff --git a/lib/drivers/bq25896_reg.h b/lib/drivers/bq25896_reg.h
index b3704106..3cb3d714 100644
--- a/lib/drivers/bq25896_reg.h
+++ b/lib/drivers/bq25896_reg.h
@@ -8,105 +8,99 @@
 #endif
 
 #define BQ25896_ADDRESS 0xD6
+#define BQ25896_I2C_TIMEOUT 50
 
-#define IILIM_1600          (1<<5)
-#define IILIM_800           (1<<4)
-#define IILIM_400           (1<<3)
-#define IILIM_200           (1<<2)
-#define IILIM_100           (1<<1)
-#define IILIM_50            (1<<0)
+#define IILIM_1600 (1 << 5)
+#define IILIM_800 (1 << 4)
+#define IILIM_400 (1 << 3)
+#define IILIM_200 (1 << 2)
+#define IILIM_100 (1 << 1)
+#define IILIM_50 (1 << 0)
 
 typedef struct {
-    uint8_t IINLIM:6;       // Input Current Limit, mA, offset: +100mA
-    bool EN_ILIM:1;         // Enable ILIM Pin
-    bool EN_HIZ:1;          // Enable HIZ Mode
+    uint8_t IINLIM : 6; // Input Current Limit, mA, offset: +100mA
+    bool EN_ILIM : 1; // Enable ILIM Pin
+    bool EN_HIZ : 1; // Enable HIZ Mode
 } REG00;
 
-
-#define VINDPM_OS_1600      (1<<4)
-#define VINDPM_OS_800       (1<<3)
-#define VINDPM_OS_400       (1<<2)
-#define VINDPM_OS_200       (1<<1)
-#define VINDPM_OS_100       (1<<0)
+#define VINDPM_OS_1600 (1 << 4)
+#define VINDPM_OS_800 (1 << 3)
+#define VINDPM_OS_400 (1 << 2)
+#define VINDPM_OS_200 (1 << 1)
+#define VINDPM_OS_100 (1 << 0)
 
 typedef enum {
-    Bhot34 = 0b00,         // – VBHOT1 Threshold (34.75%) (default)
-    Bhot37 = 0b01,         // – VBHOT0 Threshold (Typ. 37.75%)
-    Bhot31 = 0b10,         // – VBHOT2 Threshold (Typ. 31.25%)
-    BhotDisable = 0b11,    // – Disable boost mode thermal protection
+    Bhot34 = 0b00, // – VBHOT1 Threshold (34.75%) (default)
+    Bhot37 = 0b01, // – VBHOT0 Threshold (Typ. 37.75%)
+    Bhot31 = 0b10, // – VBHOT2 Threshold (Typ. 31.25%)
+    BhotDisable = 0b11, // – Disable boost mode thermal protection
 } Bhot;
 
 typedef struct {
-    uint8_t VINDPM_OS:5;    // Input Voltage Limit Offset, mV
-    bool BCOLD:1;           // Boost Mode Cold Temperature Monitor Threshold
-    Bhot BHOT:2;            // Boost Mode Hot Temperature Monitor Threshold
+    uint8_t VINDPM_OS : 5; // Input Voltage Limit Offset, mV
+    bool BCOLD : 1; // Boost Mode Cold Temperature Monitor Threshold
+    Bhot BHOT : 2; // Boost Mode Hot Temperature Monitor Threshold
 } REG01;
 
-
 typedef struct {
-    bool AUTO_DPDM_EN:1;    // Automatic Input Detection Enable
-    bool FORCE_DPDM:1;      // Force Input Detection
-    uint8_t RES:2;          // Reserved
-    bool ICO_EN:1;          // Input Current Optimizer (ICO) Enable
-    bool BOOST_FREQ:1;      // Boost Mode Frequency Selection
-    bool CONV_RATE:1;       // ADC Conversion Rate Selection
-    bool CONV_START:1;      // ADC Conversion Start Control
+    bool AUTO_DPDM_EN : 1; // Automatic Input Detection Enable
+    bool FORCE_DPDM : 1; // Force Input Detection
+    uint8_t RES : 2; // Reserved
+    bool ICO_EN : 1; // Input Current Optimizer (ICO) Enable
+    bool BOOST_FREQ : 1; // Boost Mode Frequency Selection
+    bool CONV_RATE : 1; // ADC Conversion Rate Selection
+    bool CONV_START : 1; // ADC Conversion Start Control
 } REG02;
 
-
-#define SYS_MIN_400         (1<<2)
-#define SYS_MIN_200         (1<<1)
-#define SYS_MIN_100         (1<<0)
+#define SYS_MIN_400 (1 << 2)
+#define SYS_MIN_200 (1 << 1)
+#define SYS_MIN_100 (1 << 0)
 
 typedef struct {
-    bool MIN_VBAT_SEL:1;    // Minimum Battery Voltage (falling) to exit boost mode
-    uint8_t SYS_MIN:3;      // Minimum System Voltage Limit, mV, offset: +3000mV
-    bool CHG_CONFIG:1;      // Charge Enable Configuration
-    bool OTG_CONFIG:1;      // Boost (OTG) Mode Configuration
-    bool WD_RST:1;          // I2C Watchdog Timer Reset
-    bool BAT_LOADEN:1;      // Battery Load (IBATLOAD) Enable
+    bool MIN_VBAT_SEL : 1; // Minimum Battery Voltage (falling) to exit boost mode
+    uint8_t SYS_MIN : 3; // Minimum System Voltage Limit, mV, offset: +3000mV
+    bool CHG_CONFIG : 1; // Charge Enable Configuration
+    bool OTG_CONFIG : 1; // Boost (OTG) Mode Configuration
+    bool WD_RST : 1; // I2C Watchdog Timer Reset
+    bool BAT_LOADEN : 1; // Battery Load (IBATLOAD) Enable
 } REG03;
 
-
-#define ICHG_4096           (1<<6)
-#define ICHG_2048           (1<<5)
-#define ICHG_1024           (1<<4)
-#define ICHG_512            (1<<3)
-#define ICHG_256            (1<<2)
-#define ICHG_128            (1<<1)
-#define ICHG_64             (1<<0)
+#define ICHG_4096 (1 << 6)
+#define ICHG_2048 (1 << 5)
+#define ICHG_1024 (1 << 4)
+#define ICHG_512 (1 << 3)
+#define ICHG_256 (1 << 2)
+#define ICHG_128 (1 << 1)
+#define ICHG_64 (1 << 0)
 
 typedef struct {
-    uint8_t ICHG:7;         // Fast Charge Current Limit, mA
-    bool EN_PUMPX:1;        // Current pulse control Enable
+    uint8_t ICHG : 7; // Fast Charge Current Limit, mA
+    bool EN_PUMPX : 1; // Current pulse control Enable
 } REG04;
 
-
-#define IPRETERM_512        (1<<3)
-#define IPRETERM_256        (1<<2)
-#define IPRETERM_128        (1<<1)
-#define IPRETERM_64         (1<<0)
+#define IPRETERM_512 (1 << 3)
+#define IPRETERM_256 (1 << 2)
+#define IPRETERM_128 (1 << 1)
+#define IPRETERM_64 (1 << 0)
 
 typedef struct {
-    uint8_t ITERM:4;        // Termination Current Limit, offset: +64mA
-    uint8_t IPRECHG:4;      // Precharge Current Limit, offset: +64mA
+    uint8_t ITERM : 4; // Termination Current Limit, offset: +64mA
+    uint8_t IPRECHG : 4; // Precharge Current Limit, offset: +64mA
 } REG05;
 
-
-#define VREG_512            (1<<5)
-#define VREG_256            (1<<4)
-#define VREG_128            (1<<3)
-#define VREG_64             (1<<2)
-#define VREG_32             (1<<1)
-#define VREG_16             (1<<0)
+#define VREG_512 (1 << 5)
+#define VREG_256 (1 << 4)
+#define VREG_128 (1 << 3)
+#define VREG_64 (1 << 2)
+#define VREG_32 (1 << 1)
+#define VREG_16 (1 << 0)
 
 typedef struct {
-    bool VRECHG:1;          // Battery Recharge Threshold Offset
-    bool BATLOWV:1;         // Battery Precharge to Fast Charge Threshold
-    uint8_t VREG:6;         // Charge Voltage Limit, offset: +3840mV
+    bool VRECHG : 1; // Battery Recharge Threshold Offset
+    bool BATLOWV : 1; // Battery Precharge to Fast Charge Threshold
+    uint8_t VREG : 6; // Charge Voltage Limit, offset: +3840mV
 } REG06;
 
-
 typedef enum {
     WatchdogDisable = 0b00,
     Watchdog40 = 0b01,
@@ -122,68 +116,64 @@ typedef enum {
 } ChgTimer;
 
 typedef struct {
-    bool JEITA_ISET:1;      // JEITA Low Temperature Current Setting
-    ChgTimer CHG_TIMER:2;    // Fast Charge Timer Setting
-    bool EN_TIMER:1;        // Charging Safety Timer Enable
-    Watchdog WATCHDOG:2;     // I2C Watchdog Timer Setting
-    bool STAT_DIS:1;        // STAT Pin Disable
-    bool EN_TERM:1;         // Charging Termination Enable
+    bool JEITA_ISET : 1; // JEITA Low Temperature Current Setting
+    ChgTimer CHG_TIMER : 2; // Fast Charge Timer Setting
+    bool EN_TIMER : 1; // Charging Safety Timer Enable
+    Watchdog WATCHDOG : 2; // I2C Watchdog Timer Setting
+    bool STAT_DIS : 1; // STAT Pin Disable
+    bool EN_TERM : 1; // Charging Termination Enable
 } REG07;
 
+#define BAT_COMP_80 (1 << 2)
+#define BAT_COMP_40 (1 << 1)
+#define BAT_COMP_20 (1 << 0)
 
-#define BAT_COMP_80         (1<<2)
-#define BAT_COMP_40         (1<<1)
-#define BAT_COMP_20         (1<<0)
+#define VCLAMP_128 (1 << 2)
+#define VCLAMP_64 (1 << 1)
+#define VCLAMP_32 (1 << 0)
 
-#define VCLAMP_128          (1<<2)
-#define VCLAMP_64           (1<<1)
-#define VCLAMP_32           (1<<0)
-
-#define TREG_60             (0b00)
-#define TREG_80             (0b01)
-#define TREG_100            (0b10)
-#define TREG_120            (0b11)
+#define TREG_60 (0b00)
+#define TREG_80 (0b01)
+#define TREG_100 (0b10)
+#define TREG_120 (0b11)
 
 typedef struct {
-    uint8_t TREG:2;         // Thermal Regulation Threshold
-    uint8_t VCLAMP:3;       // IR Compensation Voltage Clamp
-    uint8_t BAT_COMP:3;     // IR Compensation Resistor Setting 
+    uint8_t TREG : 2; // Thermal Regulation Threshold
+    uint8_t VCLAMP : 3; // IR Compensation Voltage Clamp
+    uint8_t BAT_COMP : 3; // IR Compensation Resistor Setting
 } REG08;
 
-
 typedef struct {
-    bool PUMPX_DN:1;        // Current pulse control voltage down enable
-    bool PUMPX_UP:1;        // Current pulse control voltage up enable
-    bool BATFET_RST_EN:1;   // BATFET full system reset enable
-    bool BATFET_DLY:1;      // BATFET turn off delay control
-    bool JEITA_VSET:1;      // JEITA High Temperature Voltage Setting
-    bool BATFET_DIS:1;      // Force BATFET off to enable ship mode
-    bool TMR2X_EN:1;        // Safety Timer Setting during DPM or Thermal Regulation
-    bool FORCE_ICO:1;       // Force Start Input Current Optimizer 
+    bool PUMPX_DN : 1; // Current pulse control voltage down enable
+    bool PUMPX_UP : 1; // Current pulse control voltage up enable
+    bool BATFET_RST_EN : 1; // BATFET full system reset enable
+    bool BATFET_DLY : 1; // BATFET turn off delay control
+    bool JEITA_VSET : 1; // JEITA High Temperature Voltage Setting
+    bool BATFET_DIS : 1; // Force BATFET off to enable ship mode
+    bool TMR2X_EN : 1; // Safety Timer Setting during DPM or Thermal Regulation
+    bool FORCE_ICO : 1; // Force Start Input Current Optimizer
 } REG09;
 
+#define BOOSTV_512 (1 << 3)
+#define BOOSTV_256 (1 << 2)
+#define BOOSTV_128 (1 << 1)
+#define BOOSTV_64 (1 << 0)
 
-#define BOOSTV_512          (1<<3)
-#define BOOSTV_256          (1<<2)
-#define BOOSTV_128          (1<<1)
-#define BOOSTV_64           (1<<0)
-
-#define BOOST_LIM_500       (0b000)
-#define BOOST_LIM_750       (0b001)
-#define BOOST_LIM_1200      (0b010)
-#define BOOST_LIM_1400      (0b011)
-#define BOOST_LIM_1650      (0b100)
-#define BOOST_LIM_1875      (0b101)
-#define BOOST_LIM_2150      (0b110)
-#define BOOST_LIM_RSVD      (0b111)
+#define BOOST_LIM_500 (0b000)
+#define BOOST_LIM_750 (0b001)
+#define BOOST_LIM_1200 (0b010)
+#define BOOST_LIM_1400 (0b011)
+#define BOOST_LIM_1650 (0b100)
+#define BOOST_LIM_1875 (0b101)
+#define BOOST_LIM_2150 (0b110)
+#define BOOST_LIM_RSVD (0b111)
 
 typedef struct {
-    uint8_t BOOST_LIM:3;    // Boost Mode Current Limit
-    bool PFM_OTG_DIS:1;     // PFM mode allowed in boost mode
-    uint8_t BOOSTV:4;       // Boost Mode Voltage Regulation, offset: +4550mV
+    uint8_t BOOST_LIM : 3; // Boost Mode Current Limit
+    bool PFM_OTG_DIS : 1; // PFM mode allowed in boost mode
+    uint8_t BOOSTV : 4; // Boost Mode Voltage Regulation, offset: +4550mV
 } REG0A;
 
-
 typedef enum {
     VBusStatNo = 0b000,
     VBusStatUSB = 0b001,
@@ -199,14 +189,13 @@ typedef enum {
 } ChrgStat;
 
 typedef struct {
-    bool VSYS_STAT:1;       // VSYS Regulation Status
-    bool RES:1;             // Reserved: Always reads 1
-    bool PG_STAT:1;         // Power Good Status
-    ChrgStat CHRG_STAT:2;    // Charging Status
-    VBusStat VBUS_STAT:3;    // VBUS Status register
+    bool VSYS_STAT : 1; // VSYS Regulation Status
+    bool RES : 1; // Reserved: Always reads 1
+    bool PG_STAT : 1; // Power Good Status
+    ChrgStat CHRG_STAT : 2; // Charging Status
+    VBusStat VBUS_STAT : 3; // VBUS Status register
 } REG0B;
 
-
 typedef enum {
     ChrgFaultNO = 0b00,
     ChrgFaultIN = 0b01,
@@ -223,69 +212,62 @@ typedef enum {
 } NtcFault;
 
 typedef struct {
-    NtcFault NTC_FAULT:3;    // NTC Fault Status
-    bool BAT_FAULT:1;       // Battery Fault Status
-    ChrgFault CHRG_FAULT:2;   // Charge Fault Status
-    bool BOOST_FAULT:1;     // Boost Mode Fault Status
-    bool WATCHDOG_FAULT:1;  // Watchdog Fault Status
+    NtcFault NTC_FAULT : 3; // NTC Fault Status
+    bool BAT_FAULT : 1; // Battery Fault Status
+    ChrgFault CHRG_FAULT : 2; // Charge Fault Status
+    bool BOOST_FAULT : 1; // Boost Mode Fault Status
+    bool WATCHDOG_FAULT : 1; // Watchdog Fault Status
 } REG0C;
 
-
-#define VINDPM_6400         (1<<6)
-#define VINDPM_3200         (1<<5)
-#define VINDPM_1600         (1<<4)
-#define VINDPM_800          (1<<3)
-#define VINDPM_400          (1<<2)
-#define VINDPM_200          (1<<1)
-#define VINDPM_100          (1<<0)
+#define VINDPM_6400 (1 << 6)
+#define VINDPM_3200 (1 << 5)
+#define VINDPM_1600 (1 << 4)
+#define VINDPM_800 (1 << 3)
+#define VINDPM_400 (1 << 2)
+#define VINDPM_200 (1 << 1)
+#define VINDPM_100 (1 << 0)
 
 typedef struct {
-    uint8_t VINDPM:7;       // Absolute VINDPM Threshold, offset: +2600mV
-    bool FORCE_VINDPM:1;    // VINDPM Threshold Setting Method
+    uint8_t VINDPM : 7; // Absolute VINDPM Threshold, offset: +2600mV
+    bool FORCE_VINDPM : 1; // VINDPM Threshold Setting Method
 } REG0D;
 
-
 typedef struct {
-    uint8_t BATV:7;         // ADC conversion of Battery Voltage (VBAT), offset: +2304mV
-    bool THERM_STAT:1;      // Thermal Regulation Status
+    uint8_t BATV : 7; // ADC conversion of Battery Voltage (VBAT), offset: +2304mV
+    bool THERM_STAT : 1; // Thermal Regulation Status
 } REG0E;
 
-
 typedef struct {
-    uint8_t SYSV:7;         // ADDC conversion of System Voltage (VSYS), offset: +2304mV
-    uint8_t RES:1;          // Reserved: Always reads 0
+    uint8_t SYSV : 7; // ADDC conversion of System Voltage (VSYS), offset: +2304mV
+    uint8_t RES : 1; // Reserved: Always reads 0
 } REG0F;
 
-
 typedef struct {
-    uint8_t TSPCT:7;        // ADC conversion of TS Voltage (TS) as percentage of REGN, offset: +21%
-    uint8_t RES:1;          // Reserved: Always reads 0
+    uint8_t TSPCT : 7; // ADC conversion of TS Voltage (TS) as percentage of REGN, offset: +21%
+    uint8_t RES : 1; // Reserved: Always reads 0
 } REG10;
 
-
 typedef struct {
-    uint8_t VBUSV:7;        // ADC conversion of VBUS voltage (VBUS), offset: +2600mV
-    bool VBUS_GD:1;         // VBUS Good Status
+    uint8_t VBUSV : 7; // ADC conversion of VBUS voltage (VBUS), offset: +2600mV
+    bool VBUS_GD : 1; // VBUS Good Status
 } REG11;
 
-
 typedef struct {
-    uint8_t ICHGR:7;        // ADC conversion of Charge Current (IBAT) when VBAT > VBATSHORT
-    uint8_t RES:1;          // Reserved: Always reads 0
+    uint8_t ICHGR : 7; // ADC conversion of Charge Current (IBAT) when VBAT > VBATSHORT
+    uint8_t RES : 1; // Reserved: Always reads 0
 } REG12;
 
-
 typedef struct {
-    uint8_t IDPM_LIM:6;     // Input Current Limit in effect while Input Current Optimizer (ICO) is enabled, offset: 100mA (default)
-    bool IDPM_STAT:1;       // IINDPM Status
-    bool VDPM_STAT:1;       // VINDPM Status
+    uint8_t
+        IDPM_LIM : 6; // Input Current Limit in effect while Input Current Optimizer (ICO) is enabled, offset: 100mA (default)
+    bool IDPM_STAT : 1; // IINDPM Status
+    bool VDPM_STAT : 1; // VINDPM Status
 } REG13;
 
-
 typedef struct {
-    uint8_t DEV_REV:2;      // Device Revision
-    bool TS_PROFILE:1;      // Temperature Profile
-    uint8_t PN:3;           // Device Configuration
-    bool ICO_OPTIMIZED:1;   // Input Current Optimizer (ICO) Status
-    bool REG_RST:1;         // Register Reset
+    uint8_t DEV_REV : 2; // Device Revision
+    bool TS_PROFILE : 1; // Temperature Profile
+    uint8_t PN : 3; // Device Configuration
+    bool ICO_OPTIMIZED : 1; // Input Current Optimizer (ICO) Status
+    bool REG_RST : 1; // Register Reset
 } REG14;
diff --git a/lib/drivers/bq27220.c b/lib/drivers/bq27220.c
index 51d90a63..487560bf 100644
--- a/lib/drivers/bq27220.c
+++ b/lib/drivers/bq27220.c
@@ -5,28 +5,30 @@
 #include <stdbool.h>
 
 uint16_t bq27220_read_word(uint8_t address) {
-    uint8_t buffer[2] = { address };
+    uint8_t buffer[2] = {address};
     uint16_t ret;
-    with_api_hal_i2c(uint16_t, &ret, (){
-        api_hal_i2c_trx(
-            POWER_I2C, BQ27220_ADDRESS, 
-            buffer, 1, buffer, 2
-        );
-        return *(uint16_t*)buffer;
-    });
+    with_api_hal_i2c(
+        uint16_t, &ret, () {
+            if(api_hal_i2c_trx(
+                   POWER_I2C, BQ27220_ADDRESS, buffer, 1, buffer, 2, BQ27220_I2C_TIMEOUT)) {
+                return *(uint16_t*)buffer;
+            } else {
+                return 0;
+            }
+        });
     return ret;
 }
 
 bool bq27220_control(uint16_t control) {
     bool ret;
-    with_api_hal_i2c(bool, &ret, (){
-        uint8_t buffer[3];
-        buffer[0] = CommandControl;
-        buffer[1] = (control>>8) & 0xFF;
-        buffer[2] = control & 0xFF;
-        api_hal_i2c_tx(POWER_I2C, BQ27220_ADDRESS, buffer, 3);
-        return true;
-    });
+    with_api_hal_i2c(
+        bool, &ret, () {
+            uint8_t buffer[3];
+            buffer[0] = CommandControl;
+            buffer[1] = (control >> 8) & 0xFF;
+            buffer[2] = control & 0xFF;
+            return api_hal_i2c_tx(POWER_I2C, BQ27220_ADDRESS, buffer, 3, BQ27220_I2C_TIMEOUT);
+        });
     return ret;
 }
 
@@ -46,7 +48,7 @@ int16_t bq27220_get_current() {
 
 uint8_t bq27220_get_battery_status(BatteryStatus* battery_status) {
     uint16_t data = bq27220_read_word(CommandBatteryStatus);
-    if (data == BQ27220_ERROR) {
+    if(data == BQ27220_ERROR) {
         return BQ27220_ERROR;
     } else {
         *(uint16_t*)battery_status = data;
@@ -56,7 +58,7 @@ uint8_t bq27220_get_battery_status(BatteryStatus* battery_status) {
 
 uint8_t bq27220_get_operation_status(OperationStatus* operation_status) {
     uint16_t data = bq27220_read_word(CommandOperationStatus);
-    if (data == BQ27220_ERROR) {
+    if(data == BQ27220_ERROR) {
         return BQ27220_ERROR;
     } else {
         *(uint16_t*)operation_status = data;
diff --git a/lib/drivers/bq27220.h b/lib/drivers/bq27220.h
index bf0f8de1..d55c2bb7 100644
--- a/lib/drivers/bq27220.h
+++ b/lib/drivers/bq27220.h
@@ -8,39 +8,39 @@
 
 typedef struct {
     // Low byte, Low bit first
-    bool DSG:1;         // The device is in DISCHARGE
-    bool SYSDWN:1;      // System down bit indicating the system should shut down
-    bool TDA:1;         // Terminate Discharge Alarm
-    bool BATTPRES:1;    // Battery Present detected
-    bool AUTH_GD:1;     // Detect inserted battery
-    bool OCVGD:1;       // Good OCV measurement taken
-    bool TCA:1;         // Terminate Charge Alarm
-    bool RSVD:1;        // Reserved
+    bool DSG : 1; // The device is in DISCHARGE
+    bool SYSDWN : 1; // System down bit indicating the system should shut down
+    bool TDA : 1; // Terminate Discharge Alarm
+    bool BATTPRES : 1; // Battery Present detected
+    bool AUTH_GD : 1; // Detect inserted battery
+    bool OCVGD : 1; // Good OCV measurement taken
+    bool TCA : 1; // Terminate Charge Alarm
+    bool RSVD : 1; // Reserved
     // High byte, Low bit first
-    bool CHGINH:1;      // Charge inhibit
-    bool FC:1;          // Full-charged is detected
-    bool OTD:1;         // Overtemperature in discharge condition is detected
-    bool OTC:1;         // Overtemperature in charge condition is detected
-    bool SLEEP:1;       // Device is operating in SLEEP mode when set
-    bool OCVFAIL:1;     // Status bit indicating that the OCV reading failed due to current
-    bool OCVCOMP:1;     // An OCV measurement update is complete
-    bool FD:1;          // Full-discharge is detected
+    bool CHGINH : 1; // Charge inhibit
+    bool FC : 1; // Full-charged is detected
+    bool OTD : 1; // Overtemperature in discharge condition is detected
+    bool OTC : 1; // Overtemperature in charge condition is detected
+    bool SLEEP : 1; // Device is operating in SLEEP mode when set
+    bool OCVFAIL : 1; // Status bit indicating that the OCV reading failed due to current
+    bool OCVCOMP : 1; // An OCV measurement update is complete
+    bool FD : 1; // Full-discharge is detected
 } BatteryStatus;
 
 typedef struct {
     // Low byte, Low bit first
-    bool CALMD:1;
-    bool SEC0:1;
-    bool SEC1:1;
-    bool EDV2:1;
-    bool VDQ:1;
-    bool INITCOMP:1;
-    bool SMTH:1;
-    bool BTPINT:1;
+    bool CALMD : 1;
+    bool SEC0 : 1;
+    bool SEC1 : 1;
+    bool EDV2 : 1;
+    bool VDQ : 1;
+    bool INITCOMP : 1;
+    bool SMTH : 1;
+    bool BTPINT : 1;
     // High byte, Low bit first
-    uint8_t RSVD1:2;
-    bool CFGUPDATE:1;
-    uint8_t RSVD0:5;
+    uint8_t RSVD1 : 2;
+    bool CFGUPDATE : 1;
+    uint8_t RSVD0 : 5;
 } OperationStatus;
 
 /* Initialize Driver */
diff --git a/lib/drivers/bq27220_reg.h b/lib/drivers/bq27220_reg.h
index daa1c184..0ed35301 100644
--- a/lib/drivers/bq27220_reg.h
+++ b/lib/drivers/bq27220_reg.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #define BQ27220_ADDRESS 0xAA
+#define BQ27220_I2C_TIMEOUT 50
 
 #define CommandControl 0x00
 #define CommandAtRate 0x02
diff --git a/lib/drivers/lp5562.c b/lib/drivers/lp5562.c
index 2bd9e6a3..1f3f8e8b 100644
--- a/lib/drivers/lp5562.c
+++ b/lib/drivers/lp5562.c
@@ -4,25 +4,25 @@
 #include <api-hal-i2c.h>
 #include <stdio.h>
 
-bool lp5562_write_reg(uint8_t address, uint8_t *data) {
-    uint8_t buffer[2] = { address, *data };
+bool lp5562_write_reg(uint8_t address, uint8_t* data) {
+    uint8_t buffer[2] = {address, *data};
     bool ret;
-    with_api_hal_i2c(bool, &ret, (){
-        api_hal_i2c_tx(POWER_I2C, LP5562_ADDRESS, buffer, 2);
-        return true;
-    });
+    with_api_hal_i2c(
+        bool, &ret, () {
+            return api_hal_i2c_tx(POWER_I2C, LP5562_ADDRESS, buffer, 2, LP5562_I2C_TIMEOUT);
+        });
     return ret;
 }
 
 void lp5562_reset() {
-    Reg0D_Reset reg = {.value = 0xFF };
+    Reg0D_Reset reg = {.value = 0xFF};
     lp5562_write_reg(0x0D, (uint8_t*)&reg);
 }
 
 void lp5562_configure() {
-    Reg08_Config config = { .INT_CLK_EN = true, .PS_EN = true, .PWM_HF = true };
+    Reg08_Config config = {.INT_CLK_EN = true, .PS_EN = true, .PWM_HF = true};
     lp5562_write_reg(0x08, (uint8_t*)&config);
-    
+
     Reg70_LedMap map = {
         .red = EngSelectI2C,
         .green = EngSelectI2C,
@@ -33,19 +33,19 @@ void lp5562_configure() {
 }
 
 void lp5562_enable() {
-    Reg00_Enable reg = { .CHIP_EN = true, .LOG_EN = true };
+    Reg00_Enable reg = {.CHIP_EN = true, .LOG_EN = true};
     lp5562_write_reg(0x00, (uint8_t*)&reg);
 }
 
 void lp5562_set_channel_current(LP5562Channel channel, uint8_t value) {
     uint8_t reg_no;
-    if (channel == LP5562ChannelRed) {
+    if(channel == LP5562ChannelRed) {
         reg_no = 0x07;
-    } else if (channel == LP5562ChannelGreen) {
+    } else if(channel == LP5562ChannelGreen) {
         reg_no = 0x06;
-    } else if (channel == LP5562ChannelBlue) {
+    } else if(channel == LP5562ChannelBlue) {
         reg_no = 0x05;
-    } else if (channel == LP5562ChannelWhite) {
+    } else if(channel == LP5562ChannelWhite) {
         reg_no = 0x0F;
     } else {
         return;
@@ -55,13 +55,13 @@ void lp5562_set_channel_current(LP5562Channel channel, uint8_t value) {
 
 void lp5562_set_channel_value(LP5562Channel channel, uint8_t value) {
     uint8_t reg_no;
-    if (channel == LP5562ChannelRed) {
+    if(channel == LP5562ChannelRed) {
         reg_no = 0x04;
-    } else if (channel == LP5562ChannelGreen) {
+    } else if(channel == LP5562ChannelGreen) {
         reg_no = 0x03;
-    } else if (channel == LP5562ChannelBlue) {
+    } else if(channel == LP5562ChannelBlue) {
         reg_no = 0x02;
-    } else if (channel == LP5562ChannelWhite) {
+    } else if(channel == LP5562ChannelWhite) {
         reg_no = 0x0E;
     } else {
         return;
diff --git a/lib/drivers/lp5562.h b/lib/drivers/lp5562.h
index d2c02d9e..7096f738 100644
--- a/lib/drivers/lp5562.h
+++ b/lib/drivers/lp5562.h
@@ -4,10 +4,10 @@
 #include <stdbool.h>
 
 typedef enum {
-	LP5562ChannelRed,
-	LP5562ChannelGreen,
-	LP5562ChannelBlue,
-	LP5562ChannelWhite,
+    LP5562ChannelRed,
+    LP5562ChannelGreen,
+    LP5562ChannelBlue,
+    LP5562ChannelWhite,
 } LP5562Channel;
 
 /* Initialize Driver */
diff --git a/lib/drivers/lp5562_reg.h b/lib/drivers/lp5562_reg.h
index a5ad8b76..2f2c1a52 100644
--- a/lib/drivers/lp5562_reg.h
+++ b/lib/drivers/lp5562_reg.h
@@ -5,66 +5,67 @@
 #endif
 
 #define LP5562_ADDRESS 0x60
+#define LP5562_I2C_TIMEOUT 50
 
 typedef enum {
-    EngExecHold=0b00,
-    EngExecStep=0b01,
-    EngExecRun=0b10,
-    EngExecPC=0b11,
+    EngExecHold = 0b00,
+    EngExecStep = 0b01,
+    EngExecRun = 0b10,
+    EngExecPC = 0b11,
 } EngExec;
 
 typedef struct {
-    EngExec ENG3_EXEC:2;
-    EngExec ENG2_EXEC:2;
-    EngExec ENG1_EXEC:2;
-    bool CHIP_EN:1;
-    bool LOG_EN:1;
+    EngExec ENG3_EXEC : 2;
+    EngExec ENG2_EXEC : 2;
+    EngExec ENG1_EXEC : 2;
+    bool CHIP_EN : 1;
+    bool LOG_EN : 1;
 } Reg00_Enable;
 
 typedef enum {
-    EngModeDisable=0b00,
-    EngModeLoad=0b01,
-    EngModeRun=0b10,
-    EngModeDirect=0b11,
+    EngModeDisable = 0b00,
+    EngModeLoad = 0b01,
+    EngModeRun = 0b10,
+    EngModeDirect = 0b11,
 } EngMode;
 
 typedef struct {
-    EngMode ENG3_MODE:2;
-    EngMode ENG2_MODE:2;
-    EngMode ENG1_MODE:2;
-    uint8_t reserved:2;
+    EngMode ENG3_MODE : 2;
+    EngMode ENG2_MODE : 2;
+    EngMode ENG1_MODE : 2;
+    uint8_t reserved : 2;
 } Reg01_OpMode;
 
 typedef struct {
-    bool INT_CLK_EN:1;
-    bool CLK_DET_EN:1;
-    uint8_t reserved0:3;
-    bool PS_EN:1;
-    bool PWM_HF:1;
-    uint8_t reserved1:1;
+    bool INT_CLK_EN : 1;
+    bool CLK_DET_EN : 1;
+    uint8_t reserved0 : 3;
+    bool PS_EN : 1;
+    bool PWM_HF : 1;
+    uint8_t reserved1 : 1;
 } Reg08_Config;
 
 typedef struct {
-    uint8_t pc:3;
-    uint8_t reserved:5;
+    uint8_t pc : 3;
+    uint8_t reserved : 5;
 } Reg09_Engine1PC;
 
 typedef struct {
-    uint8_t pc:3;
-    uint8_t reserved:5;
+    uint8_t pc : 3;
+    uint8_t reserved : 5;
 } Reg0A_Engine2PC;
 
 typedef struct {
-    uint8_t pc:3;
-    uint8_t reserved:5;
+    uint8_t pc : 3;
+    uint8_t reserved : 5;
 } Reg0B_Engine3PC;
 
 typedef struct {
-    bool ENG3_INT:1;
-    bool ENG2_INT:1;
-    bool ENG1_INT:1;
-    bool EXT_CLK_USED:1;
-    uint8_t reserved:5;
+    bool ENG3_INT : 1;
+    bool ENG2_INT : 1;
+    bool ENG1_INT : 1;
+    bool EXT_CLK_USED : 1;
+    uint8_t reserved : 5;
 } Reg0C_Status;
 
 typedef struct {
@@ -72,15 +73,15 @@ typedef struct {
 } Reg0D_Reset;
 
 typedef enum {
-    EngSelectI2C=0b00,
-    EngSelectEngine1=0b01,
-    EngSelectEngine2=0b10,
-    EngSelectEngine3=0b11,
+    EngSelectI2C = 0b00,
+    EngSelectEngine1 = 0b01,
+    EngSelectEngine2 = 0b10,
+    EngSelectEngine3 = 0b11,
 } EngSelect;
 
 typedef struct {
-    EngSelect blue:2;
-    EngSelect green:2;
-    EngSelect red:2;
-    EngSelect white:2;
+    EngSelect blue : 2;
+    EngSelect green : 2;
+    EngSelect red : 2;
+    EngSelect white : 2;
 } Reg70_LedMap;