Add frequency to hardware i2c

Tested with Naze32 @72MHz & BMP280 working at 1.2Mhz
It doesn't break existing code

STM32F1/cores/maple/libmaple/i2c.c

@@ -47,7 +47,7 @@
 static inline int32 wait_for_state_change(i2c_dev *dev,
                                           i2c_state state,
                                           uint32 timeout);
-static void set_ccr_trise(i2c_dev *dev, uint32 flags);
+static void set_ccr_trise(i2c_dev *dev, uint32 flags, uint32 freq);
 
 /**
  * @brief Fill data register with slave address
@@ -165,7 +165,6 @@ void i2c_init(i2c_dev *dev) {
  * @brief Initialize an I2C device as bus master
  * @param dev Device to enable
  * @param flags Bitwise or of the following I2C options:
- *              I2C_FAST_MODE: 400 khz operation,
  *              I2C_DUTY_16_9: 16/9 Tlow/Thigh duty cycle (only applicable for
  *                             fast mode),
  *              I2C_BUS_RESET: Reset the bus and clock out any hung slaves on
@@ -174,7 +173,7 @@ void i2c_init(i2c_dev *dev) {
  *              I2C_REMAP: (deprecated, STM32F1 only) Remap I2C1 to SCL/PB8
  *                         SDA/PB9.
  */
-void i2c_master_enable(i2c_dev *dev, uint32 flags) {
+void i2c_master_enable(i2c_dev *dev, uint32 flags, uint32 freq) {
     /* PE must be disabled to configure the device */
     ASSERT(!(dev->regs->CR1 & I2C_CR1_PE));
 
@@ -191,7 +190,7 @@ void i2c_master_enable(i2c_dev *dev, uint32 flags) {
     i2c_config_gpios(dev);
 
     /* Configure clock and rise time */
-    set_ccr_trise(dev, flags);
+    set_ccr_trise(dev, flags, freq);
 
     /* Enable event and buffer interrupts */
     nvic_irq_enable(dev->ev_nvic_line);
@@ -483,30 +482,30 @@ void _i2c_irq_error_handler(i2c_dev *dev) {
 /*
  * CCR/TRISE configuration helper
  */
-static void set_ccr_trise(i2c_dev *dev, uint32 flags) {
+static void set_ccr_trise(i2c_dev *dev, uint32 flags, uint32 freq) {
     uint32 ccr     = 0;
     uint32 trise   = 0;
     uint32 clk_mhz = _i2c_bus_clk(dev);
     uint32 clk_hz  = clk_mhz * (1000 * 1000);
 
     i2c_set_input_clk(dev, clk_mhz);
 
-    if (flags & I2C_FAST_MODE) {
+    if (freq > 100000) {		//FAST MODE & FAST MODE+
         ccr |= I2C_CCR_FS;
 
         if (flags & I2C_DUTY_16_9) {
             /* Tlow/Thigh = 16/9 */
             ccr |= I2C_CCR_DUTY_16_9;
-            ccr |= clk_hz / (400000 * 25);
+            ccr |= clk_hz / (freq * 25);
         } else {
             /* Tlow/Thigh = 2 */
-            ccr |= clk_hz / (400000 * 3);
+            ccr |= clk_hz / (freq * 3);
         }
 
         trise = (300 * clk_mhz / 1000) + 1;
     } else {
         /* Tlow/Thigh = 1 */
-        ccr = clk_hz / (100000 * 2);
+        ccr = clk_hz / (freq * 2);
         trise = clk_mhz + 1;
     }
 

STM32F1/libraries/Wire/Wire.cpp

@@ -50,7 +50,7 @@ uint8 TwoWire::process(uint8 stop) {
             res = EOTHER;
         }
         i2c_disable(sel_hard);
-        i2c_master_enable(sel_hard, dev_flags);
+        i2c_master_enable(sel_hard, dev_flags, frequency);
     }
     return res;
 }
@@ -60,15 +60,22 @@ uint8 TwoWire::process(){
 }
 
 // TODO: Add in Error Handling if devsel is out of range for other Maples
-TwoWire::TwoWire(uint8 dev_sel, uint8 flags) {
-    if (dev_sel == 1) {
+TwoWire::TwoWire(uint8 dev_sel, uint8 flags, uint32 freq) {
+
+	if (dev_sel == 1) {
         sel_hard = I2C1;
     } else if (dev_sel == 2) {
         sel_hard = I2C2;
     } else {
         ASSERT(1);
     }
     dev_flags = flags;
+
+	if (freq == 100000 && (flags & I2C_FAST_MODE))  // compatibility patch
+		frequency = 400000;
+	else
+		frequency = freq;
+
 }
 
 TwoWire::~TwoWire() {
@@ -77,7 +84,7 @@ TwoWire::~TwoWire() {
 }
 
 void TwoWire::begin(uint8 self_addr) {
-    i2c_master_enable(sel_hard, dev_flags);
+    i2c_master_enable(sel_hard, dev_flags, frequency);
 }
 
 void TwoWire::end() {
@@ -87,19 +94,11 @@ void TwoWire::end() {
 
 void TwoWire::setClock(uint32_t frequencyHz)
 {
-	switch(frequencyHz)
-	{
-		case 400000:
-			dev_flags |= I2C_FAST_MODE;// set FAST_MODE bit
-			break;
-		case 100000:
-		default:
-			dev_flags &= ~I2C_FAST_MODE;// clear FAST_MODE bit
-			break;
-	}
+
 	if (sel_hard->regs->CR1 & I2C_CR1_PE){
+		frequency = frequencyHz;
 	    i2c_disable(sel_hard);
-	    i2c_master_enable(sel_hard, dev_flags);
+	    i2c_master_enable(sel_hard, dev_flags, frequency);
 	}
 }
 

STM32F1/libraries/Wire/Wire.h

@@ -47,32 +47,37 @@ class TwoWire : public WireBase {
 private:
     i2c_dev* sel_hard;
     uint8    dev_flags;
+	uint32	frequency; //new variable to store i2c frequency
 protected:
     /*
      * Processes the incoming I2C message defined by WireBase to the
      * hardware. If an error occured, restart the I2C device.
      */
-    uint8 process(uint8);
+    uint8 process(uint8 stop);
     uint8 process();
 public:
     /*
      * Check if devsel is within range and enable selected I2C interface with
-     * passed flags
+     * passed flags and frequency.
+	 *(frequency tested go up to 1.2Mhz @F_CPU 72Mhz with capable slaves)
      */
-    TwoWire(uint8, uint8 = 0);
+    TwoWire(uint8 dev_sel, uint8 flags = 0, uint32 freq = 100000);
 
 	/*
 	 * Shuts down (disables) the hardware I2C
 	 */
 	void end();
 
+	/*
+	* Sets the hardware I2C clock
+	*/
 	void setClock(uint32_t frequencyHz);
     /*
      * Disables the I2C device and remove the device address.
      */
     ~TwoWire();
 
-    void begin(uint8 = 0x00);
+    void begin(uint8 self_addr = 0x00);
 };
 extern TwoWire Wire;
 #endif // _TWOWIRE_H_

STM32F1/system/libmaple/include/libmaple/i2c.h

@@ -198,7 +198,7 @@ typedef struct i2c_msg {
 #define I2C_DUTY_16_9           0x2           // 16/9 duty ratio
 /* Flag 0x4 is reserved; DO NOT USE. */
 #define I2C_BUS_RESET           0x8           // Perform a bus reset
-void i2c_master_enable(i2c_dev *dev, uint32 flags);
+void i2c_master_enable(i2c_dev *dev, uint32 flags, uint32 freq);
 
 #define I2C_ERROR_PROTOCOL      (-1)
 #define I2C_ERROR_TIMEOUT       (-2)