extmod/machine_i2c: Implement I2C memory reading/writing.

1 files changed, 106 insertions, 6 deletions

diff --git a/extmod/machine_i2c.c b/extmod/machine_i2c.c
index d8ce58890..061b44378 100644
--- a/extmod/machine_i2c.c
+++ b/extmod/machine_i2c.c
@@ -188,6 +188,55 @@ er:
     nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "I2C bus error"));
 }
 
+STATIC void mp_hal_i2c_write_mem(machine_i2c_obj_t *self, uint8_t addr, uint16_t memaddr, const uint8_t *src, size_t len) {
+    // start the I2C transaction
+    mp_hal_i2c_start(self);
+
+    // write the slave address and the memory address within the slave
+    if (!mp_hal_i2c_write_byte(self, addr << 1)) {
+        goto er;
+    }
+    if (!mp_hal_i2c_write_byte(self, memaddr)) {
+        goto er;
+    }
+
+    // write the buffer to the I2C memory
+    while (len--) {
+        if (!mp_hal_i2c_write_byte(self, *src++)) {
+            goto er;
+        }
+    }
+
+    // finish the I2C transaction
+    mp_hal_i2c_stop(self);
+    return;
+
+er:
+    mp_hal_i2c_stop(self);
+    nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "I2C bus error"));
+}
+
+STATIC void mp_hal_i2c_read_mem(machine_i2c_obj_t *self, uint8_t addr, uint16_t memaddr, uint8_t *dest, size_t len) {
+    // start the I2C transaction
+    mp_hal_i2c_start(self);
+
+    // write the slave address and the memory address within the slave
+    if (!mp_hal_i2c_write_byte(self, addr << 1)) {
+        goto er;
+    }
+    if (!mp_hal_i2c_write_byte(self, memaddr)) {
+        goto er;
+    }
+
+    // i2c_read will do a repeated start, and then read the I2C memory
+    mp_hal_i2c_read(self, addr, dest, len);
+    return;
+
+er:
+    mp_hal_i2c_stop(self);
+    nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "I2C bus error"));
+}
+
 /******************************************************************************/
 // MicroPython bindings for I2C
 
@@ -316,18 +365,69 @@ STATIC mp_obj_t machine_i2c_writeto(mp_obj_t self_in, mp_obj_t addr_in, mp_obj_t
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_3(machine_i2c_writeto_obj, machine_i2c_writeto);
 
-STATIC mp_obj_t machine_i2c_readfrom_mem(size_t n_args, const mp_obj_t *args, mp_map_t *kw_args) {
-    mp_not_implemented("I2C.readfrom_mem");
+STATIC mp_obj_t machine_i2c_readfrom_mem(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    enum { ARG_addr, ARG_memaddr, ARG_n, ARG_addrsize };
+    static const mp_arg_t allowed_args[] = {
+        { MP_QSTR_addr,    MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_memaddr, MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_n,       MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        //{ MP_QSTR_addrsize, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 8} }, TODO
+    };
+    machine_i2c_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
+    mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
+    mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
+
+    // create the buffer to store data into
+    vstr_t vstr;
+    vstr_init_len(&vstr, args[ARG_n].u_int);
+
+    // do the transfer
+    mp_hal_i2c_read_mem(self, args[ARG_addr].u_int, args[ARG_memaddr].u_int, (uint8_t*)vstr.buf, vstr.len);
+    return mp_obj_new_str_from_vstr(&mp_type_bytes, &vstr);
 }
 MP_DEFINE_CONST_FUN_OBJ_KW(machine_i2c_readfrom_mem_obj, 1, machine_i2c_readfrom_mem);
 
-STATIC mp_obj_t machine_i2c_readfrom_mem_into(size_t n_args, const mp_obj_t *args, mp_map_t *kw_args) {
-    mp_not_implemented("I2C.readfrom_mem_into");
+STATIC mp_obj_t machine_i2c_readfrom_mem_into(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    enum { ARG_addr, ARG_memaddr, ARG_buf, ARG_addrsize };
+    static const mp_arg_t allowed_args[] = {
+        { MP_QSTR_addr,    MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_memaddr, MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_buf,     MP_ARG_REQUIRED | MP_ARG_OBJ, {.u_obj = MP_OBJ_NULL} },
+        //{ MP_QSTR_addrsize, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 8} }, TODO
+    };
+    machine_i2c_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
+    mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
+    mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
+
+    // get the buffer to store data into
+    mp_buffer_info_t bufinfo;
+    mp_get_buffer_raise(args[ARG_buf].u_obj, &bufinfo, MP_BUFFER_WRITE);
+
+    // do the transfer
+    mp_hal_i2c_read_mem(self, args[ARG_addr].u_int, args[ARG_memaddr].u_int, bufinfo.buf, bufinfo.len);
+    return mp_const_none;
 }
 MP_DEFINE_CONST_FUN_OBJ_KW(machine_i2c_readfrom_mem_into_obj, 1, machine_i2c_readfrom_mem_into);
 
-STATIC mp_obj_t machine_i2c_writeto_mem(size_t n_args, const mp_obj_t *args, mp_map_t *kw_args) {
-    mp_not_implemented("I2C.writeto_mem");
+STATIC mp_obj_t machine_i2c_writeto_mem(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    enum { ARG_addr, ARG_memaddr, ARG_buf, ARG_addrsize };
+    static const mp_arg_t allowed_args[] = {
+        { MP_QSTR_addr,    MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_memaddr, MP_ARG_REQUIRED | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_buf,     MP_ARG_REQUIRED | MP_ARG_OBJ, {.u_obj = MP_OBJ_NULL} },
+        //{ MP_QSTR_addrsize, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 8} }, TODO
+    };
+    machine_i2c_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
+    mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
+    mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
+
+    // get the buffer to write the data from
+    mp_buffer_info_t bufinfo;
+    mp_get_buffer_raise(args[ARG_buf].u_obj, &bufinfo, MP_BUFFER_READ);
+
+    // do the transfer
+    mp_hal_i2c_write_mem(self, args[ARG_addr].u_int, args[ARG_memaddr].u_int, bufinfo.buf, bufinfo.len);
+    return mp_const_none;
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_KW(machine_i2c_writeto_mem_obj, 1, machine_i2c_writeto_mem);