Index: generic/src/ipc/sysipc.c
===================================================================
--- generic/src/ipc/sysipc.c	(revision 9f222130bdbe659719771a8bd4c6c15f3d4afd38)
+++ generic/src/ipc/sysipc.c	(revision b6d4566fc9565b3a6eff5c6344350e4dbf778359)
@@ -48,5 +48,5 @@
 }
 
-#define STRUCT_TO_USPACE(dst,src) copy_to_uspace(dst,src,sizeof(*src))
+#define STRUCT_TO_USPACE(dst,src) copy_to_uspace(dst,src,sizeof(*(src)))
 
 /** Return true if the method is a system method */
@@ -106,6 +106,8 @@
 			phone_dealloc(phoneid);
 		} else {
-				/* The connection was accepted */
+			/* The connection was accepted */
 			phone_connect(phoneid,&answer->sender->answerbox);
+			/* Set 'phone identification' as arg3 of response */
+			IPC_SET_ARG3(answer->data, (__native)&TASK->phones[phoneid]);
 		}
 	} else if (IPC_GET_METHOD(*olddata) == IPC_M_CONNECT_ME_TO) {
@@ -118,4 +120,28 @@
 }
 
+/** Called before the request is sent
+ *
+ * @return 0 - no error, -1 - report error to user
+ */
+static int request_preprocess(call_t *call)
+{
+	int newphid;
+
+	switch (IPC_GET_METHOD(call->data)) {
+	case IPC_M_CONNECT_ME_TO:
+		newphid = phone_alloc();
+		if (newphid < 0)
+			return ELIMIT;
+		/* Set arg3 for server */
+		IPC_SET_ARG3(call->data, (__native)&TASK->phones[newphid]);
+		call->flags |= IPC_CALL_CONN_ME_TO;
+		call->private = newphid;
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
 /****************************************************/
 /* Functions called to process received call/answer 
@@ -124,10 +150,16 @@
 
 /** Do basic kernel processing of received call answer */
-static int process_answer(answerbox_t *box,call_t *call)
+static void process_answer(call_t *call)
 {
 	if (IPC_GET_RETVAL(call->data) == EHANGUP && \
 	    call->flags & IPC_CALL_FORWARDED)
 		IPC_SET_RETVAL(call->data, EFORWARD);
-	return 0;
+
+	if (call->flags & IPC_CALL_CONN_ME_TO) {
+		if (IPC_GET_RETVAL(call->data))
+			phone_dealloc(call->private);
+		else
+			IPC_SET_ARG3(call->data, call->private);
+	}
 }
 
@@ -162,7 +194,5 @@
 	call_t call;
 	phone_t *phone;
-
-	if (is_system_method(method))
-		return EPERM;
+	int res;
 
 	GET_CHECK_PHONE(phone, phoneid, return ENOENT);
@@ -171,7 +201,10 @@
 	IPC_SET_METHOD(call.data, method);
 	IPC_SET_ARG1(call.data, arg1);
-	
-	ipc_call_sync(phone, &call);
-
+
+	if (!(res=request_preprocess(&call))) {
+		ipc_call_sync(phone, &call);
+		process_answer(&call);
+	} else 
+		IPC_SET_RETVAL(call.data, res);
 	STRUCT_TO_USPACE(&data->args, &call.data.args);
 
@@ -185,14 +218,16 @@
 	call_t call;
 	phone_t *phone;
+	int res;
 
 	ipc_call_static_init(&call);
 	copy_from_uspace(&call.data.args, &question->args, sizeof(call.data.args));
 
-	if (is_system_method(IPC_GET_METHOD(call.data)))
-		return EPERM;
-	
 	GET_CHECK_PHONE(phone, phoneid, return ENOENT);
 
-	ipc_call_sync(phone, &call);
+	if (!(res=request_preprocess(&call))) {
+		ipc_call_sync(phone, &call);
+		process_answer(&call);
+	} else 
+		IPC_SET_RETVAL(call.data, res);
 
 	STRUCT_TO_USPACE(&reply->args, &call.data.args);
@@ -224,12 +259,10 @@
 	call_t *call;
 	phone_t *phone;
-
-	if (is_system_method(method))
-		return IPC_CALLRET_FATAL;
+	int res;
 
 	if (check_call_limit())
 		return IPC_CALLRET_TEMPORARY;
 
-	GET_CHECK_PHONE(phone, phoneid, return ENOENT);
+	GET_CHECK_PHONE(phone, phoneid, return IPC_CALLRET_FATAL);
 
 	call = ipc_call_alloc();
@@ -238,5 +271,8 @@
 	IPC_SET_ARG2(call->data, arg2);
 
-	ipc_call(phone, call);
+	if (!(res=request_preprocess(call)))
+		ipc_call(phone, call);
+	else
+		ipc_backsend_err(phone, call, res);
 
 	return (__native) call;
@@ -251,19 +287,17 @@
 	call_t *call;
 	phone_t *phone;
+	int res;
 
 	if (check_call_limit())
 		return IPC_CALLRET_TEMPORARY;
 
-	GET_CHECK_PHONE(phone, phoneid, return ENOENT);
+	GET_CHECK_PHONE(phone, phoneid, return IPC_CALLRET_FATAL);
 
 	call = ipc_call_alloc();
 	copy_from_uspace(&call->data.args, &data->args, sizeof(call->data.args));
-
-	if (is_system_method(IPC_GET_METHOD(call->data))) {
-		ipc_call_free(call);
-		return EPERM;
-	}
-	
-	ipc_call(phone, call);
+	if (!(res=request_preprocess(call)))
+		ipc_call(phone, call);
+	else
+		ipc_backsend_err(phone, call, res);
 
 	return (__native) call;
@@ -305,4 +339,7 @@
 	 */
 	if (is_system_method(IPC_GET_METHOD(call->data))) {
+		if (IPC_GET_METHOD(call->data) == IPC_M_CONNECT_TO_ME)
+			phone_dealloc(IPC_GET_ARG3(call->data));
+
 		IPC_SET_ARG1(call->data, method);
 		IPC_SET_ARG2(call->data, arg1);
@@ -366,65 +403,5 @@
 }
 
-/** Ask the other side of connection to do 'callback' connection
- *
- * @return 0 if no error, error otherwise 
- */
-__native sys_ipc_connect_to_me(__native phoneid, __native arg1,
-			       __native arg2, task_id_t *taskid)
-{
-	call_t call;
-	phone_t *phone;
-
-	ipc_call_static_init(&call);
-	IPC_SET_METHOD(call.data, IPC_M_CONNECT_TO_ME);
-	IPC_SET_ARG1(call.data, arg1);
-	IPC_SET_ARG2(call.data, arg2);
-	
-	GET_CHECK_PHONE(phone, phoneid, return ENOENT);
-
-	ipc_call_sync(phone, &call);
-
-	if (!IPC_GET_RETVAL(call.data) && taskid)
-		STRUCT_TO_USPACE(taskid, &phone->callee->task->taskid);
-
-	return IPC_GET_RETVAL(call.data);
-}
-
-/** Ask target process to connect me somewhere
- *
- * @return phoneid - on success, error otherwise
- */
-__native sys_ipc_connect_me_to(__native phoneid, __native arg1,
-			       __native arg2)
-{
-	call_t call;
-	phone_t *phone;
-	int newphid;
-
-	GET_CHECK_PHONE(phone, phoneid, return ENOENT);
-
-	newphid = phone_alloc();
-	if (newphid < 0)
-		return ELIMIT;
-
-	ipc_call_static_init(&call);
-	IPC_SET_METHOD(call.data, IPC_M_CONNECT_ME_TO);
-	IPC_SET_ARG1(call.data, arg1);
-	IPC_SET_ARG2(call.data, arg2);
-	IPC_SET_ARG3(call.data, (__native)&TASK->phones[newphid]);
-
-	ipc_call_sync(phone, &call);
-
-	if (IPC_GET_RETVAL(call.data)) { /* Connection failed */
-		phone_dealloc(newphid);
-		return IPC_GET_RETVAL(call.data);
-	}
-
-	return newphid;
-}
-
 /** Hang up the phone
- *
- * 
  *
  */
@@ -449,7 +426,5 @@
  * @return Callid, if callid & 1, then the call is answer
  */
-__native sys_ipc_wait_for_call(ipc_data_t *calldata, task_id_t *taskid,
-			       __native flags)
-					     
+__native sys_ipc_wait_for_call(ipc_data_t *calldata, __native flags)
 {
 	call_t *call;
@@ -461,6 +436,5 @@
 
 	if (call->flags & IPC_CALL_ANSWERED) {
-		if (process_answer(&TASK->answerbox, call))
-			goto restart;
+		process_answer(call);
 
 		ASSERT(! (call->flags & IPC_CALL_STATIC_ALLOC));
@@ -482,8 +456,7 @@
 		goto restart;
 
-	/* Include phone address('id') of the caller in the request */
+	/* Include phone address('id') of the caller in the request,
+	 * copy whole call->data, not only call->data.args */
 	STRUCT_TO_USPACE(calldata, &call->data);
-	if (IPC_GET_METHOD(call->data) == IPC_M_CONNECT_ME_TO)
-		STRUCT_TO_USPACE(taskid, &TASK->taskid);
 	return (__native)call;
 }
