diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index 6d7f07286..2e979164b 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -160,6 +160,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ drv/hardware/hardware_nxt.c \ drv/hardware/hardware_stm32.c \ drv/i2c/i2c_ev3.c \ + drv/i2c/i2c_nxt.c \ drv/imu/imu_lsm6ds3tr_c_stm32.c \ drv/ioport/ioport.c \ drv/led/led_array_pwm.c \ diff --git a/lib/pbio/drv/i2c/i2c_nxt.c b/lib/pbio/drv/i2c/i2c_nxt.c new file mode 100644 index 000000000..4180542b0 --- /dev/null +++ b/lib/pbio/drv/i2c/i2c_nxt.c @@ -0,0 +1,177 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2026 The Pybricks Authors + +// I2C driver for NXT +// +// The sensor ports have no I2C peripheral, so the nxos SoftMAC driver bit bangs +// the bus. This translates its asynchronous transaction API to pbdrv. + +#include + +#if PBDRV_CONFIG_I2C_NXT + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include "i2c.h" + +#include "nxos/nxt.h" +#include "nxos/drivers/i2c.h" +#include "nxos/drivers/_sensors.h" + +/** A bit banged transfer of a few bytes at 9.6 kHz takes a handful of ms. */ +#define PBDRV_I2C_NXT_TIMEOUT_MS 500 + +/** + * Minimum bus idle time before a START. The ultrasonic sensor doesn't + * acknowledge its address otherwise (3 ms fails, 5 ms works). The nxos + * i2c_memory.c helpers got this implicitly by polling every 10 ms. + */ +#define PBDRV_I2C_NXT_IDLE_MS 10 + +/** + * ADC channel wired to pin 6 of each sensor port, which is also the I2C data + * line. Must match the adc_p6 values in the platform data. + */ +static const uint8_t pbdrv_i2c_nxt_pin6_adc_channel[NXT_N_SENSORS] = { 1, 2, 3, 7 }; + +struct _pbdrv_i2c_dev_t { + /** Sensor port this bus belongs to. */ + uint8_t port_index; + /** Whether a device is currently registered on this port. */ + bool registered; + /** Expires once the bus has been idle for PBDRV_I2C_NXT_IDLE_MS. */ + pbio_os_timer_t idle_timer; + /** Holds data written to the device. */ + uint8_t write_buf[I2C_MAX_DATA_SIZE]; + /** Holds data read back from the device. */ + uint8_t read_buf[I2C_MAX_DATA_SIZE]; +}; + +static pbdrv_i2c_dev_t i2c_devs[NXT_N_SENSORS]; + +/** Gives pin 6 back to the ADC after a transfer. */ +static void pbdrv_i2c_nxt_release_pins(pbdrv_i2c_dev_t *i2c_dev) { + *AT91C_ADC_CHER = 1 << pbdrv_i2c_nxt_pin6_adc_channel[i2c_dev->port_index]; +} + +pbio_error_t pbdrv_i2c_get_instance(uint8_t id, pbdrv_i2c_dev_t **i2c_dev) { + if (id >= NXT_N_SENSORS) { + return PBIO_ERROR_INVALID_ARG; + } + *i2c_dev = &i2c_devs[id]; + return PBIO_SUCCESS; +} + +pbio_error_t pbdrv_i2c_write_then_read( + pbio_os_state_t *state, + pbdrv_i2c_dev_t *i2c_dev, + uint8_t dev_addr, + const uint8_t *wdata, + size_t wlen, + uint8_t **rdata, + size_t rlen, + bool nxt_quirk) { + + static pbio_os_timer_t timer; + + PBIO_OS_ASYNC_BEGIN(state); + + if (wlen > I2C_MAX_DATA_SIZE || rlen > I2C_MAX_DATA_SIZE) { + return PBIO_ERROR_INVALID_ARG; + } + if (wlen && !wdata) { + return PBIO_ERROR_INVALID_ARG; + } + + // Copy the write data, since the caller only provides it on the first call + // but the SoftMAC driver needs it until the transfer completes. + memcpy(i2c_dev->write_buf, wdata, wlen); + + // Restart the idle time if SCL isn't driven high (e.g. the pins were + // released as inputs). Between transfers SCL stays high, so the time since + // the previous transfer counts. + { + const nx__sensors_pins *pins = nx__sensors_get_pins(i2c_dev->port_index); + if (!(*AT91C_PIOA_OSR & *AT91C_PIOA_ODSR & pins->scl)) { + pbio_os_timer_set(&i2c_dev->idle_timer, PBDRV_I2C_NXT_IDLE_MS); + } + } + + // Re-register for every transfer to reset the port state, or the next + // transfer returns stale data without touching the bus. Registering + // asserts the port is free, hence the unregister. + if (i2c_dev->registered) { + nx_i2c_unregister(i2c_dev->port_index); + } + nx_i2c_register(i2c_dev->port_index, dev_addr, nxt_quirk); + i2c_dev->registered = true; + + // Take pin 6 from the ADC for the transfer (SDA can't be pulled low while + // it's an analog input). Turn the internal pull-ups off, as in LEGO's + // driver (sensors provide them, and with them on, the ultrasonic doesn't + // acknowledge on port 4). + { + const nx__sensors_pins *pins = nx__sensors_get_pins(i2c_dev->port_index); + *AT91C_ADC_CHDR = 1 << pbdrv_i2c_nxt_pin6_adc_channel[i2c_dev->port_index]; + *AT91C_PIOA_PER = pins->scl | pins->sda; + *AT91C_PIOA_PPUDR = pins->scl | pins->sda; + } + + PBIO_OS_AWAIT_UNTIL(state, pbio_os_timer_is_expired(&i2c_dev->idle_timer)); + + pbio_os_timer_set(&timer, PBDRV_I2C_NXT_TIMEOUT_MS); + PBIO_OS_AWAIT_UNTIL(state, !nx_i2c_busy(i2c_dev->port_index) || pbio_os_timer_is_expired(&timer)); + if (nx_i2c_busy(i2c_dev->port_index)) { + pbdrv_i2c_nxt_release_pins(i2c_dev); + return PBIO_ERROR_TIMEDOUT; + } + + // Zero the receive buffer, since the SoftMAC driver ORs received bits in. + memset(i2c_dev->read_buf, 0, rlen); + + if (nx_i2c_start_transaction(i2c_dev->port_index, rlen ? TXN_MODE_READ : TXN_MODE_WRITE, + i2c_dev->write_buf, wlen, i2c_dev->read_buf, rlen) != I2C_ERR_OK) { + pbdrv_i2c_nxt_release_pins(i2c_dev); + return PBIO_ERROR_FAILED; + } + + pbio_os_timer_set(&timer, PBDRV_I2C_NXT_TIMEOUT_MS); + PBIO_OS_AWAIT_UNTIL(state, !nx_i2c_busy(i2c_dev->port_index) || pbio_os_timer_is_expired(&timer)); + + pbio_os_timer_set(&i2c_dev->idle_timer, PBDRV_I2C_NXT_IDLE_MS); + pbdrv_i2c_nxt_release_pins(i2c_dev); + + if (nx_i2c_busy(i2c_dev->port_index)) { + return PBIO_ERROR_TIMEDOUT; + } + + if (nx_i2c_get_txn_status(i2c_dev->port_index) != TXN_STAT_SUCCESS) { + return PBIO_ERROR_IO; + } + + if (rlen) { + *rdata = i2c_dev->read_buf; + } + + PBIO_OS_ASYNC_END(PBIO_SUCCESS); +} + +void pbdrv_i2c_init(void) { + for (uint8_t i = 0; i < NXT_N_SENSORS; i++) { + i2c_devs[i].port_index = i; + } + + nx_i2c_init(); +} + +#endif // PBDRV_CONFIG_I2C_NXT diff --git a/lib/pbio/platform/nxt/nxos/drivers/i2c.c b/lib/pbio/platform/nxt/nxos/drivers/i2c.c index 2bbb75742..d197bfed8 100644 --- a/lib/pbio/platform/nxt/nxos/drivers/i2c.c +++ b/lib/pbio/platform/nxt/nxos/drivers/i2c.c @@ -27,6 +27,9 @@ /** Length of the I2C pauses (in 4th of BUS_SPEED) */ #define I2C_PAUSE_LEN 3 +/** Clock pulses to free a stuck SDA before a START, as in I2C bus recovery. */ +#define I2C_MAX_RECLOCKS 9 + /** Number of currently processed I2C transactions. */ uint32_t i2c_txn_count = 0; @@ -115,6 +118,9 @@ static volatile struct i2c_port { uint8_t p_ticks; uint8_t p_next; + /* Clock pulses issued so far to free SDA before a START. */ + uint8_t reclocks; + } i2c_state[NXT_N_SENSORS]; /* Forward declarations. */ @@ -349,6 +355,18 @@ i2c_txn_status nx_i2c_get_txn_status(uint32_t sensor) p = &i2c_state[sensor]; + /* Once done, current_txn is n_txns (one past the end of txns). Fail if any + * sub transaction failed, since a NAKed address skips the remaining ones + * and leaves them looking successful. + */ + if (p->n_txns && p->current_txn >= p->n_txns) { + for (uint32_t i = 0; i < p->n_txns; i++) { + if (p->txns[i].result != TXN_STAT_SUCCESS) + return p->txns[i].result; + } + return TXN_STAT_SUCCESS; + } + /* If the current sub transaction was left in the FAILED state, * the whole transaction is failed. */ @@ -551,6 +569,7 @@ static void i2c_isr(void) { } /* Prepare the first bit to be sent. */ + p->reclocks = 0; p->processed = 0; p->current_byte = t->data[p->processed]; p->current_pos = 7; @@ -575,11 +594,20 @@ static void i2c_isr(void) { codr |= pins->sda; i2c_set_bus_state(sensor, I2C_SEND_START_BIT1); - } else { + } else if (p->reclocks < I2C_MAX_RECLOCKS) { /* Something is holding SDA low. Reclock until we get our data * line back. */ + p->reclocks++; p->bus_state = I2C_RECLOCK0; + } else { + /* SDA is still low (e.g. no sensor attached, since sensors + * provide the pull-ups). Fail instead of reclocking forever. + */ + t->result = TXN_STAT_FAILED; + p->current_txn = p->n_txns; + p->txn_state = TXN_NONE; + p->bus_state = I2C_IDLE; } break; diff --git a/lib/pbio/platform/nxt/nxos/drivers/sensors.c b/lib/pbio/platform/nxt/nxos/drivers/sensors.c index b33774d1e..88c9d6c95 100644 --- a/lib/pbio/platform/nxt/nxos/drivers/sensors.c +++ b/lib/pbio/platform/nxt/nxos/drivers/sensors.c @@ -59,15 +59,20 @@ void nx__sensors_i2c_enable(uint32_t sensor) { sensors_mode[sensor] = DIGITAL; - /* In digital mode, the DIGI outputs (SDA and SCL) are left up, and - * enabled in multi-drive mode. + /* In digital mode, the DIGI outputs (SDA and SCL) are left up. + * + * SDA is open drain (the slave drives it to acknowledge and return data). + * SCL is push-pull, as in the LEGO firmware (these sensors don't stretch + * the clock). With the internal pull-ups off, the sensor alone can't pull + * an open drain SCL to a valid high on port 4 (shared with RS485). */ pinmask = sensors_pinmap[sensor].sda | sensors_pinmap[sensor].scl; *AT91C_PIOA_OER = pinmask; *AT91C_PIOA_SODR = pinmask; - *AT91C_PIOA_MDER = pinmask; + *AT91C_PIOA_MDER = sensors_pinmap[sensor].sda; + *AT91C_PIOA_MDDR = sensors_pinmap[sensor].scl; } const nx__sensors_pins *nx__sensors_get_pins(uint32_t sensor) { diff --git a/lib/pbio/platform/nxt/pbdrvconfig.h b/lib/pbio/platform/nxt/pbdrvconfig.h index 8401f6499..079b07cc5 100644 --- a/lib/pbio/platform/nxt/pbdrvconfig.h +++ b/lib/pbio/platform/nxt/pbdrvconfig.h @@ -48,6 +48,9 @@ #define PBDRV_CONFIG_HARDWARE (1) #define PBDRV_CONFIG_HARDWARE_NXT (1) +#define PBDRV_CONFIG_I2C (1) +#define PBDRV_CONFIG_I2C_NXT (1) + #define PBDRV_CONFIG_IOPORT (1) #define PBDRV_CONFIG_IOPORT_HAS_ADC (1) #define PBDRV_CONFIG_IOPORT_HAS_UART (0)