ll_scheduler: add check for newly calculated timeout

CODEOWNERS

@@ -48,6 +48,7 @@ src/ipc/*				@xiulipan @bardliao
 src/ipc/sue-ipc.c			@lyakh
 src/lib/*				@libinyang
 src/gdb/*				@mrajwa
+src/schedule				@tlauda @mrajwa
 
 # other helpers
 test/**					@jajanusz

src/arch/host/include/arch/drivers/timer.h

@@ -21,8 +21,8 @@ static inline void arch_timer_unregister(struct timer *timer) {}
 static inline void arch_timer_enable(struct timer *timer) {}
 static inline void arch_timer_disable(struct timer *timer) {}
 static inline uint32_t arch_timer_get_system(struct timer *timer) {return 0; }
-static inline int arch_timer_set(struct timer *timer,
-	uint64_t ticks) {return 0; }
+static inline int64_t arch_timer_set(struct timer *timer,
+				     uint64_t ticks) {return 0; }
 static inline void arch_timer_clear(struct timer *timer) {}
 
 #endif /* __ARCH_DRIVERS_TIMER_H__ */

src/arch/xtensa/drivers/timer.c

@@ -103,7 +103,7 @@ uint64_t arch_timer_get_system(struct timer *timer)
 	return time;
 }
 
-int arch_timer_set(struct timer *timer, uint64_t ticks)
+int64_t arch_timer_set(struct timer *timer, uint64_t ticks)
 {
 	uint32_t time = 1;
 	uint32_t hitimeout = ticks >> 32;

src/arch/xtensa/include/arch/drivers/timer.h

@@ -63,7 +63,7 @@ static inline void arch_timer_disable(struct timer *timer)
 
 uint64_t arch_timer_get_system(struct timer *timer);
 
-int arch_timer_set(struct timer *timer, uint64_t ticks);
+int64_t arch_timer_set(struct timer *timer, uint64_t ticks);
 
 static inline void arch_timer_clear(struct timer *timer)
 {

src/drivers/imx/timer.c

@@ -22,7 +22,7 @@ void platform_timer_stop(struct timer *timer)
 	arch_timer_disable(timer);
 }
 
-int platform_timer_set(struct timer *timer, uint64_t ticks)
+int64_t platform_timer_set(struct timer *timer, uint64_t ticks)
 {
 	return arch_timer_set(timer, ticks);
 }

src/drivers/intel/baytrail/timer.c

@@ -75,7 +75,7 @@ void platform_timer_stop(struct timer *timer)
 	shim_write(SHIM_EXT_TIMER_CNTLH, SHIM_EXT_TIMER_CLEAR);
 }
 
-int platform_timer_set(struct timer *timer, uint64_t ticks)
+int64_t platform_timer_set(struct timer *timer, uint64_t ticks)
 {
 	uint32_t time = 1;
 	uint32_t hitimeout = ticks >> 32;

src/drivers/intel/cavs/timer.c

@@ -32,17 +32,22 @@ void platform_timer_stop(struct timer *timer)
 		   shim_read(SHIM_DSPWCTCS) & ~SHIM_DSPWCTCS_T0A);
 }
 
-int platform_timer_set(struct timer *timer, uint64_t ticks)
+int64_t platform_timer_set(struct timer *timer, uint64_t ticks)
 {
 	/* a tick value of 0 will not generate an IRQ */
 	if (ticks == 0)
 		ticks = 1;
 
-	/* set new value and run */
-	shim_write64(SHIM_DSPWCT0C, ticks);
+	/* Check if requested time is not past time */
+	if (ticks > shim_read64(SHIM_DSPWC))
+		shim_write64(SHIM_DSPWCT0C, ticks);
+	else
+		shim_write64(SHIM_DSPWCT0C, shim_read64(SHIM_DSPWC) + 1);
+
+	/* Enable IRQ */
 	shim_write(SHIM_DSPWCTCS, SHIM_DSPWCTCS_T0A);
 
-	return 0;
+	return shim_read64(SHIM_DSPWCT0C);
 }
 
 void platform_timer_clear(struct timer *timer)

src/drivers/intel/haswell/timer.c

@@ -23,7 +23,7 @@ void platform_timer_stop(struct timer *timer)
 	arch_timer_disable(timer);
 }
 
-int platform_timer_set(struct timer *timer, uint64_t ticks)
+int64_t platform_timer_set(struct timer *timer, uint64_t ticks)
 {
 	return arch_timer_set(timer, ticks);
 }

src/include/sof/drivers/timer.h

@@ -25,7 +25,7 @@ void timer_unregister(struct timer *timer, void *arg);
 void timer_enable(struct timer *timer, void *arg, int core);
 void timer_disable(struct timer *timer, void *arg, int core);
 
-static inline int timer_set(struct timer *timer, uint64_t ticks)
+static inline int64_t timer_set(struct timer *timer, uint64_t ticks)
 {
 	return arch_timer_set(timer, ticks);
 }
@@ -46,7 +46,7 @@ static inline uint64_t timer_get_system(struct timer *timer)
 	return arch_timer_get_system(timer);
 }
 
-int platform_timer_set(struct timer *timer, uint64_t ticks);
+int64_t platform_timer_set(struct timer *timer, uint64_t ticks);
 void platform_timer_clear(struct timer *timer);
 uint64_t platform_timer_get(struct timer *timer);
 void platform_timer_start(struct timer *timer);

src/schedule/timer_domain.c

@@ -24,6 +24,16 @@ struct timer_domain {
 
 struct ll_schedule_domain_ops timer_domain_ops;
 
+static inline void timer_report_delay(int id, uint64_t delay)
+{
+	uint32_t ll_delay_us = (delay * 1000) /
+				clock_ms_to_ticks(PLATFORM_DEFAULT_CLOCK, 1);
+
+	trace_schedule_error("timer_report_delay(): timer %d "
+			     "delayed by %d uS %d ticks", id, ll_delay_us,
+			     delay);
+}
+
 static int timer_domain_register(struct ll_schedule_domain *domain,
 				 uint64_t period, struct task *task,
 				 void (*handler)(void *arg), void *arg)
@@ -72,12 +82,20 @@ static void timer_domain_disable(struct ll_schedule_domain *domain, int core)
 static void timer_domain_set(struct ll_schedule_domain *domain, uint64_t start)
 {
 	struct timer_domain *timer_domain = ll_sch_domain_get_pdata(domain);
-	uint64_t ticks = domain->ticks_per_ms * timer_domain->timeout / 1000 +
-		start;
+	uint64_t ticks_req = domain->ticks_per_ms * timer_domain->timeout /
+			     1000 + start;
+	uint64_t ticks_set;
+
+	ticks_set = platform_timer_set(timer_domain->timer, ticks_req);
 
-	platform_timer_set(timer_domain->timer, ticks);
+	/* Was timer set to the value we requested? If no it means some
+	 * delay occurred and we should report that in error log.
+	 */
+	if (ticks_req < ticks_set)
+		timer_report_delay(timer_domain->timer->id,
+				   ticks_set - ticks_req);
 
-	domain->last_tick = ticks;
+	domain->last_tick = ticks_set;
 }
 
 static void timer_domain_clear(struct ll_schedule_domain *domain)