Merge remote-tracking branch 'origin/amorale/OP-1218_pios_com_thread_safe' into rel-14.01

flight/modules/RadioComBridge/RadioComBridge.c

@@ -415,7 +415,11 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
                     // Send the data straight to the telemetry port.
                     // FIXME following call can fail (with -2 error code) if buffer is full
                     // it is the caller responsibility to retry in such cases...
-                    PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
+                    int32_t ret = -2;
+                    uint8_t count = 5;
+                    while(count-- > 0 && ret < -1){
+                        ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
+                    }
                 }
             }
         } else {
@@ -511,7 +515,11 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
                 // Send the data over the radio link.
                 // FIXME following call can fail (with -2 error code) if buffer is full
                 // it is the caller responsibility to retry in such cases...
-                PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
+                int32_t ret = -2;
+                uint8_t count = 5;
+                while(count-- > 0 && ret < -1){
+                    PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
+                }
             }
         } else {
             vTaskDelay(5);
@@ -541,7 +549,11 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
     if (outputPort) {
         // FIXME following call can fail (with -2 error code) if buffer is full
         // it is the caller responsibility to retry in such cases...
-        ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        ret = -2;
+        uint8_t count = 5;
+        while(count-- > 0 && ret < -1){
+            ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        }
     } else {
         ret = -1;
     }
@@ -567,7 +579,12 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
     if (outputPort && PIOS_COM_Available(outputPort)) {
         // FIXME following call can fail (with -2 error code) if buffer is full
         // it is the caller responsibility to retry in such cases...
-        return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        int32_t ret = -2;
+        uint8_t count = 5;
+        while(count-- > 0 && ret < -1){
+            ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        }
+        return ret;
     } else {
         return -1;
     }

flight/pios/common/pios_com.c

@@ -51,6 +51,7 @@ struct pios_com_dev {
 #if defined(PIOS_INCLUDE_FREERTOS)
     xSemaphoreHandle tx_sem;
     xSemaphoreHandle rx_sem;
+    xSemaphoreHandle sendbuffer_sem;
 #endif
 
     bool has_rx;
@@ -155,6 +156,9 @@ int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, ui
 #endif /* PIOS_INCLUDE_FREERTOS */
         (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev);
     }
+#if defined(PIOS_INCLUDE_FREERTOS)
+    com_dev->sendbuffer_sem = xSemaphoreCreateMutex();
+#endif /* PIOS_INCLUDE_FREERTOS */
 
     *com_id = (uint32_t)com_dev;
     return 0;
@@ -267,6 +271,40 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
     return 0;
 }
 
+
+static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len)
+{
+PIOS_Assert(com_dev);
+PIOS_Assert(com_dev->has_tx);
+if (com_dev->driver->available && !com_dev->driver->available(com_dev->lower_id)) {
+    /*
+     * Underlying device is down/unconnected.
+     * Dump our fifo contents and act like an infinite data sink.
+     * Failure to do this results in stale data in the fifo as well as
+     * possibly having the caller block trying to send to a device that's
+     * no longer accepting data.
+     */
+    fifoBuf_clearData(&com_dev->tx);
+    return len;
+}
+
+if (len > fifoBuf_getFree(&com_dev->tx)) {
+    /* Buffer cannot accept all requested bytes (retry) */
+    return -2;
+}
+
+uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
+
+if (bytes_into_fifo > 0) {
+    /* More data has been put in the tx buffer, make sure the tx is started */
+    if (com_dev->driver->tx_start) {
+        com_dev->driver->tx_start(com_dev->lower_id,
+                                  fifoBuf_getUsed(&com_dev->tx));
+    }
+}
+return bytes_into_fifo;
+}
+
 /**
  * Sends a package over given port
  * \param[in] port COM port
@@ -275,6 +313,8 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
  * \return -1 if port not available
  * \return -2 if non-blocking mode activated: buffer is full
  *            caller should retry until buffer is free again
+ * \return -3 another thread is already sending, caller should
+ *            retry until com is available again
  * \return number of bytes transmitted on success
  */
 int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
@@ -285,39 +325,19 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
         /* Undefined COM port for this board (see pios_board.c) */
         return -1;
     }
-
+#if defined(PIOS_INCLUDE_FREERTOS)
-    PIOS_Assert(com_dev->has_tx);
+    if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) {
-
+        return -3;
-    if (com_dev->driver->available && !com_dev->driver->available(com_dev->lower_id)) {
-        /*
-         * Underlying device is down/unconnected.
-         * Dump our fifo contents and act like an infinite data sink.
-         * Failure to do this results in stale data in the fifo as well as
-         * possibly having the caller block trying to send to a device that's
-         * no longer accepting data.
-         */
-        fifoBuf_clearData(&com_dev->tx);
-        return len;
     }
-
+#endif /* PIOS_INCLUDE_FREERTOS */
-    if (len > fifoBuf_getFree(&com_dev->tx)) {
+    int32_t ret = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, len);
-        /* Buffer cannot accept all requested bytes (retry) */
+#if defined(PIOS_INCLUDE_FREERTOS)
-        return -2;
+    xSemaphoreGive(com_dev->sendbuffer_sem);
-    }
+#endif /* PIOS_INCLUDE_FREERTOS */
-
+    return ret;
-    uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
-
-    if (bytes_into_fifo > 0) {
-        /* More data has been put in the tx buffer, make sure the tx is started */
-        if (com_dev->driver->tx_start) {
-            com_dev->driver->tx_start(com_dev->lower_id,
-                                      fifoBuf_getUsed(&com_dev->tx));
-        }
-    }
-
-    return bytes_into_fifo;
 }
 
+
 /**
  * Sends a package over given port
  * (blocking function)
@@ -325,6 +345,7 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
  * \param[in] buffer character buffer
  * \param[in] len buffer length
  * \return -1 if port not available
+ * \return -2 if mutex can't be taken;
  * \return number of bytes transmitted on success
  */
 int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
@@ -335,9 +356,12 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
         /* Undefined COM port for this board (see pios_board.c) */
         return -1;
     }
-
     PIOS_Assert(com_dev->has_tx);
-
+#if defined(PIOS_INCLUDE_FREERTOS)
+    if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) {
+        return -2;
+    }
+#endif /* PIOS_INCLUDE_FREERTOS */
     uint32_t max_frag_len  = fifoBuf_getSize(&com_dev->tx);
     uint32_t bytes_to_send = len;
     while (bytes_to_send) {
@@ -348,13 +372,16 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
         } else {
             frag_size = bytes_to_send;
         }
-        int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size);
+        int32_t rc = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, frag_size);
         if (rc >= 0) {
             bytes_to_send -= rc;
             buffer += rc;
         } else {
             switch (rc) {
             case -1:
+#if defined(PIOS_INCLUDE_FREERTOS)
+                xSemaphoreGive(com_dev->sendbuffer_sem);
+#endif /* PIOS_INCLUDE_FREERTOS */
                 /* Device is invalid, this will never work */
                 return -1;
 
@@ -367,17 +394,23 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
                 }
 #if defined(PIOS_INCLUDE_FREERTOS)
                 if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) {
+                    xSemaphoreGive(com_dev->sendbuffer_sem);
                     return -3;
                 }
 #endif
                 continue;
             default:
                 /* Unhandled return code */
+#if defined(PIOS_INCLUDE_FREERTOS)
+                xSemaphoreGive(com_dev->sendbuffer_sem);
+#endif /* PIOS_INCLUDE_FREERTOS */
                 return rc;
             }
         }
     }
-
+#if defined(PIOS_INCLUDE_FREERTOS)
+    xSemaphoreGive(com_dev->sendbuffer_sem);
+#endif /* PIOS_INCLUDE_FREERTOS */
     return len;
 }