2 files changed, 36 insertions, 11 deletions

diff --git a/stmhal/i2c.c b/stmhal/i2c.c
index 9227d1d9dc..8e1970962b 100644
--- a/stmhal/i2c.c
+++ b/stmhal/i2c.c
@@ -96,14 +96,6 @@
 #define PYB_I2C_MASTER (0)
 #define PYB_I2C_SLAVE  (1)
 
-typedef struct _pyb_i2c_obj_t {
-    mp_obj_base_t base;
-    I2C_HandleTypeDef *i2c;
-    const dma_descr_t *tx_dma_descr;
-    const dma_descr_t *rx_dma_descr;
-    bool *use_dma;
-} pyb_i2c_obj_t;
-
 #if defined(MICROPY_HW_I2C1_SCL)
 I2C_HandleTypeDef I2CHandle1 = {.Instance = NULL};
 #endif
@@ -119,7 +111,7 @@ I2C_HandleTypeDef I2CHandle4 = {.Instance = NULL};
 
 STATIC bool pyb_i2c_use_dma[4];
 
-STATIC const pyb_i2c_obj_t pyb_i2c_obj[] = {
+const pyb_i2c_obj_t pyb_i2c_obj[] = {
     #if defined(MICROPY_HW_I2C1_SCL)
     {{&pyb_i2c_type}, &I2CHandle1, &dma_I2C_1_TX, &dma_I2C_1_RX, &pyb_i2c_use_dma[0]},
     #else
@@ -164,7 +156,7 @@ STATIC void i2c_set_baudrate(I2C_InitTypeDef *init, uint32_t baudrate) {
                                             "Unsupported I2C baudrate: %lu", baudrate));
 }
 
-STATIC uint32_t i2c_get_baudrate(I2C_InitTypeDef *init) {
+uint32_t i2c_get_baudrate(I2C_InitTypeDef *init) {
     for (int i = 0; i < NUM_BAUDRATE_TIMINGS; i++) {
         if (pyb_i2c_baudrate_timing[i].timing == init->Timing) {
             return pyb_i2c_baudrate_timing[i].baudrate;
@@ -180,7 +172,7 @@ STATIC void i2c_set_baudrate(I2C_InitTypeDef *init, uint32_t baudrate) {
     init->DutyCycle = I2C_DUTYCYCLE_16_9;
 }
 
-STATIC uint32_t i2c_get_baudrate(I2C_InitTypeDef *init) {
+uint32_t i2c_get_baudrate(I2C_InitTypeDef *init) {
     return init->ClockSpeed;
 }
 
@@ -327,6 +319,26 @@ void i2c_deinit(I2C_HandleTypeDef *i2c) {
     }
 }
 
+void i2c_init_freq(const pyb_i2c_obj_t *self, mp_int_t freq) {
+    I2C_InitTypeDef *init = &self->i2c->Init;
+
+    init->AddressingMode    = I2C_ADDRESSINGMODE_7BIT;
+    init->DualAddressMode   = I2C_DUALADDRESS_DISABLED;
+    init->GeneralCallMode   = I2C_GENERALCALL_DISABLED;
+    init->NoStretchMode     = I2C_NOSTRETCH_DISABLE;
+    init->OwnAddress1       = PYB_I2C_MASTER_ADDRESS;
+    init->OwnAddress2       = 0; // unused
+    if (freq != -1) {
+        i2c_set_baudrate(init, MIN(freq, MICROPY_HW_I2C_BAUDRATE_MAX));
+    }
+
+    *self->use_dma = false;
+
+    // init the I2C bus
+    i2c_deinit(self->i2c);
+    i2c_init(self->i2c);
+}
+
 STATIC void i2c_reset_after_error(I2C_HandleTypeDef *i2c) {
     // wait for bus-busy flag to be cleared, with a timeout
     for (int timeout = 50; timeout > 0; --timeout) {
diff --git a/stmhal/i2c.h b/stmhal/i2c.h
index c7e64e1142..fc7a6f613e 100644
--- a/stmhal/i2c.h
+++ b/stmhal/i2c.h
@@ -24,15 +24,28 @@
  * THE SOFTWARE.
  */
 
+#include "dma.h"
+
 // use this for OwnAddress1 to configure I2C in master mode
 #define PYB_I2C_MASTER_ADDRESS (0xfe)
 
+typedef struct _pyb_i2c_obj_t {
+    mp_obj_base_t base;
+    I2C_HandleTypeDef *i2c;
+    const dma_descr_t *tx_dma_descr;
+    const dma_descr_t *rx_dma_descr;
+    bool *use_dma;
+} pyb_i2c_obj_t;
+
 extern I2C_HandleTypeDef I2CHandle1;
 extern I2C_HandleTypeDef I2CHandle2;
 extern I2C_HandleTypeDef I2CHandle3;
 extern const mp_obj_type_t pyb_i2c_type;
+extern const pyb_i2c_obj_t pyb_i2c_obj[4];
 
 void i2c_init0(void);
 void i2c_init(I2C_HandleTypeDef *i2c);
+void i2c_init_freq(const pyb_i2c_obj_t *self, mp_int_t freq);
+uint32_t i2c_get_baudrate(I2C_InitTypeDef *init);
 void i2c_ev_irq_handler(mp_uint_t i2c_id);
 void i2c_er_irq_handler(mp_uint_t i2c_id);