From 17700d9fafe11fcf7f3e50ea3e2c2488a874bc7b Mon Sep 17 00:00:00 2001
From: Michael Buesch <mb@bu3sch.de>
Date: Mon, 23 Nov 2009 16:42:05 +0100
Subject: pressure_control: Implement async TX queue

Signed-off-by: Michael Buesch <mb@bu3sch.de>
---
 pressure_control/firmware/remote.c | 102 ++++++++++++++++++++++++++++++-------
 pressure_control/firmware/remote.h |   5 +-
 pressure_control/firmware/util.h   |   3 +-
 3 files changed, 88 insertions(+), 22 deletions(-)

(limited to 'pressure_control/firmware')

diff --git a/pressure_control/firmware/remote.c b/pressure_control/firmware/remote.c
index a80683b..1911b8b 100644
--- a/pressure_control/firmware/remote.c
+++ b/pressure_control/firmware/remote.c
@@ -33,29 +33,99 @@
 #define USE_2X		1
 
 
+/* Last received message. */
 static struct remote_message rx_msg;
 static uint8_t rx_msg_count;
 static bool rx_msg_valid;
 static bool rx_softirq;
 static uint16_t rx_timeout;
 
+/* Transmission queue. */
+static struct remote_message tx_queue[32];
+static uint8_t tx_queue_in_ptr;
+static uint8_t tx_queue_out_ptr;
+static uint8_t tx_queue_used;
+static uint8_t tx_byte_ptr;
 
-static inline void usart_tx(uint8_t data)
+
+static inline uint8_t message_calc_crc(const struct remote_message *msg)
+{
+	uint8_t crc;
+
+	crc = crc8_block_update(0xFF, msg,
+				sizeof(*msg) - sizeof(msg->crc));
+	crc ^= 0xFF;
+
+	return crc;
+}
+
+static void tx_put_next_byte(void)
+{
+	const struct remote_message *msg;
+	const uint8_t *buf;
+
+	msg = &tx_queue[tx_queue_out_ptr];
+	buf = (const uint8_t *)msg;
+
+	UDR = buf[tx_byte_ptr];
+	tx_byte_ptr++;
+	if (tx_byte_ptr >= sizeof(struct remote_message)) {
+		tx_byte_ptr = 0;
+		tx_queue_out_ptr++;
+		if (tx_queue_out_ptr >= ARRAY_SIZE(tx_queue))
+			tx_queue_out_ptr = 0;
+		tx_queue_used--;
+		if (!tx_queue_used)
+			UCSRB &= ~(1 << UDRIE);
+	}
+}
+
+ISR(USART_UDRE_vect)
 {
-	while (!(UCSRA & (1 << UDRE)))
-		;
-	UDR = data;
+	if (tx_queue_used)
+		tx_put_next_byte();
 }
 
-static void usart_tx_buf(const void *_buf, uint8_t size)
+static void queue_tx_message(struct remote_message *msg)
 {
-	const uint8_t *buf = _buf;
+	uint8_t sreg;
 
-	while (size) {
-		usart_tx(*buf);
-		buf++;
-		size--;
+	sreg = irq_disable_save();
+
+	mb();
+	if (unlikely(tx_queue_used >= ARRAY_SIZE(tx_queue))) {
+		/* TX queue is full. Notify the overflow condition
+		 * to the remote control, once we get the message out. */
+		msg->id |= MSG_FLAG_QOVERFLOW;
+		msg->crc = message_calc_crc(msg);
+
+		/* Emergency situation. Manually push TX to get things
+		 * out of the box. */
+		do {
+			if (UCSRA & (1 << UDRE))
+				tx_put_next_byte();
+			if (!__irqs_disabled(sreg)) {
+				/* IRQs were enabled before we were called.
+				 * Be nice to other interrupts and re-enable them
+				 * for a microsecond. */
+				sei();
+				udelay(1);
+				cli();
+				mb();
+			}
+		} while (tx_queue_used >= ARRAY_SIZE(tx_queue));
 	}
+
+	memcpy(&tx_queue[tx_queue_in_ptr], msg, sizeof(*msg));
+	tx_queue_used++;
+	tx_queue_in_ptr++;
+	if (tx_queue_in_ptr >= ARRAY_SIZE(tx_queue))
+		tx_queue_in_ptr = 0;
+
+	if (tx_queue_used == 1)
+		UCSRB |= (1 << UDRIE);
+
+	irq_restore(sreg);
 }
 
 #define ERXFE		1 /* USART RX frame error */
@@ -86,12 +156,8 @@ static inline int8_t usart_rx(uint8_t *data)
 
 static void send_message(struct remote_message *msg)
 {
-	/* Calculate the CRC. */
-	msg->crc = crc8_block_update(0xFF, msg,
-				     sizeof(*msg) - sizeof(msg->crc));
-	msg->crc ^= 0xFF;
-	/* And transmit the bits. */
-	usart_tx_buf(msg, sizeof(*msg));
+	msg->crc = message_calc_crc(msg);
+	queue_tx_message(msg);
 }
 
 static void handle_received_message(void)
@@ -100,9 +166,7 @@ static void handle_received_message(void)
 	uint8_t calc_crc;
 	uint8_t err = MSG_ERR_NONE;
 
-	calc_crc = crc8_block_update(0xFF, &rx_msg,
-				     sizeof(rx_msg) - sizeof(rx_msg.crc));
-	calc_crc ^= 0xFF;
+	calc_crc = message_calc_crc(&rx_msg);
 	if (calc_crc != rx_msg.crc) {
 		/* CRC mismatch. */
 		err = MSG_ERR_CHKSUM;
diff --git a/pressure_control/firmware/remote.h b/pressure_control/firmware/remote.h
index 36caf40..a513d56 100644
--- a/pressure_control/firmware/remote.h
+++ b/pressure_control/firmware/remote.h
@@ -29,8 +29,9 @@ enum remote_message_id {
 	MSG_TURNON,
 
 
-	MSG_ID_MASK		= 0x7F,
-	MSG_FLAG_REQ_ERRCODE	= 0x80,
+	MSG_ID_MASK		= 0x3F,
+	MSG_FLAG_QOVERFLOW	= 0x40, /* TX queue overflow */
+	MSG_FLAG_REQ_ERRCODE	= 0x80, /* Error code is requested */
 };
 
 enum remote_message_error {
diff --git a/pressure_control/firmware/util.h b/pressure_control/firmware/util.h
index 992df0e..fbfaa76 100644
--- a/pressure_control/firmware/util.h
+++ b/pressure_control/firmware/util.h
@@ -78,7 +78,8 @@ static inline void irq_restore(uint8_t sreg_flags)
 	SREG = sreg_flags;
 }
 
-#define irqs_disabled()		(!(SREG & (1 << SREG_I)))
+#define __irqs_disabled(sreg)	(!(sreg & (1 << SREG_I)))
+#define irqs_disabled()		__irqs_disabled(SREG)
 
 
 uint8_t crc8_block_update(uint8_t crc, const void *data, uint8_t size);
-- 
cgit v1.2.3