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>

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index fc768cc9ae1cdc166f4add87866d4e505df101f9..73057820933244555ac40b33c55ada50f4a42da5 100644 (file)
--- 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

diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index f01e9e83b81ce9ca52ff0b2f3e32a9d07af7175d..a42f69c411fd4ab416cbea4bda0bd2400523c203 100644 (file)
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -133,13 +133,13 @@
 #define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET  1
 #define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET   0
 
-#define MPU6000_USER_CONTROL           0x6a
-#define MPU6000_USER_CONTROL_FIFO_EN           6
-#define MPU6000_USER_CONTROL_I2C_MST_EN                5
-#define MPU6000_USER_CONTROL_I2C_IF_DIS                4
-#define MPU6000_USER_CONTROL_FIFO_RESET                2
-#define MPU6000_USER_CONTROL_I2C_MST_RESET     1
-#define MPU6000_USER_CONTROL_SIG_COND_RESET    0
+#define MPU6000_USER_CTRL              0x6a
+#define MPU6000_USER_CTRL_FIFO_EN              6
+#define MPU6000_USER_CTRL_I2C_MST_EN           5
+#define MPU6000_USER_CTRL_I2C_IF_DIS           4
+#define MPU6000_USER_CTRL_FIFO_RESET           2
+#define MPU6000_USER_CTRL_I2C_MST_RESET                1
+#define MPU6000_USER_CTRL_SIG_COND_RESET       0
 
 #define MPU6000_PWR_MGMT_1     0x6b
 #define MPU6000_PWR_MGMT_1_DEVICE_RESET                7

diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h
index 1e78cabc5fbadb9a9519aaf9a63f1321a7a6d7a0..6fe86e62c4dc5718a6c159d1cb15e8324423488c 100644 (file)
--- a/src/stm/ao_arch_funcs.h
+++ b/src/stm/ao_arch_funcs.h
@@ -87,13 +87,16 @@ extern uint16_t     ao_spi_speed[STM_NUM_SPI];
 void
 ao_spi_init(void);
 
+#define ao_spi_set_cs(reg,mask) ((reg)->bsrr = ((uint32_t) (mask)) << 16)
+#define ao_spi_clr_cs(reg,mask) ((reg)->bsrr = (mask))
+
 #define ao_spi_get_mask(reg,mask,bus, speed) do {              \
                ao_spi_get(bus, speed);                         \
-               (reg)->bsrr = ((uint32_t) mask) << 16;  \
+               ao_spi_set_cs(reg,mask);                        \
        } while (0)
 
 #define ao_spi_put_mask(reg,mask,bus) do {     \
-               (reg)->bsrr = mask;             \
+               ao_spi_clr_cs(reg,mask);        \
                ao_spi_put(bus);                \
        } while (0)