stm32/i2c: Factor I2C finding code to i2c_find_peripheral function.

Signed-off-by: Damien George <damien@micropython.org>

4 files changed, 54 insertions, 64 deletions

diff --git a/ports/stm32/i2c.c b/ports/stm32/i2c.c
index 5981df11c..d67201b71 100644
--- a/ports/stm32/i2c.c
+++ b/ports/stm32/i2c.c
@@ -26,6 +26,7 @@
 
 #include "py/mperrno.h"
 #include "py/mphal.h"
+#include "py/runtime.h"
 #include "i2c.h"
 
 #if MICROPY_HW_ENABLE_HW_I2C
@@ -487,4 +488,53 @@ int i2c_writeto(i2c_t *i2c, uint16_t addr, const uint8_t *src, size_t len, bool
 
 #endif
 
+STATIC const uint8_t i2c_available =
+    0
+    #if defined(MICROPY_HW_I2C1_SCL)
+    | 1 << 1
+    #endif
+    #if defined(MICROPY_HW_I2C2_SCL)
+    | 1 << 2
+    #endif
+    #if defined(MICROPY_HW_I2C3_SCL)
+    | 1 << 3
+    #endif
+    #if defined(MICROPY_HW_I2C4_SCL)
+    | 1 << 4
+    #endif
+;
+
+int i2c_find_peripheral(mp_obj_t id) {
+    int i2c_id = 0;
+    if (mp_obj_is_str(id)) {
+        const char *port = mp_obj_str_get_str(id);
+        if (0) {
+        #ifdef MICROPY_HW_I2C1_NAME
+        } else if (strcmp(port, MICROPY_HW_I2C1_NAME) == 0) {
+            i2c_id = 1;
+        #endif
+        #ifdef MICROPY_HW_I2C2_NAME
+        } else if (strcmp(port, MICROPY_HW_I2C2_NAME) == 0) {
+            i2c_id = 2;
+        #endif
+        #ifdef MICROPY_HW_I2C3_NAME
+        } else if (strcmp(port, MICROPY_HW_I2C3_NAME) == 0) {
+            i2c_id = 3;
+        #endif
+        #ifdef MICROPY_HW_I2C4_NAME
+        } else if (strcmp(port, MICROPY_HW_I2C4_NAME) == 0) {
+            i2c_id = 4;
+        #endif
+        } else {
+            mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("I2C(%s) doesn't exist"), port);
+        }
+    } else {
+        i2c_id = mp_obj_get_int(id);
+        if (i2c_id < 1 || i2c_id >= 8 * sizeof(i2c_available) || !(i2c_available & (1 << i2c_id))) {
+            mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("I2C(%d) doesn't exist"), i2c_id);
+        }
+    }
+    return i2c_id;
+}
+
 #endif // MICROPY_HW_ENABLE_HW_I2C
diff --git a/ports/stm32/i2c.h b/ports/stm32/i2c.h
index 4846f1cf3..d494ad66d 100644
--- a/ports/stm32/i2c.h
+++ b/ports/stm32/i2c.h
@@ -62,4 +62,6 @@ int i2c_write(i2c_t *i2c, const uint8_t *src, size_t len, size_t next_len);
 int i2c_readfrom(i2c_t *i2c, uint16_t addr, uint8_t *dest, size_t len, bool stop);
 int i2c_writeto(i2c_t *i2c, uint16_t addr, const uint8_t *src, size_t len, bool stop);
 
+int i2c_find_peripheral(mp_obj_t id);
+
 #endif // MICROPY_INCLUDED_STM32_I2C_H
diff --git a/ports/stm32/machine_i2c.c b/ports/stm32/machine_i2c.c
index cfa00b86d..41e65cf05 100644
--- a/ports/stm32/machine_i2c.c
+++ b/ports/stm32/machine_i2c.c
@@ -210,39 +210,8 @@ mp_obj_t machine_hard_i2c_make_new(const mp_obj_type_t *type, size_t n_args, siz
     mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
     mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
 
-    // work out i2c bus
-    int i2c_id = 0;
-    if (mp_obj_is_str(args[ARG_id].u_obj)) {
-        const char *port = mp_obj_str_get_str(args[ARG_id].u_obj);
-        if (0) {
-        #ifdef MICROPY_HW_I2C1_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C1_NAME) == 0) {
-            i2c_id = 1;
-        #endif
-        #ifdef MICROPY_HW_I2C2_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C2_NAME) == 0) {
-            i2c_id = 2;
-        #endif
-        #ifdef MICROPY_HW_I2C3_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C3_NAME) == 0) {
-            i2c_id = 3;
-        #endif
-        #ifdef MICROPY_HW_I2C4_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C4_NAME) == 0) {
-            i2c_id = 4;
-        #endif
-        } else {
-            mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("I2C(%s) doesn't exist"), port);
-        }
-    } else {
-        i2c_id = mp_obj_get_int(args[ARG_id].u_obj);
-        if (i2c_id < 1 || i2c_id > MP_ARRAY_SIZE(machine_hard_i2c_obj)
-            || machine_hard_i2c_obj[i2c_id - 1].base.type == NULL) {
-            mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("I2C(%d) doesn't exist"), i2c_id);
-        }
-    }
-
     // get static peripheral object
+    int i2c_id = i2c_find_peripheral(args[ARG_id].u_obj);
     machine_hard_i2c_obj_t *self = (machine_hard_i2c_obj_t *)&machine_hard_i2c_obj[i2c_id - 1];
 
     // here we would check the scl/sda pins and configure them, but it's not implemented
diff --git a/ports/stm32/pyb_i2c.c b/ports/stm32/pyb_i2c.c
index 4f8f7a0e3..ee8b498a1 100644
--- a/ports/stm32/pyb_i2c.c
+++ b/ports/stm32/pyb_i2c.c
@@ -668,39 +668,8 @@ STATIC mp_obj_t pyb_i2c_make_new(const mp_obj_type_t *type, size_t n_args, size_
     // check arguments
     mp_arg_check_num(n_args, n_kw, 1, MP_OBJ_FUN_ARGS_MAX, true);
 
-    // work out i2c bus
-    int i2c_id = 0;
-    if (mp_obj_is_str(args[0])) {
-        const char *port = mp_obj_str_get_str(args[0]);
-        if (0) {
-        #ifdef MICROPY_HW_I2C1_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C1_NAME) == 0) {
-            i2c_id = 1;
-        #endif
-        #ifdef MICROPY_HW_I2C2_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C2_NAME) == 0) {
-            i2c_id = 2;
-        #endif
-        #ifdef MICROPY_HW_I2C3_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C3_NAME) == 0) {
-            i2c_id = 3;
-        #endif
-        #ifdef MICROPY_HW_I2C4_NAME
-        } else if (strcmp(port, MICROPY_HW_I2C4_NAME) == 0) {
-            i2c_id = 4;
-        #endif
-        } else {
-            mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("I2C(%s) doesn't exist"), port);
-        }
-    } else {
-        i2c_id = mp_obj_get_int(args[0]);
-        if (i2c_id < 1 || i2c_id > MP_ARRAY_SIZE(pyb_i2c_obj)
-            || pyb_i2c_obj[i2c_id - 1].i2c == NULL) {
-            mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("I2C(%d) doesn't exist"), i2c_id);
-        }
-    }
-
     // get I2C object
+    int i2c_id = i2c_find_peripheral(args[0]);
     const pyb_i2c_obj_t *i2c_obj = &pyb_i2c_obj[i2c_id - 1];
 
     if (n_args > 1 || n_kw > 0) {