epicardium: Query BLE hardware for idle periods

Query the BLE base band for idle periods and put the CPU to sleep during
this time. This allows us to reduce clock speed again and wake up in
time to quickly serve BLE interrupts.

In the future we can now even go lower than DIV4, as BLE continues to
work at least until DIV16.

epicardium/support.c

@@ -9,13 +9,45 @@
 #include "user_core/user_core.h"
 #include "os/core.h"
 #include "drivers/drivers.h"
+#include "modules/modules.h"
+
+#define BB_CLK_RATE_HZ 1600000
 
 #include "usb.h"
 #include "mxc_sys.h"
 #include "wut.h"
+#include "wsf_types.h"
+#include "wsf_os.h"
+
+#include "sch_api_ble.h"
+#include "bb_drv.h"
 
 #include "card10.h"
 
+#define US_TO_BBTICKS(x) (((x) * (BB_CLK_RATE_HZ / 100000) + 9) / 10)
+
+#define MIN(a, b) (a < b) ? a : b
+
+#define MIN_SLEEP_TIME_MS 1
+
+static int32_t ble_sleep_ticks(void)
+{
+	uint32_t nextDbbEventDue;
+	bool_t dueValid = SchBleGetNextDueTime(&nextDbbEventDue);
+	int sleep_ticks = nextDbbEventDue - BbDrvGetCurrentTime();
+
+	if (dueValid) {
+		if (sleep_ticks > 0) {
+			uint32_t bb_idle = BB_TICKS_TO_US(sleep_ticks) / 1000;
+			return bb_idle;
+		} else {
+			return 0;
+		}
+	} else {
+		return -1;
+	}
+}
+
 /*
  * This hook is called before FreeRTOS enters tickless idle.
  */
@@ -28,16 +60,23 @@ void pre_idle_sleep(TickType_t xExpectedIdleTime)
 		 * epicardium if it wants to issue an API call.
 		 */
 
-		/*
-		 * TODO: Ensure this is actually correct and does not have any
-		 * race conditions.
-		 */
-
 		/* If the other core is waiting for a call to return and we are able
 		 * to sleep, reduce the system clock to save energy. */
-		if (xExpectedIdleTime >= pdMS_TO_TICKS(10) &&
-		    api_dispatcher_call_pending()) {
-			uint32_t ms = xExpectedIdleTime - 1;
+		if (xExpectedIdleTime >= pdMS_TO_TICKS(MIN_SLEEP_TIME_MS) &&
+		    api_dispatcher_call_pending() && wsfOsReadyToSleep()) {
+			uint32_t ms = xExpectedIdleTime;
+
+			if (ble_is_enabled()) {
+				int32_t ble_idle = ble_sleep_ticks();
+				if (ble_idle >= 0) {
+					ms =
+						MIN(xExpectedIdleTime,
+						    (uint32_t)ble_idle);
+				}
+			}
+
+			// Us the WUT only if we can sleep a significant amount of time
+			if (ms >= MIN_SLEEP_TIME_MS) {
 				/* Initialize Wakeup timer */
 				WUT_Init(WUT_PRES_1);
 				wut_cfg_t wut_cfg;
@@ -51,7 +90,9 @@ void pre_idle_sleep(TickType_t xExpectedIdleTime)
 
 				uint32_t targetTick;
 				targetTick = WUT_GetCount();
-			targetTick += ((uint64_t)(ms)*SYS_WUT_GetFreq() / 1000);
+				targetTick +=
+					((uint64_t)(ms)*SYS_WUT_GetFreq() /
+					 1000);
 				WUT_SetCompare(targetTick);
 
 				/* Stop SysTick */
@@ -59,34 +100,53 @@ void pre_idle_sleep(TickType_t xExpectedIdleTime)
 				SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
 
 				if (usb_get_status() & MAXUSB_STATUS_VBUS_ON) {
-				SYS_Clock_Select(SYS_CLOCK_HIRC, NULL);
+					/* Need to stay on 96 MHz. USB serial becomes
+					* unstable otherwise. Don't know why. */
+					//SYS_Clock_Select(SYS_CLOCK_HIRC, NULL);
 				} else {
+					MXC_GCR->clkcn |=
+						MXC_S_GCR_CLKCN_PSC_DIV4;
 					SYS_Clock_Select(SYS_CLOCK_HIRC, NULL);
-				SYS_ClockSourceDisable(SYS_CLOCK_HIRC96);
+					SYS_ClockSourceDisable(
+						SYS_CLOCK_HIRC96
+					);
 				}
 				disp_update_backlight_clock();
 				__asm volatile("dsb" ::: "memory");
 				__asm volatile("wfe");
 				__asm volatile("isb");
+				MXC_GCR->clkcn &= ~(MXC_S_GCR_CLKCN_PSC_DIV4);
 				SYS_Clock_Select(SYS_CLOCK_HIRC96, NULL);
+#if 0
+				/* Allow to serve high priority interrupts before
+				 * doing the lengthy xTaskIncrementTick loop. */
+				portDISABLE_INTERRUPTS();
+				__set_PRIMASK(0);
+				__set_PRIMASK(1);
+				portENABLE_INTERRUPTS();
+#endif
 				disp_update_backlight_clock();
 				SYS_ClockSourceDisable(SYS_CLOCK_HIRC);
 				SysTick->LOAD = val;
 				SysTick->VAL  = 0;
 				SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;
 
-			ms = (WUT_GetCount() * 1000) / SYS_WUT_GetFreq();
+				ms = (WUT_GetCount() * 1000) /
+				     SYS_WUT_GetFreq();
 				for (uint32_t t = 0; t < ms; t++) {
 					xTaskIncrementTick();
 				}
+				return;
+			}
+		}
 
-		} else {
+		/* Fall back to light sleep with clocks on if we can't
+		 * sleep long enough for the WUT */
 		__asm volatile("dsb" ::: "memory");
 		__asm volatile("wfe");
 		__asm volatile("isb");
 	}
 }
-}
 
 /*
  * This hook is called after FreeRTOS exits tickless idle.