From 6268189d31bb8c81bfd30ce64e6fed82789708b0 Mon Sep 17 00:00:00 2001 From: "DrZiplok@gmail.com" Date: Tue, 7 Sep 2010 05:41:35 +0000 Subject: [PATCH] Dynamically allocate serial buffers at ::begin time. Allow buffer sizes to be dynamically set. This provides an opportunity for saving memory in the case of ports that do little or no work (e.g. the console) as well as increasing buffering for ports that receive large amounts of data in a short time (e.g. high-bitrate NMEA). git-svn-id: https://arducopter.googlecode.com/svn/trunk@425 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/FastSerial/FastSerial.cpp | 83 +++++++++++++++++++++++++---- libraries/FastSerial/FastSerial.h | 23 ++++---- 2 files changed, 85 insertions(+), 21 deletions(-) diff --git a/libraries/FastSerial/FastSerial.cpp b/libraries/FastSerial/FastSerial.cpp index c312d1241f..40ee85eee2 100644 --- a/libraries/FastSerial/FastSerial.cpp +++ b/libraries/FastSerial/FastSerial.cpp @@ -41,8 +41,10 @@ FastSerial *__FastSerial__ports[FS_MAX_PORTS]; -#define RX_BUFFER_SIZE sizeof(((FastSerial::RXBuffer *)1)->bytes) -#define TX_BUFFER_SIZE sizeof(((FastSerial::TXBuffer *)1)->bytes) +// Default buffer sizes +#define RX_BUFFER_SIZE 128 +#define TX_BUFFER_SIZE 64 +#define BUFFER_MAX 512 // Interrupt handlers ////////////////////////////////////////////////////////// @@ -110,10 +112,27 @@ FastSerial::FastSerial(const uint8_t portNumber, // Public Methods ////////////////////////////////////////////////////////////// void FastSerial::begin(long baud) +{ + begin(baud, RX_BUFFER_SIZE, TX_BUFFER_SIZE); +} + +void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace) { uint16_t baud_setting; bool use_u2x; + // 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 baud rates higher than (CPU Hz / 16) if (baud > F_CPU / 16) { use_u2x = true; @@ -147,13 +166,18 @@ void FastSerial::begin(long baud) void FastSerial::end() { *_ucsrb &= ~(_portEnableBits | _portTxBits); -} + _freeBuffer(&_rxBuffer); + _freeBuffer(&_txBuffer); + _open = false; +} int FastSerial::available(void) { - return((RX_BUFFER_SIZE + _rxBuffer.head - _rxBuffer.tail) % RX_BUFFER_SIZE); + if (!_open) + return(-1); + return((_rxBuffer.head - _rxBuffer.tail) & _rxBuffer.mask); } int @@ -162,12 +186,12 @@ FastSerial::read(void) uint8_t c; // if the head and tail are equal, the buffer is empty - if (_rxBuffer.head == _rxBuffer.tail) + if (!_open || (_rxBuffer.head == _rxBuffer.tail)) return(-1); // pull character from tail c = _rxBuffer.bytes[_rxBuffer.tail]; - _rxBuffer.tail = (_rxBuffer.tail + 1) % RX_BUFFER_SIZE; + _rxBuffer.tail = (_rxBuffer.tail + 1) & _rxBuffer.mask; return(c); } @@ -195,10 +219,13 @@ FastSerial::flush(void) void FastSerial::write(uint8_t c) { - uint8_t i; + int16_t i; + + if (!_open) // drop bytes if not open + return; // wait for room in the tx buffer - i = (_txBuffer.head + 1) % TX_BUFFER_SIZE; + i = (_txBuffer.head + 1) & _txBuffer.mask; while (i == _txBuffer.tail) ; @@ -272,7 +299,7 @@ FastSerial::receive(uint8_t c) // current location of the tail), we're about to overflow the buffer // and so we don't write the character or advance the head. - i = (_rxBuffer.head + 1) % RX_BUFFER_SIZE; + i = (_rxBuffer.head + 1) & _rxBuffer.mask; if (i != _rxBuffer.tail) { _rxBuffer.bytes[_rxBuffer.head] = c; _rxBuffer.head = i; @@ -285,7 +312,7 @@ FastSerial::transmit(void) // if the buffer is not empty, send the next byte if (_txBuffer.head != _txBuffer.tail) { *_udr = _txBuffer.bytes[_txBuffer.tail]; - _txBuffer.tail = (_txBuffer.tail + 1) % TX_BUFFER_SIZE; + _txBuffer.tail = (_txBuffer.tail + 1) & _txBuffer.mask; } // if the buffer is (now) empty, disable the interrupt @@ -293,3 +320,39 @@ FastSerial::transmit(void) *_ucsrb &= ~_portTxBits; } +// Buffer management /////////////////////////////////////////////////////////// + +bool +FastSerial::_allocBuffer(Buffer *buffer, unsigned int size) +{ + uint8_t shift; + + // init buffer state + buffer->head = buffer->tail = 0; + + // cap the buffer size + if (size > BUFFER_MAX) + size = BUFFER_MAX; + + // compute the power of 2 greater or equal to the requested buffer size + // and then a mask to simplify wrapping operations + shift = 16 - __builtin_clz(size - 1); + 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; + } +} + diff --git a/libraries/FastSerial/FastSerial.h b/libraries/FastSerial/FastSerial.h index 4e425eb035..65abedfd96 100644 --- a/libraries/FastSerial/FastSerial.h +++ b/libraries/FastSerial/FastSerial.h @@ -52,6 +52,7 @@ #include #include +#include #include #include @@ -99,6 +100,7 @@ public: // Serial API void begin(long baud); + void begin(long baud, unsigned int rxSpace, unsigned int txSpace); void end(void); int available(void); int read(void); @@ -129,18 +131,17 @@ private: uint8_t _u2x; // ring buffers - struct RXBuffer { - volatile uint8_t head; - uint8_t tail; - uint8_t bytes[128]; // size must be power of 2 for best results + struct Buffer { + volatile int16_t head, tail; + uint8_t *bytes; + uint16_t mask; }; - struct TXBuffer { - uint8_t head; - volatile uint8_t tail; - uint8_t bytes[64]; // size must be power of 2 for best results - }; - RXBuffer _rxBuffer; - TXBuffer _txBuffer; + Buffer _rxBuffer; + Buffer _txBuffer; + bool _open; + + bool _allocBuffer(Buffer *buffer, unsigned int size); + void _freeBuffer(Buffer *buffer); // stdio emulation FILE _fd;