ble: Interprete command sent to M4

nucleo-wb55-dongle-ble/Core/Src/main.c

@@ -25,7 +25,6 @@
 /* USER CODE BEGIN Includes */
 #include <stdio.h>
 
-#include "usbd_cdc_if.h"
 #include "commands.h"
 /* USER CODE END Includes */
 
@@ -58,9 +57,7 @@ static void MX_GPIO_Init(void);
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
-void usb_receive(uint8_t *buf, uint32_t *len) {
-  CDC_Transmit_FS(buf, *len);  // echo
-}
+
 /* USER CODE END 0 */
 
 /**
@@ -104,7 +101,10 @@ int main(void)
   	HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET);
   	HAL_Delay(50);
   	HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET);
-  	HAL_Delay(100);
+
+    pop_and_execute_commands();
+
+  	HAL_Delay(450);
     counter_value++;
 
   	log_debug("Counter value is %d", 1, counter_value);
@@ -235,10 +235,6 @@ static void MX_GPIO_Init(void)
 }
 
 /* USER CODE BEGIN 4 */
-int _write(int file, char *ptr, int len) {
-	CDC_Transmit_FS((uint8_t*)ptr, len);
-	return len;
-}
 /* USER CODE END 4 */
 
 /**

nucleo-wb55-dongle-ble/USB_Device/App/usbd_cdc_if.c

@@ -129,7 +129,7 @@ static int8_t CDC_Receive_FS(uint8_t* pbuf, uint32_t *Len);
 static int8_t CDC_TransmitCplt_FS(uint8_t *pbuf, uint32_t *Len, uint8_t epnum);
 
 /* USER CODE BEGIN PRIVATE_FUNCTIONS_DECLARATION */
-__weak void usb_receive(uint8_t *buf, uint32_t *len) {}
+//__weak void usb_receive(uint8_t *buf, uint32_t *len) {}
 /* USER CODE END PRIVATE_FUNCTIONS_DECLARATION */
 
 /**

nucleo-wb55-dongle-ble/app/Command.cpp

@@ -4,10 +4,12 @@
  *  Created on: Jul 8, 2021
  *      Author: Andreas Berthoud
  */
+#include <queue>
+#include <cstring>
 
 #include "Command.hpp"
 #include "string.h"
-#include "usbd_cdc_if.h"
+#include "commands.hpp"
 
 Command::Command(CommandId id) : id(id) {
   this->data[0] = id;
@@ -15,14 +17,31 @@ Command::Command(CommandId id) : id(id) {
   this->payload_ptr = this->data + 4;
  }
 
-void Command::send() {
-  uint16_t size = this->payload_length + 5;
-  this->data[size-1] = 0xff;
-  CDC_Transmit_FS(this->data, size);
-}
-
 void Command::set_payload_length(uint16_t payload_length) {
   this->payload_length = payload_length;
   data[1] = (payload_length & 0xFF00) >> 8;
   data[2] = payload_length & 0x00FF;
 }
+
+uint16_t Command::get_payload_length() {
+  return this->payload_length;
+}
+
+std::queue<Command*> command_queue;
+
+// TODO: Add limit and return false if queue is full. Otherwise, we will get a full heap and crash :-(
+void push_command(Command * command) {
+  command_queue.push(command);
+}
+
+void pop_and_execute_commands() {
+  while (!command_queue.empty()) {
+    Command * command = command_queue.front();
+    bool was_successful = command->execute();
+    if (!was_successful) {
+      log_error("Execution of command with ID %n was not successful!", 1, command->id);
+    }
+    delete command;
+    command_queue.pop();
+  }
+}

nucleo-wb55-dongle-ble/app/Command.hpp

@@ -10,6 +10,8 @@
 
 #include <stdint.h>
 
+extern "C" void pop_and_execute_commands();
+
 typedef enum : uint8_t {
     COMMAND_NONE = 0,
     COMMAND_LOG = 0xff,
@@ -21,8 +23,9 @@ public:
   CommandId id; 
 
   Command(CommandId command_id);
+  virtual ~Command() {};
 
-  virtual void send();
+  virtual bool execute() = 0;
 
 protected:
   uint8_t data[512];
@@ -30,10 +33,13 @@ protected:
   const int max_payload_length = 507;
 
   void set_payload_length(uint16_t length);
+  uint16_t get_payload_length();
 
 private:
   uint16_t payload_length;
 
 };
 
+void push_command(Command * command);
+
 #endif /* SRC_COMMAND_H_ */

nucleo-wb55-dongle-ble/app/LogCommand.cpp

@@ -9,43 +9,36 @@
 #include <string.h>
 #include <string>
 
+#include "usbd_cdc_if.h"
 #include "LogCommand.hpp"
 
 
 void log_debug(const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    LogCommand command = LogCommand(format, args, LOG_LEVEL_DEBUG);
+    push_command(new LogCommand(format, args, LOG_LEVEL_DEBUG));
     va_end(args);
-
-    command.send();
 }
 
 void log_info(const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    LogCommand command = LogCommand(format, args, LOG_LEVEL_INFO);
+    push_command(new LogCommand(format, args, LOG_LEVEL_INFO));
     va_end(args);
-
-    command.send();
 }
 
 void log_warning(const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    LogCommand command = LogCommand(format, args, LOG_LEVEL_WARNING);
+    push_command(new LogCommand(format, args, LOG_LEVEL_WARNING));
     va_end(args);
-
-    command.send();
 }
 
 void log_error(const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    LogCommand command = LogCommand(format, args, LOG_LEVEL_ERROR);
+    push_command(new LogCommand(format, args, LOG_LEVEL_ERROR));
     va_end(args);
-
-    command.send();
 }
 
 LogCommand::LogCommand(const char * format, va_list args, LoggingLevel logging_level) : Command(COMMAND_LOG) {
@@ -53,3 +46,10 @@ LogCommand::LogCommand(const char * format, va_list args, LoggingLevel logging_l
     vsnprintf((char *)this->payload_ptr + 1, this->max_payload_length - 1, format, args);
     this->set_payload_length(strlen((char *)this->payload_ptr) + 1);  // strlen + log level
 }
+
+bool LogCommand::execute() {
+  uint16_t size = this->get_payload_length() + 5;  // number of bytes to be sent over USB
+  this->data[size-1] = 0xff;  // set stop byte
+  uint8_t result = CDC_Transmit_FS(this->data, size);
+  return result == USBD_OK || result == USBD_BUSY;
+}

nucleo-wb55-dongle-ble/app/LogCommand.hpp

@@ -24,7 +24,15 @@ typedef enum : uint8_t{
 
 class LogCommand : public Command {
 public:
+
+  /*
+   * Create log command from formatted string 
+   */
   LogCommand(const char * format, va_list args, LoggingLevel logging_level);
+  virtual ~LogCommand() {};
+
+  bool execute() override;
+  
 };
 
 #endif /* LOGCOMMAND_H_ */

nucleo-wb55-dongle-ble/app/command_interpreter.c

@@ -0,0 +1,30 @@
+/*
+ * command_interpreter.c
+ *
+ *  Created on: Jul 10, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#include "usbd_cdc_if.h"
+#include "commands.h"
+
+void usb_receive(uint8_t *buf, uint32_t *len) {
+
+  CDC_Transmit_FS(buf, *len);  // echo
+
+  if (*len < 5) {
+    return;
+  }
+  uint8_t command_id = buf[0];
+  uint16_t length = buf[1] << 8 | buf[2];
+  uint8_t * payload_ptr = buf + 4;
+  uint8_t stop_byte = 0x1;
+
+  if (*len >= 4 + length) {
+    stop_byte = buf[4+ length];
+  }
+
+  log_debug("received command, id: 0x%x", 1, command_id);
+  log_debug("length: %n", 1, length);
+  log_debug("stop_byte: 0x%x", 1, stop_byte);
+}

nucleo-wb55-dongle-ble/app/commands.h

@@ -8,6 +8,8 @@
 #ifndef COMMANDS_H_
 #define COMMANDS_H_
 
+void pop_and_execute_commands();
+
 void log_debug(const char * format, int nargs, ...);
 void log_info(const char * format, int nargs, ...);
 void log_warning(const char * format, int nargs, ...);

nucleo-wb55-dongle-ble/app/commands.hpp

@@ -0,0 +1,16 @@
+/*
+ * commands.hpp
+ *
+ *  Created on: Jul 13, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#ifndef COMMANDS_HPP_
+#define COMMANDS_HPP_
+
+extern "C" void log_debug(const char * format, int nargs, ...);
+extern "C" void log_info(const char * format, int nargs, ...);
+extern "C" void log_warning(const char * format, int nargs, ...);
+extern "C" void log_error(const char * format, int nargs, ...);
+
+#endif /* COMMANDS_HPP_ */