Merge pull request #129 from KurtE/T2-Serial1-Write-bypass

Teensy 2 - Serial1.write() - maybe bypass queue

teensy/HardwareSerial.cpp

 #include <avr/interrupt.h>
 #include "core_pins.h"
 #include "HardwareSerial.h"
+#include "wiring_private.h" 
 
 #define RX_BUFFER_SIZE 64
 static volatile uint8_t rx_buffer[RX_BUFFER_SIZE];
 #endif
 {
 	uint8_t i;
+	uint8_t status;
 
 	if (!(UCSR1B & (1<<TXEN1))) {
 #if ARDUINO >= 100
 	if (tx_enable_pin < 255 && !transmitting) {
 		digitalWrite(tx_enable_pin, HIGH);
 	}
+	// If the buffer and the data register is empty, just write the byte
+	// to the data register and be done. This shortcut helps
+	// significantly improve the effective datarate at high (>
+	// 500kbit/s) bitrates, where interrupt overhead becomes a slowdown.
+	if ((tx_buffer_head == tx_buffer_tail) && (UCSR1A & (1<<UDRE1))) {
+		status = SREG;
+		cli();
+		UDR1 = c;
+		transmitting = 1;
+		SREG = status;
+		return 1;
+	}
+
 	i = tx_buffer_head + 1;
 	if (i >= TX_BUFFER_SIZE) i = 0;
 	while (tx_buffer_tail == i) ; // wait until space in buffer
 	tx_buffer[i] = c;
 	transmitting = 1;
 	tx_buffer_head = i;
-	UCSR1B = (1<<RXEN1) | (1<<TXCIE1) | (1<<TXEN1) | (1<<RXCIE1) | (1<<UDRIE1);
+	//UCSR1B = (1<<RXEN1) | (1<<TXCIE1) | (1<<TXEN1) | (1<<RXCIE1) | (1<<UDRIE1);
+	sbi(UCSR1B, UDRIE1);
+
 #if ARDUINO >= 100
 	return 1;
 #endif
 
 	if (tx_buffer_head == tx_buffer_tail) {
 		// buffer is empty, disable transmit interrupt
-		UCSR1B = (1<<RXEN1) | (1<<TXCIE1) | (1<<TXEN1) | (1<<RXCIE1);
+		//UCSR1B = (1<<RXEN1) | (1<<TXCIE1) | (1<<TXEN1) | (1<<RXCIE1);
+		cbi(UCSR1B, UDRIE1);
 	} else {
 		i = tx_buffer_tail + 1;
 		if (i >= TX_BUFFER_SIZE) i = 0;