Implement full command stack over BLE

backend/monsun_backend/command_endpoint.py

@@ -7,6 +7,7 @@ from flask_api import status
 
 from .command_execution import enqueue_command
 from .commands import CommandId
+from .commands import CommandTarget
 from .commands import get_command_id_from_name
 from .commands import get_request_class
 
@@ -23,6 +24,8 @@ def command(role: str):
     logger.debug(f"arguments: {arguments}")
 
     cmd = arguments.pop("cmd")
+    target = CommandTarget[arguments.pop("target", role)]
+
     try:
         command_id = CommandId(int(cmd))
     except ValueError:
@@ -30,7 +33,7 @@ def command(role: str):
 
     try:
         command = get_request_class(command_id=command_id)(
-            root_logger=logging.getLogger(role), **arguments
+            root_logger=logging.getLogger(role), target=target, **arguments
         )
     except Exception:
         return Response(status=status.HTTP_400_BAD_REQUEST)

backend/monsun_backend/command_execution.py

@@ -19,6 +19,7 @@ from backend.monsun_backend.util import log_function_call
 from . import commands
 from .commands import Command
 from .commands import CommandId
+from .commands import CommandTarget
 from .commands import Request
 from .commands import Response
 from .container import get_initialize_container
@@ -68,6 +69,7 @@ def worker_process(
                 logger.info("connected with serial device")
                 connected = True
                 enter_fsm(
+                    target=CommandTarget[role],
                     root_logger=root_logger,
                     serial=serial,
                     command_queue=queue,
@@ -82,6 +84,7 @@ def worker_process(
 
 
 def enter_fsm(
+    target: CommandTarget,
     root_logger: logging.Logger,
     serial: Serial,
     command_queue: Queue,
@@ -96,6 +99,7 @@ def enter_fsm(
     last_heart_beat_time: float = 0.0
     serial_receiver = SerialReceiver(
         root_logger=root_logger,
+        target=target,
     )
 
     while True:
@@ -104,8 +108,19 @@ def enter_fsm(
                 command_queue.put(
                     commands.HeartbeatRequest(
                         root_logger=root_logger,
+                        target=target,
                     ),
                 )
+
+                # heartbeat: client -> ble -> server -> ble -> client
+                # if target == CommandTarget.client:
+                #     command_queue.put(
+                #         commands.HeartbeatRequest(
+                #             root_logger=root_logger,
+                #             target=CommandTarget.server,
+                #         ),
+                #     )
+
                 last_heart_beat_time = time.time()
 
             state = State.executing_command
@@ -261,7 +276,7 @@ class CommandInterpreter:
         try:
             stop_byte = bytes_read[self.header_size + self.data_length]
         except IndexError:
-            self._logger.error("could not get stop byte")
+            self._logger.debug("could not get stop byte")
             raise CommandBytesReadInsufficient()
 
         if stop_byte != 0xFF:
@@ -278,8 +293,11 @@ class SerialReceiver:
     def __init__(
         self,
         root_logger: logging.Logger,
+        target: CommandTarget,
     ) -> None:
         self.root_logger = root_logger
+        self.target = target
+
         self._logger = root_logger.getChild(self.__class__.__name__)
         self._logger.setLevel(logging.INFO)
 
@@ -321,6 +339,7 @@ class SerialReceiver:
             if command_id == CommandId.command_log:
                 command = commands.LogCommand(
                     root_logger=self.root_logger,
+                    target=self.target,
                     data=command_interpreter.payload,
                 )
                 commands_received.append(command)

backend/monsun_backend/commands.py

@@ -26,6 +26,11 @@ class CommandId(Enum):
     command_gp_response = 0x7
 
 
+class CommandTarget(Enum):
+    client = 0
+    server = 1
+
+
 def get_command_id_from_name(name: str) -> CommandId:
     return {
         "log": CommandId.command_log,
@@ -80,8 +85,11 @@ class Command(abc.ABC):
     def __init__(
         self,
         root_logger: logging.Logger,
+        target: CommandTarget,
     ) -> None:
+        self.root_logger = root_logger
         self._logger = root_logger.getChild(self.__class__.__name__)
+        self._target = target
 
     @property
     @abc.abstractmethod
@@ -101,20 +109,21 @@ class Command(abc.ABC):
             ">BHB" + "B" * length + "B",
             int(self.identifier.value),
             length,
-            0,
+            self._target.value,
             *list(payload),
             0xFF,
         )
-
+        if len(data) > 247:
+            self._logger.error(
+                "Cannot send command longer than 247 bytes. "
+                "Command has length of {len(data)}",
+            )
         serial.write(data)
 
 
 class Request(Command):
-    def __init__(
-        self,
-        root_logger: logging.Logger,
-    ) -> None:
-        super().__init__(root_logger=root_logger)
+    def __init__(self, *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
 
         self.response_identifier = randint(0, pow(2, 16) - 1)
 
@@ -148,12 +157,8 @@ class LogCommand(Command):
 
     HEADER_SIZE = 2  # log level + logger name length
 
-    def __init__(
-        self,
-        root_logger: logging.Logger,
-        data: bytes,
-    ) -> None:
-        super().__init__(root_logger=root_logger)
+    def __init__(self, data: bytes, *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
 
         self._logger.setLevel(logging.INFO)
 
@@ -167,7 +172,7 @@ class LogCommand(Command):
         self._logger.debug("logger_name " + str(logger_name))
         self._logger.debug("Message: " + str(message))
 
-        self.received_logger = root_logger.getChild(logger_name.decode())
+        self.received_logger = self.root_logger.getChild(logger_name.decode())
         self.received_logger.setLevel(logging.DEBUG)
         self.level = level
         self.message = message.decode()
@@ -226,10 +231,7 @@ class LEDResponse(Response):
 
 class LEDRequest(Request):
     def __init__(
-        self,
-        root_logger: logging.Logger,
-        id: Union[int, str],
-        command: Union[int, str],
+        self, id: Union[int, str], command: Union[int, str], *args, **kwargs
     ) -> None:
         """
         led_id
@@ -244,7 +246,7 @@ class LEDRequest(Request):
         1: on
         2: toggle
         """
-        super().__init__(root_logger=root_logger)
+        super().__init__(*args, **kwargs)
         try:
             self.led_id = int(id)
         except ValueError:
@@ -269,7 +271,7 @@ class LEDRequest(Request):
 
     @property
     def timeout(self) -> float:
-        return 0.1
+        return 1
 
     def process_response(self, response: Response):
         if not isinstance(response, LEDResponse):
@@ -302,12 +304,8 @@ class GPResponse(Response):
 
 
 class GPRequest(Request):
-    def __init__(
-        self,
-        root_logger: logging.Logger,
-        command_id: Union[int, str],
-    ) -> None:
-        super().__init__(root_logger=root_logger)
+    def __init__(self, command_id: Union[int, str], *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
         self.command_id = int(command_id)
 
     @property

nucleo-wb55-ble/Core/Inc/app_conf.h

@@ -544,7 +544,7 @@ typedef enum
 #define MAX_DBG_TRACE_MSG_SIZE 1024
 
 /* USER CODE BEGIN Defines */
-
+// #define DEBUG_COMMADS
 /* USER CODE END Defines */
 
 /******************************************************************************
@@ -568,7 +568,7 @@ typedef enum
 #endif
     CFG_TASK_HCI_ASYNCH_EVT_ID,
 /* USER CODE BEGIN CFG_Task_Id_With_HCI_Cmd_t */
-
+    CFG_TASK_SEND_COMMAND_ID,
 /* USER CODE END CFG_Task_Id_With_HCI_Cmd_t */
     CFG_LAST_TASK_ID_WITH_HCICMD,                                               /**< Shall be LAST in the list */
 } CFG_Task_Id_With_HCI_Cmd_t;

nucleo-wb55-ble/Middlewares/ST/STM32_WPAN/ble/svc/Inc/p2p_stm.h

@@ -64,6 +64,7 @@ void P2PS_STM_Init( void );
 void P2PS_STM_App_Notification(P2PS_STM_App_Notification_evt_t *pNotification);
 tBleStatus P2PS_STM_App_Update_Char(uint16_t UUID,  uint8_t *pPayload);
 
+void Send_Command(void);
 
 #ifdef __cplusplus
 }

nucleo-wb55-ble/Middlewares/ST/STM32_WPAN/ble/svc/Src/p2p_stm.c

@@ -19,9 +19,10 @@
 
 
 /* Includes ------------------------------------------------------------------*/
+#include <stdio.h>
 #include "common_blesvc.h"
 #include "commands.h"
-/* Private typedef -----------------------------------------------------------*/
+#include "ble_cmd_mbx.h"
 typedef struct{
   uint16_t	PeerToPeerSvcHdle;				        /**< Service handle */
   uint16_t	P2PWriteClientToServerCharHdle;	  /**< Characteristic handle */
@@ -44,7 +45,7 @@ typedef struct{
 
 
 /* Private macros ------------------------------------------------------------*/
-
+#define COMMAND_CHAR_LENGTH (247)
 /* Private variables ---------------------------------------------------------*/
 /**
  * Reboot Characteristic UUID
@@ -150,11 +151,35 @@ static SVCCTL_EvtAckStatus_t PeerToPeer_Event_Handler(void *Event)
             
             else if(attribute_modified->Attr_Handle == (aPeerToPeerContext.P2PWriteClientToServerCharHdle + 1))
             {
-              BLE_DBG_P2P_STM_MSG("-- GATT : LED CONFIGURATION RECEIVED\n");
-              Notification.P2P_Evt_Opcode = P2PS_STM_WRITE_EVT;
-              Notification.DataTransfered.Length=attribute_modified->Attr_Data_Length;
-              Notification.DataTransfered.pPayload=attribute_modified->Attr_Data;
-              P2PS_STM_App_Notification(&Notification);  
+#ifdef DEBUG_COMMADS
+              uint8_t buffer[512];
+              buffer[0] = (uint8_t)('0');
+              buffer[1] = (uint8_t)('x');
+              uint8_t i;
+              for (i = 0; i<attribute_modified->Attr_Data_Length; i++) {
+                snprintf((char*)(buffer + i + 2), 512-i-2, "%x", attribute_modified->Attr_Data[i]);
+              }
+              buffer[i + 3] = (uint8_t)('\0');
+              log_debug("PeerToPeer_Event_Handler", "Notification payload: %s", 1, buffer);
+#endif /* DEBUG_COMMADS */ 
+              
+              if (attribute_modified->Attr_Data_Length >= 4) {
+                uint8_t * buf = attribute_modified->Attr_Data;
+                uint8_t command_id = buf[0];
+                uint16_t length = buf[1] << 8 | buf[2];
+                uint8_t * payload_ptr = buf + 4;
+#ifdef DEBUG_COMMADS
+                log_debug("PeerToPeer_Event_Handler", "calling handle_received_ble_command()", 0);
+#endif /* DEBUG_COMMADS */ 
+                handle_received_ble_command(command_id, payload_ptr, length);
+
+              } else {
+                BLE_DBG_P2P_STM_MSG("-- GATT : LED CONFIGURATION RECEIVED\n");
+                Notification.P2P_Evt_Opcode = P2PS_STM_WRITE_EVT;
+                Notification.DataTransfered.Length=attribute_modified->Attr_Data_Length;
+                Notification.DataTransfered.pPayload=attribute_modified->Attr_Data;
+                P2PS_STM_App_Notification(&Notification);  
+              }
             }
 #if(BLE_CFG_OTA_REBOOT_CHAR != 0)
             else if(attribute_modified->Attr_Handle == (aPeerToPeerContext.RebootReqCharHdle + 1))
@@ -223,7 +248,7 @@ void P2PS_STM_Init(void)
     COPY_P2P_WRITE_CHAR_UUID(uuid16.Char_UUID_128);
     aci_gatt_add_char(aPeerToPeerContext.PeerToPeerSvcHdle,
                       UUID_TYPE_128, &uuid16,
-                      2,                                   
+                      COMMAND_CHAR_LENGTH,                                   
                       CHAR_PROP_WRITE_WITHOUT_RESP|CHAR_PROP_READ,
                       ATTR_PERMISSION_NONE,
                       GATT_NOTIFY_ATTRIBUTE_WRITE, /* gattEvtMask */
@@ -237,7 +262,7 @@ void P2PS_STM_Init(void)
     COPY_P2P_NOTIFY_UUID(uuid16.Char_UUID_128);
     aci_gatt_add_char(aPeerToPeerContext.PeerToPeerSvcHdle,
                       UUID_TYPE_128, &uuid16,
-                      2,
+                      COMMAND_CHAR_LENGTH,
                       CHAR_PROP_NOTIFY,
                       ATTR_PERMISSION_NONE,
                       GATT_NOTIFY_ATTRIBUTE_WRITE, /* gattEvtMask */
@@ -293,4 +318,32 @@ tBleStatus P2PS_STM_App_Update_Char(uint16_t UUID, uint8_t *pPayload)
   return result;
 }/* end P2PS_STM_Init() */
 
+void Send_Command(void)
+{
+#ifdef DEBUG_COMMADS
+  log_debug("Send_Command", "SEND COMMAND TO CLIENT ", 0);
+#endif /* DEBUG_COMMADS */ 
+  
+  if (!get_number_of_ble_commands_in_mailbox()) {
+    return;
+  }
+
+  raw_command_t command = pop_ble_command();
+
+  tBleStatus ret = BLE_STATUS_INVALID_PARAMS;
+  uint8_t index = 0;
+
+  ret = aci_gatt_update_char_value(
+    aPeerToPeerContext.PeerToPeerSvcHdle,
+    aPeerToPeerContext.P2PNotifyServerToClientCharHdle,
+    0, /* charValOffset */
+    command.size, /* charValueLen */
+    command.payload);
+
+  if (ret) {
+    log_error("Send_Command", "aci_gatt_update_char_value() returned %d", 1, ret); 
+  }
+
+  return;
+}
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

nucleo-wb55-ble/STM32_WPAN/App/p2p_server_app.c

@@ -140,6 +140,7 @@ void P2PS_APP_Init(void)
 {
 /* USER CODE BEGIN P2PS_APP_Init */
 	UTIL_SEQ_RegTask(1<< CFG_TASK_SW1_BUTTON_PUSHED_ID, UTIL_SEQ_DEFAULT, P2PS_Send_Notification);
+	UTIL_SEQ_RegTask(1<< CFG_TASK_SEND_COMMAND_ID, UTIL_SEQ_DEFAULT, Send_Command);
 /* USER CODE END P2PS_APP_Init */
   return;
 }

nucleo-wb55-ble/commands/GPCommand.cpp

@@ -11,13 +11,14 @@
 #include "stm32_seq.h"
 
 
-GPResponse::GPResponse(uint16_t response_identifier, bool was_successful)
-  : Response(response_identifier) {
+GPResponse::GPResponse(com_channel_type_t com_channel_type, uint16_t response_identifier, bool was_successful)
+  : Response(com_channel_type, response_identifier) {
   (this->payload_ptr + this->get_payload_size())[0] = (uint8_t)was_successful;
   this->add_to_payload_size(1);
 }
 
-GPRequest::GPRequest(uint8_t * payload_ptr, uint16_t size) : Request(payload_ptr, size) {
+GPRequest::GPRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size) 
+  : Request(com_channel_type, payload_ptr, size) {
   uint16_t expected_length = this->buffer_offset + 1;
   if (expected_length != size) {
     this->has_error = true;
@@ -28,7 +29,7 @@ GPRequest::GPRequest(uint8_t * payload_ptr, uint16_t size) : Request(payload_ptr
   this->command_id = data_ptr[0];
 }
 
-GPResponse * GPRequest::execute_request(uint16_t response_identifier) {
+GPResponse * GPRequest::execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) {
   bool was_successful = true;
 
   if (this->has_error) {
@@ -45,6 +46,6 @@ GPResponse * GPRequest::execute_request(uint16_t response_identifier) {
     }
   }
 
-  return new GPResponse(response_identifier, was_successful);
+  return new GPResponse(com_channel_type, response_identifier, was_successful);
 }
 

nucleo-wb55-ble/commands/GPCommand.hpp

@@ -13,7 +13,7 @@
 
 class GPResponse : public Response {
 public:
-  GPResponse(uint16_t response_identifier, bool was_successful);
+  GPResponse(com_channel_type_t com_channel_type, uint16_t response_identifier, bool was_successful);
 
   CommandId get_command_id() override { return COMMAND_GP_RESPONSE; }
 
@@ -32,10 +32,9 @@ public:
    *
    * @param size
    */
-  GPRequest(uint8_t * payload_ptr, uint16_t size);
+  GPRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size);
 
-
-  GPResponse * execute_request(uint16_t response_identifier);
+  GPResponse * execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) override;
 
   CommandId get_command_id() override { return COMMAND_GP_REQUEST; }
 

nucleo-wb55-dongle-ble/shared_commands/command_interpreter.c → nucleo-wb55-ble/commands/command_interpreter.c

@@ -26,7 +26,7 @@ void usb_receive(uint8_t *buf, uint32_t *len) {
   if (stop_byte != 0xFF) {
     log_error("usb_receive", "received command has invalid stop byte: 0x%x", 1, stop_byte);
   } else {
-    handle_received_command(command_id, payload_ptr, length);
+    handle_received_usb_command(command_id, payload_ptr, length);
   }
 
 }

nucleo-wb55-ble/commands/dispatch.cpp

@@ -11,22 +11,38 @@
 #include "LedCommand.hpp"
 #include "GPCommand.hpp"
 
-void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {
-
+void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size, com_channel_type_t com_channel_type ) { 
   switch (command_id)
   {
   case COMMAND_HEARTBEAT_REQUEST:
-    push_command(new HeartbeatRequest(payload_ptr, size));
+    push_command(new HeartbeatRequest(com_channel_type, payload_ptr, size));
     break;
   case COMMAND_LED_REQUEST:
-    push_command(new LedRequest(payload_ptr, size));
+    push_command(new LedRequest(com_channel_type, payload_ptr, size));
     break;
   case COMMAND_GP_REQUEST:
-    push_command(new GPRequest(payload_ptr, size));
+    push_command(new GPRequest(com_channel_type, payload_ptr, size));
     break;
 
   default:
     break;
   }
+}
+
+void handle_received_usb_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {
+  handle_received_command(
+    command_id,
+    payload_ptr,
+    size,
+    com_channel_type_usb
+  );
+}
 
+void handle_received_ble_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {
+  handle_received_command(
+    command_id,
+    payload_ptr,
+    size,
+    com_channel_type_ble
+  );
 }

nucleo-wb55-ble/commands/dispatch.hpp

@@ -10,6 +10,7 @@
 
 #include <stdint.h>
 
-extern "C" void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
+extern "C" void handle_received_usb_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
+extern "C" void handle_received_ble_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
 
 #endif /* DISPATCH_HPP_ */

nucleo-wb55-dongle-ble/Core/Inc/app_conf.h

@@ -506,7 +506,7 @@ typedef enum
 #define MAX_DBG_TRACE_MSG_SIZE 1024
 
 /* USER CODE BEGIN Defines */
-
+// #define DEBUG_COMMADS
 /* USER CODE END Defines */
 
 /******************************************************************************
@@ -530,7 +530,7 @@ typedef enum
     CFG_TASK_CONN_UPDATE_ID,
     CFG_TASK_HCI_ASYNCH_EVT_ID,
 /* USER CODE BEGIN CFG_Task_Id_With_HCI_Cmd_t */
-
+    CFG_TASK_SEND_COMMAND_ID,
 /* USER CODE END CFG_Task_Id_With_HCI_Cmd_t */
     CFG_LAST_TASK_ID_WITH_HCICMD,                                               /**< Shall be LAST in the list */
 } CFG_Task_Id_With_HCI_Cmd_t;

nucleo-wb55-dongle-ble/STM32_WPAN/App/p2p_client_app.c

@@ -34,6 +34,9 @@
 
 /* USER CODE BEGIN Includes */
 #include "commands.h"
+#include "cmd_mbx.h"
+#include "ble_cmd_mbx.h"
+#include "usb_cmd_mbx.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -164,6 +167,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event);
 /* USER CODE BEGIN PFP */
 static tBleStatus Write_Char(uint16_t UUID, uint8_t Service_Instance, uint8_t *pPayload);
 static void Button_Trigger_Received( void );
+static void Send_Command(void);
 static void Update_Service( void );
 /* USER CODE END PFP */
 
@@ -180,6 +184,7 @@ void P2PC_APP_Init(void)
   log_debug("P2PC_APP_Init", "start", 0);
   UTIL_SEQ_RegTask( 1<< CFG_TASK_SEARCH_SERVICE_ID, UTIL_SEQ_RFU, Update_Service );
   UTIL_SEQ_RegTask( 1<< CFG_TASK_SW1_BUTTON_PUSHED_ID, UTIL_SEQ_RFU, Button_Trigger_Received );
+  UTIL_SEQ_RegTask( 1<< CFG_TASK_SEND_COMMAND_ID, UTIL_SEQ_RFU, Send_Command );
 
   /**
    * Initialize LedButton Service
@@ -531,6 +536,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 
           if(index < BLE_CFG_CLT_MAX_NBR_CB)
           {
+#ifdef DEBUG_COMMADS
             uint8_t buffer[512];
             buffer[0] = (uint8_t)('0');
             buffer[1] = (uint8_t)('x');
@@ -538,8 +544,9 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
             for (i = 0; i<pr->Attribute_Value_Length; i++) {
               snprintf((char*)(buffer + i + 2), 512-i-2, "%x", pr->Attribute_Value[i]);
             }
-            buffer[i + 2] = (uint8_t)('\0');
+            buffer[i + 3] = (uint8_t)('\0');
             log_debug("Event_Handler", "Notification payload: %s", 1, buffer);
+#endif /* DEBUG_COMMADS */ 
 
             if ( (pr->Attribute_Handle == aP2PClientContext[index].P2PNotificationCharHdle) &&
                     (pr->Attribute_Value_Length == (2)) )
@@ -553,6 +560,11 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 
               /* INFORM APPLICATION BUTTON IS PUSHED BY END DEVICE */
 
+            } else {
+              push_usb_command(
+                &pr->Attribute_Value,
+                pr->Attribute_Value_Length 
+              );
             }
           }
         }
@@ -706,6 +718,37 @@ void Button_Trigger_Received(void)
   return;
 }
 
+void Send_Command(void)
+{
+#ifdef DEBUG_COMMADS
+  log_debug("Send_Command", "SEND COMMAND TO SERVER ", 0);
+#endif /* DEBUG_COMMADS */ 
+  
+  if (!get_number_of_ble_commands_in_mailbox()) {
+    return;
+  }
+
+  raw_command_t command = pop_ble_command();
+
+  tBleStatus ret = BLE_STATUS_INVALID_PARAMS;
+  uint8_t index = 0;
+
+  if (aP2PClientContext[index].state != APP_BLE_IDLE)
+  {
+    ret = aci_gatt_write_without_resp(
+      aP2PClientContext[index].connHandle,
+      aP2PClientContext[index].P2PWriteToServerCharHdle,
+      command.size, /* charValueLen */
+      command.payload);
+  }
+
+  if (ret) {
+    log_error("Send_Command", "aci_gatt_write_without_resp() returned %d", 1, ret); 
+  }
+
+  return;
+}
+
 void Update_Service()
 {
   log_debug("Update_Service", "enter", 0);

nucleo-wb55-dongle-ble/commands/GPCommand.cpp

@@ -9,13 +9,14 @@
 
 #include "app_ble.h"
 
-GPResponse::GPResponse(uint16_t response_identifier, bool was_successful) 
-  : Response(response_identifier) {
+GPResponse::GPResponse(com_channel_type_t com_channel_type, uint16_t response_identifier, bool was_successful) 
+  : Response(com_channel_type, response_identifier) {
   (this->payload_ptr + this->get_payload_size())[0] = (uint8_t)was_successful;
   this->add_to_payload_size(1);
 }
 
-GPRequest::GPRequest(uint8_t * payload_ptr, uint16_t size) : Request(payload_ptr, size) {
+GPRequest::GPRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size) 
+  : Request(com_channel_type, payload_ptr, size) {
   uint16_t expected_length = this->buffer_offset + 1;
   if (expected_length != size) {
     this->has_error = true;
@@ -26,7 +27,7 @@ GPRequest::GPRequest(uint8_t * payload_ptr, uint16_t size) : Request(payload_ptr
   this->command_id = data_ptr[0];
 }
 
-GPResponse * GPRequest::execute_request(uint16_t response_identifier) {
+GPResponse * GPRequest::execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) {
   bool was_successful = true;
 
   if (this->has_error) {
@@ -43,6 +44,6 @@ GPResponse * GPRequest::execute_request(uint16_t response_identifier) {
     }
   }
 
-  return new GPResponse(response_identifier, was_successful);
+  return new GPResponse(com_channel_type, response_identifier, was_successful);
 }
 

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

@@ -13,7 +13,7 @@
 
 class GPResponse : public Response {
 public:
-  GPResponse(uint16_t response_identifier, bool was_successful);
+  GPResponse(com_channel_type_t com_channel_type, uint16_t response_identifier, bool was_successful);
 
   CommandId get_command_id() override { return COMMAND_GP_RESPONSE; }
 
@@ -32,10 +32,9 @@ public:
    * 
    * @param size 
    */
-  GPRequest(uint8_t * payload_ptr, uint16_t size);
+  GPRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size);
 
-
-  GPResponse * execute_request(uint16_t response_identifier);
+  GPResponse * execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) override;
 
   CommandId get_command_id() override { return COMMAND_GP_REQUEST; }
 

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

@@ -0,0 +1,64 @@
+/*
+ * command_interpreter.c
+ *
+ *  Created on: Jul 10, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#include <stdio.h>
+#include "usbd_cdc_if.h"
+#include "commands.h"
+#include "ble_cmd_mbx.h"
+
+enum command_target_t {
+  client,
+  server,
+};
+
+// #define DEBUG_COMMADS 
+
+void usb_receive(uint8_t *buf, uint32_t *len) {
+
+  if (*len < 5) {
+    log_error("usb_receive", "received command which cannot be interpreted", 0);
+    return;
+  }
+  uint16_t length = buf[1] << 8 | buf[2];
+  uint16_t command_total_length = length + 5;
+  uint8_t command_target = buf[3];
+  uint8_t stop_byte = 0x1;
+
+  if (*len >= command_total_length - 1) {
+    stop_byte = buf[command_total_length - 1];
+  }
+
+  if (stop_byte != 0xFF) {
+    log_error("usb_receive", "received command has invalid stop byte: 0x%x", 1, stop_byte);
+    return;
+  }
+
+  if (command_target == server) {
+#ifdef DEBUG_COMMADS
+    uint8_t buffer[512];
+    buffer[0] = (uint8_t)('0');
+    buffer[1] = (uint8_t)('x');
+    uint8_t i;
+    for (i = 0; i<command_total_length; i++) {
+      snprintf((char*)(buffer + i + 2), 512-i-2, "%x", buf[i]);
+    }
+    buffer[i + 2] = (uint8_t)('\0');
+    log_debug("usb_receive", "raw command: %s", 1, buffer);
+#endif /* DEBUG_COMMADS */ 
+    push_ble_command(buf, command_total_length);
+    return;
+  }
+
+  uint8_t command_id = buf[0];
+  uint8_t * payload_ptr = buf + 4;
+  if (command_target == client) {
+    handle_received_usb_command(command_id, payload_ptr, length);
+  } else {
+    log_error("usb_receive", "unknown command target: 0x%x", 1, command_target);
+  }
+
+}

nucleo-wb55-dongle-ble/commands/dispatch.cpp

@@ -11,22 +11,38 @@
 #include "HeartbeatCommand.hpp"
 #include "LedCommand.hpp"
 
-void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {
-
+void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size, com_channel_type_t com_channel_type ) { 
   switch (command_id)
   {
   case COMMAND_HEARTBEAT_REQUEST:
-    push_command(new HeartbeatRequest(payload_ptr, size));
+    push_command(new HeartbeatRequest(com_channel_type, payload_ptr, size));
     break;
   case COMMAND_LED_REQUEST:
-    push_command(new LedRequest(payload_ptr, size));
+    push_command(new LedRequest(com_channel_type, payload_ptr, size));
     break;
   case COMMAND_GP_REQUEST:
-    push_command(new GPRequest(payload_ptr, size));
+    push_command(new GPRequest(com_channel_type, payload_ptr, size));
     break;
 
   default:
     break;
   }
+}
+
+void handle_received_usb_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {
+  handle_received_command(
+    command_id,
+    payload_ptr,
+    size,
+    com_channel_type_usb
+  );
+}
 
+void handle_received_ble_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t size) {
+  handle_received_command(
+    command_id,
+    payload_ptr,
+    size,
+    com_channel_type_ble
+  );
 }

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

@@ -10,6 +10,7 @@
 
 #include <stdint.h>
 
-extern "C" void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
+extern "C" void handle_received_usb_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
+extern "C" void handle_received_ble_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
 
 #endif /* DISPATCH_HPP_ */

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

@@ -10,6 +10,11 @@
 
 #include "commands.hpp"
 
+typedef enum {
+  com_channel_type_usb,
+  com_channel_type_ble,
+} com_channel_type_t;
+
 class Command {
 public:
   CommandId id; 

nucleo-wb55-dongle-ble/shared_commands/HeartbeatCommand.cpp

@@ -7,13 +7,13 @@
 
 #include "HeartbeatCommand.hpp"
 
-HeartbeatResponse::HeartbeatResponse(uint16_t response_identifier) 
-  : Response(response_identifier) {
+HeartbeatResponse::HeartbeatResponse(com_channel_type_t com_channel_type, uint16_t response_identifier) 
+  : Response(com_channel_type, response_identifier) {
 };
 
-HeartbeatRequest::HeartbeatRequest(uint8_t * request_payload_ptr, uint16_t size) 
-  : Request(request_payload_ptr, size) { }
+HeartbeatRequest::HeartbeatRequest(com_channel_type_t com_channel_type, uint8_t * request_payload_ptr, uint16_t size) 
+  : Request(com_channel_type, request_payload_ptr, size) { }
 
-Response * HeartbeatRequest::execute_request(uint16_t response_identifier) {
-  return new HeartbeatResponse(response_identifier);
+Response * HeartbeatRequest::execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) {
+  return new HeartbeatResponse(com_channel_type, response_identifier);
 }

nucleo-wb55-dongle-ble/shared_commands/HeartbeatCommand.hpp

@@ -13,7 +13,7 @@
 
 class HeartbeatResponse : public Response {
 public:
-  HeartbeatResponse(uint16_t response_identifier);
+  HeartbeatResponse(com_channel_type_t com_channel_type, uint16_t response_identifier);
 
   virtual CommandId get_command_id() override { return COMMAND_HEARTBEAT_RESPONSE; }
 };
@@ -21,8 +21,8 @@ public:
 class HeartbeatRequest : public Request {
 public:
 
-  HeartbeatRequest(uint8_t * payload_ptr, uint16_t size);
-  virtual Response * execute_request(uint16_t response_identifier) override;
+  HeartbeatRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size);
+  virtual Response * execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) override;
   virtual CommandId get_command_id() override { return COMMAND_HEARTBEAT_REQUEST; }
 };
 

nucleo-wb55-dongle-ble/shared_commands/LedCommand.cpp

@@ -10,13 +10,14 @@
 #include "stm32wbxx_hal.h"
 #include "main.h"
 
-LedResponse::LedResponse(uint16_t response_identifier, bool was_successful) 
-  : Response(response_identifier) {
+LedResponse::LedResponse(com_channel_type_t com_channel_type, uint16_t response_identifier, bool was_successful) 
+  : Response(com_channel_type, response_identifier) {
   (this->payload_ptr + this->get_payload_size())[0] = (uint8_t)was_successful;
   this->add_to_payload_size(1);
 }
 
-LedRequest::LedRequest(uint8_t * payload_ptr, uint16_t size) : Request(payload_ptr, size) {
+LedRequest::LedRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size) 
+  : Request(com_channel_type, payload_ptr, size) {
   uint16_t expected_length = this->buffer_offset + 2;
   if (expected_length != size) {
     //log_error("LedCommand: received request with length %d, expected length %d", 2, size, expected_length);
@@ -29,7 +30,7 @@ LedRequest::LedRequest(uint8_t * payload_ptr, uint16_t size) : Request(payload_p
   this->led_command = static_cast<LedCommand>(data_ptr[1]);
 }
 
-LedResponse * LedRequest::execute_request(uint16_t response_identifier) {
+LedResponse * LedRequest::execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) {
   bool was_successful = true;
   int led_pin_mapping[3] = {LED_GREEN_Pin, LED_RED_Pin, LED_BLUE_Pin};
   GPIO_TypeDef* led_prio_port_mapping[3] = {LED_GREEN_GPIO_Port, LED_RED_GPIO_Port, LED_BLUE_GPIO_Port};
@@ -56,6 +57,6 @@ LedResponse * LedRequest::execute_request(uint16_t response_identifier) {
     }
   }
 
-  return new LedResponse(response_identifier, was_successful);
+  return new LedResponse(com_channel_type, response_identifier, was_successful);
 }
 

nucleo-wb55-dongle-ble/shared_commands/LedCommand.hpp

@@ -13,7 +13,7 @@
 
 class LedResponse : public Response {
 public:
-  LedResponse(uint16_t response_identifier, bool was_successful);
+  LedResponse(com_channel_type_t com_channel_type, uint16_t response_identifier, bool was_successful);
 
   CommandId get_command_id() override { return COMMAND_LED_RESPONSE; }
 
@@ -44,7 +44,7 @@ public:
    * 
    * @param size 
    */
-  LedRequest(uint8_t * payload_ptr, uint16_t size);
+  LedRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size);
 
   enum LedID {
     greed_led,
@@ -60,7 +60,7 @@ public:
     led_command_max,
   };
 
-  LedResponse * execute_request(uint16_t response_identifier);
+  LedResponse * execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) override;
 
   CommandId get_command_id() override { return COMMAND_LED_REQUEST; }
 

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

@@ -45,7 +45,7 @@ LogCommand::LogCommand(
     const char * logger_name, 
     const char * format, 
     va_list args, 
-    LoggingLevel logging_level) : Notification() {
+    LoggingLevel logging_level) : Notification(com_channel_type_usb) /* send it always over USB */ {
 
     uint16_t max_payload_size = NOTIFICATION_COMMAND_FREE_BUFFER - this->get_payload_size();
 

nucleo-wb55-dongle-ble/shared_commands/Notification.cpp

@@ -7,8 +7,11 @@
 
 #include "usbd_cdc_if.h"
 #include "Notification.hpp"
+#include "ble_cmd_mbx.h"
 
-Notification::Notification() {
+
+Notification::Notification(com_channel_type_t com_channel_type) 
+  : Command(), com_channel_type(com_channel_type) {
   this->payload_ptr = this->data +4;
 }
 
@@ -21,8 +24,13 @@ bool Notification::execute() {
   this->data[3] = 0x00; // reserved
   this->data[size-1] = 0xff;  // set stop byte
 
-  uint8_t result = CDC_Transmit_FS(this->data, size);
-  return result == USBD_OK || result == USBD_BUSY;
+  if (this->com_channel_type == com_channel_type_usb) {
+    uint8_t result = CDC_Transmit_FS(this->data, size);
+    return result == USBD_OK || result == USBD_BUSY;
+  } else {
+    push_ble_command(this->data, size);
+    return true;
+  }
 }
 
 uint16_t Notification::get_payload_size() {
@@ -31,4 +39,4 @@ uint16_t Notification::get_payload_size() {
 
 void Notification::add_to_payload_size(uint16_t size) {
   this->payload_size += size; 
-}
+}

nucleo-wb55-dongle-ble/shared_commands/Notification.hpp

@@ -19,7 +19,7 @@
 class Notification: public Command {
 public:
 
-  Notification();
+  Notification(com_channel_type_t com_channel_type);
   virtual ~Notification() {};
 
   bool execute() override;
@@ -33,6 +33,7 @@ protected:
 private:
   uint16_t payload_size = 0;
   uint8_t data[BUFFER_SIZE];
+  com_channel_type_t com_channel_type;
 };
 
 #endif /* NOTIFICATION_HPP_ */

nucleo-wb55-dongle-ble/shared_commands/Request.cpp

@@ -7,12 +7,13 @@
 
 #include "Request.hpp"
 
-Request::Request(uint8_t * payload_ptr, uint16_t size) : Command() {
+Request::Request(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size) 
+  : Command(), com_channel_type(com_channel_type) {
   this->response_identifier = payload_ptr[0] << 8 | payload_ptr[1];
 }
 
 bool Request::execute() {
-  Response * response = this->execute_request(this->response_identifier);
+  Response * response = this->execute_request(this->com_channel_type, this->response_identifier);
   bool result = response->execute();
   delete response;
   return result;

nucleo-wb55-dongle-ble/shared_commands/Request.hpp

@@ -13,17 +13,18 @@
 
 class Request : public Command {
 public:
-  Request(uint8_t * payload_ptr, uint16_t size);
+  Request(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size);
 
   bool execute() override;
 
-  virtual Response * execute_request(uint16_t response_identifier) = 0;
+  virtual Response * execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) = 0;
 
 protected:
   int buffer_offset = 2;
 
 private:
   uint16_t response_identifier;
+  com_channel_type_t com_channel_type;
 
 };
 

nucleo-wb55-dongle-ble/shared_commands/Response.cpp

@@ -7,8 +7,8 @@
 
 #include "Response.hpp"
 
-Response::Response(uint16_t response_identifier) 
-  : Notification(), response_identifier(response_identifier) { 
+Response::Response(com_channel_type_t com_channel_type, uint16_t response_identifier) 
+  : Notification(com_channel_type), response_identifier(response_identifier) { 
     this->payload_ptr[0] = (response_identifier & 0xff00) >> 8;
     this->payload_ptr[1] = response_identifier & 0x00ff;
     this->add_to_payload_size(2);

nucleo-wb55-dongle-ble/shared_commands/Response.hpp

@@ -12,7 +12,7 @@
 
 class Response : public Notification {
 public:
-  Response(uint16_t response_identifier);
+  Response(com_channel_type_t com_channel_type, uint16_t response_identifier);
   virtual ~Response() {};
 
 private:

nucleo-wb55-dongle-ble/shared_commands/ble_cmd_mbx.cpp

@@ -0,0 +1,36 @@
+/*
+ * ble_cmd_mbx.cpp
+ *
+ *  Created on: Jul 23, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#include <queue>
+#include "string.h"
+#include "ble_cmd_mbx.h"
+#include "stm32_seq.h"
+#include "app_conf.h"
+
+
+std::queue<raw_command_t> raw_ble_command_queue;
+
+void push_ble_command(uint8_t * payload_ptr, uint16_t length) {
+  raw_command_t command = {
+    .size = length,
+  };
+
+  memcpy(command.payload, payload_ptr, length);
+  raw_ble_command_queue.push(command);
+  UTIL_SEQ_SetTask(1<<CFG_TASK_SEND_COMMAND_ID, CFG_SCH_PRIO_0);
+  return;
+}
+
+uint8_t get_number_of_ble_commands_in_mailbox() {
+  return raw_ble_command_queue.size();
+}
+
+raw_command_t pop_ble_command() {
+  raw_command_t command = raw_ble_command_queue.front();
+  raw_ble_command_queue.pop();
+  return command;
+}

nucleo-wb55-dongle-ble/shared_commands/ble_cmd_mbx.h

@@ -0,0 +1,28 @@
+/*
+ * ble_cmd_mbx.h
+ *
+ *  Created on: Jul 23, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#ifndef BLE_CMD_MBX_H_
+#define BLE_CMD_MBX_H_
+
+#include "cmd_mbx.h"
+
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+void push_ble_command(uint8_t * payload_ptr, uint16_t length);
+
+uint8_t get_number_of_ble_commands_in_mailbox();
+
+raw_command_t pop_ble_command();
+
+#ifdef  __cplusplus
+}
+#endif
+
+#endif /* BLE_CMD_MBX_H_ */

nucleo-wb55-dongle-ble/shared_commands/cmd_mbx.h

@@ -0,0 +1,28 @@
+/*
+ * cmd_mbx.h
+ *
+ *  Created on: Jul 27, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#ifndef CMD_MBX_H_
+#define CMD_MBX_H_
+
+#include <stdio.h>
+
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+    uint8_t payload[247];
+    uint16_t size;
+} raw_command_t;
+
+#ifdef  __cplusplus
+}
+#endif
+
+
+#endif /* CMD_MBX_H_ */

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

@@ -14,6 +14,7 @@ void log_warning(const char * logger_name, const char * format, int nargs, ...);
 void log_error(const char * logger_name, const char * format, int nargs, ...);
 
 void pop_and_execute_commands();
-void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
+void handle_received_usb_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
+void handle_received_ble_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
 
 #endif /* COMMANDS_H_ */

nucleo-wb55-dongle-ble/shared_commands/execute.cpp

@@ -12,6 +12,8 @@
 #include "execute.hpp"
 #include "stm32wbxx_hal.h"
 #include "LogCommand.hpp"
+#include "usb_cmd_mbx.h"
+#include "usbd_cdc_if.h"
 
 std::queue<Command*> command_queue;
 
@@ -31,6 +33,19 @@ void pop_and_execute_commands() {
     delete command;
     command_queue.pop();
   }
+
+  uint8_t result;
+  while (get_number_of_usb_commands_in_mailbox()) {
+#ifdef DEBUG_COMMADS
+    log_debug("pop_and_execute_commands", "pop raw command", 0);
+#endif /* DEBUG_COMMADS */ 
+    raw_command_t raw_command = pop_usb_command();
+    result = CDC_Transmit_FS(raw_command.payload, raw_command.size);
+
+    if (!(result == USBD_OK || result == USBD_BUSY)) {
+      log_error("pop_and_execute_commands", "Sending of raw command was not successful!", 0);
+    }
+  }
 }
 
 

nucleo-wb55-dongle-ble/shared_commands/usb_cmd_mbx.cpp

@@ -0,0 +1,33 @@
+/*
+ * usb_cmd_mbx.cpp
+ *
+ *  Created on: Jul 23, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#include <queue>
+#include "string.h"
+#include "usb_cmd_mbx.h"
+
+
+std::queue<raw_command_t> raw_usb_command_queue;
+
+void push_usb_command(uint8_t * payload_ptr, uint16_t length) {
+  raw_command_t command = {
+    .size = length,
+  };
+
+  memcpy(command.payload, payload_ptr, length);
+  raw_usb_command_queue.push(command);
+  return;
+}
+
+uint8_t get_number_of_usb_commands_in_mailbox() {
+  return raw_usb_command_queue.size();
+}
+
+raw_command_t pop_usb_command() {
+  raw_command_t command = raw_usb_command_queue.front();
+  raw_usb_command_queue.pop();
+  return command;
+}

nucleo-wb55-dongle-ble/shared_commands/usb_cmd_mbx.h

@@ -0,0 +1,28 @@
+/*
+ * ble_cmd_mbx.h
+ *
+ *  Created on: Jul 23, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#ifndef BLE_CMD_MBX_H_
+#define BLE_CMD_MBX_H_
+
+#include "cmd_mbx.h"
+
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+void push_usb_command(uint8_t * payload_ptr, uint16_t length);
+
+uint8_t get_number_of_usb_commands_in_mailbox();
+
+raw_command_t pop_usb_command();
+
+#ifdef  __cplusplus
+}
+#endif
+
+#endif /* BLE_CMD_MBX_H_ */