ardupilot/libraries/FastSerial/FastSerial.cpp

// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
//
// Interrupt-driven serial transmit/receive library.
//
//      Copyright (c) 2010 Michael Smith. All rights reserved.
//
// Receive and baudrate calculations derived from the Arduino 
// HardwareSerial driver:
//
//      Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
//
// Transmit algorithm inspired by work:
//
//      Code Jose Julio and Jordi Munoz. DIYDrones.com
//
//      This library is free software; you can redistribute it and/or
//      modify it under the terms of the GNU Lesser General Public
//      License as published by the Free Software Foundation; either
//      version 2.1 of the License, or (at your option) any later version.
//
//      This library 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
//      Lesser General Public License for more details.
//
//      You should have received a copy of the GNU Lesser General Public
//      License along with this library; if not, write to the Free Software
//      Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
//


//#include "../AP_Common/AP_Common.h"
#include "FastSerial.h"
#include "WProgram.h"

#if   defined(UDR3)
# define FS_MAX_PORTS   4
#elif defined(UDR2)
# define FS_MAX_PORTS   3
#elif defined(UDR1)
# define FS_MAX_PORTS   2
#else
# define FS_MAX_PORTS   1
#endif

FastSerial::Buffer	__FastSerial__rxBuffer[FS_MAX_PORTS];
FastSerial::Buffer	__FastSerial__txBuffer[FS_MAX_PORTS];

// Default buffer sizes
#define RX_BUFFER_SIZE  128
#define TX_BUFFER_SIZE  64
#define BUFFER_MAX      512

// Constructor /////////////////////////////////////////////////////////////////

FastSerial::FastSerial(const uint8_t portNumber,
                       volatile uint8_t *ubrrh,
                       volatile uint8_t *ubrrl,
                       volatile uint8_t *ucsra,
                       volatile uint8_t *ucsrb,
                       const uint8_t u2x,
                       const uint8_t portEnableBits,
                       const uint8_t portTxBits)
{
        _ubrrh = ubrrh;
        _ubrrl = ubrrl;
        _ucsra = ucsra;
        _ucsrb = ucsrb;
        _u2x   = u2x;
        _portEnableBits = portEnableBits;
        _portTxBits     = portTxBits;

        // init buffers
        _rxBuffer = &__FastSerial__rxBuffer[portNumber];
        _txBuffer->head = _txBuffer->tail = 0;
        _txBuffer = &__FastSerial__txBuffer[portNumber];
        _rxBuffer->head = _rxBuffer->tail = 0;
}

// Public Methods //////////////////////////////////////////////////////////////

void FastSerial::begin(long baud)
{
        unsigned int    rxb, txb;

        // If we are re-configuring an already-open port, preserve the
        // existing buffer sizes.
        if (_open) {
                rxb = _rxBuffer->mask + 1;
                txb = _txBuffer->mask + 1;
        } else {
                rxb = RX_BUFFER_SIZE;
                txb = TX_BUFFER_SIZE;
        }

        begin(baud, RX_BUFFER_SIZE, TX_BUFFER_SIZE);
}

void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace)
{
        uint16_t        ubrr;
        bool            use_u2x = false;
        int             ureg, u2;
        long            breg, b2, dreg, d2;
       
        // if we are currently open, close and restart
        if (_open)
                end();

        // allocate buffers
        if (!_allocBuffer(_rxBuffer, rxSpace ? : RX_BUFFER_SIZE) ||
            !_allocBuffer(_txBuffer, txSpace ? : TX_BUFFER_SIZE)) {
                end();
                return;                 // couldn't allocate buffers - fatal
        }
        _open = true;

        // U2X mode is needed for bitrates higher than (F_CPU / 16)
        if (baud > F_CPU / 16) {
                use_u2x = true;
                ubrr = F_CPU / (8 * baud) - 1;
        } else {
                // Determine whether u2x mode would give a closer
                // approximation of the desired speed.
    
                // ubrr for non-2x mode, corresponding baudrate and delta
                ureg = F_CPU / 16 / baud - 1;
                breg = F_CPU / 16 / (ureg + 1);
                dreg = abs(baud - breg);
                
                // ubrr for 2x mode, corresponding bitrate and delta
                u2   = F_CPU / 8 / baud - 1;
                b2   = F_CPU / 8 / (u2 + 1); 
                d2   = abs(baud - b2);

                // Pick the setting that gives the smallest rate
                // error, preferring non-u2x mode if the delta is
                // identical.
                if (dreg <= d2) {
                        ubrr = ureg;
                } else {
                        ubrr = u2;
                        use_u2x = true;
                }                
        }
  
        *_ucsra = use_u2x ? _BV(_u2x) : 0;
        *_ubrrh = ubrr >> 8;
        *_ubrrl = ubrr;
        *_ucsrb |= _portEnableBits;
}

void FastSerial::end()
{
        *_ucsrb &= ~(_portEnableBits | _portTxBits);

        _freeBuffer(_rxBuffer);
        _freeBuffer(_txBuffer);
        _open = false;
}

int
FastSerial::available(void)
{
        if (!_open)
                return(-1);
        return((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask);
}

int
FastSerial::read(void)
{
        uint8_t         c;

        // if the head and tail are equal, the buffer is empty
        if (!_open || (_rxBuffer->head == _rxBuffer->tail))
                return(-1);

        // pull character from tail
        c = _rxBuffer->bytes[_rxBuffer->tail];
        _rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask;

        return(c);
}

int
FastSerial::peek(void)
{

        // if the head and tail are equal, the buffer is empty
        if (!_open || (_rxBuffer->head == _rxBuffer->tail))
                return(-1);

        // pull character from tail
        return(_rxBuffer->bytes[_rxBuffer->tail]);
}


void
FastSerial::flush(void)
{
        // don't reverse this or there may be problems if the RX interrupt
        // occurs after reading the value of _rxBuffer->head but before writing
        // the value to _rxBuffer->tail; the previous value of head
        // may be written to tail, making it appear as if the buffer
        // don't reverse this or there may be problems if the RX interrupt
        // occurs after reading the value of head but before writing
        // the value to tail; the previous value of rx_buffer_head
        // may be written to tail, making it appear as if the buffer
        // were full, not empty.
        _rxBuffer->head = _rxBuffer->tail;

        // don't reverse this or there may be problems if the TX interrupt
        // occurs after reading the value of _txBuffer->tail but before writing
        // the value to _txBuffer->head.
        _txBuffer->tail = _rxBuffer->head;
}

void
FastSerial::write(uint8_t c)
{
        int16_t         i;

        if (!_open)                     // drop bytes if not open
                return;

        // wait for room in the tx buffer
        i = (_txBuffer->head + 1) & _txBuffer->mask;
        while (i == _txBuffer->tail)
                ;

        // add byte to the buffer
        _txBuffer->bytes[_txBuffer->head] = c;
        _txBuffer->head = i;

        // enable the data-ready interrupt, as it may be off if the buffer is empty
        *_ucsrb |= _portTxBits;
}

// Buffer management ///////////////////////////////////////////////////////////

bool
FastSerial::_allocBuffer(Buffer *buffer, unsigned int size)
{
        uint8_t shift;

        // init buffer state
        buffer->head = buffer->tail = 0;

        // Compute the power of 2 greater or equal to the requested buffer size
        // and then a mask to simplify wrapping operations.  Using __builtin_clz
        // would seem to make sense, but it uses a 256(!) byte table.
        // Note that we ignore requests for more than BUFFER_MAX space.
        for (shift = 1; (1U << shift) < min(BUFFER_MAX, size); shift++)
                ;
        buffer->mask = (1 << shift) - 1;

        // allocate memory for the buffer - if this fails, we fail
        buffer->bytes = (uint8_t *)malloc(buffer->mask + 1);

        return(buffer->bytes != NULL);
}

void
FastSerial::_freeBuffer(Buffer *buffer)
{
        buffer->head = buffer->tail = 0;
        buffer->mask = 0;
        if (NULL != buffer->bytes) {
                free(buffer->bytes);
                buffer->bytes = NULL;
        }
}