From 17700d9fafe11fcf7f3e50ea3e2c2488a874bc7b Mon Sep 17 00:00:00 2001 From: Michael Buesch Date: Mon, 23 Nov 2009 16:42:05 +0100 Subject: pressure_control: Implement async TX queue Signed-off-by: Michael Buesch --- pressure_control/firmware/remote.c | 102 +++++++++++++++++++++++++++++------- pressure_control/firmware/remote.h | 5 +- pressure_control/firmware/util.h | 3 +- pressure_control/remote/pctl-remote | 39 ++++++++------ 4 files changed, 112 insertions(+), 37 deletions(-) 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); diff --git a/pressure_control/remote/pctl-remote b/pressure_control/remote/pctl-remote index 9ba991e..ab8d023 100755 --- a/pressure_control/remote/pctl-remote +++ b/pressure_control/remote/pctl-remote @@ -38,7 +38,7 @@ CONFIG_STOPBITS = 2 MSG_SIZE = 6 MSG_PAYLOAD_SIZE = 4 # Message IDs -MSG_ID_MASK = 0x7F +MSG_ID_MASK = 0x3F MSG_INVALID = 0 MSG_ERROR = 1 MSG_LOGMESSAGE = 2 @@ -60,6 +60,7 @@ MSG_RESTARTED = 17 MSG_SHUTDOWN = 18 MSG_TURNON = 19 # Message flags +MSG_FLAG_QOVERFLOW = 0x40 MSG_FLAG_REQ_ERRCODE = 0x80 # Message error codes MSG_ERR_NONE = 0 # No error @@ -124,16 +125,16 @@ def parseArgs(): if o in ("-l", "--log"): opt_logfile = v -class Log(QObject): - def __init__(self, logfile): +class LogFile(QObject): + def __init__(self, logfileName): QObject.__init__(self) self.fd = None - if not logfile: + if not logfileName: return try: - self.fd = file(logfile, "w+b") + self.fd = file(logfileName, "w+b") except IOError, e: - print "Failed to open logfile %s: %s" % (logfile, e.strerror) + print "Failed to open logfile %s: %s" % (logfileName, e.strerror) sys.exit(1) self.write("X/Y,X/Y lower threshold,X/Y upper threshold,"+\ "Z,Z lower threshold,Z upper threshold,\n") @@ -225,7 +226,7 @@ class RemoteProtocol(QObject): def __poll(self): try: if self.serial.inWaiting() >= MSG_SIZE: - self.parseMessage(self.serial.read(MSG_SIZE)) + self.parseMessage(self.__readMessage()) except (SerialException, OSError, IOError), e: mainwnd.statusBar().showMessage("Failed to poll message. %s" % e) return @@ -249,6 +250,14 @@ class RemoteProtocol(QObject): return False return True + def __readMessage(self): + msg = self.serial.read(MSG_SIZE) + flags = ord(msg[0]) & ~MSG_ID_MASK + if flags & MSG_FLAG_QOVERFLOW: + mainwnd.centralWidget().log.hostLog( + "Warning: TX queue overflow on the device") + return msg + def parseMessage(self, msg): if not self.checksumMessage(msg): return @@ -298,10 +307,10 @@ class RemoteProtocol(QObject): if self.serial.inWaiting() < MSG_SIZE: QThread.msleep(1) continue - msg = self.serial.read(MSG_SIZE) + msg = self.__readMessage() if not self.checksumMessage(msg): continue - msgid = ord(msg[0]) + msgid = ord(msg[0]) & MSG_ID_MASK if msgid == replyId: break # This is not a reply to our message. @@ -700,10 +709,10 @@ class MainWidget(QWidget): z_mbar = ord(msg[2]) | (ord(msg[3]) << 8) self.xy.gauge.setValue(float(xy_mbar) / 1000) self.z.gauge.setValue(float(z_mbar) / 1000) - log.logPressure(xy_mbar, self.xy.getDesiredPressure(), - self.xy.getHysteresis(), - z_mbar, self.z.getDesiredPressure(), - self.z.getHysteresis()) + logfile.logPressure(xy_mbar, self.xy.getDesiredPressure(), + self.xy.getHysteresis(), + z_mbar, self.z.getDesiredPressure(), + self.z.getHysteresis()) class MainWindow(QMainWindow): def __init__(self, parent=None): @@ -739,14 +748,14 @@ def main(): global remote global mainwnd global app - global log + global logfile mainwnd = None app = QApplication(sys.argv) parseArgs() - log = Log(opt_logfile) + logfile = LogFile(opt_logfile) mainwnd = MainWindow() remote = RemoteProtocol(opt_ttyfile) -- cgit v1.2.3