Remove the method call from the interrupt handlers. This brings the rx and tx interrupt paths down to < 60 instructions worst case, or ~4us.

At 115200 we expect ~100us between interrupts, or around 5% CPU overhead.  4us latency is probably acceptable for servo signal jitter too if we decide to consider using the Arduino Servo library.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@513 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/FastSerial/FastSerial.cpp

@@ -39,7 +39,8 @@
 # define FS_MAX_PORTS   1
 #endif
 
-FastSerial       *__FastSerial__ports[FS_MAX_PORTS];
+FastSerial::Buffer	__FastSerial__rxBuffer[FS_MAX_PORTS];
+FastSerial::Buffer	__FastSerial__txBuffer[FS_MAX_PORTS];
 
 // Default buffer sizes
 #define RX_BUFFER_SIZE  128
@@ -96,11 +97,10 @@ FastSerial::FastSerial(const uint8_t portNumber,
         _portTxBits     = portTxBits;
 
         // init buffers
-        _txBuffer.head = _txBuffer.tail = 0;
-        _rxBuffer.head = _rxBuffer.tail = 0;
-
-        // claim the port
-        __FastSerial__ports[portNumber] = this;
+        _rxBuffer = &__FastSerial__rxBuffer[portNumber];
+        _txBuffer->head = _txBuffer->tail = 0;
+        _txBuffer = &__FastSerial__txBuffer[portNumber];
+        _rxBuffer->head = _rxBuffer->tail = 0;
 
         // init stdio
         fdev_setup_stream(&_fd, &FastSerial::_putchar, NULL, _FDEV_SETUP_WRITE);
@@ -126,8 +126,8 @@ void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace)
                 end();
 
         // allocate buffers
-        if (!_allocBuffer(&_rxBuffer, rxSpace ? : RX_BUFFER_SIZE) ||
-            !_allocBuffer(&_txBuffer, txSpace ? : TX_BUFFER_SIZE)) {
+        if (!_allocBuffer(_rxBuffer, rxSpace ? : RX_BUFFER_SIZE) ||
+            !_allocBuffer(_txBuffer, txSpace ? : TX_BUFFER_SIZE)) {
                 end();
                 return;                 // couldn't allocate buffers - fatal
         }
@@ -167,8 +167,8 @@ void FastSerial::end()
 {
         *_ucsrb &= ~(_portEnableBits | _portTxBits);
 
-        _freeBuffer(&_rxBuffer);
-        _freeBuffer(&_txBuffer);
+        _freeBuffer(_rxBuffer);
+        _freeBuffer(_txBuffer);
         _open = false;
 }
 
@@ -177,7 +177,7 @@ FastSerial::available(void)
 {
         if (!_open)
                 return(-1);
-        return((_rxBuffer.head - _rxBuffer.tail) & _rxBuffer.mask);
+        return((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask);
 }
 
 int
@@ -186,12 +186,12 @@ FastSerial::read(void)
         uint8_t         c;
 
         // if the head and tail are equal, the buffer is empty
-        if (!_open || (_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) & _rxBuffer.mask;
+        c = _rxBuffer->bytes[_rxBuffer->tail];
+        _rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask;
 
         return(c);
 }
@@ -200,20 +200,20 @@ 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
+        // 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;
+        _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;
+        // occurs after reading the value of _txBuffer->tail but before writing
+        // the value to _txBuffer->head.
+        _txBuffer->tail = _rxBuffer->head;
 }
 
 void
@@ -225,13 +225,13 @@ FastSerial::write(uint8_t c)
                 return;
 
         // wait for room in the tx buffer
-        i = (_txBuffer.head + 1) & _txBuffer.mask;
-        while (i == _txBuffer.tail)
+        i = (_txBuffer->head + 1) & _txBuffer->mask;
+        while (i == _txBuffer->tail)
                 ;
 
         // add byte to the buffer
-        _txBuffer.bytes[_txBuffer.head] = c;
-        _txBuffer.head = i;
+        _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;
@@ -287,39 +287,6 @@ FastSerial::printf_P(const char *fmt, ...)
         return(i);
 }
 
-// Interrupt methods ///////////////////////////////////////////////////////////
-
-void
-FastSerial::receive(uint8_t c)
-{
-        uint8_t         i;
-
-        // if we should be storing the received character into the location
-        // just before the tail (meaning that the head would advance to the
-        // 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) & _rxBuffer.mask;
-        if (i != _rxBuffer.tail) {
-                _rxBuffer.bytes[_rxBuffer.head] = c;
-                _rxBuffer.head = i;
-        }
-}
-
-void
-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) & _txBuffer.mask;
-        }
-
-        // if the buffer is (now) empty, disable the interrupt
-        if (_txBuffer.head == _txBuffer.tail)
-                *_ucsrb &= ~_portTxBits;
-}
-
 // Buffer management ///////////////////////////////////////////////////////////
 
 bool
@@ -330,13 +297,12 @@ FastSerial::_allocBuffer(Buffer *buffer, unsigned int size)
         // 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);
+        // 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

libraries/FastSerial/FastSerial.h

@@ -37,9 +37,6 @@
 // wish to use.  This is less friendly than the stock Arduino driver,
 // but it saves ~200 bytes for every unused port.
 //
-// To adjust the transmit/receive buffer sizes, change the size of the
-// 'bytes' member in the RXBuffer and TXBuffer structures.
-//
 
 #ifndef FastSerial_h
 #define FastSerial_h
@@ -113,9 +110,12 @@ public:
         int             printf_P(const char *fmt, ...);
         FILE            *getfd(void) { return &_fd; };
 
-        // Interrupt methods
-        void            receive(uint8_t c);
-        void            transmit(void);
+        // public so the interrupt handlers can see it
+        struct Buffer {
+                volatile uint16_t head, tail;
+                uint16_t        mask;
+                uint8_t         *bytes;
+        };
 
 private:
         // register accessors
@@ -131,13 +131,8 @@ private:
         uint8_t         _u2x;
 
         // ring buffers
-        struct Buffer {
-                volatile int16_t head, tail;
-                uint8_t         *bytes;
-                uint16_t        mask;
-        };
-        Buffer          _rxBuffer;
-        Buffer          _txBuffer;
+        Buffer          *_rxBuffer;
+        Buffer          *_txBuffer;
         bool            _open;
 
         bool            _allocBuffer(Buffer *buffer, unsigned int size);
@@ -150,19 +145,38 @@ private:
 };
 
 // Used by the per-port interrupt vectors
-extern FastSerial       *__FastSerial__ports[];
+extern FastSerial::Buffer	__FastSerial__rxBuffer[];
+extern FastSerial::Buffer	__FastSerial__txBuffer[];
 
 // Generic Rx/Tx vectors for a serial port - needs to know magic numbers
-#define FastSerialHandler(_PORT, _RXVECTOR, _TXVECTOR, _UDR)    \
-ISR(_RXVECTOR, ISR_BLOCK)                                       \
-{                                                               \
-        unsigned char c = _UDR;                                 \
-        __FastSerial__ports[_PORT]->receive(c);                 \
-}                                                               \
-ISR(_TXVECTOR, ISR_BLOCK)                                       \
-{                                                               \
-        __FastSerial__ports[_PORT]->transmit();                 \
-}                                                               \
+#define FastSerialHandler(_PORT, _RXVECTOR, _TXVECTOR, _UDR, _UCSRB, _TXBITS) \
+ISR(_RXVECTOR, ISR_BLOCK)                                               \
+{                                                                       \
+        uint8_t c;                                                      \
+        int16_t i;                                                      \
+                                                                        \
+        /* read the byte as quickly as possible */                      \
+        c = _UDR;                                                       \
+        /* work out where the head will go next */                      \
+        i = (__FastSerial__rxBuffer[_PORT].head + 1) & __FastSerial__rxBuffer[_PORT].mask; \
+        /* decide whether we have space for another byte */             \
+        if (i != __FastSerial__rxBuffer[_PORT].tail) {                  \
+                /* we do, move the head */                              \
+                __FastSerial__rxBuffer[_PORT].bytes[__FastSerial__rxBuffer[_PORT].head] = c; \
+                __FastSerial__rxBuffer[_PORT].head = i;                 \
+        }                                                               \
+}                                                                       \
+ISR(_TXVECTOR, ISR_BLOCK)                                               \
+{                                                                       \
+        /* if we have taken an interrupt we are ready to transmit the next byte */ \
+        _UDR = __FastSerial__txBuffer[_PORT].bytes[__FastSerial__txBuffer[_PORT].tail]; \
+        /* increment the tail */                                        \
+        __FastSerial__txBuffer[_PORT].tail =                            \
+                (__FastSerial__txBuffer[_PORT].tail + 1) & __FastSerial__txBuffer[_PORT].mask; \
+        /* if there are no more bytes to send, disable the interrupt */ \
+        if (__FastSerial__txBuffer[_PORT].head == __FastSerial__txBuffer[_PORT].tail) \
+                _UCSRB &= ~_TXBITS;                                     \
+}                                                                       \
 struct hack
 
 // Macros defining serial ports
@@ -177,7 +191,7 @@ struct hack
                              U2X0,                                      \
                              (_BV(RXEN0) |  _BV(TXEN0) | _BV(RXCIE0)),  \
                              (_BV(UDRIE0)));                            \
-	FastSerialHandler(0, SIG_USART0_RECV, SIG_USART0_DATA, UDR0)
+	FastSerialHandler(0, SIG_USART0_RECV, SIG_USART0_DATA, UDR0, UCSR0B, _BV(UDRIE0))
 #if defined(__AVR_ATmega1280__)
 #define FastSerialPort1(_portName)                                      \
 	FastSerial _portName(1,                                         \
@@ -189,7 +203,7 @@ struct hack
                              U2X1,                                      \
                              (_BV(RXEN1) |  _BV(TXEN1) | _BV(RXCIE1)),  \
                              (_BV(UDRIE1)));                            \
-	FastSerialHandler(1, SIG_USART1_RECV, SIG_USART1_DATA, UDR1)
+	FastSerialHandler(1, SIG_USART1_RECV, SIG_USART1_DATA, UDR1, UCSR1B, _BV(UDRIE1))
 #define FastSerialPort2(_portName)                                      \
 	FastSerial _portName(2,                                         \
                              &UBRR2H,                                   \
@@ -200,7 +214,7 @@ struct hack
                              U2X2,                                      \
                              (_BV(RXEN2) |  _BV(TXEN2) | _BV(RXCIE2)),  \
                              (_BV(UDRIE2)));                            \
-	FastSerialHandler(2, SIG_USART2_RECV, SIG_USART2_DATA, UDR2)
+	FastSerialHandler(2, SIG_USART2_RECV, SIG_USART2_DATA, UDR2, UCSR2B, _BV(UDRIE2))
 #define FastSerialPort3(_portName)                                      \
 	FastSerial _portName(3,                                         \
                              &UBRR3H,                                   \
@@ -211,7 +225,7 @@ struct hack
                              U2X3,                                      \
                              (_BV(RXEN3) |  _BV(TXEN3) | _BV(RXCIE3)),  \
                              (_BV(UDRIE3)));                            \
-	FastSerialHandler(3, SIG_USART3_RECV, SIG_USART3_DATA, UDR3)
+	FastSerialHandler(3, SIG_USART3_RECV, SIG_USART3_DATA, UDR3, UCSR3B, _BV(UDRIE3))
 #endif
 
 #endif // FastSerial_h

libraries/FastSerial/examples/FastSerial/FastSerial.pde

@@ -22,9 +22,9 @@ FastSerialPort0(Serial);
 
 //
 // To create a driver for a different serial port, on a board that
-// supports more than one, pass an argument to the constructor:
+// supports more than one, use the appropriate macro:
 //
-//FastSerial      Serial2(2);
+//FastSerialPort2(Serial2);
 
 
 void setup(void)