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,13 +24,17 @@ 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:
         command_id = get_command_id_from_name(cmd)
 
     try:
-        command = get_request_class(command_id=command_id)(**arguments)
+        command = get_request_class(command_id=command_id)(
+            root_logger=logging.getLogger(role), target=target, **arguments
+        )
     except Exception:
         return Response(status=status.HTTP_400_BAD_REQUEST)
 

backend/monsun_backend/command_execution.py

@@ -4,6 +4,7 @@ import time
 from enum import Enum
 from multiprocessing import Process
 from multiprocessing import Queue
+from struct import error
 from struct import unpack
 from typing import Dict
 from typing import List
@@ -18,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
@@ -67,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,
@@ -81,6 +84,7 @@ def worker_process(
 
 
 def enter_fsm(
+    target: CommandTarget,
     root_logger: logging.Logger,
     serial: Serial,
     command_queue: Queue,
@@ -93,11 +97,30 @@ def enter_fsm(
     responses_received: List[Response] = list()
     time_at_beginning_waiting_for_response: float = 0.0
     last_heart_beat_time: float = 0.0
+    serial_receiver = SerialReceiver(
+        root_logger=root_logger,
+        target=target,
+    )
 
     while True:
         if state == State.heart_beat:
             if time.time() - heartbeat_interval > last_heart_beat_time:
-                command_queue.put(commands.HeartbeatRequest())
+                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
@@ -124,8 +147,7 @@ def enter_fsm(
                 )
             else:
                 request: Request = current_command
-            commands_, responses = receive(
-                root_logger=root_logger,
+            commands_, responses = serial_receiver.receive(
                 serial=serial,
             )
             responses_received.extend(responses)
@@ -160,8 +182,7 @@ def enter_fsm(
                     continue
 
         elif state == State.receiving_command:
-            commands_, responses = receive(
-                root_logger=root_logger,
+            commands_, responses = serial_receiver.receive(
                 serial=serial,
             )
             responses_received.extend(responses)
@@ -196,67 +217,148 @@ def enqueue_command(
     _command_queue[role].put(command)
 
 
-def receive(
-    root_logger: logging.Logger,
-    serial: Serial,
-) -> Tuple[Sequence[Command], Sequence[Response]]:
-    logger = root_logger.getChild("receive_and_log")
-    logger.setLevel(logging.INFO)
-    commands_received: List[Command] = list()
-    responses_received: List[Response] = list()
-    header_size = 4
+class CommandInterpretationError(Exception):
+    """Raised in case the command could not be interpreted"""
 
-    bytes_read = serial.read(serial.in_waiting)
 
-    while bytes_read:
+class CommandBytesReadInsufficient(CommandInterpretationError):
+    """Raised in case the command could not be interpreted"""
 
-        logger.debug(f"bytes: {bytes_read.hex()}")
+
+class CommandInterpreter:
+    header_size = 4
+
+    def __init__(
+        self,
+        root_logger: logging.Logger,
+    ) -> None:
+        self._logger = root_logger.getChild(self.__class__.__name__)
+        self._logger.setLevel(logging.INFO)
+        self.command_id_int = 0
+        self.data_length = 0
+        self.payload = bytes()
+
+    def interpret(
+        self,
+        bytes_read: bytes,
+    ) -> bytes:
+        """Interpret the first command in a byte stream.
+
+        :param bytes_read: The bytes which are not yet parsed.
+        :returns: The byte which are not yet parsed.
+        :raises CommandInterpretationError: If the command could not be parsed.
+        :raises CommandBytesReadInsufficient: Not enough bytes to fully parse the
+            command.
+        """
+        self._logger.debug(f"bytes: {bytes_read.hex()}")
         try:
-            command_id_int, data_length, _ = unpack(">BHB", bytes_read[:header_size])
-            payload = bytes(bytes_read[header_size : header_size + data_length])
-            stop_byte = bytes_read[header_size + data_length]
+            self.command_id_int, self.data_length, _ = unpack(
+                ">BHB",
+                bytes_read[: self.header_size],
+            )
+        except error:
+            self._logger.error("error while interpreting command header")
+            raise CommandBytesReadInsufficient()
+
+        try:
+            self.payload = bytes(
+                bytes_read[self.header_size : self.header_size + self.data_length],
+            )
         except IndexError:
-            # okay, something went wrong.
-            logger.error(
+            self._logger.error(
                 "There are less bytes than expected: "
-                f"Expected={header_size + data_length -1}, "
+                f"Expected={self.header_size + self.data_length -1}, "
                 f"received={len(bytes_read)}",
             )
-            logger.debug(f"bytes: {bytes_read.hex()}")
-            bytes_read = None
-            continue
+            self._logger.debug(f"bytes: {bytes_read.hex()}")
+            raise CommandBytesReadInsufficient()
 
         try:
-            command_id = CommandId(command_id_int)
-        except ValueError:
-            logger.error(
-                f"invalid command {command_id_int} with payload {str(payload)}",
-            )
-            bytes_read = bytes_read[header_size + data_length + 1 :]
-            continue
+            stop_byte = bytes_read[self.header_size + self.data_length]
+        except IndexError:
+            self._logger.debug("could not get stop byte")
+            raise CommandBytesReadInsufficient()
 
-        logger.debug(f"stop_byte: {stop_byte}")
         if stop_byte != 0xFF:
-            logger.error("Invalid stop byte")
-            bytes_read = None
-        else:
-            bytes_read = bytes_read[header_size + data_length + 1 :]
+            self._logger.error("Invalid stop byte")
+            raise CommandInterpretationError()
 
-        if command_id == CommandId.command_log:
-            command = commands.LogCommand(
-                data=payload,
-            )
-            commands_received.append(command)
-        elif command_id == CommandId.command_heartbeat_response:
-            responses_received.append(commands.HeartbeatResponse(payload))
-        elif command_id == CommandId.command_led_response:
-            responses_received.append(commands.LEDResponse(payload))
-        elif command_id == CommandId.command_gp_response:
-            responses_received.append(commands.GPResponse(payload))
-        else:
-            raise RuntimeError
+        try:
+            return bytes_read[self.header_size + self.data_length + 1 :]
+        except IndexError:
+            return bytes()
+
+
+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)
+
+        self._bytes_unread = bytearray()
+
+    def receive(
+        self,
+        serial: Serial,
+    ) -> Tuple[Sequence[Command], Sequence[Response]]:
+        commands_received: List[Command] = list()
+        responses_received: List[Response] = list()
+
+        self._bytes_unread.extend(serial.read(serial.in_waiting))
+
+        while self._bytes_unread:
+
+            try:
+                command_interpreter = CommandInterpreter(
+                    root_logger=self.root_logger,
+                )
+                self._bytes_unread = bytearray(
+                    command_interpreter.interpret(
+                        bytes_read=self._bytes_unread,
+                    ),
+                )
+            # except CommandBytesReadInsufficient:
+            #     return commands_received, responses_received
+            except CommandInterpretationError:
+                return commands_received, responses_received
+
+            try:
+                command_id = CommandId(command_interpreter.command_id_int)
+            except ValueError:
+                self._logger.error(
+                    f"invalid command {command_interpreter.command_id_int} with "
+                    f"payload {str(command_interpreter.payload)}",
+                )
+
+            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)
+            elif command_id == CommandId.command_heartbeat_response:
+                responses_received.append(
+                    commands.HeartbeatResponse(command_interpreter.payload),
+                )
+            elif command_id == CommandId.command_led_response:
+                responses_received.append(
+                    commands.LEDResponse(command_interpreter.payload),
+                )
+            elif command_id == CommandId.command_gp_response:
+                responses_received.append(
+                    commands.GPResponse(command_interpreter.payload),
+                )
+            else:
+                raise RuntimeError
 
-    return commands_received, responses_received
+        return commands_received, responses_received
 
 
 _process: Dict[str, Process] = dict()

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,
@@ -77,8 +82,14 @@ class Response(abc.ABC):
 
 
 class Command(abc.ABC):
-    def __init__(self) -> None:
-        self._logger = logging.getLogger(self.__class__.__name__)
+    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
@@ -98,17 +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) -> None:
-        super().__init__()
+    def __init__(self, *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
 
         self.response_identifier = randint(0, pow(2, 16) - 1)
 
@@ -142,11 +157,8 @@ class LogCommand(Command):
 
     HEADER_SIZE = 2  # log level + logger name length
 
-    def __init__(
-        self,
-        data: bytes,
-    ) -> None:
-        super().__init__()
+    def __init__(self, data: bytes, *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
 
         self._logger.setLevel(logging.INFO)
 
@@ -160,7 +172,7 @@ class LogCommand(Command):
         self._logger.debug("logger_name " + str(logger_name))
         self._logger.debug("Message: " + str(message))
 
-        self.received_logger = logging.getLogger(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()
@@ -215,17 +227,11 @@ class LEDResponse(Response):
         data: bytes,
     ):
         self.was_successful = bool(data[0])
-        if self.was_successful:
-            self._logger.debug("LED command was successful")
-        else:
-            self._logger.debug("LED command was not successful")
 
 
 class LEDRequest(Request):
     def __init__(
-        self,
-        id: Union[int, str],
-        command: Union[int, str],
+        self, id: Union[int, str], command: Union[int, str], *args, **kwargs
     ) -> None:
         """
         led_id
@@ -240,7 +246,7 @@ class LEDRequest(Request):
         1: on
         2: toggle
         """
-        super().__init__()
+        super().__init__(*args, **kwargs)
         try:
             self.led_id = int(id)
         except ValueError:
@@ -265,11 +271,15 @@ class LEDRequest(Request):
 
     @property
     def timeout(self) -> float:
-        return 0.1
+        return 1
 
     def process_response(self, response: Response):
         if not isinstance(response, LEDResponse):
             raise TypeError(f"{response} is not a {LEDResponse}")
+        if response.was_successful:
+            self._logger.debug("LED command was successful")
+        else:
+            self._logger.debug("LED command was not successful")
 
     def execute(self, serial: Serial):
         payload = pack(
@@ -291,18 +301,11 @@ class GPResponse(Response):
         data: bytes,
     ):
         self.was_successful = bool(data[0])
-        if self.was_successful:
-            self._logger.debug("GP command was successful")
-        else:
-            self._logger.debug("GP command was not successful")
 
 
 class GPRequest(Request):
-    def __init__(
-        self,
-        command_id: Union[int, str],
-    ) -> None:
-        super().__init__()
+    def __init__(self, command_id: Union[int, str], *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
         self.command_id = int(command_id)
 
     @property
@@ -316,6 +319,10 @@ class GPRequest(Request):
     def process_response(self, response: Response):
         if not isinstance(response, GPResponse):
             raise TypeError(f"{response} is not a {GPResponse}")
+        if response.was_successful:
+            self._logger.debug("GP command was successful")
+        else:
+            self._logger.debug("GP command was not successful")
 
     def execute(self, serial: Serial):
         payload = pack(

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/Core/Inc/stm32wbxx_it.h

@@ -56,7 +56,6 @@ void SVC_Handler(void);
 void DebugMon_Handler(void);
 void PendSV_Handler(void);
 void SysTick_Handler(void);
-void EXTI4_IRQHandler(void);
 void USB_LP_IRQHandler(void);
 void HSEM_IRQHandler(void);
 /* USER CODE BEGIN EFP */

nucleo-wb55-ble/Core/Src/app_entry.c

@@ -96,15 +96,6 @@ void APPE_Init( void )
    return;
 }
 /* USER CODE BEGIN FD */
-void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
-	switch (GPIO_Pin) {
-		case SW1_Pin:
-			UTIL_SEQ_SetTask(1<<CFG_TASK_SW1_BUTTON_PUSHED_ID, CFG_SCH_PRIO_0);
-			break;
-		default:
-			break;
-	}
-}
 
 /* USER CODE END FD */
 

nucleo-wb55-ble/Core/Src/stm32wbxx_it.c

@@ -200,20 +200,6 @@ void SysTick_Handler(void)
 /* please refer to the startup file (startup_stm32wbxx.s).                    */
 /******************************************************************************/
 
-/**
-  * @brief This function handles EXTI line4 interrupt.
-  */
-void EXTI4_IRQHandler(void)
-{
-  /* USER CODE BEGIN EXTI4_IRQn 0 */
-
-  /* USER CODE END EXTI4_IRQn 0 */
-  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);
-  /* USER CODE BEGIN EXTI4_IRQn 1 */
-
-  /* USER CODE END EXTI4_IRQn 1 */
-}
-
 /**
   * @brief This function handles USB low priority interrupt, USB wake-up interrupt through EXTI line 28.
   */

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"
-
-/* Private typedef -----------------------------------------------------------*/
+#include "commands.h"
+#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
@@ -118,11 +119,14 @@ static SVCCTL_EvtAckStatus_t PeerToPeer_Event_Handler(void *Event)
   {
     case HCI_VENDOR_SPECIFIC_DEBUG_EVT_CODE:
     {
+      log_debug("PeerToPeer_Event_Handler", "HCI_VENDOR_SPECIFIC_DEBUG_EVT_CODE", 0);
       blecore_evt = (evt_blecore_aci*)event_pckt->data;
       switch(blecore_evt->ecode)
       {
         case ACI_GATT_ATTRIBUTE_MODIFIED_VSEVT_CODE:
        {
+          log_debug("PeerToPeer_Event_Handler", "blecore_evt->ecode=ACI_GATT_ATTRIBUTE_MODIFIED_VSEVT_CODE", 0);
+          log_debug("PeerToPeer_Event_Handler", "aPeerToPeerContext.P2PNotifyServerToClientCharHdle=0x%x", 1, aPeerToPeerContext.P2PNotifyServerToClientCharHdle);
           attribute_modified = (aci_gatt_attribute_modified_event_rp0*)blecore_evt->data;
             if(attribute_modified->Attr_Handle == (aPeerToPeerContext.P2PNotifyServerToClientCharHdle + 2))
             {
@@ -147,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))
@@ -220,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 */
@@ -234,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 */
@@ -290,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

@@ -139,7 +139,8 @@ void P2PS_APP_Notification(P2PS_APP_ConnHandle_Not_evt_t *pNotification)
 void P2PS_APP_Init(void)
 {
 /* USER CODE BEGIN P2PS_APP_Init */
-	UTIL_SEQ_RegTask(CFG_TASK_SW1_BUTTON_PUSHED_ID, UTIL_SEQ_DEFAULT, P2PS_Send_Notification);
+	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

@@ -0,0 +1,51 @@
+/*
+ * GPCommand.cpp
+ *
+ *  Created on: Jul 18, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#include "GPCommand.hpp"
+
+#include "app_conf.h"
+#include "stm32_seq.h"
+
+
+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(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;
+    return;
+  }
+
+  uint8_t * data_ptr = payload_ptr + this->buffer_offset;
+  this->command_id = data_ptr[0];
+}
+
+GPResponse * GPRequest::execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) {
+  bool was_successful = true;
+
+  if (this->has_error) {
+    was_successful = false;
+  } else {
+    switch (this->command_id)
+    {
+    case 1:
+      UTIL_SEQ_SetTask(1<<CFG_TASK_SW1_BUTTON_PUSHED_ID, CFG_SCH_PRIO_0);
+      break;
+
+    default:
+      break;
+    }
+  }
+
+  return new GPResponse(com_channel_type, response_identifier, was_successful);
+}
+

nucleo-wb55-ble/commands/GPCommand.hpp

@@ -0,0 +1,46 @@
+/*
+ * GPCommand.hpp
+ *
+ *  Created on: Jul 18, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#ifndef GPCOMMAND_HPP_
+#define GPCOMMAND_HPP_
+
+#include "Request.hpp"
+#include "Response.hpp"
+
+class GPResponse : public Response {
+public:
+  GPResponse(com_channel_type_t com_channel_type, uint16_t response_identifier, bool was_successful);
+
+  CommandId get_command_id() override { return COMMAND_GP_RESPONSE; }
+
+private:
+  bool was_successful = false;
+};
+
+class GPRequest : public Request {
+public:
+  /**
+   * @brief Construct a new general purpose request object
+   *
+   * @param payload_ptr
+   *    HEADER | command_id |
+   *           | 1 byte     |
+   *
+   * @param size
+   */
+  GPRequest(com_channel_type_t com_channel_type, uint8_t * payload_ptr, uint16_t size);
+
+  GPResponse * execute_request(com_channel_type_t com_channel_type, uint16_t response_identifier) override;
+
+  CommandId get_command_id() override { return COMMAND_GP_REQUEST; }
+
+private:
+  bool has_error = false;
+  int command_id = 0;
+};
+
+#endif /* GPCOMMAND_HPP_ */

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

@@ -9,20 +9,40 @@
 
 #include "HeartbeatCommand.hpp"
 #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(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/app_ble.c

@@ -323,7 +323,8 @@ void APP_BLE_Init( void )
 
     /*Radio mask Activity*/
 #if (OOB_DEMO != 0)
-  aci_hal_set_radio_activity_mask(0x0020);
+  aci_hal_set_radio_activity_mask(0x0020); // connection event master
+  log_debug("APP_BLE_Init", "aci_hal_set_radio_activity_mask=0x0020 (connection event master)", 0);
 #endif
   /**
    * Initialize P2P Client Application
@@ -339,6 +340,7 @@ void APP_BLE_Init( void )
    * Start scanning
    */
   UTIL_SEQ_SetTask(1 << CFG_TASK_START_SCAN_ID, CFG_SCH_PRIO_0);
+  log_debug("APP_BLE_Init", "trigger scanning task", 0);
 #endif
 /* USER CODE BEGIN APP_BLE_Init_2 */
   log_debug("APP_BLE_Init", "done", 0);
@@ -391,12 +393,12 @@ SVCCTL_UserEvtFlowStatus_t SVCCTL_App_Notification( void *pckt )
           {
             /* USER CODE BEGIN GAP_GENERAL_DISCOVERY_PROC */
 
-            log_debug("SVCCTL_App_Notification", "-- GAP GENERAL DISCOVERY PROCEDURE_COMPLETED", 0);
             /* USER CODE END GAP_GENERAL_DISCOVERY_PROC */
-            APP_DBG_MSG("-- GAP GENERAL DISCOVERY PROCEDURE_COMPLETED\n");
+            log_debug("SVCCTL_App_Notification", "-- GAP GENERAL DISCOVERY PROCEDURE_COMPLETED", 0);
             /*if a device found, connect to it, device 1 being chosen first if both found*/
             if (BleApplicationContext.DeviceServerFound == 0x01 && BleApplicationContext.Device_Connection_Status != APP_BLE_CONNECTED_CLIENT)
             {
+              log_debug("SVCCTL_App_Notification", "Set task: CFG_TASK_CONN_DEV_1_ID", 0);
               UTIL_SEQ_SetTask(1 << CFG_TASK_CONN_DEV_1_ID, CFG_SCH_PRIO_0);
             }
           }
@@ -407,6 +409,7 @@ SVCCTL_UserEvtFlowStatus_t SVCCTL_App_Notification( void *pckt )
         {
           /* USER CODE BEGIN EVT_BLUE_L2CAP_CONNECTION_UPDATE_REQ */
 
+          log_debug("SVCCTL_App_Notification", "Event: ACI_L2CAP_CONNECTION_UPDATE_REQ_VSEVT_CODE", 0);
           /* USER CODE END EVT_BLUE_L2CAP_CONNECTION_UPDATE_REQ */
           aci_l2cap_connection_update_req_event_rp0 *pr = (aci_l2cap_connection_update_req_event_rp0 *) blecore_evt->data;
           aci_hal_set_radio_activity_mask(0x0000);
@@ -504,7 +507,7 @@ SVCCTL_UserEvtFlowStatus_t SVCCTL_App_Notification( void *pckt )
           BleApplicationContext.Device_Connection_Status = APP_BLE_CONNECTED_CLIENT;
 
           /* CONNECTION WITH CLIENT */
-          APP_DBG_MSG("\r\n\r** CONNECTION EVENT WITH SERVER \n");
+          log_debug("SVCCTL_App_Notification", "** CONNECTION EVENT WITH SERVER", 0);
           handleNotification.P2P_Evt_Opcode = PEER_CONN_HANDLE_EVT;
           handleNotification.ConnectionHandle = BleApplicationContext.BleApplicationContext_legacy.connectionHandle;
           P2PC_APP_Notification(&handleNotification);
@@ -512,12 +515,12 @@ SVCCTL_UserEvtFlowStatus_t SVCCTL_App_Notification( void *pckt )
           result = aci_gatt_disc_all_primary_services(BleApplicationContext.BleApplicationContext_legacy.connectionHandle);
           if (result == BLE_STATUS_SUCCESS)
           {
-            APP_DBG_MSG("\r\n\r** GATT SERVICES & CHARACTERISTICS DISCOVERY  \n");
-            APP_DBG_MSG("* GATT :  Start Searching Primary Services \r\n\r");
+            log_debug("SVCCTL_App_Notification", "** GATT SERVICES & CHARACTERISTICS DISCOVERY", 0);
+            log_debug("SVCCTL_App_Notification", "* GATT :  Start Searching Primary Services", 0);
           }
           else
           {
-            APP_DBG_MSG("BLE_CTRL_App_Notification(), All services discovery Failed \r\n\r");
+            log_error("SVCCTL_App_Notification", "All services discovery Failed", 0);
           }
 
           break; /* HCI_LE_CONNECTION_COMPLETE_SUBEVT_CODE */
@@ -672,17 +675,20 @@ void APP_BLE_Key_Button1_Action(void)
  *************************************************************/
 static void Ble_Tl_Init( void )
 {
+  log_debug("Ble_Tl_Init", "start", 0);
   HCI_TL_HciInitConf_t Hci_Tl_Init_Conf;
 
   Hci_Tl_Init_Conf.p_cmdbuffer = (uint8_t*)&BleCmdBuffer;
   Hci_Tl_Init_Conf.StatusNotCallBack = BLE_StatusNot;
   hci_init(BLE_UserEvtRx, (void*) &Hci_Tl_Init_Conf);
 
+  log_debug("Ble_Tl_Init", "end", 0);
   return;
 }
 
- static void Ble_Hci_Gap_Gatt_Init(void){
+static void Ble_Hci_Gap_Gatt_Init(void){
 
+  log_debug("Ble_Hci_Gap_Gatt_Init", "start", 0);
   uint8_t role;
   uint16_t gap_service_handle, gap_dev_name_char_handle, gap_appearance_char_handle;
   const uint8_t *bd_addr;
@@ -751,6 +757,7 @@ static void Ble_Tl_Init( void )
   {
     const char *name = "Monsun1";
 
+    log_debug("Ble_Hci_Gap_Gatt_Init", "aci_gap_init()", 0);
     aci_gap_init(role, 0,
                  APPBLE_GAP_DEVICE_NAME_LENGTH,
                  &gap_service_handle, &gap_dev_name_char_handle, &gap_appearance_char_handle);
@@ -761,14 +768,15 @@ static void Ble_Tl_Init( void )
     }
   }
 
-    if(aci_gatt_update_char_value(gap_service_handle,
-                                  gap_appearance_char_handle,
-                                  0,
-                                  2,
-                                 (uint8_t *)&appearance))
-    {
-      BLE_DBG_SVCCTL_MSG("Appearance aci_gatt_update_char_value failed.\n");
-    }
+  log_debug("Ble_Hci_Gap_Gatt_Init", "aci_gatt_update_char_value()", 0);
+  if(aci_gatt_update_char_value(gap_service_handle,
+                                gap_appearance_char_handle,
+                                0,
+                                2,
+                                (uint8_t *)&appearance))
+  {
+    BLE_DBG_SVCCTL_MSG("Appearance aci_gatt_update_char_value failed.\n");
+  }
 
     /**
      * Initialize IO capability
@@ -786,6 +794,7 @@ static void Ble_Tl_Init( void )
     BleApplicationContext.BleApplicationContext_legacy.bleSecurityParam.Fixed_Pin = CFG_FIXED_PIN;
     BleApplicationContext.BleApplicationContext_legacy.bleSecurityParam.bonding_mode = CFG_BONDING_MODE;
 
+    log_debug("Ble_Hci_Gap_Gatt_Init", "aci_gap_set_authentication_requirement()", 0);
     aci_gap_set_authentication_requirement(BleApplicationContext.BleApplicationContext_legacy.bleSecurityParam.bonding_mode,
                                            BleApplicationContext.BleApplicationContext_legacy.bleSecurityParam.mitm_mode,
                                            CFG_SC_SUPPORT,
@@ -802,16 +811,18 @@ static void Ble_Tl_Init( void )
      */
     if (BleApplicationContext.BleApplicationContext_legacy.bleSecurityParam.bonding_mode)
     {
+      log_debug("Ble_Hci_Gap_Gatt_Init", "aci_gap_configure_whitelist()", 0);
       aci_gap_configure_whitelist();
     }
 
+  log_debug("Ble_Hci_Gap_Gatt_Init", "end", 0);
 }
 
 static void Scan_Request( void )
 {
   /* USER CODE BEGIN Scan_Request_1 */
 
-  log_debug("Scan_Request", "enter", 0);
+  log_debug("Scan_Request", "start", 0);
   /* USER CODE END Scan_Request_1 */
   tBleStatus result;
   if (BleApplicationContext.Device_Connection_Status != APP_BLE_CONNECTED_CLIENT)
@@ -819,22 +830,21 @@ static void Scan_Request( void )
     /* USER CODE BEGIN APP_BLE_CONNECTED_CLIENT */
 
     /* USER CODE END APP_BLE_CONNECTED_CLIENT */
+    log_debug("Scan_Request", "aci_gap_start_general_discovery_proc()", 0);
     result = aci_gap_start_general_discovery_proc(SCAN_P, SCAN_L, PUBLIC_ADDR, 1);
     if (result == BLE_STATUS_SUCCESS)
     {
     /* USER CODE BEGIN BLE_SCAN_SUCCESS */
 
-      log_debug("Scan_Request", "START GENERAL DISCOVERY (SCAN)", 0);
     /* USER CODE END BLE_SCAN_SUCCESS */
-      APP_DBG_MSG(" \r\n\r** START GENERAL DISCOVERY (SCAN) **  \r\n\r");
+      log_debug("Scan_Request", "** START GENERAL DISCOVERY (SCAN) **", 0);
     }
     else
     {
     /* USER CODE BEGIN BLE_SCAN_FAILED */
 
-    log_debug("Scan_Request", "BLE_App_Start_Limited_Disc_Req, Failed", 0);
     /* USER CODE END BLE_SCAN_FAILED */
-      APP_DBG_MSG("-- BLE_App_Start_Limited_Disc_Req, Failed \r\n\r");
+      log_debug("Scan_Request", "-- BLE_App_Start_Limited_Disc_Req, Failed ", 0);
     }
   }
   /* USER CODE BEGIN Scan_Request_2 */
@@ -847,14 +857,15 @@ static void Connect_Request( void )
 {
   /* USER CODE BEGIN Connect_Request_1 */
 
-  log_debug("Scan_Request", "enter", 0);
+  log_debug("Connect_Request", "start", 0);
   /* USER CODE END Connect_Request_1 */
   tBleStatus result;
 
-  APP_DBG_MSG("\r\n\r** CREATE CONNECTION TO SERVER **  \r\n\r");
+  log_debug("Connect_Request", "** CREATE CONNECTION TO SERVER **", 0);
 
   if (BleApplicationContext.Device_Connection_Status != APP_BLE_CONNECTED_CLIENT)
   {
+    log_debug("Connect_Request", "aci_gap_create_connection()", 0);
     result = aci_gap_create_connection(SCAN_P,
                                        SCAN_L,
                                        PUBLIC_ADDR, SERVER_REMOTE_BDADDR,
@@ -871,6 +882,7 @@ static void Connect_Request( void )
     /* USER CODE BEGIN BLE_CONNECT_SUCCESS */
 
     /* USER CODE END BLE_CONNECT_SUCCESS */
+    log_debug("Device_Connection_Status", "APP_BLE_LP_CONNECTING", 0);
     BleApplicationContext.Device_Connection_Status = APP_BLE_LP_CONNECTING;
 
     }
@@ -879,6 +891,8 @@ static void Connect_Request( void )
     /* USER CODE BEGIN BLE_CONNECT_FAILED */
 
     /* USER CODE END BLE_CONNECT_FAILED */
+      log_error("Connect_Request", "Connection failed", 0);
+      log_debug("Device_Connection_Status", "APP_BLE_IDLE", 0);
       BleApplicationContext.Device_Connection_Status = APP_BLE_IDLE;
 
     }
@@ -892,11 +906,13 @@ static void Connect_Request( void )
 static void Switch_OFF_GPIO(){
 /* USER CODE BEGIN Switch_OFF_GPIO */
 
+  log_debug("Switch_OFF_GPIO", "start/end", 0);
 /* USER CODE END Switch_OFF_GPIO */
 }
 
 const uint8_t* BleGetBdAddress( void )
 {
+  log_debug("BleGetBdAddress", "start", 0);
   uint8_t *otp_addr;
   const uint8_t *bd_addr;
   uint32_t udn;
@@ -940,6 +956,7 @@ const uint8_t* BleGetBdAddress( void )
     }
   }
 
+  log_debug("BleGetBdAddress", "end", 0);
   return bd_addr;
 }
 /* USER CODE BEGIN FD_LOCAL_FUNCTIONS */
@@ -970,6 +987,7 @@ void hci_cmd_resp_wait(uint32_t timeout)
 
 static void BLE_UserEvtRx( void * pPayload )
 {
+  // log_debug("BLE_UserEvtRx", "start", 0);
   SVCCTL_UserEvtFlowStatus_t svctl_return_status;
   tHCI_UserEvtRxParam *pParam;
 
@@ -984,10 +1002,12 @@ static void BLE_UserEvtRx( void * pPayload )
 {
     pParam->status = HCI_TL_UserEventFlow_Disable;
   }
+  // log_debug("BLE_UserEvtRx", "end", 0);
 }
 
 static void BLE_StatusNot( HCI_TL_CmdStatus_t status )
 {
+  // log_debug("BLE_StatusNot", "start", 0);
   uint32_t task_id_list;
   switch (status)
   {
@@ -1014,6 +1034,7 @@ static void BLE_StatusNot( HCI_TL_CmdStatus_t status )
     default:
       break;
   }
+  // log_debug("BLE_StatusNot", "end", 0);
   return;
 }
 

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

@@ -20,6 +20,7 @@
 
 /* Includes ------------------------------------------------------------------*/
 
+#include <stdio.h>
 #include "main.h"
 #include "app_common.h"
 
@@ -33,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 -----------------------------------------------------------*/
@@ -163,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 */
 
@@ -176,9 +181,10 @@ void P2PC_APP_Init(void)
 {
   uint8_t index =0;
 /* USER CODE BEGIN P2PC_APP_Init_1 */
-  log_debug("P2PC_APP_Init", "enter", 0);
+  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
@@ -190,6 +196,7 @@ void P2PC_APP_Init(void)
   P2P_Client_App_Context.LedControl.Led1=0x00; /* led OFF */
   P2P_Client_App_Context.ButtonStatus.Device_Button_Selection=0x01;/* Device1 */
   P2P_Client_App_Context.ButtonStatus.Button1=0x00;
+  log_debug("P2PC_APP_Init", "BLE_CFG_CLT_MAX_NBR_CB=%d", 1, BLE_CFG_CLT_MAX_NBR_CB);
 /* USER CODE END P2PC_APP_Init_1 */
   for(index = 0; index < BLE_CFG_CLT_MAX_NBR_CB; index++)
   {
@@ -207,6 +214,7 @@ void P2PC_APP_Init(void)
 
 /* USER CODE BEGIN P2PC_APP_Init_2 */
 
+  log_debug("P2PC_APP_Init", "end", 0);
 /* USER CODE END P2PC_APP_Init_2 */
   return;
 }
@@ -214,7 +222,7 @@ void P2PC_APP_Init(void)
 void P2PC_APP_Notification(P2PC_APP_ConnHandle_Not_evt_t *pNotification)
 {
 /* USER CODE BEGIN P2PC_APP_Notification_1 */
-  log_debug("P2PC_APP_Notification", "enter", 0);
+  log_debug("P2PC_APP_Notification", "start", 0);
 /* USER CODE END P2PC_APP_Notification_1 */
   switch(pNotification->P2P_Evt_Opcode)
   {
@@ -224,13 +232,15 @@ void P2PC_APP_Notification(P2PC_APP_ConnHandle_Not_evt_t *pNotification)
 
   case PEER_CONN_HANDLE_EVT :
 /* USER CODE BEGIN PEER_CONN_HANDLE_EVT */
+    log_debug("P2PC_APP_Notification", "PEER_CONN_HANDLE_EVT", 0);
     P2P_Client_App_Context.ConnectionHandle = pNotification->ConnectionHandle;
 /* USER CODE END PEER_CONN_HANDLE_EVT */
-      break;
+    break;
 
     case PEER_DISCON_HANDLE_EVT :
 /* USER CODE BEGIN PEER_DISCON_HANDLE_EVT */
       {
+      log_debug("P2PC_APP_Notification", "PEER_DISCON_HANDLE_EVT", 0);
       uint8_t index = 0;
       P2P_Client_App_Context.ConnectionHandle =  0x00;
       while((index < BLE_CFG_CLT_MAX_NBR_CB) &&
@@ -261,6 +271,7 @@ void P2PC_APP_Notification(P2PC_APP_ConnHandle_Not_evt_t *pNotification)
 /* USER CODE BEGIN FD */
 void P2PC_APP_SW1_Button_Action(void)
 {
+  log_debug("P2PC_APP_SW1_Button_Action", "set task: BLE_CFG_CLT_MAX_NBR_CB", 0);
   UTIL_SEQ_SetTask(1<<CFG_TASK_SW1_BUTTON_PUSHED_ID, CFG_SCH_PRIO_0);
 }
 /* USER CODE END FD */
@@ -282,7 +293,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
   hci_event_pckt *event_pckt;
   evt_blecore_aci *blecore_evt;
 
-  log_debug("Event_Handler", "enter", 0);
+  log_debug("Event_Handler", "start", 0);
   P2P_Client_App_Notification_evt_t Notification;
 
   return_value = SVCCTL_EvtNotAck;
@@ -292,12 +303,14 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
   {
     case HCI_VENDOR_SPECIFIC_DEBUG_EVT_CODE:
     {
+      // log_debug("Event_Handler", "HCI_VENDOR_SPECIFIC_DEBUG_EVT_CODE", 0);
       blecore_evt = (evt_blecore_aci*)event_pckt->data;
       switch(blecore_evt->ecode)
       {
 
         case ACI_ATT_READ_BY_GROUP_TYPE_RESP_VSEVT_CODE:
         {
+          log_debug("Event_Handler", "blecore_evt->ecode=ACI_ATT_READ_BY_GROUP_TYPE_RESP_VSEVT_CODE", 0);
           aci_att_read_by_group_type_resp_event_rp0 *pr = (void*)blecore_evt->data;
           uint8_t numServ, i, idx;
           uint16_t uuid, handle;
@@ -315,7 +328,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
             if((aP2PClientContext[index].state == APP_BLE_CONNECTED_CLIENT)&&
                     (status == APP_BLE_IDLE))
             {
-              /* Handle deconnected */
+              /* Handle disconnected */
 
               aP2PClientContext[index].state = APP_BLE_IDLE;
               aP2PClientContext[index].connHandle = 0xFFFF;
@@ -351,16 +364,23 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
                 uuid = UNPACK_2_BYTE_PARAMETER(&pr->Attribute_Data_List[idx]);
                 if(uuid == P2P_SERVICE_UUID)
                 {
+                  log_debug("Event_Handler", "uuid=0x%x", 1, uuid);
+                  log_debug("Event_Handler", "-- GATT : P2P_SERVICE_UUID FOUND - connection handle=0x%x", 1, aP2PClientContext[index].connHandle);
 #if(CFG_DEBUG_APP_TRACE != 0)
                   APP_DBG_MSG("-- GATT : P2P_SERVICE_UUID FOUND - connection handle 0x%x \n", aP2PClientContext[index].connHandle);
 #endif
 #if (UUID_128BIT_FORMAT==1)
+                /*
+                | 2 byte             | 2 byte                 | 14 byte | 2 byte |
+                | p2p service handle | p2p service end handle | ????    | uuid   |
+                */
                 aP2PClientContext[index].P2PServiceHandle = UNPACK_2_BYTE_PARAMETER(&pr->Attribute_Data_List[idx-16]);
                 aP2PClientContext[index].P2PServiceEndHandle = UNPACK_2_BYTE_PARAMETER (&pr->Attribute_Data_List[idx-14]);
 #else
                 aP2PClientContext[index].P2PServiceHandle = UNPACK_2_BYTE_PARAMETER(&pr->Attribute_Data_List[idx-4]);
                 aP2PClientContext[index].P2PServiceEndHandle = UNPACK_2_BYTE_PARAMETER (&pr->Attribute_Data_List[idx-2]);
 #endif
+                  log_debug("Event_Handler", "aP2PClientContext.state=APP_BLE_DISCOVER_CHARACS", 0);
                   aP2PClientContext[index].state = APP_BLE_DISCOVER_CHARACS ;
                 }
                 idx += 6;
@@ -373,6 +393,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
         case ACI_ATT_READ_BY_TYPE_RESP_VSEVT_CODE:
         {
 
+          log_debug("Event_Handler", "blecore_evt->ecode=ACI_ATT_READ_BY_TYPE_RESP_VSEVT_CODE", 0);
           aci_att_read_by_type_resp_event_rp0 *pr = (void*)blecore_evt->data;
           uint8_t idx;
           uint16_t uuid, handle;
@@ -406,6 +427,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
               pr->Data_Length -= 1;
               while(pr->Data_Length > 0)
               {
+                log_debug("Event_Handler", "uuid=0x%x", 1, uuid);
                 uuid = UNPACK_2_BYTE_PARAMETER(&pr->Handle_Value_Pair_Data[idx]);
                 /* store the characteristic handle not the attribute handle */
 #if (UUID_128BIT_FORMAT==1)
@@ -415,6 +437,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 #endif
                 if(uuid == P2P_WRITE_CHAR_UUID)
                 {
+                  log_debug("Event_Handler", "-- GATT : WRITE_UUID FOUND - connection handle=0x%x", 1, aP2PClientContext[index].connHandle);
 #if(CFG_DEBUG_APP_TRACE != 0)
                   APP_DBG_MSG("-- GATT : WRITE_UUID FOUND - connection handle 0x%x\n", aP2PClientContext[index].connHandle);
 #endif
@@ -424,6 +447,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 
                 else if(uuid == P2P_NOTIFY_CHAR_UUID)
                 {
+                  log_debug("Event_Handler", "-- GATT : NOTIFICATION_CHAR_UUID FOUND - connection handle=0x%x", 1, aP2PClientContext[index].connHandle);
 #if(CFG_DEBUG_APP_TRACE != 0)
                   APP_DBG_MSG("-- GATT : NOTIFICATION_CHAR_UUID FOUND  - connection handle 0x%x\n", aP2PClientContext[index].connHandle);
 #endif
@@ -445,6 +469,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 
         case ACI_ATT_FIND_INFO_RESP_VSEVT_CODE:
         {
+          log_debug("Event_Handler", "blecore_evt->ecode=ACI_ATT_FIND_INFO_RESP_VSEVT_CODE", 0);
           aci_att_find_info_resp_event_rp0 *pr = (void*)blecore_evt->data;
 
           uint8_t numDesc, idx, i;
@@ -479,6 +504,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 
                 if(uuid == CLIENT_CHAR_CONFIG_DESCRIPTOR_UUID)
                 {
+                  log_debug("Event_Handler", "-- GATT : CLIENT_CHAR_CONFIG_DESCRIPTOR_UUID FOUND - connection handle=0x%x", 1, aP2PClientContext[index].connHandle);
 #if(CFG_DEBUG_APP_TRACE != 0)
                   APP_DBG_MSG("-- GATT : CLIENT_CHAR_CONFIG_DESCRIPTOR_UUID- connection handle 0x%x\n", aP2PClientContext[index].connHandle);
 #endif
@@ -499,6 +525,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 
         case ACI_GATT_NOTIFICATION_VSEVT_CODE:
         {
+          log_debug("Event_Handler", "blecore_evt->ecode=ACI_GATT_NOTIFICATION_VSEVT_CODE", 0);
           aci_gatt_notification_event_rp0 *pr = (void*)blecore_evt->data;
           uint8_t index;
 
@@ -509,6 +536,17 @@ 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');
+            uint8_t i;
+            for (i = 0; i<pr->Attribute_Value_Length; i++) {
+              snprintf((char*)(buffer + i + 2), 512-i-2, "%x", pr->Attribute_Value[i]);
+            }
+            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)) )
@@ -522,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 
+              );
             }
           }
         }
@@ -529,6 +572,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 
         case ACI_GATT_PROC_COMPLETE_VSEVT_CODE:
         {
+          log_debug("Event_Handler", "blecore_evt->ecode=ACI_GATT_PROC_COMPLETE_VSEVT_CODE", 0);
           aci_gatt_proc_complete_event_rp0 *pr = (void*)blecore_evt->data;
 #if(CFG_DEBUG_APP_TRACE != 0)
           APP_DBG_MSG("-- GATT : ACI_GATT_PROC_COMPLETE_VSEVT_CODE \n");
@@ -561,6 +605,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
       break;
   }
 
+  log_debug("Event_Handler", "end", 0);
   return(return_value);
 }/* end BLE_CTRL_Event_Acknowledged_Status_t */
 
@@ -568,6 +613,8 @@ void Gatt_Notification(P2P_Client_App_Notification_evt_t *pNotification)
 {
 /* USER CODE BEGIN Gatt_Notification_1*/
   log_debug("Gatt_Notification", "enter", 0);
+  log_debug("Gatt_Notification", "pNotification->P2P_Client_Evt_Opcode=0x%x", 1, pNotification->P2P_Client_Evt_Opcode);
+
 
 /* USER CODE END Gatt_Notification_1 */
   switch(pNotification->P2P_Client_Evt_Opcode)
@@ -580,6 +627,8 @@ void Gatt_Notification(P2P_Client_App_Notification_evt_t *pNotification)
 /* USER CODE BEGIN P2P_NOTIFICATION_INFO_RECEIVED_EVT */
     {
       P2P_Client_App_Context.LedControl.Device_Led_Selection=pNotification->DataTransfered.pPayload[0];
+      log_debug("Gatt_Notification", "DataTransfered.pPayload[0]=0x%x", 1, pNotification->DataTransfered.pPayload[0]);
+      log_debug("Gatt_Notification", "DataTransfered.pPayload[1]=0x%x", 1, pNotification->DataTransfered.pPayload[1]);
       switch(P2P_Client_App_Context.LedControl.Device_Led_Selection) {
 
         case 0x01 : {
@@ -669,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_ */

request-sequence-diagram.puml

@@ -0,0 +1,33 @@
+@startuml
+participant backend as be
+participant usb_receive as usb_r
+participant command_exucutor as ce
+participant Request as req
+participant Response as resp
+
+be -> usb_r : send command()
+usb_r -> ce : enqueue command()
+ce -> req : execute()
+activate req
+    req -> req : interpret_request()
+    req -> req : execute_request()
+    activate req
+        create resp
+        req -> resp : new
+        activate resp
+            req -> resp : execute()
+            resp -> resp : set_payload()
+            activate resp
+                resp -> resp : set_response_payload()
+                activate resp
+                resp --> resp : response_payload_size
+                deactivate resp
+            resp --> resp : payload_size
+            deactivate resp
+        resp --> req : was_successful
+        deactivate resp
+    req --> req : was_successful
+    deactivate req
+req --> ce : was_successful
+deactivate req
+@enduml