diff --git a/src/i2c_software.c b/src/i2c_software.c index 990948644..77da94471 100644 --- a/src/i2c_software.c +++ b/src/i2c_software.c @@ -26,12 +26,19 @@ command_i2c_set_software_bus(uint32_t *args) { struct i2cdev_s *i2c = i2cdev_oid_lookup(args[0]); struct i2c_software *is = alloc_chunk(sizeof(*is)); + uint32_t rate = args[3]; is->ticks = CONFIG_CLOCK_FREQ / (100000 * 2); // 100KHz is->addr = (args[4] & 0x7f) << 1; // address format shifted is->scl_in = gpio_in_setup(args[1], 1); is->scl_out = gpio_out_setup(args[1], 1); is->sda_in = gpio_in_setup(args[2], 1); is->sda_out = gpio_out_setup(args[2], 1); + while (rate > 100000) { + rate = rate >> 1; + if (rate < 100000) + break; + is->ticks = is->ticks >> 1; + } i2cdev_set_software_bus(i2c, is); } DECL_COMMAND(command_i2c_set_software_bus,