From 6f0f21072067880ba9db4aba13603ea69aa06509 Mon Sep 17 00:00:00 2001
From: Shawn Silverman <shawn@pobox.com>
Date: Wed, 16 Nov 2022 16:13:04 -0800
Subject: [PATCH] Fix 7-bit support in teensy4 HardwareSerial

---
 teensy4/HardwareSerial.cpp | 15 ++++++++++++---
 teensy4/HardwareSerial.h   |  3 +++
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/teensy4/HardwareSerial.cpp b/teensy4/HardwareSerial.cpp
index fe3cb08f0..83fd5dbc3 100644
--- a/teensy4/HardwareSerial.cpp
+++ b/teensy4/HardwareSerial.cpp
@@ -192,6 +192,13 @@ void HardwareSerial::begin(uint32_t baud, uint16_t format)
 	if (format & 0x04) ctrl |= LPUART_CTRL_M;		// 9 bits (might include parity)
 	if ((format & 0x0F) == 0x04) ctrl |=  LPUART_CTRL_R9T8; // 8N2 is 9 bit with 9th bit always 1
 
+	// 7-bit support
+	if ((format & 0x0E) == 0x02) {
+		data_mask_ = 0x7f;	// Use only 7 bits of data
+	} else {
+		data_mask_ = 0x3ff;	// Use only up to 10 bits of data
+	}
+
 	// Bit 5 TXINVERT
 	if (format & 0x20) ctrl |= LPUART_CTRL_TXINV;		// tx invert
 
@@ -465,7 +472,7 @@ int HardwareSerial::peek(void)
 			// Still empty Now check for stuff in FIFO Queue.
 			int c = -1;	// assume nothing to return
 			if (port->WATER & 0x7000000) {
-				c = port->DATA & 0x3ff;		// Use only up to 10 bits of data
+				c = port->DATA & data_mask_;
 				// But we don't want to throw it away...
 				// since queue is empty, just going to reset to front of queue...
 				rx_buffer_head_ = 1;
@@ -500,7 +507,7 @@ int HardwareSerial::read(void)
 			// Still empty Now check for stuff in FIFO Queue.
 			c = -1;	// assume nothing to return
 			if (port->WATER & 0x7000000) {
-				c = port->DATA & 0x3ff;		// Use only up to 10 bits of data
+				c = port->DATA & data_mask_;
 			}
 			__enable_irq();
 			return c;
@@ -549,6 +556,8 @@ size_t HardwareSerial::write9bit(uint32_t c)
 		//digitalWriteFast(2, HIGH);
 	}
 
+	c &= data_mask_;
+
 	head = tx_buffer_head_;
 	if (++head >= tx_buffer_total_size_) head = 0;
 	while (tx_buffer_tail_ == head) {
@@ -603,7 +612,7 @@ void HardwareSerial::IRQHandler()
 			head = rx_buffer_head_;
 			tail = rx_buffer_tail_;
 			do {
-				n = port->DATA & 0x3ff;		// Use only up to 10 bits of data
+				n = port->DATA & data_mask_;
 				newhead = head + 1;
 
 				if (newhead >= rx_buffer_total_size_) newhead = 0;
diff --git a/teensy4/HardwareSerial.h b/teensy4/HardwareSerial.h
index 9ae42b33f..92f2dedbe 100644
--- a/teensy4/HardwareSerial.h
+++ b/teensy4/HardwareSerial.h
@@ -288,6 +288,9 @@ class HardwareSerial : public Stream
 	uint8_t				tx_pin_index_ = 0x0;
 	uint8_t				half_duplex_mode_ = 0; // are we in half duplex mode?
 
+	// 7-bit mode support
+	int				data_mask_ = 0x3ff;	// Use only up to 10 bits of data
+
 	volatile BUFTYPE 	*tx_buffer_;
 	volatile BUFTYPE 	*rx_buffer_;
 	volatile BUFTYPE	*rx_buffer_storage_ = nullptr;