diff --git a/CMakeLists.txt b/CMakeLists.txt
index 559d86c..828a047 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -102,9 +102,9 @@ set(TARGET_LIB_DIRECTORIES
 
 # Conditional flags
 # DEBUG
-set(CMAKE_C_FLAGS_DEBUG     "-DDEBUG -O -g")
-set(CMAKE_CXX_FLAGS_DEBUG   "-DDEBUG -O -g")
-set(CMAKE_ASM_FLAGS_DEBUG   "-DDEBUG -O -g")
+set(CMAKE_C_FLAGS_DEBUG     "-DDEBUG -O0 -g")
+set(CMAKE_CXX_FLAGS_DEBUG   "-DDEBUG -O0 -g")
+set(CMAKE_ASM_FLAGS_DEBUG   "-DDEBUG -O0 -g")
 
 # RELEASE
 set(CMAKE_C_FLAGS_RELEASE        "-DNDEBUG -O2 -flto")
@@ -121,8 +121,7 @@ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--gc-sections")
 # Submodules
 add_subdirectory(lib/esp_nano_hosted)
 
-add_library(freertos_config INTERFACE)
-target_include_directories(freertos_config SYSTEM INTERFACE include)
+set(FREERTOS_CONFIG_FILE_DIRECTORY "${CMAKE_SOURCE_DIR}/include" CACHE STRING "")
 set(FREERTOS_PORT "GCC_ARM_CM33_NTZ_NONSECURE" CACHE STRING "")
 set(FREERTOS_HEAP "4" CACHE STRING "")
 add_subdirectory(lib/freertos)
diff --git a/lib/esp_nano_hosted b/lib/esp_nano_hosted
index a628889..6241501 160000
--- a/lib/esp_nano_hosted
+++ b/lib/esp_nano_hosted
@@ -1 +1 @@
-Subproject commit a628889e74f306b82a1d3fe0e0df75158ab53fbd
+Subproject commit 624150181c4d8a86c8f098b5f5c02c3ab9b756e2
diff --git a/lib/freertos b/lib/freertos
index 6d65558..def7d2d 160000
--- a/lib/freertos
+++ b/lib/freertos
@@ -1 +1 @@
-Subproject commit 6d65558ba01141d7c50ff6f93a4054cc5f16940e
+Subproject commit def7d2df2b0506d3d249334974f51e427c17a41c
diff --git a/src/app_nh_impl.c b/src/app_nh_impl.c
index 01f94f2..b37dfc5 100644
--- a/src/app_nh_impl.c
+++ b/src/app_nh_impl.c
@@ -31,7 +31,9 @@ static void app_nh_impl_hs_callback(pint_pin_int_t pintr, uint32_t pmatch_status
 
 static nh_ret_t app_nh_impl_ops_xfer(void *handle, uint8_t *tx_data, uint8_t *rx_data, uint32_t len);
 static nh_ret_t app_nh_impl_ops_drdy_read(void *handle, bool *rdy);
-static void     app_nh_impl_cb_event(void *handle, uint8_t *payload);
+static void     app_nh_impl_cb_startup(void *handle, nh_event_init_t *init_data);
+
+static void app_nh_impl_cb_init(void *handle);
 
 static nh_ret_t app_nh_impl_buf_allocate(void *handle, uint8_t **buf, uint32_t size);
 static nh_ret_t app_nh_impl_buf_free(void *handle, uint8_t *buf);
@@ -49,6 +51,8 @@ static nh_ret_t app_impl_queue_destroy(void *handle, nh_osa_queue_t queue);
 static void app_nh_impl_shared_if_task(void *parameters);
 static void app_nh_impl_ctrl_task(void *parameters);
 
+static SemaphoreHandle_t s_esp_init_semphore;
+
 static nh_osa_t s_nh_osa = {
     .buf_allocate = app_nh_impl_buf_allocate,
     .buf_free     = app_nh_impl_buf_free,
@@ -73,16 +77,25 @@ static nh_shared_if_t s_nh_shared_if = {
         },
     .cb =
         {
-            .event = app_nh_impl_cb_event,
+            .init = app_nh_impl_cb_startup,
         },
 };
 
 static nh_ctrl_api_t s_nh_ctrl_api = {
     .osa       = &s_nh_osa,
     .shared_if = &s_nh_shared_if,
+    .cb =
+        {
+            .init = app_nh_impl_cb_init,
+        },
 };
 
 int app_nh_impl_init(void) {
+    s_esp_init_semphore = xSemaphoreCreateBinary();
+    if (s_esp_init_semphore == NULL) {
+        return -1;
+    }
+
     if (app_nh_impl_pin_init() != 0) {
         return -1;
     }
@@ -95,7 +108,7 @@ int app_nh_impl_init(void) {
         return -3;
     }
 
-    if (xTaskCreate(app_nh_impl_shared_if_task, "NH_SHARED_IF", 1024, &s_nh_shared_if, 2, NULL) != pdPASS) {
+    if (xTaskCreate(app_nh_impl_shared_if_task, "NH_SHARED_IF", 512, &s_nh_shared_if, 2, NULL) != pdPASS) {
         return -4;
     }
 
@@ -107,6 +120,19 @@ int app_nh_impl_init(void) {
         return -6;
     }
 
+    /* FIXME: De-init properly */
+
+    if (xSemaphoreTake(s_esp_init_semphore, pdMS_TO_TICKS(5000)) != pdPASS) {
+        PRINTF("Did not received ESPInit event in time, bailing out (assumed soft reset).\r\n");
+    }
+
+    uint8_t mac_addr[18] = {0x00};
+    if (nh_ctrl_api_get_mac_address(&s_nh_ctrl_api, mac_addr) != NH_RET_SUCCESS) {
+        PRINTF("Failed to acquire MAC address.\r\n");
+    } else {
+        PRINTF("STA mode MAC address: %s\r\n", mac_addr);
+    }
+
     return 0;
 }
 
@@ -197,8 +223,74 @@ static nh_ret_t app_nh_impl_ops_xfer(void *handle, uint8_t *tx_data, uint8_t *rx
     return NH_RET_SUCCESS;
 }
 
-static void app_nh_impl_cb_event(void *handle, uint8_t *payload) {
-    PRINTF("Received event from ESP..\r\n");
+static void app_nh_impl_cb_startup(void *handle, nh_event_init_t *init_data) {
+    PRINTF("Received [INIT] event from ESP..\r\n");
+
+    char *buf;
+    switch (init_data->chip_id) {
+        case NH_ESP_INIT_CHIP_ESP32:
+            buf = "ESP32";
+            break;
+        case NH_ESP_INIT_CHIP_ESP32S2:
+            buf = "ESP32S2";
+            break;
+        case NH_ESP_INIT_CHIP_ESP32S3:
+            buf = "ESP32S3";
+            break;
+        case NH_ESP_INIT_CHIP_ESP32C2:
+            buf = "ESP32C2";
+            break;
+        case NH_ESP_INIT_CHIP_ESP32C3:
+            buf = "ESP32C3";
+            break;
+        default:
+            buf = "UNKNOWN";
+            break;
+    }
+
+    PRINTF("Target SoC is: %s\r\n", buf);
+    PRINTF("Maximum SPI frequency: %dMHz\r\n", init_data->spi_freq);
+
+    PRINTF("Target capabilities: ");
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_WLAN_SPI) {
+        PRINTF("WLAN_SPI | ");
+    }
+
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_BT_SPI) {
+        PRINTF("BT_SPI | ");
+    }
+
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_WLAN_SDIO) {
+        PRINTF("WLAN_SDIO | ");
+    }
+
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_BT_SDIO) {
+        PRINTF("BT_SDIO | ");
+    }
+
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_BT_UART) {
+        PRINTF("BT_UART | ");
+    }
+
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_BLE_ONLY) {
+        PRINTF("BLE_ONLY | ");
+    }
+
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_BR_EDR_ONLY) {
+        PRINTF("BR_EDR_ONLY | ");
+    }
+
+    if (init_data->capabilities & NH_EVENT_INIT_CAP_CHECKSUM) {
+        PRINTF("CHECKSUM | ");
+    }
+
+    PRINTF("\r\n");
+}
+
+static void app_nh_impl_cb_init(void *handle) {
+    PRINTF("ESPStart event received.\r\n");
+
+    xSemaphoreGive(s_esp_init_semphore);
 }
 
 static nh_ret_t app_nh_impl_buf_allocate(void *handle, uint8_t **buf, uint32_t size) {
diff --git a/src/main.c b/src/main.c
index 06f0f95..80f745e 100644
--- a/src/main.c
+++ b/src/main.c
@@ -48,5 +48,7 @@ static void initialization_task(void *params) {
         PRINTF("Failed to init nano_hosted\r\n");
     }
 
+    PRINTF("Initialization completed, INIT task exit.\r\n");
+
     vTaskDelete(NULL);
 }
\ No newline at end of file