serialhdl: Fully deallocate serialqueue on disconnect

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

1 files changed, 34 insertions, 0 deletions

diff --git a/klippy/serialqueue.c b/klippy/serialqueue.c
index 23ed6858..6c1b9d63 100644
--- a/klippy/serialqueue.c
+++ b/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)