diff options
Diffstat (limited to 'extmod/machine_i2c.c')
| -rw-r--r-- | extmod/machine_i2c.c | 22 |
1 files changed, 15 insertions, 7 deletions
diff --git a/extmod/machine_i2c.c b/extmod/machine_i2c.c index 14cba9623..c804cf570 100644 --- a/extmod/machine_i2c.c +++ b/extmod/machine_i2c.c @@ -526,13 +526,24 @@ STATIC mp_obj_t machine_i2c_writevto(size_t n_args, const mp_obj_t *args) { } STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(machine_i2c_writevto_obj, 3, 4, machine_i2c_writevto); -STATIC int read_mem(mp_obj_t self_in, uint16_t addr, uint32_t memaddr, uint8_t addrsize, uint8_t *buf, size_t len) { - mp_obj_base_t *self = (mp_obj_base_t *)MP_OBJ_TO_PTR(self_in); - uint8_t memaddr_buf[4]; +STATIC size_t fill_memaddr_buf(uint8_t *memaddr_buf, uint32_t memaddr, uint8_t addrsize) { size_t memaddr_len = 0; + if ((addrsize & 7) != 0 || addrsize > 32) { + mp_raise_ValueError(MP_ERROR_TEXT("invalid addrsize")); + } for (int16_t i = addrsize - 8; i >= 0; i -= 8) { memaddr_buf[memaddr_len++] = memaddr >> i; } + return memaddr_len; +} + +STATIC int read_mem(mp_obj_t self_in, uint16_t addr, uint32_t memaddr, uint8_t addrsize, uint8_t *buf, size_t len) { + mp_obj_base_t *self = (mp_obj_base_t *)MP_OBJ_TO_PTR(self_in); + + // Create buffer with memory address + uint8_t memaddr_buf[4]; + size_t memaddr_len = fill_memaddr_buf(&memaddr_buf[0], memaddr, addrsize); + int ret = mp_machine_i2c_writeto(self, addr, memaddr_buf, memaddr_len, false); if (ret != memaddr_len) { // must generate STOP @@ -546,11 +557,8 @@ STATIC int write_mem(mp_obj_t self_in, uint16_t addr, uint32_t memaddr, uint8_t mp_obj_base_t *self = (mp_obj_base_t *)MP_OBJ_TO_PTR(self_in); // Create buffer with memory address - size_t memaddr_len = 0; uint8_t memaddr_buf[4]; - for (int16_t i = addrsize - 8; i >= 0; i -= 8) { - memaddr_buf[memaddr_len++] = memaddr >> i; - } + size_t memaddr_len = fill_memaddr_buf(&memaddr_buf[0], memaddr, addrsize); // Create partial write buffers mp_machine_i2c_buf_t bufs[2] = { |