Index: uspace/lib/c/generic/ipc.c
===================================================================
--- uspace/lib/c/generic/ipc.c	(revision 503ffceaab0f4c54907677d3399c3ed1363e507f)
+++ uspace/lib/c/generic/ipc.c	(revision 706b4de86801bb57b4394e17f933610630608b59)
@@ -1,5 +1,4 @@
 /*
  * Copyright (c) 2006 Ondrej Palkovsky
- * Copyright (c) 2017 Jakub Jermar
  * All rights reserved.
  *
@@ -51,15 +50,49 @@
 
 /**
- * Structures of this type are used for keeping track of sent asynchronous calls.
- */
-typedef struct async_call {
+ * Structures of this type are used for keeping track
+ * of sent asynchronous calls and queing unsent calls.
+ */
+typedef struct {
+	link_t list;
+	
 	ipc_async_callback_t callback;
 	void *private;
 	
-	struct {
-		ipc_call_t data;
-		int phoneid;
-	} msg;
+	union {
+		ipc_callid_t callid;
+		struct {
+			ipc_call_t data;
+			int phoneid;
+		} msg;
+	} u;
+	
+	/** Fibril waiting for sending this call. */
+	fid_t fid;
 } async_call_t;
+
+LIST_INITIALIZE(dispatched_calls);
+
+/** List of asynchronous calls that were not accepted by kernel.
+ *
+ * Protected by async_futex, because if the call is not accepted
+ * by the kernel, the async framework is used automatically.
+ *
+ */
+LIST_INITIALIZE(queued_calls);
+
+static futex_t ipc_futex = FUTEX_INITIALIZER;
+
+/** Send asynchronous message via syscall.
+ *
+ * @param phoneid Phone handle for the call.
+ * @param data    Call data with the request.
+ *
+ * @return Hash of the call or an error code.
+ *
+ */
+static ipc_callid_t ipc_call_async_internal(int phoneid, ipc_call_t *data)
+{
+	return __SYSCALL2(SYS_IPC_CALL_ASYNC_SLOW, phoneid, (sysarg_t) data);
+}
 
 /** Prologue for ipc_call_async_*() functions.
@@ -100,8 +133,11 @@
 	if (!call) {
 		/* Nothing to do regardless if failed or not */
+		futex_unlock(&ipc_futex);
 		return;
 	}
 	
 	if (callid == (ipc_callid_t) IPC_CALLRET_FATAL) {
+		futex_unlock(&ipc_futex);
+		
 		/* Call asynchronous handler with error code */
 		if (call->callback)
@@ -111,9 +147,15 @@
 		return;
 	}
+	
+	call->u.callid = callid;
+	
+	/* Add call to the list of dispatched calls */
+	list_append(&call->list, &dispatched_calls);
+	futex_unlock(&ipc_futex);
 }
 
 /** Fast asynchronous call.
  *
- * This function can only handle three arguments of payload. It is, however,
+ * This function can only handle four arguments of payload. It is, however,
  * faster than the more generic ipc_call_async_slow().
  *
@@ -129,16 +171,28 @@
  * @param arg2        Service-defined payload argument.
  * @param arg3        Service-defined payload argument.
+ * @param arg4        Service-defined payload argument.
  * @param private     Argument to be passed to the answer/error callback.
  * @param callback    Answer or error callback.
  */
 void ipc_call_async_fast(int phoneid, sysarg_t imethod, sysarg_t arg1,
-    sysarg_t arg2, sysarg_t arg3, void *private, ipc_async_callback_t callback)
-{
-	async_call_t *call = ipc_prepare_async(private, callback);
-	if (!call)
-		return;
-	
+    sysarg_t arg2, sysarg_t arg3, sysarg_t arg4, void *private,
+    ipc_async_callback_t callback)
+{
+	async_call_t *call = NULL;
+	
+	if (callback) {
+		call = ipc_prepare_async(private, callback);
+		if (!call)
+			return;
+	}
+	
+	/*
+	 * We need to make sure that we get callid
+	 * before another thread accesses the queue again.
+	 */
+	
+	futex_lock(&ipc_futex);
 	ipc_callid_t callid = __SYSCALL6(SYS_IPC_CALL_ASYNC_FAST, phoneid,
-	    imethod, arg1, arg2, arg3, (sysarg_t) call);
+	    imethod, arg1, arg2, arg3, arg4);
 	
 	ipc_finish_async(callid, phoneid, call);
@@ -171,13 +225,19 @@
 		return;
 	
-	IPC_SET_IMETHOD(call->msg.data, imethod);
-	IPC_SET_ARG1(call->msg.data, arg1);
-	IPC_SET_ARG2(call->msg.data, arg2);
-	IPC_SET_ARG3(call->msg.data, arg3);
-	IPC_SET_ARG4(call->msg.data, arg4);
-	IPC_SET_ARG5(call->msg.data, arg5);
-	
-	ipc_callid_t callid = __SYSCALL3(SYS_IPC_CALL_ASYNC_SLOW, phoneid,
-	    (sysarg_t) &call->msg.data, (sysarg_t) call);
+	IPC_SET_IMETHOD(call->u.msg.data, imethod);
+	IPC_SET_ARG1(call->u.msg.data, arg1);
+	IPC_SET_ARG2(call->u.msg.data, arg2);
+	IPC_SET_ARG3(call->u.msg.data, arg3);
+	IPC_SET_ARG4(call->u.msg.data, arg4);
+	IPC_SET_ARG5(call->u.msg.data, arg5);
+	
+	/*
+	 * We need to make sure that we get callid
+	 * before another threadaccesses the queue again.
+	 */
+	
+	futex_lock(&ipc_futex);
+	ipc_callid_t callid =
+	    ipc_call_async_internal(phoneid, &call->u.msg.data);
 	
 	ipc_finish_async(callid, phoneid, call);
@@ -236,19 +296,88 @@
 }
 
+/** Try to dispatch queued calls from the async queue.
+ *
+ */
+static void dispatch_queued_calls(void)
+{
+	/** @todo
+	 * Integrate intelligently ipc_futex so that it is locked during
+	 * ipc_call_async_*() until it is added to dispatched_calls.
+	 */
+	
+	futex_down(&async_futex);
+	
+	while (!list_empty(&queued_calls)) {
+		async_call_t *call =
+		    list_get_instance(list_first(&queued_calls), async_call_t, list);
+		ipc_callid_t callid =
+		    ipc_call_async_internal(call->u.msg.phoneid, &call->u.msg.data);
+		
+		list_remove(&call->list);
+		
+		futex_up(&async_futex);
+		
+		assert(call->fid);
+		fibril_add_ready(call->fid);
+		
+		if (callid == (ipc_callid_t) IPC_CALLRET_FATAL) {
+			if (call->callback)
+				call->callback(call->private, ENOENT, NULL);
+			
+			free(call);
+		} else {
+			call->u.callid = callid;
+			
+			futex_lock(&ipc_futex);
+			list_append(&call->list, &dispatched_calls);
+			futex_unlock(&ipc_futex);
+		}
+		
+		futex_down(&async_futex);
+	}
+	
+	futex_up(&async_futex);
+}
+
 /** Handle received answer.
+ *
+ * Find the hash of the answer and call the answer callback.
+ *
+ * The answer has the same hash as the request OR'ed with
+ * the IPC_CALLID_ANSWERED bit.
+ *
+ * @todo Use hash table.
  *
  * @param callid Hash of the received answer.
  * @param data   Call data of the answer.
+ *
  */
 static void handle_answer(ipc_callid_t callid, ipc_call_t *data)
 {
-	async_call_t *call = data->label;
-
-	if (!call)
-		return;
-
-	if (call->callback)
-		call->callback(call->private, IPC_GET_RETVAL(*data), data);
-	free(call);
+	callid &= ~IPC_CALLID_ANSWERED;
+	
+	futex_lock(&ipc_futex);
+	
+	link_t *item;
+	for (item = dispatched_calls.head.next; item != &dispatched_calls.head;
+	    item = item->next) {
+		async_call_t *call =
+		    list_get_instance(item, async_call_t, list);
+		
+		if (call->u.callid == callid) {
+			list_remove(&call->list);
+			
+			futex_unlock(&ipc_futex);
+			
+			if (call->callback)
+				call->callback(call->private,
+				    IPC_GET_RETVAL(*data), data);
+			
+			free(call);
+			return;
+		}
+	}
+	
+	futex_unlock(&ipc_futex);
 }
 
@@ -259,5 +388,8 @@
  * @param flags Flags passed to SYS_IPC_WAIT (blocking, nonblocking).
  *
- * @return Hash of the call.
+ * @return Hash of the call. Note that certain bits have special
+ *         meaning: IPC_CALLID_ANSWERED is set in an answer
+ *         and IPC_CALLID_NOTIFICATION is used for notifications.
+ *
  */
 ipc_callid_t ipc_wait_cycle(ipc_call_t *call, sysarg_t usec,
@@ -268,6 +400,8 @@
 	
 	/* Handle received answers */
-	if (callid && (call->flags & IPC_CALLID_ANSWERED))
+	if (callid & IPC_CALLID_ANSWERED) {
 		handle_answer(callid, call);
+		dispatch_queued_calls();
+	}
 	
 	return callid;
@@ -298,5 +432,5 @@
 	do {
 		callid = ipc_wait_cycle(call, usec, SYNCH_FLAGS_NONE);
-	} while (callid && (call->flags & IPC_CALLID_ANSWERED));
+	} while (callid & IPC_CALLID_ANSWERED);
 	
 	return callid;
@@ -319,5 +453,5 @@
 		callid = ipc_wait_cycle(call, SYNCH_NO_TIMEOUT,
 		    SYNCH_FLAGS_NON_BLOCKING);
-	} while (callid && (call->flags & IPC_CALLID_ANSWERED));
+	} while (callid & IPC_CALLID_ANSWERED);
 	
 	return callid;
