AP_HAL_SMACCM: Panic if taking a semaphore held by current thread.

- Catches bugs with attempted recursive semaphore taking.

libraries/AP_HAL_SMACCM/Semaphores.cpp

@@ -1,10 +1,20 @@
 
 #include "Semaphores.h"
+#include <task.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
 
 using namespace SMACCM;
 
+extern const AP_HAL::HAL& hal;
+
+/** Return true if this thread already holds "sem". */
+static bool already_held(xSemaphoreHandle sem)
+{
+  xTaskHandle self = xTaskGetCurrentTaskHandle();
+  return xSemaphoreGetMutexHolder(sem) == self;
+}
+
 SMACCMSemaphore::SMACCMSemaphore()
   : m_semaphore(NULL)
 {
@@ -19,6 +29,9 @@ bool SMACCMSemaphore::take(uint32_t timeout_ms)
 {
   portTickType delay;
 
+  if (already_held(m_semaphore))
+    hal.scheduler->panic("PANIC: Recursive semaphore take.");
+
   if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER)
     delay = portMAX_DELAY;
   else
@@ -29,6 +42,9 @@ bool SMACCMSemaphore::take(uint32_t timeout_ms)
 
 bool SMACCMSemaphore::take_nonblocking()
 {
+  if (already_held(m_semaphore))
+    hal.scheduler->panic("PANIC: Recursive semaphore take.");
+
   return xSemaphoreTake(m_semaphore, 0);
 }