diff --git a/software/soapy/src/AXIIIC.cpp b/software/soapy/src/AXIIIC.cpp
index 6a8b7594..e5b7fa4b 100644
--- a/software/soapy/src/AXIIIC.cpp
+++ b/software/soapy/src/AXIIIC.cpp
@@ -122,17 +122,29 @@ void AXIIIC::transmit(uint8_t address, uint8_t *buf, uint8_t count, AXIIIC::Stop
     if((this->readReg(AXI_IIC_REG_SR) & AXI_IIC_REG_SR_BB) && !(this->readReg(AXI_IIC_REG_ISR) & AXI_IIC_REG_IxR_INT2_TX_EMPTY)) // Bus busy and not owner
         throw std::runtime_error("AXI IIC: Bus busy");
 
+    uint32_t cr = this->readReg(AXI_IIC_REG_CR);
+
+    if(!(cr & AXI_IIC_REG_CR_EN))
+        throw std::runtime_error("AXI IIC: Peripheral not enabled");
+
     if(!(this->readReg(AXI_IIC_REG_SR) & AXI_IIC_REG_SR_TX_FIFO_EMPTY)) // Clear TX FIFO
     {
-        uint32_t reg = this->readReg(AXI_IIC_REG_CR);
-
-        this->writeReg(AXI_IIC_REG_CR, reg | AXI_IIC_REG_CR_TXFIFO_RESET);
-        this->writeReg(AXI_IIC_REG_CR, reg & ~AXI_IIC_REG_CR_TXFIFO_RESET);
+        this->writeReg(AXI_IIC_REG_CR, cr | AXI_IIC_REG_CR_TXFIFO_RESET);
+        this->writeReg(AXI_IIC_REG_CR, cr & ~AXI_IIC_REG_CR_TXFIFO_RESET);
     }
 
-    while(!(this->readReg(AXI_IIC_REG_SR) & AXI_IIC_REG_SR_RX_FIFO_EMPTY)) // Clear RX FIFO
+    uint32_t timeout = 50;
+
+    while(--timeout && !(this->readReg(AXI_IIC_REG_SR) & AXI_IIC_REG_SR_RX_FIFO_EMPTY)) // Clear RX FIFO
         (void)this->readReg(AXI_IIC_REG_RX_FIFO);
 
+    if(!timeout)
+    {
+        this->reinit();
+
+        throw std::runtime_error("AXI IIC: Timeout while clearing RX FIFO");
+    }
+
     this->writeReg(AXI_IIC_REG_ISR, this->readReg(AXI_IIC_REG_ISR)); // Clear IRQs
     this->writeReg(AXI_IIC_REG_TX_FIFO, ((stop == AXIIIC::Stop::STOP && !count) ? AXI_IIC_REG_TX_FIFO_STOP : 0) | AXI_IIC_REG_TX_FIFO_START | address);
 
@@ -142,17 +154,23 @@ void AXIIIC::transmit(uint8_t address, uint8_t *buf, uint8_t count, AXIIIC::Stop
     {
         this->writeReg(AXI_IIC_REG_TX_FIFO, (stop == AXIIIC::Stop::STOP ? AXI_IIC_REG_TX_FIFO_STOP : 0) | count);
     }
-    else
+    else // Write
     {
-        uint32_t timeout = 50;
+        timeout = 50;
 
         flags = this->readReg(AXI_IIC_REG_ISR);
 
-        while(--timeout && (flags & AXI_IIC_REG_IxR_INT4_BUS_NOT_BUSY))
+        while(--timeout)
         {
             this->writeReg(AXI_IIC_REG_ISR, flags & AXI_IIC_REG_IxR_INT4_BUS_NOT_BUSY);
             flags = this->readReg(AXI_IIC_REG_ISR);
 
+            if(!(flags & AXI_IIC_REG_IxR_INT4_BUS_NOT_BUSY))
+                break; // Bus busy, START + ADDR sent, good to go!
+
+            if(flags & (AXI_IIC_REG_IxR_INT1_TERR_STC | AXI_IIC_REG_IxR_INT0_ARB_LOST))
+                break; // Some kind of error occured, will be handled later, but the START + ADDR was sent, good to go!
+
             std::this_thread::sleep_for(std::chrono::microseconds(1));
         }
 
@@ -170,7 +188,7 @@ void AXIIIC::transmit(uint8_t address, uint8_t *buf, uint8_t count, AXIIIC::Stop
         {
             if(address & 1) // Read
             {
-                uint32_t timeout = 500;
+                timeout = 500;
 
                 while(--timeout)
                 {
@@ -202,7 +220,7 @@ void AXIIIC::transmit(uint8_t address, uint8_t *buf, uint8_t count, AXIIIC::Stop
             }
             else // Write
             {
-                uint32_t timeout = 500;
+                timeout = 500;
 
                 while(--timeout)
                 {
@@ -239,7 +257,7 @@ void AXIIIC::transmit(uint8_t address, uint8_t *buf, uint8_t count, AXIIIC::Stop
             this->writeReg(AXI_IIC_REG_ISR, this->readReg(AXI_IIC_REG_ISR) & AXI_IIC_REG_IxR_INT4_BUS_NOT_BUSY); // Clear NOT_BUSY IRQ, so that if we encounter it set again it is really not busy
     }
 
-    uint32_t timeout = 500;
+    timeout = 500;
 
     while(--timeout)
     {
@@ -262,15 +280,13 @@ void AXIIIC::transmit(uint8_t address, uint8_t *buf, uint8_t count, AXIIIC::Stop
         if(stop == AXIIIC::Stop::STOP && (flags & AXI_IIC_REG_IxR_INT4_BUS_NOT_BUSY))
             break; // Stop was requested and bus is free, we are done!
 
-        if(stop == AXIIIC::Stop::RESTART && !(address & 1) && (flags & AXI_IIC_REG_IxR_INT2_TX_EMPTY))
+        if(stop != AXIIIC::Stop::STOP && !(address & 1) && (flags & AXI_IIC_REG_IxR_INT2_TX_EMPTY))
             break; // No stop was requested and bus is throttled by the master, waiting for another transaction, we are done!
 
-        if(stop == AXIIIC::Stop::RESTART && (address & 1))
+        if(stop != AXIIIC::Stop::STOP && (address & 1))
             break; // No stop was requested and we already read all the bytes we wanted, no need to wait for the bus to be free, we are done!
 
         std::this_thread::sleep_for(std::chrono::microseconds(1));
-
-        flags = this->readReg(AXI_IIC_REG_ISR);
     }
 
     if(!timeout)
diff --git a/software/soapy/src/SPIFlash.cpp b/software/soapy/src/SPIFlash.cpp
index 6377802b..bd626c82 100644
--- a/software/soapy/src/SPIFlash.cpp
+++ b/software/soapy/src/SPIFlash.cpp
@@ -12,11 +12,25 @@ SPIFlash::SPIFlash(SPIFlash::SPIConfig spi)
 
     this->resetContinuousRead();
 
-    this->writeEnable();
-    this->writeStatus2(BIT(1)); // Set the QE bit
-    this->writeEnable();
-    this->writeStatus3(0x00); // Set maximum drive strength
-    this->writeDisable(); // Needed in case device does not support status 3 register, otherwise the WE bit may be left set
+    // Set the QE bit
+    uint8_t sr2 = this->readStatus2();
+
+    if(!(sr2 & BIT(1)))
+    {
+        this->writeEnable();
+        this->writeStatus2(sr2 | BIT(1));
+        this->writeDisable(); // Needed in case device does not support status 2 register, otherwise the WE bit may be left set
+    }
+
+    // Set drive strength (0 = 100%, 1 = 75%, 2 = 50%, 3 = 25%)
+    uint8_t sr3 = this->readStatus3();
+
+    if(sr3 != 0xFF && (sr3 & 0x60) != 0x00)
+    {
+        this->writeEnable();
+        this->writeStatus3(0x00);
+        this->writeDisable(); // Needed in case device does not support status 3 register, otherwise the WE bit may be left set
+    }
 
     this->detectDevice();
 }