From c551723914ac5ce123216c1dd776bc4fdf25cc65 Mon Sep 17 00:00:00 2001
From: Andrew Leech <andrew.leech@planetinnovation.com.au>
Date: Wed, 23 Feb 2022 13:06:44 +1100
Subject: [PATCH] stm32/powerctrl: Disable WB55 BLE before entering deepsleep.

Signed-off-by: Andrew Leech <andrew.leech@planetinnovation.com.au>
---
 ports/stm32/mpbthciport.c |  2 +-
 ports/stm32/powerctrl.c   |  5 +++++
 ports/stm32/rfcore.c      | 30 +++++++++++++++++++++++++-----
 ports/stm32/rfcore.h      |  1 +
 4 files changed, 32 insertions(+), 6 deletions(-)

diff --git a/ports/stm32/mpbthciport.c b/ports/stm32/mpbthciport.c
index d73aa50c37..fe061a1240 100644
--- a/ports/stm32/mpbthciport.c
+++ b/ports/stm32/mpbthciport.c
@@ -116,7 +116,7 @@ int mp_bluetooth_hci_uart_init(uint32_t port, uint32_t baudrate) {
 
 int mp_bluetooth_hci_uart_deinit(void) {
     DEBUG_printf("mp_bluetooth_hci_uart_deinit (stm32 rfcore)\n");
-
+    rfcore_ble_reset();
     return 0;
 }
 
diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c
index 3439b0d1fa..373f6b1ab8 100644
--- a/ports/stm32/powerctrl.c
+++ b/ports/stm32/powerctrl.c
@@ -29,6 +29,7 @@
 #include "powerctrl.h"
 #include "rtc.h"
 #include "genhdr/pllfreqtable.h"
+#include "extmod/modbluetooth.h"
 
 #if defined(STM32H7)
 #define RCC_SR          RSR
@@ -947,6 +948,10 @@ void powerctrl_enter_standby_mode(void) {
     }
     #endif
 
+    #if defined(STM32WB) && MICROPY_PY_BLUETOOTH
+    mp_bluetooth_deinit();
+    #endif
+
     // We need to clear the PWR wake-up-flag before entering standby, since
     // the flag may have been set by a previous wake-up event.  Furthermore,
     // we need to disable the wake-up sources while clearing this flag, so
diff --git a/ports/stm32/rfcore.c b/ports/stm32/rfcore.c
index 20b2c5a7e6..2a75f7c74a 100644
--- a/ports/stm32/rfcore.c
+++ b/ports/stm32/rfcore.c
@@ -600,18 +600,38 @@ static const struct {
 void rfcore_ble_init(void) {
     DEBUG_printf("rfcore_ble_init\n");
 
-    // Clear any outstanding messages from ipcc_init.
-    tl_check_msg(&ipcc_mem_sys_queue, IPCC_CH_SYS, NULL);
-
     // Configure and reset the BLE controller.
-    tl_sys_hci_cmd_resp(HCI_OPCODE(OGF_VENDOR, OCF_BLE_INIT), (const uint8_t *)&ble_init_params, sizeof(ble_init_params), 0);
-    tl_ble_hci_cmd_resp(HCI_OPCODE(0x03, 0x0003), NULL, 0);
+    if (!rfcore_ble_reset()) {
+        // ble init can fail if core2 has previously locked up. Reset HSI & rfcore to retry.
+        LL_RCC_HSI_Disable();
+        mp_hal_delay_ms(100);
+        LL_RCC_HSI_Enable();
+
+        rfcore_init();
+        rfcore_ble_reset();
+    }
 
     // Enable PES rather than SEM7 to moderate flash access between the cores.
     uint8_t buf = 0; // FLASH_ACTIVITY_CONTROL_PES
     tl_sys_hci_cmd_resp(HCI_OPCODE(OGF_VENDOR, OCF_C2_SET_FLASH_ACTIVITY_CONTROL), &buf, 1, 0);
 }
 
+bool rfcore_ble_reset(void) {
+    DEBUG_printf("rfcore_ble_reset\n");
+
+    // Clear any outstanding messages from ipcc_init.
+    tl_check_msg(&ipcc_mem_sys_queue, IPCC_CH_SYS, NULL);
+
+    // Configure and reset the BLE controller.
+    int ret = tl_sys_hci_cmd_resp(HCI_OPCODE(OGF_VENDOR, OCF_BLE_INIT), (const uint8_t *)&ble_init_params, sizeof(ble_init_params), 500);
+
+    if (ret == -MP_ETIMEDOUT) {
+        return false;
+    }
+    tl_ble_hci_cmd_resp(HCI_OPCODE(0x03, 0x0003), NULL, 0);
+    return true;
+}
+
 void rfcore_ble_hci_cmd(size_t len, const uint8_t *src) {
     DEBUG_printf("rfcore_ble_hci_cmd\n");
 
diff --git a/ports/stm32/rfcore.h b/ports/stm32/rfcore.h
index 5c67f194e2..39267b325e 100644
--- a/ports/stm32/rfcore.h
+++ b/ports/stm32/rfcore.h
@@ -33,6 +33,7 @@ typedef void (*rfcore_ble_msg_callback_t)(void *, const uint8_t *, size_t);
 void rfcore_init(void);
 
 void rfcore_ble_init(void);
+bool rfcore_ble_reset(void);
 void rfcore_ble_hci_cmd(size_t len, const uint8_t *src);
 size_t rfcore_ble_check_msg(rfcore_ble_msg_callback_t cb, void *env);
 void rfcore_ble_set_txpower(uint8_t level);