ble-dongle: WIP: pairing procedure

backend/command_execution.py

@@ -214,6 +214,8 @@ def receive(
             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
 

backend/commands.py

@@ -22,11 +22,15 @@ class CommandId(Enum):
     command_led_request = 0x4
     command_led_response = 0x5
 
+    command_gp_request = 0x6
+    command_gp_response = 0x7
+
 
 def get_command_id_from_name(name: str) -> CommandId:
     return {
         "log": CommandId.command_log,
         "led": CommandId.command_led_request,
+        "gp": CommandId.command_gp_request,
     }[name]
 
 
@@ -37,6 +41,7 @@ def get_request_class(
         CommandId.command_log: LogCommand,
         CommandId.command_heartbeat_request: HeartbeatRequest,
         CommandId.command_led_request: LEDRequest,
+        CommandId.command_gp_request: GPRequest,
     }[command_id]
 
 
@@ -46,6 +51,7 @@ def get_response_class(
     return {
         CommandId.command_heartbeat_response: HeartbeatResponse,
         CommandId.command_led_response: LEDResponse,
+        CommandId.command_gp_response: GPResponse,
     }[command_id]
 
 
@@ -134,24 +140,28 @@ class LogCommand(Command):
     level: int
     message: str
 
-    HEADER_SIZE = 1  # log level
+    HEADER_SIZE = 2  # log level + logger name length
 
     def __init__(
         self,
         data: bytes,
     ) -> None:
         super().__init__()
-        self.received_logger = logging.getLogger("stm32wb55")
-        self.received_logger.setLevel(logging.DEBUG)
 
         self._logger.setLevel(logging.INFO)
 
         level = int(data[0])
+        logger_name_length = int(data[1])
         self._logger.debug(f"level: {level}")
+        self._logger.debug(f"logger_name_length: {logger_name_length}")
 
-        message = data[self.HEADER_SIZE :]
+        logger_name = data[self.HEADER_SIZE : self.HEADER_SIZE + logger_name_length]
+        message = data[self.HEADER_SIZE + logger_name_length :]
+        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.setLevel(logging.DEBUG)
         self.level = level
         self.message = message.decode()
 
@@ -271,3 +281,48 @@ class LEDRequest(Request):
             payload=payload,
             serial=serial,
         )
+
+
+class GPResponse(Response):
+    was_successful = True
+
+    def unpack_payload(
+        self,
+        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__()
+        self.command_id = int(command_id)
+
+    @property
+    def identifier(self) -> CommandId:
+        return CommandId.command_gp_request
+
+    @property
+    def timeout(self) -> float:
+        return 0.1
+
+    def process_response(self, response: Response):
+        if not isinstance(response, GPResponse):
+            raise TypeError(f"{response} is not a {GPResponse}")
+
+    def execute(self, serial: Serial):
+        payload = pack(
+            ">B",
+            self.command_id,
+        )
+        self.send_command(
+            payload=payload,
+            serial=serial,
+        )

ble-client-seq.puml

@@ -0,0 +1,85 @@
+@startuml
+== Button pressed ==
+participant unknown as uk
+participant main
+participant interrupt as it
+participant app_entry
+participant app_ble
+participant p2p_client_app
+participant stm32_seq
+it -> app_entry : HAL_GPIO_EXTI_Callback()
+app_entry -> app_ble : APP_BLE_Key_Button1_Action()
+app_ble -> p2p_client_app : P2PC_APP_SW1_Button_Action()
+p2p_client_app -> stm32_seq : UTIL_SEQ_SetTask(CFG_TASK_SW1_BUTTON_PUSHED_ID)
+
+...waiting for task gets executed....
+
+stm32_seq -> p2p_client_app : Button_Trigger_Received()
+p2p_client_app -> p2p_client_app : toggleButtonStatus()
+p2p_client_app -> p2p_client_app : Write_Char(P2P_WRITE_CHAR_UUID, 0, buttonStatus)
+activate p2p_client_app
+p2p_client_app -> "?b" : aci_gatt_write_without_resp()
+deactivate p2p_client_app
+
+== Event received ==
+"?a" -> p2p_client_app : Event_Handler()
+activate p2p_client_app
+opt event == HCI_VENDOR_SPECIFIC_DEBUG_EVT_CODE
+opt event->core_event == ACI_GATT_PROC_COMPLETE_VSEVT_CODE
+opt aP2PClientContext[index].connHandle != pr->Connection_Handle
+p2p_client_app -> stm32_seq : UTIL_SEQ_SetTask(CFG_TASK_SEARCH_SERVICE_ID)
+end
+end
+end
+deactivate p2p_client_app
+
+...waiting for task gets executed....
+
+stm32_seq -> p2p_client_app : Update_Service()
+
+......
+uk -> app_entry : UTIL_SEQ_Idle()
+uk -> app_entry : shci_notify_asynch_evt()
+activate app_entry
+    app_entry -> stm32_seq : UTIL_SEQ_SetTask(CFG_TASK_SYSTEM_HCI_ASYNCH_EVT_ID)
+deactivate app_entry
+uk -> app_entry : shci_cmd_resp_release()
+activate app_entry
+    app_entry -> stm32_seq : UTIL_SEQ_SetEvt(CFG_IDLEEVT_SYSTEM_HCI_CMD_EVT_RSP_ID)
+deactivate app_entry
+uk -> app_entry : shci_cmd_resp_wait()
+activate app_entry
+    app_entry -> stm32_seq : UTIL_SEQ_WaitEvt(CFG_IDLEEVT_SYSTEM_HCI_CMD_EVT_RSP_ID)
+deactivate app_entry
+
+== INIT ==
+main -> stm32_seq : UTIL_SEQ_Run(UTIL_SEQ_DEFAULT)
+activate stm32_seq
+stm32_seq -> shci_tl : shci_user_evt_proc()
+    activate shci_tl
+    shci_tl -> app_entry : APPE_SysUserEvtRx()
+        activate app_entry
+        app_entry -> app_debug : APPD_EnableCPU2()
+        app_entry -> app_ble : APP_BLE_Init()
+            activate app_ble
+            app_ble -> app_ble : Ble_Tl_Init()
+            app_ble -> stm32_seq : UTIL_SEQ_RegTask(CFG_TASK_HCI_ASYNCH_EVT_ID, UTIL_SEQ_RFU, hci_user_evt_proc)
+            app_ble -> shci : SHCI_C2_BLE_Init()
+            app_ble -> app_ble : Ble_Hci_Gap_Gatt_Init()
+            app_ble -> svc_ctl : SVCCTL_Init()
+            app_ble -> stm32_seq : UTIL_SEQ_RegTask(CFG_TASK_START_SCAN_ID, UTIL_SEQ_RFU, Scan_Request)
+            app_ble -> stm32_seq : UTIL_SEQ_RegTask(CFG_TASK_CONN_DEV_1_ID, UTIL_SEQ_RFU, Connect_Request)
+            app_ble -> uk : aci_hal_set_radio_activity_mask()
+            app_ble -> app_p2p_client : P2PC_APP_Init()
+                activate app_p2p_client
+                app_p2p_client -> svc_ctl : SVCCTL_RegisterCltHandler(Event_Handler)
+                deactivate app_p2p_client
+            app_ble -> hw_timeserver : HW_TS_Create(CFG_TIM_PROC_ID_ISR, &(BleApplicationContext.SwitchOffGPIO_timer_Id), hw_ts_SingleShot, Switch_OFF_GPIO)
+            deactivate app_ble
+        deactivate app_entry
+    deactivate shci_tl
+stm32_seq -> stm32_seq : UTIL_SEQ_PreIdle()
+stm32_seq -> stm32_seq : UTIL_SEQ_Idle()
+stm32_seq -> stm32_seq : UTIL_SEQ_PostIdle()
+deactivate stm32_seq
+@enduml

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

@@ -114,14 +114,15 @@ int main(void)
 
   /* Init code for STM32_WPAN */
   APPE_Init();
+  HAL_Delay(1000);
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
   while (1)
   {
-  	HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET);
-  	HAL_Delay(20);
-  	HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET);
-    HAL_Delay(20);
+  	// HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET);
+  	// HAL_Delay(20);
+  	// HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET);
+    // HAL_Delay(20);
 
     UTIL_SEQ_Run(UTIL_SEQ_DEFAULT);
     pop_and_execute_commands();
@@ -129,10 +130,10 @@ int main(void)
   	// HAL_Delay(450);
     // counter_value++;
 
-  	// log_debug("Counter value is %d", 1, counter_value);
-  	// log_info("Counter value is %d", 1,  counter_value);
-  	// log_warning("Counter value is %d", 1, counter_value);
-  	// log_error("Counter value is %d", 1, counter_value);
+  	// log_debug("main", "Counter value is %d", 1, counter_value);
+  	// log_info("main", "Counter value is %d", 1,  counter_value);
+  	// log_warning("main", "Counter value is %d", 1, counter_value);
+  	// log_error("a very long logger name .........................", "Counter value is %d", 1, counter_value);
 
     /* USER CODE END WHILE */
 

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

@@ -39,7 +39,7 @@
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include "commands.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -341,7 +341,7 @@ void APP_BLE_Init( void )
   UTIL_SEQ_SetTask(1 << CFG_TASK_START_SCAN_ID, CFG_SCH_PRIO_0);
 #endif
 /* USER CODE BEGIN APP_BLE_Init_2 */
-
+  log_debug("APP_BLE_Init", "done", 0);
 /* USER CODE END APP_BLE_Init_2 */
   return;
 }
@@ -360,6 +360,7 @@ SVCCTL_UserEvtFlowStatus_t SVCCTL_App_Notification( void *pckt )
   int k = 0;
   uint8_t adtype, adlength;
 
+  log_debug("SVCCTL_App_Notification", "enter, event=%d", 1, event_pckt->evt);
   switch (event_pckt->evt)
   {
     /* USER CODE BEGIN evt */
@@ -390,6 +391,7 @@ 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");
             /*if a device found, connect to it, device 1 being chosen first if both found*/
@@ -426,7 +428,7 @@ SVCCTL_UserEvtFlowStatus_t SVCCTL_App_Notification( void *pckt )
                                                            0x01);
           if(result != BLE_STATUS_SUCCESS) {
               /* USER CODE BEGIN BLE_STATUS_SUCCESS */
-
+            log_debug("SVCCTL_App_Notification", "BLE_STATUS_SUCCESS: false", 0);
               /* USER CODE END BLE_STATUS_SUCCESS */
           }
           aci_hal_set_radio_activity_mask(0x0020);
@@ -438,6 +440,7 @@ SVCCTL_UserEvtFlowStatus_t SVCCTL_App_Notification( void *pckt )
         case 0x0004:
         {
           /* USER CODE BEGIN RADIO_ACTIVITY_EVENT */
+        log_debug("SVCCTL_App_Notification", "RADIO_ACTIVITY_EVENT", 0);
         HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET);
         HAL_Delay(5);
         HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET);
@@ -646,7 +649,21 @@ APP_BLE_ConnStatus_t APP_BLE_Get_Client_Connection_Status( uint16_t Connection_H
   return APP_BLE_IDLE;
 }
 /* USER CODE BEGIN FD */
-
+void APP_BLE_Key_Button1_Action(void)
+{
+#if OOB_DEMO == 0 
+      P2PC_APP_SW1_Button_Action();
+#else 
+      if(P2P_Client_APP_Get_State () != APP_BLE_CONNECTED_CLIENT)
+      {
+        UTIL_SEQ_SetTask(1 << CFG_TASK_START_SCAN_ID, CFG_SCH_PRIO_0);
+      }
+      else 
+      {
+        P2PC_APP_SW1_Button_Action();
+      }   
+#endif 
+}
 /* USER CODE END FD */
 /*************************************************************
  *
@@ -794,6 +811,7 @@ static void Scan_Request( void )
 {
   /* USER CODE BEGIN Scan_Request_1 */
 
+  log_debug("Scan_Request", "enter", 0);
   /* USER CODE END Scan_Request_1 */
   tBleStatus result;
   if (BleApplicationContext.Device_Connection_Status != APP_BLE_CONNECTED_CLIENT)
@@ -806,6 +824,7 @@ static void Scan_Request( void )
     {
     /* 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");
     }
@@ -813,6 +832,7 @@ static void Scan_Request( void )
     {
     /* 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");
     }
@@ -827,6 +847,7 @@ static void Connect_Request( void )
 {
   /* USER CODE BEGIN Connect_Request_1 */
 
+  log_debug("Scan_Request", "enter", 0);
   /* USER CODE END Connect_Request_1 */
   tBleStatus result;
 

nucleo-wb55-dongle-ble/STM32_WPAN/App/app_ble.h

@@ -80,7 +80,7 @@ extern "C" {
   APP_BLE_ConnStatus_t APP_BLE_Get_Client_Connection_Status( uint16_t Connection_Handle );
 
 /* USER CODE BEGIN EF */
-
+  void APP_BLE_Key_Button1_Action(void);
 /* USER CODE END EF */
 
 #ifdef __cplusplus

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

@@ -32,7 +32,7 @@
 #include "app_ble.h"
 
 /* USER CODE BEGIN Includes */
-
+#include "commands.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -106,7 +106,28 @@ typedef struct
 }P2P_ClientContext_t;
 
 /* USER CODE BEGIN PTD */
+typedef struct{
+  uint8_t                                     Device_Led_Selection;
+  uint8_t                                     Led1;
+}P2P_LedCharValue_t;
+
+typedef struct{
+  uint8_t                                     Device_Button_Selection;
+  uint8_t                                     Button1;
+}P2P_ButtonCharValue_t;
+
+typedef struct
+{
+
+  uint8_t       Notification_Status; /* used to chek if P2P Server is enabled to Notify */
 
+  P2P_LedCharValue_t         LedControl;
+  P2P_ButtonCharValue_t      ButtonStatus;
+
+  uint16_t ConnectionHandle; 
+
+
+} P2P_Client_App_Context_t;
 /* USER CODE END PTD */
 
 /* Private defines ------------------------------------------------------------*/
@@ -133,14 +154,16 @@ PLACE_IN_SECTION("BLE_APP_CONTEXT") static P2P_ClientContext_t aP2PClientContext
  * END of Section BLE_APP_CONTEXT
  */
 /* USER CODE BEGIN PV */
-
+PLACE_IN_SECTION("BLE_APP_CONTEXT") static P2P_Client_App_Context_t P2P_Client_App_Context;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
 static void Gatt_Notification(P2P_Client_App_Notification_evt_t *pNotification);
 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 Update_Service( void );
 /* USER CODE END PFP */
 
 /* Functions Definition ------------------------------------------------------*/
@@ -153,7 +176,20 @@ void P2PC_APP_Init(void)
 {
   uint8_t index =0;
 /* USER CODE BEGIN P2PC_APP_Init_1 */
+  log_debug("P2PC_APP_Init", "enter", 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 );
 
+  /**
+   * Initialize LedButton Service
+   */
+  P2P_Client_App_Context.Notification_Status=0;
+  P2P_Client_App_Context.ConnectionHandle =  0x00;
+
+  P2P_Client_App_Context.LedControl.Device_Led_Selection=0x00;/* device Led */
+  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;
 /* USER CODE END P2PC_APP_Init_1 */
   for(index = 0; index < BLE_CFG_CLT_MAX_NBR_CB; index++)
   {
@@ -178,7 +214,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);
 /* USER CODE END P2PC_APP_Notification_1 */
   switch(pNotification->P2P_Evt_Opcode)
   {
@@ -188,13 +224,26 @@ void P2PC_APP_Notification(P2PC_APP_ConnHandle_Not_evt_t *pNotification)
 
   case PEER_CONN_HANDLE_EVT :
 /* USER CODE BEGIN PEER_CONN_HANDLE_EVT */
-
+    P2P_Client_App_Context.ConnectionHandle = pNotification->ConnectionHandle;
 /* USER CODE END PEER_CONN_HANDLE_EVT */
       break;
 
     case PEER_DISCON_HANDLE_EVT :
 /* USER CODE BEGIN PEER_DISCON_HANDLE_EVT */
-
+      {
+      uint8_t index = 0;
+      P2P_Client_App_Context.ConnectionHandle =  0x00;
+      while((index < BLE_CFG_CLT_MAX_NBR_CB) &&
+                  (aP2PClientContext[index].state != APP_BLE_IDLE))
+      {
+        aP2PClientContext[index].state = APP_BLE_IDLE;
+      }
+      // BSP_LED_Off(LED_BLUE); 
+        
+#if OOB_DEMO == 0
+      UTIL_SEQ_SetTask(1<<CFG_TASK_CONN_DEV_1_ID, CFG_SCH_PRIO_0);
+#endif 
+      }
 /* USER CODE END PEER_DISCON_HANDLE_EVT */
       break;
 
@@ -210,7 +259,10 @@ void P2PC_APP_Notification(P2PC_APP_ConnHandle_Not_evt_t *pNotification)
   return;
 }
 /* USER CODE BEGIN FD */
-
+void P2PC_APP_SW1_Button_Action(void)
+{
+  UTIL_SEQ_SetTask(1<<CFG_TASK_SW1_BUTTON_PUSHED_ID, CFG_SCH_PRIO_0);
+}
 /* USER CODE END FD */
 
 /*************************************************************
@@ -230,6 +282,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);
   P2P_Client_App_Notification_evt_t Notification;
 
   return_value = SVCCTL_EvtNotAck;
@@ -514,6 +567,7 @@ static SVCCTL_EvtAckStatus_t Event_Handler(void *Event)
 void Gatt_Notification(P2P_Client_App_Notification_evt_t *pNotification)
 {
 /* USER CODE BEGIN Gatt_Notification_1*/
+  log_debug("Gatt_Notification", "enter", 0);
 
 /* USER CODE END Gatt_Notification_1 */
   switch(pNotification->P2P_Client_Evt_Opcode)
@@ -524,7 +578,28 @@ void Gatt_Notification(P2P_Client_App_Notification_evt_t *pNotification)
 
     case P2P_NOTIFICATION_INFO_RECEIVED_EVT:
 /* USER CODE BEGIN P2P_NOTIFICATION_INFO_RECEIVED_EVT */
+    {
+      P2P_Client_App_Context.LedControl.Device_Led_Selection=pNotification->DataTransfered.pPayload[0];
+      switch(P2P_Client_App_Context.LedControl.Device_Led_Selection) {
+
+        case 0x01 : {
 
+          P2P_Client_App_Context.LedControl.Led1=pNotification->DataTransfered.pPayload[1];
+
+          if(P2P_Client_App_Context.LedControl.Led1==0x00){
+            // BSP_LED_Off(LED_BLUE);
+            log_debug("Gatt_Notification", " -- P2P APPLICATION CLIENT : NOTIFICATION RECEIVED - LED OFF ", 0);
+          } else {
+            log_debug("Gatt_Notification", " -- P2P APPLICATION CLIENT : NOTIFICATION RECEIVED - LED ON", 0);
+            // BSP_LED_On(LED_BLUE);
+          }
+
+          break;
+        }
+        default : break;
+      }
+
+    }
 /* USER CODE END P2P_NOTIFICATION_INFO_RECEIVED_EVT */
       break;
 
@@ -544,7 +619,124 @@ uint8_t P2P_Client_APP_Get_State( void ) {
   return aP2PClientContext[0].state;
 }
 /* USER CODE BEGIN LF */
+/**
+ * @brief  Feature Characteristic update
+ * @param  pFeatureValue: The address of the new value to be written
+ * @retval None
+ */
+tBleStatus Write_Char(uint16_t UUID, uint8_t Service_Instance, uint8_t *pPayload)
+{
+
+  tBleStatus ret = BLE_STATUS_INVALID_PARAMS;
+  uint8_t index;
+
+  index = 0;
+  while((index < BLE_CFG_CLT_MAX_NBR_CB) &&
+          (aP2PClientContext[index].state != APP_BLE_IDLE))
+  {
 
+    switch(UUID)
+    {
+      case P2P_WRITE_CHAR_UUID: /* SERVER RX -- so CLIENT TX */
+        ret =aci_gatt_write_without_resp(aP2PClientContext[index].connHandle,
+                                         aP2PClientContext[index].P2PWriteToServerCharHdle,
+                                         2, /* charValueLen */
+                                         (uint8_t *)  pPayload);
+
+        break;
+
+      default:
+        break;
+    }
+    index++;
+  }
+
+  return ret;
+}/* end Write_Char() */
+
+void Button_Trigger_Received(void)
+{
+  log_debug("Button_Trigger_Received", "-- P2P APPLICATION CLIENT  : BUTTON PUSHED - WRITE TO SERVER ", 0);
+  if(P2P_Client_App_Context.ButtonStatus.Button1==0x00){
+    P2P_Client_App_Context.ButtonStatus.Button1=0x01;
+  }else {
+    P2P_Client_App_Context.ButtonStatus.Button1=0x00;
+  }
+
+  Write_Char( P2P_WRITE_CHAR_UUID, 0, (uint8_t *)&P2P_Client_App_Context.ButtonStatus);
+
+  return;
+}
+
+void Update_Service()
+{
+  log_debug("Update_Service", "enter", 0);
+  uint16_t enable = 0x0001;
+  uint16_t disable = 0x0000;
+
+  uint8_t index;
+
+  index = 0;
+  while((index < BLE_CFG_CLT_MAX_NBR_CB) &&
+          (aP2PClientContext[index].state != APP_BLE_IDLE))
+  {
+
+
+    switch(aP2PClientContext[index].state)
+    {
+
+      case APP_BLE_DISCOVER_SERVICES:
+        log_debug("Update_Service", "P2P_DISCOVER_SERVICES", 0);
+        break;
+      case APP_BLE_DISCOVER_CHARACS:
+        log_debug("Update_Service", "* GATT : Discover P2P Characteristics", 0);
+        aci_gatt_disc_all_char_of_service(aP2PClientContext[index].connHandle,
+                                          aP2PClientContext[index].P2PServiceHandle,
+                                          aP2PClientContext[index].P2PServiceEndHandle);
+
+        break;
+      case APP_BLE_DISCOVER_WRITE_DESC: /* Not Used - No decriptor */
+        log_debug("Update_Service", "* GATT : Discover Descriptor of TX - Write Characteritic", 0);
+        aci_gatt_disc_all_char_desc(aP2PClientContext[index].connHandle,
+                                    aP2PClientContext[index].P2PWriteToServerCharHdle,
+                                    aP2PClientContext[index].P2PWriteToServerCharHdle+2);
+
+        break;
+      case APP_BLE_DISCOVER_NOTIFICATION_CHAR_DESC:
+        log_debug("Update_Service", "* GATT : Discover Descriptor of Rx - Notification Characteritic", 0);
+        aci_gatt_disc_all_char_desc(aP2PClientContext[index].connHandle,
+                                    aP2PClientContext[index].P2PNotificationCharHdle,
+                                    aP2PClientContext[index].P2PNotificationCharHdle+2);
+
+        break;
+      case APP_BLE_ENABLE_NOTIFICATION_DESC:
+        log_debug("Update_Service", "* GATT : Enable Server Notification", 0);
+        aci_gatt_write_char_desc(aP2PClientContext[index].connHandle,
+                                 aP2PClientContext[index].P2PNotificationDescHandle,
+                                 2,
+                                 (uint8_t *)&enable);
+
+        aP2PClientContext[index].state = APP_BLE_CONNECTED_CLIENT;
+        // BSP_LED_Off(LED_RED);
+
+        break;
+      case APP_BLE_DISABLE_NOTIFICATION_DESC :
+        log_debug("Update_Service", "* GATT : Disable Server Notification", 0);
+        aci_gatt_write_char_desc(aP2PClientContext[index].connHandle,
+                                 aP2PClientContext[index].P2PNotificationDescHandle,
+                                 2,
+                                 (uint8_t *)&disable);
+
+        aP2PClientContext[index].state = APP_BLE_CONNECTED_CLIENT;
+
+        break;
+      default:
+        break;
+    }
+    index++;
+  }
+  return;
+}
 /* USER CODE END LF */
 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

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

@@ -70,7 +70,7 @@ void P2PC_APP_Init( void );
 void P2PC_APP_Notification( P2PC_APP_ConnHandle_Not_evt_t *pNotification );
 uint8_t P2P_Client_APP_Get_State( void );
 /* USER CODE BEGIN EFP */
-
+void P2PC_APP_SW1_Button_Action(void);
 /* USER CODE END EFP */
 
 #ifdef __cplusplus

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

@@ -13,6 +13,7 @@
 #include "commands.hpp"
 #include "HeartbeatCommand.hpp"
 #include "LedCommand.hpp"
+#include "GPCommand.hpp"
 
 std::queue<Command*> command_queue;
 
@@ -44,6 +45,9 @@ void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t
   case COMMAND_LED_REQUEST:
     push_command(new LedRequest(payload_ptr, size));
     break;
+  case COMMAND_GP_REQUEST:
+    push_command(new GPRequest(payload_ptr, size));
+    break;
 
   default:
     break;

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

@@ -20,6 +20,8 @@ typedef enum : uint8_t {
     COMMAND_HEARTBEAT_RESPONSE = 0x3,
     COMMAND_LED_REQUEST = 0x4,
     COMMAND_LED_RESPONSE = 0x5,
+    COMMAND_GP_REQUEST = 0x6,
+    COMMAND_GP_RESPONSE = 0x7,
 } CommandId;
 
 

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

@@ -0,0 +1,47 @@
+/*
+ * GPCommand.cpp
+ *
+ *  Created on: Jul 18, 2021
+ *      Author: Andreas Berthoud
+ */
+
+#include "GPCommand.hpp"
+#include "app_ble.h"
+
+GPResponse::GPResponse(uint16_t response_identifier, bool was_successful) 
+  : Response(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) {
+  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(uint16_t response_identifier) {
+  bool was_successful = true;
+
+  if (this->has_error) {
+    was_successful = false;
+  } else {
+    switch (this->command_id)
+    {
+    case 1:
+      APP_BLE_Key_Button1_Action();
+      break;
+    
+    default:
+      break;
+    }
+  }
+
+  return new GPResponse(response_identifier, was_successful);
+}
+

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

@@ -0,0 +1,47 @@
+/*
+ * 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(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(uint8_t * payload_ptr, uint16_t size);
+
+
+  GPResponse * execute_request(uint16_t response_identifier);
+
+  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/app/LogCommand.cpp

@@ -8,48 +8,57 @@
 #include <stdarg.h>
 #include <string.h>
 #include <string>
+#include <algorithm>
 
 #include "LogCommand.hpp"
 
 
-void log_debug(const char * format, int nargs, ...) {
+void log_debug(const char * logger_name, const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    push_command(new LogCommand(format, args, LOG_LEVEL_DEBUG));
+    push_command(new LogCommand(logger_name, format, args, LOG_LEVEL_DEBUG));
     va_end(args);
 }
 
-void log_info(const char * format, int nargs, ...) {
+void log_info(const char * logger_name, const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    push_command(new LogCommand(format, args, LOG_LEVEL_INFO));
+    push_command(new LogCommand(logger_name, format, args, LOG_LEVEL_INFO));
     va_end(args);
 }
 
-void log_warning(const char * format, int nargs, ...) {
+void log_warning(const char * logger_name, const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    push_command(new LogCommand(format, args, LOG_LEVEL_WARNING));
+    push_command(new LogCommand(logger_name, format, args, LOG_LEVEL_WARNING));
     va_end(args);
 }
 
-void log_error(const char * format, int nargs, ...) {
+void log_error(const char * logger_name, const char * format, int nargs, ...) {
     va_list args;
     va_start(args, nargs);
-    push_command(new LogCommand(format, args, LOG_LEVEL_ERROR));
+    push_command(new LogCommand(logger_name, format, args, LOG_LEVEL_ERROR));
     va_end(args);
 }
 
 LogCommand::LogCommand(
+    const char * logger_name, 
     const char * format, 
     va_list args, 
     LoggingLevel logging_level) : Notification() {
 
     uint16_t max_payload_size = NOTIFICATION_COMMAND_FREE_BUFFER - this->get_payload_size();
 
+    uint8_t logger_name_length = std::min(30, (int)strlen(logger_name));
+
     this->payload_ptr[0] = logging_level;
+    this->payload_ptr[1] = logger_name_length;
+
+    char * name = (char *)(this->payload_ptr + 2);
+    char * message = (char *)(this->payload_ptr + 2 + logger_name_length);
+    uint16_t message_length = max_payload_size - 2 - logger_name_length;
 
-    char * message = (char *)(this->payload_ptr + 1);
-    vsnprintf(message, max_payload_size - 1, format, args);
-    this->add_to_payload_size(strlen(message) + 1);
+    memcpy(name, logger_name, logger_name_length);
+    vsnprintf(message, message_length, format, args);
+    this->add_to_payload_size(strlen(message) + 2 + logger_name_length);
 }

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

@@ -10,10 +10,10 @@
 
 #include "Notification.hpp"
 
-extern "C" void log_debug(const char * format, int nargs, ...);
-extern "C" void log_info(const char * format, int nargs, ...);
-extern "C" void log_warning(const char * format, int nargs, ...);
-extern "C" void log_error(const char * format, int nargs, ...);
+extern "C" void log_debug(const char * logger_name, const char * format, int nargs, ...);
+extern "C" void log_info(const char * logger_name, const char * format, int nargs, ...);
+extern "C" void log_warning(const char * logger_name, const char * format, int nargs, ...);
+extern "C" void log_error(const char * logger_name, const char * format, int nargs, ...);
 
 typedef enum : uint8_t{
   LOG_LEVEL_DEBUG = 10,
@@ -28,7 +28,7 @@ public:
   /*
    * Create log command from formatted string 
    */
-  LogCommand(const char * format, va_list args, LoggingLevel logging_level);
+  LogCommand(const char * logger_name, const char * format, va_list args, LoggingLevel logging_level);
   virtual ~LogCommand() {};
 
   virtual CommandId get_command_id() override { return COMMAND_LOG; }

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

@@ -11,7 +11,7 @@
 void usb_receive(uint8_t *buf, uint32_t *len) {
 
   if (*len < 5) {
-    log_error("received command which cannot be interpreted", 0);
+    log_error("usb_receive", "received command which cannot be interpreted", 0);
     return;
   }
   uint8_t command_id = buf[0];
@@ -24,7 +24,7 @@ void usb_receive(uint8_t *buf, uint32_t *len) {
   }
 
   if (stop_byte != 0xFF) {
-    log_error("received command has invalid stop byte: 0x%x", 1, stop_byte);
+    log_error("usb_receive", "received command has invalid stop byte: 0x%x", 1, stop_byte);
   } else {
     handle_received_command(command_id, payload_ptr, length);
   }

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

@@ -11,9 +11,9 @@
 void pop_and_execute_commands();
 void handle_received_command(uint8_t command_id, uint8_t * payload_ptr, uint16_t length);
 
-void log_debug(const char * format, int nargs, ...);
-void log_info(const char * format, int nargs, ...);
-void log_warning(const char * format, int nargs, ...);
-void log_error(const char * format, int nargs, ...);
+void log_debug(const char * logger_name, const char * format, int nargs, ...);
+void log_info(const char * logger_name, const char * format, int nargs, ...);
+void log_warning(const char * logger_name, const char * format, int nargs, ...);
+void log_error(const char * logger_name, const char * format, int nargs, ...);
 
 #endif /* COMMANDS_H_ */