From 72afbf8f764994340b4aa21b71ab9d668a7e3ea9 Mon Sep 17 00:00:00 2001
From: Andreas Berthoud <andreasberthoud@gmail.com>
Date: Mon, 19 Jul 2021 11:02:23 +0200
Subject: [PATCH] ble: Implement pop_and_execute_commands()

---
 nucleo-wb55-ble/Core/Src/main.c              |  3 ++-
 nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.c |  9 ++++++-
 nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.h |  2 +-
 nucleo-wb55-ble/commands/dispatch.cpp        | 28 ++++++++++++++++++++
 nucleo-wb55-ble/commands/dispatch.hpp        | 15 +++++++++++
 5 files changed, 54 insertions(+), 3 deletions(-)
 create mode 100644 nucleo-wb55-ble/commands/dispatch.cpp
 create mode 100644 nucleo-wb55-ble/commands/dispatch.hpp

diff --git a/nucleo-wb55-ble/Core/Src/main.c b/nucleo-wb55-ble/Core/Src/main.c
index 79ccee7..e774549 100644
--- a/nucleo-wb55-ble/Core/Src/main.c
+++ b/nucleo-wb55-ble/Core/Src/main.c
@@ -23,7 +23,7 @@
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include "commands.h"
 #include "stm32_lpm.h"
 #include "stm32_seq.h"
 #include "dbg_trace.h"
@@ -114,6 +114,7 @@ int main(void)
   while (1)
   {
     UTIL_SEQ_Run(UTIL_SEQ_DEFAULT);
+    pop_and_execute_commands();
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
diff --git a/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.c b/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.c
index 88aa4e0..015e343 100644
--- a/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.c
+++ b/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.c
@@ -32,7 +32,7 @@
 
 /* USER CODE BEGIN PV */
 /* Private variables ---------------------------------------------------------*/
-
+uint8_t buffer[7];
 /* USER CODE END PV */
 
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
@@ -221,10 +221,16 @@ static int8_t CDC_Control_FS(uint8_t cmd, uint8_t* pbuf, uint16_t length)
   /* 6      | bDataBits  |   1   | Number Data bits (5, 6, 7, 8 or 16).          */
   /*******************************************************************************/
     case CDC_SET_LINE_CODING:
+      for (int i = 0; i< 7; i++) {
+        buffer[i] = pbuf[i];
+      }
 
     break;
 
     case CDC_GET_LINE_CODING:
+      for (int i = 0; i< 7; i++) {
+        pbuf[i] = buffer[i];
+      }
 
     break;
 
@@ -264,6 +270,7 @@ static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len)
   /* USER CODE BEGIN 6 */
   USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
   USBD_CDC_ReceivePacket(&hUsbDeviceFS);
+  usb_receive(Buf, Len);
   return (USBD_OK);
   /* USER CODE END 6 */
 }
diff --git a/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.h b/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.h
index fabb17a..56b3657 100644
--- a/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.h
+++ b/nucleo-wb55-ble/USB_Device/App/usbd_cdc_if.h
@@ -110,7 +110,7 @@ extern USBD_CDC_ItfTypeDef USBD_Interface_fops_FS;
 uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len);
 
 /* USER CODE BEGIN EXPORTED_FUNCTIONS */
-
+__weak void usb_receive(uint8_t *buf, uint32_t *len);
 /* USER CODE END EXPORTED_FUNCTIONS */
 
 /**
diff --git a/nucleo-wb55-ble/commands/dispatch.cpp b/nucleo-wb55-ble/commands/dispatch.cpp
new file mode 100644
index 0000000..2dfcd16
--- /dev/null
+++ b/nucleo-wb55-ble/commands/dispatch.cpp
@@ -0,0 +1,28 @@
+/*
+ * dispatch.cpp
+ *
+ *  Created on: 18 Jul 2021
+ *      Author: Andreas Berthoud
+ */
+
+#include "dispatch.hpp"
+
+#include "HeartbeatCommand.hpp"
+#include "LedCommand.hpp"
+
+void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {
+
+  switch (command_id)
+  {
+  case COMMAND_HEARTBEAT_REQUEST:
+    push_command(new HeartbeatRequest(payload_ptr, size));
+    break;
+  case COMMAND_LED_REQUEST:
+    push_command(new LedRequest(payload_ptr, size));
+    break;
+
+  default:
+    break;
+  }
+
+}
diff --git a/nucleo-wb55-ble/commands/dispatch.hpp b/nucleo-wb55-ble/commands/dispatch.hpp
new file mode 100644
index 0000000..4064550
--- /dev/null
+++ b/nucleo-wb55-ble/commands/dispatch.hpp
@@ -0,0 +1,15 @@
+/*
+ * dispatch.hpp
+ *
+ *  Created on: 18 Jul 2021
+ *      Author: Andreas Berthoud
+ */
+
+#ifndef DISPATCH_HPP_
+#define DISPATCH_HPP_
+
+#include <stdint.h>
+
+extern "C" void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
+
+#endif /* DISPATCH_HPP_ */