serialqueue: Improve canbus timing

Adjust timing based on the minimum transmission time of canbus
messages.

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

5 files changed, 58 insertions, 28 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index c7e2155c4..fa776d14d 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -168,8 +168,8 @@ defs_serialqueue = """
         , uint64_t notify_id);
     void serialqueue_pull(struct serialqueue *sq
         , struct pull_queue_message *pqm);
-    void serialqueue_set_baud_adjust(struct serialqueue *sq
-        , double baud_adjust);
+    void serialqueue_set_wire_frequency(struct serialqueue *sq
+        , double frequency);
     void serialqueue_set_receive_window(struct serialqueue *sq
         , int receive_window);
     void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
diff --git a/klippy/chelper/serialqueue.c b/klippy/chelper/serialqueue.c
index b74605fff..75d39d212 100644
--- a/klippy/chelper/serialqueue.c
+++ b/klippy/chelper/serialqueue.c
@@ -49,7 +49,7 @@ struct serialqueue {
     int receive_waiting;
     // Baud / clock tracking
     int receive_window;
-    double baud_adjust, idle_time;
+    double bittime_adjust, idle_time;
     struct clock_estimate ce;
     double last_receive_sent_time;
     // Retransmit support
@@ -136,6 +136,23 @@ kick_bg_thread(struct serialqueue *sq)
         report_errno("pipe write", ret);
 }
 
+// Minimum number of bits in a canbus message
+#define CANBUS_PACKET_BITS ((1 + 11 + 3 + 4) + (16 + 2 + 7 + 3))
+#define CANBUS_IFS_BITS 4
+
+// Determine minimum time needed to transmit a given number of bytes
+static double
+calculate_bittime(struct serialqueue *sq, uint32_t bytes)
+{
+    if (sq->serial_fd_type == SQT_CAN) {
+        uint32_t pkts = DIV_ROUND_UP(bytes, 8);
+        uint32_t bits = bytes * 8 + pkts * CANBUS_PACKET_BITS - CANBUS_IFS_BITS;
+        return sq->bittime_adjust * bits;
+    } else {
+        return sq->bittime_adjust * bytes;
+    }
+}
+
 // Update internal state when the receive sequence increases
 static void
 update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
@@ -192,7 +209,7 @@ update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
     } else {
         struct queue_message *sent = list_first_entry(
             &sq->sent_queue, struct queue_message, node);
-        double nr = eventtime + sq->rto + sent->len * sq->baud_adjust;
+        double nr = eventtime + sq->rto + calculate_bittime(sq, sent->len);
         pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, nr);
     }
 }
@@ -251,7 +268,7 @@ handle_message(struct serialqueue *sq, double eventtime, int 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;
+        qm->receive_time -= calculate_bittime(sq, len);
         list_add_tail(&qm->node, &sq->receive_queue);
         must_wake = 1;
     }
@@ -407,8 +424,8 @@ retransmit_event(struct serialqueue *sq, double eventtime)
     }
     sq->retransmit_seq = sq->send_seq;
     sq->rtt_sample_seq = 0;
-    sq->idle_time = eventtime + buflen * sq->baud_adjust;
-    double waketime = eventtime + first_buflen * sq->baud_adjust + sq->rto;
+    sq->idle_time = eventtime + calculate_bittime(sq, buflen);
+    double waketime = eventtime + sq->rto + calculate_bittime(sq, first_buflen);
 
     pthread_mutex_unlock(&sq->lock);
     return waketime;
@@ -416,7 +433,8 @@ retransmit_event(struct serialqueue *sq, double eventtime)
 
 // Construct a block of data to be sent to the serial port
 static int
-build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
+build_and_send_command(struct serialqueue *sq, uint8_t *buf, int pending
+                       , double eventtime)
 {
     int len = MESSAGE_HEADER_SIZE;
     while (sq->ready_bytes) {
@@ -463,17 +481,15 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
     buf[len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
 
     // Store message block
-    if (eventtime > sq->idle_time)
-        sq->idle_time = eventtime;
-    sq->idle_time += len * sq->baud_adjust;
+    double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
+    idletime += calculate_bittime(sq, pending + len);
     struct queue_message *out = message_alloc();
     memcpy(out->msg, buf, len);
     out->len = len;
     out->sent_time = eventtime;
-    out->receive_time = sq->idle_time;
+    out->receive_time = idletime;
     if (list_empty(&sq->sent_queue))
-        pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT
-                                 , sq->idle_time + sq->rto);
+        pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, idletime + sq->rto);
     if (!sq->rtt_sample_seq)
         sq->rtt_sample_seq = sq->send_seq;
     sq->send_seq++;
@@ -484,7 +500,7 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
 
 // Determine the time the next serial data should be sent
 static double
-check_send_command(struct serialqueue *sq, double eventtime)
+check_send_command(struct serialqueue *sq, int pending, double eventtime)
 {
     if (sq->send_seq - sq->receive_seq >= MAX_PENDING_BLOCKS
         && sq->receive_seq != (uint64_t)-1)
@@ -501,7 +517,7 @@ check_send_command(struct serialqueue *sq, double eventtime)
 
     // Check for stalled messages now ready
     double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
-    idletime += MESSAGE_MIN * sq->baud_adjust;
+    idletime += calculate_bittime(sq, pending + MESSAGE_MIN);
     uint64_t ack_clock = clock_from_time(&sq->ce, idletime);
     uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK;
     struct command_queue *cq;
@@ -525,9 +541,10 @@ check_send_command(struct serialqueue *sq, double eventtime)
             struct queue_message *qm = list_first_entry(
                 &cq->ready_queue, struct queue_message, node);
             uint64_t req_clock = qm->req_clock;
+            double bgtime = pending ? idletime : sq->idle_time;
             double bgoffset = MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA;
             if (req_clock == BACKGROUND_PRIORITY_CLOCK)
-                req_clock = clock_from_time(&sq->ce, sq->idle_time + bgoffset);
+                req_clock = clock_from_time(&sq->ce, bgtime + bgoffset);
             if (req_clock < min_ready_clock)
                 min_ready_clock = req_clock;
         }
@@ -561,18 +578,21 @@ command_event(struct serialqueue *sq, double eventtime)
     int buflen = 0;
     double waketime;
     for (;;) {
-        waketime = check_send_command(sq, eventtime);
+        waketime = check_send_command(sq, buflen, eventtime);
         if (waketime != PR_NOW || buflen + MESSAGE_MAX > sizeof(buf)) {
             if (buflen) {
                 // Write message blocks
                 do_write(sq, buf, buflen);
                 sq->bytes_write += buflen;
+                double idletime = (eventtime > sq->idle_time
+                                   ? eventtime : sq->idle_time);
+                sq->idle_time = idletime + calculate_bittime(sq, buflen);
                 buflen = 0;
             }
             if (waketime != PR_NOW)
                 break;
         }
-        buflen += build_and_send_command(sq, &buf[buflen], eventtime);
+        buflen += build_and_send_command(sq, &buf[buflen], buflen, eventtime);
     }
     pthread_mutex_unlock(&sq->lock);
     return waketime;
@@ -847,10 +867,15 @@ exit:
 }
 
 void __visible
-serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust)
+serialqueue_set_wire_frequency(struct serialqueue *sq, double frequency)
 {
     pthread_mutex_lock(&sq->lock);
-    sq->baud_adjust = baud_adjust;
+    if (sq->serial_fd_type == SQT_CAN) {
+        sq->bittime_adjust = 1. / frequency;
+    } else {
+        // An 8N1 serial line is 10 bits per byte (1 start, 8 data, 1 stop)
+        sq->bittime_adjust = 10. / frequency;
+    }
     pthread_mutex_unlock(&sq->lock);
 }
 
diff --git a/klippy/chelper/serialqueue.h b/klippy/chelper/serialqueue.h
index 724a86a5f..4d447f2fb 100644
--- a/klippy/chelper/serialqueue.h
+++ b/klippy/chelper/serialqueue.h
@@ -42,7 +42,7 @@ void serialqueue_send(struct serialqueue *sq, struct command_queue *cq
                       , uint8_t *msg, int len, uint64_t min_clock
                       , uint64_t req_clock, uint64_t notify_id);
 void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm);
-void serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust);
+void serialqueue_set_wire_frequency(struct serialqueue *sq, double frequency);
 void serialqueue_set_receive_window(struct serialqueue *sq, int receive_window);
 void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
                                , double conv_time, uint64_t conv_clock
diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py
index b916ab20d..15d1cc21d 100644
--- a/klippy/serialhdl.py
+++ b/klippy/serialhdl.py
@@ -12,7 +12,6 @@ class error(Exception):
     pass
 
 class SerialReader:
-    BITS_PER_BYTE = 10.
     def __init__(self, reactor, warn_prefix=""):
         self.reactor = reactor
         self.warn_prefix = warn_prefix
@@ -97,11 +96,13 @@ class SerialReader:
         self.msgparser = msgparser
         self.register_response(self.handle_unknown, '#unknown')
         # Setup baud adjust
-        mcu_baud = msgparser.get_constant_float('SERIAL_BAUD', None)
-        if mcu_baud is not None:
-            baud_adjust = self.BITS_PER_BYTE / mcu_baud
-            self.ffi_lib.serialqueue_set_baud_adjust(
-                self.serialqueue, baud_adjust)
+        if serial_fd_type == b'c':
+            wire_freq = msgparser.get_constant_float('CANBUS_FREQUENCY', None)
+        else:
+            wire_freq = msgparser.get_constant_float('SERIAL_BAUD', None)
+        if wire_freq is not None:
+            self.ffi_lib.serialqueue_set_wire_frequency(self.serialqueue,
+                                                        wire_freq)
         receive_window = msgparser.get_constant_int('RECEIVE_WINDOW', None)
         if receive_window is not None:
             self.ffi_lib.serialqueue_set_receive_window(
diff --git a/src/generic/canbus.c b/src/generic/canbus.c
index a4f33aa1f..941ccd00b 100644
--- a/src/generic/canbus.c
+++ b/src/generic/canbus.c
@@ -4,8 +4,12 @@
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
+#include "autoconf.h" // CONFIG_CANBUS_FREQUENCY
 #include "canbus.h" // canbus_send
 #include "canserial.h" // canserial_send
+#include "command.h" // DECL_CONSTANT
+
+DECL_CONSTANT("CANBUS_FREQUENCY", CONFIG_CANBUS_FREQUENCY);
 
 int
 canserial_send(struct canbus_msg *msg)