serialqueue: Support notification of when a command is processed

Add ability for the host code to get a notification when the ack for a
command sent to the micro-controller is received.  This is in
preparation for improved detection of message loss between mcu and
host.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 45 insertions, 10 deletions

diff --git a/klippy/chelper/serialqueue.c b/klippy/chelper/serialqueue.c
index 61331379..779156ce 100644
--- a/klippy/chelper/serialqueue.c
+++ b/klippy/chelper/serialqueue.c
@@ -1,9 +1,9 @@
 // Serial port command queuing
 //
-// Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2016-2020  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
-//
+
 // This goal of this code is to handle low-level serial port
 // communications with a microcontroller (mcu).  This code is written
 // in C (instead of python) to reduce communication latencies and to
@@ -372,6 +372,7 @@ struct serialqueue {
     struct list_head pending_queues;
     int ready_bytes, stalled_bytes, need_ack_bytes, last_ack_bytes;
     uint64_t need_kick_clock;
+    struct list_head notify_queue;
     // Received messages
     struct list_head receive_queue;
     // Debugging
@@ -512,6 +513,25 @@ handle_message(struct serialqueue *sq, double eventtime, int len)
     if (rseq != sq->receive_seq)
         // New sequence number
         update_receive_seq(sq, eventtime, rseq);
+
+    // Check for pending messages on notify_queue
+    int must_wake = 0;
+    while (!list_empty(&sq->notify_queue)) {
+        struct queue_message *qm = list_first_entry(
+            &sq->notify_queue, struct queue_message, node);
+        uint64_t wake_seq = rseq - 1 - (len > MESSAGE_MIN ? 1 : 0);
+        uint64_t notify_msg_sent_seq = qm->req_clock;
+        if (notify_msg_sent_seq > wake_seq)
+            break;
+        list_del(&qm->node);
+        qm->len = 0;
+        qm->sent_time = sq->last_receive_sent_time;
+        qm->receive_time = eventtime;
+        list_add_tail(&qm->node, &sq->receive_queue);
+        must_wake = 1;
+    }
+
+    // Process message
     if (len == MESSAGE_MIN) {
         // Ack/nak message
         if (sq->last_ack_seq < rseq)
@@ -519,18 +539,19 @@ handle_message(struct serialqueue *sq, double eventtime, int len)
         else if (rseq > sq->ignore_nak_seq && !list_empty(&sq->sent_queue))
             // Duplicate Ack is a Nak - do fast retransmit
             pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NOW);
-    }
-
-    if (len > MESSAGE_MIN) {
-        // Add message to receive queue
+    } else {
+        // Data message - add to receive queue
         struct queue_message *qm = message_fill(sq->input_buf, len);
         qm->sent_time = (rseq > sq->retransmit_seq
                          ? sq->last_receive_sent_time : 0.);
         qm->receive_time = get_monotonic(); // must be time post read()
         qm->receive_time -= sq->baud_adjust * len;
         list_add_tail(&qm->node, &sq->receive_queue);
-        check_wake_receive(sq);
+        must_wake = 1;
     }
+
+    if (must_wake)
+        check_wake_receive(sq);
 }
 
 // Callback for input activity on the serial fd
@@ -661,7 +682,13 @@ build_and_send_command(struct serialqueue *sq, double eventtime)
         memcpy(&out->msg[out->len], qm->msg, qm->len);
         out->len += qm->len;
         sq->ready_bytes -= qm->len;
-        message_free(qm);
+        if (qm->notify_id) {
+            // Message requires notification - add to notify list
+            qm->req_clock = sq->send_seq;
+            list_add_tail(&qm->node, &sq->notify_queue);
+        } else {
+            message_free(qm);
+        }
     }
 
     // Fill header / trailer
@@ -835,6 +862,7 @@ serialqueue_alloc(int serial_fd, int write_only)
     list_init(&sq->pending_queues);
     list_init(&sq->sent_queue);
     list_init(&sq->receive_queue);
+    list_init(&sq->notify_queue);
 
     // Debugging
     list_init(&sq->old_sent);
@@ -882,6 +910,7 @@ serialqueue_free(struct serialqueue *sq)
     pthread_mutex_lock(&sq->lock);
     message_queue_free(&sq->sent_queue);
     message_queue_free(&sq->receive_queue);
+    message_queue_free(&sq->notify_queue);
     message_queue_free(&sq->old_sent);
     message_queue_free(&sq->old_receive);
     while (!list_empty(&sq->pending_queues)) {
@@ -960,11 +989,13 @@ serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq
 // given time and priority.
 void __visible
 serialqueue_send(struct serialqueue *sq, struct command_queue *cq, uint8_t *msg
-                 , int len, uint64_t min_clock, uint64_t req_clock)
+                 , int len, uint64_t min_clock, uint64_t req_clock
+                 , uint64_t notify_id)
 {
     struct queue_message *qm = message_fill(msg, len);
     qm->min_clock = min_clock;
     qm->req_clock = req_clock;
+    qm->notify_id = notify_id;
 
     struct list_head msgs;
     list_init(&msgs);
@@ -998,7 +1029,11 @@ serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm)
     pqm->len = qm->len;
     pqm->sent_time = qm->sent_time;
     pqm->receive_time = qm->receive_time;
-    debug_queue_add(&sq->old_receive, qm);
+    pqm->notify_id = qm->notify_id;
+    if (qm->len)
+        debug_queue_add(&sq->old_receive, qm);
+    else
+        message_free(qm);
 
     pthread_mutex_unlock(&sq->lock);
     return;