Rewrite MCU I2C to support non blocking execution.

klippy/extras/bus.py

@@ -179,7 +179,7 @@ def build_config(self):
         self.i2c_read_cmd = self.mcu.lookup_query_command(
             "i2c_read oid=%c reg=%*s read_len=%u",
             "i2c_read_response oid=%c response=%*s", oid=self.oid,
-            cq=self.cmd_queue)
+            cq=self.cmd_queue, is_async=True)
         self.i2c_modify_bits_cmd = self.mcu.lookup_command(
             "i2c_modify_bits oid=%c reg=%*s clear_set_bits=%*s",
             cq=self.cmd_queue)

src/atsam/gpio.h

@@ -2,6 +2,7 @@
 #define __ATSAM_GPIO_H
 
 #include <stdint.h> // uint32_t
+#include "sched.h"  // timer
 
 struct gpio_out {
     void *regs;
@@ -53,5 +54,6 @@ struct i2c_config i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr);
 void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write);
 void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg
               , uint8_t read_len, uint8_t *read);
+uint_fast8_t i2c_async(struct timer *timer);
 
 #endif // gpio.h

src/atsam/i2c.c

@@ -10,6 +10,7 @@
 #include "gpio.h" // i2c_setup
 #include "internal.h" // gpio_peripheral
 #include "sched.h" // sched_shutdown
+#include "i2ccmds.h" // struct i2cdev_s
 
 #if CONFIG_MACH_SAME70
 #include "same70_i2c.h" // Fixes for upstream header changes
@@ -193,3 +194,37 @@ i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg
         ;
     (void)p_twi->TWI_SR;
 }
+
+uint_fast8_t i2c_async(struct timer *timer)
+{
+    struct i2cdev_s *i2c = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config config = i2c->i2c_config;
+    if (i2c->data_len[1] & I2C_R) {
+        uint8_t reg_len = i2c->data_len[0];
+        uint8_t *reg = i2c->buf;
+        uint8_t read_len = i2c->data_len[1] & ~(I2C_R);
+        uint8_t *resp = &i2c->buf[reg_len];
+        i2c_read(config, reg_len, reg, read_len, resp);
+        for (int i = 0; i < reg_len; i++) {
+            i2c_buf_read_b(i2c);
+        }
+        i2c_cmd_done(i2c);
+        i2c->cur = i2c->tail + read_len;
+        timer->func = i2c->callback;
+        timer->waketime = timer_read_time() + timer_from_us(50);
+        return SF_RESCHEDULE;
+    } else if (i2c->data_len[0]) {
+        uint8_t to_write = i2c->data_len[0];
+        uint8_t buf[to_write];
+        for (int i = 0; i < to_write; i++) {
+            buf[i] = i2c_buf_read_b(i2c);
+        }
+        i2c_write(config, to_write, buf);
+        i2c_cmd_done(i2c);
+        timer->waketime = timer_read_time() + timer_from_us(50);
+        return SF_RESCHEDULE;
+    }
+
+    i2c->flags &= ~IF_ACTIVE;
+    return SF_DONE;
+}

src/atsamd/gpio.h

@@ -2,6 +2,7 @@
 #define __ATSAMD_GPIO_H
 
 #include <stdint.h>
+#include "sched.h" // timer
 
 struct gpio_out {
     void *regs;
@@ -54,5 +55,6 @@ struct i2c_config i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr);
 void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write);
 void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg
               , uint8_t read_len, uint8_t *read);
+uint_fast8_t i2c_async(struct timer *timer);
 
 #endif // gpio.h

src/atsamd/i2c.c

@@ -9,6 +9,7 @@
 #include "command.h" // shutdown
 #include "gpio.h" // i2c_setup
 #include "sched.h" // sched_shutdown
+#include "i2ccmds.h" // struct i2cdev_s
 
 #define TIME_RISE 125ULL // 125 nanoseconds
 #define I2C_FREQ 100000
@@ -144,3 +145,37 @@ i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg
         *read++ = si->DATA.reg;
     }
 }
+
+uint_fast8_t i2c_async(struct timer *timer)
+{
+    struct i2cdev_s *i2c = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config config = i2c->i2c_config;
+    if (i2c->data_len[1] & I2C_R) {
+        uint8_t reg_len = i2c->data_len[0];
+        uint8_t *reg = i2c->buf;
+        uint8_t read_len = i2c->data_len[1] & ~(I2C_R);
+        uint8_t *resp = &i2c->buf[reg_len];
+        i2c_read(config, reg_len, reg, read_len, resp);
+        for (int i = 0; i < reg_len; i++) {
+            i2c_buf_read_b(i2c);
+        }
+        i2c_cmd_done(i2c);
+        i2c->cur = i2c->tail + read_len;
+        timer->func = i2c->callback;
+        timer->waketime = timer_read_time() + timer_from_us(50);
+        return SF_RESCHEDULE;
+    } else if (i2c->data_len[0]) {
+        uint8_t to_write = i2c->data_len[0];
+        uint8_t buf[to_write];
+        for (int i = 0; i < to_write; i++) {
+            buf[i] = i2c_buf_read_b(i2c);
+        }
+        i2c_write(config, to_write, buf);
+        i2c_cmd_done(i2c);
+        timer->waketime = timer_read_time() + timer_from_us(50);
+        return SF_RESCHEDULE;
+    }
+
+    i2c->flags &= ~IF_ACTIVE;
+    return SF_DONE;
+}

src/avr/gpio.h

@@ -2,6 +2,7 @@
 #define __AVR_GPIO_H
 
 #include <stdint.h>
+#include "sched.h" // timer
 
 struct gpio_out {
     struct gpio_digital_regs *regs;
@@ -47,11 +48,13 @@ void spi_transfer(struct spi_config config, uint8_t receive_data
 
 struct i2c_config {
     uint8_t addr;
+    uint32_t pause;
 };
 
 struct i2c_config i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr);
 void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write);
 void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg
               , uint8_t read_len, uint8_t *read);
+uint_fast8_t i2c_async(struct timer *timer);
 
 #endif // gpio.h

src/avr/i2c.c

@@ -11,6 +11,7 @@
 #include "gpio.h" // i2c_setup
 #include "internal.h" // GPIO
 #include "sched.h" // sched_shutdown
+#include "i2ccmds.h" // struct i2cdev_s
 
 DECL_ENUMERATION("i2c_bus", "twi", 0);
 
@@ -30,6 +31,7 @@ DECL_CONSTANT_STR("BUS_PINS_twi", "PD0,PD1");
 struct i2c_config
 i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr)
 {
+    uint8_t us = 23;
     if (bus)
         shutdown("Unsupported i2c bus");
 
@@ -40,15 +42,16 @@ i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr)
 
         // Set frequency avoiding pulling in integer divide
         TWSR = 0;
-        if (rate >= 400000)
+        if (rate >= 400000) {
             TWBR = ((CONFIG_CLOCK_FREQ / 400000) - 16) / 2;
-        else
+        } else {
             TWBR = ((CONFIG_CLOCK_FREQ / 100000) - 16) / 2;
-
+            us = 90;
+        }
         // Enable interface
         TWCR = (1<<TWEN);
     }
-    return (struct i2c_config){ .addr=addr<<1 };
+    return (struct i2c_config){.addr = addr << 1, .pause = timer_from_us(us)};
 }
 
 static void
@@ -121,3 +124,139 @@ i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg
         i2c_receive_byte(read++, timeout, read_len);
     i2c_stop(timeout);
 }
+
+static uint_fast8_t i2c_async_read(struct timer *timer);
+
+static uint_fast8_t i2c_async_read_wait(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config *config = &i2c_slv->i2c_config;
+
+    if (TWCR & (1 << TWINT)) {
+        i2c_slv->buf[i2c_slv->cur] = TWDR;
+        i2c_slv->cur++;
+        if (i2c_slv->data_len[0] == 0) {
+            TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWSTO);
+            timer->func = i2c_slv->callback;
+        }
+        return i2c_async_read(timer);
+    }
+
+    timer->waketime = timer_read_time() + config->pause;
+    return SF_RESCHEDULE;
+}
+
+static uint_fast8_t i2c_async_read(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config *config = &i2c_slv->i2c_config;
+
+    i2c_slv->data_len[0]--;
+    TWCR = (1<<TWEN) | (1<<TWINT) | ((i2c_slv->data_len[0] ? 1 : 0)<<TWEA);
+
+    timer->func = i2c_async_read_wait;
+    timer->waketime = timer_read_time() + config->pause;
+    return SF_RESCHEDULE;
+}
+
+static uint_fast8_t i2c_async_read_start_wait(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config *config = &i2c_slv->i2c_config;
+    if (TWCR & (1 << TWINT)) {
+        timer->func = i2c_async_read;
+        return i2c_async_read(timer);
+    }
+
+    timer->waketime = timer_read_time() + config->pause;
+    return SF_RESCHEDULE;
+}
+
+static uint_fast8_t i2c_async_start(struct timer *timer);
+
+static uint_fast8_t i2c_async_write_end(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config *config = &i2c_slv->i2c_config;
+    if (TWCR & (1 << TWINT)) {
+        if (i2c_slv->data_len[0] & I2C_R) {
+            return i2c_async_start(timer);
+        }
+        TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWSTO);
+        timer->func = i2c_async;
+    }
+
+    timer->waketime = timer_read_time() + config->pause;
+    return SF_RESCHEDULE;
+}
+
+static uint_fast8_t i2c_async_write(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config *config = &i2c_slv->i2c_config;
+    if (TWCR & (1 << TWINT)) {
+        TWDR = i2c_buf_read_b(i2c_slv);
+        TWCR = (1 << TWEN) | (1 << TWINT);
+        i2c_slv->data_len[0]--;
+        if (i2c_slv->data_len[0] == 0) {
+            i2c_cmd_done(i2c_slv);
+            timer->func = i2c_async_write_end;
+        }
+    }
+
+    timer->waketime = timer_read_time() + config->pause;
+    return SF_RESCHEDULE;
+}
+
+static uint_fast8_t i2c_async_start_wait(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config *config = &i2c_slv->i2c_config;
+    if (TWCR & (1 << TWINT)) {
+        uint32_t status = TWSR;
+        if (status != 0x10 && status != 0x08)
+            shutdown("Failed to send i2c start");
+
+        if (i2c_slv->data_len[0] & I2C_R) {
+            TWDR = config->addr | 0x1;
+            i2c_slv->data_len[0] &= ~(I2C_R);
+            i2c_slv->cur = i2c_slv->tail;
+            timer->func = i2c_async_read_start_wait;
+        } else {
+            TWDR = config->addr;
+            timer->func = i2c_async_write;
+        }
+        TWCR = (1 << TWEN) | (1 << TWINT);
+    }
+
+    timer->waketime = timer_read_time() + config->pause;
+    return SF_RESCHEDULE;
+}
+
+static uint_fast8_t i2c_async_start(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+    struct i2c_config *config = &i2c_slv->i2c_config;
+    TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWSTA);
+    timer->func = i2c_async_start_wait;
+    timer->waketime = timer_read_time() + config->pause;
+    return SF_RESCHEDULE;
+}
+
+uint_fast8_t i2c_async(struct timer *timer)
+{
+    struct i2cdev_s *i2c_slv = container_of(timer, struct i2cdev_s, timer);
+
+    // write register + read
+    if (i2c_slv->data_len[0] || i2c_slv->data_len[1] & I2C_R) {
+        if (i2c_slv->data_len[0] == 0) {
+            // cleanup empty write, start read
+            i2c_cmd_done(i2c_slv);
+        }
+
+        return i2c_async_start(timer);
+    }
+
+    i2c_slv->flags &= ~IF_ACTIVE;
+    return SF_DONE;
+}