diff --git a/source/firm.c b/source/firm.c
index 967f848..4e369d8 100755
--- a/source/firm.c
+++ b/source/firm.c
@@ -439,8 +439,6 @@ bool checkFirmPayload(u32 payloadSize)
 
     for(u32 i = 0; i < 4; i++)
     {
-        __attribute__((aligned(4))) u8 hash[0x20];
-
         FirmSection *section = &firm->section[i];
 
         //Allow empty sections
@@ -457,6 +455,8 @@ bool checkFirmPayload(u32 payloadSize)
             (!inRange((u32)section->address, (u32)section->address + section->size, 0x20000000, 0x20000000 + 0x8000000))))
             return false;
 
+        __attribute__((aligned(4))) u8 hash[0x20];
+
         sha(hash, (u8 *)firm + section->offset, section->size, SHA_256_MODE);
 
         if(memcmp(hash, section->hash, 0x20) != 0)
diff --git a/source/i2c.c b/source/i2c.c
index 8b7c250..d3dfac7 100644
--- a/source/i2c.c
+++ b/source/i2c.c
@@ -24,6 +24,7 @@
 *   Thanks to the everyone who contributed in the development of this file
 */
 
+#include "utils.h"
 #include "i2c.h"
 
 //-----------------------------------------------------------------------------
@@ -118,9 +119,10 @@ static bool i2cSelectRegister(u8 bus_id, u8 reg)
 u8 i2cReadRegister(u8 dev_id, u8 reg)
 {
     u8 bus_id = i2cGetDeviceBusId(dev_id),
-       dev_addr = i2cGetDeviceRegAddr(dev_id);
+       dev_addr = i2cGetDeviceRegAddr(dev_id),
+       ret = 0xFF;
 
-    for(u32 i = 0; i < 8; i++)
+    for(u32 i = 0; i < 8 && ret == 0xFF; i++)
     {
         if(i2cSelectDevice(bus_id, dev_addr) && i2cSelectRegister(bus_id, reg))
         {
@@ -130,14 +132,16 @@ u8 i2cReadRegister(u8 dev_id, u8 reg)
                 i2cStop(bus_id, 1);
                 i2cWaitBusy(bus_id);
 
-                return *i2cGetDataReg(bus_id);
+                ret = *i2cGetDataReg(bus_id);
             }
         }
         *i2cGetCntReg(bus_id) = 0xC5;
         i2cWaitBusy(bus_id);
     }
 
-    return 0xFF;
+    wait(3ULL);
+
+    return ret;
 }
 
 bool i2cWriteRegister(u8 dev_id, u8 reg, u8 data)
@@ -145,7 +149,9 @@ bool i2cWriteRegister(u8 dev_id, u8 reg, u8 data)
     u8 bus_id = i2cGetDeviceBusId(dev_id),
        dev_addr = i2cGetDeviceRegAddr(dev_id);
 
-    for(u32 i = 0; i < 8; i++)
+    bool ret = false;
+
+    for(u32 i = 0; i < 8 && !ret; i++)
     {
         if(i2cSelectDevice(bus_id, dev_addr) && i2cSelectRegister(bus_id, reg))
         {
@@ -154,11 +160,13 @@ bool i2cWriteRegister(u8 dev_id, u8 reg, u8 data)
             *i2cGetCntReg(bus_id) = 0xC1;
             i2cStop(bus_id, 0);
 
-            if(i2cGetResult(bus_id)) return true;
+            if(i2cGetResult(bus_id)) ret = true;
         }
         *i2cGetCntReg(bus_id) = 0xC5;
         i2cWaitBusy(bus_id);
     }
 
-    return false;
+    wait(3ULL);
+
+    return ret;
 }
\ No newline at end of file
diff --git a/source/screen.c b/source/screen.c
index 94758d2..7afae09 100644
--- a/source/screen.c
+++ b/source/screen.c
@@ -115,7 +115,6 @@ void initScreens(void)
 
             //Turn on backlight
             i2cWriteRegister(I2C_DEV_MCU, 0x22, 0x2A);
-            wait(3ULL);
         }
         else updateBrightness(MULTICONFIG(BRIGHTNESS));