diff --git a/i2c_master.c b/i2c_master.c
index 1bde345..ae4423c 100644
--- a/i2c_master.c
+++ b/i2c_master.c
@@ -17,20 +17,36 @@
 
 #include "main.h"
 
-bool i2c_master_setup(bool fast)
+// timeout for while loops (at least 100 us)
+#define TIMEOUT 20000U
+static uint16_t timeout;
+
+bool i2c_master_setup(uint16_t freq_khz)
 {
 	// configure I²C peripheral
+	I2C_CR1 &= ~I2C_CR1_PE; // disable I²C peripheral to configure it
+/*
 	if (!i2c_master_check_signals()) { // check the signal lines
 		return false;
 	}
+*/
 	I2C_FREQR = 16; // the peripheral frequency (must match CPU frequency)
-	if (fast) {
-		I2C_CCRL = 0x2; // set SCL at 320 kHz (for less error)
-		I2C_CCRH = (I2C_CCRH_FS | I2C_CCRH_DUTY); // set fast speed mode
-		I2C_CCRH = (1U << 7); // set fast speed mode
+	if (freq_khz > 100) {
+		uint16_t ccr = (I2C_FREQR * 1000) / (3 * freq_khz);
+		if (ccr > 0x0fff) {
+			ccr = 0x0fff;
+		}
+		I2C_CCRL = (ccr & 0xff); // set SCL at 320 kHz (for less error)
+		I2C_CCRH = ((ccr >> 8) & 0x0f) | (I2C_CCRH_FS); // set fast speed mode
+		I2C_TRISER = ((I2C_FREQR * 3 / 10) + 1); // set rise time
 	} else {
-		I2C_CCRL = 0x50; // set SCL at 100 kHz
-		I2C_CCRH = 0; // set standard speed mode
+		uint16_t ccr = (I2C_FREQR * 1000) / (2 * freq_khz);
+		if (ccr > 0x0fff) {
+			ccr = 0x0fff;
+		}
+		I2C_CCRL = (ccr & 0xff); // set SCL at 320 kHz (for less error)
+		I2C_CCRH = ((ccr >> 8) & 0x0f); // set fast speed mode
+		I2C_TRISER = (I2C_FREQR + 1); // set rise time
 	}
 	I2C_CR1 |= I2C_CR1_PE; // enable I²C peripheral
 	return true;
@@ -48,10 +64,10 @@ bool i2c_master_check_signals(void)
 	GPIO_PB->DDR.reg |= (PB4 | PB5); // set SCL/SDA as output to test pull-up
 	GPIO_PB->ODR.reg |= PB4; // ensure SCL is high
 	GPIO_PB->ODR.reg &= ~PB5; // set SDA low (start condition)
-	for (volatile uint8_t t = 0; t < 10; t++); // wait 10 us
+	for (volatile uint8_t t = 0; t < 10; t++); // wait a bit to be sure signal is low
 	GPIO_PB->ODR.reg |= PB5; // set SDA high (stop condition)
-	for (volatile uint8_t t = 0; t < 10; t++); // wait 10 us for pull-up to take effect
 	GPIO_PB->DDR.reg &= ~(PB4 | PB5); // set SCL/SDA as input before it is used as alternate function by the peripheral
+	for (volatile uint8_t t = 0; t < 50; t++); // wait 10 us for pull-up to take effect
 
 	return ((GPIO_PB->IDR.reg & PB4) && (GPIO_PB->IDR.reg & PB5)); // test if both lines are up
 }
@@ -62,10 +78,10 @@ void i2c_master_reset(void)
 	// don't check if BUSY is cleared since its state might be erroneous
 	// rewriting I2C_CR2 before I2C_CR2_STOP is cleared might cause a second STOP, but at this point we don't care
 	I2C_CR2 |= I2C_CR2_SWRST; // reset peripheral, in case we got stuck and the dog bit
+	// be sure a watchdog is present as this can take forever
 	while ((0 == (GPIO_PB->IDR.reg & PB4) && (0 == (GPIO_PB->IDR.reg & PB5)))); // wait for SDA/SCL line to be released
 	I2C_CR2 &= ~I2C_CR2_SWRST; // release reset
 	I2C_CR1 &= ~I2C_CR1_PE; // disable I²C peripheral to clear some bits
-	I2C_CR1 |= I2C_CR1_PE; // re-enable I²C peripheral (must be done before any other register is written)
 }
 
 enum i2c_master_rc i2c_master_start(void)
@@ -74,19 +90,25 @@ enum i2c_master_rc i2c_master_start(void)
 	if (I2C_CR2 & (I2C_CR2_START | I2C_CR2_STOP)) { // ensure start or stop operations are not in progress
 		return I2C_MASTER_RC_START_STOP_IN_PROGESS;
 	}
+
+	// don't check BUSY flag as this might be for a re-start
 	I2C_CR2 |= I2C_CR2_START; // sent start condition
-	while ((I2C_CR2 & I2C_CR2_START) && !(I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) && !(I2C_CR2 & I2C_CR2_STOP)); // wait until start condition has been accepted and cleared
-	if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-		return I2C_MASTER_RC_BUS_ERROR;
-	}
-	if (I2C_CR2 & I2C_CR2_STOP) {
-		return I2C_MASTER_RC_TIMEOUT;
-	}
-	if (!(I2C_SR1 & I2C_SR1_SB)) { // the start bit has not been set
-		return I2C_MASTER_RC_BUS_ERROR;
-	}
-	if (!(I2C_SR3 & I2C_SR3_MSL)) { // verify if in master mode
-		return I2C_MASTER_RC_NOT_MASTER;
+
+	// in theory, when the START condition is sent, CR2_START is cleared, SR1_SB is set, and SR3_MSL is sent
+	// in practice this is not synchronous, and the TRM doesn't document it
+
+	I2C_SR2 = 0; // clear error flags
+	timeout = TIMEOUT; // start timeout count
+	while ((I2C_CR2 & I2C_CR2_START) || !(I2C_SR1 & I2C_SR1_SB) || !(I2C_SR3 & I2C_SR3_MSL)) { // wait until start condition has been accepted, send, and we are in aster mode
+		if (0 == timeout--) {
+			return I2C_MASTER_RC_TIMEOUT;
+		}
+		if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
+			return I2C_MASTER_RC_BUS_ERROR;
+		}
+		if (I2C_CR2 & I2C_CR2_STOP) {
+			return I2C_MASTER_RC_TIMEOUT;
+		}
 	}
 
 	return I2C_MASTER_RC_NONE;
@@ -97,9 +119,15 @@ enum i2c_master_rc i2c_master_start(void)
  */
 static enum i2c_master_rc i2c_master_wait_stop(void)
 {
-	while ((I2C_CR2 & I2C_CR2_STOP) && !(I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO))); // wait until stop condition is accepted and cleared
-	if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-		return I2C_MASTER_RC_BUS_ERROR;
+	I2C_SR2 = 0; // clear error flags
+	timeout = TIMEOUT; // start timeout counter
+	while (I2C_CR2 & I2C_CR2_STOP) { // wait until stop condition is accepted and cleared
+		if (0 == timeout--) {
+			return I2C_MASTER_RC_TIMEOUT;
+		}
+		if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
+			return I2C_MASTER_RC_BUS_ERROR;
+		}
 	}
 	// this time we can't use I2C_CR2_STOP to check for timeout
 	if (I2C_SR3 & I2C_SR3_MSL) { // ensure we are not in master mode anymore
@@ -108,9 +136,11 @@ static enum i2c_master_rc i2c_master_wait_stop(void)
 	if (I2C_SR3 & I2C_SR3_BUSY) { // ensure bus is released
 		return I2C_MASTER_RC_BUS_ERROR;
 	}
+/*
 	if (!i2c_master_check_signals()) { // ensure lines are released
 		return I2C_MASTER_RC_BUS_ERROR;
 	}
+*/
 
 	return I2C_MASTER_RC_NONE;
 }
@@ -145,52 +175,12 @@ enum i2c_master_rc i2c_master_select_slave(uint16_t slave, bool address_10bit, b
 	I2C_SR2 &= ~(I2C_SR2_AF); // clear acknowledgement failure
 	if (!address_10bit) { // 7-bit address
 		I2C_DR = (slave << 1) | (write ? 0 : 1); // select slave, with read/write flag
-		while (!(I2C_SR1 & I2C_SR1_ADDR) && !(I2C_SR2 & (I2C_SR2_AF | I2C_SR2_BERR | I2C_SR2_ARLO)) && !(I2C_CR2 & I2C_CR2_STOP)); // wait until address is transmitted (or error)
-		if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-			return I2C_MASTER_RC_BUS_ERROR;
-		}
-		if (I2C_CR2 & I2C_CR2_STOP) {
-			return I2C_MASTER_RC_TIMEOUT;
-		}
-		if (I2C_SR2 & I2C_SR2_AF) { // address has not been acknowledged
-			return I2C_MASTER_RC_NAK;
-		}
-	} else { // 10-bit address
-		// send first part of address
-		I2C_DR = 11110000 | (((slave >> 8 ) & 0x3) << 1); // send first header (11110xx0, where xx are 2 MSb of slave address)
-		while (!(I2C_SR1 & I2C_SR1_ADD10) && !(I2C_SR2 & (I2C_SR2_AF | I2C_SR2_BERR | I2C_SR2_ARLO)) && !(I2C_CR2 & I2C_CR2_STOP)); // wait until address is transmitted (or error)
-		if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-			return I2C_MASTER_RC_BUS_ERROR;
-		}
-		if (I2C_CR2 & I2C_CR2_STOP) {
-			return I2C_MASTER_RC_TIMEOUT;
-		}
-		if (I2C_SR2 & I2C_SR2_AF) { // address has not been acknowledged
-			return I2C_MASTER_RC_NAK;
-		}
-		// send second part of address
-		I2C_SR2 &= ~(I2C_SR2_AF); // clear acknowledgement failure
-		I2C_DR = (slave & 0xff); // send remaining of address
-		while (!(I2C_SR1 & I2C_SR1_ADDR) && !(I2C_SR2 & (I2C_SR2_AF | I2C_SR2_BERR | I2C_SR2_ARLO)) && !(I2C_CR2 & I2C_CR2_STOP)); // wait until address is transmitted (or error)
-		if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-			return I2C_MASTER_RC_BUS_ERROR;
-		}
-		if (I2C_CR2 & I2C_CR2_STOP) {
-			return I2C_MASTER_RC_TIMEOUT;
-		}
-		if (I2C_SR2 & I2C_SR2_AF) { // address has not been acknowledged
-			return I2C_MASTER_RC_NAK;
-		}
-		// go into receive mode if necessary
-		if (!write) {
-			enum i2c_master_rc rc = i2c_master_start(); // send start condition
-			if (I2C_MASTER_RC_NONE != rc) {
-				return rc;
+		I2C_SR2 = 0; // clear error flags
+		timeout = TIMEOUT; // start timeout counter
+		while (!(I2C_SR1 & I2C_SR1_ADDR)) { // wait until address is transmitted (or error)
+			if (0 == timeout--) {
+				return I2C_MASTER_RC_TIMEOUT;
 			}
-			// send first part of address with receive flag
-			I2C_SR2 &= ~(I2C_SR2_AF); // clear acknowledgement failure
-			I2C_DR = 11110001 | (((slave >> 8) & 0x3) << 1); // send header (11110xx1, where xx are 2 MSb of slave address)
-			while (!(I2C_SR1 & I2C_SR1_ADDR) && !(I2C_SR2 & (I2C_SR2_AF | I2C_SR2_BERR | I2C_SR2_ARLO)) && !(I2C_CR2 & I2C_CR2_STOP)); // wait until address is transmitted (or error)
 			if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
 				return I2C_MASTER_RC_BUS_ERROR;
 			}
@@ -201,8 +191,74 @@ enum i2c_master_rc i2c_master_select_slave(uint16_t slave, bool address_10bit, b
 				return I2C_MASTER_RC_NAK;
 			}
 		}
+	} else { // 10-bit address
+		// send first part of address
+		I2C_DR = 11110000 | (((slave >> 8 ) & 0x3) << 1); // send first header (11110xx0, where xx are 2 MSb of slave address)
+		I2C_SR2 = 0; // clear error flags
+		timeout = TIMEOUT; // start timeout counter
+		while (!(I2C_SR1 & I2C_SR1_ADD10)) { // wait until address is transmitted (or error)
+			if (0 == timeout--) {
+				return I2C_MASTER_RC_TIMEOUT;
+			}
+			if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
+				return I2C_MASTER_RC_BUS_ERROR;
+			}
+			if (I2C_CR2 & I2C_CR2_STOP) {
+				return I2C_MASTER_RC_TIMEOUT;
+			}
+			if (I2C_SR2 & I2C_SR2_AF) { // address has not been acknowledged
+				return I2C_MASTER_RC_NAK;
+			}
+		}
+		// send second part of address
+		I2C_SR2 &= ~(I2C_SR2_AF); // clear acknowledgement failure
+		I2C_DR = (slave & 0xff); // send remaining of address
+		I2C_SR2 = 0; // clear error flags
+		timeout = TIMEOUT; // start timeout counter
+		while (!(I2C_SR1 & I2C_SR1_ADDR)) { // wait until address is transmitted (or error)
+			if (0 == timeout--) {
+				return I2C_MASTER_RC_TIMEOUT;
+			}
+			if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
+				return I2C_MASTER_RC_BUS_ERROR;
+			}
+			if (I2C_CR2 & I2C_CR2_STOP) {
+				return I2C_MASTER_RC_TIMEOUT;
+			}
+			if (I2C_SR2 & I2C_SR2_AF) { // address has not been acknowledged
+				return I2C_MASTER_RC_NAK;
+			}
+		}
+		// go into receive mode if necessary
+		if (!write) {
+			enum i2c_master_rc rc = i2c_master_start(); // send start condition
+			if (I2C_MASTER_RC_NONE != rc) {
+				return rc;
+			}
+			// send first part of address with receive flag
+			I2C_SR2 &= ~(I2C_SR2_AF); // clear acknowledgement failure
+			I2C_DR = 11110001 | (((slave >> 8) & 0x3) << 1); // send header (11110xx1, where xx are 2 MSb of slave address)
+			I2C_SR2 = 0; // clear error flags
+			timeout = TIMEOUT; // start timeout counter
+			while (!(I2C_SR1 & I2C_SR1_ADDR)) { // wait until address is transmitted (or error)
+				if (0 == timeout--) {
+					return I2C_MASTER_RC_TIMEOUT;
+				}
+				if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
+					return I2C_MASTER_RC_BUS_ERROR;
+				}
+				if (I2C_CR2 & I2C_CR2_STOP) {
+					return I2C_MASTER_RC_TIMEOUT;
+				}
+				if (I2C_SR2 & I2C_SR2_AF) { // address has not been acknowledged
+					return I2C_MASTER_RC_NAK;
+				}
+			}
+		}
 	}
+	// I2C_SR3_TRA should be set after I2C_SR1_ADDR is cleared (end of address transmission), but this is not the case and the TRM/errata does not provide more info
 	// verify if we are in the right mode
+	// final check
 	if (write && !(I2C_SR3 & I2C_SR3_TRA)) {
 		return I2C_MASTER_RC_NOT_TRANSMIT;
 	} else if (!write && (I2C_SR3 & I2C_SR3_TRA)) {
@@ -225,17 +281,23 @@ enum i2c_master_rc i2c_master_read(uint8_t* data, uint16_t data_size)
 	}
 
 	// read data
+	I2C_CR2 |= I2C_CR2_ACK; // enable ACK by default
+	I2C_SR2 = 0; // clear error flags
 	for (uint16_t i = 0; i < data_size; i++) { // read bytes
+		IWDG->KR.fields.KEY = IWDG_KR_KEY_REFRESH; // reset watchdog
 		// set (N)ACK (EV6_3, EV6_1)
-		if (1 == data_size - i) { // prepare to sent NACK for last byte
+		if (1 == (data_size - i)) { // prepare to sent NACK for last byte
 			I2C_CR2 &= ~(I2C_CR2_ACK); // disable ACK
 			I2C_CR2 |= I2C_CR2_STOP; // prepare to send the stop
-		} else {
-			I2C_CR2 |= I2C_CR2_ACK; // enable ACK
 		}
-		while (!(I2C_SR1 & I2C_SR1_RXNE) && !(I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO))); // wait until data is received (or error)
-		if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-			return I2C_MASTER_RC_BUS_ERROR;
+		timeout = TIMEOUT; // start timeout counter
+		while (!(I2C_SR1 & I2C_SR1_RXNE)) { // wait until data is received (or error)
+			if (0 == timeout--) {
+				return I2C_MASTER_RC_TIMEOUT;
+			}
+			if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
+				return I2C_MASTER_RC_BUS_ERROR;
+			}
 		}
 		data[i] = I2C_DR; // read the received byte
 	}
@@ -246,7 +308,7 @@ enum i2c_master_rc i2c_master_read(uint8_t* data, uint16_t data_size)
 enum i2c_master_rc i2c_master_write(const uint8_t* data, uint16_t data_size)
 {
 	if (NULL == data || 0 == data_size) { // no data to read
-		return I2C_MASTER_RC_NONE; // we don't indicate an error because the stop is done seperatly
+		return I2C_MASTER_RC_NONE; // we don't indicate an error because the stop is done separately
 	}
 	if (!(I2C_SR3 & I2C_SR3_MSL)) { // I²C device is not in master mode
 		return I2C_MASTER_RC_NOT_MASTER;
@@ -259,27 +321,25 @@ enum i2c_master_rc i2c_master_write(const uint8_t* data, uint16_t data_size)
 	// write data
 	for (uint16_t i = 0; i < data_size; i++) { // write bytes
 		I2C_SR2 &= ~(I2C_SR2_AF); // clear acknowledgement failure
+		(void)(I2C_SR1 & I2C_SR1_BTF); // clear BTF (when followed by write) in case the clock is stretched because there was no data to send on the next transmission slot
 		I2C_DR = data[i]; // send byte
-		while (!(I2C_SR1 & I2C_SR1_TXE) && !(I2C_SR2 & (I2C_SR2_AF | I2C_SR2_BERR | I2C_SR2_ARLO)) && !(I2C_CR2 & I2C_CR2_STOP)); // wait until byte has been transmitted
-		if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-			return I2C_MASTER_RC_BUS_ERROR;
+		I2C_SR2 = 0; // clear error flags
+		timeout = TIMEOUT; // start timeout counter
+		while (!(I2C_SR1 & I2C_SR1_TXE)) { // wait until byte has been transmitted
+			IWDG->KR.fields.KEY = IWDG_KR_KEY_REFRESH; // reset watchdog
+			if (0 == timeout--) {
+				return I2C_MASTER_RC_NOT_MASTER;
+			}
+			if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
+				return I2C_MASTER_RC_BUS_ERROR;
+			}
+			if (I2C_CR2 & I2C_CR2_STOP) {
+				return I2C_MASTER_RC_TIMEOUT;
+			}
+			if (I2C_SR2 & I2C_SR2_AF) { // data has not been acknowledged
+				return I2C_MASTER_RC_NAK;
+			}
 		}
-		if (I2C_CR2 & I2C_CR2_STOP) {
-			return I2C_MASTER_RC_TIMEOUT;
-		}
-		if (I2C_SR2 & I2C_SR2_AF) { // data has not been acknowledged
-			return I2C_MASTER_RC_NAK;
-		}
-	}
-	while (!(I2C_SR1 & I2C_SR1_BTF) && !(I2C_SR2 & (I2C_SR2_AF | I2C_SR2_BERR | I2C_SR2_ARLO)) && !(I2C_CR2 & I2C_CR2_STOP)); // wait until last byte has been transmitted
-	if (I2C_SR2 & (I2C_SR2_BERR | I2C_SR2_ARLO)) {
-		return I2C_MASTER_RC_BUS_ERROR;
-	}
-	if (I2C_CR2 & I2C_CR2_STOP) {
-		return I2C_MASTER_RC_TIMEOUT;
-	}
-	if (I2C_SR2 & I2C_SR2_AF) { // data has not been acknowledged
-		return I2C_MASTER_RC_NAK;
 	}
 
 	return I2C_MASTER_RC_NONE;
@@ -417,12 +477,12 @@ enum i2c_master_rc i2c_master_address_write(uint16_t slave, bool address_10bit,
 		}
 	}
 
+	rc = i2c_master_stop(); // sent stop condition
+	if (I2C_MASTER_RC_NONE != rc) {
+		goto error;
+	}
+
 	rc = I2C_MASTER_RC_NONE; // all went fine
 error:
-	if (I2C_MASTER_RC_NONE != rc) {
-		i2c_master_stop();
-	} else {
-		rc = i2c_master_stop(); // sent stop condition
-	}
 	return rc;
 }
diff --git a/i2c_master.h b/i2c_master.h
index 2daaee9..82a41e6 100644
--- a/i2c_master.h
+++ b/i2c_master.h
@@ -21,10 +21,10 @@ enum i2c_master_rc {
 };
 
 /** setup I²C peripheral
- *  @param[in] fast if Standard mode (Sm) is used for frequencies at 100 kHz, and Fast mode (Fm) is used for frequencies at 320 kHz
+ *  @param[in] freq_khz desired clock frequency, in kHz
  *  @return if I²C bus is ready to be used (same as i2c_master_check_signals)
  */
-bool i2c_master_setup(bool fast);
+bool i2c_master_setup(uint16_t freq_khz);
 /** release I²C peripheral */
 void i2c_master_release(void);
 /** reset I²C peripheral, fixing any locked state