Add AP_HAL_SMACCM implementation.

- Add a board definition for SMACCMPilot.
- Support the SMACCM HAL in required utility libraries.

libraries/AP_Common/c++.cpp

@@ -9,7 +9,7 @@
 #include <AP_HAL.h>
 #include <stdlib.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
 
 void * operator new(size_t size)
 {

libraries/AP_HAL/AP_HAL_Boards.h

@@ -13,6 +13,7 @@
 #define HAL_BOARD_APM1     1
 #define HAL_BOARD_APM2     2
 #define HAL_BOARD_AVR_SITL 3
+#define HAL_BOARD_SMACCM   4
 #define HAL_BOARD_EMPTY    99
 
 /*
@@ -29,6 +30,8 @@
 #define AP_HAL_BOARD_DRIVER AP_HAL_AVR_SITL
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
 #define AP_HAL_BOARD_DRIVER AP_HAL_PX4
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
+#define AP_HAL_BOARD_DRIVER AP_HAL_SMACCM
 #elif CONFIG_HAL_BOARD == HAL_BOARD_EMPTY
 #define AP_HAL_BOARD_DRIVER AP_HAL_Empty
 #else

libraries/AP_HAL_SMACCM/AP_HAL_SMACCM.h

@@ -0,0 +1,33 @@
+
+#ifndef __AP_HAL_SMACCM_H__
+#define __AP_HAL_SMACCM_H__
+
+/* Your layer exports should depend on AP_HAL.h ONLY. */
+#include <AP_HAL.h>
+
+/**
+ * Umbrella header for AP_HAL_SMACCM module.
+ * The module header exports singleton instances which must conform the
+ * AP_HAL::HAL interface. It may only expose implementation details (class
+ * names, headers) via the SMACCM namespace.
+ * The class implementing AP_HAL::HAL should be called HAL_SMACCM and exist
+ * in the global namespace. There should be a single const instance of the
+ * HAL_SMACCM class called AP_HAL_SMACCM, instantiated in the HAL_SMACCM_Class.cpp
+ * and exported as `extern const HAL_SMACCM AP_HAL_SMACCM;` in HAL_SMACCM_Class.h
+ *
+ * All declaration and compilation should be guarded by CONFIG_HAL_BOARD macros.
+ * In this case, we're using CONFIG_HAL_BOARD == HAL_BOARD_SMACCM.
+ * When creating a new HAL, declare a new HAL_BOARD_ in AP_HAL/AP_HAL_Boards.h
+ *
+ * The module should also export an appropriate AP_HAL_MAIN() macro iff the
+ * appropriate CONFIG_HAL_BOARD value is set.
+ * The AP_HAL_MAIN macro expands to a main function (either an `int main (void)`
+ * or `int main (int argc, const char * argv[]), depending on platform) of an
+ * ArduPilot application, whose entry points are the c++ functions
+ * `void setup()` and `void loop()`, ala Arduino.
+ */
+
+#include "HAL_SMACCM_Class.h"
+#include "AP_HAL_SMACCM_Main.h"
+
+#endif //__AP_HAL_SMACCM_H__

libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Main.h

@@ -0,0 +1,17 @@
+
+
+#ifndef __AP_HAL_SMACCM_MAIN_H__
+#define __AP_HAL_SMACCM_MAIN_H__
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
+#define AP_HAL_MAIN() extern "C" {\
+    int main (void) {\
+	hal.init(0, NULL);			\
+        setup();\
+        for(;;) loop();\
+        return 0;\
+    }\
+    }
+#endif // HAL_BOARD_SMACCM
+
+#endif // __AP_HAL_SMACCM_MAIN_H__

libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Namespace.h

@@ -0,0 +1,30 @@
+
+#ifndef __AP_HAL_SMACCM_NAMESPACE_H__
+#define __AP_HAL_SMACCM_NAMESPACE_H__
+
+/* While not strictly required, names inside the SMACCM namespace are prefixed
+ * with SMACCM for clarity. (Some of our users aren't familiar with all of the
+ * C++ namespace rules.)
+ */
+
+namespace SMACCM {
+    class SMACCMUARTDriver;
+    class SMACCMI2CDriver;
+    class SMACCMSPIDeviceManager;
+    class SMACCMSPIDeviceDriver;
+    class SMACCMAnalogSource;
+    class SMACCMAnalogIn;
+    class SMACCMStorage;
+    class SMACCMConsoleDriver;
+    class SMACCMGPIO;
+    class SMACCMDigitalSource;
+    class SMACCMRCInput;
+    class SMACCMRCOutput;
+    class SMACCMSemaphore;
+    class SMACCMScheduler;
+    class SMACCMUtil;
+    class SMACCMPrivateMember;
+}
+
+#endif // __AP_HAL_SMACCM_NAMESPACE_H__
+

libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Private.h

@@ -0,0 +1,24 @@
+
+#ifndef __AP_HAL_SMACCM_PRIVATE_H__
+#define __AP_HAL_SMACCM_PRIVATE_H__
+
+/* Umbrella header for all private headers of the AP_HAL_SMACCM module.
+ * Only import this header from inside AP_HAL_SMACCM
+ */
+
+#include "UARTDriver.h"
+#include "I2CDriver.h"
+#include "SPIDriver.h"
+#include "AnalogIn.h"
+#include "Storage.h"
+#include "Console.h"
+#include "GPIO.h"
+#include "RCInput.h"
+#include "RCOutput.h"
+#include "Semaphore.h"
+#include "Scheduler.h"
+#include "Util.h"
+#include "PrivateMember.h"
+
+#endif // __AP_HAL_SMACCM_PRIVATE_H__
+

libraries/AP_HAL_SMACCM/AnalogIn.cpp

@@ -0,0 +1,35 @@
+
+#include "AnalogIn.h"
+
+using namespace SMACCM;
+
+SMACCMAnalogSource::SMACCMAnalogSource(float v) :
+    _v(v)
+{}
+
+float SMACCMAnalogSource::read_average() {
+    return _v;
+}
+
+float SMACCMAnalogSource::read_latest() {
+    return _v;
+}
+
+void SMACCMAnalogSource::set_pin(uint8_t p)
+{}
+
+
+SMACCMAnalogIn::SMACCMAnalogIn()
+{}
+
+void SMACCMAnalogIn::init(void* machtnichts)
+{}
+
+AP_HAL::AnalogSource* SMACCMAnalogIn::channel(int16_t n) {
+    return new SMACCMAnalogSource(1.11);
+}
+
+AP_HAL::AnalogSource* SMACCMAnalogIn::channel(int16_t n, float scale) {
+    return new SMACCMAnalogSource(scale/2);
+}
+

libraries/AP_HAL_SMACCM/AnalogIn.h

@@ -0,0 +1,24 @@
+
+#ifndef __AP_HAL_SMACCM_ANALOGIN_H__
+#define __AP_HAL_SMACCM_ANALOGIN_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMAnalogSource : public AP_HAL::AnalogSource {
+public:
+    SMACCMAnalogSource(float v);
+    float read_average();
+    float read_latest();
+    void set_pin(uint8_t p);
+private:
+    float _v;
+};
+
+class SMACCM::SMACCMAnalogIn : public AP_HAL::AnalogIn {
+public:
+    SMACCMAnalogIn();
+    void init(void* implspecific);
+    AP_HAL::AnalogSource* channel(int16_t n);
+    AP_HAL::AnalogSource* channel(int16_t n, float scale);
+};
+#endif // __AP_HAL_SMACCM_ANALOGIN_H__

libraries/AP_HAL_SMACCM/Console.cpp

@@ -0,0 +1,212 @@
+/*
+ * Console.cpp --- AP_HAL_SMACCM console driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ */
+
+#include <stdarg.h>
+#include <stdlib.h>
+
+#include "Console.h"
+
+using namespace SMACCM;
+
+SMACCMConsoleDriver::SMACCMConsoleDriver(AP_HAL::BetterStream* delegate) :
+  _d(delegate), _user_backend(false)
+{
+}
+
+void SMACCMConsoleDriver::init(void *args)
+{
+}
+
+void SMACCMConsoleDriver::backend_open()
+{
+  _txbuf.allocate(128);
+  _rxbuf.allocate(16);
+  _user_backend = true;
+}
+
+void SMACCMConsoleDriver::backend_close()
+{
+  _user_backend = false;
+}
+
+size_t SMACCMConsoleDriver::backend_read(uint8_t *data, size_t len)
+{
+  for (size_t i = 0; i < len; i++) {
+    int16_t b = _txbuf.pop();
+    if (b != -1) {
+      data[i] = (uint8_t) b;
+    } else {
+      return i;
+    }
+  }
+
+  return len;
+}
+
+size_t SMACCMConsoleDriver::backend_write(const uint8_t *data, size_t len)
+{
+   for (size_t i = 0; i < len; i++) {
+     bool valid = _rxbuf.push(data[i]);
+     if (!valid) {
+       return i;
+     }
+   }
+
+   return len;
+ }
+
+void SMACCMConsoleDriver::print_P(const prog_char_t *pstr)
+{
+  _d->print_P(pstr);
+}
+
+void SMACCMConsoleDriver::println_P(const prog_char_t *pstr)
+{
+  _d->println_P(pstr);
+}
+
+void SMACCMConsoleDriver::printf(const char *pstr, ...)
+{
+  va_list ap;
+  va_start(ap, pstr);
+  _d->vprintf(pstr, ap);
+  va_end(ap);
+}
+
+void SMACCMConsoleDriver::_printf_P(const prog_char *pstr, ...)
+{
+  va_list ap;
+  va_start(ap, pstr);
+  _d->vprintf_P(pstr, ap);
+  va_end(ap);
+}
+
+void SMACCMConsoleDriver::vprintf(const char *pstr, va_list ap)
+{
+  _d->vprintf(pstr, ap);
+}
+
+void SMACCMConsoleDriver::vprintf_P(const prog_char *pstr, va_list ap)
+{
+  _d->vprintf(pstr, ap);
+}
+
+int16_t SMACCMConsoleDriver::available()
+{
+  if (_user_backend) {
+    return _rxbuf.bytes_used();
+  } else {
+    return _d->available();
+  }
+}
+
+int16_t SMACCMConsoleDriver::txspace()
+{
+  if (_user_backend) {
+    return _txbuf.bytes_free();
+  } else {
+    return _d->txspace();
+  }
+}
+
+int16_t SMACCMConsoleDriver::read()
+{
+  if (_user_backend) {
+    return _rxbuf.pop();
+  } else {
+    return _d->read();
+  }
+}
+
+int16_t SMACCMConsoleDriver::peek()
+{
+  if (_user_backend) {
+    return _rxbuf.peek();
+  } else {
+    return _d->peek();
+  }
+}
+
+size_t SMACCMConsoleDriver::write(uint8_t c)
+{
+  if (_user_backend) {
+    return (_txbuf.push(c) ? 1 : 0);
+  } else {
+    return _d->write(c);
+  }
+}
+
+/**
+ * SMACCMConsoleDriver::Buffer implementation.
+ * A synchronous nonblocking ring buffer, based on the AVRUARTDriver::Buffer
+ */
+
+bool SMACCMConsoleDriver::Buffer::allocate(uint16_t size)
+{
+  _head = 0;
+  _tail = 0;
+  uint8_t shift;
+  /* Hardcoded max size of 1024. sue me. */
+  for ( shift = 1;
+        ( 1 << shift ) < 1024 && ( 1 << shift) < size;
+        shift++
+    ) ;
+  uint16_t tmpmask  = (1 << shift) - 1;
+
+  if ( _bytes != NULL ) {
+    if ( _mask == tmpmask ) {
+      return true;
+    }
+    free(_bytes);
+  }
+  _mask = tmpmask;
+  _bytes = (uint8_t*) malloc(_mask+1);
+  return (_bytes != NULL);
+}
+
+bool SMACCMConsoleDriver::Buffer::push(uint8_t b)
+{
+  uint16_t next = (_head + 1) & _mask;
+  if ( next == _tail ) {
+    return false;
+  }
+  _bytes[_head] = b;
+  _head = next;
+  return true;
+}
+
+int16_t SMACCMConsoleDriver::Buffer::pop()
+{
+  if ( _tail == _head ) {
+    return -1;
+  }
+  uint8_t b = _bytes[_tail];
+  _tail = ( _tail + 1 ) & _mask;
+  return (int16_t) b;
+}
+
+int16_t SMACCMConsoleDriver::Buffer::peek()
+{
+  if ( _tail == _head ) {
+    return -1;
+  }
+  uint8_t b = _bytes[_tail];
+  return (int16_t) b;
+}
+
+uint16_t SMACCMConsoleDriver::Buffer::bytes_used()
+{
+  return ((_head - _tail) & _mask);
+}
+
+uint16_t SMACCMConsoleDriver::Buffer::bytes_free()
+{
+  return ((_mask+1) - ((_head - _tail) & _mask));
+}

libraries/AP_HAL_SMACCM/Console.h

@@ -0,0 +1,62 @@
+/*
+ * Console.h --- AP_HAL_SMACCM console driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ */
+
+#ifndef __AP_HAL_SMACCM_CONSOLE_H__
+#define __AP_HAL_SMACCM_CONSOLE_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMConsoleDriver : public AP_HAL::ConsoleDriver {
+public:
+    SMACCMConsoleDriver(AP_HAL::BetterStream* delegate);
+    void init(void *arg);
+    void backend_open();
+    void backend_close();
+    size_t backend_read(uint8_t *data, size_t len);
+    size_t backend_write(const uint8_t *data, size_t len);
+
+    void print_P(const prog_char_t *pstr);
+    void println_P(const prog_char_t *pstr);
+    void printf(const char *pstr, ...);
+    void _printf_P(const prog_char *pstr, ...);
+    void vprintf(const char *pstr, va_list ap);
+    void vprintf_P(const prog_char *pstr, va_list ap);
+
+    int16_t available();
+    int16_t txspace();
+    int16_t read();
+    int16_t peek();
+
+    size_t write(uint8_t c);
+private:
+    AP_HAL::BetterStream *_d;
+
+    // Buffer implementation copied from the AVR HAL.
+    struct Buffer {
+        /* public methods:*/
+        bool allocate(uint16_t size);
+        bool push(uint8_t b);
+        int16_t  pop();
+        int16_t  peek();
+
+        uint16_t bytes_free();
+        uint16_t bytes_used();
+    private:
+        uint16_t _head, _tail; /* Head and tail indicies */
+        uint16_t _mask;       /* Buffer size mask for index wrap */
+        uint8_t *_bytes;      /* Pointer to allocated buffer */
+    };
+
+    Buffer _txbuf;
+    Buffer _rxbuf;
+    bool _user_backend;
+};
+
+#endif // __AP_HAL_SMACCM_CONSOLE_H__

libraries/AP_HAL_SMACCM/GPIO.cpp

@@ -0,0 +1,57 @@
+
+#include "GPIO.h"
+
+using namespace SMACCM;
+
+SMACCMGPIO::SMACCMGPIO()
+{
+}
+
+void SMACCMGPIO::init()
+{
+}
+
+void SMACCMGPIO::pinMode(uint8_t pin, uint8_t output)
+{
+}
+
+uint8_t SMACCMGPIO::read(uint8_t pin)
+{
+  return 0;
+}
+
+void SMACCMGPIO::write(uint8_t pin, uint8_t value)
+{
+}
+
+/* Alternative interface: */
+AP_HAL::DigitalSource* SMACCMGPIO::channel(uint16_t n)
+{
+  return NULL;
+}
+
+/* Interrupt interface: */
+bool SMACCMGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
+                                  uint8_t mode)
+{
+  return true;
+}
+
+SMACCMDigitalSource::SMACCMDigitalSource(uint8_t v) :
+  _v(v)
+{
+}
+
+void SMACCMDigitalSource::mode(uint8_t output)
+{
+}
+
+uint8_t SMACCMDigitalSource::read()
+{
+  return _v;
+}
+
+void SMACCMDigitalSource::write(uint8_t value)
+{
+  _v = value;
+}

libraries/AP_HAL_SMACCM/GPIO.h

@@ -0,0 +1,34 @@
+
+#ifndef __AP_HAL_SMACCM_GPIO_H__
+#define __AP_HAL_SMACCM_GPIO_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMGPIO : public AP_HAL::GPIO {
+public:
+    SMACCMGPIO();
+    void    init();
+    void    pinMode(uint8_t pin, uint8_t output);
+    uint8_t read(uint8_t pin);
+    void    write(uint8_t pin, uint8_t value);
+
+    /* Alternative interface: */
+    AP_HAL::DigitalSource* channel(uint16_t n);
+
+    /* Interrupt interface: */
+    bool    attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
+            uint8_t mode);
+
+};
+
+class SMACCM::SMACCMDigitalSource : public AP_HAL::DigitalSource {
+public:
+    SMACCMDigitalSource(uint8_t v);
+    void    mode(uint8_t output);
+    uint8_t read();
+    void    write(uint8_t value); 
+private:
+    uint8_t _v;
+};
+
+#endif // __AP_HAL_SMACCM_GPIO_H__

libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp

@@ -0,0 +1,57 @@
+
+#include <AP_HAL.h>
+
+#include "HAL_SMACCM_Class.h"
+#include "AP_HAL_SMACCM_Private.h"
+
+using namespace SMACCM;
+
+// XXX make sure these are assigned correctly
+static SMACCMUARTDriver uartADriver(usart1);
+static SMACCMUARTDriver uartBDriver(usart2);
+static SMACCMUARTDriver uartCDriver(usart3);
+
+static SMACCMI2CDriver  i2cDriver;
+static SMACCMSPIDeviceManager spiDeviceManager;
+static SMACCMAnalogIn analogIn;
+static SMACCMStorage storageDriver;
+static SMACCMConsoleDriver consoleDriver(&uartADriver);
+static SMACCMGPIO gpioDriver;
+static SMACCMRCInput rcinDriver;
+static SMACCMRCOutput rcoutDriver;
+static SMACCMScheduler schedulerInstance;
+static SMACCMUtil utilInstance;
+
+HAL_SMACCM::HAL_SMACCM() :
+    AP_HAL::HAL(
+      &uartADriver,
+      &uartBDriver,
+      &uartCDriver,
+      &i2cDriver,
+      &spiDeviceManager,
+      &analogIn,
+      &storageDriver,
+      &consoleDriver,
+      &gpioDriver,
+      &rcinDriver,
+      &rcoutDriver,
+      &schedulerInstance,
+      &utilInstance)
+{
+}
+
+void HAL_SMACCM::init(int argc,char* const argv[]) const
+{
+  /* initialize all drivers and private members here.
+   * up to the programmer to do this in the correct order.
+   * Scheduler should likely come first. */
+  scheduler->init(NULL);
+  uartA->begin(115200);
+  console->init(uartA);
+  i2c->begin();
+  spi->init(NULL);
+  storage->init(NULL);
+}
+
+const HAL_SMACCM AP_HAL_SMACCM;
+

libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.h

@@ -0,0 +1,19 @@
+
+#ifndef __AP_HAL_SMACCM_CLASS_H__
+#define __AP_HAL_SMACCM_CLASS_H__
+
+#include <AP_HAL.h>
+
+#include "AP_HAL_SMACCM_Namespace.h"
+#include "PrivateMember.h"
+
+class HAL_SMACCM : public AP_HAL::HAL {
+public:
+    HAL_SMACCM();
+    void init(int argc, char * const * argv) const;
+};
+
+extern const HAL_SMACCM AP_HAL_SMACCM;
+
+#endif // __AP_HAL_SMACCM_CLASS_H__
+

libraries/AP_HAL_SMACCM/I2CDriver.cpp

@@ -0,0 +1,84 @@
+/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/*
+ * I2CDriver.cpp --- AP_HAL_SMACCM I2C driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ *
+ * Written by James Bielman <jamesjb@galois.com>, 20 December 2012
+ */
+
+#include <AP_HAL.h>
+#include <hwf4/i2c.h>
+#include <hwf4/gpio.h>
+
+#include "I2CDriver.h"
+
+using namespace SMACCM;
+
+// For now, we are assuming all devices are on a single bus.  This
+// will need to be refactored at some point to use a device manager
+// like SPI if we need support for multiple busses.
+#define I2C_BUS i2c2
+#define I2C_SDA pin_b11
+#define I2C_SCL pin_b10
+
+void SMACCMI2CDriver::begin()
+{
+  i2c_init(I2C_BUS, I2C_SDA, I2C_SCL);
+}
+
+// XXX hwf4 doesn't support de-initialization
+void SMACCMI2CDriver::end()
+{
+}
+
+// XXX hwf4 doesn't support non-blocking I2C
+void SMACCMI2CDriver::setTimeout(uint16_t ms)
+{
+}
+
+// XXX hwf4 always uses standard speed
+void SMACCMI2CDriver::setHighSpeed(bool active)
+{
+}
+
+uint8_t SMACCMI2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data)
+{
+  return i2c_transfer(I2C_BUS, addr, data, len, NULL, 0) ? 0 : 1;
+}
+
+uint8_t SMACCMI2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
+{
+  return i2c_write_reg(I2C_BUS, addr, reg, val) ? 0 : 1;
+}
+
+uint8_t SMACCMI2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
+                                        uint8_t len, uint8_t* data)
+{
+  return i2c_write_regs(I2C_BUS, addr, reg, data, len) ? 0 : 1;
+}
+
+uint8_t SMACCMI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
+{
+  return i2c_transfer(I2C_BUS, addr, NULL, 0, data, len) ? 0 : 1;
+}
+
+uint8_t SMACCMI2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data)
+{
+  return i2c_transfer(I2C_BUS, addr, &reg, 1, data, 1) ? 0 : 1;
+}
+
+uint8_t SMACCMI2CDriver::readRegisters(uint8_t addr, uint8_t reg,
+                                       uint8_t len, uint8_t* data)
+{
+  return i2c_transfer(I2C_BUS, addr, &reg, 1, data, len) ? 0 : 1;
+}
+
+uint8_t SMACCMI2CDriver::lockup_count()
+{
+  return 0;
+}

libraries/AP_HAL_SMACCM/I2CDriver.h

@@ -0,0 +1,48 @@
+/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/*
+ * I2CDriver.h --- AP_HAL_SMACCM I2C driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ *
+ * Written by James Bielman <jamesjb@galois.com>, 20 December 2012
+ */
+
+#ifndef __AP_HAL_SMACCM_I2CDRIVER_H__
+#define __AP_HAL_SMACCM_I2CDRIVER_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMI2CDriver : public AP_HAL::I2CDriver {
+public:
+    void begin();
+    void end();
+    void setTimeout(uint16_t ms);
+    void setHighSpeed(bool active);
+
+    /* write: for i2c devices which do not obey register conventions */
+    uint8_t write(uint8_t addr, uint8_t len, uint8_t* data);
+    /* writeRegister: write a single 8-bit value to a register */
+    uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val);
+    /* writeRegisters: write bytes to contigious registers */
+    uint8_t writeRegisters(uint8_t addr, uint8_t reg,
+                                   uint8_t len, uint8_t* data);
+
+    /* read: for i2c devices which do not obey register conventions */
+    uint8_t read(uint8_t addr, uint8_t len, uint8_t* data);
+    /* readRegister: read from a device register - writes the register,
+     * then reads back an 8-bit value. */
+    uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data);
+    /* readRegister: read contigious device registers - writes the first 
+     * register, then reads back multiple bytes */
+    uint8_t readRegisters(uint8_t addr, uint8_t reg,
+                                  uint8_t len, uint8_t* data);
+
+    uint8_t lockup_count();
+
+};
+
+#endif // __AP_HAL_SMACCM_I2CDRIVER_H__

libraries/AP_HAL_SMACCM/PrivateMember.cpp

@@ -0,0 +1,11 @@
+
+#include "PrivateMember.h"
+
+using namespace SMACCM;
+
+SMACCMPrivateMember::SMACCMPrivateMember(uint16_t foo) :
+    _foo(foo)
+{}
+
+void SMACCMPrivateMember::init() {}
+

libraries/AP_HAL_SMACCM/PrivateMember.h

@@ -0,0 +1,19 @@
+
+#ifndef __AP_HAL_SMACCM_PRIVATE_MEMBER_H__
+#define __AP_HAL_SMACCM_PRIVATE_MEMBER_H__
+
+/* Just a stub as an example of how to implement a private member of an
+ * AP_HAL module */
+
+#include "AP_HAL_SMACCM.h"
+
+class SMACCM::SMACCMPrivateMember {
+public:
+    SMACCMPrivateMember(uint16_t foo);
+    void init();
+private:
+    uint16_t _foo;
+};
+
+#endif // __AP_HAL_SMACCM_PRIVATE_MEMBER_H__
+

libraries/AP_HAL_SMACCM/RCInput.cpp

@@ -0,0 +1,38 @@
+
+#include "RCInput.h"
+
+using namespace SMACCM;
+SMACCMRCInput::SMACCMRCInput()
+{}
+
+void SMACCMRCInput::init(void* machtnichts)
+{}
+
+uint8_t SMACCMRCInput::valid() {
+    return 0;
+}
+
+uint16_t SMACCMRCInput::read(uint8_t ch) {
+    if (ch == 3) return 900; /* throttle should be low, for safety */
+    else return 1500;
+}
+
+uint8_t SMACCMRCInput::read(uint16_t* periods, uint8_t len) {
+    for (uint8_t i = 0; i < len; i++){
+        if (i == 3) periods[i] = 900;
+        else periods[i] = 1500;
+    }
+    return len;
+}
+
+bool SMACCMRCInput::set_overrides(int16_t *overrides, uint8_t len) {
+    return true;
+}
+
+bool SMACCMRCInput::set_override(uint8_t channel, int16_t override) {
+    return true;
+}
+
+void SMACCMRCInput::clear_overrides()
+{}
+

libraries/AP_HAL_SMACCM/RCInput.h

@@ -0,0 +1,20 @@
+
+#ifndef __AP_HAL_SMACCM_RCINPUT_H__
+#define __AP_HAL_SMACCM_RCINPUT_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMRCInput : public AP_HAL::RCInput {
+public:
+    SMACCMRCInput();
+    void init(void* machtnichts);
+    uint8_t  valid();
+    uint16_t read(uint8_t ch);
+    uint8_t read(uint16_t* periods, uint8_t len);
+
+    bool set_overrides(int16_t *overrides, uint8_t len);
+    bool set_override(uint8_t channel, int16_t override);
+    void clear_overrides();
+};
+
+#endif // __AP_HAL_SMACCM_RCINPUT_H__

libraries/AP_HAL_SMACCM/RCOutput.cpp

@@ -0,0 +1,38 @@
+
+#include "RCOutput.h"
+
+using namespace SMACCM;
+
+void SMACCMRCOutput::init(void* machtnichts) {}
+
+void SMACCMRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {}
+
+uint16_t SMACCMRCOutput::get_freq(uint8_t ch) {
+    return 50;
+}
+
+void SMACCMRCOutput::enable_ch(uint8_t ch)
+{}
+
+void SMACCMRCOutput::enable_mask(uint32_t chmask)
+{}
+
+void SMACCMRCOutput::disable_ch(uint8_t ch)
+{}
+
+void SMACCMRCOutput::disable_mask(uint32_t chmask)
+{}
+
+void SMACCMRCOutput::write(uint8_t ch, uint16_t period_us)
+{}
+
+void SMACCMRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
+{}
+
+uint16_t SMACCMRCOutput::read(uint8_t ch) {
+    return 900;
+}
+
+void SMACCMRCOutput::read(uint16_t* period_us, uint8_t len)
+{}
+

libraries/AP_HAL_SMACCM/RCOutput.h

@@ -0,0 +1,21 @@
+
+#ifndef __AP_HAL_SMACCM_RCOUTPUT_H__
+#define __AP_HAL_SMACCM_RCOUTPUT_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMRCOutput : public AP_HAL::RCOutput {
+    void     init(void* machtnichts);
+    void     set_freq(uint32_t chmask, uint16_t freq_hz);
+    uint16_t get_freq(uint8_t ch);
+    void     enable_ch(uint8_t ch);
+    void     enable_mask(uint32_t chmask);
+    void     disable_ch(uint8_t ch);
+    void     disable_mask(uint32_t chmask);
+    void     write(uint8_t ch, uint16_t period_us);
+    void     write(uint8_t ch, uint16_t* period_us, uint8_t len);
+    uint16_t read(uint8_t ch);
+    void     read(uint16_t* period_us, uint8_t len);
+};
+
+#endif // __AP_HAL_SMACCM_RCOUTPUT_H__

libraries/AP_HAL_SMACCM/SPIDriver.cpp

@@ -0,0 +1,102 @@
+/*
+ * SPIDriver.cpp --- AP_HAL_SMACCM SPI driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ */
+
+#include "SPIDriver.h"
+#include <FreeRTOS.h>
+#include <hwf4/spi.h>
+
+using namespace SMACCM;
+
+//////////////////////////////////////////////////////////////////////
+// SPI Device Driver
+
+SMACCMSPIDeviceDriver::SMACCMSPIDeviceDriver(spi_bus *bus, spi_device *device)
+  : _bus(bus), _device(device)
+{
+}
+
+void SMACCMSPIDeviceDriver::init()
+{
+}
+
+AP_HAL::Semaphore* SMACCMSPIDeviceDriver::get_semaphore()
+{
+  return &_semaphore;
+}
+
+void SMACCMSPIDeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t len)
+{
+  spi_transfer(_bus, _device, portMAX_DELAY, tx, rx, len);
+}
+
+// XXX these methods are not implemented
+void SMACCMSPIDeviceDriver::cs_assert()
+{
+}
+
+void SMACCMSPIDeviceDriver::cs_release()
+{
+}
+
+uint8_t SMACCMSPIDeviceDriver::transfer (uint8_t data)
+{
+  return 0;
+}
+
+//////////////////////////////////////////////////////////////////////
+// SPI Device Instances
+
+// SPIDevice_Dataflash (I2C device in PX4FMU)
+
+// SPIDevice_ADS7844 (not present in PX4FMU)
+
+// SPIDevice_MS5611 (I2C device in PX4FMU)
+
+// SPIDevice_MPU6000 (on SPI1)
+static spi_device g_mpu6000_spi_dev = {
+  pin_b0,                       // chip_select
+  false,                        // chip_select_active
+  SPI_BAUD_DIV_128,             // baud XXX check frequency
+  SPI_CLOCK_POLARITY_LOW,       // clock_polarity
+  SPI_CLOCK_PHASE_1,            // clock_phase
+  SPI_BIT_ORDER_MSB_FIRST       // bit_order
+};
+
+static SMACCMSPIDeviceDriver g_mpu6000_dev(spi1, &g_mpu6000_spi_dev);
+
+// SPIDevice_ADNS3080_SPI0 (not present in PX4FMU)
+
+// SPIDevice_ADNS3080_SPI3 (not present in PX4FMU)
+
+//////////////////////////////////////////////////////////////////////
+// SPI Device Manager
+
+SMACCMSPIDeviceManager::SMACCMSPIDeviceManager()
+{
+}
+
+// Initialize all SPI busses and devices.
+void SMACCMSPIDeviceManager::init(void *)
+{
+  spi_init(spi1);
+
+  spi_device_init(&g_mpu6000_spi_dev);
+}
+
+AP_HAL::SPIDeviceDriver* SMACCMSPIDeviceManager::device(AP_HAL::SPIDevice dev)
+{
+  switch (dev) {
+    case AP_HAL::SPIDevice_MPU6000:
+      return &g_mpu6000_dev;
+
+    default:
+      return NULL;
+  }
+}

libraries/AP_HAL_SMACCM/SPIDriver.h

@@ -0,0 +1,42 @@
+/*
+ * SPIDriver.h --- AP_HAL_SMACCM SPI driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ */
+
+#ifndef __AP_HAL_SMACCM_SPIDRIVER_H__
+#define __AP_HAL_SMACCM_SPIDRIVER_H__
+
+#include <AP_HAL_SMACCM.h>
+#include "Semaphore.h"
+
+#include <hwf4/spi.h>
+
+class SMACCM::SMACCMSPIDeviceDriver : public AP_HAL::SPIDeviceDriver {
+public:
+    SMACCMSPIDeviceDriver(spi_bus *bus, spi_device *device);
+    void init();
+    AP_HAL::Semaphore* get_semaphore();
+    void transaction(const uint8_t *tx, uint8_t *rx, uint16_t len);
+
+    void cs_assert();
+    void cs_release();
+    uint8_t transfer (uint8_t data);
+private:
+    SMACCMSemaphore _semaphore;
+    struct spi_bus *_bus;
+    struct spi_device *_device;
+};
+
+class SMACCM::SMACCMSPIDeviceManager : public AP_HAL::SPIDeviceManager {
+public:
+    SMACCMSPIDeviceManager();
+    void init(void *);
+    AP_HAL::SPIDeviceDriver* device(AP_HAL::SPIDevice);
+};
+
+#endif // __AP_HAL_SMACCM_SPIDRIVER_H__

libraries/AP_HAL_SMACCM/Scheduler.cpp

@@ -0,0 +1,281 @@
+/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/*
+ * Scheduler.cpp --- AP_HAL_SMACCM scheduler.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ *
+ * Written by James Bielman <jamesjb@galois.com>, 20 December 2012
+ */
+
+#include <hwf4/gpio.h>
+#include <hwf4/timer.h>
+
+#include <FreeRTOS.h>
+#include <task.h>
+#include <semphr.h>
+
+#include "Scheduler.h"
+
+using namespace SMACCM;
+
+extern const AP_HAL::HAL& hal;
+
+/** Rate in milliseconds of timed process execution. (1kHz) */
+#define SCHEDULER_TICKS  (1 / (portTICK_RATE_MS))
+
+/** Stack size of the scheduler thread. */
+#define SCHEDULER_STACK_SIZE 256
+
+/** Priority of the scheduler timer process task. */
+#define SCHEDULER_PRIORITY (configMAX_PRIORITIES - 1)
+
+/** Rate in milliseconds of the delay callback task. */
+#define DELAY_CB_TICKS (1 / (portTICK_RATE_MS))
+
+/** Stack size of the delay callback task. */
+#define DELAY_CB_STACK_SIZE 64
+
+/** Priority of the delay callback task. */
+#define DELAY_CB_PRIORITY 0
+
+/**
+ * Recursive mutex used to block "scheduler_task" during atomic
+ * sections.
+ */
+static xSemaphoreHandle g_atomic;
+
+/** High-priority thread managing timer procedures. */
+static void scheduler_task(void *arg)
+{
+  SMACCMScheduler *sched = (SMACCMScheduler *)arg;
+  portTickType last_wake_time;
+  portTickType now;
+
+  last_wake_time = xTaskGetTickCount();
+
+  for (;;) {
+    /* If "vTaskDelayUntil" would return immediately without blocking,
+     * call the failsafe callback to notify the client that we've
+     * missed our deadline, and reset the wakeup time to the current
+     * time. */
+    now = xTaskGetTickCount();
+    if (last_wake_time + SCHEDULER_TICKS <= now) {
+      hal.console->printf("now %lu  last %lu\r\n", last_wake_time, now);
+      sched->run_failsafe_cb();
+      last_wake_time = now;
+    } else {
+      vTaskDelayUntil(&last_wake_time, SCHEDULER_TICKS);
+
+      if (!xSemaphoreTakeRecursive(g_atomic, 5)) {
+        asm volatile ("bkpt");
+        hal.scheduler->panic("g_atomic took too long");
+      }
+
+      sched->run_callbacks();
+      xSemaphoreGiveRecursive(g_atomic);
+    }
+  }
+}
+
+/** Count of the number of threads in "delay". */
+static uint32_t g_delay_count;
+
+/** Binary semaphore given when a thread enters "delay". */
+static xSemaphoreHandle g_delay_event;
+
+/**
+ * Low-priority thread that calls the delay callback every 1ms.
+ *
+ * Whenever any thread enters a call to "delay", it unblocks this
+ * task.
+ *
+ * We use a count of the number of threads currently in "delay"
+ * because there could be more than one.
+ */
+static void delay_cb_task(void *arg)
+{
+  SMACCMScheduler *sched = (SMACCMScheduler *)arg;
+  portTickType last_wake_time;
+  portTickType now;
+
+  last_wake_time = xTaskGetTickCount();
+
+  for (;;) {
+    portENTER_CRITICAL();
+    uint32_t delay_count = g_delay_count;
+    portEXIT_CRITICAL();
+
+    if (delay_count > 0) {
+      /* If some thread is in "delay", call the delay callback. */
+      sched->run_delay_cb();
+
+      /* If "vTaskDelayUntil" would return immediately without
+       * blocking, that means we've missed our deadline.  However,
+       * this thread is best-effort, so we'll just readjust our
+       * schedule accordingly and run 1ms from now.
+       *
+       * Without this, the thread runs many times to "catch up" if
+       * something takes too long in a higher priority thread. */
+      now = xTaskGetTickCount();
+      if (last_wake_time + DELAY_CB_TICKS <= now) {
+        last_wake_time = now;
+      } else {
+        vTaskDelayUntil(&last_wake_time, DELAY_CB_TICKS);
+      }
+    } else {
+      /* Wait for a thread to enter a delay. */
+      xSemaphoreTake(g_delay_event, portMAX_DELAY);
+      last_wake_time = xTaskGetTickCount();
+    }
+  }
+}
+
+SMACCMScheduler::SMACCMScheduler()
+  : m_delay_cb(NULL), m_suspended(false),
+    m_task(NULL), m_delay_cb_task(NULL),
+    m_failsafe_cb(NULL), m_num_procs(0)
+{
+}
+
+void SMACCMScheduler::init(void *arg)
+{
+  timer_init();
+
+  g_atomic = xSemaphoreCreateRecursiveMutex();
+
+  vSemaphoreCreateBinary(g_delay_event);
+  xSemaphoreTake(g_delay_event, portMAX_DELAY);
+
+  xTaskCreate(scheduler_task, (signed char *)"scheduler",
+              SCHEDULER_STACK_SIZE, this, SCHEDULER_PRIORITY,
+              &m_task);
+
+  xTaskCreate(delay_cb_task, (signed char *)"delay_cb",
+              DELAY_CB_STACK_SIZE, this, DELAY_CB_PRIORITY,
+              &m_delay_cb_task);
+}
+
+void SMACCMScheduler::delay(uint16_t ms)
+{
+  /* Wake up the delay callback thread. */
+  portENTER_CRITICAL();
+  ++g_delay_count;
+  portEXIT_CRITICAL();
+  xSemaphoreGive(g_delay_event);
+
+  timer_msleep(ms);
+
+  /* Put the delay callback thread back to sleep. */
+  portENTER_CRITICAL();
+  --g_delay_count;
+  portEXIT_CRITICAL();
+}
+
+uint32_t SMACCMScheduler::millis()
+{
+  return (uint32_t)(timer_get_ticks() / 1000ULL);
+}
+
+// XXX this is going to wrap every 1.1 hours
+uint32_t SMACCMScheduler::micros()
+{
+  return (uint32_t)timer_get_ticks();
+}
+
+void SMACCMScheduler::delay_microseconds(uint16_t us)
+{
+  timer_usleep(us);
+}
+
+void SMACCMScheduler::register_delay_callback(AP_HAL::Proc k, uint16_t)
+{
+  m_delay_cb = k;
+}
+
+void SMACCMScheduler::register_timer_process(AP_HAL::TimedProc k)
+{
+  for (int i = 0; i < m_num_procs; ++i) {
+    if (m_procs[i] == k)
+      return;
+  }
+
+  if (m_num_procs < SMACCM_SCHEDULER_MAX_TIMER_PROCS) {
+    portENTER_CRITICAL();
+    m_procs[m_num_procs] = k;
+    ++m_num_procs;
+    portEXIT_CRITICAL();
+  }
+}
+
+void SMACCMScheduler::register_timer_failsafe(AP_HAL::TimedProc k, uint32_t)
+{
+  m_failsafe_cb = k;
+}
+
+void SMACCMScheduler::suspend_timer_procs()
+{
+  m_suspended = true;
+}
+
+void SMACCMScheduler::resume_timer_procs()
+{
+  m_suspended = false;
+}
+
+void SMACCMScheduler::begin_atomic()
+{
+  xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
+}
+
+void SMACCMScheduler::end_atomic()
+{
+  xSemaphoreGiveRecursive(g_atomic);
+}
+
+void SMACCMScheduler::panic(const prog_char_t *errormsg)
+{
+  hal.console->println_P(errormsg);
+  m_suspended = true;
+
+  for(;;)
+    ;
+}
+
+void SMACCMScheduler::reboot()
+{
+  for(;;)
+    ;
+}
+
+void SMACCMScheduler::run_callbacks()
+{
+  uint32_t now = micros();
+
+  // Run timer processes if not suspended.
+  if (!m_suspended) {
+    portENTER_CRITICAL();
+    uint8_t num_procs = m_num_procs;
+    portEXIT_CRITICAL();
+
+    for (int i = 0; i < num_procs; ++i) {
+      if (m_procs[i] != NULL)
+        m_procs[i](now);
+    }
+  }
+}
+
+void SMACCMScheduler::run_failsafe_cb()
+{
+  if (m_failsafe_cb)
+    m_failsafe_cb(micros());
+}
+
+void SMACCMScheduler::run_delay_cb()
+{
+  if (m_delay_cb)
+    m_delay_cb();
+}

libraries/AP_HAL_SMACCM/Scheduler.h

@@ -0,0 +1,121 @@
+/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/*
+ * Scheduler.h --- AP_HAL_SMACCM scheduler.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ *
+ * Written by James Bielman <jamesjb@galois.com>, 20 December 2012
+ */
+
+#ifndef __AP_HAL_SMACCM_SCHEDULER_H__
+#define __AP_HAL_SMACCM_SCHEDULER_H__
+
+#include <AP_HAL_SMACCM.h>
+
+#define SMACCM_SCHEDULER_MAX_TIMER_PROCS 4
+
+class SMACCM::SMACCMScheduler : public AP_HAL::Scheduler
+{
+public:
+  SMACCMScheduler();
+
+  /**
+   * Initialize the scheduler.  This initializes the HWF4 timer
+   * driver.
+   *
+   * @param arg reserved, pass NULL
+   */
+  void init(void *arg);
+
+  /** Delay for "ms" milliseconds. */
+  void delay(uint16_t ms);
+
+  /** Delay for "us" microseconds. */
+  void delay_microseconds(uint16_t us);
+
+  /** Return the time since init in milliseconds. */
+  uint32_t millis();
+
+  /** Return the time since init in microseconds. */
+  uint32_t micros();
+
+  /**
+   * Register a callback to run every millisecond during a call to
+   * "delay" as long as the delay remaining is at least "min_time_ms".
+   *
+   * The callback is executed in the thread that calls "delay".
+   */
+  void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
+
+  /** Register a callback to run every 1ms (1kHz). */
+  void register_timer_process(AP_HAL::TimedProc);
+
+  /**
+   * Register a callback to run if a timer process takes too long to
+   * execute.  The second argument is ignored.
+   */
+  void register_timer_failsafe(AP_HAL::TimedProc, uint32_t);
+
+  /**
+   * Suspend execution of timed procedures.  Calls to this function do
+   * not nest.
+   */
+  void suspend_timer_procs();
+
+  /**
+   * Resume execution of timed procedures.  Calls to this function do
+   * not nest.
+   */
+  void resume_timer_procs();
+
+  /**
+   * Enter a critical section where timed processes will not be run.
+   * Calls to this function nest.
+   */
+  void begin_atomic();
+
+  /**
+   * Leave a critical section, re-enabling timed processes.  Calls to
+   * this function nest.
+   */
+  void end_atomic();
+
+  /** Print an error message and halt execution. */
+  void panic(const prog_char_t *errormsg);
+
+  /** Reboot the firmware.  Not implemented. */
+  void reboot();
+
+  /**
+   * Run timed and deferred processes.  This should not be called from
+   * client code.
+   */
+  void run_callbacks();
+
+  /**
+   * Run the failsafe callback.  This should not be called from client
+   * code.
+   */
+  void run_failsafe_cb();
+
+  /**
+   * Run the delay callback.  This should not be called from client
+   * code.
+   */
+  void run_delay_cb();
+
+private:
+  AP_HAL::Proc m_delay_cb;      /* delay callback */
+  bool m_suspended;             /* true if timers suspended */
+  void *m_task;                 /* opaque scheduler task handle */
+  void *m_delay_cb_task;        /* opaque delay cb task handle */
+  AP_HAL::TimedProc m_procs[SMACCM_SCHEDULER_MAX_TIMER_PROCS];
+  AP_HAL::TimedProc m_failsafe_cb;
+  uint8_t m_num_procs;          /* number of entries in "m_procs" */
+};
+
+#endif // __AP_HAL_SMACCM_SCHEDULER_H__

libraries/AP_HAL_SMACCM/Semaphore.cpp

@@ -0,0 +1,40 @@
+
+#include "Semaphore.h"
+
+using namespace SMACCM;
+
+SMACCMSemaphore::SMACCMSemaphore() :
+    _owner(NULL),
+    _k(NULL)
+{}
+
+
+bool SMACCMSemaphore::get(void* owner) {
+    if (_owner == NULL) {
+        _owner = owner;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool SMACCMSemaphore::release(void* owner) {
+    if (_owner == NULL || _owner != owner) {
+        return false;
+    } else {
+        _owner = NULL;
+        if (_k){
+            _k();
+            _k = NULL;
+        }
+        return true;
+    }
+}
+
+bool SMACCMSemaphore::call_on_release(void* caller, AP_HAL::Proc k) {
+    /* idk what semantics randy was looking for here, honestly.
+     * seems like a bad idea. */
+    _k = k;
+    return true;
+}
+

libraries/AP_HAL_SMACCM/Semaphore.h

@@ -0,0 +1,22 @@
+
+#ifndef __AP_HAL_SMACCM_SEMAPHORE_H__
+#define __AP_HAL_SMACCM_SEMAPHORE_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMSemaphore : public AP_HAL::Semaphore {
+public:
+    SMACCMSemaphore();
+    // get - to claim ownership of the semaphore
+    bool get(void* owner);
+    // release - to give up ownership of the semaphore
+    bool release(void* owner);
+    // call_on_release - returns true if caller successfully added to the
+    // queue to be called back
+    bool call_on_release(void* caller, AP_HAL::Proc k);
+private:
+    void* _owner;
+    AP_HAL::Proc _k;
+};
+
+#endif // __AP_HAL_SMACCM_SEMAPHORE_H__

libraries/AP_HAL_SMACCM/Storage.cpp

@@ -0,0 +1,80 @@
+/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/*
+ * Storage.cpp --- AP_HAL_SMACCM storage driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ *
+ * Written by James Bielman <jamesjb@galois.com>, 20 December 2012
+ */
+
+#include <string.h>
+#include <hwf4/eeprom.h>
+
+#include "Storage.h"
+
+using namespace SMACCM;
+
+#define EEPROM_I2C_ADDR 0x50
+
+// Note: These functions write multi-byte integers to the EEPROM in
+// the native byte order, and so the format will depend on the
+// endianness of the machine.
+
+SMACCMStorage::SMACCMStorage()
+{
+}
+
+void SMACCMStorage::init(void*)
+{
+  eeprom_init(i2c2, EEPROM_I2C_ADDR);
+}
+
+uint8_t SMACCMStorage::read_byte(uint16_t loc)
+{
+  uint8_t result = 0;
+  eeprom_read_byte(loc, &result);
+  return result;
+}
+
+uint16_t SMACCMStorage::read_word(uint16_t loc)
+{
+  uint16_t result = 0;
+  eeprom_read(loc, (uint8_t*)&result, sizeof(result));
+  return result;
+}
+
+uint32_t SMACCMStorage::read_dword(uint16_t loc)
+{
+  uint32_t result = 0;
+  eeprom_read(loc, (uint8_t*)&result, sizeof(result));
+  return result;
+}
+
+void SMACCMStorage::read_block(void* dst, uint16_t src, size_t n)
+{
+  eeprom_read(src, (uint8_t*)dst, n);
+}
+
+void SMACCMStorage::write_byte(uint16_t loc, uint8_t value)
+{
+  eeprom_write_byte(loc, value);
+}
+
+void SMACCMStorage::write_word(uint16_t loc, uint16_t value)
+{
+  eeprom_write(loc, (uint8_t*)&value, sizeof(value));
+}
+
+void SMACCMStorage::write_dword(uint16_t loc, uint32_t value)
+{
+  eeprom_write(loc, (uint8_t*)&value, sizeof(value));
+}
+
+void SMACCMStorage::write_block(uint16_t loc, void* src, size_t n)
+{
+  eeprom_write(loc, (const uint8_t *)src, n);
+}

libraries/AP_HAL_SMACCM/Storage.h

@@ -0,0 +1,35 @@
+/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/*
+ * Storage.h --- AP_HAL_SMACCM storage driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ *
+ * Written by James Bielman <jamesjb@galois.com>, 20 December 2012
+ */
+
+#ifndef __AP_HAL_SMACCM_STORAGE_H__
+#define __AP_HAL_SMACCM_STORAGE_H__
+
+#include <AP_HAL_SMACCM.h>
+
+class SMACCM::SMACCMStorage : public AP_HAL::Storage
+{
+public:
+  SMACCMStorage();
+  void init(void *);
+  uint8_t  read_byte(uint16_t loc);
+  uint16_t read_word(uint16_t loc);
+  uint32_t read_dword(uint16_t loc);
+  void     read_block(void *dst, uint16_t src, size_t n);
+
+  void write_byte(uint16_t loc, uint8_t value);
+  void write_word(uint16_t loc, uint16_t value);
+  void write_dword(uint16_t loc, uint32_t value);
+  void write_block(uint16_t dst, void* src, size_t n);
+};
+
+#endif // __AP_HAL_SMACCM_STORAGE_H__

libraries/AP_HAL_SMACCM/UARTDriver.cpp

@@ -0,0 +1,145 @@
+/*
+ * UARTDriver.cpp --- AP_HAL_SMACCM UART driver.
+ *
+ * Copyright (C) 2012, Galois, Inc.
+ * All Rights Reserved.
+ *
+ * This software is released under the "BSD3" license.  Read the file
+ * "LICENSE" for more information.
+ */
+
+#include "UARTDriver.h"
+#include <stdio.h>              // for vsnprintf
+
+using namespace SMACCM;
+
+// XXX the AVR driver enables itself in the constructor.  This seems
+// like a very bad idea, since it will run somewhere in the startup
+// code before our clocks are all set up and such.
+SMACCMUARTDriver::SMACCMUARTDriver(struct usart *dev)
+  : m_dev(dev), m_initialized(false), m_blocking(true)
+{
+}
+
+void SMACCMUARTDriver::begin(uint32_t baud)
+{
+  usart_init(m_dev, baud);
+  usart_enable(m_dev);
+  m_initialized = true;
+}
+
+// XXX buffer sizes ignored
+void SMACCMUARTDriver::begin(uint32_t baud, uint16_t rxS, uint16_t txS)
+{
+  begin(baud);
+}
+
+// XXX hwf4 doesn't support de-initializing a USART
+void SMACCMUARTDriver::end()
+{
+}
+
+// XXX hwf4 doesn't support flushing, could be tricky to get the
+// synchronization right.  Would we just force the TX/RX queues to
+// empty?
+void SMACCMUARTDriver::flush()
+{
+}
+
+bool SMACCMUARTDriver::is_initialized()
+{
+  return m_initialized;
+}
+
+void SMACCMUARTDriver::set_blocking_writes(bool blocking)
+{
+  m_blocking = blocking;
+}
+
+bool SMACCMUARTDriver::tx_pending()
+{
+  return usart_is_tx_pending(m_dev);
+}
+
+/* SMACCM implementations of BetterStream virtual methods */
+void SMACCMUARTDriver::print_P(const prog_char_t *pstr)
+{
+  while (*pstr)
+    write(*pstr++);
+}
+
+void SMACCMUARTDriver::println_P(const prog_char_t *pstr)
+{
+  print_P(pstr);
+  println();
+}
+
+// XXX this will be changing, putting this on the stack hurts but
+// allows us to be easily re-entrant
+void SMACCMUARTDriver::printf(const char *fmt, ...)
+{
+  va_list ap;
+  va_start(ap, fmt);
+  vprintf(fmt, ap);
+  va_end(ap);
+}
+
+void SMACCMUARTDriver::_printf_P(const prog_char *fmt, ...)
+{
+  va_list ap;
+  va_start(ap, fmt);
+  vprintf(fmt, ap);
+  va_end(ap);
+}
+
+void SMACCMUARTDriver::vprintf(const char *pstr, va_list ap)
+{
+  char buf[128];
+  vsnprintf(buf, sizeof(buf), pstr, ap);
+  print(buf);
+}
+
+void SMACCMUARTDriver::vprintf_P(const prog_char *pstr, va_list ap)
+{
+  vprintf(pstr, ap);
+}
+
+/* SMACCM implementations of Stream virtual methods */
+int16_t SMACCMUARTDriver::available()
+{
+  return (int16_t)usart_available(m_dev);
+}
+
+int16_t SMACCMUARTDriver::txspace()
+{
+  return (int16_t)usart_txspace(m_dev);
+}
+
+// It looks like this should always be a non-blocking read, so return
+// -1 if there is nothing to receive immediately.
+int16_t SMACCMUARTDriver::read()
+{
+  uint8_t c;
+
+  if (usart_read_timeout(m_dev, 0, &c, 1) == 0)
+    return -1;
+
+  return (int16_t)c;
+}
+
+int16_t SMACCMUARTDriver::peek()
+{
+  uint8_t c;
+
+  if (!usart_peek(m_dev, &c))
+    return -1;
+
+  return (int16_t)c;
+}
+
+/* SMACCM implementations of Print virtual methods */
+size_t SMACCMUARTDriver::write(uint8_t c)
+{
+  portTickType delay = m_blocking ? portMAX_DELAY : 0;
+  return usart_write_timeout(m_dev, delay, &c, 1);
+}

libraries/AP_HAL_SMACCM/UARTDriver.h

@@ -0,0 +1,46 @@
+
+#ifndef __AP_HAL_SMACCM_UARTDRIVER_H__
+#define __AP_HAL_SMACCM_UARTDRIVER_H__
+
+#include <AP_HAL_SMACCM.h>
+#include <hwf4/usart.h>
+
+class SMACCM::SMACCMUARTDriver : public AP_HAL::UARTDriver
+{
+public:
+  SMACCMUARTDriver(struct usart *dev);
+
+  /* SMACCM implementations of UARTDriver virtual methods */
+  void begin(uint32_t b);
+  void begin(uint32_t b, uint16_t rxS, uint16_t txS);
+  void end();
+  void flush();
+  bool is_initialized();
+  void set_blocking_writes(bool blocking);
+  bool tx_pending();
+
+  /* SMACCM implementations of BetterStream virtual methods */
+  void print_P(const prog_char_t *pstr);
+  void println_P(const prog_char_t *pstr);
+  void printf(const char *pstr, ...);
+  void _printf_P(const prog_char *pstr, ...);
+
+  void vprintf(const char* fmt, va_list ap);
+  void vprintf_P(const prog_char* fmt, va_list ap);
+
+  /* SMACCM implementations of Stream virtual methods */
+  int16_t available();
+  int16_t txspace();
+  int16_t read();
+  int16_t peek();
+
+  /* SMACCM implementations of Print virtual methods */
+  size_t write(uint8_t c);
+
+private:
+  struct usart *m_dev;
+  bool m_initialized;
+  bool m_blocking;
+};
+
+#endif // __AP_HAL_SMACCM_UARTDRIVER_H__

libraries/AP_HAL_SMACCM/Util.cpp

@@ -0,0 +1,35 @@
+
+#include "Util.h"
+using namespace SMACCM;
+
+int SMACCMUtil::snprintf(char* str, size_t size, const char *format, ...)
+{
+    va_list ap;
+    va_start(ap, format);
+    int res = this->vsnprintf(str, size, format, ap);
+    va_end(ap);
+    return res;
+}
+
+int SMACCMUtil::snprintf_P(char* str, size_t size, const prog_char_t *format, ...)
+{
+    va_list ap;
+    va_start(ap, format);
+    int res = this->vsnprintf_P(str, size, format, ap);
+    va_end(ap);
+    return res;
+}
+
+
+int SMACCMUtil::vsnprintf(char* str, size_t size, const char *format, va_list ap)
+{
+    return 0;
+}
+
+int SMACCMUtil::vsnprintf_P(char* str, size_t size, const prog_char_t *format,
+            va_list ap)
+{
+    return 0;
+}
+
+

libraries/AP_HAL_SMACCM/Util.h

@@ -0,0 +1,17 @@
+
+#ifndef __AP_HAL_SMACCM_UTIL_H__
+#define __AP_HAL_SMACCM_UTIL_H__
+
+#include <AP_HAL.h>
+#include "AP_HAL_SMACCM_Namespace.h"
+
+class SMACCM::SMACCMUtil : public AP_HAL::Util {
+public:
+    int snprintf(char* str, size_t size, const char *format, ...);
+    int snprintf_P(char* str, size_t size, const prog_char_t *format, ...);
+    int vsnprintf(char* str, size_t size, const char *format, va_list ap);
+    int vsnprintf_P(char* str, size_t size, const prog_char_t *format,
+            va_list ap);
+};
+
+#endif // __AP_HAL_SMACCM_UTIL_H__

libraries/AP_HAL_SMACCM/examples/empty_example/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/Arduino.mk

libraries/AP_HAL_SMACCM/examples/empty_example/empty_example.pde

@@ -0,0 +1,18 @@
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Param.h>
+#include <AP_Math.h>
+
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include <AP_HAL_Empty.h>
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+void setup() {}
+void loop() {}
+
+AP_HAL_MAIN();
+

libraries/AP_Progmem/AP_Progmem.h

@@ -5,7 +5,7 @@
 #include <AP_HAL_Boards.h>
 #if defined(__AVR__) 
 #include "AP_Progmem_AVR.h"
-#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL || CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
 #include "AP_Progmem_Identity.h"
 #else
 #error "this build type is unknown - please edit AP_Progmem.h"