From 9cf9fc152b32b528e9eea427a5285dac5ee15872 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Wed, 3 Feb 2016 01:42:47 -0200
Subject: [PATCH] AP_HAL_Linux: use pthread's barrier instead of polling

Use pthread's barrier so we don't keep waking up threads with possibly
higher priority during initialization phase.

This also synchronizes all of them to a single point. With the previous
approach it was possible (but unlikely) that a thread hadn't reach the
synchronization point when main thread signalize "system initialized".
---
 libraries/AP_HAL_Linux/Scheduler.cpp | 18 ++++++++++++++----
 libraries/AP_HAL_Linux/Scheduler.h   |  4 ++++
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp
index 07c00a0083..06550c87d9 100644
--- a/libraries/AP_HAL_Linux/Scheduler.cpp
+++ b/libraries/AP_HAL_Linux/Scheduler.cpp
@@ -3,7 +3,6 @@
 #include <algorithm>
 #include <errno.h>
 #include <poll.h>
-#include <pthread.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <sys/mman.h>
@@ -66,6 +65,8 @@ void Scheduler::init()
     struct sched_param param = { .sched_priority = APM_LINUX_MAIN_PRIORITY };
     sched_setscheduler(0, SCHED_FIFO, &param);
 
+    /* set barrier to 6 threads: worker threads below + main thread */
+    pthread_barrier_init(&_initialized_barrier, nullptr, 6);
     _timer_thread.start("sched-timer", SCHED_FIFO, APM_LINUX_TIMER_PRIORITY);
     _uart_thread.start("sched-uart", SCHED_FIFO, APM_LINUX_UART_PRIORITY);
     _rcin_thread.start("sched-rcin", SCHED_FIFO, APM_LINUX_RCIN_PRIORITY);
@@ -406,12 +407,23 @@ bool Scheduler::system_initializing() {
     return !_initialized;
 }
 
+void Scheduler::_wait_all_threads()
+{
+    int r = pthread_barrier_wait(&_initialized_barrier);
+    if (r == PTHREAD_BARRIER_SERIAL_THREAD) {
+        pthread_barrier_destroy(&_initialized_barrier);
+    }
+}
+
 void Scheduler::system_initialized()
 {
     if (_initialized) {
         AP_HAL::panic("PANIC: scheduler::system_initialized called more than once");
     }
+
     _initialized = true;
+
+    _wait_all_threads();
 }
 
 void Scheduler::reboot(bool hold_in_bootloader)
@@ -429,9 +441,7 @@ void Scheduler::stop_clock(uint64_t time_usec)
 
 bool Scheduler::SchedulerThread::_run()
 {
-    while (_sched.system_initializing()) {
-        poll(NULL, 0, 1);
-    }
+    _sched._wait_all_threads();
 
     return Thread::_run();
 }
diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h
index cdc592959f..1bf30d7b6c 100644
--- a/libraries/AP_HAL_Linux/Scheduler.h
+++ b/libraries/AP_HAL_Linux/Scheduler.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <pthread.h>
+
 #include "AP_HAL_Linux.h"
 #include "Semaphores.h"
 #include "Thread.h"
@@ -60,6 +62,7 @@ private:
         Scheduler &_sched;
     };
 
+    void _wait_all_threads();
     void _timer_handler(int signum);
 
     AP_HAL::Proc _delay_cb;
@@ -68,6 +71,7 @@ private:
     AP_HAL::Proc _failsafe;
 
     bool _initialized;
+    pthread_barrier_t _initialized_barrier;
     volatile bool _timer_pending;
 
     AP_HAL::MemberProc _timer_proc[LINUX_SCHEDULER_MAX_TIMER_PROCS];