diff --git a/nucleo-wb55-zigbee/Core/Inc/app_conf.h b/nucleo-wb55-zigbee/Core/Inc/app_conf.h
index ec4749b..cd88e80 100644
--- a/nucleo-wb55-zigbee/Core/Inc/app_conf.h
+++ b/nucleo-wb55-zigbee/Core/Inc/app_conf.h
@@ -266,8 +266,8 @@ typedef enum
 /******************************************************************************
  * Configure Log level for Application
  ******************************************************************************/
-#define APPLI_CONFIG_LOG_LEVEL    LOG_LEVEL_INFO
-#define APPLI_PRINT_FILE_FUNC_LINE    0
+#define APPLI_CONFIG_LOG_LEVEL    LOG_LEVEL_DEBG
+#define APPLI_PRINT_FILE_FUNC_LINE    1
 
 /* USER CODE BEGIN Defines */
 
diff --git a/nucleo-wb55-zigbee/Core/Src/main.c b/nucleo-wb55-zigbee/Core/Src/main.c
index 4e9d418..fccb00e 100644
--- a/nucleo-wb55-zigbee/Core/Src/main.c
+++ b/nucleo-wb55-zigbee/Core/Src/main.c
@@ -22,7 +22,7 @@
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include <stdio.h>
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -36,7 +36,7 @@
 
 /* Private macro -------------------------------------------------------------*/
 /* USER CODE BEGIN PM */
-
+#define ITM_Port32(n)		(*((volatile unsigned long *)(0xE0000000+4*n)))
 /* USER CODE END PM */
 
 /* Private variables ---------------------------------------------------------*/
@@ -68,7 +68,11 @@ static void MX_RF_Init(void);
 int main(void)
 {
   /* USER CODE BEGIN 1 */
-
+  /**
+   * The OPTVERR flag is wrongly set at power on
+   * It shall be cleared before using any HAL_FLASH_xxx() api
+   */
+  __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR);
   /* USER CODE END 1 */
 
   /* MCU Configuration--------------------------------------------------------*/
@@ -84,6 +88,7 @@ int main(void)
   SystemClock_Config();
 
   /* USER CODE BEGIN SysInit */
+  ITM_Port32(31) = 1;
 
   /* USER CODE END SysInit */
 
@@ -92,7 +97,8 @@ int main(void)
   MX_RTC_Init();
   MX_RF_Init();
   /* USER CODE BEGIN 2 */
-
+  printf("GPIO init done \r\n");
+  ITM_Port32(31) = 2;
   /* USER CODE END 2 */
 
   /* Init code for STM32_WPAN */
@@ -101,7 +107,8 @@ int main(void)
   /* USER CODE BEGIN WHILE */
   while (1)
   {
-	  //HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
+  	printf("test\r\n");
+	  HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
 	  HAL_Delay(500);
     /* USER CODE END WHILE */
 
@@ -245,6 +252,7 @@ static void MX_GPIO_Init(void)
   /* GPIO Ports Clock Enable */
   __HAL_RCC_GPIOC_CLK_ENABLE();
   __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
   __HAL_RCC_GPIOD_CLK_ENABLE();
 
   /*Configure GPIO pin Output Level */
@@ -272,7 +280,13 @@ static void MX_GPIO_Init(void)
 }
 
 /* USER CODE BEGIN 4 */
-
+int _write(int file, char *ptr, int len) {
+	int i = 0;
+	for (i = 0; i<len; i++) {
+		ITM_SendChar(*ptr++);
+	}
+	return len;
+}
 /* USER CODE END 4 */
 
 /**
diff --git a/nucleo-wb55-zigbee/STM32WB55RGVX_FLASH.ld b/nucleo-wb55-zigbee/STM32WB55RGVX_FLASH.ld
index a041fac..9dd31df 100644
--- a/nucleo-wb55-zigbee/STM32WB55RGVX_FLASH.ld
+++ b/nucleo-wb55-zigbee/STM32WB55RGVX_FLASH.ld
@@ -39,8 +39,8 @@ ENTRY(Reset_Handler)
 /* Highest address of the user mode stack */
 _estack = 0x20030000;    /* end of RAM */
 /* Generate a link error if heap and stack don't fit into RAM */
-_Min_Heap_Size = 0x200 ;  /* required amount of heap  */
-_Min_Stack_Size = 0x400 ; /* required amount of stack */
+_Min_Heap_Size = 0x19000 ;  /* required amount of heap  */
+_Min_Stack_Size = 0x1000 ; /* required amount of stack */
 
 /* Specify the memory areas */
 MEMORY
diff --git a/nucleo-wb55-zigbee/nucleo-wb55-zigbee.ioc b/nucleo-wb55-zigbee/nucleo-wb55-zigbee.ioc
index 47206d7..5cc5a05 100644
--- a/nucleo-wb55-zigbee/nucleo-wb55-zigbee.ioc
+++ b/nucleo-wb55-zigbee/nucleo-wb55-zigbee.ioc
@@ -1,44 +1,26 @@
 #MicroXplorer Configuration settings - do not modify
-VP_RTC_VS_RTC_Activate.Signal=RTC_VS_RTC_Activate
 Mcu.Family=STM32WB
 ProjectManager.MainLocation=Core/Src
-PCC.Ble.Mode=NOT_SELECTED
-PC15-OSC32_OUT.Mode=LSE-External-Oscillator
-RCC.PLLQoutputFreq_Value=128000000
 RCC.USART1Freq_Value=32000000
-ProjectManager.ProjectFileName=nucleo-wb55-zigbee.ioc
 RCC.SAI1Freq_Value=128000000
 RCC.CortexFreq_Value=32000000
 ProjectManager.KeepUserCode=true
 Mcu.UserName=STM32WB55RGVx
-Mcu.PinsNb=17
 RF1.Signal=RF_RF1
-ProjectManager.NoMain=false
 RCC.RFWKPFreq_Value=976.5625
 RCC.SMPSCLockSelectionVirtualVirtual=RCC_SMPSCLKSOURCE_HSI
-RCC.HCLK3Freq_Value=32000000
 RCC.PLLSAI1RoutputFreq_Value=128000000
-ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_RTC_Init-RTC-false-HAL-true,4-MX_RF_Init-RF-false-HAL-true,0-MX_HSEM_Init-HSEM-false-HAL-true
-PC4.Signal=GPIO_Input
+ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_RTC_Init-RTC-false-HAL-true,4-MX_RF_Init-RF-false-HAL-true,5-APPE_Init-STM32_WPAN-false-HAL-false,0-MX_HSEM_Init-HSEM-false-HAL-true
 PD0.GPIO_Label=B2 [Push Button]
 VP_RTC_VS_RTC_Activate.Mode=RTC_Enabled
-PD1.Signal=GPIO_Input
 RCC.RTCFreq_Value=32768
-ProjectManager.DefaultFWLocation=true
 PB0.GPIO_Label=LED_GREEN
-VP_HSEM_VS_HSEM.Signal=HSEM_VS_HSEM
 STM32_WPAN.ZCL_ON_OFF_SWITCH_CONFIGURATION_ENDPOINT1=SERVER
 PD0.Locked=true
-ProjectManager.DeletePrevious=true
-RCC.HCLK2Freq_Value=32000000
-PC14-OSC32_IN.Signal=RCC_OSC32_IN
-boardIOC=true
-STM32_WPAN.ENDPOINT1_ID_ZGB=1
 PinOutPanel.RotationAngle=0
 VP_SEQUENCER_VS_SEQUENCER.Signal=SEQUENCER_VS_SEQUENCER
-RCC.FamilyName=M
 RCC.MCO1PinFreq_Value=32000000
-ProjectManager.StackSize=0x400
+ProjectManager.StackSize=0x1000
 PB5.GPIOParameters=GPIO_Label
 RCC.I2C3Freq_Value=32000000
 RCC.LPTIM1Freq_Value=32000000
@@ -48,141 +30,175 @@ Mcu.IP5=SEQUENCER
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false
 Mcu.IP2=RCC
 Mcu.IP3=RF
+STM32_WPAN.APPLI_PRINT_FILE_FUNC_LINE=1
 Mcu.IP0=HSEM
 Mcu.IP1=NVIC
 Mcu.UserConstants=
 RCC.VCOSAI1OutputFreq_Value=256000000
-ProjectManager.TargetToolchain=STM32CubeIDE
+STM32_WPAN.CFG_CLI_UART=hw_uart1
 Mcu.ThirdPartyNb=0
 RCC.HCLKFreq_Value=32000000
 Mcu.IPNb=9
-RCC.HCLKRFFreq_Value=16000000
 ProjectManager.PreviousToolchain=
 RCC.APB2TimFreq_Value=32000000
 PCC.Ble.PowerLevel=Min
-PB5.Locked=true
 Mcu.Pin6=PB0
 PD0.Signal=GPIO_Input
 Mcu.Pin7=PB1
-ProjectManager.RegisterCallBack=
-Mcu.Pin8=PD0
-OSC_IN.Signal=RCC_OSC_IN
-RCC.USBFreq_Value=128000000
+Mcu.Pin8=PA13
 OSC_OUT.Mode=HSE-External-Oscillator
-Mcu.Pin9=PD1
-PC15-OSC32_OUT.Signal=RCC_OSC32_OUT
+Mcu.Pin9=PA14
 OSC_OUT.Signal=RCC_OSC_OUT
 RCC.AHBFreq_Value=32000000
-RCC.PLLSAI1PoutputFreq_Value=128000000
 Mcu.Pin0=PC14-OSC32_IN
-PB1.Locked=true
 Mcu.Pin1=PC15-OSC32_OUT
 VP_STM32_WPAN_VS_ZIGBEE.Signal=STM32_WPAN_VS_ZIGBEE
 Mcu.Pin2=PC4
 Mcu.Pin3=RF1
 Mcu.Pin4=OSC_OUT
 Mcu.Pin5=OSC_IN
-RCC.SMPSCLockSelectionVirtual=RCC_SMPSCLKSOURCE_HSI
 ProjectManager.ProjectBuild=false
 RCC.HSE_VALUE=32000000
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
 RCC.FCLK2Freq_Value=32000000
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false
-board=P-NUCLEO-WB55-Nucleo
-VP_SEQUENCER_VS_SEQUENCER.Mode=SEQUENCER_Enabled
-RCC.Cortex2Freq_Value=32000000
-RCC.VCOOutputFreq_Value=256000000
-PD1.GPIOParameters=GPIO_Label
-NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true
+PB3.Mode=Trace_Asynchronous_SW
 RCC.RTCClockSelection=RCC_RTCCLKSOURCE_LSE
-ProjectManager.LastFirmware=true
-RCC.SMPS1Freq_Value=16000000
+NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true
+PD1.GPIOParameters=GPIO_Label
 PD1.GPIO_Label=B3 [Push Button]
 ProjectManager.FirmwarePackage=STM32Cube FW_WB V1.11.1
 MxDb.Version=DB.6.0.21
 PB0.GPIOParameters=GPIO_Label
-RCC.APB2Freq_Value=32000000
 ProjectManager.BackupPrevious=false
-MxCube.Version=6.2.1
 PC14-OSC32_IN.Mode=LSE-External-Oscillator
 RCC.VCOInputFreq_Value=32000000
-PCC.Ble.DataLength=6
 PC4.GPIO_Label=B1 [Push Button]
+PA14.Mode=Trace_Asynchronous_SW
+PB1.GPIO_Label=LED_RED
+File.Version=6
+RCC.PLLRCLKFreq_Value=128000000
+NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false
+ProjectManager.HalAssertFull=false
+PB0.Locked=true
+ProjectManager.ProjectName=nucleo-wb55-zigbee
+RCC.APB3Freq_Value=16000000
+NVIC.HSEM_IRQn=true\:0\:0\:false\:false\:true\:false\:true
+Mcu.Package=VFQFPN68
+PB1.Signal=GPIO_Output
+STM32_WPAN.APPLI_CONFIG_LOG_LEVEL=LOG_LEVEL_DEBG
+PA14.Locked=true
+ProjectManager.ToolChainLocation=
+RCC.LSI_VALUE=32000
+VP_SYS_VS_Systick.Signal=SYS_VS_Systick
+RCC.LSCOPinFreq_Value=32000
+VP_TINY_LPM_VS_TINY_LPM.Mode=TINY_LPM_Enabled
+PB5.GPIO_Label=LED_BLUE
+VP_HSEM_VS_HSEM.Mode=HSEM_Activate
+RCC.PLLPoutputFreq_Value=128000000
+RCC.APB1TimFreq_Value=32000000
+NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
+PC4.Locked=true
+RCC.LPUART1Freq_Value=32000000
+RCC.SMPSDivider=1
+ProjectManager.CustomerFirmwarePackage=
+PC4.GPIOParameters=GPIO_Label
+RCC.MSIClockRange=RCC_MSIRANGE_10
+RCC.HSI48_VALUE=48000000
+VP_RTC_VS_RTC_Activate.Signal=RTC_VS_RTC_Activate
+PCC.Ble.Mode=NOT_SELECTED
+PC15-OSC32_OUT.Mode=LSE-External-Oscillator
+RCC.PLLQoutputFreq_Value=128000000
+ProjectManager.ProjectFileName=nucleo-wb55-zigbee.ioc
+Mcu.PinsNb=20
+ProjectManager.NoMain=false
+RCC.HCLK3Freq_Value=32000000
+PC4.Signal=GPIO_Input
+PD1.Signal=GPIO_Input
+ProjectManager.DefaultFWLocation=true
+VP_HSEM_VS_HSEM.Signal=HSEM_VS_HSEM
+ProjectManager.DeletePrevious=true
+RCC.HCLK2Freq_Value=32000000
+PC14-OSC32_IN.Signal=RCC_OSC32_IN
+boardIOC=true
+STM32_WPAN.ENDPOINT1_ID_ZGB=1
+RCC.FamilyName=M
+STM32_WPAN.CFG_HW_USART1_ENABLED=1
+PA13.Signal=SYS_JTMS-SWDIO
+ProjectManager.TargetToolchain=STM32CubeIDE
+RCC.HCLKRFFreq_Value=16000000
+PB5.Locked=true
+ProjectManager.RegisterCallBack=
+OSC_IN.Signal=RCC_OSC_IN
+RCC.USBFreq_Value=128000000
+PC15-OSC32_OUT.Signal=RCC_OSC32_OUT
+RCC.PLLSAI1PoutputFreq_Value=128000000
+PB1.Locked=true
+RCC.SMPSCLockSelectionVirtual=RCC_SMPSCLKSOURCE_HSI
+board=P-NUCLEO-WB55-Nucleo
+VP_SEQUENCER_VS_SEQUENCER.Mode=SEQUENCER_Enabled
+RCC.Cortex2Freq_Value=32000000
+RCC.VCOOutputFreq_Value=256000000
+ProjectManager.LastFirmware=true
+RCC.SMPS1Freq_Value=16000000
+RCC.APB2Freq_Value=32000000
+MxCube.Version=6.2.1
+PCC.Ble.DataLength=6
 RCC.I2C1Freq_Value=32000000
-RCC.RNGFreq_Value=32000
 RCC.LCDFreq_Value=32768
-PB1.GPIO_Label=LED_RED
+RCC.RNGFreq_Value=32000
 RCC.PLLSAI1QoutputFreq_Value=128000000
 RCC.ADCFreq_Value=128000000
-File.Version=6
 VP_SYS_VS_Systick.Mode=SysTick
 NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false
-RCC.PLLRCLKFreq_Value=128000000
-NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false
+PA13.Mode=Trace_Asynchronous_SW
 ProjectManager.FreePins=false
 RCC.IPParameters=ADCFreq_Value,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,APB3Freq_Value,Cortex2Freq_Value,CortexFreq_Value,FCLK2Freq_Value,FCLKCortexFreq_Value,FamilyName,HCLK2Freq_Value,HCLK3Freq_Value,HCLKFreq_Value,HCLKRFFreq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1Freq_Value,I2C3Freq_Value,LCDFreq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPUART1Freq_Value,LSCOPinFreq_Value,LSI_VALUE,MCO1PinFreq_Value,MSIClockRange,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PLLSAI1PoutputFreq_Value,PLLSAI1QoutputFreq_Value,PLLSAI1RoutputFreq_Value,PWRFreq_Value,RFWKPFreq_Value,RNGFreq_Value,RTCClockSelection,RTCFreq_Value,SAI1Freq_Value,SMPS1Freq_Value,SMPSCLockSelectionVirtual,SMPSCLockSelectionVirtualVirtual,SMPSDivider,SYSCLKFreq_VALUE,USART1Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VCOSAI1OutputFreq_Value
 ProjectManager.AskForMigrate=true
 Mcu.Name=STM32WB55RGVx
 RCC.LPTIM2Freq_Value=32000000
-ProjectManager.HalAssertFull=false
-PB0.Locked=true
-ProjectManager.ProjectName=nucleo-wb55-zigbee
 ProjectManager.UnderRoot=true
 Mcu.IP8=TINY_LPM
-RCC.APB3Freq_Value=16000000
 Mcu.IP6=STM32_WPAN
 Mcu.IP7=SYS
 ProjectManager.CoupleFile=false
+PB3.Signal=SYS_JTDO-SWO
 RCC.SYSCLKFreq_VALUE=32000000
-NVIC.HSEM_IRQn=true\:0\:0\:false\:false\:true\:false\:true
-Mcu.Package=VFQFPN68
 PB5.Signal=GPIO_Output
-PB1.Signal=GPIO_Output
 PCC.Ble.ConnectionInterval=1000.0
 NVIC.ForceEnableDMAVector=true
 KeepUserPlacement=false
 OSC_IN.Mode=HSE-External-Oscillator
 NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false
-STM32_WPAN.IPParameters=ZCL_ON_OFF_SWITCH_CONFIGURATION_ENDPOINT1,ENDPOINT1_ID_ZGB
+STM32_WPAN.IPParameters=ZCL_ON_OFF_SWITCH_CONFIGURATION_ENDPOINT1,ENDPOINT1_ID_ZGB,CFG_HW_USART1_ENABLED,CFG_FULL_LOW_POWER,CFG_CLI_UART,APPLI_CONFIG_LOG_LEVEL,APPLI_PRINT_FILE_FUNC_LINE
+PA13.Locked=true
 ProjectManager.CompilerOptimize=6
 RF1.Mode=RF1_Activate
-ProjectManager.ToolChainLocation=
-RCC.LSI_VALUE=32000
-VP_SYS_VS_Systick.Signal=SYS_VS_Systick
-RCC.LSCOPinFreq_Value=32000
-ProjectManager.HeapSize=0x200
-Mcu.Pin15=VP_SYS_VS_Systick
+PA14.Signal=SYS_JTCK-SWCLK
+STM32_WPAN.CFG_FULL_LOW_POWER=0
+ProjectManager.HeapSize=0x19000
+Mcu.Pin15=VP_RTC_VS_RTC_Activate
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
-Mcu.Pin16=VP_TINY_LPM_VS_TINY_LPM
-Mcu.Pin13=VP_SEQUENCER_VS_SEQUENCER
-Mcu.Pin14=VP_STM32_WPAN_VS_ZIGBEE
+Mcu.Pin16=VP_SEQUENCER_VS_SEQUENCER
+Mcu.Pin13=PB5
+Mcu.Pin14=VP_HSEM_VS_HSEM
+Mcu.Pin19=VP_TINY_LPM_VS_TINY_LPM
 ProjectManager.ComputerToolchain=false
+Mcu.Pin17=VP_STM32_WPAN_VS_ZIGBEE
 RCC.HSI_VALUE=16000000
-VP_TINY_LPM_VS_TINY_LPM.Mode=TINY_LPM_Enabled
+Mcu.Pin18=VP_SYS_VS_Systick
 VP_STM32_WPAN_VS_ZIGBEE.Mode=MOD_ZGB_ENDPOINT
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
-Mcu.Pin11=VP_HSEM_VS_HSEM
+Mcu.Pin11=PD1
+Mcu.Pin12=PB3
 PD0.GPIOParameters=GPIO_Label
-PB5.GPIO_Label=LED_BLUE
-VP_HSEM_VS_HSEM.Mode=HSEM_Activate
-Mcu.Pin12=VP_RTC_VS_RTC_Activate
-RCC.PLLPoutputFreq_Value=128000000
-Mcu.Pin10=PB5
-RCC.APB1TimFreq_Value=32000000
-NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false
+Mcu.Pin10=PD0
 RCC.PWRFreq_Value=32000000
-PC4.Locked=true
-RCC.LPUART1Freq_Value=32000000
 VP_TINY_LPM_VS_TINY_LPM.Signal=TINY_LPM_VS_TINY_LPM
 RCC.APB1Freq_Value=32000000
 PD1.Locked=true
-RCC.SMPSDivider=1
 PB0.Signal=GPIO_Output
-ProjectManager.CustomerFirmwarePackage=
-PC4.GPIOParameters=GPIO_Label
 ProjectManager.DeviceId=STM32WB55RGVx
-RCC.MSIClockRange=RCC_MSIRANGE_10
 ProjectManager.LibraryCopy=1
-RCC.HSI48_VALUE=48000000
 PB1.GPIOParameters=GPIO_Label
 isbadioc=false