copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/crio/shared_libs/ByteBuffer.h b/aos/crio/shared_libs/ByteBuffer.h
new file mode 100644
index 0000000..b5c4902
--- /dev/null
+++ b/aos/crio/shared_libs/ByteBuffer.h
@@ -0,0 +1,91 @@
+#ifndef __CRIO_SHARED_LIBS_BYTE_BUFFER_H_
+#define __CRIO_SHARED_LIBS_BYTE_BUFFER_H_
+
+#include "aos/common/network/ReceiveSocket.h"
+#include <algorithm>
+
+namespace aos {
+
+class ByteBuffer {
+ public:
+ int m_size;
+ int m_length;
+ int m_i;
+ char *m_buffer;
+ bool recv_from_sock(ReceiveSocket *sock) {
+ m_length = sock->Recv(m_buffer, m_size, 40000);
+ if (m_length < 0) {
+ m_length = 0;
+ }
+ m_i = 0;
+ return m_length != 0;
+ }
+ ByteBuffer(int size) {
+ m_buffer = new char(size);
+ m_size = size;
+ }
+ ~ByteBuffer() {
+ delete m_buffer;
+ }
+ // Reads an uint32_t into *number and returns true on success. *number is
+ // unmodified on failure.
+ bool read_uint32(uint32_t *number) {
+ uint32_t vals[4];
+ if (m_i + 4 > m_length) {
+ m_i = m_length;
+ return false;
+ }
+ for (int i = 0; i < 4; ++i) {
+ vals[i] = read_char();
+ }
+ *number = vals[3] + (vals[2] << 8) + (vals[1] << 16) + (vals[0] << 24);
+ return true;
+ }
+ float read_float() {
+ if (m_i + 4 <= m_length) {
+ float r;
+ memcpy(&r, &m_buffer[m_i], 4);
+ m_i += 4;
+ return r;
+ } else {
+ return 1.0 / 0.0;
+ }
+ }
+ int read_char() {
+ if (m_i < m_length) {
+ int val = m_buffer[m_i];
+ m_i ++;
+ return val;
+ } else {
+ return -1;
+ }
+ }
+
+ int read_string(char *buf, size_t max_len) {
+ int data_len = read_char();
+ if (data_len <= 0) {
+ return -1;
+ }
+ size_t to_read = std::min<size_t>(static_cast<uint8_t>(data_len), max_len);
+ memcpy(buf, &m_buffer[m_i], to_read);
+ m_i += to_read;
+ return 0;
+ }
+ // Returns success or not.
+ bool read_bytes(void *buf, size_t bytes) {
+ if (m_length - m_i < static_cast<ssize_t>(bytes)) return false;
+ memcpy(buf, &m_buffer[m_i], bytes);
+ m_i += bytes;
+ return true;
+ }
+ char *get_bytes(size_t number) {
+ if (m_length - m_i < static_cast<ssize_t>(number)) return NULL;
+ m_i += number;
+ return &m_buffer[m_i - number];
+ }
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/shared_libs/interrupt_bridge-tmpl.h b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
new file mode 100644
index 0000000..3adba38
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
@@ -0,0 +1,243 @@
+#include <sysLib.h>
+#include <usrLib.h>
+#include <sigevent.h>
+
+#include "WPILib/Task.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/motor_server/MotorServer.h"
+#include "aos/common/time.h"
+
+extern "C" {
+// A really simple function implemented in a .c file because the header that
+// declares the variable doesn't work in C++.
+// Returns the value of the global variable excExtendedVectors declared by
+// <usrConfig.h>.
+int aos_getExcExtendedVectors();
+}
+
+namespace aos {
+namespace crio {
+
+// Conceptually a TimerNotifier*[]. Used by the signal handler so that it can
+// figure out which instance it's supposed to be using.
+// Doesn't need a lock because a value is put in here before the signal handler
+// is set up, so everything that might have concurrency issues will be reads
+// which will happen after the set to each index.
+// Declared like this instead of as an actual array because its size might be
+// determined dynamically (SIGRTMAX and SIGRTMIN can be function calls).
+extern void **const timer_notifiers;
+
+template<typename T>
+InterruptBridge<T>::InterruptBridge(Handler handler,
+ T *param, int priority)
+ : handler_(handler),
+ param_(param),
+ task_(new Task("_NotifierLooper", reinterpret_cast<FUNCPTR>(HandlerLoop),
+ priority)),
+ sem_(semBCreate(SEM_Q_PRIORITY, SEM_EMPTY)) {
+ // Basically, makes sure that it does -mlongcall for calling interrupt handler
+ // functions.
+ // See <http://www.vxdev.com/docs/vx55man/vxworks/ppc/powerpc.html#91321> for
+ // details about why this is important.
+ if (!aos_getExcExtendedVectors()) {
+ LOG(FATAL, "extended-call interrupts are not enabled\n");
+ }
+}
+
+template<typename T>
+InterruptBridge<T>::~InterruptBridge() {
+ // Can't call Stop() because that calls StopNotifications(), which is virtual,
+ // so you can't call it after the subclass's destructor has run.
+ semDelete(sem_);
+}
+
+template<typename T>
+void InterruptBridge<T>::StartTask() {
+ // The inner cast to InterruptBridge<T>* is so that Loop knows what to
+ // cast the void* to (the implementation of polymorphism means that
+ // casting the pointer might actually change its value).
+ task_->Start(reinterpret_cast<UINT32>(
+ static_cast<InterruptBridge<T> *>(this)));
+}
+
+template<typename T>
+void InterruptBridge<T>::Stop() {
+ StopNotifications();
+ task_->Stop();
+}
+
+template<typename T>
+void InterruptBridge<T>::HandlerLoop(void *self_in) {
+ InterruptBridge<T> *const self = static_cast<InterruptBridge<T> *>(self_in);
+ while (true) {
+ semTake(self->sem_, WAIT_FOREVER);
+ self->handler_(self->param_);
+ }
+}
+
+template<typename T>
+PeriodicNotifier<T>::PeriodicNotifier(
+ typename InterruptBridge<T>::Handler handler,
+ T *param, int priority)
+ : InterruptBridge<T>(handler, param, priority) {}
+
+template<typename T>
+void PeriodicNotifier<T>::StartPeriodic(double period) {
+ this->StartTask();
+ StartNotifications(period);
+}
+
+template<typename T>
+TimerNotifier<T>::TimerNotifier(typename InterruptBridge<T>::Handler handler,
+ T *param, int unique, int priority)
+ : PeriodicNotifier<T>(handler, param, priority),
+ signal_(SIGRTMIN + unique) {
+ timer_notifiers[signal_] = static_cast<void *>(this);
+
+ struct sigaction sa;
+ sa.sa_flags = 0;
+ sa.sa_handler = StaticNotify;
+ sigemptyset(&sa.sa_mask);
+ if (sigaction(signal_, &sa, &old_sa_) != OK) {
+ LOG(FATAL, "sigaction(%d, %p, %p) failed with %d: %s\n",
+ signal_, &sa, &old_sa_, errno, strerror(errno));
+ }
+
+ sigevent event;
+ event.sigev_notify = SIGEV_TASK_SIGNAL;
+ event.sigev_signo = signal_;
+ event.sigev_value.sival_ptr = this;
+ if (timer_create(CLOCK_REALTIME, &event, &timer_) != OK) {
+ LOG(FATAL, "timer_create(CLOCK_REALTIME, %p, %p) failed with %d: %s\n",
+ &event, &timer_, errno, strerror(errno));
+ }
+}
+
+template<typename T>
+TimerNotifier<T>::~TimerNotifier() {
+ if (timer_delete(timer_) != OK) {
+ LOG(FATAL, "timer_delete(%p) failed with %d: %s\n", timer_,
+ errno, strerror(errno));
+ }
+ if (sigaction(signal_, &old_sa_, NULL) != OK) {
+ LOG(FATAL, "sigaction(%d, %p, NULL) failed with %d: %s\n",
+ signal_, &old_sa_, errno, strerror(errno));
+ }
+ StopNotifications();
+}
+
+template<typename T>
+void TimerNotifier<T>::StartNotifications(double period) {
+ itimerspec timer_spec;
+ timer_spec.it_value.tv_sec = 0;
+ timer_spec.it_value.tv_nsec = 1; // 0 would mean to disarm the timer
+ timer_spec.it_interval = time::Time::InSeconds(period).ToTimespec();
+ if (timer_settime(timer_, 0, &timer_spec, NULL) != OK) {
+ LOG(FATAL, "timer_settime(%p, 0, %p, NULL) failed with %d: %s\n",
+ timer_, &timer_spec, errno, strerror(errno));
+ }
+}
+
+template<typename T>
+void TimerNotifier<T>::StopNotifications() {
+ timer_cancel(timer_);
+}
+
+template<typename T>
+WDInterruptNotifier<T>::WDInterruptNotifier(
+ typename InterruptBridge<T>::Handler handler, T *param, int priority)
+ : PeriodicNotifier<T>(handler, param, priority), wd_(wdCreate()) {
+ if (wd_ == NULL) {
+ LOG(FATAL, "wdCreate() failed with %d: %s\n", errno, strerror(errno));
+ }
+}
+
+template<typename T>
+WDInterruptNotifier<T>::~WDInterruptNotifier() {
+ if (wdDelete(wd_) != OK) {
+ LOG(FATAL, "wdDelete(%p) failed with %d: %s\n",
+ wd_, errno, strerror(errno));
+ }
+ StopNotifications();
+}
+
+template<typename T>
+void WDInterruptNotifier<T>::StartNotifications(double period) {
+ delay_ = time::Time::InSeconds(period).ToTicks();
+
+ if (wdStart(wd_,
+ 1, // run it really soon
+ (FUNCPTR)StaticNotify,
+ (UINT32)this) != OK) {
+ LOG(FATAL, "wdStart(%p) failed with %d: %s", wd_, errno, strerror(errno));
+ }
+}
+
+template<typename T>
+void WDInterruptNotifier<T>::StopNotifications() {
+ wdCancel(wd_);
+}
+
+// THESE GET CALLED AT INTERRUPT LEVEL. BE CAREFUL CHANGING THEM!!
+//
+// It might be tempting to use floating point math here, but DON'T!
+//
+// Also, do NOT use 64-bit integers (at least not without checking the assembly
+// code again). GCC optimizes those using the floating point registers (see
+// <http://compgroups.net/comp.os.vxworks/int64-where-gcc-ppc-and-vxworks-don-t-play-we/1110156>
+// for an example of that being a problem).
+//
+// The assembly code comments are in there to make looking at assembly code
+// dumps to verify that there aren't any floating point instructions easier.
+// brians did that on 10/14/12 to his then-uncommited code and didn't find any.
+// For future reference, here is a list of powerpc instruction mnemonics (what
+// g++ -S gives) that do not involve any floating point:
+// lwz
+// lis
+// mflr
+// la
+// stwu
+// stw
+// mr
+// mtctr
+// bctrl
+// addi
+// mtlr
+// blr
+// cmpwi
+// bne
+// li
+// NOTE: This macro is used in interrupt_notifier-tmpl.h too.
+#define ASM_COMMENT(str) __asm__("# " str)
+template<typename T>
+void WDInterruptNotifier<T>::StaticNotify(void *self_in) {
+ ASM_COMMENT("beginning of WDInterruptNotifier::StaticNotify");
+ WDInterruptNotifier<T> *const self =
+ static_cast<WDInterruptNotifier<T> *>(self_in);
+ wdStart(self->wd_,
+ self->delay_,
+ (FUNCPTR)StaticNotify,
+ (UINT32)self); // call the same thing again
+ self->Notify();
+ ASM_COMMENT("end of WDInterruptNotifier::StaticNotify");
+}
+
+template<typename T>
+void TimerNotifier<T>::StaticNotify(int signum) {
+ ASM_COMMENT("beginning of TimerNotifier::StaticNotify");
+ TimerNotifier<T> *const self =
+ static_cast<TimerNotifier<T> *>(timer_notifiers[signum]);
+ self->Notify();
+ ASM_COMMENT("end of TimerNotifier::StaticNotify");
+}
+
+template<typename T>
+void InterruptBridge<T>::Notify() {
+ ASM_COMMENT("beginning of InterruptBridge::Notify");
+ semGive(sem_);
+ ASM_COMMENT("end of InterruptBridge::Notify");
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/shared_libs/interrupt_bridge.cc b/aos/crio/shared_libs/interrupt_bridge.cc
new file mode 100644
index 0000000..a905c55
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge.cc
@@ -0,0 +1,9 @@
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+
+namespace aos {
+namespace crio {
+
+void **const timer_notifiers = new void *[SIGRTMAX - SIGRTMIN];
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/shared_libs/interrupt_bridge.h b/aos/crio/shared_libs/interrupt_bridge.h
new file mode 100644
index 0000000..85ecd0f
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge.h
@@ -0,0 +1,140 @@
+#ifndef AOS_CRIO_SHARED_LIBS_INTERRUPT_BRIDGE_H_
+#define AOS_CRIO_SHARED_LIBS_INTERRUPT_BRIDGE_H_
+
+#include <wdLib.h>
+#include <semLib.h>
+#include <timers.h>
+#include <signal.h>
+
+#include "aos/common/scoped_ptr.h"
+
+#include "aos/aos_core.h"
+
+class Task;
+
+namespace aos {
+namespace crio {
+
+// Handles creating a separate Task at a high priority with a semaphore to
+// link it to something else that provides timing information (likely in an ISR
+// context).
+template<typename T>
+class InterruptBridge {
+ public:
+ // The default priority. Here as a constant for subclasses to use as a default
+ // too.
+ static const int kDefaultPriority = 96;
+ typedef void (*Handler)(T *param);
+
+ // Stops calling the handler at all until a Start function is called again.
+ void Stop();
+
+ protected:
+ // < 95 priority does not work, and < 100 isn't guaranteed to work
+ InterruptBridge(Handler handler, T *param, int priority);
+ // Subclasses should call StopNotifications.
+ virtual ~InterruptBridge();
+
+ // Subclasses should call this whenever the Notifier triggers.
+ // It is safe to call from an ISR.
+ void Notify();
+ // Starts the actual Task.
+ void StartTask();
+
+ private:
+ const Handler handler_;
+ T *const param_;
+
+ // Subclasses should do whatever they have to to stop calling Notify().
+ virtual void StopNotifications() = 0;
+
+ // The function that the Task runs.
+ // Loops forever, waiting for sem_ and then calling handler_.
+ static void HandlerLoop(void *self_in);
+ const scoped_ptr<Task> task_;
+ // For synchronizing between the Task and Notify().
+ SEM_ID sem_;
+ DISALLOW_COPY_AND_ASSIGN(InterruptBridge<T>);
+};
+
+// An accurate version of WPILib's Notifier class.
+template<typename T>
+class PeriodicNotifier : public InterruptBridge<T> {
+ public:
+ // Period is how much (in seconds) to wait between running the handler.
+ void StartPeriodic(double period);
+
+ protected:
+ PeriodicNotifier(typename InterruptBridge<T>::Handler handler, T *param,
+ int priority);
+ virtual ~PeriodicNotifier() {}
+
+ private:
+ virtual void StopNotifications() = 0;
+ // Subclasses should do whatever they have to to start calling Notify() every
+ // period seconds.
+ virtual void StartNotifications(double period) = 0;
+};
+
+// This one works accurately, but it has the potential to drift over time.
+// It has only sysClockRateGet() resolution.
+template<typename T>
+class WDInterruptNotifier : public PeriodicNotifier<T> {
+ public:
+ WDInterruptNotifier(typename InterruptBridge<T>::Handler handler,
+ T *param = NULL,
+ int priority = InterruptBridge<T>::kDefaultPriority);
+ virtual ~WDInterruptNotifier();
+
+ private:
+ // The argument is the general callback parameter which will be a pointer to
+ // an instance. This function calls Notify() on that instance.
+ static void StaticNotify(void *self_in);
+ virtual void StopNotifications();
+ virtual void StartNotifications(double period);
+
+ WDOG_ID wd_;
+ int delay_; // what to pass to wdStart
+ DISALLOW_COPY_AND_ASSIGN(WDInterruptNotifier<T>);
+};
+
+// Vxworks (really really) should take care of making sure that this one
+// doesn't drift over time, but IT DOESN'T!! Brian found the implementation
+// online (file timerLib.c), and it's just doing the same thing as
+// WDInterruptNotifier, except with extra conversions and overhead to implement
+// the POSIX standard. There really is no reason to use this.
+// It has only sysClockRateGet() resolution.
+template<typename T>
+class TimerNotifier : public PeriodicNotifier<T> {
+ public:
+ // unique should be different for all instances created in the same Task. It
+ // can range from 0 to (SIGRTMAX - SIGRTMIN). This instance will use the
+ // signal (SIGRTMIN + unique), so nothing else should use that signal.
+ TimerNotifier(typename InterruptBridge<T>::Handler handler,
+ T *param = NULL,
+ int unique = 0,
+ int priority = InterruptBridge<T>::kDefaultPriority);
+ virtual ~TimerNotifier();
+
+ private:
+ // The first argument is the signal number that is being triggered. This
+ // function looks up a pointer to an instance in timer_notifiers using that
+ // and calls Notify() on that instance.
+ static void StaticNotify(int signum);
+ virtual void StopNotifications();
+ virtual void StartNotifications(double period);
+
+ timer_t timer_;
+ // Which signal timer_ will notify on.
+ const int signal_;
+ // What the action for signal_ was before we started messing with it.
+ struct sigaction old_sa_;
+ DISALLOW_COPY_AND_ASSIGN(TimerNotifier<T>);
+};
+
+} // namespace crio
+} // namespace aos
+
+#include "aos/crio/shared_libs/interrupt_bridge-tmpl.h"
+
+#endif // AOS_CRIO_SHARED_LIBS_INTERRUPT_BRIDGE_H_
diff --git a/aos/crio/shared_libs/interrupt_bridge_c.c b/aos/crio/shared_libs/interrupt_bridge_c.c
new file mode 100644
index 0000000..7a74ea1
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge_c.c
@@ -0,0 +1,9 @@
+#include <vxWorksCommon.h> // or else it blows up...
+// This header ends up trying to #include a header which declares a struct with
+// a member named "delete". This means that it can not be used from code
+// compiled with the C++ compiler.
+#include <usrConfig.h>
+
+int aos_getExcExtendedVectors(void) {
+ return excExtendedVectors;
+}
diff --git a/aos/crio/shared_libs/interrupt_bridge_demo.cc b/aos/crio/shared_libs/interrupt_bridge_demo.cc
new file mode 100644
index 0000000..d7e66be
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge_demo.cc
@@ -0,0 +1,45 @@
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+
+#include <unistd.h>
+
+#include "WPILib/Timer.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/time.h"
+
+using aos::time::Time;
+
+namespace aos {
+namespace crio {
+
+class InterruptBridgeTest {
+ public:
+ void Run(double seconds) {
+ int interruptVal = 1;
+ WDInterruptNotifier<int> interrupt(Notify, &interruptVal);
+ interrupt.StartPeriodic(0.01);
+ int timerVal = 2;
+ TimerNotifier<int> timer(Notify, &timerVal);
+ time::SleepFor(Time::InSeconds(0.005)); // get them offset by ~180 degrees
+ timer.StartPeriodic(0.01);
+ time::SleepFor(Time::InSeconds(seconds));
+ }
+ private:
+ static void Notify(int *value) {
+ printf("notified %f,%d\n", Timer::GetFPGATimestamp(), *value);
+ }
+};
+
+} // namespace crio
+} // namespace aos
+
+// Designed to be called from the vxworks shell if somebody wants to run this.
+extern "C" int interrupt_bridge_demo(int seconds) {
+ if (seconds == 0) {
+ printf("arguments: number of seconds\n");
+ return -1;
+ }
+ aos::crio::InterruptBridgeTest runner;
+ runner.Run(seconds);
+ return 0;
+}
diff --git a/aos/crio/shared_libs/interrupt_notifier-tmpl.h b/aos/crio/shared_libs/interrupt_notifier-tmpl.h
new file mode 100644
index 0000000..78e7ca8
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_notifier-tmpl.h
@@ -0,0 +1,48 @@
+#include <intLib.h>
+#include <logLib.h>
+
+namespace aos {
+namespace crio {
+
+template<typename T>
+InterruptNotifier<T>::InterruptNotifier(
+ typename InterruptBridge<T>::Handler handler,
+ InterruptableSensorBase *sensor, T *param, int priority)
+ : InterruptBridge<T>(handler, param, priority), sensor_(sensor) {
+ sensor_->RequestInterrupts(StaticNotify, this);
+}
+
+template<typename T>
+InterruptNotifier<T>::~InterruptNotifier() {
+ sensor_->CancelInterrupts();
+}
+
+template<typename T>
+void InterruptNotifier<T>::Start() {
+ this->StartTask();
+ sensor_->EnableInterrupts();
+}
+
+template<typename T>
+void InterruptNotifier<T>::StopNotifications() {
+ sensor_->DisableInterrupts();
+}
+
+// WARNING: This IS called from an ISR. Don't use floating point. Look in
+// interrupt_bridge-tmpl.h for details.
+template<typename T>
+void InterruptNotifier<T>::StaticNotify(uint32_t, void *self_in) {
+ ASM_COMMENT("beginning of InterruptNotifier::StaticNotify");
+ if (!intContext()) { // if we're not in an actual ISR
+ logMsg(const_cast<char *>("WPILib is not calling callbacks"
+ " in actual ISRs any more!!\n"),
+ 0, 0, 0, 0, 0, 0);
+ }
+ InterruptNotifier<T> *const self =
+ static_cast<InterruptNotifier<T> *>(self_in);
+ self->Notify();
+ ASM_COMMENT("end of InterruptNotifier::StaticNotify");
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/shared_libs/interrupt_notifier.h b/aos/crio/shared_libs/interrupt_notifier.h
new file mode 100644
index 0000000..87aa4a3
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_notifier.h
@@ -0,0 +1,49 @@
+#ifndef AOS_CRIO_SHARED_LIBS_INTERRUPT_NOTIFIER_H_
+#define AOS_CRIO_SHARED_LIBS_INTERRUPT_NOTIFIER_H_
+
+#include "WPILib/InterruptableSensorBase.h"
+
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+
+namespace aos {
+namespace crio {
+
+// An InterruptBridge that notifies based on interrupts from a WPILib
+// InterruptableSensorBase object (which DigitalInput is an example of).
+template<typename T>
+class InterruptNotifier : public InterruptBridge<T> {
+ public:
+ // This object will hold a reference to sensor, but will not free it. This
+ // object will take ownership of everything related to interrupts for sensor
+ // (ie this constructor will call sensor->RequestInterrupts).
+ // Interrupts should be cancelled (the state InterruptableSensorBases are
+ // constructed in) when this constructor is called.
+ // Any setup of sensor that is required should happen before Start() is
+ // called, but after this constructor (ie SetUpSourceEdge).
+ InterruptNotifier(typename InterruptBridge<T>::Handler handler,
+ InterruptableSensorBase *sensor,
+ T *param = NULL,
+ int priority = InterruptBridge<T>::kDefaultPriority);
+ virtual ~InterruptNotifier();
+
+ // Starts calling the handler whenever the interrupt triggers.
+ void Start();
+
+ private:
+ // The only docs that seem to exist on the first arg is that it's named
+ // interruptAssertedMask in WPILib/ChipObject/tInterruptManager.h
+ // The second arg is the general callback parameter which will be a pointer to
+ // an instance. This function calls Notify() on that instance.
+ static void StaticNotify(uint32_t, void *self_in);
+ virtual void StopNotifications();
+
+ InterruptableSensorBase *const sensor_;
+ DISALLOW_COPY_AND_ASSIGN(InterruptNotifier<T>);
+};
+
+} // namespace crio
+} // namespace aos
+
+#include "aos/crio/shared_libs/interrupt_notifier-tmpl.h"
+
+#endif // AOS_CRIO_SHARED_LIBS_INTERRUPT_NOTIFIER_H_
diff --git a/aos/crio/shared_libs/mutex.cpp b/aos/crio/shared_libs/mutex.cpp
new file mode 100644
index 0000000..35ac4e3
--- /dev/null
+++ b/aos/crio/shared_libs/mutex.cpp
@@ -0,0 +1,48 @@
+#include "aos/common/mutex.h"
+
+#include <semLib.h>
+
+namespace aos {
+
+Mutex::Mutex() : impl_(semBCreate(SEM_Q_PRIORITY, SEM_FULL)) {
+ if (impl_ == NULL) {
+ LOG(FATAL,
+ "semBCreate(SEM_Q_PRIORITY, SEM_FULL) failed because of %d: %s\n",
+ errno, strerror(errno));
+ }
+}
+
+Mutex::~Mutex() {
+ if (semDelete(impl_) != 0) {
+ LOG(FATAL, "semDelete(%p) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+}
+
+void Mutex::Lock() {
+ if (semTake(impl_, WAIT_FOREVER) != 0) {
+ LOG(FATAL, "semTake(%p, WAIT_FOREVER) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+}
+
+void Mutex::Unlock() {
+ if (semGive(impl_) != 0) {
+ LOG(FATAL, "semGive(%p) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+}
+
+bool Mutex::TryLock() {
+ if (semTake(impl_, NO_WAIT) == 0) {
+ return true;
+ }
+ // The semLib documention is wrong about what the errno will be.
+ if (errno != S_objLib_OBJ_UNAVAILABLE) {
+ LOG(FATAL, "semTake(%p, WAIT_FOREVER) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+ return false;
+}
+
+} // namespace aos