serialhdl: Fully deallocate serialqueue on disconnect

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

klippy/chelper.py

@@ -51,6 +51,7 @@ defs_serialqueue = """
 
     struct serialqueue *serialqueue_alloc(int serial_fd, int write_only);
     void serialqueue_exit(struct serialqueue *sq);
+    void serialqueue_free(struct serialqueue *sq);
     struct command_queue *serialqueue_alloc_commandqueue(void);
     void serialqueue_send(struct serialqueue *sq, struct command_queue *cq
         , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock);

klippy/serialhdl.py

@@ -95,12 +95,15 @@ class SerialReader:
             self.serialqueue, self.est_clock, self.last_ack_time
             , self.last_ack_clock)
     def disconnect(self):
+        if self.serialqueue is None:
+            return
         self.send_flush()
         time.sleep(0.010)
-        if self.ffi_lib is not None:
+        self.ffi_lib.serialqueue_exit(self.serialqueue)
-            self.ffi_lib.serialqueue_exit(self.serialqueue)
         if self.background_thread is not None:
             self.background_thread.join()
+        self.ffi_lib.serialqueue_free(self.serialqueue)
+        self.background_thread = self.serialqueue = None
     def stats(self, eventtime):
         if self.serialqueue is None:
             return ""

klippy/serialqueue.c

@@ -438,6 +438,18 @@ debug_queue_add(struct list_head *root, struct queue_message *qm)
     message_free(old);
 }
 
+// Free all the messages on a queue
+static void
+queue_free(struct list_head *root)
+{
+    while (!list_empty(root)) {
+        struct queue_message *qm = list_first_entry(
+            root, struct queue_message, node);
+        list_del(&qm->node);
+        message_free(qm);
+    }
+}
+
 // Wake up the receiver thread if it is waiting
 static void
 check_wake_receive(struct serialqueue *sq)
@@ -847,6 +859,28 @@ serialqueue_exit(struct serialqueue *sq)
         report_errno("pthread_join", ret);
 }
 
+// Free all resources associated with a serialqueue
+void
+serialqueue_free(struct serialqueue *sq)
+{
+    if (!pollreactor_is_exit(&sq->pr))
+        serialqueue_exit(sq);
+    pthread_mutex_lock(&sq->lock);
+    queue_free(&sq->sent_queue);
+    queue_free(&sq->receive_queue);
+    queue_free(&sq->old_sent);
+    queue_free(&sq->old_receive);
+    while (!list_empty(&sq->pending_queues)) {
+        struct command_queue *cq = list_first_entry(
+            &sq->pending_queues, struct command_queue, node);
+        list_del(&cq->node);
+        queue_free(&cq->ready_queue);
+        queue_free(&cq->stalled_queue);
+    }
+    pthread_mutex_unlock(&sq->lock);
+    free(sq);
+}
+
 // Allocate a 'struct command_queue'
 struct command_queue *
 serialqueue_alloc_commandqueue(void)

klippy/serialqueue.h

@@ -45,6 +45,7 @@ struct pull_queue_message {
 struct serialqueue;
 struct serialqueue *serialqueue_alloc(int serial_fd, int write_only);
 void serialqueue_exit(struct serialqueue *sq);
+void serialqueue_free(struct serialqueue *sq);
 struct command_queue *serialqueue_alloc_commandqueue(void);
 void serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq
                             , struct list_head *msgs);