From 5e864ecde98fbb2f115f75a3c5c13fa871f4d3f9 Mon Sep 17 00:00:00 2001
From: Aidan MacDonald <amachronic@protonmail.com>
Date: Sun, 31 Jul 2022 16:23:55 +0100
Subject: [PATCH] linked list: add lld_insert_prev/next()

Change-Id: I55ff061ccc0e6e2dad3125a8150c082c163db329
---
 firmware/common/linked_list.c  | 32 ++++++++++++++++++++++++++++++++
 firmware/include/linked_list.h |  4 ++++
 2 files changed, 36 insertions(+)

diff --git a/firmware/common/linked_list.c b/firmware/common/linked_list.c
index 7697a052ca..30e4e6079a 100644
--- a/firmware/common/linked_list.c
+++ b/firmware/common/linked_list.c
@@ -133,6 +133,38 @@ void ll_remove(struct ll_head *list, struct ll_node *node)
 
 /** (L)inked (L)ist (D)ouble **/
 
+void lld_insert_next(struct lld_head *list, struct lld_node *node,
+                     struct lld_node *newnode)
+{
+    struct lld_node **nodep = node != NULL ? &node->next : &list->head;
+    struct lld_node *next = *nodep;
+
+    newnode->next = next;
+    newnode->prev = node;
+    *nodep = newnode;
+
+    if (next == NULL)
+        list->tail = newnode;
+    else
+        next->prev = newnode;
+}
+
+void lld_insert_prev(struct lld_head *list, struct lld_node *node,
+                     struct lld_node *newnode)
+{
+    struct lld_node **nodep = node != NULL ? &node->prev : &list->tail;
+    struct lld_node *prev = *nodep;
+
+    newnode->next = node;
+    newnode->prev = prev;
+    *nodep = newnode;
+
+    if (prev == NULL)
+        list->head = newnode;
+    else
+        prev->next = newnode;
+}
+
 /**
  * Adds a node to a doubly-linked list using "insert first"
  */
diff --git a/firmware/include/linked_list.h b/firmware/include/linked_list.h
index b307977f2e..1e077be22a 100644
--- a/firmware/include/linked_list.h
+++ b/firmware/include/linked_list.h
@@ -103,6 +103,10 @@ static inline void lld_init(struct lld_head *list)
        the routines and maintains the non-circularity */
 }
 
+void lld_insert_next(struct lld_head *list, struct lld_node *node,
+                     struct lld_node *newnode);
+void lld_insert_prev(struct lld_head *list, struct lld_node *node,
+                     struct lld_node *newnode);
 void lld_insert_first(struct lld_head *list, struct lld_node *node);
 void lld_insert_last(struct lld_head *list, struct lld_node *node);
 void lld_remove(struct lld_head *list, struct lld_node *node);