diff --git a/core/containers/queue.c b/core/containers/queue.c
index 4d4c1b2f..f6c694da 100644
--- a/core/containers/queue.c
+++ b/core/containers/queue.c
@@ -4,6 +4,20 @@
 
 #include <core/containers/queue.h>
 
+guint _q_add_sorted(struct _queue *queue,  gpointer data,
+		    GCompareDataFunc func, gpointer user_data)
+{
+	struct _q_iter it;
+
+	_q_for_each(queue, &it) {
+		if (func(_q_iter_val(&it), data, user_data) > 0) {
+			g_queue_insert_before(&queue->_queue, it.it_iter, data);
+			return it.it_pos;
+		}
+	}
+	return _q_add_tail(queue, data);
+}
+
 gpointer _q_remove_it(struct _queue *queue, struct _q_iter *it)
 {
 	gpointer ret = _q_iter_val(it);
diff --git a/include/core/containers/queue.h b/include/core/containers/queue.h
index eecbcdc5..d38ca0ba 100644
--- a/include/core/containers/queue.h
+++ b/include/core/containers/queue.h
@@ -86,6 +86,9 @@ static inline guint _q_add_tail(struct _queue *queue, gpointer data)
 	return _q_size(queue) - 1;
 }
 
+/* Called to add an item to a sorted queue. */
+guint _q_add_sorted(struct _queue *, gpointer, GCompareDataFunc, gpointer);
+
 /* Called to remove an item by iterator. */
 gpointer _q_remove_it(struct _queue *, struct _q_iter *);
 
diff --git a/tests/core/containers/queue.c b/tests/core/containers/queue.c
index 0134f075..1bbeb4ea 100644
--- a/tests/core/containers/queue.c
+++ b/tests/core/containers/queue.c
@@ -107,18 +107,36 @@ void test_sort()
 	unsigned int i, N = 10;
 	struct _q_iter it;
 
-	for (i = 0; i < N; i++)
-		_q_add_head(&queue, GINT_TO_POINTER(i));
-
-	/* _q_sort() */
-	_q_sort(&queue, test_sort_int, GINT_TO_POINTER(42));
+	/* _q_add_sorted() (10 .. 19) */
+	for (i = 0; i < N; i++) {
+		test_loop_equal(_q_add_sorted(&queue, GINT_TO_POINTER(i + 10),
+					      test_sort_int,
+					      GINT_TO_POINTER(42)), i, i);
+	} test_loop_passed();
+	test_equal(_q_size(&queue), N);
 	test_equal(data_val, 42);
 
+	/* _q_add_sorted() (0 .. 9) */
+	for (i = 0; i < N; i++) {
+		test_loop_equal(_q_add_sorted(&queue, GINT_TO_POINTER(i),
+					      test_sort_int,
+					      GINT_TO_POINTER(43)), i, i);
+	} test_loop_passed();
+	test_equal(_q_size(&queue), 2 * N);
+	test_equal(data_val, 43);
+
+	/* _q_sort(), after reversing the queue. */
+	g_queue_reverse(&queue._queue);
+	_q_sort(&queue, test_sort_int, GINT_TO_POINTER(44));
+
 	i = 0;
 	_q_for_each(&queue, &it) {
 		test_loop_equal(test_q_iter_val(&it), i, i);
 		i++;
 	} test_loop_passed();
+	test_equal(data_val, 44);
+
+	g_queue_clear(&queue._queue);
 }
 
 DECLARE_UNIT_TESTS(