mbedPi

File content as of revision 0:91392e1f8551:

/*
 *  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();