From 08b66e18b76a11dcfe6aef0769daa3aeaa248384 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 30 Mar 2012 17:05:18 +1100
Subject: [PATCH] FastSerial: added set_blocking_writes() interface

this allows us to put a serial port into non-blocking mode, so that
writes that don't fit in the transmit buffer are dropped. This will be
used in flight to prevent stray printf() calls from causing large time
delays in the code
---
 libraries/FastSerial/FastSerial.cpp | 7 +++++++
 libraries/FastSerial/FastSerial.h   | 9 +++++++++
 2 files changed, 16 insertions(+)

diff --git a/libraries/FastSerial/FastSerial.cpp b/libraries/FastSerial/FastSerial.cpp
index efc8c896e..67f71483c 100644
--- a/libraries/FastSerial/FastSerial.cpp
+++ b/libraries/FastSerial/FastSerial.cpp
@@ -212,6 +212,13 @@ size_t FastSerial::write(uint8_t c)
 
 	// wait for room in the tx buffer
 	i = (_txBuffer->head + 1) & _txBuffer->mask;
+
+	// if the port is set into non-blocking mode, then drop the byte
+	// if there isn't enough room for it in the transmit buffer
+	if (_nonblocking_writes && i == _txBuffer->tail) {
+		return 0;
+	}
+
 	while (i == _txBuffer->tail)
 		;
 
diff --git a/libraries/FastSerial/FastSerial.h b/libraries/FastSerial/FastSerial.h
index 5fcaa95c7..b64ff1fb2 100644
--- a/libraries/FastSerial/FastSerial.h
+++ b/libraries/FastSerial/FastSerial.h
@@ -160,6 +160,11 @@ public:
 		return (1<<port) & _serialInitialized;
 	}
 
+	// ask for writes to be blocking or non-blocking
+	void set_blocking_writes(bool blocking) {
+		_nonblocking_writes = !blocking;
+	}
+
 private:
 
 	/// Bit mask for initialized ports
@@ -187,6 +192,10 @@ private:
 	Buffer			* const _txBuffer;
 	bool 			_open;
 
+	// whether writes to the port should block waiting
+	// for enough space to appear
+	bool			_nonblocking_writes;
+
 	/// Allocates a buffer of the given size
 	///
 	/// @param	buffer		The buffer descriptor for which the buffer will
-- 
GitLab