From 4956feffdfc8459c5f27e471b50978db29f23c07 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 6 Dec 2013 20:51:00 +1100
Subject: [PATCH] drv_hrt: added hrt_call_init() and hrt_call_delay() APIs

hrt_call_init() can be used to initialise (zero) a hrt_call structure
to ensure safe usage. The hrt_call_every() interface calls this
automatically.

hrt_call_delay() can be used to delay a current callout by the given
number of microseconds
---
 src/drivers/drv_hrt.h       | 14 ++++++++++++++
 src/drivers/stm32/drv_hrt.c | 19 ++++++++++++++++++-
 2 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index 8a99eeca79..d130d68b34 100644
--- a/src/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
@@ -141,6 +141,20 @@ __EXPORT extern bool	hrt_called(struct hrt_call *entry);
  */
 __EXPORT extern void	hrt_cancel(struct hrt_call *entry);
 
+/*
+ * initialise a hrt_call structure
+ */
+__EXPORT extern void	hrt_call_init(struct hrt_call *entry);
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+__EXPORT extern void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
+
 /*
  * Initialise the HRT.
  */
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index e79d7e10a3..dc28f446b4 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -720,6 +720,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
 void
 hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
 {
+	hrt_call_init(entry);
 	hrt_call_internal(entry,
 			  hrt_absolute_time() + delay,
 			  interval,
@@ -839,7 +840,12 @@ hrt_call_invoke(void)
 
 		/* if the callout has a non-zero period, it has to be re-entered */
 		if (call->period != 0) {
-			call->deadline = deadline + call->period;
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves 
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
 			hrt_call_enter(call);
 		}
 	}
@@ -906,5 +912,16 @@ hrt_latency_update(void)
 	latency_counters[index]++;
 }
 
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void
+hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
 
 #endif /* HRT_TIMER */