Add NVIC priority handling in Serial2 & Serial3

teensy3/mk20dx128.c

 {
 }
 
+int nvic_execution_priority(void)
+{
+	int priority=256;
+	uint32_t primask, faultmask, basepri, ipsr;
+
+	// full algorithm in ARM DDI0403D, page B1-639
+	// this isn't quite complete, but hopefully good enough
+	asm volatile("mrs %0, faultmask\n" : "=r" (faultmask)::);
+	if (faultmask) return -1;
+	asm volatile("mrs %0, primask\n" : "=r" (primask)::);
+	if (primask) return 0;
+	asm volatile("mrs %0, ipsr\n" : "=r" (ipsr)::);
+	if (ipsr) {
+		if (ipsr < 16) priority = 0; // could be non-zero
+		else priority = NVIC_GET_PRIORITY(ipsr - 16);
+	}
+	asm volatile("mrs %0, basepri\n" : "=r" (basepri)::);
+	if (basepri > 0 && basepri < priority) priority = basepri;
+	return priority;
+}
+

teensy3/mk20dx128.h

 #define ARM_DWT_CTRL_CYCCNTENA          (1 << 0)                // Enable cycle count
 #define ARM_DWT_CYCCNT          *(volatile uint32_t *)0xE0001004 // Cycle count register
 
+extern int nvic_execution_priority(void);
 
 extern void nmi_isr(void);
 extern void hard_fault_isr(void);

teensy3/serial1.c

 	rx_buffer_tail = 0;
 }
 
-
-static int get_nvic_execution_priority(void)
-{
-	int priority=256;
-	uint32_t primask, faultmask, basepri, ipsr;
-
-	// full algorithm in ARM DDI0403D, page B1-639
-	// this isn't quite complete, but hopefully good enough
-	asm volatile("mrs %0, faultmask\n" : "=r" (faultmask)::);
-	if (faultmask) return -1;
-	asm volatile("mrs %0, primask\n" : "=r" (primask)::);
-	if (primask) return 0;
-	asm volatile("mrs %0, ipsr\n" : "=r" (ipsr)::);
-	if (ipsr) {
-		if (ipsr < 16) priority = 0; // could be non-zero
-		else priority = NVIC_GET_PRIORITY(ipsr - 16);
-	}
-	asm volatile("mrs %0, basepri\n" : "=r" (basepri)::);
-	if (basepri > 0 && basepri < priority) priority = basepri;
-	return priority;
-}
-
-
 void serial_putchar(uint32_t c)
 {
 	uint32_t head;
 	head = tx_buffer_head;
 	if (++head >= TX_BUFFER_SIZE) head = 0;
 	while (tx_buffer_tail == head) {
-		if (get_nvic_execution_priority() <= IRQ_PRIORITY) {
+		int priority = nvic_execution_priority();
+		if (priority <= IRQ_PRIORITY) {
 			if ((UART0_S1 & UART_S1_TDRE)) {
 				uint32_t tail = tx_buffer_tail;
 				if (++tail >= TX_BUFFER_SIZE) tail = 0;
 				UART0_D = tx_buffer[tail];
 				tx_buffer_tail = tail;
 			}
-		} else {
+		} else if (priority >= 256) {
 			yield();
 		}
 	}
 		if (tx_buffer_tail == head) {
         		UART0_C2 = C2_TX_ACTIVE;
 			do {
-				if (get_nvic_execution_priority() <= IRQ_PRIORITY) {
+				int priority = nvic_execution_priority();
+				if (priority <= IRQ_PRIORITY) {
 					if ((UART0_S1 & UART_S1_TDRE)) {
 						uint32_t tail = tx_buffer_tail;
 						if (++tail >= TX_BUFFER_SIZE) tail = 0;
 						UART0_D = tx_buffer[tail];
 						tx_buffer_tail = tail;
 					}
-				} else {
+				} else if (priority >= 256) {
 					yield();
 				}
 			} while (tx_buffer_tail == head);

teensy3/serial2.c

 #include "core_pins.h"
 #include "HardwareSerial.h"
 
-// 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
+////////////////////////////////////////////////////////////////
+// Tunable parameters (relatively safe to edit these numbers)
+////////////////////////////////////////////////////////////////
+
+#define TX_BUFFER_SIZE 40
+#define RX_BUFFER_SIZE 64
+#define IRQ_PRIORITY  64  // 0 = highest priority, 255 = lowest
+
+
+////////////////////////////////////////////////////////////////
+// changes not recommended below this point....
+////////////////////////////////////////////////////////////////
 
 #ifdef SERIAL_9BIT_SUPPORT
 static uint8_t use9Bits = 0;
 #define use9Bits 0
 #endif
 
-#define TX_BUFFER_SIZE 40
 static volatile BUFTYPE tx_buffer[TX_BUFFER_SIZE];
+static volatile BUFTYPE rx_buffer[RX_BUFFER_SIZE];
 static volatile uint8_t transmitting = 0;
 #if TX_BUFFER_SIZE > 255
 static volatile uint16_t tx_buffer_head = 0;
 static volatile uint8_t tx_buffer_head = 0;
 static volatile uint8_t tx_buffer_tail = 0;
 #endif
-
-#define RX_BUFFER_SIZE 64
-static volatile BUFTYPE rx_buffer[RX_BUFFER_SIZE];
+#if RX_BUFFER_SIZE > 255
+static volatile uint16_t rx_buffer_head = 0;
+static volatile uint16_t rx_buffer_tail = 0;
+#else
 static volatile uint8_t rx_buffer_head = 0;
 static volatile uint8_t rx_buffer_tail = 0;
+#endif
+
+// 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
 
 #define C2_ENABLE		UART_C2_TE | UART_C2_RE | UART_C2_RIE
 #define C2_TX_ACTIVE		C2_ENABLE | UART_C2_TIE
 	UART1_C1 = 0;
 	UART1_PFIFO = 0;
 	UART1_C2 = C2_TX_INACTIVE;
-	NVIC_SET_PRIORITY(IRQ_UART1_STATUS, 64);
+	NVIC_SET_PRIORITY(IRQ_UART1_STATUS, IRQ_PRIORITY);
 	NVIC_ENABLE_IRQ(IRQ_UART1_STATUS);
 }
 
 	head = tx_buffer_head;
 	if (++head >= TX_BUFFER_SIZE) head = 0;
 	while (tx_buffer_tail == head) {
-		yield(); // wait
+		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;
+				UART1_D = tx_buffer[tail];
+				tx_buffer_tail = tail;
+			}
+		} else if (priority >= 256) {
+			yield(); // wait
+		}
 	}
 	tx_buffer[head] = c;
 	transmitting = 1;

teensy3/serial3.c

 #include "core_pins.h"
 #include "HardwareSerial.h"
 
-// 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
+////////////////////////////////////////////////////////////////
+// Tunable parameters (relatively safe to edit these numbers)
+////////////////////////////////////////////////////////////////
+
+#define TX_BUFFER_SIZE 40
+#define RX_BUFFER_SIZE 64
+#define IRQ_PRIORITY  64  // 0 = highest priority, 255 = lowest
+
+
+////////////////////////////////////////////////////////////////
+// changes not recommended below this point....
+////////////////////////////////////////////////////////////////
 
 #ifdef SERIAL_9BIT_SUPPORT
 static uint8_t use9Bits = 0;
 #define use9Bits 0
 #endif
 
-#define TX_BUFFER_SIZE 40
 static volatile BUFTYPE tx_buffer[TX_BUFFER_SIZE];
+static volatile BUFTYPE rx_buffer[RX_BUFFER_SIZE];
 static volatile uint8_t transmitting = 0;
 #if TX_BUFFER_SIZE > 255
 static volatile uint16_t tx_buffer_head = 0;
 static volatile uint8_t tx_buffer_head = 0;
 static volatile uint8_t tx_buffer_tail = 0;
 #endif
-
-#define RX_BUFFER_SIZE 64
-static volatile BUFTYPE rx_buffer[RX_BUFFER_SIZE];
+#if RX_BUFFER_SIZE > 255
+static volatile uint16_t rx_buffer_head = 0;
+static volatile uint16_t rx_buffer_tail = 0;
+#else
 static volatile uint8_t rx_buffer_head = 0;
 static volatile uint8_t rx_buffer_tail = 0;
+#endif
+
+// 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
 
 #define C2_ENABLE		UART_C2_TE | UART_C2_RE | UART_C2_RIE
 #define C2_TX_ACTIVE		C2_ENABLE | UART_C2_TIE
 	head = tx_buffer_head;
 	if (++head >= TX_BUFFER_SIZE) head = 0;
 	while (tx_buffer_tail == head) {
-		yield(); // wait
+		int priority = nvic_execution_priority();
+		if (priority <= IRQ_PRIORITY) {
+			if ((UART2_S1 & UART_S1_TDRE)) {
+				uint32_t tail = tx_buffer_tail;
+				if (++tail >= TX_BUFFER_SIZE) tail = 0;
+				UART2_D = tx_buffer[tail];
+				tx_buffer_tail = tail;
+			}
+		} else if (priority >= 256) {
+			yield(); // wait
+		}
 	}
 	tx_buffer[head] = c;
 	transmitting = 1;