3 files changed, 48 insertions, 3 deletions
diff --git a/os/hal/include/hal_serial_usb.h b/os/hal/include/hal_serial_usb.h
index 9d3e016b8..be3579e94 100644
--- a/os/hal/include/hal_serial_usb.h
+++ b/os/hal/include/hal_serial_usb.h
@@ -185,6 +185,7 @@ extern "C" {
   void sduDataTransmitted(USBDriver *usbp, usbep_t ep);
   void sduDataReceived(USBDriver *usbp, usbep_t ep);
   void sduInterruptTransmitted(USBDriver *usbp, usbep_t ep);
+  msg_t sduControl(USBDriver *usbp, unsigned int operation, void *arg);
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/src/hal_serial.c b/os/hal/src/hal_serial.c
index 573b139be..5c24c8216 100644
--- a/os/hal/src/hal_serial.c
+++ b/os/hal/src/hal_serial.c
@@ -101,11 +101,10 @@ static msg_t _ctl(void *ip, unsigned int operation, void *arg) {
   default:
 #if defined(SD_LLD_IMPLEMENTS_CTL)
     return sd_lld_control(sdp, operation, arg);
-#else
+#endif
   case CHN_CTL_INVALID:
     osalDbgAssert(false, "invalid CTL operation");
     break;
-#endif
   }
   return MSG_OK;
 }
diff --git a/os/hal/src/hal_serial_usb.c b/os/hal/src/hal_serial_usb.c
index f3c606942..1af9b50a3 100644
--- a/os/hal/src/hal_serial_usb.c
+++ b/os/hal/src/hal_serial_usb.c
@@ -124,9 +124,35 @@ static size_t _readt(void *ip, uint8_t *bp, size_t n, systime_t timeout) {
   return ibqReadTimeout(&((SerialUSBDriver *)ip)->ibqueue, bp, n, timeout);
 }
 
+static msg_t _ctl(void *ip, unsigned int operation, void *arg) {
+  SerialUSBDriver *sdup = (SerialUSBDriver *)ip;
+
+  osalDbgCheck(sdup != NULL);
+
+  switch (operation) {
+  case CHN_CTL_NOP:
+    osalDbgCheck(arg == NULL);
+    break;
+  default:
+#if defined(SDU_LLD_IMPLEMENTS_CTL)
+    /* The SDU driver does not have a LLD but the application can use this
+       hook to implement extra controls by supplying this function.*/ 
+    extern msg_t sdu_lld_control(SerialUSBDriver *sdup,
+                                 unsigned int operation,
+                                 void *arg);
+    return sdu_lld_control(sdup, operation, arg);
+#endif
+  case CHN_CTL_INVALID:
+    osalDbgAssert(false, "invalid CTL operation");
+    break;
+  }
+  return MSG_OK;
+}
+
 static const struct SerialUSBDriverVMT vmt = {
   _write, _read, _put, _get,
-  _putt, _gett, _writet, _readt
+  _putt, _gett, _writet, _readt,
+  _ctl
 };
 
 /**
@@ -490,6 +516,25 @@ void sduInterruptTransmitted(USBDriver *usbp, usbep_t ep) {
   (void)ep;
 }
 
+/**
+ * @brief   Control operation on a serial USB port.
+ *
+ * @param[in] usbp       pointer to a @p USBDriver object
+ * @param[in] operation control operation code
+ * @param[in,out] arg   operation argument
+ *
+ * @return              The control operation status.
+ * @retval MSG_OK       in case of success.
+ * @retval MSG_TIMEOUT  in case of operation timeout.
+ * @retval MSG_RESET    in case of operation reset.
+ *
+ * @api
+ */
+msg_t sduControl(USBDriver *usbp, unsigned int operation, void *arg) {
+
+  return _ctl((void *)usbp, operation, arg);
+}
+
 #endif /* HAL_USE_SERIAL_USB == TRUE */
 
 /** @} */