OP-950: Adds "higher priority task woken" logic to SPI and SPI bus resident

sensor device drivers. Based partly on code contributed by lilvinz.

+review OPReview

flight/pios/common/pios_bma180.c

@@ -128,10 +128,11 @@ int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_
  */
 int32_t PIOS_BMA180_ClaimBus()
 {
-	if(PIOS_BMA180_Validate(dev) != 0)
+	if (PIOS_BMA180_Validate(dev) != 0) {
 		return -1;
+	}
 
-	if(PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
+	if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
 		return -1;
 	}
 
@@ -141,19 +142,22 @@ int32_t PIOS_BMA180_ClaimBus()
 }
 
 /**
- * @brief Claim the SPI bus for the accel communications and select this chip
+ * @brief Claim the SPI bus for the accel communications and select this chip. Call from an ISR.
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
  * @return 0 if successful, -1 if unable to claim bus
  */
-int32_t PIOS_BMA180_ClaimBusISR()
+int32_t PIOS_BMA180_ClaimBusISR(bool *woken)
 {
-	if(PIOS_BMA180_Validate(dev) != 0)
+	if (PIOS_BMA180_Validate(dev) != 0) {
-		return -1;
-
-	if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0) {
 		return -1;
 	}
 
-	PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
+	if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
+		return -1;
+	}
+
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 	return 0;
 }
 
@@ -162,13 +166,29 @@ int32_t PIOS_BMA180_ClaimBusISR()
  * @return 0 if successful
  */
 int32_t PIOS_BMA180_ReleaseBus()
+{
+	if (PIOS_BMA180_Validate(dev) != 0) {
+		return -1;
+	}
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
+
+	return PIOS_SPI_ReleaseBus(dev->spi_id);
+}
+
+/**
+ * @brief Release the SPI bus for the accel communications and end the transaction. Call from an ISR
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
+ * @return 0 if successful
+ */
+int32_t PIOS_BMA180_ReleaseBusISR(bool *woken)
 {
 	if(PIOS_BMA180_Validate(dev) != 0)
 		return -1;
 
-	PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
 
-	return PIOS_SPI_ReleaseBus(dev->spi_id);
+	return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
 }
 
 /**
@@ -431,19 +451,21 @@ int32_t PIOS_BMA180_Test()
 }
 
 /**
- * @brief IRQ Handler.  Read data from the BMA180 FIFO and push onto a local fifo.
+ * @brief EXTI IRQ Handler.  Read data from the BMA180 FIFO and push onto a local fifo.
+ * @return a boolean to the EXTI IRQ Handler wrapper indicating if a
+ *         higher priority task is now eligible to run
  */
 int32_t bma180_irqs = 0;
 bool PIOS_BMA180_IRQHandler(void)
 {
-	bma180_irqs++;
+	bool woken = false;
-	
 	static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
 	uint8_t pios_bma180_dmabuf[8];
-
+	bma180_irqs++;
+	
 	// If we can't get the bus then just move on for efficiency
-	if(PIOS_BMA180_ClaimBusISR() != 0) {
+	if (PIOS_BMA180_ClaimBusISR(&woken) != 0) {
-		return false; // Something else is using bus, miss this data
+		return woken; // Something else is using bus, miss this data
 	}
 		
 	PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, 
@@ -453,11 +475,12 @@ bool PIOS_BMA180_IRQHandler(void)
 	struct pios_bma180_data data;
 	
 	// Don't release bus till data has copied
-	PIOS_BMA180_ReleaseBus();	
+	PIOS_BMA180_ReleaseBusISR(&woken);
 	
 	// Must not return before releasing bus
-	if(fifoBuf_getFree(&dev->fifo) < sizeof(data))
+	if (fifoBuf_getFree(&dev->fifo) < sizeof(data)) {
-		return false;
+		return woken;
+	}
 	
 	// Bottom two bits indicate new data and are constant zeros.  Don't right 
 	// shift because it drops sign bit
@@ -471,7 +494,7 @@ bool PIOS_BMA180_IRQHandler(void)
 	
 	fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data));
 	
-	return false;
+	return woken;
 }
 
 #endif /* PIOS_INCLUDE_BMA180 */

flight/pios/common/pios_l3gd20.c

@@ -60,9 +60,11 @@ static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev * dev);
 static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg);
 static int32_t PIOS_L3GD20_SetReg(uint8_t address, uint8_t buffer);
 static int32_t PIOS_L3GD20_GetReg(uint8_t address);
+static int32_t PIOS_L3GD20_GetRegISR(uint8_t address, bool *woken);
 static int32_t PIOS_L3GD20_ClaimBus();
-static int32_t PIOS_L3GD20_ClaimBusIsr();
+static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken);
 static int32_t PIOS_L3GD20_ReleaseBus();
+static int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken);
 
 volatile bool l3gd20_configured = false;
 
@@ -191,17 +193,19 @@ static int32_t PIOS_L3GD20_ClaimBus()
 
 /**
  * @brief Claim the SPI bus for the accel communications and select this chip
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
  */
-static int32_t PIOS_L3GD20_ClaimBusIsr()
+static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken)
 {
-	if(PIOS_L3GD20_Validate(dev) != 0)
+	if(PIOS_L3GD20_Validate(dev) != 0) {
 		return -1;
-
+	}
-	if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0)
+	if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
 		return -2;
-
+	}
-	PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 	return 0;
 }
 
@@ -211,14 +215,28 @@ static int32_t PIOS_L3GD20_ClaimBusIsr()
  */
 int32_t PIOS_L3GD20_ReleaseBus()
 {
-	if(PIOS_L3GD20_Validate(dev) != 0)
+	if(PIOS_L3GD20_Validate(dev) != 0) {
 		return -1;
-
+	}
-	PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
-
 	return PIOS_SPI_ReleaseBus(dev->spi_id);
 }
 
+/**
+ * @brief Release the SPI bus for the accel communications and end the transaction
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
+ * @return 0 if successful, -1 for invalid device
+ */
+int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken)
+{
+	if(PIOS_L3GD20_Validate(dev) != 0) {
+		return -1;
+	}
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
+	return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
+}
+
 /**
  * @brief Read a register from L3GD20
  * @returns The register value or -1 if failure to get bus
@@ -238,6 +256,26 @@ static int32_t PIOS_L3GD20_GetReg(uint8_t reg)
 	return data;
 }
 
+/**
+ * @brief Read a register from L3GD20 in an ISR context
+ * @param reg[in] Register address to be read
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
+ * @return The register value or -1 if failure to get bus
+ */
+static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken)
+{
+	uint8_t data;
+
+	if(PIOS_L3GD20_ClaimBusISR(woken) != 0) {
+		return -1;
+	}
+	PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
+	data = PIOS_SPI_TransferByte(dev->spi_id,0 );     // receive response
+	PIOS_L3GD20_ReleaseBusISR(woken);
+	return data;
+}
+
 /**
  * @brief Writes one byte to the L3GD20
  * \param[in] reg Register address
@@ -349,34 +387,38 @@ uint8_t PIOS_L3GD20_Test(void)
 }
 
 /**
-* @brief IRQ Handler.  Read all the data from onboard buffer
+* @brief EXTI IRQ Handler.  Read all the data from onboard buffer
+ * @return a boolean to the EXTI IRQ Handler wrapper indicating if a
+ *         higher priority task is now eligible to run
 */
 bool PIOS_L3GD20_IRQHandler(void)
 {
-	l3gd20_irq++;
+	bool woken = false;
-
 	struct pios_l3gd20_data data;
 	uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0};
 	uint8_t rec[7];
 
+	l3gd20_irq++;
+
 	/* This code duplicates ReadGyros above but uses ClaimBusIsr */
-	if(PIOS_L3GD20_ClaimBusIsr() != 0)
+	if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) {
-		return false;
+		return woken;
-	
-	if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
-		PIOS_L3GD20_ReleaseBus();
-		return false;
 	}
 	
-	PIOS_L3GD20_ReleaseBus();
+	if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
+		PIOS_L3GD20_ReleaseBusISR(&woken);
+		return woken;
+	}
+	
+	PIOS_L3GD20_ReleaseBusISR(&woken);
 	
 	memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6);
-	data.temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP);
+	data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken);
 	
-	portBASE_TYPE xHigherPriorityTaskWoken;
+	signed portBASE_TYPE higherPriorityTaskWoken;
-	xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken);
+	xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken);
 	
-	return xHigherPriorityTaskWoken == pdTRUE;
+	return (woken || higherPriorityTaskWoken == pdTRUE);
 }
 
 #endif /* PIOS_INCLUDE_L3GD20 */

flight/pios/common/pios_mpu6000.c

@@ -197,7 +197,12 @@ int32_t PIOS_MPU6000_ConfigureRanges(
 {
 	if(dev == NULL)
 		return -1;
+
+	// TODO: check that changing the SPI clock speed is safe
+	// to do here given that interrupts are enabled and the bus has
+	// not been claimed/is not claimed during this call
 	PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
+
 	// update filter settings
 	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0);
 	
@@ -225,39 +230,63 @@ int32_t PIOS_MPU6000_ConfigureRanges(
 /**
  * @brief Claim the SPI bus for the accel communications and select this chip
  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
- * @param fromIsr[in] Tells if the function is being called from a ISR or not
  */
-static int32_t PIOS_MPU6000_ClaimBus(bool fromIsr)
+static int32_t PIOS_MPU6000_ClaimBus()
 {
-	if(PIOS_MPU6000_Validate(dev) != 0)
+	if(PIOS_MPU6000_Validate(dev) != 0) {
 		return -1;
-	if(fromIsr){
-        if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0)
-            return -2;
-	} else {
-        if(PIOS_SPI_ClaimBus(dev->spi_id) != 0)
-            return -2;
 	}
-	PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
+	if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
+		return -2;
+	}
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
+	return 0;
+}
+
+/**
+ * @brief Claim the SPI bus for the accel communications and select this chip
+ * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
+ */
+static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
+{
+	if (PIOS_MPU6000_Validate(dev) != 0) {
+		return -1;
+	}
+	if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
+		return -2;
+	}
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 	return 0;
 }
 
 /**
  * @brief Release the SPI bus for the accel communications and end the transaction
  * @return 0 if successful
- * @param fromIsr[in] Tells if the function is being called from a ISR or not
  */
-int32_t PIOS_MPU6000_ReleaseBus(bool fromIsr)
+static int32_t PIOS_MPU6000_ReleaseBus()
 {
-	if(PIOS_MPU6000_Validate(dev) != 0)
+	if(PIOS_MPU6000_Validate(dev) != 0) {
 		return -1;
-	
-	PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
-	if(fromIsr){
-	    return PIOS_SPI_ReleaseBusISR(dev->spi_id);
-	} else {
-	    return PIOS_SPI_ReleaseBus(dev->spi_id);
 	}
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
+	return PIOS_SPI_ReleaseBus(dev->spi_id);
+}
+
+/**
+ * @brief Release the SPI bus for the accel communications and end the transaction
+ * @return 0 if successful
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
+ */
+static int32_t PIOS_MPU6000_ReleaseBusISR(bool *woken)
+{
+	if(PIOS_MPU6000_Validate(dev) != 0) {
+		return -1;
+	}
+	PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
+	return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
 }
 
 /**
@@ -269,13 +298,14 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
 {
 	uint8_t data;
 	
-	if(PIOS_MPU6000_ClaimBus(false) != 0)
+	if(PIOS_MPU6000_ClaimBus() != 0) {
 		return -1;	
+	}
 	
 	PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
 	data = PIOS_SPI_TransferByte(dev->spi_id,0 );     // receive response
 	
-	PIOS_MPU6000_ReleaseBus(false);
+	PIOS_MPU6000_ReleaseBus();
 	return data;
 }
 
@@ -289,20 +319,21 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
  */
 static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
 {
-	if(PIOS_MPU6000_ClaimBus(false) != 0)
+	if (PIOS_MPU6000_ClaimBus() != 0) {
 		return -1;
+	}
 	
-	if(PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) {
+	if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) {
-		PIOS_MPU6000_ReleaseBus(false);
+		PIOS_MPU6000_ReleaseBus();
 		return -2;
 	}
 	
-	if(PIOS_SPI_TransferByte(dev->spi_id, data) != 0) {
+	if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) {
-		PIOS_MPU6000_ReleaseBus(false);
+		PIOS_MPU6000_ReleaseBus();
 		return -3;
 	}
-	
+
-	PIOS_MPU6000_ReleaseBus(false);
+	PIOS_MPU6000_ReleaseBus();
 	
 	return 0;
 }
@@ -318,13 +349,15 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data)
 	uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0};
 	uint8_t rec[7];
 	
-	if(PIOS_MPU6000_ClaimBus(false) != 0)
+	if (PIOS_MPU6000_ClaimBus() != 0) {
 		return -1;
+	}
 
-	if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0)
+	if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
 		return -2;
-		
+	}
-	PIOS_MPU6000_ReleaseBus(false);
+
+	PIOS_MPU6000_ReleaseBus();
 	
 	data->gyro_x = rec[1] << 8 | rec[2];
 	data->gyro_y = rec[3] << 8 | rec[4];
@@ -407,31 +440,34 @@ int32_t PIOS_MPU6000_Test(void)
 }
 
 /**
- * @brief Run self-test operation.
+ * @brief Obtains the number of bytes in the FIFO. Call from ISR only.
- * \return 0 if test succeeded
+ * @return the number of bytes in the FIFO
- * \return non-zero value if test succeeded
+ * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
- * @param fromIsr[in] Tells if the function is being called from a ISR or not
+ *                      task has is now eligible to run, else unchanged
  */
-static int32_t PIOS_MPU6000_FifoDepth(bool fromIsr)
+static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
 {
 	uint8_t mpu6000_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0};
 	uint8_t mpu6000_rec_buf[3];
 
-	if(PIOS_MPU6000_ClaimBus(fromIsr) != 0)
+	if(PIOS_MPU6000_ClaimBusISR(woken) != 0) {
-		return -1;
-
-	if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
-		PIOS_MPU6000_ReleaseBus(fromIsr);
 		return -1;
 	}
 
-	PIOS_MPU6000_ReleaseBus(fromIsr);
+	if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
+		PIOS_MPU6000_ReleaseBusISR(woken);
+		return -1;
+	}
+
+	PIOS_MPU6000_ReleaseBusISR(woken);
 	
 	return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2];
 }
 
 /**
-* @brief IRQ Handler.  Read all the data from onboard buffer
+* @brief EXTI IRQ Handler.  Read all the data from onboard buffer
+* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
+*         higher priority task is now eligible to run
 */
 uint32_t mpu6000_irq = 0;
 int32_t mpu6000_count;
@@ -446,46 +482,50 @@ uint32_t mpu6000_transfer_size;
 
 bool PIOS_MPU6000_IRQHandler(void)
 {
+	bool woken = false;
 	static uint32_t timeval;
 	mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
 	timeval = PIOS_DELAY_GetRaw();
 
-	if (!mpu6000_configured)
+	if (!mpu6000_configured) {
 		return false;
+	}
 
-	mpu6000_count = PIOS_MPU6000_FifoDepth(true);
+	mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken);
-	if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data))
+	if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data)) {
-		return false;
+		return woken;
+	}
 
-	if (PIOS_MPU6000_ClaimBus(true) != 0)
+	if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
-		return false;
+		return woken;
+	}
 
 	static uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data) ] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
 	static uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data) ];
 
 	if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
-		PIOS_MPU6000_ReleaseBus(true);
+		PIOS_MPU6000_ReleaseBusISR(&woken);
 		mpu6000_fails++;
-		return false;
+		return woken;
 	}
 
-	PIOS_MPU6000_ReleaseBus(true);
+	PIOS_MPU6000_ReleaseBusISR(&woken);
 
 	static struct pios_mpu6000_data data;
 
 	// In the case where extras samples backed up grabbed an extra
 	if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) {
 		mpu6000_fifo_backup++;
-		if (PIOS_MPU6000_ClaimBus(true) != 0)
+		if (PIOS_MPU6000_ClaimBusISR(&woken) != 0)
-			return false;
+			return woken;
 
 		if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
-			PIOS_MPU6000_ReleaseBus(true);
+			PIOS_MPU6000_ReleaseBusISR(&woken);
 			mpu6000_fails++;
-			return false;
+			return woken;
 		}
 
-		PIOS_MPU6000_ReleaseBus(true);
+		PIOS_MPU6000_ReleaseBusISR(&woken);
 	}
 
 	// Rotate the sensor to OP convention.  The datasheet defines X as towards the right
@@ -548,14 +588,14 @@ bool PIOS_MPU6000_IRQHandler(void)
 	data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
 #endif
 
-	portBASE_TYPE xHigherPriorityTaskWoken;
+	signed portBASE_TYPE higherPriorityTaskWoken;
-	xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken);
+	xQueueSendToBackFromISR(dev->queue, (void *) &data, &higherPriorityTaskWoken);
 
 	mpu6000_irq++;
 
 	mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
 
-	return xHigherPriorityTaskWoken == pdTRUE;
+	return (woken || higherPriorityTaskWoken == pdTRUE);
 }
 
 #endif /* PIOS_INCLUDE_MPU6000 */

flight/pios/inc/pios_spi.h

@@ -49,9 +49,9 @@ extern int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b);
 extern int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback);
 extern int32_t PIOS_SPI_Busy(uint32_t spi_id);
 extern int32_t PIOS_SPI_ClaimBus(uint32_t spi_id);
-extern int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id);
+extern int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken);
 extern int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id);
-extern int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id);
+extern int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken);
 extern void    PIOS_SPI_IRQ_Handler(uint32_t spi_id);
 extern void    PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescalar);
 

flight/pios/stm32f10x/pios_spi.c

@@ -265,22 +265,33 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id)
 /**
  * Claim the SPI bus semaphore from an ISR.  Has no timeout.
  * \param[in] spi SPI number (0 or 1)
+ * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
  * \return 0 if no error
  * \return -1 if timeout before claiming semaphore
  */
-int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id)
+int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken)
 {
 #if defined(PIOS_INCLUDE_FREERTOS)
-	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
+ 	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
-	
+    signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
+
 	bool valid = PIOS_SPI_validate(spi_dev);
 	PIOS_Assert(valid)
 	
-	if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){
+	if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){
 		return -1;
 	}
-#endif
+	if (woken) {
+		*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
+	}
 	return 0;
+#else
+	if (woken) {
+		*woken = false;
+	}
+	return PIOS_SPI_ClaimBus(spi_id);
+#endif
 }
 
 /**
@@ -302,7 +313,6 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
 	PIOS_IRQ_Disable();
 	spi_dev->busy = 0;
 	PIOS_IRQ_Enable();
-	
 #endif
 	return 0;
 }
@@ -310,25 +320,30 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
 /**
  * Release the SPI bus semaphore from ISR.  Calling the SPI functions does not require this
  * \param[in] spi SPI number (0 or 1)
+ * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
  * \return 0 if no error
  */
-int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id)
+int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken)
 {
 #if defined(PIOS_INCLUDE_FREERTOS)
     struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
+	signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
 
     bool valid = PIOS_SPI_validate(spi_dev);
     PIOS_Assert(valid)
 
-    xSemaphoreGiveFromISR(spi_dev->busy, NULL);
+    xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken);
+    if (woken) {
+    	*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
+    }
+	return 0;
 #else
-    struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
+	if (woken) {
-    PIOS_IRQ_Disable();
+		*woken = false;
-    spi_dev->busy = 0;
+	}
-    PIOS_IRQ_Enable();
+	return PIOS_SPI_ReleaseBus(spi_id);
-
 #endif
-    return 0;
 }
 
 

flight/pios/stm32f4xx/pios_spi.c

@@ -257,6 +257,18 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id)
 
 	if (xSemaphoreTake(spi_dev->busy, 0xffff) != pdTRUE)
 		return -1;
+#else
+	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
+	uint32_t timeout = 0xffff;
+	while((PIOS_SPI_Busy(spi_id) || spi_dev->busy) && --timeout);
+	if(timeout == 0) //timed out
+		return -1;
+	
+	PIOS_IRQ_Disable();
+	if(spi_dev->busy)
+		return -1;
+	spi_dev->busy = 1;
+	PIOS_IRQ_Enable();
 #endif
 	return 0;
 }
@@ -264,25 +276,35 @@ int32_t PIOS_SPI_ClaimBus(uint32_t spi_id)
 /**
  * Claim the SPI bus semaphore from an ISR.  Has no timeout.
  * \param[in] spi SPI number (0 or 1)
+ * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
  * \return 0 if no error
  * \return -1 if timeout before claiming semaphore
  */
-int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id)
+int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id, bool *woken)
 {
 #if defined(PIOS_INCLUDE_FREERTOS)
-	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
+ 	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
-	
+    signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
+
 	bool valid = PIOS_SPI_validate(spi_dev);
 	PIOS_Assert(valid)
 	
-	if (xSemaphoreTakeFromISR(spi_dev->busy, NULL) != pdTRUE){
+	if (xSemaphoreTakeFromISR(spi_dev->busy, &higherPriorityTaskWoken) != pdTRUE){
 		return -1;
 	}
-#endif
+	if (woken) {
+		*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
+	}
 	return 0;
+#else
+	if (woken) {
+		*woken = false;
+	}
+	return PIOS_SPI_ClaimBus(spi_id);
+#endif
 }
 
-
 /**
  * Release the SPI bus semaphore.  Calling the SPI functions does not require this
  * \param[in] spi SPI number (0 or 1)
@@ -297,6 +319,11 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
 	PIOS_Assert(valid)
 
 	xSemaphoreGive(spi_dev->busy);
+#else
+	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
+	PIOS_IRQ_Disable();
+	spi_dev->busy = 0;
+	PIOS_IRQ_Enable();
 #endif
 	return 0;
 }
@@ -304,21 +331,33 @@ int32_t PIOS_SPI_ReleaseBus(uint32_t spi_id)
 /**
  * Release the SPI bus semaphore from ISR.  Calling the SPI functions does not require this
  * \param[in] spi SPI number (0 or 1)
+ * \param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
+ *                      task has is now eligible to run, else unchanged
  * \return 0 if no error
  */
-int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id)
+int32_t PIOS_SPI_ReleaseBusISR(uint32_t spi_id, bool *woken)
 {
 #if defined(PIOS_INCLUDE_FREERTOS)
     struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
+	signed portBASE_TYPE higherPriorityTaskWoken = pdFALSE;
 
     bool valid = PIOS_SPI_validate(spi_dev);
     PIOS_Assert(valid)
 
-    xSemaphoreGiveFromISR(spi_dev->busy, NULL);
+    xSemaphoreGiveFromISR(spi_dev->busy, &higherPriorityTaskWoken);
+    if (woken) {
+    	*woken = *woken || (higherPriorityTaskWoken == pdTRUE);
+    }
+	return 0;
+#else
+	if (woken) {
+		*woken = false;
+	}
+	return PIOS_SPI_ReleaseBus(spi_id);
 #endif
-    return 0;
 }
 
+
 /**
 * Controls the RC (Register Clock alias Chip Select) pin of a SPI port
 * \param[in] spi SPI number (0 or 1)