From ebcb76de2503af1cb28f98ed3479fc81da9410e8 Mon Sep 17 00:00:00 2001
From: Nate Karstens <nate.karstens@gmail.com>
Date: Wed, 22 Nov 2023 23:00:00 -0600
Subject: [PATCH] Configure USB device and process pybricks commands

Configures the USB device and processes pybricks commands.
---
 lib/pbio/drv/usb/usb_stm32.c | 101 ++++++++++++++++++++++++++++++++++-
 1 file changed, 100 insertions(+), 1 deletion(-)

diff --git a/lib/pbio/drv/usb/usb_stm32.c b/lib/pbio/drv/usb/usb_stm32.c
index e7ab1fd5b..1817a25fd 100644
--- a/lib/pbio/drv/usb/usb_stm32.c
+++ b/lib/pbio/drv/usb/usb_stm32.c
@@ -8,22 +8,33 @@
 #if PBDRV_CONFIG_USB_STM32F4
 
 #include <stdbool.h>
+#include <string.h>
 
 #include <contiki.h>
 #include <stm32f4xx_hal.h>
 #include <stm32f4xx_hal_pcd_ex.h>
 #include <usbd_core.h>
+#include <usbd_pybricks.h>
 
 #include <pbdrv/usb.h>
 #include <pbio/util.h>
+#include <pbsys/command.h>
 
 #include "../charger/charger.h"
 #include "./usb_stm32.h"
 
+#define UUID_SZ     (128 / 8)
+
 PROCESS(pbdrv_usb_process, "USB");
 
+static uint8_t usb_in_buf[USBD_PYBRICKS_MAX_PACKET_SIZE];
+static uint8_t usb_out_buf[USBD_PYBRICKS_MAX_PACKET_SIZE];
+static volatile uint32_t usb_in_sz;
+static volatile uint32_t usb_out_sz;
+
 static USBD_HandleTypeDef husbd;
 static PCD_HandleTypeDef hpcd;
+
 static volatile bool vbus_active;
 static pbdrv_usb_bcd_t pbdrv_usb_bcd;
 
@@ -121,6 +132,75 @@ void pbdrv_usb_stm32_handle_vbus_irq(bool active) {
     process_poll(&pbdrv_usb_process);
 }
 
+/**
+  * @brief  Pybricks_Itf_Init
+  *         Initializes the Pybricks media low layer
+  * @param  None
+  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+  */
+static int8_t Pybricks_Itf_Init(void) {
+    USBD_Pybricks_SetTxBuffer(&husbd, usb_out_buf, sizeof(usb_out_buf));
+    USBD_Pybricks_SetRxBuffer(&husbd, usb_in_buf);
+    usb_in_sz = 0;
+    usb_out_sz = 0;
+
+    return USBD_OK;
+}
+
+/**
+  * @brief  Pybricks_Itf_DeInit
+  *         DeInitializes the Pybricks media low layer
+  * @param  None
+  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+  */
+static int8_t Pybricks_Itf_DeInit(void) {
+    return USBD_OK;
+}
+
+/**
+  * @brief  Pybricks_Itf_DataRx
+  *         Data received over USB OUT endpoint are sent over CDC interface
+  *         through this function.
+  * @param  Buf: Buffer of data to be transmitted
+  * @param  Len: Number of data received (in bytes)
+  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+  */
+static int8_t Pybricks_Itf_Receive(uint8_t *Buf, uint32_t *Len) {
+    if (*Len > sizeof(usb_in_buf)) {
+        return USBD_FAIL;
+    }
+
+    memcpy(usb_in_buf, Buf, *Len);
+    usb_in_sz = *Len;
+    process_poll(&pbdrv_usb_process);
+    return USBD_OK;
+}
+
+/**
+  * @brief  Pybricks_Itf_TransmitCplt
+  *         Data transmitted callback
+  *
+  * @note
+  *         This function is IN transfer complete callback used to inform user that
+  *         the submitted Data is successfully sent over USB.
+  *
+  * @param  Buf: Buffer of data to be received
+  * @param  Len: Number of data received (in bytes)
+  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
+  */
+static int8_t Pybricks_Itf_TransmitCplt(uint8_t *Buf, uint32_t *Len, uint8_t epnum) {
+    usb_out_sz = 0;
+    process_poll(&pbdrv_usb_process);
+    return USBD_OK;
+}
+
+static USBD_Pybricks_ItfTypeDef USBD_Pybricks_fops = {
+    .Init = Pybricks_Itf_Init,
+    .DeInit = Pybricks_Itf_DeInit,
+    .Receive = Pybricks_Itf_Receive,
+    .TransmitCplt = Pybricks_Itf_TransmitCplt,
+};
+
 // Common USB driver implementation.
 
 void pbdrv_usb_init(void) {
@@ -128,7 +208,11 @@ void pbdrv_usb_init(void) {
     husbd.pData = &hpcd;
     hpcd.pData = &husbd;
 
-    USBD_Init(&husbd, NULL, 0);
+    USBD_Init(&husbd, &Pybricks_Desc, 0);
+    USBD_RegisterClass(&husbd, &USBD_Pybricks_ClassDriver);
+    USBD_Pybricks_RegisterInterface(&husbd, &USBD_Pybricks_fops);
+    USBD_Start(&husbd);
+
     process_start(&pbdrv_usb_process);
 
     // VBUS may already be active
@@ -164,12 +248,27 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) {
 
     PROCESS_BEGIN();
 
+    // Prepare to receive the first packet
+    USBD_Pybricks_ReceivePacket(&husbd);
+
     for (;;) {
         PROCESS_WAIT_EVENT();
 
         if (bcd_busy) {
             bcd_busy = PT_SCHEDULE(pbdrv_usb_stm32_bcd_detect(&bcd_pt));
         }
+
+        if (usb_in_sz) {
+            if ((usb_in_sz >= UUID_SZ) &&
+                pbio_uuid128_le_compare(usb_in_buf, pbio_pybricks_command_event_char_uuid)) {
+                pbsys_command(usb_in_buf + UUID_SZ, usb_in_sz - UUID_SZ);
+            }
+
+            usb_in_sz = 0;
+
+            // Prepare to receive the next packet
+            USBD_Pybricks_ReceivePacket(&husbd);
+        }
     }
 
     PROCESS_END();