diff --git a/extmod/machine_i2c.c b/extmod/machine_i2c.c index 63493141cb..0687aa9d2d 100644 --- a/extmod/machine_i2c.c +++ b/extmod/machine_i2c.c @@ -480,6 +480,52 @@ STATIC mp_obj_t machine_i2c_writeto(size_t n_args, const mp_obj_t *args) { } STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(machine_i2c_writeto_obj, 3, 4, machine_i2c_writeto); +STATIC mp_obj_t machine_i2c_writevto(size_t n_args, const mp_obj_t *args) { + mp_obj_base_t *self = (mp_obj_base_t*)MP_OBJ_TO_PTR(args[0]); + mp_int_t addr = mp_obj_get_int(args[1]); + + // Get the list of data buffer(s) to write + size_t nitems; + const mp_obj_t *items; + mp_obj_get_array(args[2], &nitems, (mp_obj_t**)&items); + + // Get the stop argument + bool stop = (n_args == 3) ? true : mp_obj_is_true(args[3]); + + // Extract all buffer data, skipping zero-length buffers + size_t alloc = nitems == 0 ? 1 : nitems; + size_t nbufs = 0; + mp_machine_i2c_buf_t *bufs = mp_local_alloc(alloc * sizeof(mp_machine_i2c_buf_t)); + for (; nitems--; ++items) { + mp_buffer_info_t bufinfo; + mp_get_buffer_raise(*items, &bufinfo, MP_BUFFER_READ); + if (bufinfo.len > 0) { + bufs[nbufs].len = bufinfo.len; + bufs[nbufs++].buf = bufinfo.buf; + } + } + + // Make sure there is at least one buffer, empty if needed + if (nbufs == 0) { + bufs[0].len = 0; + bufs[0].buf = NULL; + nbufs = 1; + } + + // Do the I2C transfer + mp_machine_i2c_p_t *i2c_p = (mp_machine_i2c_p_t*)self->type->protocol; + int ret = i2c_p->transfer(self, addr, nbufs, bufs, stop ? MP_MACHINE_I2C_FLAG_STOP : 0); + mp_local_free(bufs); + + if (ret < 0) { + mp_raise_OSError(-ret); + } + + // Return number of acks received + return MP_OBJ_NEW_SMALL_INT(ret); +} +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]; @@ -601,6 +647,7 @@ STATIC const mp_rom_map_elem_t machine_i2c_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_readfrom), MP_ROM_PTR(&machine_i2c_readfrom_obj) }, { MP_ROM_QSTR(MP_QSTR_readfrom_into), MP_ROM_PTR(&machine_i2c_readfrom_into_obj) }, { MP_ROM_QSTR(MP_QSTR_writeto), MP_ROM_PTR(&machine_i2c_writeto_obj) }, + { MP_ROM_QSTR(MP_QSTR_writevto), MP_ROM_PTR(&machine_i2c_writevto_obj) }, // memory operations { MP_ROM_QSTR(MP_QSTR_readfrom_mem), MP_ROM_PTR(&machine_i2c_readfrom_mem_obj) },