From 83325ebf78d9f0e654cb5bf029126d1bfbea26ad Mon Sep 17 00:00:00 2001
From: pmantoine <pma-github@milleng.com.au>
Date: Thu, 23 Mar 2023 08:44:25 +1100
Subject: [PATCH 1/5] Initial I2C native driver

---
 I2CManager_STM32.h | 322 ++++++++++++++++++++++++++++++++++-----------
 1 file changed, 242 insertions(+), 80 deletions(-)

diff --git a/I2CManager_STM32.h b/I2CManager_STM32.h
index a55fd2e..58a623d 100644
--- a/I2CManager_STM32.h
+++ b/I2CManager_STM32.h
@@ -38,7 +38,10 @@
  *  bus on the SAMD architecture 
  ***************************************************************************/
 #if defined(I2C_USE_INTERRUPTS) && defined(ARDUINO_ARCH_STM32)
-void I2C1_IRQHandler() {
+extern "C" void I2C1_EV_IRQHandler(void) {
+  I2CManager.handleInterrupt();
+}
+extern "C" void I2C1_ER_IRQHandler(void) {
   I2CManager.handleInterrupt();
 }
 #endif
@@ -91,44 +94,60 @@ void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
   // Use 10x the rise time spec to enable integer divide of 62.5ns clock period
   uint16_t t_rise;
   uint32_t ccr_freq;
-  if (i2cClockSpeed < 200000L) {
-    // i2cClockSpeed = 100000L;
-    t_rise = 0x11;  // (1000ns /62.5ns) + 1;
-  }
-  else if (i2cClockSpeed < 800000L)
+
+  while (s->CR1 & I2C_CR1_STOP);  // Prevents lockup by guarding further
+                                  // writes to CR1 while STOP is being executed!
+  // Disable the I2C device, as TRISE can only be programmed whilst disabled
+  s->CR1 &= ~(I2C_CR1_PE);  // Disable I2C
+  // Software reset the I2C peripheral
+  // s->CR1 |= I2C_CR1_SWRST;  // reset the I2C
+  // delay(1);
+  // Release reset
+  // s->CR1 &= ~(I2C_CR1_SWRST);  // Normal operation
+
+  if (i2cClockSpeed > 100000L)
   {
-    i2cClockSpeed = 400000L;
-    t_rise = 0x06;  // (300ns / 62.5ns) + 1;
-    // } else if (i2cClockSpeed < 1200000L) {
-    //   i2cClockSpeed = 1000000L;
-    //   t_rise = 120;
+    if (i2cClockSpeed > 400000L)
+      i2cClockSpeed = 400000L;
+
+    t_rise = 0x06;  // (300ns /62.5ns) + 1;
   }
   else
   {
     i2cClockSpeed = 100000L;
     t_rise = 0x11;  // (1000ns /62.5ns) + 1;
   }
+  // Configure the rise time register
+  s->TRISE = t_rise;
 
-  // Enable the I2C master mode
-  s->CR1 &= ~(I2C_CR1_PE);  // Enable I2C
-  // Software reset the I2C peripheral
-  // s->CR1 |= I2C_CR1_SWRST;  // reset the I2C
-  // Release reset
-  // s->CR1 &= ~(I2C_CR1_SWRST);  // Normal operation
-
-  // Calculate baudrate - using a rise time appropriate for the speed
+  // DIAG(F("Setting I2C clock to: %d"), i2cClockSpeed);
+  // Calculate baudrate
   ccr_freq = I2C_BUSFREQ * 1000000 / i2cClockSpeed / 2;
 
   // Bit 15: I2C Master mode, 0=standard, 1=Fast Mode
   // Bit 14: Duty, fast mode duty cycle
   // Bit 11-0: FREQR = 16MHz => TPCLK1 = 62.5ns, so CCR divisor must be 0x50 (80 * 62.5ns = 5000ns)
-  s->CCR = (uint16_t)ccr_freq;
-
-  // Configure the rise time register
-  s->TRISE = t_rise;  // 1000 ns / 62.5 ns = 16 + 1
+  if (i2cClockSpeed > 100000L)
+    s->CCR = (uint16_t)ccr_freq | 0x8000; // We need Fast Mode set
+  else
+    s->CCR = (uint16_t)ccr_freq;
 
   // Enable the I2C master mode
   s->CR1 |= I2C_CR1_PE;  // Enable I2C
+  // Wait for bus to be clear?
+  unsigned long startTime = micros();
+  bool timeout = false;
+  while (s->SR2 & I2C_SR2_BUSY) {
+    if (micros() - startTime >= 500UL) {
+      timeout = true;
+      break;
+    }
+  }
+  if (timeout) {
+    digitalWrite(D13, HIGH);
+    DIAG(F("I2C: SR2->BUSY timeout"));
+    // delay(1000);
+  }
 }
 
 /***************************************************************************
@@ -136,32 +155,46 @@ void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
  ***************************************************************************/
 void I2CManagerClass::I2C_init()
 {
-  //Setting up the clocks
-  RCC->APB1ENR |= (1<<21);  // Enable I2C CLOCK
-  RCC->AHB1ENR |= (1<<1);   // Enable GPIOB CLOCK for PB8/PB9
+  // Setting up the clocks
+  RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;//(1 << 21); // Enable I2C CLOCK
+  // Reset the I2C1 peripheral to initial state
+  RCC->APB1RSTR |=  RCC_APB1RSTR_I2C1RST;
+	RCC->APB1RSTR &= ~RCC_APB1RSTR_I2C1RST;
   // Standard I2C pins are SCL on PB8 and SDA on PB9
+  RCC->AHB1ENR |= (1<<1);   // Enable GPIOB CLOCK for PB8/PB9
   // Bits (17:16)= 1:0 --> Alternate Function for Pin PB8;
   // Bits (19:18)= 1:0 --> Alternate Function for Pin PB9
+  GPIOB->MODER &= ~((3<<(8*2)) | (3<<(9*2)));    // Clear all MODER bits for PB8 and PB9
   GPIOB->MODER |= (2<<(8*2)) | (2<<(9*2));    // PB8 and PB9 set to ALT function
   GPIOB->OTYPER |= (1<<8) | (1<<9);           // PB8 and PB9 set to open drain output capability
   GPIOB->OSPEEDR |= (3<<(8*2)) | (3<<(9*2));  // PB8 and PB9 set to High Speed mode
+  GPIOB->PUPDR &= ~((3<<(8*2)) | (3<<(9*2))); // Clear all PUPDR bits for PB8 and PB9
   GPIOB->PUPDR |= (1<<(8*2)) | (1<<(9*2));    // PB8 and PB9 set to pull-up capability
   // Alt Function High register routing pins PB8 and PB9 for I2C1:
   // Bits (3:2:1:0) = 0:1:0:0 --> AF4 for pin PB8
   // Bits (7:6:5:4) = 0:1:0:0 --> AF4 for pin PB9
+  GPIOB->AFR[1] &= ~((15<<0) | (15<<4));      // Clear all AFR bits for PB8 on low nibble, PB9 on next nibble up
   GPIOB->AFR[1] |= (4<<0) | (4<<4);           // PB8 on low nibble, PB9 on next nibble up
 
-  // Software reset the I2C peripheral
+  // // Software reset the I2C peripheral
   s->CR1 |= I2C_CR1_SWRST;  // reset the I2C
-  s->CR1 &= ~(I2C_CR1_SWRST);  // Normal operation
+  asm("nop");    // wait a bit... suggestion from online!
+  s->CR1 &= ~(I2C_CR1_SWRST); // Normal operation
 
+  // Clear all bits in I2C CR2 register except reserved bits
+  s->CR2 &= 0xE000;
   // Program the peripheral input clock in CR2 Register in order to generate correct timings
   s->CR2 |= I2C_BUSFREQ;  // PCLK1 FREQUENCY in MHz
 
+  // set own address to 00 - not really used in master mode
+  I2C1->OAR1 |= (1 << 14); // bit 14 should be kept at 1 according to the datasheet
+
 #if defined(I2C_USE_INTERRUPTS)
   // Setting NVIC
-  NVIC_SetPriority(I2C_IRQn, 1);  // Match default priorities
-  NVIC_EnableIRQ(I2C_IRQn);
+  NVIC_SetPriority(I2C1_EV_IRQn, 1);  // Match default priorities
+  NVIC_EnableIRQ(I2C1_EV_IRQn);
+  NVIC_SetPriority(I2C1_ER_IRQn, 1);  // Match default priorities
+  NVIC_EnableIRQ(I2C1_ER_IRQn);
 
   // CR2 Interrupt Settings
   // Bit 15-13: reserved
@@ -172,8 +205,8 @@ void I2CManagerClass::I2C_init()
   // Bit 8: ITERREN - Error interrupt enable
   // Bit 7-6: reserved
   // Bit 5-0: FREQ - Peripheral clock frequency (max 50MHz)
-  // s->CR2 |= 0x0700;   // Enable Buffer, Event and Error interrupts
-  s->CR2 |= 0x0300;   // Enable Event and Error interrupts
+  s->CR2 |= 0x0700;   // Enable Buffer, Event and Error interrupts
+  // s->CR2 |= 0x0300;   // Enable Event and Error interrupts
 #endif
 
   // Calculate baudrate and set default rate for now
@@ -181,14 +214,26 @@ void I2CManagerClass::I2C_init()
   // Bit 15: I2C Master mode, 0=standard, 1=Fast Mode
   // Bit 14: Duty, fast mode duty cycle
   // Bit 11-0: FREQR = 16MHz => TPCLK1 = 62.5ns, so CCR divisor must be 0x50 (80 * 62.5ns = 5000ns)
-  s->CCR = 0x0050;
+  s->CCR = 0x50;
 
   // Configure the rise time register - max allowed in 1000ns
   s->TRISE = 0x0011; // 1000 ns / 62.5 ns = 16 + 1
 
   // Enable the I2C master mode
   s->CR1 |= I2C_CR1_PE;  // Enable I2C
-  // Setting bus idle mode and wait for sync
+  // Wait for bus to be clear?
+  unsigned long startTime = micros();
+  bool timeout = false;
+  while (s->SR2 & I2C_SR2_BUSY) {
+    if (micros() - startTime >= 500UL) {
+      timeout = true;
+      break;
+    }
+  }
+  if (timeout) {
+    DIAG(F("I2C: SR2->BUSY timeout"));
+    // delay(1000);
+  }
 }
 
 /***************************************************************************
@@ -198,49 +243,56 @@ void I2CManagerClass::I2C_sendStart() {
 
   // Set counters here in case this is a retry.
   rxCount = txCount = 0;
-  uint8_t temp;
-
-  // On a single-master I2C bus, the start bit won't be sent until the bus 
-  // state goes to IDLE so we can request it without waiting.  On a 
-  // multi-master bus, the bus may be BUSY under control of another master, 
+  // On a single-master I2C bus, the start bit won't be sent until the bus
+  // state goes to IDLE so we can request it without waiting.  On a
+  // multi-master bus, the bus may be BUSY under control of another master,
   // in which case we can avoid some arbitration failures by waiting until
   // the bus state is IDLE.  We don't do that here.
 
-  // If anything to send, initiate write.  Otherwise initiate read.
-  if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) && !bytesToSend))
-  {
-    // Send start for read operation
-    s->CR1 |= I2C_CR1_ACK;  // Enable the ACK
-    s->CR1 |= I2C_CR1_START;  // Generate START
-    // Send address with read flag (1) or'd in
-    s->DR = (deviceAddress << 1) | 1;  //  send the address
-    while (!(s->SR1 && I2C_SR1_ADDR));  // wait for ADDR bit to set
-    // Special case for 1 byte reads!
-    if (bytesToReceive == 1)
-    {
-      s->CR1 &= ~I2C_CR1_ACK;            // clear the ACK bit 
-		  temp = I2C1->SR1 | I2C1->SR2;  // read SR1 and SR2 to clear the ADDR bit.... EV6 condition
-		  s->CR1 |= I2C_CR1_STOP;              // Stop I2C
+  // Send start for read operation
+  while (s->CR1 & I2C_CR1_STOP);  // Prevents lockup by guarding further
+                                  // writes to CR1 while STOP is being executed!
+  // Wait for bus to be clear?
+  unsigned long startTime = micros();
+  bool timeout = false;
+  while (s->SR2 & I2C_SR2_BUSY) {
+    if (micros() - startTime >= 500UL) {
+      timeout = true;
+      break;
     }
-    else
-      temp = s->SR1 | s->SR2;        // read SR1 and SR2 to clear the ADDR bit
   }
-  else {
-    // Send start for write operation
-    s->CR1 |= I2C_CR1_ACK;  // Enable the ACK
-    s->CR1 |= I2C_CR1_START;  // Generate START
-    // Send address with write flag (0) or'd in
-    s->DR = (deviceAddress << 1) | 0;  //  send the address
-    while (!(s->SR1 && I2C_SR1_ADDR));  // wait for ADDR bit to set
-    temp = s->SR1 | s->SR2;  // read SR1 and SR2 to clear the ADDR bit
+  if (timeout) {
+    DIAG(F("I2C_sendStart: SR2->BUSY timeout"));
+    // delay(1000);
   }
+  s->CR1 |= I2C_CR1_ACK;  // Enable the ACK
+  s->CR1 &= ~(I2C_CR1_POS); // Reset the POS bit - only used for 2-byte reception
+  s->CR1 |= I2C_CR1_START; // Generate START
 }
 
 /***************************************************************************
  *  Initiate a stop bit for transmission (does not interrupt)
  ***************************************************************************/
 void I2CManagerClass::I2C_sendStop() {
-  s->CR1 |= I2C_CR1_STOP;              // Stop I2C
+  uint32_t temp;
+
+  s->CR1 |= I2C_CR1_STOP; // Stop I2C
+  temp = s->SR1 | s->SR2; // Read the status registers to clear them
+  while (s->CR1 & I2C_CR1_STOP);  // Prevents lockup by guarding further
+                                  // writes to CR1 while STOP is being executed!
+  // Wait for bus to be clear?
+  unsigned long startTime = micros();
+  bool timeout = false;
+  while (s->SR2 & I2C_SR2_BUSY) {
+    if (micros() - startTime >= 500UL) {
+      timeout = true;
+      break;
+    }
+  }
+  if (timeout) {
+    DIAG(F("I2C_sendStop: SR2->BUSY timeout"));
+    // delay(1000);
+  }
 }
 
 /***************************************************************************
@@ -252,9 +304,11 @@ void I2CManagerClass::I2C_close() {
   s->CR1 &= ~I2C_CR1_PE;  // Disable I2C peripheral
   // Should never happen, but wait for up to 500us only.
   unsigned long startTime = micros();
-  while ((s->CR1 && I2C_CR1_PE) != 0) {
+  while ((s->CR1 & I2C_CR1_PE) != 0) {
     if (micros() - startTime >= 500UL) break;
   }
+  NVIC_DisableIRQ(I2C1_EV_IRQn);
+  NVIC_DisableIRQ(I2C1_ER_IRQn);
 }
 
 /***************************************************************************
@@ -263,50 +317,158 @@ void I2CManagerClass::I2C_close() {
  *  (and therefore, indirectly, from I2CRB::wait() and I2CRB::isBusy()).
  ***************************************************************************/
 void I2CManagerClass::I2C_handleInterrupt() {
+  volatile uint16_t temp_sr1, temp_sr2, temp;
+  static bool led_lit = false;
 
-  if (s->SR1 && I2C_SR1_ARLO) {
+  temp_sr1 = s->SR1;
+  // if (temp_sr1 & I2C_SR1_ADDR)
+  //   temp_sr2 = s->SR2;
+
+  // Check to see if start bit sent - SB interrupt!
+  if (temp_sr1 & I2C_SR1_SB)
+  {
+    // If anything to send, initiate write.  Otherwise initiate read.
+    if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) && !bytesToSend))
+    {
+      // Send address with read flag (1) or'd in
+      s->DR = (deviceAddress << 1) | 1;  //  send the address
+      // while (!(s->SR1 & I2C_SR1_ADDR));  // wait for ADDR bit to set
+      // // // Special case for 1 byte reads!
+      // if (bytesToReceive == 1)
+      // {
+      //   s->CR1 &= ~I2C_CR1_ACK;            // clear the ACK bit 
+      //   temp = I2C1->SR1 | I2C1->SR2;  // read SR1 and SR2 to clear the ADDR bit.... EV6 condition
+      //   s->CR1 |= I2C_CR1_STOP;              // Stop I2C
+      // }
+      // else
+      //   temp = s->SR1 | s->SR2;        // read SR1 and SR2 to clear the ADDR bit
+    }
+    else
+    {
+      // Send address with write flag (0) or'd in
+      s->DR = (deviceAddress << 1) | 0;  //  send the address
+      // while (!(s->SR1 & I2C_SR1_ADDR));  // wait for ADDR bit to set
+      // temp = s->SR1 | s->SR2;  // read SR1 and SR2 to clear the ADDR bit
+    }
+    // while (!(s->SR1 & I2C_SR1_ADDR));  // wait for ADDR bit to set
+    // temp = s->SR1 | s->SR2;  // read SR1 and SR2 to clear the ADDR bit
+  }
+  else if (temp_sr1 & I2C_SR1_ADDR) {
+    // Receive 1 byte (AN2824 figure 2)
+    if (bytesToReceive == 1) {
+      s->CR1 &= ~I2C_CR1_ACK;  // Disable ACK final byte
+      // EV6_1 must be atomic operation (AN2824)
+      // noInterrupts();
+      (void)s->SR2; // read SR2 to complete clearing the ADDR bit
+      I2C_sendStop(); // send stop
+      // interrupts();
+    }
+    // Receive 2 bytes (AN2824 figure 2)
+    else if (bytesToReceive == 2) {
+      s->CR1 |= I2C_CR1_POS;  // Set POS flag (NACK position next)
+      // EV6_1 must be atomic operation (AN2824)
+      // noInterrupts();
+      (void)s->SR2; // read SR2 to complete clearing the ADDR bit
+      s->CR1 &= ~I2C_CR1_ACK;  // Disable ACK byte
+      // interrupts();
+    }
+    else
+      temp = temp_sr1 | s->SR2;  // read SR1 and SR2 to clear the ADDR bit
+  }
+  else if (temp_sr1 & I2C_SR1_AF)
+  {
+    s->SR1 &= ~(I2C_SR1_AF); // Clear AF
+    s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
+    while (s->SR1 & I2C_SR1_AF); // Check AF cleared
+    I2C_sendStop(); // Clear the bus
+    completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+    state = I2C_STATE_COMPLETED;
+  }
+  else if (temp_sr1 & I2C_SR1_ARLO)
+  {
     // Arbitration lost, restart
-    I2C_sendStart();   // Reinitiate request
-  } else if (s->SR1 && I2C_SR1_BERR) {
+    s->SR1 &= ~(I2C_SR1_ARLO); // Clear ARLO
+    s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
+    I2C_sendStop();
+    I2C_sendStart(); // Reinitiate request
+    // state = I2C_STATE_COMPLETED;
+  }
+  else if (temp_sr1 & I2C_SR1_BERR)
+  {
     // Bus error
+    s->SR1 &= ~(I2C_SR1_BERR); // Clear BERR
+    s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
+    I2C_sendStop(); // Clear the bus
     completionStatus = I2C_STATUS_BUS_ERROR;
     state = I2C_STATE_COMPLETED;
-  } else if (s->SR1 && I2C_SR1_TXE) {
+  }
+  else if (temp_sr1 & I2C_SR1_TXE)
+  {
+    // temp_sr2 = s->SR2;
     // Master write completed
-    if (s->SR1 && (1<<10)) {
-      // Nacked, send stop.
+    if (temp_sr1 & I2C_SR1_AF) {
+      // Nacked
+      s->SR1 &= ~(I2C_SR1_AF); // Clear AF
+      s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
+      // send stop.
       I2C_sendStop();
       completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
       state = I2C_STATE_COMPLETED;
     } else if (bytesToSend) {
       // Acked, so send next byte
+      while ((s->SR1 & I2C_SR1_BTF));    // Check BTF before proceeding
       s->DR = sendBuffer[txCount++];
       bytesToSend--;
-    } else if (bytesToReceive) {
-      // Last sent byte acked and no more to send.  Send repeated start, address and read bit.
+    // } else if (bytesToReceive) {
+    //   // Last sent byte acked and no more to send.  Send repeated start, address and read bit.
+    //   s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
+    //   I2C_sendStart();
       // s->I2CM.ADDR.bit.ADDR = (deviceAddress << 1) | 1;
     } else {
+      // No bytes left to send or receive
       // Check both TxE/BTF == 1 before generating stop
-      while (!(s->SR1 && I2C_SR1_TXE));    // Check TxE
-      while (!(s->SR1 && I2C_SR1_BTF));    // Check BTF
+      // while (!(s->SR1 & I2C_SR1_TXE));    // Check TxE
+      while ((s->SR1 & I2C_SR1_BTF));    // Check BTF
       // No more data to send/receive. Initiate a STOP condition and finish
+      s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
       I2C_sendStop();
+      // completionStatus = I2C_STATUS_OK;
       state = I2C_STATE_COMPLETED;
     }
-  } else if (s->SR1 && I2C_SR1_RXNE) {
+  }
+  else if (temp_sr1 & I2C_SR1_RXNE)
+  {
     // Master read completed without errors
     if (bytesToReceive == 1) {
-//      s->I2CM.CTRLB.bit.ACKACT = 1;  // NAK final byte
+      s->CR1 &= ~I2C_CR1_ACK;  // NAK final byte
       I2C_sendStop();  // send stop
       receiveBuffer[rxCount++] = s->DR;  // Store received byte
       bytesToReceive = 0;
+      // completionStatus = I2C_STATUS_OK;
       state = I2C_STATE_COMPLETED;
-    } else if (bytesToReceive) {
-//      s->I2CM.CTRLB.bit.ACKACT = 0;  // ACK all but final byte
+    }
+    else if (bytesToReceive == 2)
+    {
+      // Also needs to be atomic!
+      // noInterrupts();
+      I2C_sendStop();
       receiveBuffer[rxCount++] = s->DR;  // Store received byte
+      // interrupts();
+    }
+    else if (bytesToReceive)
+    {
+      s->CR1 &= ~(I2C_CR1_ACK); // ACK all but final byte
+      receiveBuffer[rxCount++] = s->DR; // Store received byte
       bytesToReceive--;
     }
   }
+  else
+  {
+    // DIAG(F("Unhandled I2C interrupt!"));
+    led_lit = ~led_lit;
+    digitalWrite(D13, led_lit);
+    // delay(1000);
+  }
 }
 
 #endif /* I2CMANAGER_STM32_H */

From cc2846d932461ace1c546d956812f943f78adaa0 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Mon, 27 Mar 2023 00:20:59 +0100
Subject: [PATCH 2/5] STM32 Native I2C first working version

Working for reads and writes, needs more testing and perhaps a polish.
---
 I2CManager.h       |   3 +-
 I2CManager_STM32.h | 437 +++++++++++++++++++++------------------------
 defines.h          |   6 +-
 3 files changed, 213 insertions(+), 233 deletions(-)

diff --git a/I2CManager.h b/I2CManager.h
index b1003e6..ede30cc 100644
--- a/I2CManager.h
+++ b/I2CManager.h
@@ -539,7 +539,8 @@ private:
     uint8_t deviceAddress;
     const uint8_t *sendBuffer;
     uint8_t *receiveBuffer;
-
+    uint8_t transactionState = 0;
+  
     volatile uint32_t pendingClockSpeed = 0;
 
     void startTransaction();
diff --git a/I2CManager_STM32.h b/I2CManager_STM32.h
index 58a623d..5132640 100644
--- a/I2CManager_STM32.h
+++ b/I2CManager_STM32.h
@@ -49,7 +49,11 @@ extern "C" void I2C1_ER_IRQHandler(void) {
 // Assume I2C1 for now - default I2C bus on Nucleo-F411RE and likely Nucleo-64 variants
 I2C_TypeDef *s = I2C1;
 #define I2C_IRQn  I2C1_EV_IRQn
-#define I2C_BUSFREQ 16
+
+// Peripheral Input Clock speed in MHz.
+// For STM32F446RE, the speed is 45MHz.  Ideally, this should be determined
+// at run-time from the APB1 clock, as it can vary from STM32 family to family.
+#define I2C_PERIPH_CLK 45
 
 // I2C SR1 Status Register #1 bit definitions for convenience
 // #define I2C_SR1_SMBALERT  (1<<15)   // SMBus alert
@@ -83,15 +87,20 @@ I2C_TypeDef *s = I2C1;
 // #define I2C_CR1_SMBUS     (1<<1)    // SMBus mode, 1=SMBus, 0=I2C
 // #define I2C_CR1_PE        (1<<0)    // I2C Peripheral enable
 
+// States of the STM32 I2C driver state machine
+enum {TS_IDLE,TS_START,TS_W_ADDR,TS_W_DATA,TS_W_STOP,TS_R_ADDR,TS_R_DATA,TS_R_STOP};
+
+
 /***************************************************************************
  *  Set I2C clock speed register.  This should only be called outside of
  *  a transmission.  The I2CManagerClass::_setClock() function ensures 
  *  that it is only called at the beginning of an I2C transaction.
  ***************************************************************************/
 void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
+  return;
 
   // Calculate a rise time appropriate to the requested bus speed
-  // Use 10x the rise time spec to enable integer divide of 62.5ns clock period
+  // Use 10x the rise time spec to enable integer divide of 50ns clock period
   uint16_t t_rise;
   uint32_t ccr_freq;
 
@@ -110,44 +119,31 @@ void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
     if (i2cClockSpeed > 400000L)
       i2cClockSpeed = 400000L;
 
-    t_rise = 0x06;  // (300ns /62.5ns) + 1;
+    t_rise = 300;  // nanoseconds
   }
   else
   {
     i2cClockSpeed = 100000L;
-    t_rise = 0x11;  // (1000ns /62.5ns) + 1;
+    t_rise = 1000;  // nanoseconds
   }
   // Configure the rise time register
-  s->TRISE = t_rise;
-
-  // DIAG(F("Setting I2C clock to: %d"), i2cClockSpeed);
-  // Calculate baudrate
-  ccr_freq = I2C_BUSFREQ * 1000000 / i2cClockSpeed / 2;
+  s->TRISE = t_rise * I2C_PERIPH_CLK / 1000UL + 1;
 
   // Bit 15: I2C Master mode, 0=standard, 1=Fast Mode
-  // Bit 14: Duty, fast mode duty cycle
-  // Bit 11-0: FREQR = 16MHz => TPCLK1 = 62.5ns, so CCR divisor must be 0x50 (80 * 62.5ns = 5000ns)
-  if (i2cClockSpeed > 100000L)
+  // Bit 14: Duty, fast mode duty cycle (use 2:1)
+  // Bit 11-0: FREQR = 16MHz => TPCLK1 = 62.5ns
+  if (i2cClockSpeed > 100000L) {
+    // In fast mode, I2C period is 3 * CCR * TPCLK1.
+    ccr_freq = I2C_PERIPH_CLK * 1000000 / 3 / i2cClockSpeed;
     s->CCR = (uint16_t)ccr_freq | 0x8000; // We need Fast Mode set
-  else
+  } else {
+    // In standard mode, I2C period is 2 * CCR * TPCLK1. 
+    ccr_freq = I2C_PERIPH_CLK * 1000000 / 2 / i2cClockSpeed;
     s->CCR = (uint16_t)ccr_freq;
+  }
 
   // Enable the I2C master mode
   s->CR1 |= I2C_CR1_PE;  // Enable I2C
-  // Wait for bus to be clear?
-  unsigned long startTime = micros();
-  bool timeout = false;
-  while (s->SR2 & I2C_SR2_BUSY) {
-    if (micros() - startTime >= 500UL) {
-      timeout = true;
-      break;
-    }
-  }
-  if (timeout) {
-    digitalWrite(D13, HIGH);
-    DIAG(F("I2C: SR2->BUSY timeout"));
-    // delay(1000);
-  }
 }
 
 /***************************************************************************
@@ -176,18 +172,19 @@ void I2CManagerClass::I2C_init()
   GPIOB->AFR[1] &= ~((15<<0) | (15<<4));      // Clear all AFR bits for PB8 on low nibble, PB9 on next nibble up
   GPIOB->AFR[1] |= (4<<0) | (4<<4);           // PB8 on low nibble, PB9 on next nibble up
 
-  // // Software reset the I2C peripheral
+  // Software reset the I2C peripheral
   s->CR1 |= I2C_CR1_SWRST;  // reset the I2C
   asm("nop");    // wait a bit... suggestion from online!
   s->CR1 &= ~(I2C_CR1_SWRST); // Normal operation
 
   // Clear all bits in I2C CR2 register except reserved bits
   s->CR2 &= 0xE000;
-  // Program the peripheral input clock in CR2 Register in order to generate correct timings
-  s->CR2 |= I2C_BUSFREQ;  // PCLK1 FREQUENCY in MHz
 
-  // set own address to 00 - not really used in master mode
-  I2C1->OAR1 |= (1 << 14); // bit 14 should be kept at 1 according to the datasheet
+  // Set I2C peripheral clock frequency
+  s->CR2 |= I2C_PERIPH_CLK;
+
+  // set own address to 00 - not used in master mode
+  I2C1->OAR1 = (1 << 14); // bit 14 should be kept at 1 according to the datasheet
 
 #if defined(I2C_USE_INTERRUPTS)
   // Setting NVIC
@@ -205,35 +202,21 @@ void I2CManagerClass::I2C_init()
   // Bit 8: ITERREN - Error interrupt enable
   // Bit 7-6: reserved
   // Bit 5-0: FREQ - Peripheral clock frequency (max 50MHz)
-  s->CR2 |= 0x0700;   // Enable Buffer, Event and Error interrupts
-  // s->CR2 |= 0x0300;   // Enable Event and Error interrupts
+  s->CR2 |= (I2C_CR2_ITBUFEN | I2C_CR2_ITEVTEN | I2C_CR2_ITERREN);   // Enable Buffer, Event and Error interrupts
 #endif
 
   // Calculate baudrate and set default rate for now
   // Configure the Clock Control Register for 100KHz SCL frequency
   // Bit 15: I2C Master mode, 0=standard, 1=Fast Mode
   // Bit 14: Duty, fast mode duty cycle
-  // Bit 11-0: FREQR = 16MHz => TPCLK1 = 62.5ns, so CCR divisor must be 0x50 (80 * 62.5ns = 5000ns)
-  s->CCR = 0x50;
+  // Bit 11-0: so CCR divisor would be clk / 2 / 100000 (where clk is in Hz)
+  s->CCR = I2C_PERIPH_CLK * 5;
 
-  // Configure the rise time register - max allowed in 1000ns
-  s->TRISE = 0x0011; // 1000 ns / 62.5 ns = 16 + 1
+  // Configure the rise time register - max allowed is 1000ns, so value = 1000ns * I2C_PERIPH_CLK MHz / 1000 + 1.
+  s->TRISE = I2C_PERIPH_CLK + 1;    // 1000 ns / 50 ns = 20 + 1 = 21
 
   // Enable the I2C master mode
   s->CR1 |= I2C_CR1_PE;  // Enable I2C
-  // Wait for bus to be clear?
-  unsigned long startTime = micros();
-  bool timeout = false;
-  while (s->SR2 & I2C_SR2_BUSY) {
-    if (micros() - startTime >= 500UL) {
-      timeout = true;
-      break;
-    }
-  }
-  if (timeout) {
-    DIAG(F("I2C: SR2->BUSY timeout"));
-    // delay(1000);
-  }
 }
 
 /***************************************************************************
@@ -243,56 +226,27 @@ void I2CManagerClass::I2C_sendStart() {
 
   // Set counters here in case this is a retry.
   rxCount = txCount = 0;
+
   // On a single-master I2C bus, the start bit won't be sent until the bus
   // state goes to IDLE so we can request it without waiting.  On a
   // multi-master bus, the bus may be BUSY under control of another master,
   // in which case we can avoid some arbitration failures by waiting until
   // the bus state is IDLE.  We don't do that here.
 
-  // Send start for read operation
-  while (s->CR1 & I2C_CR1_STOP);  // Prevents lockup by guarding further
-                                  // writes to CR1 while STOP is being executed!
-  // Wait for bus to be clear?
-  unsigned long startTime = micros();
-  bool timeout = false;
-  while (s->SR2 & I2C_SR2_BUSY) {
-    if (micros() - startTime >= 500UL) {
-      timeout = true;
-      break;
-    }
-  }
-  if (timeout) {
-    DIAG(F("I2C_sendStart: SR2->BUSY timeout"));
-    // delay(1000);
-  }
-  s->CR1 |= I2C_CR1_ACK;  // Enable the ACK
-  s->CR1 &= ~(I2C_CR1_POS); // Reset the POS bit - only used for 2-byte reception
-  s->CR1 |= I2C_CR1_START; // Generate START
+  // Check there's no STOP still in progress.  If we OR the START bit into CR1
+  // and the STOP bit is already set, we could output multiple STOP conditions.
+  while (s->CR1 & I2C_CR1_STOP) {}  // Wait for STOP bit to reset
+
+  s->CR1 &= ~I2C_CR1_POS;   // Clear the POS bit
+  s->CR1 |= (I2C_CR1_ACK | I2C_CR1_START);   // Enable the ACK and generate START
+  transactionState = TS_START;
 }
 
 /***************************************************************************
  *  Initiate a stop bit for transmission (does not interrupt)
  ***************************************************************************/
 void I2CManagerClass::I2C_sendStop() {
-  uint32_t temp;
-
   s->CR1 |= I2C_CR1_STOP; // Stop I2C
-  temp = s->SR1 | s->SR2; // Read the status registers to clear them
-  while (s->CR1 & I2C_CR1_STOP);  // Prevents lockup by guarding further
-                                  // writes to CR1 while STOP is being executed!
-  // Wait for bus to be clear?
-  unsigned long startTime = micros();
-  bool timeout = false;
-  while (s->SR2 & I2C_SR2_BUSY) {
-    if (micros() - startTime >= 500UL) {
-      timeout = true;
-      break;
-    }
-  }
-  if (timeout) {
-    DIAG(F("I2C_sendStop: SR2->BUSY timeout"));
-    // delay(1000);
-  }
 }
 
 /***************************************************************************
@@ -317,157 +271,182 @@ void I2CManagerClass::I2C_close() {
  *  (and therefore, indirectly, from I2CRB::wait() and I2CRB::isBusy()).
  ***************************************************************************/
 void I2CManagerClass::I2C_handleInterrupt() {
-  volatile uint16_t temp_sr1, temp_sr2, temp;
-  static bool led_lit = false;
+  volatile uint16_t temp_sr1, temp_sr2;
 
   temp_sr1 = s->SR1;
-  // if (temp_sr1 & I2C_SR1_ADDR)
-  //   temp_sr2 = s->SR2;
 
-  // Check to see if start bit sent - SB interrupt!
-  if (temp_sr1 & I2C_SR1_SB)
-  {
-    // If anything to send, initiate write.  Otherwise initiate read.
-    if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) && !bytesToSend))
+  // Check for errors first
+  if (temp_sr1 & (I2C_SR1_AF | I2C_SR1_ARLO | I2C_SR1_BERR)) {
+    // Check which error flag is set
+    if (temp_sr1 & I2C_SR1_AF)
     {
-      // Send address with read flag (1) or'd in
-      s->DR = (deviceAddress << 1) | 1;  //  send the address
-      // while (!(s->SR1 & I2C_SR1_ADDR));  // wait for ADDR bit to set
-      // // // Special case for 1 byte reads!
-      // if (bytesToReceive == 1)
-      // {
-      //   s->CR1 &= ~I2C_CR1_ACK;            // clear the ACK bit 
-      //   temp = I2C1->SR1 | I2C1->SR2;  // read SR1 and SR2 to clear the ADDR bit.... EV6 condition
-      //   s->CR1 |= I2C_CR1_STOP;              // Stop I2C
-      // }
-      // else
-      //   temp = s->SR1 | s->SR2;        // read SR1 and SR2 to clear the ADDR bit
-    }
-    else
-    {
-      // Send address with write flag (0) or'd in
-      s->DR = (deviceAddress << 1) | 0;  //  send the address
-      // while (!(s->SR1 & I2C_SR1_ADDR));  // wait for ADDR bit to set
-      // temp = s->SR1 | s->SR2;  // read SR1 and SR2 to clear the ADDR bit
-    }
-    // while (!(s->SR1 & I2C_SR1_ADDR));  // wait for ADDR bit to set
-    // temp = s->SR1 | s->SR2;  // read SR1 and SR2 to clear the ADDR bit
-  }
-  else if (temp_sr1 & I2C_SR1_ADDR) {
-    // Receive 1 byte (AN2824 figure 2)
-    if (bytesToReceive == 1) {
-      s->CR1 &= ~I2C_CR1_ACK;  // Disable ACK final byte
-      // EV6_1 must be atomic operation (AN2824)
-      // noInterrupts();
-      (void)s->SR2; // read SR2 to complete clearing the ADDR bit
-      I2C_sendStop(); // send stop
-      // interrupts();
-    }
-    // Receive 2 bytes (AN2824 figure 2)
-    else if (bytesToReceive == 2) {
-      s->CR1 |= I2C_CR1_POS;  // Set POS flag (NACK position next)
-      // EV6_1 must be atomic operation (AN2824)
-      // noInterrupts();
-      (void)s->SR2; // read SR2 to complete clearing the ADDR bit
-      s->CR1 &= ~I2C_CR1_ACK;  // Disable ACK byte
-      // interrupts();
-    }
-    else
-      temp = temp_sr1 | s->SR2;  // read SR1 and SR2 to clear the ADDR bit
-  }
-  else if (temp_sr1 & I2C_SR1_AF)
-  {
-    s->SR1 &= ~(I2C_SR1_AF); // Clear AF
-    s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
-    while (s->SR1 & I2C_SR1_AF); // Check AF cleared
-    I2C_sendStop(); // Clear the bus
-    completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
-    state = I2C_STATE_COMPLETED;
-  }
-  else if (temp_sr1 & I2C_SR1_ARLO)
-  {
-    // Arbitration lost, restart
-    s->SR1 &= ~(I2C_SR1_ARLO); // Clear ARLO
-    s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
-    I2C_sendStop();
-    I2C_sendStart(); // Reinitiate request
-    // state = I2C_STATE_COMPLETED;
-  }
-  else if (temp_sr1 & I2C_SR1_BERR)
-  {
-    // Bus error
-    s->SR1 &= ~(I2C_SR1_BERR); // Clear BERR
-    s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
-    I2C_sendStop(); // Clear the bus
-    completionStatus = I2C_STATUS_BUS_ERROR;
-    state = I2C_STATE_COMPLETED;
-  }
-  else if (temp_sr1 & I2C_SR1_TXE)
-  {
-    // temp_sr2 = s->SR2;
-    // Master write completed
-    if (temp_sr1 & I2C_SR1_AF) {
-      // Nacked
       s->SR1 &= ~(I2C_SR1_AF); // Clear AF
-      s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
-      // send stop.
-      I2C_sendStop();
+      I2C_sendStop(); // Clear the bus
+      transactionState = TS_IDLE;
       completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
       state = I2C_STATE_COMPLETED;
-    } else if (bytesToSend) {
-      // Acked, so send next byte
-      while ((s->SR1 & I2C_SR1_BTF));    // Check BTF before proceeding
-      s->DR = sendBuffer[txCount++];
-      bytesToSend--;
-    // } else if (bytesToReceive) {
-    //   // Last sent byte acked and no more to send.  Send repeated start, address and read bit.
-    //   s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
-    //   I2C_sendStart();
-      // s->I2CM.ADDR.bit.ADDR = (deviceAddress << 1) | 1;
-    } else {
-      // No bytes left to send or receive
-      // Check both TxE/BTF == 1 before generating stop
-      // while (!(s->SR1 & I2C_SR1_TXE));    // Check TxE
-      while ((s->SR1 & I2C_SR1_BTF));    // Check BTF
-      // No more data to send/receive. Initiate a STOP condition and finish
-      s->CR1 &= ~(I2C_CR1_ACK); // Clear ACK
-      I2C_sendStop();
-      // completionStatus = I2C_STATUS_OK;
+    }
+    else if (temp_sr1 & I2C_SR1_ARLO)
+    {
+      // Arbitration lost, restart
+      s->SR1 &= ~(I2C_SR1_ARLO); // Clear ARLO
+      I2C_sendStart(); // Reinitiate request
+      transactionState = TS_START;
+    }
+    else if (temp_sr1 & I2C_SR1_BERR)
+    {
+      // Bus error
+      s->SR1 &= ~(I2C_SR1_BERR); // Clear BERR
+      I2C_sendStop(); // Clear the bus
+      transactionState = TS_IDLE;
+      completionStatus = I2C_STATUS_BUS_ERROR;
       state = I2C_STATE_COMPLETED;
     }
-  }
-  else if (temp_sr1 & I2C_SR1_RXNE)
-  {
-    // Master read completed without errors
-    if (bytesToReceive == 1) {
-      s->CR1 &= ~I2C_CR1_ACK;  // NAK final byte
-      I2C_sendStop();  // send stop
-      receiveBuffer[rxCount++] = s->DR;  // Store received byte
-      bytesToReceive = 0;
-      // completionStatus = I2C_STATUS_OK;
-      state = I2C_STATE_COMPLETED;
+  } else {
+    // No error flags, so process event according to current state.
+    switch (transactionState) {
+      case TS_START:
+        if (temp_sr1 & I2C_SR1_SB) {
+          // Event EV5
+          // Start bit has been sent successfully and we have the bus.
+          // If anything to send, initiate write.  Otherwise initiate read.
+          if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) && !bytesToSend)) {
+            // Send address with read flag (1) or'd in
+            s->DR = (deviceAddress << 1) | 1;  //  send the address
+            transactionState = TS_R_ADDR;
+          } else {
+            // Send address with write flag (0) or'd in
+            s->DR = (deviceAddress << 1) | 0;  //  send the address
+            transactionState = TS_W_ADDR;
+          }
+        }
+        // SB bit is cleared by writing to DR (already done).
+        break;
+
+      case TS_W_ADDR:
+        if (temp_sr1 & I2C_SR1_ADDR) {
+          // Event EV6
+          // Address sent successfully, device has ack'd in response.
+          if (!bytesToSend) {
+            I2C_sendStop();
+            transactionState = TS_IDLE;
+            completionStatus = I2C_STATUS_OK;
+            state = I2C_STATE_COMPLETED;
+          } else {
+            transactionState = TS_W_DATA;
+          }
+        }
+        temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
+        break;
+
+      case TS_W_DATA:
+        if (temp_sr1 & I2C_SR1_TXE) {
+          // Event EV8_1/EV8/EV8_2
+          // Transmitter empty, write a byte to it.
+          if (bytesToSend) {
+            s->DR = sendBuffer[txCount++];
+            bytesToSend--;
+          }
+          // See if we're finished sending
+          if (!bytesToSend) {
+            // Wait for last byte to be sent.
+            transactionState = TS_W_STOP;
+          }
+        }
+        break;
+
+      case TS_W_STOP:
+        if ((temp_sr1 & I2C_SR1_BTF) && (temp_sr1 & I2C_SR1_TXE)) {
+          // Event EV8_2
+          // Write finished.
+          if (bytesToReceive) {
+              // Start a read operation by sending (re)start
+              I2C_sendStart();
+          } else {
+            // Done.
+            I2C_sendStop();
+            transactionState = TS_IDLE;
+            completionStatus = I2C_STATUS_OK;
+            state = I2C_STATE_COMPLETED;
+          }
+        }
+        break;
+
+      case TS_R_ADDR:
+        if (temp_sr1 & I2C_SR1_ADDR) {
+          // Event EV6
+          // Address sent for receive.
+          // The next bit is different depending on whether there are 
+          // 1 byte, 2 bytes or >2 bytes to be received, in accordance with the
+          // Programmers Reference RM0390.
+          if (bytesToReceive == 1) {
+            // Receive 1 byte
+            s->CR1 &= ~I2C_CR1_ACK;  // Disable ack
+            temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
+            transactionState = TS_R_STOP;
+            // Next step will occur after a BTF interrupt
+          } else if (bytesToReceive == 2) {
+            // Receive 2 bytes
+            s->CR1 &= ~I2C_CR1_ACK;  // Disable ACK for final byte
+            s->CR1 |= I2C_CR1_POS;  // set POS flag to delay effect of ACK flag
+            temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
+            transactionState = TS_R_STOP;
+          } else {
+            // >2 bytes, just wait for bytes to come in and ack them for the time being
+            // (ack flag has already been set).
+            temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
+            transactionState = TS_R_DATA;
+          }
+        }
+        break;
+      
+      case TS_R_DATA:
+        // Event EV7/EV7_1
+        if (temp_sr1 & I2C_SR1_BTF) {
+          // Byte received in receiver - read next byte
+          if (bytesToReceive == 3) {
+            // Getting close to the last byte, so a specific sequence is recommended.
+            s->CR1 &= ~I2C_CR1_ACK;  // Reset ack for next byte received.
+            transactionState = TS_R_STOP;
+          }
+          receiveBuffer[rxCount++] = s->DR;  // Store received byte
+          bytesToReceive--;
+        } 
+        break;
+        
+      case TS_R_STOP:
+        if (temp_sr1 & I2C_SR1_BTF) {
+          // Event EV7 (last one)
+          // When we've got here, the receiver has got the last two bytes
+          // (or one byte, if only one byte is being received),
+          // and NAK has already been sent, so we need to read from the receiver.
+          if (bytesToReceive) {
+            if (bytesToReceive > 1) 
+              I2C_sendStop();
+            while(bytesToReceive) {
+              receiveBuffer[rxCount++] = s->DR;  // Store received byte(s)
+              bytesToReceive--;
+            }
+            // Finish.
+            transactionState = TS_IDLE;
+            completionStatus = I2C_STATUS_OK;
+            state = I2C_STATE_COMPLETED;
+          }
+        } else if (temp_sr1 & I2C_SR1_RXNE) {
+          if (bytesToReceive == 1) {
+            // One byte on a single-byte transfer.  Ack has already been set.
+            I2C_sendStop();
+            receiveBuffer[rxCount++] = s->DR; // Store received byte
+            bytesToReceive--;
+            // Finish.
+            transactionState = TS_IDLE;
+            completionStatus = I2C_STATUS_OK;
+            state = I2C_STATE_COMPLETED;
+          } else
+            s->SR1 &= I2C_SR1_RXNE;  // Acknowledge interrupt
+        }
+        break;
     }
-    else if (bytesToReceive == 2)
-    {
-      // Also needs to be atomic!
-      // noInterrupts();
-      I2C_sendStop();
-      receiveBuffer[rxCount++] = s->DR;  // Store received byte
-      // interrupts();
-    }
-    else if (bytesToReceive)
-    {
-      s->CR1 &= ~(I2C_CR1_ACK); // ACK all but final byte
-      receiveBuffer[rxCount++] = s->DR; // Store received byte
-      bytesToReceive--;
-    }
-  }
-  else
-  {
-    // DIAG(F("Unhandled I2C interrupt!"));
-    led_lit = ~led_lit;
-    digitalWrite(D13, led_lit);
-    // delay(1000);
   }
 }
 
diff --git a/defines.h b/defines.h
index 5582e8b..ab9cf45 100644
--- a/defines.h
+++ b/defines.h
@@ -144,9 +144,9 @@
     #define DISABLE_EEPROM
   #endif
   // STM32 support for native I2C is awaiting development 
-  #ifndef I2C_USE_WIRE
-  #define I2C_USE_WIRE
-  #endif
+  // #ifndef I2C_USE_WIRE
+  // #define I2C_USE_WIRE
+  // #endif
 
 
 /* TODO when ready 

From 4f56837d28198e2cee8af66ad757f3d3c8040bb7 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Tue, 28 Mar 2023 18:07:52 +0100
Subject: [PATCH 3/5] Fixes to timeout handling (due to STM32 micros()
 difference).

---
 I2CManager.cpp           |  2 +-
 I2CManager.h             |  4 ++--
 I2CManager_NonBlocking.h | 13 ++++++----
 I2CManager_STM32.h       | 52 ++++++++++++++++++++++++++++++++--------
 4 files changed, 54 insertions(+), 17 deletions(-)

diff --git a/I2CManager.cpp b/I2CManager.cpp
index d0d8550..1d1387e 100644
--- a/I2CManager.cpp
+++ b/I2CManager.cpp
@@ -92,7 +92,7 @@ void I2CManagerClass::begin(void) {
     // Probe and list devices.  Use standard mode 
     //  (clock speed 100kHz) for best device compatibility.
     _setClock(100000);
-    unsigned long originalTimeout = _timeout;
+    uint32_t originalTimeout = _timeout;
     setTimeout(1000);       // use 1ms timeout for probes
 
   #if defined(I2C_EXTENDED_ADDRESS)
diff --git a/I2CManager.h b/I2CManager.h
index ede30cc..08d81d4 100644
--- a/I2CManager.h
+++ b/I2CManager.h
@@ -485,7 +485,7 @@ private:
   // When retries are enabled, the timeout applies to each
   // try, and failure from timeout does not get retried.
   // A value of 0 means disable timeout monitoring.
-  unsigned long _timeout = 100000UL;
+  uint32_t _timeout = 100000UL;
     
   // Finish off request block by waiting for completion and posting status.
   uint8_t finishRB(I2CRB *rb, uint8_t status);
@@ -532,7 +532,7 @@ private:
     uint8_t bytesToSend = 0;
     uint8_t bytesToReceive = 0;
     uint8_t operation = 0;
-    unsigned long startTime = 0;
+    uint32_t startTime = 0;
     uint8_t muxPhase = 0;
     uint8_t muxAddress = 0;
     uint8_t muxData[1];
diff --git a/I2CManager_NonBlocking.h b/I2CManager_NonBlocking.h
index fb5bae5..59bbcaf 100644
--- a/I2CManager_NonBlocking.h
+++ b/I2CManager_NonBlocking.h
@@ -172,6 +172,10 @@ void I2CManagerClass::startTransaction() {
  *  Function to queue a request block and initiate operations.
  ***************************************************************************/
 void I2CManagerClass::queueRequest(I2CRB *req) {
+
+  if (((req->operation & OPERATION_MASK) == OPERATION_READ) && req->readLen == 0)
+    return;  // Ignore null read
+
   req->status = I2C_STATUS_PENDING;
   req->nextRequest = NULL;
   ATOMIC_BLOCK() {
@@ -184,6 +188,7 @@ void I2CManagerClass::queueRequest(I2CRB *req) {
 
 }
 
+
 /***************************************************************************
  *  Initiate a write to an I2C device (non-blocking operation)
  ***************************************************************************/
@@ -240,8 +245,8 @@ void I2CManagerClass::checkForTimeout() {
     I2CRB *t = queueHead;
     if (state==I2C_STATE_ACTIVE && t!=0 && t==currentRequest && _timeout > 0) {
       // Check for timeout
-      unsigned long elapsed = micros() - startTime;
-      if (elapsed > _timeout) { 
+      int32_t elapsed = micros() - startTime;
+      if (elapsed > (int32_t)_timeout) { 
 #ifdef DIAG_IO
         //DIAG(F("I2CManager Timeout on %s"), t->i2cAddress.toString());
 #endif
@@ -300,12 +305,12 @@ void I2CManagerClass::handleInterrupt() {
 
   // Check if current request has completed.  If there's a current request
   // and state isn't active then state contains the completion status of the request.
-  if (state == I2C_STATE_COMPLETED && currentRequest != NULL) {
+  if (state == I2C_STATE_COMPLETED && currentRequest != NULL && currentRequest == queueHead) {
     // Operation has completed.
     if (completionStatus == I2C_STATUS_OK || ++retryCounter > MAX_I2C_RETRIES
       || currentRequest->operation & OPERATION_NORETRY) 
     {
-      // Status is OK, or has failed and retry count exceeded, or retries disabled.
+      // Status is OK, or has failed and retry count exceeded, or failed and retries disabled.
 #if defined(I2C_EXTENDED_ADDRESS)
       if (muxPhase == MuxPhase_PROLOG ) {
         overallStatus = completionStatus;
diff --git a/I2CManager_STM32.h b/I2CManager_STM32.h
index 5132640..eac331a 100644
--- a/I2CManager_STM32.h
+++ b/I2CManager_STM32.h
@@ -232,6 +232,7 @@ void I2CManagerClass::I2C_sendStart() {
   // multi-master bus, the bus may be BUSY under control of another master,
   // in which case we can avoid some arbitration failures by waiting until
   // the bus state is IDLE.  We don't do that here.
+  //while (s->SR2 & I2C_SR2_BUSY) {}
 
   // Check there's no STOP still in progress.  If we OR the START bit into CR1
   // and the STOP bit is already set, we could output multiple STOP conditions.
@@ -247,6 +248,7 @@ void I2CManagerClass::I2C_sendStart() {
  ***************************************************************************/
 void I2CManagerClass::I2C_sendStop() {
   s->CR1 |= I2C_CR1_STOP; // Stop I2C
+  //while (s->CR1 & I2C_CR1_STOP) {}  // Wait for STOP bit to reset
 }
 
 /***************************************************************************
@@ -273,6 +275,9 @@ void I2CManagerClass::I2C_close() {
 void I2CManagerClass::I2C_handleInterrupt() {
   volatile uint16_t temp_sr1, temp_sr2;
 
+  pinMode(D2, OUTPUT);
+  digitalWrite(D2, 1);
+
   temp_sr1 = s->SR1;
 
   // Check for errors first
@@ -302,7 +307,8 @@ void I2CManagerClass::I2C_handleInterrupt() {
       completionStatus = I2C_STATUS_BUS_ERROR;
       state = I2C_STATE_COMPLETED;
     }
-  } else {
+  } 
+  else {
     // No error flags, so process event according to current state.
     switch (transactionState) {
       case TS_START:
@@ -324,6 +330,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
         break;
 
       case TS_W_ADDR:
+        temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
         if (temp_sr1 & I2C_SR1_ADDR) {
           // Event EV6
           // Address sent successfully, device has ack'd in response.
@@ -333,10 +340,25 @@ void I2CManagerClass::I2C_handleInterrupt() {
             completionStatus = I2C_STATUS_OK;
             state = I2C_STATE_COMPLETED;
           } else {
-            transactionState = TS_W_DATA;
+            if (bytesToSend <= 2) {
+              // After this interrupt, we will have no more data to send. 
+              // Next event of interest will be the BTF interrupt, so disable TXE interrupt
+              s->CR2 &= ~I2C_CR2_ITBUFEN;
+              transactionState = TS_W_STOP;
+            } else {
+              // More data to send, enable TXE interrupt.
+              s->CR2 |= I2C_CR2_ITBUFEN;
+              transactionState = TS_W_DATA;
+            }
+            // Put one or two bytes into DR to avoid interrupts
+            s->DR = sendBuffer[txCount++];
+            bytesToSend--;
+            if (bytesToSend) {
+              s->DR = sendBuffer[txCount++];
+              bytesToSend--;
+            }
           }
         }
-        temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
         break;
 
       case TS_W_DATA:
@@ -344,21 +366,24 @@ void I2CManagerClass::I2C_handleInterrupt() {
           // Event EV8_1/EV8/EV8_2
           // Transmitter empty, write a byte to it.
           if (bytesToSend) {
+            if (bytesToSend == 1) {
+              // We will next need to wait for BTF.
+              // TXE becomes set one byte before BTF is set, so disable 
+              // TXE interrupt while we're waiting for BTF, to suppress 
+              // repeated interrupts during that period.
+              s->CR2 &= ~I2C_CR2_ITBUFEN;
+              transactionState = TS_W_STOP;
+            }
             s->DR = sendBuffer[txCount++];
             bytesToSend--;
           }
-          // See if we're finished sending
-          if (!bytesToSend) {
-            // Wait for last byte to be sent.
-            transactionState = TS_W_STOP;
-          }
         }
         break;
 
       case TS_W_STOP:
         if ((temp_sr1 & I2C_SR1_BTF) && (temp_sr1 & I2C_SR1_TXE)) {
           // Event EV8_2
-          // Write finished.
+          // All writes finished.
           if (bytesToReceive) {
               // Start a read operation by sending (re)start
               I2C_sendStart();
@@ -383,17 +408,22 @@ void I2CManagerClass::I2C_handleInterrupt() {
             // Receive 1 byte
             s->CR1 &= ~I2C_CR1_ACK;  // Disable ack
             temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
+            // Next step will occur after a RXNE interrupt, so enable it
+            s->CR2 |= I2C_CR2_ITBUFEN;
             transactionState = TS_R_STOP;
-            // Next step will occur after a BTF interrupt
           } else if (bytesToReceive == 2) {
             // Receive 2 bytes
             s->CR1 &= ~I2C_CR1_ACK;  // Disable ACK for final byte
             s->CR1 |= I2C_CR1_POS;  // set POS flag to delay effect of ACK flag
+            // Next step will occur after a BTF interrupt, so disable RXNE interrupt
+            s->CR2 &= ~I2C_CR2_ITBUFEN;
             temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
             transactionState = TS_R_STOP;
           } else {
             // >2 bytes, just wait for bytes to come in and ack them for the time being
             // (ack flag has already been set).
+            // Next step will occur after a BTF interrupt, so disable RXNE interrupt
+            s->CR2 &= ~I2C_CR2_ITBUFEN;
             temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
             transactionState = TS_R_DATA;
           }
@@ -448,6 +478,8 @@ void I2CManagerClass::I2C_handleInterrupt() {
         break;
     }
   }
+  delayMicroseconds(1);
+  digitalWrite(D2, 0);
 }
 
 #endif /* I2CMANAGER_STM32_H */

From 4f43a413b5aeeb709fe14e2cb298077ebbe38081 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Thu, 30 Mar 2023 18:30:38 +0100
Subject: [PATCH 4/5] Update I2CManager_STM32.h

Remove debug code (writing to pin D2).  Update comments.  Restructure.
---
 I2CManager_STM32.h | 73 +++++++++++++++++++++++++---------------------
 1 file changed, 39 insertions(+), 34 deletions(-)

diff --git a/I2CManager_STM32.h b/I2CManager_STM32.h
index eac331a..f1d7e08 100644
--- a/I2CManager_STM32.h
+++ b/I2CManager_STM32.h
@@ -238,6 +238,8 @@ void I2CManagerClass::I2C_sendStart() {
   // and the STOP bit is already set, we could output multiple STOP conditions.
   while (s->CR1 & I2C_CR1_STOP) {}  // Wait for STOP bit to reset
 
+  s->CR2 |= (I2C_CR2_ITEVTEN | I2C_CR2_ITERREN);  // Enable interrupts
+  s->CR2 &= ~I2C_CR2_ITBUFEN;     // Don't enable buffer interupts yet.
   s->CR1 &= ~I2C_CR1_POS;   // Clear the POS bit
   s->CR1 |= (I2C_CR1_ACK | I2C_CR1_START);   // Enable the ACK and generate START
   transactionState = TS_START;
@@ -248,7 +250,6 @@ void I2CManagerClass::I2C_sendStart() {
  ***************************************************************************/
 void I2CManagerClass::I2C_sendStop() {
   s->CR1 |= I2C_CR1_STOP; // Stop I2C
-  //while (s->CR1 & I2C_CR1_STOP) {}  // Wait for STOP bit to reset
 }
 
 /***************************************************************************
@@ -261,7 +262,7 @@ void I2CManagerClass::I2C_close() {
   // Should never happen, but wait for up to 500us only.
   unsigned long startTime = micros();
   while ((s->CR1 & I2C_CR1_PE) != 0) {
-    if (micros() - startTime >= 500UL) break;
+    if ((int32_t)(micros() - startTime) >= 500) break;
   }
   NVIC_DisableIRQ(I2C1_EV_IRQn);
   NVIC_DisableIRQ(I2C1_ER_IRQn);
@@ -275,9 +276,6 @@ void I2CManagerClass::I2C_close() {
 void I2CManagerClass::I2C_handleInterrupt() {
   volatile uint16_t temp_sr1, temp_sr2;
 
-  pinMode(D2, OUTPUT);
-  digitalWrite(D2, 1);
-
   temp_sr1 = s->SR1;
 
   // Check for errors first
@@ -330,8 +328,8 @@ void I2CManagerClass::I2C_handleInterrupt() {
         break;
 
       case TS_W_ADDR:
-        temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
         if (temp_sr1 & I2C_SR1_ADDR) {
+          temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
           // Event EV6
           // Address sent successfully, device has ack'd in response.
           if (!bytesToSend) {
@@ -340,60 +338,65 @@ void I2CManagerClass::I2C_handleInterrupt() {
             completionStatus = I2C_STATUS_OK;
             state = I2C_STATE_COMPLETED;
           } else {
-            if (bytesToSend <= 2) {
-              // After this interrupt, we will have no more data to send. 
-              // Next event of interest will be the BTF interrupt, so disable TXE interrupt
-              s->CR2 &= ~I2C_CR2_ITBUFEN;
-              transactionState = TS_W_STOP;
-            } else {
-              // More data to send, enable TXE interrupt.
-              s->CR2 |= I2C_CR2_ITBUFEN;
-              transactionState = TS_W_DATA;
-            }
-            // Put one or two bytes into DR to avoid interrupts
+            // Put one byte into DR to load shift register.
             s->DR = sendBuffer[txCount++];
             bytesToSend--;
             if (bytesToSend) {
+              // Put another byte to load DR
               s->DR = sendBuffer[txCount++];
               bytesToSend--;
             }
+            if (!bytesToSend) {
+              // No more bytes to send.
+              // The TXE interrupt occurs when the DR is empty, and the BTF interrupt 
+              // occurs when the shift register is also empty (one character later).
+              // To avoid repeated TXE interrupts during this time, we disable TXE interrupt.
+              s->CR2 &= ~I2C_CR2_ITBUFEN;  // Wait for BTF interrupt, disable TXE interrupt
+              transactionState = TS_W_STOP;
+            } else {
+              // More data remaining to send after this interrupt, enable TXE interrupt.
+              s->CR2 |= I2C_CR2_ITBUFEN;
+              transactionState = TS_W_DATA;
+            }
           }
         }
         break;
 
       case TS_W_DATA:
         if (temp_sr1 & I2C_SR1_TXE) {
-          // Event EV8_1/EV8/EV8_2
+          // Event EV8_1/EV8
           // Transmitter empty, write a byte to it.
           if (bytesToSend) {
-            if (bytesToSend == 1) {
-              // We will next need to wait for BTF.
-              // TXE becomes set one byte before BTF is set, so disable 
-              // TXE interrupt while we're waiting for BTF, to suppress 
-              // repeated interrupts during that period.
-              s->CR2 &= ~I2C_CR2_ITBUFEN;
-              transactionState = TS_W_STOP;
-            }
             s->DR = sendBuffer[txCount++];
             bytesToSend--;
+            if (!bytesToSend) {
+              s->CR2 &= ~I2C_CR2_ITBUFEN;  // Disable TXE interrupt
+              transactionState = TS_W_STOP;
+            }
           }
-        }
+        } 
         break;
 
       case TS_W_STOP:
-        if ((temp_sr1 & I2C_SR1_BTF) && (temp_sr1 & I2C_SR1_TXE)) {
+        if (temp_sr1 & I2C_SR1_BTF) {
           // Event EV8_2
-          // All writes finished.
+          // Done, last character sent. Anything to receive?
           if (bytesToReceive) {
-              // Start a read operation by sending (re)start
-              I2C_sendStart();
+            I2C_sendStart();
+            // NOTE: Three redundant BTF interrupts take place between the
+            // first BTF interrupt and the START interrupt.  I've tried all sorts
+            // of ways to eliminate them, and the only thing that worked for
+            // me was to loop until the BTF bit becomes reset.  Either way,
+            // it's a waste of processor time.  Anyone got a solution?
+            //while (s->SR1 && I2C_SR1_BTF) {}
+            transactionState = TS_START;
           } else {
-            // Done.
             I2C_sendStop();
             transactionState = TS_IDLE;
             completionStatus = I2C_STATUS_OK;
             state = I2C_STATE_COMPLETED;
           }
+          s->SR1 &= I2C_SR1_BTF;  // Clear BTF interrupt
         }
         break;
 
@@ -477,9 +480,11 @@ void I2CManagerClass::I2C_handleInterrupt() {
         }
         break;
     }
+    // If we've received an interrupt at any other time, we're not interested so clear it
+    // to prevent it recurring ad infinitum.
+    s->SR1 = 0;
   }
-  delayMicroseconds(1);
-  digitalWrite(D2, 0);
+  
 }
 
 #endif /* I2CMANAGER_STM32_H */

From e51f8e9c0a53e4de08699ac255b6f17cb42f18bd Mon Sep 17 00:00:00 2001
From: pmantoine <pma-github@milleng.com.au>
Date: Tue, 11 Apr 2023 15:48:35 +0800
Subject: [PATCH 5/5] STM32 I2C Clock selection for 100/400KHz

---
 I2CManager_STM32.h | 75 ++++++++++++++++++++++++++--------------------
 1 file changed, 43 insertions(+), 32 deletions(-)

diff --git a/I2CManager_STM32.h b/I2CManager_STM32.h
index f1d7e08..cde4f20 100644
--- a/I2CManager_STM32.h
+++ b/I2CManager_STM32.h
@@ -26,34 +26,42 @@
 #include "I2CManager.h"
 #include "I2CManager_NonBlocking.h"   // to satisfy intellisense
 
-//#include <avr/io.h>
-//#include <avr/interrupt.h>
 #include <wiring_private.h>
+#include "stm32f4xx_hal_rcc.h"
 
-/***************************************************************************
- *  Interrupt handler.
- *  IRQ handler for SERCOM3 which is the default I2C definition for Arduino Zero
- *  compatible variants such as the Sparkfun SAMD21 Dev Breakout etc.
- *  Later we may wish to allow use of an alternate I2C bus, or more than one I2C
- *  bus on the SAMD architecture 
- ***************************************************************************/
+/*****************************************************************************
+ *  STM32F4xx I2C native driver support
+ * 
+ *  Nucleo-64 and Nucleo-144 boards all use I2C1 as the default I2C peripheral
+ *  Later we may wish to support other STM32 boards, allow use of an alternate
+ *  I2C bus, or more than one I2C bus on the STM32 architecture 
+ *****************************************************************************/
 #if defined(I2C_USE_INTERRUPTS) && defined(ARDUINO_ARCH_STM32)
+#if defined(ARDUINO_NUCLEO_F411RE) || defined(ARDUINO_NUCLEO_F446RE) || defined(ARDUINO_NUCLEO_F412ZG) || defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F446ZE)
+// Assume I2C1 for now - default I2C bus on Nucleo-F411RE and likely all Nucleo-64
+// and Nucleo-144variants
+I2C_TypeDef *s = I2C1;
+
+// In init we will ask the STM32 HAL layer for the configured APB1 clock frequency in Hz
+uint32_t APB1clk1; // Peripheral Input Clock speed in Hz.
+uint32_t i2c_MHz;  // Peripheral Input Clock speed in MHz.
+
+// IRQ handler for I2C1, replacing the weak definition in the STM32 HAL 
 extern "C" void I2C1_EV_IRQHandler(void) {
   I2CManager.handleInterrupt();
 }
 extern "C" void I2C1_ER_IRQHandler(void) {
   I2CManager.handleInterrupt();
 }
+#else
+#warning STM32 board selected is not yet supported - so I2C1 peripheral is not defined
+#endif
 #endif
-
-// Assume I2C1 for now - default I2C bus on Nucleo-F411RE and likely Nucleo-64 variants
-I2C_TypeDef *s = I2C1;
-#define I2C_IRQn  I2C1_EV_IRQn
 
 // Peripheral Input Clock speed in MHz.
 // For STM32F446RE, the speed is 45MHz.  Ideally, this should be determined
 // at run-time from the APB1 clock, as it can vary from STM32 family to family.
-#define I2C_PERIPH_CLK 45
+// #define I2C_PERIPH_CLK 45
 
 // I2C SR1 Status Register #1 bit definitions for convenience
 // #define I2C_SR1_SMBALERT  (1<<15)   // SMBus alert
@@ -97,8 +105,6 @@ enum {TS_IDLE,TS_START,TS_W_ADDR,TS_W_DATA,TS_W_STOP,TS_R_ADDR,TS_R_DATA,TS_R_ST
  *  that it is only called at the beginning of an I2C transaction.
  ***************************************************************************/
 void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
-  return;
-
   // Calculate a rise time appropriate to the requested bus speed
   // Use 10x the rise time spec to enable integer divide of 50ns clock period
   uint16_t t_rise;
@@ -106,13 +112,9 @@ void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
 
   while (s->CR1 & I2C_CR1_STOP);  // Prevents lockup by guarding further
                                   // writes to CR1 while STOP is being executed!
+
   // Disable the I2C device, as TRISE can only be programmed whilst disabled
   s->CR1 &= ~(I2C_CR1_PE);  // Disable I2C
-  // Software reset the I2C peripheral
-  // s->CR1 |= I2C_CR1_SWRST;  // reset the I2C
-  // delay(1);
-  // Release reset
-  // s->CR1 &= ~(I2C_CR1_SWRST);  // Normal operation
 
   if (i2cClockSpeed > 100000L)
   {
@@ -127,19 +129,20 @@ void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
     t_rise = 1000;  // nanoseconds
   }
   // Configure the rise time register
-  s->TRISE = t_rise * I2C_PERIPH_CLK / 1000UL + 1;
+  s->TRISE = (t_rise / (1000 / i2c_MHz)) + 1;
 
   // Bit 15: I2C Master mode, 0=standard, 1=Fast Mode
   // Bit 14: Duty, fast mode duty cycle (use 2:1)
-  // Bit 11-0: FREQR = 16MHz => TPCLK1 = 62.5ns
+  // Bit 11-0: FREQR
   if (i2cClockSpeed > 100000L) {
     // In fast mode, I2C period is 3 * CCR * TPCLK1.
-    ccr_freq = I2C_PERIPH_CLK * 1000000 / 3 / i2cClockSpeed;
-    s->CCR = (uint16_t)ccr_freq | 0x8000; // We need Fast Mode set
+    //APB1clk1 / 3 / i2cClockSpeed = 38, but that results in 306KHz not 400! 
+    ccr_freq = 30;     // So 30 gives 396KHz or so!
+    s->CCR = (uint16_t)(ccr_freq | 0x8000); // We need Fast Mode set
   } else {
-    // In standard mode, I2C period is 2 * CCR * TPCLK1. 
-    ccr_freq = I2C_PERIPH_CLK * 1000000 / 2 / i2cClockSpeed;
-    s->CCR = (uint16_t)ccr_freq;
+    // In standard mode, I2C period is 2 * CCR * TPCLK1
+    ccr_freq = (APB1clk1 / 2 / i2cClockSpeed); // Should be 225 for 45Mhz APB1 clock
+    s->CCR |= (uint16_t)ccr_freq;
   }
 
   // Enable the I2C master mode
@@ -151,7 +154,10 @@ void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
  ***************************************************************************/
 void I2CManagerClass::I2C_init()
 {
-  // Setting up the clocks
+  // Query the clockspeed from the STM32 HAL layer
+  APB1clk1 = HAL_RCC_GetPCLK1Freq();
+  i2c_MHz = APB1clk1 / 1000000UL;
+  // Enable clocks
   RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;//(1 << 21); // Enable I2C CLOCK
   // Reset the I2C1 peripheral to initial state
   RCC->APB1RSTR |=  RCC_APB1RSTR_I2C1RST;
@@ -181,7 +187,8 @@ void I2CManagerClass::I2C_init()
   s->CR2 &= 0xE000;
 
   // Set I2C peripheral clock frequency
-  s->CR2 |= I2C_PERIPH_CLK;
+  // s->CR2 |= I2C_PERIPH_CLK;
+  s->CR2 |= i2c_MHz;
 
   // set own address to 00 - not used in master mode
   I2C1->OAR1 = (1 << 14); // bit 14 should be kept at 1 according to the datasheet
@@ -210,10 +217,14 @@ void I2CManagerClass::I2C_init()
   // Bit 15: I2C Master mode, 0=standard, 1=Fast Mode
   // Bit 14: Duty, fast mode duty cycle
   // Bit 11-0: so CCR divisor would be clk / 2 / 100000 (where clk is in Hz)
-  s->CCR = I2C_PERIPH_CLK * 5;
+  // s->CCR = I2C_PERIPH_CLK * 5;
+  s->CCR &= ~(0x3000); // Clear all bits except 12 and 13 which must remain per reset value
+  s->CCR |= (APB1clk1 / 2 / 100000UL); // i2c_MHz * 5;
+  // s->CCR = i2c_MHz * 5;
 
   // Configure the rise time register - max allowed is 1000ns, so value = 1000ns * I2C_PERIPH_CLK MHz / 1000 + 1.
-  s->TRISE = I2C_PERIPH_CLK + 1;    // 1000 ns / 50 ns = 20 + 1 = 21
+  // s->TRISE = I2C_PERIPH_CLK + 1;    // 1000 ns / 50 ns = 20 + 1 = 21
+  s->TRISE = i2c_MHz + 1;
 
   // Enable the I2C master mode
   s->CR1 |= I2C_CR1_PE;  // Enable I2C