2 files changed, 100 insertions, 0 deletions

diff --git a/src/gqueue/gqueue.c b/src/gqueue/gqueue.c
index 8540bcea..45ca6fce 100644
--- a/src/gqueue/gqueue.c
+++ b/src/gqueue/gqueue.c
@@ -87,6 +87,30 @@ void _gqueueDeinit(void)
 			pqueue->tail = pitem;
 	}
 
+	void gfxQueueASyncInsert(gfxQueueASync *pqueue, gfxQueueASyncItem *pitem, gfxQueueASyncItem *pafter) {
+		gfxSystemLock();
+		gfxQueueASyncInsertI(pqueue, pitem, pafter);
+		gfxSystemUnlock();
+	}
+	void gfxQueueASyncInsertI(gfxQueueASync *pqueue, gfxQueueASyncItem *pitem, gfxQueueASyncItem *pafter) {
+		if (!pitem) return;				// Safety
+
+		if (pafter && gfxQueueASyncIsInI(pqueue, pafter)) {
+			pitem->next = pafter->next;
+			pafter->next = pitem;
+			if (pqueue->tail == pafter)
+				pqueue->tail = pitem;
+		} else {
+			pitem->next = 0;
+			if (!pqueue->head) {
+				pqueue->head = pqueue->tail = pitem;
+			} else {
+				pqueue->tail->next = pitem;
+				pqueue->tail = pitem;
+			}
+		}
+	}
+
 	void gfxQueueASyncRemove(gfxQueueASync *pqueue, gfxQueueASyncItem *pitem) {
 		gfxSystemLock();
 		gfxQueueASyncRemoveI(pqueue, pitem);
@@ -201,6 +225,30 @@ void _gqueueDeinit(void)
 		gfxSemSignalI(&pqueue->sem);
 	}
 
+	void gfxQueueGSyncInsert(gfxQueueGSync *pqueue, gfxQueueGSyncItem *pitem, gfxQueueASyncItem *pafter) {
+		gfxSystemLock();
+		gfxQueueGSyncInsertI(pqueue, pitem, pafter);
+		gfxSystemUnlock();
+	}
+	void gfxQueueGSyncInsertI(gfxQueueGSync *pqueue, gfxQueueGSyncItem *pitem, gfxQueueASyncItem *pafter) {
+		if (!pitem) return;				// Safety
+
+		if (pafter && gfxQueueGSyncIsInI(pqueue, pafter)) {
+			pitem->next = pafter->next;
+			pafter->next = pitem;
+			if (pqueue->tail == pafter)
+				pqueue->tail = pitem;
+		} else {
+			pitem->next = 0;
+			if (!pqueue->head) {
+				pqueue->head = pqueue->tail = pitem;
+			} else {
+				pqueue->tail->next = pitem;
+				pqueue->tail = pitem;
+			}
+		}
+	}
+
 	void gfxQueueGSyncRemove(gfxQueueGSync *pqueue, gfxQueueGSyncItem *pitem) {
 		gfxSystemLock();
 		gfxQueueGSyncRemoveI(pqueue, pitem);
@@ -312,6 +360,33 @@ void _gqueueDeinit(void)
 		return gfxSemWait(&pitem->sem, ms);
 	}
 
+	bool_t gfxQueueFSyncInsert(gfxQueueFSync *pqueue, gfxQueueFSyncItem *pitem, gfxQueueASyncItem *pafter, delaytime_t ms) {
+		if (!pitem) return;				// Safety
+		gfxSemInit(&pitem->sem, 0, 1);
+
+		gfxSystemLock();
+		if (pafter && gfxQueueGSyncIsInI(pqueue, pafter)) {
+			pitem->next = pafter->next;
+			pafter->next = pitem;
+			if (pqueue->tail == pafter)
+				pqueue->tail = pitem;
+		} else {
+			pitem->next = 0;
+			if (!pqueue->head) {
+				pqueue->head = pqueue->tail = pitem;
+			} else {
+				pqueue->tail->next = pitem;
+				pqueue->tail = pitem;
+			}
+		}
+		gfxSystemUnlock();
+
+		gfxSemSignal(&pqueue->sem);
+
+		return gfxSemWait(&pitem->sem, ms);
+
+	}
+
 	void gfxQueueFSyncRemove(gfxQueueFSync *pqueue, gfxQueueFSyncItem *pitem) {
 		gfxQueueFSyncItem *pi;
 
diff --git a/src/gqueue/sys_defs.h b/src/gqueue/sys_defs.h
index b97f2f4d..d0e46be1 100644
--- a/src/gqueue/sys_defs.h
+++ b/src/gqueue/sys_defs.h
@@ -199,6 +199,31 @@ bool_t gfxQueueFSyncPush(gfxQueueFSync *pqueue, gfxQueueFSyncItem *pitem, delayt
 /* @} */
 
 /**
+ * @brief	Insert an item on the queue after the specified item.
+ * @return	none for ASync and GSync queues; For FSync queues - FALSE on timeout, otherwise TRUE
+ *
+ * @param[in]	pqueue	A pointer to the queue
+ * @param[in]	pitem	A pointer to the queue item
+ * @param[in]	pafter	A pointer to the queue item this new item must be inserted after. If NULL or
+ * 							pafter can't be found in the queue, it puts the new item at the end of the queue.
+ * @param[in]	ms		The maxmimum time to wait for an item to be removed from the queue (only for FSync queues)
+ *
+ * @note		FSync: Use a delay time of TIME_IMMEDIATE if you don't want to wait until the
+ * 				item is removed from the queue. Note that even if the timeout occurs - the item
+ * 				remains in the queue.
+ * @note		The routines ending in "I" are interrupt/system/iclass level routines.
+ *
+ * @api
+ * @{
+ */
+void gfxQueueASyncInsert(gfxQueueASync *pqueue, gfxQueueASyncItem *pitem, gfxQueueASyncItem *pafter);
+void gfxQueueASyncInsertI(gfxQueueASync *pqueue, gfxQueueASyncItem *pitem, gfxQueueASyncItem *pafter);
+void gfxQueueGSyncInsert(gfxQueueGSync *pqueue, gfxQueueGSyncItem *pitem, gfxQueueASyncItem *pafter);
+void gfxQueueGSyncInsertI(gfxQueueGSync *pqueue, gfxQueueGSyncItem *pitem, gfxQueueASyncItem *pafter);
+bool_t gfxQueueFSyncInsert(gfxQueueFSync *pqueue, gfxQueueFSyncItem *pitem, gfxQueueASyncItem *pafter, delaytime_t ms);
+/* @} */
+
+/**
  * @brief	Remove an item from the queue.
  * @note	Removes the specified item from the queue where-ever it is in the queue
  *