Use Serial2's FIFO on Teensy 3.1

teensy3/serial2.c

 #define RX_BUFFER_SIZE 64
 #define IRQ_PRIORITY  64  // 0 = highest priority, 255 = lowest
 
-
 ////////////////////////////////////////////////////////////////
 // changes not recommended below this point....
 ////////////////////////////////////////////////////////////////
 // UART0 and UART1 are clocked by F_CPU, UART2 is clocked by F_BUS
 // UART0 has 8 byte fifo, UART1 and UART2 have 1 byte buffer
 
+#ifdef KINETISK_UART1_FIFO
+#define C2_ENABLE		UART_C2_TE | UART_C2_RE | UART_C2_RIE | UART_C2_ILIE
+#else
 #define C2_ENABLE		UART_C2_TE | UART_C2_RE | UART_C2_RIE
+#endif
 #define C2_TX_ACTIVE		C2_ENABLE | UART_C2_TIE
 #define C2_TX_COMPLETING	C2_ENABLE | UART_C2_TCIE
 #define C2_TX_INACTIVE		C2_ENABLE
 	UART1_BDH = (divisor >> 13) & 0x1F;
 	UART1_BDL = (divisor >> 5) & 0xFF;
 	UART1_C4 = divisor & 0x1F;
+#ifdef KINETISK_UART1_FIFO
+	UART1_C1 = UART_C1_ILT;
+	UART1_TWFIFO = 2; // tx watermark, causes S1_TDRE to set
+	UART1_RWFIFO = 4; // rx watermark, causes S1_RDRF to set
+	UART1_PFIFO = UART_PFIFO_TXFE | UART_PFIFO_RXFE;
+#else
 	UART1_C1 = 0;
 	UART1_PFIFO = 0;
+#endif
 	UART1_C2 = C2_TX_INACTIVE;
 	NVIC_SET_PRIORITY(IRQ_UART1_STATUS, IRQ_PRIORITY);
 	NVIC_ENABLE_IRQ(IRQ_UART1_STATUS);
 
 void serial2_putchar(uint32_t c)
 {
-	uint32_t head;
+	uint32_t head, n;
 
 	if (!(SIM_SCGC4 & SIM_SCGC4_UART1)) return;
 	head = tx_buffer_head;
 			if ((UART1_S1 & UART_S1_TDRE)) {
 				uint32_t tail = tx_buffer_tail;
 				if (++tail >= TX_BUFFER_SIZE) tail = 0;
-				UART1_D = tx_buffer[tail];
+				n = tx_buffer[tail];
+				if (use9Bits) UART1_C3 = (UART1_C3 & ~0x40) | ((n & 0x100) >> 2);
+				UART1_D = n;
 				tx_buffer_tail = tail;
 			}
 		} else if (priority >= 256) {
 	UART1_C2 = C2_TX_ACTIVE;
 }
 
+#ifdef KINETISK_UART1_FIFO
+void serial2_write(const void *buf, unsigned int count)
+{
+	const uint8_t *p = (const uint8_t *)buf;
+	const uint8_t *end = p + count;
+        uint32_t head, n;
+
+	if (!(SIM_SCGC4 & SIM_SCGC4_UART1)) return;
+	while (p < end) {
+		head = tx_buffer_head;
+		if (++head >= TX_BUFFER_SIZE) head = 0;
+		if (tx_buffer_tail == head) {
+			UART1_C2 = C2_TX_ACTIVE;
+			do {
+				int priority = nvic_execution_priority();
+				if (priority <= IRQ_PRIORITY) {
+					if ((UART1_S1 & UART_S1_TDRE)) {
+						uint32_t tail = tx_buffer_tail;
+						if (++tail >= TX_BUFFER_SIZE) tail = 0;
+						n = tx_buffer[tail];
+						if (use9Bits) UART1_C3 = (UART1_C3 & ~0x40) | ((n & 0x100) >> 2);
+						UART1_D = n;
+						tx_buffer_tail = tail;
+					}
+				} else if (priority >= 256) {
+					yield();
+				}
+			} while (tx_buffer_tail == head);
+		}
+		tx_buffer[head] = *p++;
+		transmitting = 1;
+		tx_buffer_head = head;
+	}
+        UART1_C2 = C2_TX_ACTIVE;
+}
+#else
 void serial2_write(const void *buf, unsigned int count)
 {
 	const uint8_t *p = (const uint8_t *)buf;
 	while (count-- > 0) serial2_putchar(*p++);
 }
+#endif
 
 void serial2_flush(void)
 {
 
 void serial2_clear(void)
 {
+#ifdef KINETISK_UART1_FIFO
+	if (!(SIM_SCGC4 & SIM_SCGC4_UART1)) return;
+	UART1_C2 &= ~(UART_C2_RE | UART_C2_RIE | UART_C2_ILIE);
+	UART1_CFIFO = UART_CFIFO_RXFLUSH;
+	UART1_C2 |= (UART_C2_RE | UART_C2_RIE | UART_C2_ILIE);
+#endif
 	rx_buffer_head = rx_buffer_tail;
 }
 
 {
 	uint32_t head, tail, n;
 	uint8_t c;
+#ifdef KINETISK_UART1_FIFO
+	uint32_t newhead;
+	uint8_t avail;
 
-	//digitalWriteFast(4, HIGH);
+	if (UART1_S1 & (UART_S1_RDRF | UART_S1_IDLE)) {
+		__disable_irq();
+		avail = UART1_RCFIFO;
+		if (avail == 0) {
+			// The only way to clear the IDLE interrupt flag is
+			// to read the data register.  But reading with no
+			// data causes a FIFO underrun, which causes the
+			// FIFO to return corrupted data.  If anyone from
+			// Freescale reads this, what a poor design!  There
+			// write should be a write-1-to-clear for IDLE.
+			c = UART1_D;
+			// flushing the fifo recovers from the underrun,
+			// but there's a possible race condition where a
+			// new character could be received between reading
+			// RCFIFO == 0 and flushing the FIFO.  To minimize
+			// the chance, interrupts are disabled so a higher
+			// priority interrupt (hopefully) doesn't delay.
+			// TODO: change this to disabling the IDLE interrupt
+			// which won't be simple, since we already manage
+			// which transmit interrupts are enabled.
+			UART1_CFIFO = UART_CFIFO_RXFLUSH;
+			__enable_irq();
+		} else {
+			__enable_irq();
+			head = rx_buffer_head;
+			tail = rx_buffer_tail;
+			do {
+				n = UART1_D;
+				if (use9Bits && (UART1_C3 & 0x80)) n |= 0x100;
+				newhead = head + 1;
+				if (newhead >= RX_BUFFER_SIZE) newhead = 0;
+				if (newhead != tail) {
+					head = newhead;
+					rx_buffer[head] = n;
+				}
+			} while (--avail > 0);
+			rx_buffer_head = head;
+		}
+	}
+	c = UART1_C2;
+	if ((c & UART_C2_TIE) && (UART1_S1 & UART_S1_TDRE)) {
+		head = tx_buffer_head;
+		tail = tx_buffer_tail;
+		do {
+			if (tail == head) break;
+			if (++tail >= TX_BUFFER_SIZE) tail = 0;
+			avail = UART1_S1;
+			n = tx_buffer[tail];
+			if (use9Bits) UART1_C3 = (UART1_C3 & ~0x40) | ((n & 0x100) >> 2);
+			UART1_D = n;
+		} while (UART1_TCFIFO < 8);
+		tx_buffer_tail = tail;
+		if (UART1_S1 & UART_S1_TDRE) UART1_C2 = C2_TX_COMPLETING;
+	}
+#else
 	if (UART1_S1 & UART_S1_RDRF) {
-		//digitalWriteFast(5, HIGH);
 		n = UART1_D;
 		if (use9Bits && (UART1_C3 & 0x80)) n |= 0x100;
 		head = rx_buffer_head + 1;
 			rx_buffer[head] = n;
 			rx_buffer_head = head; 
 		}
-		//digitalWriteFast(5, LOW);
 	}
 	c = UART1_C2;
 	if ((c & UART_C2_TIE) && (UART1_S1 & UART_S1_TDRE)) {
-		//digitalWriteFast(5, HIGH);
 		head = tx_buffer_head;
 		tail = tx_buffer_tail;
 		if (head == tail) {
 			UART1_D = n;
 			tx_buffer_tail = tail;
 		}
-		//digitalWriteFast(5, LOW);
 	}
+#endif
 	if ((c & UART_C2_TCIE) && (UART1_S1 & UART_S1_TC)) {
 		transmitting = 0;
 		UART1_C2 = C2_TX_INACTIVE;
 	}
-	//digitalWriteFast(4, LOW);
 }