altos: Grab SPI mutex until MPU6000 I2C mode is disabled

If other drivers use the SPI bus, the MPU6000 gets confused as its
sitting on the bus looking for I2C messages. Just grab the mutex
before the OS is running and hold onto it until the MPU6000 has been initialized.

Signed-off-by: Keith Packard <keithp@keithp.com>

1 files changed, 156 insertions, 109 deletions

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index fc768cc9..73057820 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -24,81 +24,89 @@
 static uint8_t	ao_mpu6000_wake;
 static uint8_t	ao_mpu6000_configured;
 
-#define ao_mpu6000_spi_get() 	ao_spi_get_bit(AO_MPU6000_SPI_CS_PORT,	\
-					       AO_MPU6000_SPI_CS_PIN,	\
-					       AO_MPU6000_SPI_CS,	\
-					       AO_MPU6000_SPI_BUS,	\
-					       AO_SPI_SPEED_1MHz)
+#ifndef AO_MPU6000_I2C_INDEX
+#define AO_MPU6000_SPI	1
+#else
+#define AO_MPU6000_SPI	0
+#endif
+
+#if AO_MPU6000_SPI
 
-#define ao_mpu6000_spi_put() 	ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT,	\
-					       AO_MPU6000_SPI_CS_PIN,	\
-					       AO_MPU6000_SPI_CS,	\
-					       AO_MPU6000_SPI_BUS)
+#define ao_mpu6000_spi_get()	ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz)
+#define ao_mpu6000_spi_put()	ao_spi_put(AO_MPU6000_SPI_BUS)
+
+#define ao_mpu6000_spi_start() 	ao_spi_set_cs(AO_MPU6000_SPI_CS_PORT,	\
+					      (1 << AO_MPU6000_SPI_CS_PIN))
+
+#define ao_mpu6000_spi_end() 	ao_spi_clr_cs(AO_MPU6000_SPI_CS_PORT,	\
+					      (1 << AO_MPU6000_SPI_CS_PIN))
+
+#endif
 
 
 static void
-ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
+_ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
 {
 	uint8_t	d[2] = { addr, value };
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+	ao_mpu6000_spi_start();
+	ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_end();
+#else
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
 	ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
-	ao_mpu6000_spi_get();
-	ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
-	ao_mpu6000_spi_put();
 #endif
 }
 
 static void
-ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
+_ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
 {
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+	addr |= 0x80;
+	ao_mpu6000_spi_start();
+	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+	ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_end();
+#else
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
 	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
 	ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
-	addr |= 0x80;
-	ao_mpu6000_spi_get();
-	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
-	ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
-	ao_mpu6000_spi_put();
 #endif
 }
 
 static uint8_t
-ao_mpu6000_reg_read(uint8_t addr)
+_ao_mpu6000_reg_read(uint8_t addr)
 {
 	uint8_t	value;
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+	addr |= 0x80;
+	ao_mpu6000_spi_start();
+	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+	ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_end();
+#else
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
 	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
 	ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
-	addr |= 0x80;
-	ao_mpu6000_spi_get();
-	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
-	ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
-	ao_mpu6000_spi_put();
 #endif
 	return value;
 }
 
 static void
-ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
+_ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
 {
 	uint16_t	*d = (uint16_t *) sample;
 	int		i = sizeof (*sample) / 2;
 
-	ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
+	_ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
 #if __BYTE_ORDER == __LITTLE_ENDIAN
 	/* byte swap */
 	while (i--) {
@@ -153,7 +161,22 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
 }
 
 static void
-ao_mpu6000_setup(void)
+_ao_mpu6000_wait_alive(void)
+{
+	uint8_t	i;
+
+	/* Wait for the chip to wake up */
+	for (i = 0; i < 30; i++) {
+		ao_delay(AO_MS_TO_TICKS(100));
+		if (_ao_mpu6000_reg_read(MPU6000_WHO_AM_I) == 0x68)
+			break;
+	}
+	if (i == 30)
+		ao_panic(AO_PANIC_SELF_TEST_MPU6000);
+}
+
+static void
+_ao_mpu6000_setup(void)
 {
 	struct ao_mpu6000_sample	normal_mode, test_mode;
 	int				errors =0;
@@ -161,104 +184,107 @@ ao_mpu6000_setup(void)
 	if (ao_mpu6000_configured)
 		return;
 
+	_ao_mpu6000_wait_alive();
+
 	/* Reset the whole chip */
 	
-	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
-			     (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
+	_ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			      (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
 
 	/* Wait for it to reset. If we talk too quickly, it appears to get confused */
-	ao_delay(AO_MS_TO_TICKS(100));
 
-	/* Reset signal conditioning */
-	ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
-			     (0 << MPU6000_USER_CONTROL_FIFO_EN) |
-			     (0 << MPU6000_USER_CONTROL_I2C_MST_EN) |
-			     (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) |
-			     (0 << MPU6000_USER_CONTROL_FIFO_RESET) |
-			     (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) |
-			     (1 << MPU6000_USER_CONTROL_SIG_COND_RESET));
+	_ao_mpu6000_wait_alive();
+
+	/* Reset signal conditioning, disabling I2C on SPI systems */
+	_ao_mpu6000_reg_write(MPU6000_USER_CTRL,
+			      (0 << MPU6000_USER_CTRL_FIFO_EN) |
+			      (0 << MPU6000_USER_CTRL_I2C_MST_EN) |
+			      (AO_MPU6000_SPI << MPU6000_USER_CTRL_I2C_IF_DIS) |
+			      (0 << MPU6000_USER_CTRL_FIFO_RESET) |
+			      (0 << MPU6000_USER_CTRL_I2C_MST_RESET) |
+			      (1 << MPU6000_USER_CTRL_SIG_COND_RESET));
 
-	while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET))
-		ao_yield();
+	while (_ao_mpu6000_reg_read(MPU6000_USER_CTRL) & (1 << MPU6000_USER_CTRL_SIG_COND_RESET))
+		ao_delay(AO_MS_TO_TICKS(10));
 
 	/* Reset signal paths */
-	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
-			     (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
-			     (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
-			     (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+	_ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			      (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			      (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			      (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
 
-	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
-			     (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
-			     (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
-			     (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+	_ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			      (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			      (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			      (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
 
 	/* Select clocks, disable sleep */
-	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
-			     (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
-			     (0 << MPU6000_PWR_MGMT_1_SLEEP) |
-			     (0 << MPU6000_PWR_MGMT_1_CYCLE) |
-			     (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
-			     (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
+	_ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			      (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
+			      (0 << MPU6000_PWR_MGMT_1_SLEEP) |
+			      (0 << MPU6000_PWR_MGMT_1_CYCLE) |
+			      (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
+			      (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
 
-	/* Set sample rate divider to sample at full speed
-	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
+	/* Set sample rate divider to sample at full speed */
+	_ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
 
 	/* Disable filtering */
-	ao_mpu6000_reg_write(MPU6000_CONFIG,
-			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
-			     (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
+	_ao_mpu6000_reg_write(MPU6000_CONFIG,
+			      (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			      (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
 
 	/* Configure accelerometer to +/-16G in self-test mode */
-	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
-			     (1 << MPU600_ACCEL_CONFIG_XA_ST) |
-			     (1 << MPU600_ACCEL_CONFIG_YA_ST) |
-			     (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
-			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+	_ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+			      (1 << MPU600_ACCEL_CONFIG_XA_ST) |
+			      (1 << MPU600_ACCEL_CONFIG_YA_ST) |
+			      (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
+			      (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
 
 	/* Configure gyro to +/- 2000°/s in self-test mode */
-	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
-			     (1 << MPU600_GYRO_CONFIG_XG_ST) |
-			     (1 << MPU600_GYRO_CONFIG_YG_ST) |
-			     (1 << MPU600_GYRO_CONFIG_ZG_ST) |
-			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+	_ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+			      (1 << MPU600_GYRO_CONFIG_XG_ST) |
+			      (1 << MPU600_GYRO_CONFIG_YG_ST) |
+			      (1 << MPU600_GYRO_CONFIG_ZG_ST) |
+			      (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
 
 	ao_delay(AO_MS_TO_TICKS(200));
-	ao_mpu6000_sample(&test_mode);
+	_ao_mpu6000_sample(&test_mode);
 
 #if TRIDGE
 	// read the product ID rev c has 1/2 the sensitivity of rev d
-    _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
-    //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
-
-    if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
-        (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
-        // Accel scale 8g (4096 LSB/g)
-        // Rev C has different scaling than rev D
-        register_write(MPUREG_ACCEL_CONFIG,1<<3);
-    } else {
-        // Accel scale 8g (4096 LSB/g)
-        register_write(MPUREG_ACCEL_CONFIG,2<<3);
-    }
-    hal.scheduler->delay(1);
+	_mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
+	//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
+
+	if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
+	    (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
+		// Accel scale 8g (4096 LSB/g)
+		// Rev C has different scaling than rev D
+		register_write(MPUREG_ACCEL_CONFIG,1<<3);
+	} else {
+		// Accel scale 8g (4096 LSB/g)
+		register_write(MPUREG_ACCEL_CONFIG,2<<3);
+	}
+	hal.scheduler->delay(1);
 
 #endif
 
 	/* Configure accelerometer to +/-16G */
-	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
-			     (0 << MPU600_ACCEL_CONFIG_XA_ST) |
-			     (0 << MPU600_ACCEL_CONFIG_YA_ST) |
-			     (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
-			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+	_ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+			      (0 << MPU600_ACCEL_CONFIG_XA_ST) |
+			      (0 << MPU600_ACCEL_CONFIG_YA_ST) |
+			      (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
+			      (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
 
 	/* Configure gyro to +/- 2000°/s */
-	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
-			     (0 << MPU600_GYRO_CONFIG_XG_ST) |
-			     (0 << MPU600_GYRO_CONFIG_YG_ST) |
-			     (0 << MPU600_GYRO_CONFIG_ZG_ST) |
-			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+	_ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+			      (0 << MPU600_GYRO_CONFIG_XG_ST) |
+			      (0 << MPU600_GYRO_CONFIG_YG_ST) |
+			      (0 << MPU600_GYRO_CONFIG_ZG_ST) |
+			      (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
 
 	ao_delay(AO_MS_TO_TICKS(10));
-	ao_mpu6000_sample(&normal_mode);
+	_ao_mpu6000_sample(&normal_mode);
 	
 	errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
 	errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
@@ -272,13 +298,13 @@ ao_mpu6000_setup(void)
 		ao_panic(AO_PANIC_SELF_TEST_MPU6000);
 
 	/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
-	ao_mpu6000_reg_write(MPU6000_CONFIG,
-			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
-			     (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
+	_ao_mpu6000_reg_write(MPU6000_CONFIG,
+			      (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			      (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
 
 	/* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */
-	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
-			     1000 / 200 - 1);
+	_ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
+			      1000 / 200 - 1);
 	
 	ao_delay(AO_MS_TO_TICKS(100));
 	ao_mpu6000_configured = 1;
@@ -289,10 +315,20 @@ struct ao_mpu6000_sample	ao_mpu6000_current;
 static void
 ao_mpu6000(void)
 {
-	ao_mpu6000_setup();
+	/* ao_mpu6000_init already grabbed the SPI bus and mutex */
+	_ao_mpu6000_setup();
+#if AO_MPU6000_SPI
+	ao_mpu6000_spi_put();
+#endif
 	for (;;)
 	{
-		ao_mpu6000_sample(&ao_mpu6000_current);
+#if AO_MPU6000_SPI
+		ao_mpu6000_spi_get();
+#endif
+		_ao_mpu6000_sample(&ao_mpu6000_current);
+#if AO_MPU6000_SPI
+		ao_mpu6000_spi_put();
+#endif	
 		ao_arch_critical(
 			AO_DATA_PRESENT(AO_DATA_MPU6000);
 			AO_DATA_WAIT();
@@ -328,9 +364,20 @@ ao_mpu6000_init(void)
 	ao_mpu6000_configured = 0;
 
 	ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
-#ifndef AO_MPU6000_I2C_INDEX
+	
+#if AO_MPU6000_SPI
 	ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN));
+
+	/* Pretend to be the mpu6000 task. Grab the SPI bus right away and
+	 * hold it for the task so that nothing else uses the SPI bus before
+	 * we get the I2C mode disabled in the chip
+	 */
+
+	ao_cur_task = &ao_mpu6000_task;
+	ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz);
+	ao_cur_task = NULL;
 #endif	
+
 	ao_cmd_register(&ao_mpu6000_cmds[0]);
 }
 #endif