Merged in mindnever/librepilot/LP-413_add_PIOS_COM_RegisterAvailableCallback (pull request #333)

LP-413 This commit adds PIOS_COM_RegisterAvailableCallback() function.

flight/pios/common/pios_com.c

@@ -741,6 +741,31 @@ uint32_t PIOS_COM_Available(uint32_t com_id)
     return (com_dev->driver->available)(com_dev->lower_id);
 }
 
+/*
+ * Set available callback
+ * \param[in] port COM port
+ * \param[in] available_cb Callback function
+ * \param[in] context context to pass to the callback function
+ * \return -1 if port not available
+ * \return 0 on success
+ */
+int32_t PIOS_COM_RegisterAvailableCallback(uint32_t com_id, pios_com_callback_available available_cb, uint32_t context)
+{
+    struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
+
+    if (!PIOS_COM_validate(com_dev)) {
+        /* Undefined COM port for this board (see pios_board.c) */
+        return -1;
+    }
+
+    /* Invoke the driver function if it exists */
+    if (com_dev->driver->bind_available_cb) {
+        com_dev->driver->bind_available_cb(com_dev->lower_id, available_cb, context);
+    }
+
+    return 0;
+}
+
 #endif /* PIOS_INCLUDE_COM */
 
 /**

flight/pios/inc/pios_com.h

@@ -38,6 +38,7 @@
 typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken);
 typedef void (*pios_com_callback_ctrl_line)(uint32_t context, uint32_t mask, uint32_t state);
 typedef void (*pios_com_callback_baud_rate)(uint32_t context, uint32_t baud);
+typedef void (*pios_com_callback_available)(uint32_t context, uint32_t available);
 
 struct pios_com_driver {
     void     (*init)(uint32_t id);
@@ -50,6 +51,7 @@ struct pios_com_driver {
     void     (*bind_ctrl_line_cb)(uint32_t id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context);
     void     (*bind_baud_rate_cb)(uint32_t id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context);
     uint32_t (*available)(uint32_t id);
+    void     (*bind_available_cb)(uint32_t id, pios_com_callback_available available_cb, uint32_t context);
 };
 
 /* Control line definitions */
@@ -72,6 +74,7 @@ extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const ch
 extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...);
 extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t *buf, uint16_t buf_len, uint32_t timeout_ms);
 extern uint32_t PIOS_COM_Available(uint32_t com_id);
+extern int32_t PIOS_COM_RegisterAvailableCallback(uint32_t com_id, pios_com_callback_available, uint32_t context);
 
 /* Event driven asynchronous API */
 extern int32_t PIOS_COM_ASYNC_TxStart(uint32_t id, uint16_t tx_bytes_avail);

flight/pios/inc/pios_usb.h

@@ -38,6 +38,7 @@ extern int32_t PIOS_USB_ChangeConnectionState(bool connected);
 extern bool PIOS_USB_CableConnected(uint8_t id);
 extern bool PIOS_USB_CheckAvailable(uint32_t id);
 extern void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void));
+extern void PIOS_USB_RegisterConnectionStateCallback(void (*connectionStateCallback)(bool connected, uint32_t context), uint32_t context);
 #endif /* PIOS_USB_H */
 
 /**

flight/pios/stm32f10x/pios_usb.c

@@ -38,11 +38,16 @@
 
 #include "pios_usb_priv.h"
 
-#ifdef PIOS_INCLUDE_USB_HID
-
 /* Rx/Tx status */
 static bool transfer_possible = false;
 
+#ifdef PIOS_INCLUDE_FREERTOS
+struct {
+    void     (*callback)(bool connected, uint32_t context);
+    uint32_t context;
+} connectionState_cb_list[3];
+#endif
+
 enum pios_usb_dev_magic {
     PIOS_USB_DEV_MAGIC = 0x17365904,
 };
@@ -51,6 +56,9 @@ struct pios_usb_dev {
     enum pios_usb_dev_magic   magic;
     const struct pios_usb_cfg *cfg;
 };
+#ifdef PIOS_INCLUDE_FREERTOS
+static void raiseConnectionStateCallback(bool connected);
+#endif
 
 /**
  * @brief Validate the usb device structure
@@ -175,6 +183,10 @@ int32_t PIOS_USB_ChangeConnectionState(bool Connected)
 #endif
     }
 
+#ifdef PIOS_INCLUDE_FREERTOS
+    raiseConnectionStateCallback(Connected);
+#endif
+
     return 0;
 }
 
@@ -242,7 +254,36 @@ bool PIOS_USB_CheckAvailable(uint32_t id)
     return PIOS_USB_CableConnected(id) && transfer_possible;
 }
 
-#endif /* PIOS_INCLUDE_USB_HID */
+#ifdef PIOS_INCLUDE_FREERTOS
+void PIOS_USB_RegisterConnectionStateCallback(void (*connectionStateCallback)(bool connected, uint32_t context), uint32_t context)
+{
+    PIOS_Assert(connectionStateCallback);
+
+    for (uint32_t i = 0; i < NELEMENTS(connectionState_cb_list); i++) {
+        if (connectionState_cb_list[i].callback == NULL) {
+            connectionState_cb_list[i].callback = connectionStateCallback;
+            connectionState_cb_list[i].context  = context;
+            return;
+        }
+    }
+
+    PIOS_Assert(0);
+}
+
+static void raiseConnectionStateCallback(bool connected)
+{
+    uint32_t i = 0;
+
+    while (i < NELEMENTS(connectionState_cb_list) && connectionState_cb_list[i].callback != NULL) {
+        connectionState_cb_list[i].callback(connected, connectionState_cb_list[i].context);
+        i++;
+    }
+}
+#else /* PIOS_INCLUDE_FREERTOS */
+void PIOS_USB_RegisterConnectionStateCallback(__attribute__((unused)) void (*connectionStateCallback)(bool connected, uint32_t context), __attribute__((unused)) uint32_t context)
+{}
+#endif /* PIOS_INCLUDE_FREERTOS */
+
 
 #endif /* PIOS_INCLUDE_USB */
 

flight/pios/stm32f10x/pios_usb_cdc.c

@@ -42,6 +42,8 @@
 static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callback tx_out_cb, uint32_t context);
 static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context);
 static void PIOS_USB_CDC_RegisterBaudRateCallback(uint32_t usbcdc_id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context);
+static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available baud_rate_cb, uint32_t context);
+static void PIOS_USB_CDC_ChangeConnectionState(bool connected, uint32_t usbcdc_id);
 static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail);
 static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail);
 static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id);
@@ -53,6 +55,7 @@ const struct pios_com_driver pios_usb_cdc_com_driver = {
     .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback,
     .bind_baud_rate_cb = PIOS_USB_CDC_RegisterBaudRateCallback,
     .available  = PIOS_USB_CDC_Available,
+    .bind_available_cb = PIOS_USB_CDC_RegisterAvailableCallback,
 };
 
 enum pios_usb_cdc_dev_magic {
@@ -72,6 +75,8 @@ struct pios_usb_cdc_dev {
 
     pios_com_callback_baud_rate baud_rate_cb;
     uint32_t baud_rate_context;
+    pios_com_callback_available available_cb;
+    uint32_t available_context;
 
     uint8_t  rx_packet_buffer[PIOS_USB_BOARD_CDC_DATA_LENGTH];
     /*
@@ -157,6 +162,8 @@ int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cf
     pEpInt_IN[cfg->data_tx_ep - 1]  = PIOS_USB_CDC_DATA_EP_IN_Callback;
     pEpInt_OUT[cfg->data_rx_ep - 1] = PIOS_USB_CDC_DATA_EP_OUT_Callback;
 
+    PIOS_USB_RegisterConnectionStateCallback(PIOS_USB_CDC_ChangeConnectionState, (uint32_t)usb_cdc_dev);
+
     *usbcdc_id = (uint32_t)usb_cdc_dev;
 
     return 0;
@@ -366,8 +373,7 @@ static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id)
 
     PIOS_Assert(valid);
 
-    return (PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) &&
+    return PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE;
-            (control_line_state & USB_CDC_CONTROL_LINE_STATE_DTE_PRESENT)) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE;
 }
 
 static struct usb_cdc_line_coding line_coding = {
@@ -487,5 +493,33 @@ void PIOS_USB_CDC_SetLineCoding_Completed()
     }
 }
 
+static void PIOS_USB_CDC_ChangeConnectionState(__attribute__((unused)) bool connected, uint32_t usbcdc_id)
+{
+    struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id;
+
+    bool valid = PIOS_USB_CDC_validate(usb_cdc_dev);
+
+    PIOS_Assert(valid);
+
+    if (usb_cdc_dev->available_cb) {
+        (usb_cdc_dev->available_cb)(usb_cdc_dev->available_context, PIOS_USB_CDC_Available(usbcdc_id));
+    }
+}
+
+static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available available_cb, uint32_t context)
+{
+    struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id;
+
+    bool valid = PIOS_USB_CDC_validate(usb_cdc_dev);
+
+    PIOS_Assert(valid);
+
+    /*
+     * Order is important in these assignments since ISR uses _cb
+     * field to determine if it's ok to dereference _cb and _context
+     */
+    usb_cdc_dev->available_context = context;
+    usb_cdc_dev->available_cb = available_cb;
+}
 
 #endif /* PIOS_INCLUDE_USB_CDC */

flight/pios/stm32f4xx/pios_usb.c

@@ -40,8 +40,15 @@
 /* Rx/Tx status */
 static uint8_t transfer_possible = 0;
 
+#ifdef PIOS_INCLUDE_FREERTOS
 static void(*disconnection_cb_list[3]) (void);
 
+struct {
+    void     (*callback)(bool connected, uint32_t context);
+    uint32_t context;
+} connectionState_cb_list[3];
+#endif
+
 enum pios_usb_dev_magic {
     PIOS_USB_DEV_MAGIC = 0x17365904,
 };
@@ -55,6 +62,7 @@ struct pios_usb_dev {
 };
 #ifdef PIOS_INCLUDE_FREERTOS
 static void raiseDisconnectionCallbacks(void);
+static void raiseConnectionStateCallback(bool connected);
 #endif
 /**
  * @brief Validate the usb device structure
@@ -158,6 +166,10 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected)
 #endif
     }
 
+#ifdef PIOS_INCLUDE_FREERTOS
+    raiseConnectionStateCallback(connected);
+#endif
+
     return 0;
 }
 
@@ -204,6 +216,7 @@ bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id)
  * Register a physical disconnection callback
  *
  */
+#ifdef PIOS_INCLUDE_FREERTOS
 void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void))
 {
     PIOS_Assert(disconnectionCB);
@@ -215,7 +228,6 @@ void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void))
     }
     PIOS_Assert(0);
 }
-#ifdef PIOS_INCLUDE_FREERTOS
 static void raiseDisconnectionCallbacks(void)
 {
     uint32_t i = 0;
@@ -224,7 +236,38 @@ static void raiseDisconnectionCallbacks(void)
         (disconnection_cb_list[i++])();
     }
 }
-#endif
+
+void PIOS_USB_RegisterConnectionStateCallback(void (*connectionStateCallback)(bool connected, uint32_t context), uint32_t context)
+{
+    PIOS_Assert(connectionStateCallback);
+
+    for (uint32_t i = 0; i < NELEMENTS(connectionState_cb_list); i++) {
+        if (connectionState_cb_list[i].callback == NULL) {
+            connectionState_cb_list[i].callback = connectionStateCallback;
+            connectionState_cb_list[i].context  = context;
+            return;
+        }
+    }
+
+    PIOS_Assert(0);
+}
+
+static void raiseConnectionStateCallback(bool connected)
+{
+    uint32_t i = 0;
+
+    while (i < NELEMENTS(connectionState_cb_list) && connectionState_cb_list[i].callback != NULL) {
+        connectionState_cb_list[i].callback(connected, connectionState_cb_list[i].context);
+        i++;
+    }
+}
+#else /* PIOS_INCLUDE_FREERTOS */
+void PIOS_USB_RegisterDisconnectionCallback(__attribute__((unused)) void (*disconnectionCB)(void))
+{}
+void PIOS_USB_RegisterConnectionStateCallback(__attribute__((unused)) void (*connectionStateCallback)(bool connected, uint32_t context), __attribute__((unused)) uint32_t context)
+{}
+#endif /* PIOS_INCLUDE_FREERTOS */
+
 /*
  *
  * Provide STM32 USB OTG BSP layer API

flight/pios/stm32f4xx/pios_usb_cdc.c

@@ -42,6 +42,8 @@ static void PIOS_USB_CDC_RegisterTxCallback(uint32_t usbcdc_id, pios_com_callbac
 static void PIOS_USB_CDC_RegisterRxCallback(uint32_t usbcdc_id, pios_com_callback rx_in_cb, uint32_t context);
 static void PIOS_USB_CDC_RegisterCtrlLineCallback(uint32_t usbcdc_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context);
 static void PIOS_USB_CDC_RegisterBaudRateCallback(uint32_t usbcdc_id, pios_com_callback_baud_rate baud_rate_cb, uint32_t context);
+static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available baud_rate_cb, uint32_t context);
+static void PIOS_USB_CDC_ChangeConnectionState(bool connected, uint32_t usbcdc_id);
 static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail);
 static void PIOS_USB_CDC_RxStart(uint32_t usbcdc_id, uint16_t rx_bytes_avail);
 static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id);
@@ -54,6 +56,7 @@ const struct pios_com_driver pios_usb_cdc_com_driver = {
     .bind_ctrl_line_cb = PIOS_USB_CDC_RegisterCtrlLineCallback,
     .bind_baud_rate_cb = PIOS_USB_CDC_RegisterBaudRateCallback,
     .available  = PIOS_USB_CDC_Available,
+    .bind_available_cb = PIOS_USB_CDC_RegisterAvailableCallback,
 };
 
 enum pios_usb_cdc_dev_magic {
@@ -74,6 +77,8 @@ struct pios_usb_cdc_dev {
     uint32_t ctrl_line_context;
     pios_com_callback_baud_rate baud_rate_cb;
     uint32_t baud_rate_context;
+    pios_com_callback_available available_cb;
+    uint32_t available_context;
 
     bool     usb_ctrl_if_enabled;
     bool     usb_data_if_enabled;
@@ -205,6 +210,8 @@ int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cf
     usb_cdc_dev->usb_data_if_enabled = false;
     PIOS_USBHOOK_RegisterIfOps(cfg->data_if, &usb_cdc_data_ifops, (uint32_t)usb_cdc_dev);
 
+    PIOS_USB_RegisterConnectionStateCallback(PIOS_USB_CDC_ChangeConnectionState, (uint32_t)usb_cdc_dev);
+
     *usbcdc_id = (uint32_t)usb_cdc_dev;
 
     return 0;
@@ -390,7 +397,7 @@ static void PIOS_USB_CDC_CTRL_IF_DeInit(uint32_t usb_cdc_id)
     }
 
     /* DeRegister endpoint specific callbacks with the USBHOOK layer */
-    usb_cdc_dev->usb_data_if_enabled = false;
+    usb_cdc_dev->usb_ctrl_if_enabled = false;
 }
 
 static uint8_t cdc_altset;
@@ -472,8 +479,7 @@ static uint32_t PIOS_USB_CDC_Available(uint32_t usbcdc_id)
 
     PIOS_Assert(valid);
 
-    return (PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) &&
+    return PIOS_USB_CheckAvailable(usb_cdc_dev->lower_id) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE;
-            (control_line_state & USB_CDC_CONTROL_LINE_STATE_DTE_PRESENT)) ? COM_AVAILABLE_RXTX : COM_AVAILABLE_NONE;
 }
 
 /**
@@ -721,4 +727,34 @@ static bool PIOS_USB_CDC_DATA_EP_OUT_Callback(
     return rc;
 }
 
+static void PIOS_USB_CDC_ChangeConnectionState(__attribute__((unused)) bool connected, uint32_t usbcdc_id)
+{
+    struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id;
+
+    bool valid = PIOS_USB_CDC_validate(usb_cdc_dev);
+
+    PIOS_Assert(valid);
+
+    if (usb_cdc_dev->available_cb) {
+        (usb_cdc_dev->available_cb)(usb_cdc_dev->available_context, PIOS_USB_CDC_Available(usbcdc_id));
+    }
+}
+
+
+static void PIOS_USB_CDC_RegisterAvailableCallback(uint32_t usbcdc_id, pios_com_callback_available available_cb, uint32_t context)
+{
+    struct pios_usb_cdc_dev *usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id;
+
+    bool valid = PIOS_USB_CDC_validate(usb_cdc_dev);
+
+    PIOS_Assert(valid);
+
+    /*
+     * Order is important in these assignments since ISR uses _cb
+     * field to determine if it's ok to dereference _cb and _context
+     */
+    usb_cdc_dev->available_context = context;
+    usb_cdc_dev->available_cb = available_cb;
+}
+
 #endif /* PIOS_INCLUDE_USB_CDC */