diff --git a/src/libhidpp/hidpp/Dispatcher.cpp b/src/libhidpp/hidpp/Dispatcher.cpp
index 721b421..be00349 100644
--- a/src/libhidpp/hidpp/Dispatcher.cpp
+++ b/src/libhidpp/hidpp/Dispatcher.cpp
@@ -38,3 +38,23 @@ Dispatcher::~Dispatcher ()
 {
 }
 
+Dispatcher::listener_iterator Dispatcher::registerEventHandler (DeviceIndex index, uint8_t sub_id, const event_handler &handler)
+{
+	return _listeners.emplace (std::make_tuple (index, sub_id), handler);
+}
+
+void Dispatcher::unregisterEventHandler (listener_iterator it)
+{
+	_listeners.erase (it);
+}
+
+void Dispatcher::processEvent (const Report &report)
+{
+	auto range = _listeners.equal_range (std::make_tuple (report.deviceIndex (), report.subID ()));
+	for (auto it = range.first; it != range.second;) {
+		if (it->second (report))
+			++it;
+		else
+			it = _listeners.erase (it);
+	}
+}
diff --git a/src/libhidpp/hidpp/Dispatcher.h b/src/libhidpp/hidpp/Dispatcher.h
index e63a29a..4b251df 100644
--- a/src/libhidpp/hidpp/Dispatcher.h
+++ b/src/libhidpp/hidpp/Dispatcher.h
@@ -21,6 +21,8 @@
 
 #include <hidpp/Report.h>
 #include <memory>
+#include <map>
+#include <functional>
 
 namespace HIDPP
 {
@@ -28,6 +30,10 @@ namespace HIDPP
 class Dispatcher
 {
 public:
+	typedef std::function<bool (const Report &)> event_handler;
+	typedef std::multimap<std::tuple<DeviceIndex, uint8_t>, event_handler> listener_container;
+	typedef listener_container::iterator listener_iterator;
+
 	/**
 	 * Exception when no HID++ report is found in the report descriptor.
 	 */
@@ -97,7 +103,27 @@ class Dispatcher
 	 */
 	virtual std::unique_ptr<AsyncReport> getNotification (DeviceIndex index, uint8_t sub_id) = 0;
 
+	/**
+	 * Add a listener function for events matching \p index and \p sub_id.
+	 *
+	 * \param index		Event device index
+	 * \param sub_id	Event sub_id (or feature index)
+	 * \param handler	Callback for handling the event
+	 *
+	 * \returns The listener iterator used for unregistering.
+	 */
+	virtual listener_iterator registerEventHandler (DeviceIndex index, uint8_t sub_id, const event_handler &handler);
+
+	/**
+	 * Unregister the event handler given by the iterator.
+	 */
+	virtual void unregisterEventHandler (listener_iterator it);
+
+protected:
+	void processEvent (const Report &);
+
 private:
+	listener_container _listeners;
 };
 
 }
diff --git a/src/libhidpp/hidpp/DispatcherThread.cpp b/src/libhidpp/hidpp/DispatcherThread.cpp
index e3282f0..65eccb4 100644
--- a/src/libhidpp/hidpp/DispatcherThread.cpp
+++ b/src/libhidpp/hidpp/DispatcherThread.cpp
@@ -26,14 +26,16 @@
 
 using namespace HIDPP;
 
-template<typename Container, Container DispatcherThread::*container>
+template<typename Iterator,
+	 void (DispatcherThread::*cancel) (Iterator),
+	 std::mutex DispatcherThread::*mutex>
 class DispatcherThread::AsyncReport: public Dispatcher::AsyncReport
 {
 	DispatcherThread *dispatcher;
 	std::future<Report> report;
-	typename Container::iterator it;
+	Iterator it;
 public:
-	AsyncReport (DispatcherThread *dispatcher, std::future<Report> &&report, typename Container::iterator it):
+	AsyncReport (DispatcherThread *dispatcher, std::future<Report> &&report, Iterator it):
 		dispatcher (dispatcher), report (std::move (report)), it (it)
 	{
 	}
@@ -47,12 +49,12 @@ class DispatcherThread::AsyncReport: public Dispatcher::AsyncReport
 	{
 		report.wait_for (std::chrono::milliseconds (timeout));
 		{
-			std::unique_lock<std::mutex> lock (dispatcher->_mutex);
+			std::unique_lock<std::mutex> lock (dispatcher->*mutex);
 			// make sure there was no race before the lock.
 			auto status = report.wait_for (std::chrono::milliseconds (0));
 			if (status != std::future_status::ready) {
 				// cancel the command
-				(dispatcher->*container).erase (it);
+				(dispatcher->*cancel) (it);
 				throw Dispatcher::TimeoutError ();
 			}
 		}
@@ -62,19 +64,15 @@ class DispatcherThread::AsyncReport: public Dispatcher::AsyncReport
 
 DispatcherThread::DispatcherThread (const char *path):
 	_dev (path),
-	_running (true)
+	_stopped (false)
 {
 	const HIDRaw::ReportDescriptor &rdesc = _dev.getReportDescriptor ();
 	if (!checkReportDescriptor (rdesc))
 		throw Dispatcher::NoHIDPPReportException ();
-	_thread = std::thread (&DispatcherThread::run, this);
 }
 
 DispatcherThread::~DispatcherThread ()
 {
-	_running = false;
-	_dev.interruptRead ();
-	_thread.join ();
 }
 
 const HIDRaw &DispatcherThread::hidraw () const
@@ -104,51 +102,54 @@ void DispatcherThread::sendCommandWithoutResponse (const Report &report)
 
 std::unique_ptr<Dispatcher::AsyncReport> DispatcherThread::sendCommand (Report &&report)
 {
-	std::unique_lock<std::mutex> lock (_mutex);
-	if (!_running)
+	std::unique_lock<std::mutex> lock (_command_mutex);
+	if (_stopped)
 		throw _exception;
 	_dev.writeReport (report.rawReport ());
 	auto it = _commands.insert (_commands.end (), Command { std::move (report) });
-	return std::make_unique<CommandResponse> (this, it->promised_report.get_future (), it);
+	return std::make_unique<AsyncCommandResponse> (this, it->response.get_future (), it);
 }
 
 std::unique_ptr<Dispatcher::AsyncReport> DispatcherThread::getNotification (DeviceIndex index, uint8_t sub_id)
 {
-	std::unique_lock<std::mutex> lock (_mutex);
-	if (!_running)
+	std::unique_lock<std::mutex> lock (_listener_mutex);
+	if (_stopped)
 		throw _exception;
-	auto promise = std::make_shared<std::promise<Report>> ();
-	std::future<Report> future = promise->get_future ();
-	auto it = _listeners.emplace (std::make_tuple (index, sub_id),
-		Listener ([promise] (const Report &report) { promise->set_value (report); },
-			  [promise] (std::exception_ptr e) { promise->set_exception (e); },
-			  true));
-	return std::make_unique<Notification> (this, std::move (future), it);
+	auto it = _notifications.emplace (_notifications.end ());
+	it->listener = Dispatcher::registerEventHandler (index, sub_id, [this, it] (const Report &report) {
+		it->notification.set_value (report);
+		_notifications.erase (it);
+		return false;
+	});
+	return std::make_unique<AsyncNotification> (this, it->notification.get_future (), it);
+}
+
+DispatcherThread::listener_iterator DispatcherThread::registerEventHandler (DeviceIndex index, uint8_t sub_id, const event_handler &handler)
+{
+	std::unique_lock<std::mutex> lock (_listener_mutex);
+	return Dispatcher::registerEventHandler (index, sub_id, handler);
 }
 
-DispatcherThread::listener_iterator DispatcherThread::registerEventQueue (DeviceIndex index, uint8_t sub_id, EventQueue<Report> *queue)
+void DispatcherThread::unregisterEventHandler (listener_iterator it)
 {
-	std::unique_lock<std::mutex> lock (_mutex);
-	return _listeners.emplace (std::make_tuple (index, sub_id),
-		Listener (std::bind (&EventQueue<Report>::push, queue, std::placeholders::_1),
-			  [queue] (std::exception_ptr) { queue->interrupt (); },
-			  false));
+	std::unique_lock<std::mutex> lock (_listener_mutex);
+	Dispatcher::unregisterEventHandler (it);
 }
 
-void DispatcherThread::unregisterEventQueue (listener_iterator it)
+void DispatcherThread::cancelCommand (command_iterator it)
 {
-	std::unique_lock<std::mutex> lock (_mutex);
-	_listeners.erase (it);
+	_commands.erase (it);
 }
 
-bool DispatcherThread::running () const
+void DispatcherThread::cancelNotification (notification_iterator it)
 {
-	return _running;
+	Dispatcher::unregisterEventHandler (it->listener);
+	_notifications.erase (it);
 }
 
 void DispatcherThread::run ()
 {
-	while (_running) {
+	while (!_stopped) {
 		try {
 			std::vector<uint8_t> raw_report (Report::MaxDataLength+1);
 			if (0 != _dev.readReport (raw_report))
@@ -168,27 +169,35 @@ void DispatcherThread::run ()
 	}
 	_exception = std::make_exception_ptr (NotRunning ());
 stop:
-	_running = false;
+	_stopped = true;
 	{
-		std::unique_lock<std::mutex> lock (_mutex);
+		std::unique_lock<std::mutex> lock (_command_mutex);
 		if (!_commands.empty ()) {
-			Log::warning () << "Unfinished commands while stopping dispatcher thread." << std::endl;
+			Log::warning () << "Unfinished commands while stopping dispatcher." << std::endl;
 			for (auto &cmd: _commands) {
-				cmd.promised_report.set_exception (_exception);
+				cmd.response.set_exception (_exception);
 			}
 		}
-		if (!_listeners.empty ()) {
-			Log::warning () << "Registered listeners while stopping dispatcher thread." << std::endl;
-			for (const auto &l: _listeners) {
-				l.second.except (_exception);
+	}
+	{
+		std::unique_lock<std::mutex> lock (_listener_mutex);
+		if (!_notifications.empty ()) {
+			Log::warning () << "Unreceived notifications while stopping dispatcher." << std::endl;
+			for (auto &n: _notifications) {
+				n.notification.set_exception (_exception);
 			}
 		}
 	}
 }
 
+void DispatcherThread::stop ()
+{
+	_stopped = true;
+	_dev.interruptRead ();
+}
+
 void DispatcherThread::processReport (std::vector<uint8_t> &&raw_report)
 {
-	std::unique_lock<std::mutex> lock (_mutex);
 
 	Report report (std::move (raw_report));
 	DeviceIndex index = report.deviceIndex ();
@@ -197,43 +206,46 @@ void DispatcherThread::processReport (std::vector<uint8_t> &&raw_report)
 	unsigned int function, sw_id;
 
 	if (report.checkErrorMessage10 (&sub_id, &address, &error_code)) {
+		std::unique_lock<std::mutex> lock (_command_mutex);
 		auto it = std::find_if (_commands.begin (), _commands.end (),
 			[index, sub_id, address] (const Command &cmd) {
-				return index == cmd.report.deviceIndex () &&
-					sub_id == cmd.report.subID () &&
-					address == cmd.report.address ();
+				return index == cmd.request.deviceIndex () &&
+					sub_id == cmd.request.subID () &&
+					address == cmd.request.address ();
 			});
 		if (it != _commands.end ()) {
-			it->promised_report.set_exception (std::make_exception_ptr (HIDPP10::Error (error_code)));
+			it->response.set_exception (std::make_exception_ptr (HIDPP10::Error (error_code)));
 			_commands.erase (it);
 		}
 		else
 			Log::warning () << "HID++1.0 error message was not matched with any command." << std::endl;
 	}
 	else if (report.checkErrorMessage20 (&feature, &function, &sw_id, &error_code)) {
+		std::unique_lock<std::mutex> lock (_command_mutex);
 		auto it = std::find_if (_commands.begin (), _commands.end (),
 			[index, feature, function, sw_id] (const Command &cmd) {
-				return index == cmd.report.deviceIndex () &&
-					feature == cmd.report.featureIndex () &&
-					function == cmd.report.function () &&
-					sw_id == cmd.report.softwareID ();
+				return index == cmd.request.deviceIndex () &&
+					feature == cmd.request.featureIndex () &&
+					function == cmd.request.function () &&
+					sw_id == cmd.request.softwareID ();
 			});
 		if (it != _commands.end ()) {
-			it->promised_report.set_exception (std::make_exception_ptr (HIDPP20::Error (error_code)));
+			it->response.set_exception (std::make_exception_ptr (HIDPP20::Error (error_code)));
 			_commands.erase (it);
 		}
 		else
 			Log::warning () << "HID++2.0 error message was not matched with any command." << std::endl;
 	}
 	else {
+		std::unique_lock<std::mutex> cmd_lock (_command_mutex);
 		auto it = std::find_if (_commands.begin (), _commands.end (),
 			[&report] (const Command &cmd) {
-				return report.deviceIndex () == cmd.report.deviceIndex () &&
-					report.subID () == cmd.report.subID () &&
-					report.address () == cmd.report.address ();
+				return report.deviceIndex () == cmd.request.deviceIndex () &&
+					report.subID () == cmd.request.subID () &&
+					report.address () == cmd.request.address ();
 			});
 		if (it != _commands.end ()) {
-			it->promised_report.set_value (std::move (report));
+			it->response.set_value (std::move (report));
 			_commands.erase (it);
 		}
 		else if (report.softwareID () == 0 || report.subID () < 0x80) { // is an event
@@ -243,14 +255,9 @@ void DispatcherThread::processReport (std::vector<uint8_t> &&raw_report)
 			// But the lowest known HID++1.0 notification is 0x40,
 			// if no HID++2.0 device has more than 64 features,
 			// there should be no confusion in practice.
-			auto range = _listeners.equal_range (std::make_tuple (report.deviceIndex (), report.subID ()));
-			for (auto it = range.first; it != range.second;) {
-				it->second.fn (report);
-				if (it->second.only_once)
-					it = _listeners.erase (it);
-				else
-					++it;
-			}
+			cmd_lock.unlock ();
+			std::unique_lock<std::mutex> lock (_listener_mutex);
+			processEvent (report);
 		}
 		else {
 			Log::warning () << "Answer was not matched with any command." << std::endl;
@@ -258,13 +265,6 @@ void DispatcherThread::processReport (std::vector<uint8_t> &&raw_report)
 	}
 }
 
-DispatcherThread::Listener::Listener (const std::function<void (const Report &)> &fn, const std::function<void (std::exception_ptr)> &except, bool only_once):
-	fn (fn),
-	except (except),
-	only_once (only_once)
-{
-}
-
 const char *DispatcherThread::NotRunning::what () const noexcept
 {
 	return "Dispatcher thread is not running";
diff --git a/src/libhidpp/hidpp/DispatcherThread.h b/src/libhidpp/hidpp/DispatcherThread.h
index 3393c0f..84e44d7 100644
--- a/src/libhidpp/hidpp/DispatcherThread.h
+++ b/src/libhidpp/hidpp/DispatcherThread.h
@@ -21,12 +21,9 @@
 
 #include <hidpp/Dispatcher.h>
 #include <misc/HIDRaw.h>
-#include <misc/EventQueue.h>
-#include <thread>
 #include <future>
 #include <list>
 #include <map>
-#include <functional>
 #include <chrono>
 
 namespace HIDPP
@@ -34,24 +31,6 @@ namespace HIDPP
 
 class DispatcherThread: public Dispatcher
 {
-	struct Command
-	{
-		Report report;
-		std::promise<Report> promised_report;
-	};
-	typedef std::list<Command> command_container;
-	typedef command_container::iterator command_iterator;
-	struct Listener
-	{
-		std::function<void (const Report &)> fn;
-		std::function<void (std::exception_ptr)> except;
-		bool only_once;
-		Listener (const std::function<void (const Report &)> &fn,
-			  const std::function<void (std::exception_ptr)> &except,
-			  bool only_once);
-	};
-	typedef std::multimap<std::tuple<DeviceIndex, uint8_t>, Listener> listener_container;
-
 public:
 	class NotRunning: public std::exception
 	{
@@ -71,49 +50,57 @@ class DispatcherThread: public Dispatcher
 	virtual std::unique_ptr<Dispatcher::AsyncReport> sendCommand (Report &&report);
 	virtual std::unique_ptr<Dispatcher::AsyncReport> getNotification (DeviceIndex index, uint8_t sub_id);
 
-	typedef listener_container::iterator listener_iterator;
-	/**
-	 * Add a listener function for events matching \p index and \p sub_id.
-	 *
-	 * The queue is interrupted is the thread is stopped while
-	 * it is still registered.
-	 *
-	 * \param index		Event device index
-	 * \param sub_id	Event sub_id (or feature index)
-	 * \param queue		Queue where events will be pushed.
-	 *
-	 * \returns The listener iterator used for unregistering.
-	 */
-	listener_iterator registerEventQueue (DeviceIndex index, uint8_t sub_id, EventQueue<Report> *queue);
-	/**
-	 * Unregister the event queue given by the iterator.
-	 */
-	void unregisterEventQueue (listener_iterator it);
-
-	bool running () const;
 
-private:
+	virtual listener_iterator registerEventHandler (DeviceIndex index, uint8_t sub_id, const event_handler &handler);
+	virtual void unregisterEventHandler (listener_iterator it);
+
 	void run ();
+	void stop ();
+
+private:
+	struct Command
+	{
+		Report request;
+		std::promise<Report> response;
+	};
+	typedef std::list<Command> command_container;
+	typedef command_container::iterator command_iterator;
+
+	void cancelCommand (command_iterator);
+
+	struct Notification
+	{
+		listener_iterator listener;
+		std::promise<Report> notification;
+	};
+	typedef std::list<Notification> notification_container;
+	typedef notification_container::iterator notification_iterator;
+
+	void cancelNotification (notification_iterator);
+
 	void processReport (std::vector<uint8_t> &&raw_report);
 
 	HIDRaw _dev;
 	command_container _commands;
-	listener_container _listeners;
-	std::mutex _mutex;
-	std::thread _thread;
-	bool _running;
+	notification_container _notifications;
+	std::mutex _command_mutex, _listener_mutex;
+	bool _stopped;
 	std::exception_ptr _exception;
 
-	template<typename Container, Container DispatcherThread::*container>
+	template<typename Iterator,
+		 void (DispatcherThread::*cancel) (Iterator),
+		 std::mutex DispatcherThread::*mutex>
 	class AsyncReport;
-	using CommandResponse = DispatcherThread::AsyncReport<
-		DispatcherThread::command_container,
-		&DispatcherThread::_commands>;
-	friend CommandResponse;
-	using Notification = DispatcherThread::AsyncReport<
-		DispatcherThread::listener_container,
-		&DispatcherThread::_listeners>;
-	friend Notification;
+	using AsyncCommandResponse = DispatcherThread::AsyncReport<
+		DispatcherThread::command_iterator,
+		&DispatcherThread::cancelCommand,
+		&DispatcherThread::_command_mutex>;
+	friend AsyncCommandResponse;
+	using AsyncNotification = DispatcherThread::AsyncReport<
+		DispatcherThread::notification_iterator,
+		&DispatcherThread::cancelNotification,
+		&DispatcherThread::_listener_mutex>;
+	friend AsyncNotification;
 };
 
 }
diff --git a/src/libhidpp/hidpp/SimpleDispatcher.cpp b/src/libhidpp/hidpp/SimpleDispatcher.cpp
index e35473c..05a49ce 100644
--- a/src/libhidpp/hidpp/SimpleDispatcher.cpp
+++ b/src/libhidpp/hidpp/SimpleDispatcher.cpp
@@ -74,16 +74,6 @@ std::unique_ptr<Dispatcher::AsyncReport> SimpleDispatcher::getNotification (Devi
 	return std::make_unique<Notification> (this, index, sub_id);
 }
 
-SimpleDispatcher::listener_iterator SimpleDispatcher::registerEventHandler (DeviceIndex index, uint8_t sub_id, std::function<void (const Report &)> fn)
-{
-	return _listeners.emplace (std::make_tuple (index, sub_id), fn);
-}
-
-void SimpleDispatcher::unregisterEventHandler (listener_iterator it)
-{
-	_listeners.erase (it);
-}
-
 void SimpleDispatcher::listen ()
 {
 	auto debug = Log::debug ("dispatcher");
@@ -117,9 +107,7 @@ Report SimpleDispatcher::getReport (int timeout)
 			if (report.checkErrorMessage20 (nullptr, nullptr, nullptr, nullptr)) {
 				return report;
 			}
-			auto range = _listeners.equal_range (std::make_tuple (report.deviceIndex (), report.subID ()));
-			for (auto it = range.first; it != range.second; ++it)
-				it->second (report);
+			processEvent (report);
 			return report;
 		}
 		catch (Report::InvalidReportID e) {
diff --git a/src/libhidpp/hidpp/SimpleDispatcher.h b/src/libhidpp/hidpp/SimpleDispatcher.h
index c47645e..a897927 100644
--- a/src/libhidpp/hidpp/SimpleDispatcher.h
+++ b/src/libhidpp/hidpp/SimpleDispatcher.h
@@ -21,8 +21,6 @@
 
 #include <hidpp/Dispatcher.h>
 #include <misc/HIDRaw.h>
-#include <map>
-#include <functional>
 
 namespace HIDPP
 {
@@ -42,8 +40,6 @@ namespace HIDPP
  */
 class SimpleDispatcher: public Dispatcher
 {
-	typedef std::multimap<std::tuple<DeviceIndex, uint8_t>, std::function<void (const Report &)>> listener_container;
-
 public:
 	SimpleDispatcher (const char *path);
 	~SimpleDispatcher ();
@@ -57,22 +53,6 @@ class SimpleDispatcher: public Dispatcher
 	virtual std::unique_ptr<Dispatcher::AsyncReport> sendCommand (Report &&report);
 	virtual std::unique_ptr<Dispatcher::AsyncReport> getNotification (DeviceIndex index, uint8_t sub_id);
 
-	typedef listener_container::iterator listener_iterator;
-	/**
-	 * Add a listener function for events matching \p index and \p sub_id.
-	 *
-	 * \param index		Event device index
-	 * \param sub_id	Event sub_id (or feature index)
-	 * \param fn		Callback for handling the event
-	 *
-	 * \returns The listener iterator used for unregistering.
-	 */
-	listener_iterator registerEventHandler (DeviceIndex index, uint8_t sub_id, std::function<void (const Report &)> fn);
-	/**
-	 * Unregister the event handler given by the iterator.
-	 */
-	void unregisterEventHandler (listener_iterator it);
-
 	void listen ();
 	void stop ();
 
@@ -80,7 +60,6 @@ class SimpleDispatcher: public Dispatcher
 	Report getReport (int timeout = -1);
 
 	HIDRaw _dev;
-	listener_container _listeners;
 
 	class CommandResponse: public Dispatcher::AsyncReport
 	{
diff --git a/src/tools/common/EventQueue.h b/src/tools/common/EventQueue.h
new file mode 100644
index 0000000..926e3c6
--- /dev/null
+++ b/src/tools/common/EventQueue.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright 2017 Clément Vuchener
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef EVENT_QUEUE_H
+#define EVENT_QUEUE_H
+
+#include <queue>
+#include <mutex>
+#include <condition_variable>
+#include <experimental/optional>
+namespace std { template<typename T> using optional = experimental::optional<T>; }
+
+/**
+ * Queue for transfering events across different thread.
+ */
+template<typename T>
+class EventQueue
+{
+public:
+	EventQueue ():
+		_interrupted (false)
+	{
+	}
+
+	~EventQueue ()
+	{
+	}
+
+	/**
+	 * Push an event in the queue.
+	 */
+	void push (const T &event)
+	{
+		std::unique_lock<std::mutex> lock (_mutex);
+		_queue.push (event);
+		lock.unlock ();
+		_condvar.notify_one ();
+	}
+
+	/**
+	 * Pop an event from the queue.
+	 *
+	 * This method will block until an event
+	 * is available unless it is interrupted.
+	 *
+	 * \see interrupt()
+	 */
+	std::optional<T> pop ()
+	{
+		std::unique_lock<std::mutex> lock (_mutex);
+		std::optional<T> ret;
+		while (_queue.empty () && !_interrupted)
+			_condvar.wait (lock);
+		if (!_interrupted) {
+			ret = std::move (_queue.front ());
+			_queue.pop ();
+		}
+		return ret;
+	}
+
+	/**
+	 * Try to pop an event from the queue.
+	 *
+	 * If the queue is empty, an invalid value is returned.
+	 */
+	std::optional<T> try_pop ()
+	{
+		std::unique_lock<std::mutex> lock (_mutex);
+		std::optional<T> ret;
+		if(!_queue.empty ()) {
+			ret = std::move (_queue.front ());
+			_queue.pop ();
+		}
+		return ret;
+	}
+
+	/**
+	 * Make the current and future calls to pop() return
+	 * immediately with an invalid value.
+	 *
+	 * \see resetInterruption()
+	 */
+	void interrupt ()
+	{
+		_interrupted = true;
+		_condvar.notify_all ();
+	}
+
+	/**
+	 * Cancel the effect of interrupt().
+	 *
+	 * Future calls to pop(), will block again.
+	 */
+	void resetInterruption ()
+	{
+		_interrupted = false;
+	}
+
+private:
+	std::mutex _mutex;
+	std::condition_variable _condvar;
+	std::queue<T> _queue;
+	bool _interrupted;
+};
+
+#endif
diff --git a/src/tools/hidpp20-mouse-event-test.cpp b/src/tools/hidpp20-mouse-event-test.cpp
index 7941f9a..16e81a9 100644
--- a/src/tools/hidpp20-mouse-event-test.cpp
+++ b/src/tools/hidpp20-mouse-event-test.cpp
@@ -29,6 +29,7 @@
 #include "common/common.h"
 #include "common/Option.h"
 #include "common/CommonOptions.h"
+#include "common/EventQueue.h"
 
 extern "C" {
 #include <unistd.h>
@@ -125,86 +126,95 @@ class ProfileHandler: public EventHandler
 
 class EventListener
 {
-public:
-	virtual ~EventListener () { }
-	virtual void addEventHandler (std::unique_ptr<EventHandler> &&handler) = 0;
-	virtual void removeEventHandlers () = 0;
-	virtual void start () = 0;
-	virtual void stop () = 0;
-};
-
-class ThreadListener: public EventListener
-{
-	HIDPP::DispatcherThread *dispatcher;
+	HIDPP::Dispatcher *dispatcher;
 	HIDPP::DeviceIndex index;
-	EventQueue<HIDPP::Report> queue;
 	std::map<uint8_t, std::unique_ptr<EventHandler>> handlers;
-	std::map<uint8_t, HIDPP::DispatcherThread::listener_iterator> iterators;
+	std::map<uint8_t, HIDPP::Dispatcher::listener_iterator> iterators;
 public:
-	ThreadListener (HIDPP::DispatcherThread *dispatcher, HIDPP::DeviceIndex index):
+	EventListener (HIDPP::Dispatcher *dispatcher, HIDPP::DeviceIndex index):
 		dispatcher (dispatcher),
 		index (index)
 	{
 	}
 
+	virtual ~EventListener ()
+	{
+		removeEventHandlers ();
+	}
+
 	virtual void addEventHandler (std::unique_ptr<EventHandler> &&handler)
 	{
 		uint8_t feature = handler->feature ()->index ();
-		handlers.emplace (feature, std::move (handler)).first;
-		iterators.emplace (feature, dispatcher->registerEventQueue (index, feature, &queue));
+		EventHandler *ptr = handler.get ();
+		handlers.emplace (feature, std::move (handler));
+		auto it = dispatcher->registerEventHandler (index, feature, [ptr] (const HIDPP::Report &report) {
+			ptr->handleEvent (report);
+			return true;
+		});
+		iterators.emplace (feature, it);
 	}
 
 	virtual void removeEventHandlers ()
 	{
 		for (const auto &p: iterators)
-			dispatcher->unregisterEventQueue (p.second);
+			dispatcher->unregisterEventHandler (p.second);
 		handlers.clear ();
 		iterators.clear ();
 	}
 
+	virtual void start () = 0;
+	virtual void stop () = 0;
+
+protected:
+	virtual bool event (EventHandler *handler, const HIDPP::Report &report) = 0;
+};
+
+class ThreadListener: public EventListener
+{
+	HIDPP::DispatcherThread *dispatcher;
+	EventQueue<std::pair<EventHandler *, HIDPP::Report>> queue;
+	std::thread thread;
+public:
+	ThreadListener (HIDPP::DispatcherThread *dispatcher, HIDPP::DeviceIndex index):
+		EventListener (dispatcher, index),
+		dispatcher (dispatcher),
+		thread (std::bind (&HIDPP::DispatcherThread::run, dispatcher))
+	{
+	}
+
 	virtual void start ()
 	{
-		while (auto opt = queue.pop ()) {
-			const auto &report = opt.value ();
-			handlers[report.featureIndex ()]->handleEvent (report);
+		while (auto event = queue.pop ()) {
+			auto pair = event.value ();
+			pair.first->handleEvent (pair.second);
 		}
+		dispatcher->stop ();
+		thread.join ();
 	}
 
 	virtual void stop ()
 	{
 		queue.interrupt ();
 	}
+
+protected:
+	virtual bool event (EventHandler *handler, const HIDPP::Report &report)
+	{
+		queue.push (std::make_pair (handler, report));
+		return true;
+	}
 };
 
 class SimpleListener: public EventListener
 {
 	HIDPP::SimpleDispatcher *dispatcher;
-	HIDPP::DeviceIndex index;
-	std::map<uint8_t, std::unique_ptr<EventHandler>> handlers;
-	std::map<uint8_t, HIDPP::SimpleDispatcher::listener_iterator> iterators;
 public:
 	SimpleListener (HIDPP::SimpleDispatcher *dispatcher, HIDPP::DeviceIndex index):
-		dispatcher (dispatcher),
-		index (index)
+		EventListener (dispatcher, index),
+		dispatcher (dispatcher)
 	{
 	}
 
-	virtual void addEventHandler (std::unique_ptr<EventHandler> &&handler)
-	{
-		uint8_t feature = handler->feature ()->index ();
-		auto it = handlers.emplace (feature, std::move (handler)).first;
-		iterators.emplace (feature, dispatcher->registerEventHandler (index, feature,
-			std::bind (&EventHandler::handleEvent, it->second.get (), std::placeholders::_1)));
-	}
-
-	virtual void removeEventHandlers ()
-	{
-		for (const auto &p: iterators)
-			dispatcher->unregisterEventHandler (p.second);
-		handlers.clear ();
-		iterators.clear ();
-	}
-
 	virtual void start ()
 	{
 		dispatcher->listen ();
@@ -214,6 +224,13 @@ class SimpleListener: public EventListener
 	{
 		dispatcher->stop ();
 	}
+
+protected:
+	virtual bool event (EventHandler *handler, const HIDPP::Report &report)
+	{
+		handler->handleEvent (report);
+		return true;
+	}
 };
 
 EventListener *listener;
@@ -259,14 +276,14 @@ int main (int argc, char *argv[])
 	try {
 		if (thread) {
 			HIDPP::DispatcherThread *d = new HIDPP::DispatcherThread (path);
-			dev = std::make_unique<HIDPP20::Device> (d, device_index);
 			listener = new ThreadListener (d, device_index);
+			dev = std::make_unique<HIDPP20::Device> (d, device_index);
 			dispatcher.reset (d);
 		}
 		else {
 			HIDPP::SimpleDispatcher *d = new HIDPP::SimpleDispatcher (path);
-			dev = std::make_unique<HIDPP20::Device> (d, device_index);
 			listener = new SimpleListener (d, device_index);
+			dev = std::make_unique<HIDPP20::Device> (d, device_index);
 			dispatcher.reset (d);
 		}
 	}
diff --git a/src/tools/hidpp20-raw-touchpad-driver.cpp b/src/tools/hidpp20-raw-touchpad-driver.cpp
index a9c4a8d..41c806c 100644
--- a/src/tools/hidpp20-raw-touchpad-driver.cpp
+++ b/src/tools/hidpp20-raw-touchpad-driver.cpp
@@ -23,6 +23,7 @@
 
 #include "common/common.h"
 #include "common/CommonOptions.h"
+#include "common/EventQueue.h"
 
 extern "C" {
 #include <unistd.h>
@@ -41,6 +42,8 @@ extern "C" {
 #include <hidpp20/Device.h>
 #include <hidpp20/ITouchpadRawXY.h>
 
+EventQueue<std::function<void ()>> task_queue;
+
 class DeviceMonitor
 {
 	int _pipe[2];
@@ -137,44 +140,44 @@ class DeviceMonitor
 
 class Driver
 {
-	HIDPP::DispatcherThread *_dispatcher;
+	HIDPP::Dispatcher *_dispatcher;
 	EventQueue<HIDPP::Report> _queue;
-	std::vector<HIDPP::DispatcherThread::listener_iterator> _iterators;
-	std::thread _thread;
+	std::vector<HIDPP::Dispatcher::listener_iterator> _iterators;
 public:
-	Driver (HIDPP::DispatcherThread *dispatcher):
-		_dispatcher (dispatcher),
-		_thread (std::bind (&Driver::run, this))
+	Driver (HIDPP::Dispatcher *dispatcher):
+		_dispatcher (dispatcher)
 	{
 	}
 
 	virtual ~Driver ()
 	{
 		for (auto it: _iterators)
-			_dispatcher->unregisterEventQueue (it);
-		_queue.interrupt ();
-		_thread.join ();
+			_dispatcher->unregisterEventHandler (it);
 	}
 
 protected:
-	HIDPP::DispatcherThread *dispatcher ()
+	HIDPP::Dispatcher *dispatcher ()
 	{
 		return _dispatcher;
 	}
 
-	void addEvent (HIDPP::DeviceIndex index, uint8_t sub_id)
+	void addEvent (HIDPP::DeviceIndex index, uint8_t sub_id, bool direct = false)
 	{
-		_iterators.push_back (_dispatcher->registerEventQueue (index, sub_id, &_queue));
+		auto it = _dispatcher->registerEventHandler (index, sub_id, (direct ?
+			(HIDPP::Dispatcher::event_handler) [this] (const HIDPP::Report &report) {
+				event (report);
+				return true;
+			}
+			:
+			(HIDPP::Dispatcher::event_handler) [this] (const HIDPP::Report &report) {
+				task_queue.push (std::bind (&Driver::event, this, report));
+				return true;
+			}
+		));
+		_iterators.push_back (it);
 	}
 
 	virtual void event (const HIDPP::Report &report) = 0;
-
-private:
-	void run ()
-	{
-		while (auto opt = _queue.pop ())
-			event (opt.value ());
-	}
 };
 
 static void setBit (int fd, int request, int code)
@@ -271,7 +274,7 @@ class TouchpadDriver: public Driver
 		}
 	} st_state;
 public:
-	TouchpadDriver (HIDPP::DispatcherThread *dispatcher, HIDPP::DeviceIndex index):
+	TouchpadDriver (HIDPP::Dispatcher *dispatcher, HIDPP::DeviceIndex index):
 		Driver (dispatcher),
 		_dev (dispatcher, index),
 		_itrxy (&_dev),
@@ -322,7 +325,7 @@ class TouchpadDriver: public Driver
 		}
 
 		printf ("Added touchpad device: %s\n", _dev.name ().c_str ());
-		addEvent (index, _itrxy.index ());
+		addEvent (index, _itrxy.index (), true);
 		_itrxy.setTouchpadRawMode (true);
 	}
 
@@ -361,7 +364,7 @@ class ReceiverDriver: public Driver
 	HIDPP10::Device _dev;
 	std::map<HIDPP::DeviceIndex, TouchpadDriver> _drivers;
 public:
-	ReceiverDriver (HIDPP::DispatcherThread *dispatcher):
+	ReceiverDriver (HIDPP::Dispatcher *dispatcher):
 		Driver (dispatcher),
 		_dev (dispatcher)
 	{
@@ -409,50 +412,66 @@ class ReceiverDriver: public Driver
 
 class MyMonitor: public DeviceMonitor
 {
-	std::map<std::string, HIDPP::DispatcherThread> _nodes;
-	std::map<std::string, std::unique_ptr<Driver>> _drivers;
+	struct node
+	{
+		HIDPP::DispatcherThread dispatcher;
+		std::thread thread;
+		std::unique_ptr<Driver> driver;
+
+		node (const char *path):
+			dispatcher (path),
+			thread (std::bind (&HIDPP::DispatcherThread::run, &dispatcher))
+		{
+		}
+
+		~node ()
+		{
+			driver.reset ();
+			dispatcher.stop ();
+			thread.join ();
+		}
+	};
+	std::map<std::string, node> _nodes;
 public:
-	void addDevice (const char *node)
+	void addDevice (const char *path)
 	{
-		std::map<std::string, HIDPP::DispatcherThread>::iterator it;
+		std::map<std::string, node>::iterator it;
 		try {
-			it = _nodes.emplace (node, node).first;
+			it = _nodes.emplace (path, path).first;
 		}
 		catch (std::exception &e) {
-			Log::debug () << "Ignored device " << node << ": " << e.what () << std::endl;
+			Log::debug () << "Ignored device " << path << ": " << e.what () << std::endl;
 			return;
 		}
+		auto &node = it->second;
 		try {
-			_drivers.emplace (node, new ReceiverDriver (&it->second));
+			node.driver = std::make_unique<ReceiverDriver> (&node.dispatcher);
 			return;
 		}
 		catch (std::exception &e) {
-			Log::debug () << "Device " << node << " is not a receiver: " << e.what () << std::endl;
+			Log::debug () << "Device " << path << " is not a receiver: " << e.what () << std::endl;
 		}
 		for (HIDPP::DeviceIndex index: { HIDPP::DefaultDevice, HIDPP::CordedDevice }) {
 			try {
-				_drivers.emplace (node, new TouchpadDriver (&it->second, index));
+				node.driver = std::make_unique<TouchpadDriver> (&node.dispatcher, index);
 				return;
 			}
 			catch (std::exception &e) {
-				Log::debug () << "Device " << node << "/" << index << " is not a touchpad device: " << e.what () << std::endl;
+				Log::debug () << "Device " << path << "/" << index << " is not a touchpad device: " << e.what () << std::endl;
 			}
 		}
 		_nodes.erase (it);
 	}
 
-	void removeDevice (const char *node)
+	void removeDevice (const char *path)
 	{
-		_drivers.erase (node);
-		_nodes.erase (node);
+		_nodes.erase (path);
 	}
 };
 
-MyMonitor *monitor;
-
 void sigint (int)
 {
-	monitor->stop ();
+	task_queue.interrupt ();
 }
 
 int main (int argc, char *argv[])
@@ -467,17 +486,21 @@ int main (int argc, char *argv[])
 	if (!Option::processOptions (argc, argv, options, first_arg))
 		return EXIT_FAILURE;
 
-	monitor = new MyMonitor;
+	MyMonitor monitor;
+	std::thread monitor_thread (std::bind (&DeviceMonitor::monitor, &monitor));
 
 	struct sigaction sa, oldsa;
 	memset (&sa, 0, sizeof (struct sigaction));
 	sa.sa_handler = sigint;
 	sigaction (SIGINT, &sa, &oldsa);
 
-	monitor->monitor ();
+	while (auto task = task_queue.pop ())
+		task.value () ();
 
 	sigaction (SIGINT, &oldsa, nullptr);
-	delete monitor;
+
+	monitor.stop ();
+	monitor_thread.join ();
 
 	return EXIT_SUCCESS;
 }