From 849bd4c3f7207e109a20a10d28f9163d6e4c1dc1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 13 Jun 2015 11:43:31 +0200
Subject: [PATCH] POSIX HRT: Use correct define, formatting fixes

---
 src/platforms/posix/px4_layer/drv_hrt.c | 55 ++++++++++++++-----------
 1 file changed, 32 insertions(+), 23 deletions(-)

diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c
index d1a26fd6c6..0be34240c6 100644
--- a/src/platforms/posix/px4_layer/drv_hrt.c
+++ b/src/platforms/posix/px4_layer/drv_hrt.c
@@ -77,7 +77,8 @@ static void hrt_unlock(void)
 	sem_post(&_hrt_lock);
 }
 
-#ifndef clock_gettime
+#ifdef __PX4_DARWIN
+
 #include <mach/mach_time.h>
 #define MAC_NANO (+1.0E-9)
 #define MAC_GIGA UINT64_C(1000000000)
@@ -89,25 +90,25 @@ static uint64_t px4_timestart = 0;
 
 int clock_gettime(clockid_t clk_id, struct timespec *t)
 {
-if (clk_id != CLOCK_MONOTONIC) {
-	return 1;
-}
+	if (clk_id != CLOCK_MONOTONIC) {
+		return 1;
+	}
 
 	// XXX multithreading locking
-  if (!px4_timestart) {
-    mach_timebase_info_data_t tb = { 0 };
-    mach_timebase_info(&tb);
-    px4_timebase = tb.numer;
-    px4_timebase /= tb.denom;
-    px4_timestart = mach_absolute_time();
-  }
+	if (!px4_timestart) {
+		mach_timebase_info_data_t tb = { 0 };
+		mach_timebase_info(&tb);
+		px4_timebase = tb.numer;
+		px4_timebase /= tb.denom;
+		px4_timestart = mach_absolute_time();
+	}
 
-  memset(t, 0, sizeof(*t));
+	memset(t, 0, sizeof(*t));
 
-  double diff = (mach_absolute_time() - px4_timestart) * px4_timebase;
-  t->tv_sec = diff * MAC_NANO;
-  t->tv_nsec = diff - (t->tv_sec * MAC_GIGA);
-  return 0;
+	double diff = (mach_absolute_time() - px4_timestart) * px4_timebase;
+	t->tv_sec = diff * MAC_NANO;
+	t->tv_nsec = diff - (t->tv_sec * MAC_GIGA);
+	return 0;
 }
 #endif
 
@@ -253,7 +254,7 @@ hrt_call_enter(struct hrt_call *entry)
  *
  * This routine simulates a timer interrupt handler
  */
-static void 
+static void
 hrt_tim_isr(void *p)
 {
 
@@ -283,7 +284,7 @@ hrt_call_reschedule()
 	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
 
 	//PX4_INFO("hrt_call_reschedule");
-	
+
 	/*
 	 * Determine what the next deadline will be.
 	 *
@@ -309,13 +310,13 @@ hrt_call_reschedule()
 		}
 	}
 
-	// There is no timer ISR, so simulate one by putting an event on the 
+	// There is no timer ISR, so simulate one by putting an event on the
 	// high priority work queue
 
 	// Remove the existing expiry and update with the new expiry
 	hrt_work_cancel(&_hrt_work);
 
-        hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, NULL, delay);
+	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, NULL, delay);
 }
 
 static void
@@ -323,6 +324,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 {
 	//PX4_INFO("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
 	hrt_lock();
+
 	//PX4_INFO("hrt_call_internal after lock");
 	/* if the entry is currently queued, remove it */
 	/* note that we are using a potentially uninitialised
@@ -332,14 +334,17 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	   queue for the uninitialised entry->link but we don't do
 	   anything actually unsafe.
 	*/
-	if (entry->deadline != 0)
+	if (entry->deadline != 0) {
 		sq_rem(&entry->link, &callout_queue);
+	}
 
 #if 0
+
 	// Use this to debug busy CPU that keeps rescheduling with 0 period time
 	if (interval < HRT_INTERVAL_MIN) {
 		PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);
 	}
+
 #endif
 	entry->deadline = deadline;
 	entry->period = interval;
@@ -403,17 +408,20 @@ hrt_call_invoke(void)
 	hrt_abstime deadline;
 
 	hrt_lock();
+
 	while (true) {
 		/* get the current time */
 		hrt_abstime now = hrt_absolute_time();
 
 		call = (struct hrt_call *)sq_peek(&callout_queue);
 
-		if (call == NULL)
+		if (call == NULL) {
 			break;
+		}
 
-		if (call->deadline > now)
+		if (call->deadline > now) {
 			break;
+		}
 
 		sq_rem(&call->link, &callout_queue);
 		//PX4_INFO("call pop");
@@ -448,6 +456,7 @@ hrt_call_invoke(void)
 			hrt_call_enter(call);
 		}
 	}
+
 	hrt_unlock();
 }