drivers: timer: Fix the logic to compensate the clock when turned off

The original logic relied on the tick passed in. This method
is inaccurate as the tick value passed in was the exit latency.
Update the code to calculate the remaining time left and set
a counter using this value.

Signed-off-by: Mahesh Mahadevan <mahesh.mahadevan@nxp.com>

drivers/timer/mcux_os_timer.c

@@ -44,7 +44,7 @@ static uint64_t cyc_sys_compensated;
  */
 static const struct device *counter_dev;
 /* Indicates if the counter is running. */
-bool counter_running;
+static bool counter_running;
 #endif
 
 static uint64_t mcux_lpc_ostick_get_compensated_timer_value(void)
@@ -90,12 +90,10 @@ static uint32_t mcux_lpc_ostick_set_counter_timeout(int32_t curr_timeout)
 	uint32_t ret = 0;
 
 	if (counter_dev) {
-		uint32_t timeout;
-		int32_t ticks;
+		uint32_t ticks;
 		struct counter_top_cfg top_cfg = { 0 };
-		timeout = k_ticks_to_us_near32(curr_timeout);
 
-		ticks = counter_us_to_ticks(counter_dev, timeout);
+		ticks = counter_us_to_ticks(counter_dev, curr_timeout);
 		ticks = CLAMP(ticks, 1, counter_get_max_top_value(counter_dev));
 
 		top_cfg.ticks = ticks;
@@ -181,21 +179,33 @@ static uint32_t mcux_lpc_ostick_compensate_system_timer(void)
 
 void sys_clock_set_timeout(int32_t ticks, bool idle)
 {
-	ARG_UNUSED(idle);
-
 	if (!IS_ENABLED(CONFIG_TICKLESS_KERNEL)) {
 		/* Only for tickless kernel system */
 		return;
 	}
 
 #if DT_NODE_HAS_STATUS_OKAY(DT_NODELABEL(standby)) && CONFIG_PM
-	if (idle) {
+	/* We intercept calls from idle with a 0 tick count */
+	if (idle && ticks == 0) {
 		/* OS Timer may not be able to wakeup in certain low power modes.
 		 * For these cases, we start a counter that can wakeup
 		 * from low power modes.
 		 */
 		if (pm_state_next_get(0)->state == PM_STATE_STANDBY) {
-			if (mcux_lpc_ostick_set_counter_timeout(ticks) == 0) {
+			uint64_t timeout;
+
+			/* Check the amount of time left and switch to a counter
+			 * that is active in this power mode.
+			 */
+			timeout = base->MATCH_L;
+			timeout |= (uint64_t)(base->MATCH_H) << 32;
+			timeout = OSTIMER_GrayToDecimal(timeout);
+			timeout -= OSTIMER_GetCurrentTimerValue(base);
+			/* Round up to the next tick boundary */
+			timeout += (CYC_PER_TICK - 1);
+			/* Convert to microseconds and round up to the next value */
+			timeout = (((timeout / CYC_PER_TICK) * CYC_PER_TICK) * CYC_PER_US);
+			if (mcux_lpc_ostick_set_counter_timeout(timeout) == 0) {
 				/* A low power counter has been started. No need to
 				 * go further, simply return
 				 */
@@ -203,6 +213,8 @@ void sys_clock_set_timeout(int32_t ticks, bool idle)
 			}
 		}
 	}
+#else
+	ARG_UNUSED(idle);
 #endif
 
 	ticks = ticks == K_TICKS_FOREVER ? MAX_TICKS : ticks;

soc/nxp/rw/power.c

@@ -12,6 +12,7 @@
 		    DT_NODE_HAS_STATUS_OKAY(DT_NODELABEL(pin1)))
 #include <zephyr/drivers/gpio/gpio_mcux_lpc.h>
 #endif
+#include <zephyr/drivers/timer/system_timer.h>
 
 #include "fsl_power.h"
 
@@ -197,6 +198,9 @@ __weak void pm_state_set(enum pm_state state, uint8_t substate_id)
 #endif /* CONFIG_MPU */
 
 		POWER_EnableWakeup(DT_IRQN(DT_NODELABEL(rtc)));
+
+		sys_clock_set_timeout(0, true);
+
 		if (POWER_EnterPowerMode(POWER_MODE3, &slp_cfg)) {
 #ifdef CONFIG_MPU
 			/* Restore MPU as is lost after PM3 exit*/