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,63 +60,91 @@ 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;
-			/* Initialize Wakeup timer */
-			WUT_Init(WUT_PRES_1);
-			wut_cfg_t wut_cfg;
-			wut_cfg.mode    = WUT_MODE_COMPARE;
-			wut_cfg.cmp_cnt = 0xFFFFFFFF;
-			WUT_Config(&wut_cfg);
-			WUT_Enable();
-
-			/* Enable WUT as a wakup source */
-			NVIC_EnableIRQ(WUT_IRQn);
-
-			uint32_t targetTick;
-			targetTick = WUT_GetCount();
-			targetTick += ((uint64_t)(ms)*SYS_WUT_GetFreq() / 1000);
-			WUT_SetCompare(targetTick);
-
-			/* Stop SysTick */
-			uint32_t val = SysTick->VAL;
-			SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
-
-			if (usb_get_status() & MAXUSB_STATUS_VBUS_ON) {
-				SYS_Clock_Select(SYS_CLOCK_HIRC, NULL);
-			} else {
-				SYS_Clock_Select(SYS_CLOCK_HIRC, NULL);
-				SYS_ClockSourceDisable(SYS_CLOCK_HIRC96);
-			}
-			disp_update_backlight_clock();
-			__asm volatile("dsb" ::: "memory");
-			__asm volatile("wfe");
-			__asm volatile("isb");
-			SYS_Clock_Select(SYS_CLOCK_HIRC96, NULL);
-			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();
-			for (uint32_t t = 0; t < ms; t++) {
-				xTaskIncrementTick();
+		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);
+				}
 			}
 
-		} else {
-			__asm volatile("dsb" ::: "memory");
-			__asm volatile("wfe");
-			__asm volatile("isb");
+			// 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;
+				wut_cfg.mode    = WUT_MODE_COMPARE;
+				wut_cfg.cmp_cnt = 0xFFFFFFFF;
+				WUT_Config(&wut_cfg);
+				WUT_Enable();
+
+				/* Enable WUT as a wakup source */
+				NVIC_EnableIRQ(WUT_IRQn);
+
+				uint32_t targetTick;
+				targetTick = WUT_GetCount();
+				targetTick +=
+					((uint64_t)(ms)*SYS_WUT_GetFreq() /
+					 1000);
+				WUT_SetCompare(targetTick);
+
+				/* Stop SysTick */
+				uint32_t val = SysTick->VAL;
+				SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
+
+				if (usb_get_status() & MAXUSB_STATUS_VBUS_ON) {
+					/* 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
+					);
+				}
+				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();
+				for (uint32_t t = 0; t < ms; t++) {
+					xTaskIncrementTick();
+				}
+				return;
+			}
 		}
+
+		/* 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");
 	}
 }