From 923a7cc505971ab6d04115d18108152f62d46283 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 24 Feb 2013 11:41:26 -0800
Subject: [PATCH] Add an interrupt-safe way of comparing a timestamp with the
 current time. Add an interrupt-safe way of storing the current time into a
 timestamp.

---
 apps/drivers/drv_hrt.h       | 16 ++++++++++++++++
 apps/drivers/stm32/drv_hrt.c | 30 ++++++++++++++++++++++++++++++
 2 files changed, 46 insertions(+)

diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h
index 3b493a81a8..2e30725bdc 100644
--- a/apps/drivers/drv_hrt.h
+++ b/apps/drivers/drv_hrt.h
@@ -91,6 +91,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
  */
 __EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
 
+/*
+ * Compute the delta between a timestamp taken in the past
+ * and now.
+ *
+ * This function is safe to use even if the timestamp is updated
+ * by an interrupt during execution.
+ */
+__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
+
+/*
+ * Store the absolute time in an interrupt-safe fashion.
+ *
+ * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
+ */
+__EXPORT extern hrt_absolute_time hrt_store_absolute_time(volatile hrt_abstime *now);
+
 /*
  * Call callout(arg) after delay has elapsed.
  *
diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c
index cd9cb45b1c..0df2a8b748 100644
--- a/apps/drivers/stm32/drv_hrt.c
+++ b/apps/drivers/stm32/drv_hrt.c
@@ -645,6 +645,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 	ts->tv_nsec = abstime * 1000;
 }
 
+/*
+ * Compare a time value with the current time.
+ */
+hrt_abstime
+hrt_elapsed_time(const volatile hrt_abstime *then)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime delta = hrt_absolute_time() - *then;
+
+	irqrestore(flags);
+
+	return delta;
+}
+
+/*
+ * Store the absolute time in an interrupt-safe fashion
+ */
+hrt_absolute_time
+hrt_store_absolute_time(volatile hrt_abstime *now)
+{
+	irqstate_t flags = irqsave();
+
+	hrt_abstime ts = hrt_absolute_time();
+
+	irqrestore(flags);
+
+	return ts;
+}
+
 /*
  * Initalise the high-resolution timing module.
  */