diff --git a/Makefile.am b/Makefile.am
index f8173f6..07492b0 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -149,6 +149,7 @@ src_bluetoothd_SOURCES = $(builtin_sources) \
 			src/device.h src/device.c src/attio.h \
 			src/dbus-common.c src/dbus-common.h \
 			src/eir.h src/eir.c \
+			src/shared/queue.h src/shared/queue.c \
 			src/shared/util.h src/shared/util.c \
 			src/shared/mgmt.h src/shared/mgmt.c
 src_bluetoothd_LDADD = lib/libbluetooth-internal.la gdbus/libgdbus-internal.la \
@@ -237,6 +238,7 @@ unit_test_crc_LDADD = @GLIB_LIBS@
 unit_tests += unit/test-mgmt
 
 unit_test_mgmt_SOURCES = unit/test-mgmt.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c
 unit_test_mgmt_LDADD = @GLIB_LIBS@
diff --git a/Makefile.tools b/Makefile.tools
index c78cc50..32e08c3 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -61,6 +61,7 @@ tools_3dsp_SOURCES = tools/3dsp.c monitor/bt.h \
 tools_mgmt_tester_SOURCES = tools/mgmt-tester.c monitor/bt.h \
 				emulator/btdev.h emulator/btdev.c \
 				emulator/bthost.h emulator/bthost.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c \
 				src/shared/hciemu.h src/shared/hciemu.c \
@@ -70,6 +71,7 @@ tools_mgmt_tester_LDADD = lib/libbluetooth-internal.la @GLIB_LIBS@
 tools_l2cap_tester_SOURCES = tools/l2cap-tester.c monitor/bt.h \
 				emulator/btdev.h emulator/btdev.c \
 				emulator/bthost.h emulator/bthost.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c \
 				src/shared/hciemu.h src/shared/hciemu.c \
@@ -79,6 +81,7 @@ tools_l2cap_tester_LDADD = lib/libbluetooth-internal.la @GLIB_LIBS@
 tools_smp_tester_SOURCES = tools/smp-tester.c monitor/bt.h \
 				emulator/btdev.h emulator/btdev.c \
 				emulator/bthost.h emulator/bthost.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c \
 				src/shared/hciemu.h src/shared/hciemu.c \
@@ -95,6 +98,7 @@ tools_gap_tester_LDADD = gdbus/libgdbus-internal.la @GLIB_LIBS@ @DBUS_LIBS@
 tools_sco_tester_SOURCES = tools/sco-tester.c monitor/bt.h \
 				emulator/btdev.h emulator/btdev.c \
 				emulator/bthost.h emulator/bthost.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c \
 				src/shared/hciemu.h src/shared/hciemu.c \
@@ -215,6 +219,7 @@ tools_hwdb_LDADD = lib/libbluetooth-internal.la
 tools_hcieventmask_LDADD = lib/libbluetooth-internal.la
 
 tools_btmgmt_SOURCES = tools/btmgmt.c src/glib-helper.c src/eir.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c
 tools_btmgmt_LDADD = lib/libbluetooth-internal.la @GLIB_LIBS@
diff --git a/android/Android.mk b/android/Android.mk
index 8eb918e..dc9a7b2 100644
--- a/android/Android.mk
+++ b/android/Android.mk
@@ -33,6 +33,7 @@ LOCAL_SRC_FILES := \
 	../src/log.c \
 	../src/shared/mgmt.c \
 	../src/shared/util.c \
+	../src/shared/queue.c \
 	../src/sdpd-database.c \
 	../src/sdpd-service.c \
 	../src/sdpd-request.c \
diff --git a/android/Makefile.am b/android/Makefile.am
index 88ffa7f..77e343e 100644
--- a/android/Makefile.am
+++ b/android/Makefile.am
@@ -21,6 +21,7 @@ android_bluetoothd_SOURCES = android/main.c \
 				src/sdpd-service.c src/sdpd-request.c \
 				src/glib-helper.h src/glib-helper.c \
 				src/eir.h src/eir.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c \
 				android/bluetooth.h android/bluetooth.c \
@@ -96,6 +97,7 @@ noinst_PROGRAMS += android/android-tester
 
 android_android_tester_SOURCES = emulator/btdev.h emulator/btdev.c \
 				emulator/bthost.h emulator/bthost.c \
+				src/shared/queue.h src/shared/queue.c \
 				src/shared/util.h src/shared/util.c \
 				src/shared/mgmt.h src/shared/mgmt.c \
 				src/shared/hciemu.h src/shared/hciemu.c \
diff --git a/src/shared/hci.c b/src/shared/hci.c
index 77d7b5c..9114c7b 100644
--- a/src/shared/hci.c
+++ b/src/shared/hci.c
@@ -535,8 +535,8 @@ bool bt_hci_flush(struct bt_hci *hci)
 
 	suspend_writer(hci);
 
-	queue_remove_all(hci->cmd_queue, cmd_free);
-	queue_remove_all(hci->rsp_queue, cmd_free);
+	queue_remove_all(hci->cmd_queue, NULL, NULL, cmd_free);
+	queue_remove_all(hci->rsp_queue, NULL, NULL, cmd_free);
 
 	return true;
 }
diff --git a/src/shared/mgmt.c b/src/shared/mgmt.c
index 2c79886..0e1b7f9 100644
--- a/src/shared/mgmt.c
+++ b/src/shared/mgmt.c
@@ -36,6 +36,7 @@
 #include "lib/mgmt.h"
 #include "lib/hci.h"
 
+#include "src/shared/queue.h"
 #include "src/shared/util.h"
 #include "src/shared/mgmt.h"
 
@@ -46,8 +47,8 @@ struct mgmt {
 	GIOChannel *io;
 	guint read_watch;
 	guint write_watch;
-	GQueue *request_queue;
-	GQueue *reply_queue;
+	struct queue *request_queue;
+	struct queue *reply_queue;
 	GList *pending_list;
 	GList *notify_list;
 	GList *notify_destroyed;
@@ -83,7 +84,7 @@ struct mgmt_notify {
 	void *user_data;
 };
 
-static void destroy_request(gpointer data, gpointer user_data)
+static void destroy_request(void *data)
 {
 	struct mgmt_request *request = data;
 
@@ -94,6 +95,27 @@ static void destroy_request(gpointer data, gpointer user_data)
 	g_free(request);
 }
 
+static void destroy_request_from_list(gpointer data, gpointer user_data)
+{
+	destroy_request(data);
+}
+
+static bool match_request_id(const void *a, const void *b)
+{
+	const struct mgmt_request *request = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return request->id == id;
+}
+
+static bool match_request_index(const void *a, const void *b)
+{
+	const struct mgmt_request *request = a;
+	uint16_t index = PTR_TO_UINT(b);
+
+	return request->index == index;
+}
+
 static int compare_request_id(gconstpointer a, gconstpointer b)
 {
 	const struct mgmt_request *request = a;
@@ -137,13 +159,13 @@ static gboolean can_write_data(GIOChannel *channel, GIOCondition cond,
 	if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL))
 		return FALSE;
 
-	request = g_queue_pop_head(mgmt->reply_queue);
+	request = queue_pop_head(mgmt->reply_queue);
 	if (!request) {
 		/* only reply commands can jump the queue */
 		if (mgmt->pending_list)
 			return FALSE;
 
-		request = g_queue_pop_head(mgmt->request_queue);
+		request = queue_pop_head(mgmt->request_queue);
 		if (!request)
 			return FALSE;
 	}
@@ -155,7 +177,7 @@ static gboolean can_write_data(GIOChannel *channel, GIOCondition cond,
 		if (request->callback)
 			request->callback(MGMT_STATUS_FAILED, 0, NULL,
 							request->user_data);
-		destroy_request(request, NULL);
+		destroy_request(request);
 		return TRUE;
 	}
 
@@ -175,7 +197,7 @@ static void wakeup_writer(struct mgmt *mgmt)
 {
 	if (mgmt->pending_list) {
 		/* only queued reply commands trigger wakeup */
-		if (g_queue_get_length(mgmt->reply_queue) == 0)
+		if (queue_isempty(mgmt->reply_queue))
 			return;
 	}
 
@@ -220,7 +242,7 @@ static void request_complete(struct mgmt *mgmt, uint8_t status,
 	if (request->callback)
 		request->callback(status, length, param, request->user_data);
 
-	destroy_request(request, NULL);
+	destroy_request(request);
 
 	if (mgmt->destroyed)
 		return;
@@ -370,8 +392,21 @@ struct mgmt *mgmt_new(int fd)
 	g_io_channel_set_encoding(mgmt->io, NULL, NULL);
 	g_io_channel_set_buffered(mgmt->io, FALSE);
 
-	mgmt->request_queue = g_queue_new();
-	mgmt->reply_queue = g_queue_new();
+	mgmt->request_queue = queue_new();
+	if (!mgmt->request_queue) {
+		g_free(mgmt->buf);
+		g_free(mgmt);
+		return NULL;
+	}
+
+	mgmt->reply_queue = queue_new();
+	if (!mgmt->reply_queue) {
+		queue_destroy(mgmt->request_queue, NULL);
+		g_free(mgmt->buf);
+		g_free(mgmt);
+		return NULL;
+	}
+
 
 	mgmt->read_watch = g_io_add_watch_full(mgmt->io, G_PRIORITY_DEFAULT,
 				G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
@@ -433,8 +468,8 @@ void mgmt_unref(struct mgmt *mgmt)
 	mgmt_unregister_all(mgmt);
 	mgmt_cancel_all(mgmt);
 
-	g_queue_free(mgmt->reply_queue);
-	g_queue_free(mgmt->request_queue);
+	queue_destroy(mgmt->reply_queue, NULL);
+	queue_destroy(mgmt->request_queue, NULL);
 
 	if (mgmt->write_watch > 0)
 		g_source_remove(mgmt->write_watch);
@@ -551,7 +586,11 @@ unsigned int mgmt_send(struct mgmt *mgmt, uint16_t opcode, uint16_t index,
 
 	request->id = mgmt->next_request_id++;
 
-	g_queue_push_tail(mgmt->request_queue, request);
+	if (!queue_push_tail(mgmt->request_queue, request)) {
+		g_free(request->buf);
+		g_free(request);
+		return 0;
+	}
 
 	wakeup_writer(mgmt);
 
@@ -578,7 +617,11 @@ unsigned int mgmt_reply(struct mgmt *mgmt, uint16_t opcode, uint16_t index,
 
 	request->id = mgmt->next_request_id++;
 
-	g_queue_push_tail(mgmt->reply_queue, request);
+	if (!queue_push_tail(mgmt->reply_queue, request)) {
+		g_free(request->buf);
+		g_free(request);
+		return 0;
+	}
 
 	wakeup_writer(mgmt);
 
@@ -593,21 +636,15 @@ bool mgmt_cancel(struct mgmt *mgmt, unsigned int id)
 	if (!mgmt || !id)
 		return false;
 
-	list = g_queue_find_custom(mgmt->request_queue, GUINT_TO_POINTER(id),
-							compare_request_id);
-	if (list) {
-		request = list->data;
-		g_queue_delete_link(mgmt->request_queue, list);
+	request = queue_remove_if(mgmt->request_queue, match_request_id,
+							UINT_TO_PTR(id));
+	if (request)
 		goto done;
-	}
 
-	list = g_queue_find_custom(mgmt->reply_queue, GUINT_TO_POINTER(id),
-							compare_request_id);
-	if (list) {
-		request = list->data;
-		g_queue_delete_link(mgmt->reply_queue, list);
+	request = queue_remove_if(mgmt->reply_queue, match_request_id,
+							UINT_TO_PTR(id));
+	if (request)
 		goto done;
-	}
 
 	list = g_list_find_custom(mgmt->pending_list, GUINT_TO_POINTER(id),
 							compare_request_id);
@@ -619,7 +656,7 @@ bool mgmt_cancel(struct mgmt *mgmt, unsigned int id)
 	mgmt->pending_list = g_list_delete_link(mgmt->pending_list, list);
 
 done:
-	destroy_request(request, NULL);
+	destroy_request(request);
 
 	wakeup_writer(mgmt);
 
@@ -633,33 +670,11 @@ bool mgmt_cancel_index(struct mgmt *mgmt, uint16_t index)
 	if (!mgmt)
 		return false;
 
-	for (list = g_queue_peek_head_link(mgmt->request_queue); list;
-								list = next) {
-		struct mgmt_request *request = list->data;
+	queue_remove_all(mgmt->request_queue, match_request_index,
+					UINT_TO_PTR(index), destroy_request);
 
-		next = g_list_next(list);
-
-		if (request->index != index)
-			continue;
-
-		g_queue_delete_link(mgmt->request_queue, list);
-
-		destroy_request(request, NULL);
-	}
-
-	for (list = g_queue_peek_head_link(mgmt->reply_queue); list;
-								list = next) {
-		struct mgmt_request *request = list->data;
-
-		next = g_list_next(list);
-
-		if (request->index != index)
-			continue;
-
-		g_queue_delete_link(mgmt->reply_queue, list);
-
-		destroy_request(request, NULL);
-	}
+	queue_remove_all(mgmt->reply_queue, match_request_index,
+					UINT_TO_PTR(index), destroy_request);
 
 	for (list = g_list_first(mgmt->pending_list); list; list = next) {
 		struct mgmt_request *request = list->data;
@@ -672,7 +687,7 @@ bool mgmt_cancel_index(struct mgmt *mgmt, uint16_t index)
 		mgmt->pending_list = g_list_delete_link(mgmt->pending_list,
 									list);
 
-		destroy_request(request, NULL);
+		destroy_request(request);
 	}
 
 	return true;
@@ -683,15 +698,12 @@ bool mgmt_cancel_all(struct mgmt *mgmt)
 	if (!mgmt)
 		return false;
 
-	g_list_foreach(mgmt->pending_list, destroy_request, NULL);
+	g_list_foreach(mgmt->pending_list, destroy_request_from_list, NULL);
 	g_list_free(mgmt->pending_list);
 	mgmt->pending_list = NULL;
 
-	g_queue_foreach(mgmt->reply_queue, destroy_request, NULL);
-	g_queue_clear(mgmt->reply_queue);
-
-	g_queue_foreach(mgmt->request_queue, destroy_request, NULL);
-	g_queue_clear(mgmt->request_queue);
+	queue_remove_all(mgmt->reply_queue, NULL, NULL, destroy_request);
+	queue_remove_all(mgmt->request_queue, NULL, NULL, destroy_request);
 
 	return true;
 }
diff --git a/src/shared/queue.c b/src/shared/queue.c
index bfc8da0..d73a535 100644
--- a/src/shared/queue.c
+++ b/src/shared/queue.c
@@ -232,13 +232,17 @@ void *queue_remove_if(struct queue *queue, queue_match_func_t function,
 	return NULL;
 }
 
-bool queue_remove_all(struct queue *queue, queue_destroy_func_t destroy)
+bool queue_remove_all(struct queue *queue, queue_match_func_t function,
+				void *user_data, queue_destroy_func_t destroy)
 {
 	struct queue_entry *entry;
 
 	if (!queue)
 		return false;
 
+	if (function)
+		return false;
+
 	entry = queue->head;
 
 	while (entry) {
diff --git a/src/shared/queue.h b/src/shared/queue.h
index 151e60c..201985a 100644
--- a/src/shared/queue.h
+++ b/src/shared/queue.h
@@ -48,7 +48,8 @@ void *queue_find(struct queue *queue, queue_match_func_t function,
 
 void *queue_remove_if(struct queue *queue, queue_match_func_t function,
 							void *user_data);
-bool queue_remove_all(struct queue *queue, queue_destroy_func_t destroy);
+bool queue_remove_all(struct queue *queue, queue_match_func_t function,
+				void *user_data, queue_destroy_func_t destroy);
 
 unsigned int queue_length(struct queue *queue);
 bool queue_isempty(struct queue *queue);