mbedPi

Revision 0:91392e1f8551, committed 2016-10-18

Comitter:

hudakz

Date:

Tue Oct 18 18:18:37 2016 +0000

Child:

1:1f2d9982fa8c

Commit message:

Initial issue.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbedPi.cpp	Tue Oct 18 18:18:37 2016 +0000
@@ -0,0 +1,1575 @@
+/*
+ *  Copyright (C) 2016 Zoltan Hudak
+ *  hudakz@outlook.com
+ *
+ *  Portions Copyright (C) Libelium Comunicaciones Distribuidas S.L.
+ *  http://www.libelium.com
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Version 1.0
+ */
+//
+#include "mbedPi.h"
+#include <stdarg.h>
+/*$off*/
+struct bcm2835_peripheral   gpio = { GPIO_BASE };
+struct bcm2835_peripheral   bsc_rev1 = { IOBASE + 0X205000 };
+struct bcm2835_peripheral   bsc_rev2 = { IOBASE + 0X804000 };
+struct bcm2835_peripheral   bsc0;
+volatile uint32_t*          bcm2835_bsc1;
+
+void*                       spi0 = MAP_FAILED;
+static uint8_t*             spi0Mem = NULL;
+
+pthread_t  idThread2;
+pthread_t  idThread3;
+pthread_t  idThread4;
+pthread_t  idThread5;
+pthread_t  idThread6;
+pthread_t  idThread7;
+pthread_t  idThread8;
+pthread_t  idThread9;
+pthread_t  idThread10;
+pthread_t  idThread11;
+pthread_t  idThread12;
+pthread_t  idThread13;
+pthread_t  idThread14;
+pthread_t  idThread15;
+pthread_t  idThread16;
+pthread_t  idThread17;
+pthread_t  idThread18;
+pthread_t  idThread19;
+pthread_t  idThread20;
+pthread_t  idThread21;
+pthread_t  idThread22;
+pthread_t  idThread23;
+pthread_t  idThread24;
+pthread_t  idThread25;
+pthread_t  idThread26;
+pthread_t  idThread27;
+
+pthread_t*   idThreads[26] =
+{
+   &idThread2,
+   &idThread3,
+   &idThread4,
+   &idThread5,
+   &idThread6,
+   &idThread7,
+   &idThread8,
+   &idThread9,
+   &idThread10,
+   &idThread11,
+   &idThread12,
+   &idThread13,
+   &idThread14,
+   &idThread15,
+   &idThread16,
+   &idThread17,
+   &idThread18,
+   &idThread19,
+   &idThread20,
+   &idThread21,
+   &idThread22,
+   &idThread23,
+   &idThread24,
+   &idThread25,
+   &idThread26,
+   &idThread27
+};
+
+timeval  start_program, end_point;
+
+/*$on*/
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void wait(float s) {
+    unistd::usleep(s * 1000 * 1000);
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void wait_ms(int ms) {
+    unistd::usleep(ms * 1000);
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void wait_us(int us) {
+    if(us > 100) {
+        struct timespec tim, tim2;
+        tim.tv_sec = 0;
+        tim.tv_nsec = us * 1000;
+
+        if(nanosleep(&tim, &tim2) < 0) {
+            fprintf(stderr, "Nano sleep system call failed \n");
+            exit(1);
+        }
+    }
+    else {
+        struct timeval  tNow, tLong, tEnd;
+
+        gettimeofday(&tNow, NULL);
+        tLong.tv_sec = us / 1000000;
+        tLong.tv_usec = us % 1000000;
+        timeradd(&tNow, &tLong, &tEnd);
+
+        while(timercmp(&tNow, &tEnd, < ))
+            gettimeofday(&tNow, NULL);
+    }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+Serial::Serial(void) {
+    //if((tx == gpio14) && (tx == gpio15)) {
+        REV = getBoardRev();
+        serialPort = "/dev/ttyAMA0";
+        timeOut = 1000;
+        //baud(9600);
+    //}
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void Serial::baud(int baudrate) {
+    switch(baudrate) {
+    case 50:
+        speed = B50;
+        break;
+
+    case 75:
+        speed = B75;
+        break;
+
+    case 110:
+        speed = B110;
+        break;
+
+    case 134:
+        speed = B134;
+        break;
+
+    case 150:
+        speed = B150;
+        break;
+
+    case 200:
+        speed = B200;
+        break;
+
+    case 300:
+        speed = B300;
+        break;
+
+    case 600:
+        speed = B600;
+        break;
+
+    case 1200:
+        speed = B1200;
+        break;
+
+    case 1800:
+        speed = B1800;
+        break;
+
+    case 2400:
+        speed = B2400;
+        break;
+
+    case 9600:
+        speed = B9600;
+        break;
+
+    case 19200:
+        speed = B19200;
+        break;
+
+    case 38400:
+        speed = B38400;
+        break;
+
+    case 57600:
+        speed = B57600;
+        break;
+
+    case 115200:
+        speed = B115200;
+        break;
+
+    default:
+        speed = B230400;
+        break;
+    }
+
+    if((sd = open(serialPort, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK)) == -1) {
+        fprintf(stderr, "Unable to open the serial port %s - \n", serialPort);
+        exit(-1);
+    }
+
+    fcntl(sd, F_SETFL, O_RDWR);
+
+    tcgetattr(sd, &options);
+    cfmakeraw(&options);
+    cfsetispeed(&options, speed);
+    cfsetospeed(&options, speed);
+
+    options.c_cflag |= (CLOCAL | CREAD);
+    options.c_cflag &= ~PARENB;
+    options.c_cflag &= ~CSTOPB;
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8;
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    options.c_oflag &= ~OPOST;
+
+    tcsetattr(sd, TCSANOW, &options);
+
+    ioctl(sd, TIOCMGET, &status);
+
+    status |= TIOCM_DTR;
+    status |= TIOCM_RTS;
+
+    ioctl(sd, TIOCMSET, &status);
+
+    unistd::usleep(10000);
+}
+
+void Serial::printf(const char* format, ...) {
+    char    *buf;
+    va_list args;
+
+    va_start(args, format);
+    vasprintf(&buf, format, args);
+    va_end(args);
+    unistd::write(sd, buf, strlen(buf));
+    free(buf);
+}
+
+/* Writes binary data to the serial port. This data is sent as a byte
+ * Returns: number of bytes written */
+int Serial::write(uint8_t message) {
+    unistd::write(sd, &message, 1);
+    return 1;
+}
+
+/* Writes binary data to the serial port. This data is sent as a series
+ * of bytes
+ * Returns: number of bytes written */
+int Serial::write(const char* message) {
+    int len = strlen(message);
+    unistd::write(sd, &message, len);
+    return len;
+}
+
+/* Writes binary data to the serial port. This data is sent as a series
+ * of bytes placed in an buffer. It needs the length of the buffer
+ * Returns: number of bytes written */
+int Serial::write(char* message, int size) {
+    unistd::write(sd, message, size);
+    return size;
+}
+
+int Serial::readable(void) {
+    int nbytes = 0;
+    if(ioctl(sd, FIONREAD, &nbytes) < 0) {
+        fprintf(stderr, "Failed to get byte count on serial.\n");
+        exit(-1);
+    }
+
+    return (nbytes > 0 ? 1 : 0);
+}
+
+/* Reads 1 byte of incoming serial data
+ * Returns: first byte of incoming serial data available */
+char Serial::read(void) {
+    unistd::read(sd, &c, 1);
+    return c;
+}
+
+/* Reads characters from th serial port into a buffer. The function
+ * terminates if the determined length has been read, or it times out
+ * Returns: number of bytes readed */
+int Serial::readBytes(char message[], int size) {
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
+
+    int count;
+    for(count = 0; count < size; count++) {
+        if(readable())
+            unistd::read(sd, &message[count], 1);
+        clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
+
+        timespec    t = timeDiff(time1, time2);
+        if((t.tv_nsec / 1000) > timeOut)
+            break;
+    }
+
+    return count;
+}
+
+/* Reads characters from the serial buffer into an array.
+ * The function terminates if the terminator character is detected,
+ * the determined length has been read, or it times out.
+ * Returns: number of characters read into the buffer. */
+int Serial::readBytesUntil(char character, char buffer[], int length) {
+    char    lastReaded = character + 1; //Just to make lastReaded != character
+    int     count = 0;
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
+    while(count != length && lastReaded != character) {
+        if(readable())
+            unistd::read(sd, &buffer[count], 1);
+        lastReaded = buffer[count];
+        count++;
+        clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
+
+        timespec    t = timeDiff(time1, time2);
+        if((t.tv_nsec / 1000) > timeOut)
+            break;
+    }
+
+    return count;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+bool Serial::find(const char* target) {
+    findUntil(target, NULL);
+}
+
+/* Reads data from the serial buffer until a target string of given length
+ * or terminator string is found.
+ * Returns: true if the target string is found, false if it times out */
+bool Serial::findUntil(const char* target, const char* terminal) {
+    int         index = 0;
+    int         termIndex = 0;
+    int         targetLen = strlen(target);
+    int         termLen = strlen(terminal);
+    char        readed;
+    timespec    t;
+
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
+
+    if(*target == 0)
+        return true;        // return true if target is a null string
+    do
+    {
+        if(readable()) {
+            unistd::read(sd, &readed, 1);
+            if(readed != target[index])
+                index = 0;  // reset index if any char does not match
+            if(readed == target[index]) {
+                if(++index >= targetLen) {
+
+                    // return true if all chars in the target match
+                    return true;
+                }
+            }
+
+            if(termLen > 0 && c == terminal[termIndex]) {
+                if(++termIndex >= termLen)
+                    return false;   // return false if terminate string found before target string
+            }
+            else {
+                termIndex = 0;
+            }
+        }
+
+        clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
+        t = timeDiff(time1, time2);
+    } while((t.tv_nsec / 1000) <= timeOut);
+
+    return false;
+}
+
+/* returns the first valid (long) integer value from the current position.
+ * initial characters that are not digits (or the minus sign) are skipped
+ * function is terminated by the first character that is not a digit. */
+long Serial::parseInt(void) {
+    bool    isNegative = false;
+    long    value = 0;
+    char    c;
+
+    //Skip characters until a number or - sign found
+    do
+    {
+        c = peek();
+        if(c == '-')
+            break;
+        if(c >= '0' && c <= '9')
+            break;
+        unistd::read(sd, &c, 1);    // discard non-numeric
+    } while(1);
+
+    do
+    {
+        if(c == '-')
+            isNegative = true;
+        else
+        if(c >= '0' && c <= '9')    // is c a digit?
+            value = value * 10 + c - '0';
+        unistd::read(sd, &c, 1);    // consume the character we got with peek
+        c = peek();
+    } while(c >= '0' && c <= '9');
+
+    if(isNegative)
+        value = -value;
+    return value;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+float Serial::parseFloat(void) {
+    boolean isNegative = false;
+    boolean isFraction = false;
+    long    value = 0;
+    char    c;
+    float   fraction = 1.0;
+
+    //Skip characters until a number or - sign found
+    do
+    {
+        c = peek();
+        if(c == '-')
+            break;
+        if(c >= '0' && c <= '9')
+            break;
+        unistd::read(sd, &c, 1);    // discard non-numeric
+    } while(1);
+
+    do
+    {
+        if(c == '-')
+            isNegative = true;
+        else
+        if(c == '.')
+            isFraction = true;
+        else
+        if(c >= '0' && c <= '9') {
+
+            // is c a digit?
+            value = value * 10 + c - '0';
+            if(isFraction)
+                fraction *= 0.1;
+        }
+
+        unistd::read(sd, &c, 1);    // consume the character we got with peek
+        c = peek();
+    } while((c >= '0' && c <= '9') || (c == '.' && isFraction == false));
+
+    if(isNegative)
+        value = -value;
+    if(isFraction)
+        return value * fraction;
+    else
+        return value;
+}
+
+// Returns the next byte (character) of incoming serial data without removing it from the internal serial buffer.
+char Serial::peek(void) {
+
+    //We obtain a pointer to FILE structure from the file descriptor sd
+    FILE*   f = fdopen(sd, "r+");
+
+    //With a pointer to FILE we can do getc and ungetc
+    c = getc(f);
+    ungetc(c, f);
+    return c;
+}
+
+// Remove any data remaining on the serial buffer
+void Serial::flush(void) {
+    while(readable()) {
+        unistd::read(sd, &c, 1);
+    }
+}
+
+/* Sets the maximum milliseconds to wait for serial data when using SerialPI::readBytes()
+ * The default value is set to 1000 */
+void Serial::setTimeout(long millis) {
+    timeOut = millis;
+}
+
+//Closes serial communication
+void Serial::close(void) {
+    unistd::close(sd);
+}
+
+/*******************
+ * Private methods *
+ *******************/
+
+//Returns a timespec struct with the time elapsed between start and end timespecs
+timespec Serial::timeDiff(timespec start, timespec end) {
+    timespec    temp;
+    if((end.tv_nsec - start.tv_nsec) < 0) {
+        temp.tv_sec = end.tv_sec - start.tv_sec - 1;
+        temp.tv_nsec = 1000000000 + end.tv_nsec - start.tv_nsec;
+    }
+    else {
+        temp.tv_sec = end.tv_sec - start.tv_sec;
+        temp.tv_nsec = end.tv_nsec - start.tv_nsec;
+    }
+
+    return temp;
+}
+
+//Constructor
+Peripheral::Peripheral(void) {
+    REV = getBoardRev();
+    if(map_peripheral(&gpio) == -1) {
+        printf("Failed to map the physical GPIO registers into the virtual memory space.\n");
+    }
+
+    memfd = -1;
+
+    //i2c_byte_wait_us = 0;
+    // Open the master /dev/memory device
+    if((memfd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
+        fprintf(stderr, "bcm2835_init: Unable to open /dev/mem: %s\n", strerror(errno));
+        exit(1);
+    }
+
+    bcm2835_bsc1 = mapmem("bsc1", BLOCK_SIZE, memfd, BCM2835_BSC1_BASE);
+    if(bcm2835_bsc1 == MAP_FAILED)
+        exit(1);
+
+    // start timer
+    gettimeofday(&start_program, NULL);
+}
+
+//Destructor
+Peripheral::~Peripheral(void) {
+    unmap_peripheral(&gpio);
+}
+
+/*******************
+ * Private methods *
+ *******************/
+
+// Exposes the physical address defined in the passed structure using mmap on /dev/mem
+int Peripheral::map_peripheral(struct bcm2835_peripheral* p) {
+
+    // Open /dev/mem
+    if((p->mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
+        printf("Failed to open /dev/mem, try checking permissions.\n");
+        return -1;
+    }
+
+    p->map = mmap
+        (
+            NULL,
+            BLOCK_SIZE,
+            PROT_READ | PROT_WRITE,
+            MAP_SHARED,
+            p->mem_fd,  // File descriptor to physical memory virtual file '/dev/mem'
+            p->addr_p   // Address in physical map that we want this memory block to expose
+        );
+
+    if(p->map == MAP_FAILED) {
+        perror("mmap");
+        return -1;
+    }
+
+    p->addr = (volatile unsigned int*)p->map;
+
+    return 0;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void Peripheral::unmap_peripheral(struct bcm2835_peripheral* p) {
+    munmap(p->map, BLOCK_SIZE);
+    unistd::close(p->mem_fd);
+}
+
+//Constructor
+I2C::I2C(void) {
+
+    //    REV = getBoardRev();
+    //    if(map_peripheral(&gpio) == -1) {
+    //        printf("Failed to map the physical GPIO registers into the virtual memory space.\n");
+    //    }
+    //    memfd = -1;
+    i2c_byte_wait_us = 0;
+
+    //    // Open the master /dev/memory device
+    //    if((memfd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
+    //        fprintf(stderr, "bcm2835_init: Unable to open /dev/mem: %s\n", strerror(errno));
+    //        exit(1);
+    //    }
+    //    bcm2835_bsc1 = mapmem("bsc1", BLOCK_SIZE, memfd, BCM2835_BSC1_BASE);
+    //    if(bcm2835_bsc1 == MAP_FAILED)
+    //        exit(1);
+    //    // start timer
+    //    gettimeofday(&start_program, NULL);
+}
+
+//Initiate the Wire library and join the I2C bus.
+void I2C::begin(void) {
+    volatile uint32_t*  paddr = bcm2835_bsc1 + BCM2835_BSC_DIV / 4;
+
+    // Set the I2C/BSC1 pins to the Alt 0 function to enable I2C access on them
+    bcm2835_gpio_fsel(RPI_V2_GPIO_P1_03, BCM2835_GPIO_FSEL_ALT0);   // SDA
+    bcm2835_gpio_fsel(RPI_V2_GPIO_P1_05, BCM2835_GPIO_FSEL_ALT0);   // SCL
+
+    // Read the clock divider register
+    uint16_t    cdiv = bcm2835_peri_read(paddr);
+
+    // Calculate time for transmitting one byte
+    // 1000000 = micros seconds in a second
+    // 9 = Clocks per byte : 8 bits + ACK
+    i2c_byte_wait_us = ((float)cdiv / BCM2835_CORE_CLK_HZ) * 1000000 * 9;
+}
+
+//Begin a transmission to the I2C slave device with the given address
+void I2C::beginTransmission(unsigned char address) {
+
+    // Set I2C Device Address
+    volatile uint32_t*  paddr = bcm2835_bsc1 + BCM2835_BSC_A / 4;
+    bcm2835_peri_write(paddr, address);
+}
+
+//Writes data to the I2C.
+void I2C::write(char data) {
+    char    i2cdata[1];
+    i2cdata[0] = data;
+
+    write(i2cdata, 1);
+}
+
+//Writes data to the I2C.
+uint8_t I2C::write(const char* buf, uint32_t len) {
+    volatile uint32_t*  dlen = bcm2835_bsc1 + BCM2835_BSC_DLEN / 4;
+    volatile uint32_t*  fifo = bcm2835_bsc1 + BCM2835_BSC_FIFO / 4;
+    volatile uint32_t*  status = bcm2835_bsc1 + BCM2835_BSC_S / 4;
+    volatile uint32_t*  control = bcm2835_bsc1 + BCM2835_BSC_C / 4;
+
+    uint32_t            remaining = len;
+    uint32_t            i = 0;
+    uint8_t             reason = BCM2835_I2C_REASON_OK;
+
+    // Clear FIFO
+    bcm2835_peri_set_bits(control, BCM2835_BSC_C_CLEAR_1, BCM2835_BSC_C_CLEAR_1);
+
+    // Clear Status
+    bcm2835_peri_write_nb(status, BCM2835_BSC_S_CLKT | BCM2835_BSC_S_ERR | BCM2835_BSC_S_DONE);
+
+    // Set Data Length
+    bcm2835_peri_write_nb(dlen, len);
+
+    // pre populate FIFO with max buffer
+    while(remaining && (i < BCM2835_BSC_FIFO_SIZE)) {
+        bcm2835_peri_write_nb(fifo, buf[i]);
+        i++;
+        remaining--;
+    }
+
+    // Enable device and start transfer
+    bcm2835_peri_write_nb(control, BCM2835_BSC_C_I2CEN | BCM2835_BSC_C_ST);
+
+    // Transfer is over when BCM2835_BSC_S_DONE
+    while(!(bcm2835_peri_read_nb(status) & BCM2835_BSC_S_DONE)) {
+        while(remaining && (bcm2835_peri_read_nb(status) & BCM2835_BSC_S_TXD)) {
+
+            // Write to FIFO, no barrier
+            bcm2835_peri_write_nb(fifo, buf[i]);
+            i++;
+            remaining--;
+        }
+    }
+
+    // Received a NACK
+    if(bcm2835_peri_read(status) & BCM2835_BSC_S_ERR) {
+        reason = BCM2835_I2C_REASON_ERROR_NACK;
+    }
+
+    // Received Clock Stretch Timeout
+    else
+    if(bcm2835_peri_read(status) & BCM2835_BSC_S_CLKT) {
+        reason = BCM2835_I2C_REASON_ERROR_CLKT;
+    }
+
+    // Not all data is sent
+    else
+    if(remaining) {
+        reason = BCM2835_I2C_REASON_ERROR_DATA;
+    }
+
+    bcm2835_peri_set_bits(control, BCM2835_BSC_S_DONE, BCM2835_BSC_S_DONE);
+
+    return reason;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void I2C::endTransmission(void) {
+
+    // Set all the I2C/BSC1 pins back to input
+    bcm2835_gpio_fsel(RPI_V2_GPIO_P1_03, BCM2835_GPIO_FSEL_INPT);   // SDA
+    bcm2835_gpio_fsel(RPI_V2_GPIO_P1_05, BCM2835_GPIO_FSEL_INPT);   // SCL
+}
+
+//Used by the master to request bytes from a slave device
+void I2C::requestFrom(unsigned char address, int quantity) {
+
+    // Set I2C Device Address
+    volatile uint32_t*  paddr = bcm2835_bsc1 + BCM2835_BSC_A / 4;
+    bcm2835_peri_write(paddr, address);
+
+    i2c_bytes_to_read = quantity;
+}
+
+//Reads a byte that was transmitted from a slave device to a master after a call to WirePi::requestFrom()
+unsigned char I2C::read(void) {
+    char    buf;
+    i2c_bytes_to_read = 1;
+    read(&buf);
+    return (unsigned char)buf;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+uint8_t I2C::read(char* buf) {
+    volatile uint32_t*  dlen = bcm2835_bsc1 + BCM2835_BSC_DLEN / 4;
+    volatile uint32_t*  fifo = bcm2835_bsc1 + BCM2835_BSC_FIFO / 4;
+    volatile uint32_t*  status = bcm2835_bsc1 + BCM2835_BSC_S / 4;
+    volatile uint32_t*  control = bcm2835_bsc1 + BCM2835_BSC_C / 4;
+
+    uint32_t            remaining = i2c_bytes_to_read;
+    uint32_t            i = 0;
+    uint8_t             reason = BCM2835_I2C_REASON_OK;
+
+    // Clear FIFO
+    bcm2835_peri_set_bits(control, BCM2835_BSC_C_CLEAR_1, BCM2835_BSC_C_CLEAR_1);
+
+    // Clear Status
+    bcm2835_peri_write_nb(status, BCM2835_BSC_S_CLKT | BCM2835_BSC_S_ERR | BCM2835_BSC_S_DONE);
+
+    // Set Data Length
+    bcm2835_peri_write_nb(dlen, i2c_bytes_to_read);
+
+    // Start read
+    bcm2835_peri_write_nb(control, BCM2835_BSC_C_I2CEN | BCM2835_BSC_C_ST | BCM2835_BSC_C_READ);
+
+    // wait for transfer to complete
+    while(!(bcm2835_peri_read_nb(status) & BCM2835_BSC_S_DONE)) {
+
+        // we must empty the FIFO as it is populated and not use any delay
+        while(bcm2835_peri_read_nb(status) & BCM2835_BSC_S_RXD) {
+
+            // Read from FIFO, no barrier
+            buf[i] = bcm2835_peri_read_nb(fifo);
+            i++;
+            remaining--;
+        }
+    }
+
+    // transfer has finished - grab any remaining stuff in FIFO
+    while(remaining && (bcm2835_peri_read_nb(status) & BCM2835_BSC_S_RXD)) {
+
+        // Read from FIFO, no barrier
+        buf[i] = bcm2835_peri_read_nb(fifo);
+        i++;
+        remaining--;
+    }
+
+    // Received a NACK
+    if(bcm2835_peri_read(status) & BCM2835_BSC_S_ERR) {
+        reason = BCM2835_I2C_REASON_ERROR_NACK;
+    }
+
+    // Received Clock Stretch Timeout
+    else
+    if(bcm2835_peri_read(status) & BCM2835_BSC_S_CLKT) {
+        reason = BCM2835_I2C_REASON_ERROR_CLKT;
+    }
+
+    // Not all data is received
+    else
+    if(remaining) {
+        reason = BCM2835_I2C_REASON_ERROR_DATA;
+    }
+
+    bcm2835_peri_set_bits(control, BCM2835_BSC_S_DONE, BCM2835_BSC_S_DONE);
+
+    return reason;
+}
+
+// Read an number of bytes from I2C sending a repeated start after writing
+
+// the required register. Only works if your device supports this mode
+uint8_t I2C::read_rs(char* regaddr, char* buf, uint32_t len) {
+    volatile uint32_t*  dlen = bcm2835_bsc1 + BCM2835_BSC_DLEN / 4;
+    volatile uint32_t*  fifo = bcm2835_bsc1 + BCM2835_BSC_FIFO / 4;
+    volatile uint32_t*  status = bcm2835_bsc1 + BCM2835_BSC_S / 4;
+    volatile uint32_t*  control = bcm2835_bsc1 + BCM2835_BSC_C / 4;
+
+    uint32_t            remaining = len;
+    uint32_t            i = 0;
+    uint8_t             reason = BCM2835_I2C_REASON_OK;
+
+    // Clear FIFO
+    bcm2835_peri_set_bits(control, BCM2835_BSC_C_CLEAR_1, BCM2835_BSC_C_CLEAR_1);
+
+    // Clear Status
+    bcm2835_peri_write_nb(status, BCM2835_BSC_S_CLKT | BCM2835_BSC_S_ERR | BCM2835_BSC_S_DONE);
+
+    // Set Data Length
+    bcm2835_peri_write_nb(dlen, 1);
+
+    // Enable device and start transfer
+    bcm2835_peri_write_nb(control, BCM2835_BSC_C_I2CEN);
+    bcm2835_peri_write_nb(fifo, regaddr[0]);
+    bcm2835_peri_write_nb(control, BCM2835_BSC_C_I2CEN | BCM2835_BSC_C_ST);
+
+    // poll for transfer has started
+    while(!(bcm2835_peri_read_nb(status) & BCM2835_BSC_S_TA)) {
+
+        // Linux may cause us to miss entire transfer stage
+        if(bcm2835_peri_read(status) & BCM2835_BSC_S_DONE)
+            break;
+    }
+
+    // Send a repeated start with read bit set in address
+    bcm2835_peri_write_nb(dlen, len);
+    bcm2835_peri_write_nb(control, BCM2835_BSC_C_I2CEN | BCM2835_BSC_C_ST | BCM2835_BSC_C_READ);
+
+    // Wait for write to complete and first byte back.
+    wait_us(i2c_byte_wait_us * 3);
+
+    // wait for transfer to complete
+    while(!(bcm2835_peri_read_nb(status) & BCM2835_BSC_S_DONE)) {
+
+        // we must empty the FIFO as it is populated and not use any delay
+        while(remaining && bcm2835_peri_read_nb(status) & BCM2835_BSC_S_RXD) {
+
+            // Read from FIFO, no barrier
+            buf[i] = bcm2835_peri_read_nb(fifo);
+            i++;
+            remaining--;
+        }
+    }
+
+    // transfer has finished - grab any remaining stuff in FIFO
+    while(remaining && (bcm2835_peri_read_nb(status) & BCM2835_BSC_S_RXD)) {
+
+        // Read from FIFO, no barrier
+        buf[i] = bcm2835_peri_read_nb(fifo);
+        i++;
+        remaining--;
+    }
+
+    // Received a NACK
+    if(bcm2835_peri_read(status) & BCM2835_BSC_S_ERR) {
+        reason = BCM2835_I2C_REASON_ERROR_NACK;
+    }
+
+    // Received Clock Stretch Timeout
+    else
+    if(bcm2835_peri_read(status) & BCM2835_BSC_S_CLKT) {
+        reason = BCM2835_I2C_REASON_ERROR_CLKT;
+    }
+
+    // Not all data is sent
+    else
+    if(remaining) {
+        reason = BCM2835_I2C_REASON_ERROR_DATA;
+    }
+
+    bcm2835_peri_set_bits(control, BCM2835_BSC_S_DONE, BCM2835_BSC_S_DONE);
+
+    return reason;
+}
+
+/*******************
+ * Private methods *
+ *******************/
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void I2C::wait_i2c_done(void) {
+
+    //Wait till done, let's use a timeout just in case
+    int timeout = 50;
+    while((!((BSC0_S) & BSC_S_DONE)) && --timeout) {
+        unistd::usleep(1000);
+    }
+
+    if(timeout == 0)
+        printf("wait_i2c_done() timeout. Something went wrong.\n");
+}
+
+/*******************************
+ *                             *
+ * SPIPi Class implementation *
+ * --------------------------- *
+ *******************************/
+
+/******************
+ * Public methods *
+ ******************/
+SPI::SPI(void) {
+    REV = getBoardRev();
+
+    uint8_t*    mapaddr;
+
+    if((spi0Mem = (uint8_t*)malloc(BLOCK_SIZE + (PAGESIZE - 1))) == NULL) {
+        fprintf(stderr, "bcm2835_init: spi0Mem malloc failed: %s\n", strerror(errno));
+        exit(1);
+    }
+
+    mapaddr = spi0Mem;
+    if(((uint32_t) mapaddr % PAGESIZE) != 0)
+        mapaddr += PAGESIZE - ((uint32_t) mapaddr % PAGESIZE);
+
+    spi0 = (uint32_t*)mmap
+        (
+            mapaddr,
+            BLOCK_SIZE,
+            PROT_READ | PROT_WRITE,
+            MAP_SHARED | MAP_FIXED,
+            gpio.mem_fd,
+            BCM2835_SPI0_BASE
+        );
+
+    if((int32_t) spi0 < 0) {
+        fprintf(stderr, "bcm2835_init: mmap failed (spi0): %s\n", strerror(errno));
+        exit(1);
+    }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void SPI::begin(void) {
+
+    // Set the SPI0 pins to the Alt 0 function to enable SPI0 access on them
+    bcm2835_gpio_fsel(7, BCM2835_GPIO_FSEL_ALT0);   // CE1
+    bcm2835_gpio_fsel(8, BCM2835_GPIO_FSEL_ALT0);   // CE0
+    bcm2835_gpio_fsel(9, BCM2835_GPIO_FSEL_ALT0);   // MISO
+    bcm2835_gpio_fsel(10, BCM2835_GPIO_FSEL_ALT0);  // MOSI
+    bcm2835_gpio_fsel(11, BCM2835_GPIO_FSEL_ALT0);  // CLK
+
+    // Set the SPI CS register to the some sensible defaults
+    volatile uint32_t*  paddr = (volatile uint32_t*)spi0 + BCM2835_SPI0_CS / 4;
+    bcm2835_peri_write(paddr, 0);   // All 0s
+
+    // Clear TX and RX fifos
+    bcm2835_peri_write_nb(paddr, BCM2835_SPI0_CS_CLEAR);
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void SPI::end(void) {
+
+    // Set all the SPI0 pins back to input
+    bcm2835_gpio_fsel(7, BCM2835_GPIO_FSEL_INPT);   // CE1
+    bcm2835_gpio_fsel(8, BCM2835_GPIO_FSEL_INPT);   // CE0
+    bcm2835_gpio_fsel(9, BCM2835_GPIO_FSEL_INPT);   // MISO
+    bcm2835_gpio_fsel(10, BCM2835_GPIO_FSEL_INPT);  // MOSI
+    bcm2835_gpio_fsel(11, BCM2835_GPIO_FSEL_INPT);  // CLK
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void SPI::setBitOrder(uint8_t order) {
+
+    // BCM2835_SPI_BIT_ORDER_MSBFIRST is the only one suported by SPI0
+}
+
+// defaults to 0, which means a divider of 65536.
+// The divisor must be a power of 2. Odd numbers
+// rounded down. The maximum SPI clock rate is
+
+// of the APB clock
+void SPI::setClockDivider(uint16_t divider) {
+    volatile uint32_t*  paddr = (volatile uint32_t*)spi0 + BCM2835_SPI0_CLK / 4;
+    bcm2835_peri_write(paddr, divider);
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void SPI::setDataMode(uint8_t mode) {
+    volatile uint32_t*  paddr = (volatile uint32_t*)spi0 + BCM2835_SPI0_CS / 4;
+
+    // Mask in the CPO and CPHA bits of CS
+    bcm2835_peri_set_bits(paddr, mode << 2, BCM2835_SPI0_CS_CPOL | BCM2835_SPI0_CS_CPHA);
+}
+
+// Writes (and reads) a single byte to SPI
+uint8_t SPI::transfer(uint8_t value) {
+    volatile uint32_t*  paddr = (volatile uint32_t*)spi0 + BCM2835_SPI0_CS / 4;
+    volatile uint32_t*  fifo = (volatile uint32_t*)spi0 + BCM2835_SPI0_FIFO / 4;
+
+    bcm2835_peri_set_bits(paddr, BCM2835_SPI0_CS_CLEAR, BCM2835_SPI0_CS_CLEAR);
+
+    bcm2835_peri_set_bits(paddr, BCM2835_SPI0_CS_TA, BCM2835_SPI0_CS_TA);
+
+    while(!(bcm2835_peri_read(paddr) & BCM2835_SPI0_CS_TXD))
+        wait_us(10);
+
+    bcm2835_peri_write_nb(fifo, value);
+
+    while(!(bcm2835_peri_read_nb(paddr) & BCM2835_SPI0_CS_DONE))
+        wait_us(10);
+
+    uint32_t    ret = bcm2835_peri_read_nb(fifo);
+
+    bcm2835_peri_set_bits(paddr, 0, BCM2835_SPI0_CS_TA);
+
+    return ret;
+}
+
+// Writes (and reads) a number of bytes to SPI
+void SPI::transfernb(char* tbuf, char* rbuf, uint32_t len) {
+    volatile uint32_t*  paddr = (volatile uint32_t*)spi0 + BCM2835_SPI0_CS / 4;
+    volatile uint32_t*  fifo = (volatile uint32_t*)spi0 + BCM2835_SPI0_FIFO / 4;
+
+    // This is Polled transfer as per section 10.6.1
+    // BUG ALERT: what happens if we get interupted in this section, and someone else
+    // accesses a different peripheral?
+    // Clear TX and RX fifos
+    bcm2835_peri_set_bits(paddr, BCM2835_SPI0_CS_CLEAR, BCM2835_SPI0_CS_CLEAR);
+
+    // Set TA = 1
+    bcm2835_peri_set_bits(paddr, BCM2835_SPI0_CS_TA, BCM2835_SPI0_CS_TA);
+
+    uint32_t    i;
+    for(i = 0; i < len; i++) {
+
+        // Maybe wait for TXD
+        while(!(bcm2835_peri_read(paddr) & BCM2835_SPI0_CS_TXD))
+            wait_us(10);
+
+        // Write to FIFO, no barrier
+        bcm2835_peri_write_nb(fifo, tbuf[i]);
+
+        // Wait for RXD
+        while(!(bcm2835_peri_read(paddr) & BCM2835_SPI0_CS_RXD))
+            wait_us(10);
+
+        // then read the data byte
+        rbuf[i] = bcm2835_peri_read_nb(fifo);
+    }
+
+    // Wait for DONE to be set
+    while(!(bcm2835_peri_read_nb(paddr) & BCM2835_SPI0_CS_DONE))
+        wait_us(10);
+
+    // Set TA = 0, and also set the barrier
+    bcm2835_peri_set_bits(paddr, 0, BCM2835_SPI0_CS_TA);
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void SPI::chipSelect(uint8_t cs) {
+    volatile uint32_t*  paddr = (volatile uint32_t*)spi0 + BCM2835_SPI0_CS / 4;
+
+    // Mask in the CS bits of CS
+    bcm2835_peri_set_bits(paddr, cs, BCM2835_SPI0_CS_CS);
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void SPI::setChipSelectPolarity(uint8_t cs, uint8_t active) {
+    volatile uint32_t*  paddr = (volatile uint32_t*)spi0 + BCM2835_SPI0_CS / 4;
+    uint8_t             shift = 21 + cs;
+
+    // Mask in the appropriate CSPOLn bit
+    bcm2835_peri_set_bits(paddr, active << shift, 1 << shift);
+}
+
+/********** FUNCTIONS OUTSIDE CLASSES **********/
+
+// Write a HIGH or a LOW value to a digital pin
+void gpio_write(PinName pin, int value) {
+    if(value == HIGH)
+        GPSET0 = (1 << pin);
+    else
+    if(value == LOW)
+        GPCLR0 = (1 << pin);
+
+    wait_us(1); // Delay to allow any change in state to be reflected in the LEVn, register bit.
+}
+
+// Reads the value from a specified digital pin, either HIGH or LOW.
+int gpio_read(PinName pin) {
+    Digivalue   value;
+    if(GPLEV0 & (1 << pin))
+        value = HIGH;
+    else
+        value = LOW;
+
+    return value;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+long us_ticker_read(void) {
+    long    elapsedTime;
+
+    // stop timer
+    gettimeofday(&end_point, NULL);
+
+    // compute and print the elapsed time in microseconds
+    //elapsedTime = (end_point.tv_sec - start_program.tv_sec) * 1000000.0;   // sec to us
+    elapsedTime += (end_point.tv_usec - start_program.tv_usec);
+    return elapsedTime;
+}
+
+/* Some helper functions */
+int getBoardRev(void) {
+    FILE*       cpu_info;
+    char        line[120];
+    char*       c, finalChar;
+    static int  rev = 0;
+
+    if(REV != 0)
+        return REV;
+
+    if((cpu_info = fopen("/proc/cpuinfo", "r")) == NULL) {
+        fprintf(stderr, "Unable to open /proc/cpuinfo. Cannot determine board reivision.\n");
+        exit(1);
+    }
+
+    while(fgets(line, 120, cpu_info) != NULL) {
+        if(strncmp(line, "Revision", 8) == 0)
+            break;
+    }
+
+    fclose(cpu_info);
+
+    if(line == NULL) {
+        fprintf(stderr, "Unable to determine board revision from /proc/cpuinfo.\n");
+        exit(1);
+    }
+
+    for(c = line; *c; ++c)
+        if(isdigit(*c))
+            break;
+
+    if(!isdigit(*c)) {
+        fprintf(stderr, "Unable to determine board revision from /proc/cpuinfo\n");
+        fprintf(stderr, "  (Info not found in: %s\n", line);
+        exit(1);
+    }
+
+    finalChar = c[strlen(c) - 2];
+
+    if((finalChar == '2') || (finalChar == '3')) {
+        bsc0 = bsc_rev1;
+        return 1;
+    }
+    else {
+        bsc0 = bsc_rev2;
+        return 2;
+    }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+uint32_t* mapmem(const char* msg, size_t size, int fd, off_t off) {
+    uint32_t*   map = (uint32_t*)mmap(NULL, size, (PROT_READ | PROT_WRITE), MAP_SHARED, fd, off);
+    if(MAP_FAILED == map)
+        fprintf(stderr, "bcm2835_init: %s mmap failed: %s\n", msg, strerror(errno));
+    return map;
+}
+
+// safe read from peripheral
+uint32_t bcm2835_peri_read(volatile uint32_t* paddr) {
+    uint32_t    ret = *paddr;
+    ret = *paddr;
+    return ret;
+}
+
+// read from peripheral without the read barrier
+uint32_t bcm2835_peri_read_nb(volatile uint32_t* paddr) {
+    return *paddr;
+}
+
+// safe write to peripheral
+void bcm2835_peri_write(volatile uint32_t* paddr, uint32_t value) {
+    *paddr = value;
+    *paddr = value;
+}
+
+// write to peripheral without the write barrier
+void bcm2835_peri_write_nb(volatile uint32_t* paddr, uint32_t value) {
+    *paddr = value;
+}
+
+// Set/clear only the bits in value covered by the mask
+void bcm2835_peri_set_bits(volatile uint32_t* paddr, uint32_t value, uint32_t mask) {
+    uint32_t    v = bcm2835_peri_read(paddr);
+    v = (v &~mask) | (value & mask);
+    bcm2835_peri_write(paddr, v);
+}
+
+//
+// Low level convenience functions
+//
+// Function select
+// pin is a BCM2835 GPIO pin number NOT RPi pin number
+//      There are 6 control registers, each control the functions of a block
+//      of 10 pins.
+//      Each control register has 10 sets of 3 bits per GPIO pin:
+//
+//      000 = GPIO Pin X is an input
+//      001 = GPIO Pin X is an output
+//      100 = GPIO Pin X takes alternate function 0
+//      101 = GPIO Pin X takes alternate function 1
+//      110 = GPIO Pin X takes alternate function 2
+//      111 = GPIO Pin X takes alternate function 3
+//      011 = GPIO Pin X takes alternate function 4
+//      010 = GPIO Pin X takes alternate function 5
+//
+// So the 3 bits for port X are:
+
+//      X / 10 + ((X % 10) * 3)
+void bcm2835_gpio_fsel(uint8_t pin, uint8_t mode) {
+
+    // Function selects are 10 pins per 32 bit word, 3 bits per pin
+    volatile uint32_t*  paddr = (volatile uint32_t*)gpio.map + BCM2835_GPFSEL0 / 4 + (pin / 10);
+    uint8_t             shift = (pin % 10) * 3;
+    uint32_t            mask = BCM2835_GPIO_FSEL_MASK << shift;
+    uint32_t            value = mode << shift;
+    bcm2835_peri_set_bits(paddr, value, mask);
+}
+
+/* This is the function that will be running in a thread if
+ * attachInterrupt() is called */
+void* threadFunction(void* args) {
+    ThreadArg*      arguments = (ThreadArg*)args;
+    int             pin = arguments->pin;
+
+    int             GPIO_FN_MAXLEN = 32;
+    int             RDBUF_LEN = 5;
+
+    char            fn[GPIO_FN_MAXLEN];
+    int             fd, ret;
+    struct pollfd   pfd;
+    char            rdbuf[RDBUF_LEN];
+
+    memset(rdbuf, 0x00, RDBUF_LEN);
+    memset(fn, 0x00, GPIO_FN_MAXLEN);
+
+    snprintf(fn, GPIO_FN_MAXLEN - 1, "/sys/class/gpio/gpio%d/value", pin);
+    fd = open(fn, O_RDONLY);
+    if(fd < 0) {
+        perror(fn);
+        exit(1);
+    }
+
+    pfd.fd = fd;
+    pfd.events = POLLPRI;
+
+    ret = unistd::read(fd, rdbuf, RDBUF_LEN - 1);
+    if(ret < 0) {
+        perror("Error reading interrupt file\n");
+        exit(1);
+    }
+
+    while(1) {
+        memset(rdbuf, 0x00, RDBUF_LEN);
+        unistd::lseek(fd, 0, SEEK_SET);
+        ret = poll(&pfd, 1, -1);
+        if(ret < 0) {
+            perror("Error waiting for interrupt\n");
+            unistd::close(fd);
+            exit(1);
+        }
+
+        if(ret == 0) {
+            printf("Timeout\n");
+            continue;
+        }
+
+        ret = unistd::read(fd, rdbuf, RDBUF_LEN - 1);
+        if(ret < 0) {
+            perror("Error reading interrupt file\n");
+            exit(1);
+        }
+
+        //Interrupt. We call user function.
+        arguments->func();
+    }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+pthread_t* getThreadIdFromPin(int pin) {
+    int i = pin - 2;
+    if((0 <= i) && (i <= 25))
+        return idThreads[i];
+    else
+        return NULL;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void gpio_dir(PinName pin, PinDirection direction) {
+    uint8_t     gpfsel = pin / 10;
+    uint8_t     shift = (pin % 10) * 3;
+    uint32_t    mask = BCM2835_GPIO_FSEL_MASK << shift;
+    uint32_t    outp = BCM2835_GPIO_FSEL_OUTP << shift;
+
+    if(direction == PIN_OUTPUT) {
+        *(gpio.addr + gpfsel) &= ~mask;
+        *(gpio.addr + gpfsel) |= outp;
+    }
+    else
+    if(direction == PIN_INPUT) {
+        *(gpio.addr + gpfsel) &= ~mask;
+    }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void gpio_mode(PinName pin, PinMode mode) {
+    mode == PullUp ? gpio_write(pin, HIGH) : gpio_write(pin, LOW);
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+uint8_t shiftIn(PinName dPin, PinName cPin, bcm2835SPIBitOrder order) {
+    uint8_t value = 0;
+    int8_t  i;
+
+    if(order == MSBFIRST)
+        for(i = 7; i >= 0; --i) {
+            gpio_write(cPin, HIGH);
+            value |= gpio_read(dPin) << i;
+            gpio_write(cPin, LOW);
+        }
+    else
+        for(i = 0; i < 8; ++i) {
+            gpio_write(cPin, HIGH);
+            value |= gpio_read(dPin) << i;
+            gpio_write(cPin, LOW);
+        }
+
+    return value;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void shiftOut(PinName dPin, PinName cPin, bcm2835SPIBitOrder order, uint8_t val) {
+    int8_t  i;
+
+    if(order == MSBFIRST)
+        for(i = 7; i >= 0; --i) {
+            gpio_write(dPin, val & (1 << i));
+            gpio_write(cPin, HIGH);
+            gpio_write(cPin, LOW);
+        }
+    else
+        for(i = 0; i < 8; ++i) {
+            gpio_write(dPin, val & (1 << i));
+            gpio_write(cPin, HIGH);
+            gpio_write(cPin, LOW);
+        }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void attachInterrupt(PinName p, void (*f) (), Digivalue m) {
+    int                 GPIOPin = p;
+    pthread_t*          threadId = getThreadIdFromPin(p);
+    struct ThreadArg*   threadArgs = (ThreadArg*)malloc(sizeof(ThreadArg));
+    threadArgs->func = f;
+    threadArgs->pin = GPIOPin;
+
+    //Export pin for interrupt
+    FILE*   fp = fopen("/sys/class/gpio/export", "w");
+    if(fp == NULL) {
+        fprintf(stderr, "Unable to export pin %d for interrupt\n", p);
+        exit(1);
+    }
+    else {
+        fprintf(fp, "%d", GPIOPin);
+    }
+
+    fclose(fp);
+
+    //The system to create the file /sys/class/gpio/gpio<GPIO number>
+    //So we wait a bit
+    wait_ms(1);
+
+    char*   interruptFile = NULL;
+    asprintf(&interruptFile, "/sys/class/gpio/gpio%d/edge", GPIOPin);
+
+    //Set detection condition
+    fp = fopen(interruptFile, "w");
+    if(fp == NULL) {
+        fprintf(stderr, "Unable to set detection type on pin %d\n", p);
+        exit(1);
+    }
+    else {
+        switch(m) {
+        case RISING:
+            fprintf(fp, "rising");
+            break;
+
+        case FALLING:
+            fprintf(fp, "falling");
+            break;
+
+        default:
+            fprintf(fp, "both");
+            break;
+        }
+    }
+
+    fclose(fp);
+
+    if(*threadId == 0) {
+
+        //Create a thread passing the pin and function
+        pthread_create(threadId, NULL, threadFunction, (void*)threadArgs);
+    }
+    else {
+
+        //First cancel the existing thread for that pin
+        pthread_cancel(*threadId);
+
+        //Create a thread passing the pin, function and mode
+        pthread_create(threadId, NULL, threadFunction, (void*)threadArgs);
+    }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void detachInterrupt(PinName p) {
+    int     GPIOPin = p;
+
+    FILE*   fp = fopen("/sys/class/gpio/unexport", "w");
+    if(fp == NULL) {
+        fprintf(stderr, "Unable to unexport pin %d for interrupt\n", p);
+        exit(1);
+    }
+    else {
+        fprintf(fp, "%d", GPIOPin);
+    }
+
+    fclose(fp);
+
+    pthread_t*  threadId = getThreadIdFromPin(p);
+    pthread_cancel(*threadId);
+}
+
+Peripheral  peripheral = Peripheral();

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbedPi.h	Tue Oct 18 18:18:37 2016 +0000
@@ -0,0 +1,601 @@
+/*
+ *  Copyright (C) 2016 Zoltan Hudak
+ *  hudakz@outlook.com
+ *
+ *  Portions Copyright (C) Libelium Comunicaciones Distribuidas S.L.
+ *  http://www.libelium.com
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Version 1.0
+ */
+//
+/*$off*/
+#ifndef MBEDPI_H
+#define MBEDPI_H
+
+#include <errno.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/ipc.h>
+#include <sys/sem.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/wait.h>
+#include <sys/mman.h>
+#include <string.h>
+#include <time.h>
+#include <termios.h>
+#include <ctype.h>
+#include <sys/ioctl.h>
+#include <limits.h>
+#include <algorithm>
+#include <limits.h>
+#include <pthread.h>
+#include <poll.h>
+
+#define IOBASE                  0x20000000
+#define GPIO_BASE               (IOBASE + 0x200000)
+#define BCM2835_SPI0_BASE       (IOBASE + 0x204000)
+#define BCM2835_BSC1_BASE       (IOBASE + 0x804000)
+
+// Defines for I2C
+// GPIO register offsets from BCM2835_BSC*_BASE.
+// Offsets into the BSC Peripheral block in bytes per 3.1 BSC Register Map
+#define BCM2835_BSC_C           0x0000          ///< BSC Master Control
+#define BCM2835_BSC_S           0x0004          ///< BSC Master Status
+#define BCM2835_BSC_DLEN        0x0008          ///< BSC Master Data Length
+#define BCM2835_BSC_A           0x000c          ///< BSC Master Slave Address
+#define BCM2835_BSC_FIFO        0x0010          ///< BSC Master Data FIFO
+#define BCM2835_BSC_DIV         0x0014          ///< BSC Master Clock Divider
+#define BCM2835_BSC_DEL         0x0018          ///< BSC Master Data Delay
+#define BCM2835_BSC_CLKT        0x001c          ///< BSC Master Clock Stretch Timeout
+
+// Register masks for BSC_C
+#define BCM2835_BSC_C_I2CEN     0x00008000  ///< I2C Enable, 0 = disabled, 1 = enabled
+#define BCM2835_BSC_C_INTR      0x00000400  ///< Interrupt on RX
+#define BCM2835_BSC_C_INTT      0x00000200  ///< Interrupt on TX
+#define BCM2835_BSC_C_INTD      0x00000100  ///< Interrupt on DONE
+#define BCM2835_BSC_C_ST        0x00000080  ///< Start transfer, 1 = Start a new transfer
+#define BCM2835_BSC_C_CLEAR_1   0x00000020  ///< Clear FIFO Clear
+#define BCM2835_BSC_C_CLEAR_2   0x00000010  ///< Clear FIFO Clear
+#define BCM2835_BSC_C_READ      0x00000001  ///<    Read transfer
+
+// Register masks for BSC_S
+#define BCM2835_BSC_S_CLKT      0x00000200  ///< Clock stretch timeout
+#define BCM2835_BSC_S_ERR       0x00000100  ///< ACK error
+#define BCM2835_BSC_S_RXF       0x00000080  ///< RXF FIFO full, 0 = FIFO is not full, 1 = FIFO is full
+#define BCM2835_BSC_S_TXE       0x00000040  ///< TXE FIFO full, 0 = FIFO is not full, 1 = FIFO is full
+#define BCM2835_BSC_S_RXD       0x00000020  ///< RXD FIFO contains data
+#define BCM2835_BSC_S_TXD       0x00000010  ///< TXD FIFO can accept data
+#define BCM2835_BSC_S_RXR       0x00000008  ///< RXR FIFO needs reading (full)
+#define BCM2835_BSC_S_TXW       0x00000004  ///< TXW FIFO needs writing (full)
+#define BCM2835_BSC_S_DONE      0x00000002  ///< Transfer DONE
+#define BCM2835_BSC_S_TA        0x00000001  ///< Transfer Active
+#define BCM2835_BSC_FIFO_SIZE   16          ///< BSC FIFO size
+#define BCM2835_CORE_CLK_HZ     250000000   ///< 250 MHz
+    
+    /// \brief bcm2835I2CClockDivider
+    /// Specifies the divider used to generate the I2C clock from the system clock.
+    /// Clock divided is based on nominal base clock rate of 250MHz
+typedef enum
+{
+    BCM2835_I2C_CLOCK_DIVIDER_2500  = 2500, ///< 2500 = 10us = 100 kHz
+    BCM2835_I2C_CLOCK_DIVIDER_626   = 626,  ///< 622 = 2.504us = 399.3610 kHz
+    BCM2835_I2C_CLOCK_DIVIDER_150   = 150,  ///< 150 = 60ns = 1.666 MHz (default at reset)
+    BCM2835_I2C_CLOCK_DIVIDER_148   = 148,  ///< 148 = 59ns = 1.689 MHz
+} bcm2835I2CClockDivider;
+
+/// \brief bcm2835I2CReasonCodes
+/// Specifies the reason codes for the bcm2835_i2c_write and bcm2835_i2c_read functions.
+typedef enum
+{
+    BCM2835_I2C_REASON_OK           = 0x00, ///< Success
+    BCM2835_I2C_REASON_ERROR_NACK   = 0x01, ///< Received a NACK
+    BCM2835_I2C_REASON_ERROR_CLKT   = 0x02, ///< Received Clock Stretch Timeout
+    BCM2835_I2C_REASON_ERROR_DATA   = 0x04, ///< Not all data is sent / received
+} bcm2835I2CReasonCodes;
+
+typedef enum
+{
+    RPI_V2_GPIO_P1_03               = 2,    ///< Version 2, Pin P1-03
+    RPI_V2_GPIO_P1_05               = 3,    ///< Version 2, Pin P1-05
+} RPiGPIOPin;
+
+#define BSC0_C        *(bsc0.addr + 0x00)
+#define BSC0_S        *(bsc0.addr + 0x01)
+#define BSC0_DLEN     *(bsc0.addr + 0x02)
+#define BSC0_A        *(bsc0.addr + 0x03)
+#define BSC0_FIFO     *(bsc0.addr + 0x04)
+
+#define BSC_C_I2CEN   (1<< 15)
+#define BSC_C_INTR    (1<< 10)
+#define BSC_C_INTT    (1<< 9)
+#define BSC_C_INTD    (1<< 8)
+#define BSC_C_ST      (1<< 7)
+#define BSC_C_CLEAR   (1<< 4)
+#define BSC_C_READ    1
+
+#define START_READ    BSC_C_I2CEN|BSC_C_ST|BSC_C_CLEAR|BSC_C_READ
+#define START_WRITE   BSC_C_I2CEN|BSC_C_ST
+
+#define BSC_S_CLKT   (1<< 9)
+#define BSC_S_ERR    (1<< 8)
+#define BSC_S_RXF    (1<< 7)
+#define BSC_S_TXE    (1<< 6)
+#define BSC_S_RXD    (1<< 5)
+#define BSC_S_TXD    (1<< 4)
+#define BSC_S_RXR    (1<< 3)
+#define BSC_S_TXW    (1<< 2)
+#define BSC_S_DONE   (1<< 1)
+#define BSC_S_TA     1
+
+#define CLEAR_STATUS    BSC_S_CLKT|BSC_S_ERR|BSC_S_DONE
+
+#define GPFSEL0     *(gpio.addr + 0)
+#define GPFSEL1     *(gpio.addr + 1)
+#define GPFSEL2     *(gpio.addr + 2)
+#define GPFSEL3     *(gpio.addr + 3)
+#define GPFSEL4     *(gpio.addr + 4)
+#define GPFSEL5     *(gpio.addr + 5)
+// Reserved @ word offset 6
+#define GPSET0      *(gpio.addr + 7)
+#define GPSET1      *(gpio.addr + 8)
+// Reserved @ word offset 9
+#define GPCLR0      *(gpio.addr + 10)
+#define GPCLR1      *(gpio.addr + 11)
+// Reserved @ word offset 12
+#define GPLEV0      *(gpio.addr + 13)
+#define GPLEV1      *(gpio.addr + 14)
+
+#define PAGESIZE    4096
+#define BLOCK_SIZE  4096
+
+/// Defines for SPI
+/// GPIO register offsets from BCM2835_SPI0_BASE.
+/// Offsets into the SPI Peripheral block in bytes per 10.5 SPI Register Map
+#define BCM2835_SPI0_CS                      0x0000 ///< SPI Master Control and Status
+#define BCM2835_SPI0_FIFO                    0x0004 ///< SPI Master TX and RX FIFOs
+#define BCM2835_SPI0_CLK                     0x0008 ///< SPI Master Clock Divider
+#define BCM2835_SPI0_DLEN                    0x000c ///< SPI Master Data Length
+#define BCM2835_SPI0_LTOH                    0x0010 ///< SPI LOSSI mode TOH
+#define BCM2835_SPI0_DC                      0x0014 ///< SPI DMA DREQ Controls
+
+// Register masks for SPI0_CS
+#define BCM2835_SPI0_CS_LEN_LONG             0x02000000 ///< Enable Long data word in Lossi mode if DMA_LEN is set
+#define BCM2835_SPI0_CS_DMA_LEN              0x01000000 ///< Enable DMA mode in Lossi mode
+#define BCM2835_SPI0_CS_CSPOL2               0x00800000 ///< Chip Select 2 Polarity
+#define BCM2835_SPI0_CS_CSPOL1               0x00400000 ///< Chip Select 1 Polarity
+#define BCM2835_SPI0_CS_CSPOL0               0x00200000 ///< Chip Select 0 Polarity
+#define BCM2835_SPI0_CS_RXF                  0x00100000 ///< RXF - RX FIFO Full
+#define BCM2835_SPI0_CS_RXR                  0x00080000 ///< RXR RX FIFO needs Reading ( full)
+#define BCM2835_SPI0_CS_TXD                  0x00040000 ///< TXD TX FIFO can accept Data
+#define BCM2835_SPI0_CS_RXD                  0x00020000 ///< RXD RX FIFO contains Data
+#define BCM2835_SPI0_CS_DONE                 0x00010000 ///< Done transfer Done
+#define BCM2835_SPI0_CS_TE_EN                0x00008000 ///< Unused
+#define BCM2835_SPI0_CS_LMONO                0x00004000 ///< Unused
+#define BCM2835_SPI0_CS_LEN                  0x00002000 ///< LEN LoSSI enable
+#define BCM2835_SPI0_CS_REN                  0x00001000 ///< REN Read Enable
+#define BCM2835_SPI0_CS_ADCS                 0x00000800 ///< ADCS Automatically Deassert Chip Select
+#define BCM2835_SPI0_CS_INTR                 0x00000400 ///< INTR Interrupt on RXR
+#define BCM2835_SPI0_CS_INTD                 0x00000200 ///< INTD Interrupt on Done
+#define BCM2835_SPI0_CS_DMAEN                0x00000100 ///< DMAEN DMA Enable
+#define BCM2835_SPI0_CS_TA                   0x00000080 ///< Transfer Active
+#define BCM2835_SPI0_CS_CSPOL                0x00000040 ///< Chip Select Polarity
+#define BCM2835_SPI0_CS_CLEAR                0x00000030 ///< Clear FIFO Clear RX and TX
+#define BCM2835_SPI0_CS_CLEAR_RX             0x00000020 ///< Clear FIFO Clear RX
+#define BCM2835_SPI0_CS_CLEAR_TX             0x00000010 ///< Clear FIFO Clear TX
+#define BCM2835_SPI0_CS_CPOL                 0x00000008 ///< Clock Polarity
+#define BCM2835_SPI0_CS_CPHA                 0x00000004 ///< Clock Phase
+#define BCM2835_SPI0_CS_CS                   0x00000003 ///< Chip Select
+
+#define BCM2835_GPFSEL0                      0x0000 ///< GPIO Function Select 0
+
+#define BCM2835_GPEDS0                       0x0040 ///< GPIO Pin Event Detect Status 0
+#define BCM2835_GPREN0                       0x004c ///< GPIO Pin Rising Edge Detect Enable 0
+#define BCM2835_GPFEN0                       0x0048 ///< GPIO Pin Falling Edge Detect Enable 0
+#define BCM2835_GPHEN0                       0x0064 ///< GPIO Pin High Detect Enable 0
+#define BCM2835_GPLEN0                       0x0070 ///< GPIO Pin Low Detect Enable 0
+
+static int REV = 0;
+
+typedef enum {
+    // Not connected
+    NC = (int)0xFFFFFFFF,
+
+    // GPIO names
+    gpio2       = 2,
+    gpio3       = 3,
+    gpio4       = 4,
+    gpio5       = 5,
+    gpio6       = 6,
+    gpio7       = 7,
+    gpio8       = 8,
+    gpio9       = 9,
+    gpio10      = 10,
+    gpio11      = 11,
+    gpio12      = 12,
+    gpio13      = 13,
+    gpio14      = 14,
+    gpio15      = 15,
+    gpio16      = 16,
+    gpio17      = 17,
+    gpio18      = 18,
+    gpio19      = 19,
+    gpio20      = 20,
+    gpio21      = 21,
+    gpio22      = 22,
+    gpio23      = 23,
+    gpio24      = 24,
+    gpio25      = 25,
+    gpio26      = 26,
+    gpio27      = 27,
+
+    // Rapsberry Pi pin names
+    p3          = gpio2,
+    p5          = gpio3,
+    p7          = gpio4,
+    p8          = gpio14,
+    p10         = gpio15,
+    p11         = gpio17,
+    p12         = gpio18,
+    p13         = gpio27,
+    p15         = gpio22,
+    p16         = gpio23,
+    p18         = gpio24,
+    p19         = gpio10,
+    p21         = gpio9,
+    p22         = gpio25,
+    p23         = gpio11,
+    p24         = gpio8,
+    p26         = gpio7,
+    p29         = gpio5,
+    p31         = gpio6,
+    p32         = gpio12,
+    p33         = gpio13,
+    p35         = gpio19,
+    p36         = gpio16,
+    p37         = gpio26,
+    p38         = gpio20,
+    p40         = gpio21,
+
+    // Extension board V2.1 pin names
+    P0          = gpio17,
+    P1          = gpio18,
+    P2          = gpio27,
+    P3          = gpio22,
+    P4          = gpio23,
+    P5          = gpio24,
+    P6          = gpio25,
+    P7          = gpio4,
+    CE1         = gpio7,
+    CE0         = gpio8,
+    CS          = gpio8,
+    SCLK        = gpio11,
+    MISO        = gpio9,
+    MOSI        = gpio10,
+    RXD         = gpio15,
+    TXD         = gpio14,
+    SCL         = gpio3,
+    SDA         = gpio2,
+    PWM         = gpio18
+
+} PinName;
+
+typedef enum
+{
+    PIN_INPUT,
+    PIN_OUTPUT
+} PinDirection;
+
+typedef enum
+{
+    PullNone,
+    PullUp,
+    PullDown,
+    OpenDrain,
+    PullDefault = PullNone
+} PinMode;
+
+typedef void (*FunctionPointer) (void);
+
+/// \brief bcm2835SPIBitOrder
+/// Specifies the SPI data bit ordering
+typedef enum
+{
+    LSBFIRST = 0,  ///< LSB First
+    MSBFIRST = 1///< MSB First
+}bcm2835SPIBitOrder;
+
+/// \brief bcm2835SPIMode
+/// Specify the SPI data mode
+typedef enum
+{
+    SPI_MODE0 = 0,  ///< CPOL = 0, CPHA = 0
+    SPI_MODE1 = 1,  ///< CPOL = 0, CPHA = 1
+    SPI_MODE2 = 2,  ///< CPOL = 1, CPHA = 0
+    SPI_MODE3 = 3,  ///< CPOL = 1, CPHA = 1
+}bcm2835SPIMode;
+
+/// \brief bcm2835SPIChipSelect
+/// Specify the SPI chip select pin(s)
+typedef enum
+{
+    SPI_CS0 = 0,     ///< Chip Select 0
+    SPI_CS1 = 1,     ///< Chip Select 1
+    SPI_CS2 = 2,     ///< Chip Select 2 (ie pins CS1 and CS2 are asserted)
+    SPI_CS_NONE = 3, ///< No CS, control it yourself
+} bcm2835SPIChipSelect;
+
+/// \brief bcm2835SPIClockDivider
+/// Specifies the divider used to generate the SPI clock from the system clock.
+/// Figures below give the divider, clock period and clock frequency.
+typedef enum
+{
+    SPI_CLOCK_DIV65536 = 0,       ///< 65536 = 256us = 4kHz
+    SPI_CLOCK_DIV32768 = 32768,   ///< 32768 = 126us = 8kHz
+    SPI_CLOCK_DIV16384 = 16384,   ///< 16384 = 64us = 15.625kHz
+    SPI_CLOCK_DIV8192  = 8192,    ///< 8192 = 32us = 31.25kHz
+    SPI_CLOCK_DIV4096  = 4096,    ///< 4096 = 16us = 62.5kHz
+    SPI_CLOCK_DIV2048  = 2048,    ///< 2048 = 8us = 125kHz
+    SPI_CLOCK_DIV1024  = 1024,    ///< 1024 = 4us = 250kHz
+    SPI_CLOCK_DIV512   = 512,     ///< 512 = 2us = 500kHz
+    SPI_CLOCK_DIV256   = 256,     ///< 256 = 1us = 1MHz
+    SPI_CLOCK_DIV128   = 128,     ///< 128 = 500ns = = 2MHz
+    SPI_CLOCK_DIV64    = 64,      ///< 64 = 250ns = 4MHz
+    SPI_CLOCK_DIV32    = 32,      ///< 32 = 125ns = 8MHz
+    SPI_CLOCK_DIV16    = 16,      ///< 16 = 50ns = 20MHz
+    SPI_CLOCK_DIV8     = 8,       ///< 8 = 25ns = 40MHz
+    SPI_CLOCK_DIV4     = 4,       ///< 4 = 12.5ns 80MHz
+    SPI_CLOCK_DIV2     = 2,       ///< 2 = 6.25ns = 160MHz
+    SPI_CLOCK_DIV1     = 1,       ///< 0 = 256us = 4kHz
+} bcm2835SPIClockDivider;
+
+typedef enum
+{
+    BCM2835_GPIO_FSEL_INPT  = 0b000,   ///< Input
+    BCM2835_GPIO_FSEL_OUTP  = 0b001,   ///< Output
+    BCM2835_GPIO_FSEL_ALT0  = 0b100,   ///< Alternate function 0
+    BCM2835_GPIO_FSEL_ALT1  = 0b101,   ///< Alternate function 1
+    BCM2835_GPIO_FSEL_ALT2  = 0b110,   ///< Alternate function 2
+    BCM2835_GPIO_FSEL_ALT3  = 0b111,   ///< Alternate function 3
+    BCM2835_GPIO_FSEL_ALT4  = 0b011,   ///< Alternate function 4
+    BCM2835_GPIO_FSEL_ALT5  = 0b010,   ///< Alternate function 5
+    BCM2835_GPIO_FSEL_MASK  = 0b111    ///< Function select bits mask
+} bcm2835FunctionSelect;
+
+namespace unistd {
+    //All functions of unistd.h must be called like this: unistd::the_function()
+    #include <unistd.h>
+}
+
+enum Representation{
+    BIN,
+    OCT,
+    DEC,
+    HEX,
+    BYTE
+};
+
+typedef enum {
+    LOW     = 0,
+    HIGH    = 1,
+    RISING  = 2,
+    FALLING = 3,
+    BOTH    = 4
+} Digivalue;
+//
+/*$on*/
+//
+typedef bool boolean;
+typedef unsigned char   byte;
+
+struct bcm2835_peripheral
+{
+    unsigned long           addr_p;
+    int                     mem_fd;
+    void*                   map;
+    volatile unsigned int*  addr;
+};
+
+struct ThreadArg
+{
+    void (*func) ();
+    int pin;
+};
+
+void    wait(float s);
+void    wait_ms(int ms);
+void    wait_us(int us);
+
+void    gpio_dir(PinName pin, PinDirection direction);
+void    gpio_mode(PinName pin, PinMode mode);
+void    gpio_write(PinName pin, int value);
+int     gpio_read(PinName pin);
+uint8_t shiftIn(PinName dPin, PinName cPin, bcm2835SPIBitOrder order);
+void    shiftOut(PinName dPin, PinName cPin, bcm2835SPIBitOrder order, uint8_t val);
+void    attachInterrupt(PinName p, void (*f) (), Digivalue m);
+void    detachInterrupt(PinName p);
+
+class   DigitalIn
+{
+public:
+    DigitalIn(PinName pin) : gpio(pin)                  { gpio_dir(gpio, PIN_INPUT); gpio_write(gpio, LOW); }
+    DigitalIn(PinName pin, PinMode mode) : gpio(pin)    { gpio_dir(gpio, PIN_INPUT); gpio_mode(pin, mode); }
+    int     read(void)                                  { return gpio_read(gpio); }
+    void    mode(PinMode mode)                          { mode == PullUp ? gpio_write(gpio, HIGH) : gpio_write(gpio, LOW); }
+    void    attach(FunctionPointer fptr, Digivalue m)   { attachInterrupt(gpio, fptr, m); }
+    operator int(void)                                  { return read(); }
+protected:
+    PinName gpio;
+};
+
+class   DigitalOut
+{
+public:
+    DigitalOut(PinName pin) : gpio(pin)                 { gpio_dir(gpio, PIN_OUTPUT); }
+    DigitalOut(PinName pin, int value) : gpio(pin)      { gpio_dir(gpio, PIN_OUTPUT); gpio_write(gpio, value); }
+    void                    write(int value)            { gpio_write(gpio, value); }
+    int                     read(void)                  { return gpio_read(gpio); }
+    DigitalOut &operator    =(int value)                { write(value); return *this; }
+    DigitalOut &operator    =(DigitalOut& rhs)          { write(rhs.read()); return *this; }
+    operator int(void)                                  { return read(); }
+protected:
+    PinName gpio;
+};
+
+class   DigitalInOut
+{
+public:
+    DigitalInOut(PinName pin) : gpio(pin)       { PinMode(PIN_INPUT); }
+    DigitalInOut(PinName pin, PinDirection direction, PinMode mode, int value) :
+    gpio(pin) {
+        if(direction == PIN_INPUT) {
+            gpio_dir(gpio, PIN_INPUT);
+            if(mode == PullUp)
+                gpio_write(gpio, HIGH);
+            else
+                gpio_write(gpio, LOW);
+        }
+        else {
+            gpio_dir(gpio, PIN_OUTPUT);
+            gpio_write(gpio, value);
+        }
+    }
+    void                    write(int value)    { gpio_write(gpio, value); }
+    int                     read(void)          { return gpio_read(gpio); }
+    void                    output(void)        { gpio_dir(gpio, PIN_OUTPUT); }
+    void                    input(void)         { gpio_dir(gpio, PIN_INPUT); }
+    DigitalInOut &operator  =(int value)        { write(value); return *this; }
+    operator int(void)                          { return read(); }
+protected:
+    PinName gpio;
+};
+
+class   InterruptIn
+{
+public:
+    InterruptIn(PinName pin) :
+    _digitalIn(pin) { }
+
+    void    rise(FunctionPointer fptr)  { _digitalIn.attach(fptr, RISING); }
+    void    fall(FunctionPointer fptr)  { _digitalIn.attach(fptr, FALLING); }
+    void    both(FunctionPointer fptr)  { _digitalIn.attach(fptr, BOTH); }
+    int     read(void)                  { return _digitalIn.read(); }
+    void    mode(PinMode mode)          { _digitalIn.mode(mode); }
+    operator int(void)                  { return _digitalIn.read(); }
+protected:
+    DigitalIn   _digitalIn;
+};
+
+class   Serial
+{
+private:
+    int             sd, status;
+    const char*     serialPort;
+    unsigned char   c;
+    struct termios  options;
+    int             speed;
+    long            timeOut;
+    timespec        time1, time2;
+    timespec        timeDiff(timespec start, timespec close);
+    char*           int2bin(int i);
+    char*           int2hex(int i);
+    char*           int2oct(int i);
+public:
+    Serial(void);
+    void    baud(int serialSpeed);
+    int     readable(void);
+    char    read(void);
+    int     readBytes(char message[], int size);
+    int     readBytesUntil(char character, char buffer[], int length);
+    bool    find(const char* target);
+    bool    findUntil(const char* target, const char* terminal);
+    long    parseInt(void);
+    float   parseFloat(void);
+    char    peek(void);
+    void    printf(const char* format, ...);
+    int     write(uint8_t message);
+    int     write(const char* message);
+    int     write(char* message, int size);
+    void    flush(void);
+    void    setTimeout(long millis);
+    void    close(void);
+};
+
+class   I2C
+{
+private:
+    int     i2c_byte_wait_us;
+    int     i2c_bytes_to_read;
+    void    dump_bsc_status(void);
+
+    void    wait_i2c_done(void);
+public:
+    I2C(void);
+    void            begin(void);
+    void            beginTransmission(unsigned char address);
+    void            write(char data);
+    uint8_t         write(const char* buf, uint32_t len);
+    void            endTransmission(void);
+    void            requestFrom(unsigned char address, int quantity);
+    unsigned char   read(void);
+    uint8_t         read(char* buf);
+    uint8_t         read_rs(char* regaddr, char* buf, uint32_t len);
+};
+
+class   SPI
+{
+public:
+    SPI(void);
+    void    begin(void);
+    void    end(void);
+    void    setBitOrder(uint8_t order);
+    void    setClockDivider(uint16_t divider);
+    void    setDataMode(uint8_t mode);
+    void    chipSelect(uint8_t cs);
+    void    setChipSelectPolarity(uint8_t cs, uint8_t active);
+    uint8_t transfer(uint8_t value);
+    void    transfernb(char* tbuf, char* rbuf, uint32_t len);
+};
+
+class   Peripheral
+{
+private:
+    int     memfd;
+    int     map_peripheral(struct bcm2835_peripheral* p);
+    void    unmap_peripheral(struct bcm2835_peripheral* p);
+public:
+    Peripheral(void);
+    ~Peripheral(void);
+};
+
+/* Helper functions */
+int         getBoardRev(void);
+uint32_t*   mapmem(const char* msg, size_t size, int fd, off_t off);
+void        setBoardRev(int rev);
+pthread_t*  getThreadIdFromPin(int pin);
+void*       threadFunction(void* args);
+uint32_t    bcm2835_peri_read(volatile uint32_t* paddr);
+uint32_t    bcm2835_peri_read_nb(volatile uint32_t* paddr);
+void        bcm2835_peri_write(volatile uint32_t* paddr, uint32_t value);
+void        bcm2835_peri_write_nb(volatile uint32_t* paddr, uint32_t value);
+void        bcm2835_peri_set_bits(volatile uint32_t* paddr, uint32_t value, uint32_t mask);
+void        bcm2835_gpio_fsel(uint8_t pin, uint8_t mode);
+
+#endif // MBEDPI_H