x86_64: expand and shrink messages in copy{in, out}msg routines

* i386/i386/copy_user.h: new file to handle 32/64 bit differences
  - add msg_usize() to recontruct the user-space message size
  - add copyin/copyout helpers for addresses and ports
* include/mach/message.h: add msg alignment macros
* ipc/ipc_kmsg.c:
  - copyin/out ports names instead of using pointer magic
* ipc/ipc_mqueue.c: use msg_usize() to check if we can actually
  receive the message
* ipc/mach_msg.c: Likewise for continuations in receive path
* x86_64/Makefrag.am: add x86_64/copy_user.c
* x86_64/copy_user.c: new file to handle message expansion and
  shrinking during copyinmsg/copyoutmsg for 64 bit kernels.
  - port names -> port pointers on all 64-bit builds
  - 32-bit pointer -> 64 bit pointer when using 32-bit userspace
* x86_64/locore.S: remove copyinmsg() and copyoutmsg()
Message-Id: <20230116105857.240210-3-luca@orpolo.org>

8 files changed, 522 insertions, 107 deletions

diff --git a/i386/i386/copy_user.h b/i386/i386/copy_user.h
new file mode 100644
index 00000000..c7e8b4e1
--- /dev/null
+++ b/i386/i386/copy_user.h
@@ -0,0 +1,102 @@
+/*
+ *  Copyright (C) 2023 Free Software Foundation
+ *
+ * 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 2 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 the program ; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef COPY_USER_H
+#define COPY_USER_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <machine/locore.h>
+#include <mach/message.h>
+
+/*
+ * The copyin_32to64() and copyout_64to32() routines are meant for data types
+ * that have different size in kernel and user space. They should be independent
+ * of endianness and hopefully can be reused in the future on other archs.
+ * These types are e.g.:
+ * - port names vs port pointers, on a 64-bit kernel
+ * - memory addresses, on a 64-bit kernel and 32-bit user
+ */
+
+static inline int copyin_32to64(const uint32_t *uaddr, uint64_t *kaddr)
+{
+  uint32_t rkaddr;
+  int ret;
+  ret = copyin(uaddr, &rkaddr, sizeof(uint32_t));
+  if (ret)
+    return ret;
+  *kaddr = rkaddr;
+  return 0;
+}
+
+static inline int copyout_64to32(const uint64_t *kaddr, uint32_t *uaddr)
+{
+  uint32_t rkaddr=*kaddr;
+  return copyout(&rkaddr, uaddr, sizeof(uint32_t));
+}
+
+static inline int copyin_address(const rpc_vm_offset_t *uaddr, vm_offset_t *kaddr)
+{
+#ifdef USER32
+  return copyin_32to64(uaddr, kaddr);
+#else /* USER32 */
+  return copyin(uaddr, kaddr, sizeof(*uaddr));
+#endif /* USER32 */
+}
+
+static inline int copyout_address(const vm_offset_t *kaddr, rpc_vm_offset_t *uaddr)
+{
+#ifdef USER32
+  return copyout_64to32(kaddr, uaddr);
+#else /* USER32 */
+  return copyout(kaddr, uaddr, sizeof(*kaddr));
+#endif /* USER32 */
+}
+
+static inline int copyin_port(const mach_port_name_t *uaddr, mach_port_t *kaddr)
+{
+#ifdef __x86_64
+  return copyin_32to64(uaddr, kaddr);
+#else /* __x86_64__ */
+  return copyin(uaddr, kaddr, sizeof(*uaddr));
+#endif /* __x86_64__ */
+}
+
+static inline int copyout_port(const mach_port_t *kaddr, mach_port_name_t *uaddr)
+{
+#ifdef __x86_64
+  return copyout_64to32(kaddr, uaddr);
+#else /* __x86_64__ */
+  return copyout(kaddr, uaddr, sizeof(*kaddr));
+#endif /* __x86_64__ */
+}
+
+// XXX we could add another field to kmsg to store the user-side size, but then we
+// should check if we can  obtain it for rpc and notifications originating from
+// the kernel
+#ifndef __x86_64__
+static inline size_t msg_usize(const mach_msg_header_t *kmsg)
+{
+  return kmsg->msgh_size;
+}
+#else /* __x86_64__ */
+size_t msg_usize(const mach_msg_header_t *kmsg);
+#endif /* __x86_64__ */
+
+#endif /* COPY_USER_H */
diff --git a/include/mach/message.h b/include/mach/message.h
index c3081e66..16788fef 100644
--- a/include/mach/message.h
+++ b/include/mach/message.h
@@ -316,6 +316,19 @@ typedef integer_t mach_msg_option_t;
 
 #define MACH_SEND_ALWAYS	0x00010000	/* internal use only */
 
+/* This is the alignment of msg descriptors and the actual data.
+ *
+ * On x86 it is made equal to the default structure alignment on
+ * 32-bit, so we can easily maintain compatibility with 32-bit user
+ * space on a 64-bit kernel. Other architectures might have different
+ * needs, so this value might change in the future for differents
+ * architectures.
+ */
+#define MACH_MSG_ALIGNMENT 4
+
+#define msg_is_misaligned(x)	( ((vm_offset_t)(x)) & (MACH_MSG_ALIGNMENT-1) )
+#define msg_align(x)	\
+	( ( ((vm_offset_t)(x)) + (MACH_MSG_ALIGNMENT-1) ) & ~(MACH_MSG_ALIGNMENT-1) )
 
 /*
  *  Much code assumes that mach_msg_return_t == kern_return_t.
@@ -401,7 +414,6 @@ typedef kern_return_t mach_msg_return_t;
 #define	MACH_RCV_BODY_ERROR		0x1000400c
 		/* Error receiving message body.  See special bits. */
 
-
 extern mach_msg_return_t
 mach_msg_trap
    (mach_msg_user_header_t *msg,
diff --git a/ipc/ipc_kmsg.c b/ipc/ipc_kmsg.c
index 55252d46..45070b7a 100644
--- a/ipc/ipc_kmsg.c
+++ b/ipc/ipc_kmsg.c
@@ -42,6 +42,7 @@
 #include <mach/message.h>
 #include <mach/port.h>
 #include <machine/locore.h>
+#include <machine/copy_user.h>
 #include <kern/assert.h>
 #include <kern/kalloc.h>
 #include <vm/vm_map.h>
@@ -68,12 +69,6 @@
 #include <ipc/ipc_print.h>
 #endif
 
-/* msg body is always aligned to 4 bytes */
-typedef uint32_t msg_align_t;
-
-#define msg_is_misaligned(x)	( ((vm_offset_t)(x)) & (sizeof(msg_align_t)-1) )
-#define msg_align(x)	\
-	( ( ((vm_offset_t)(x)) + (sizeof(msg_align_t)-1) ) & ~(sizeof(msg_align_t)-1) )
 
 ipc_kmsg_t ipc_kmsg_cache[NCPUS];
 
@@ -1388,7 +1383,7 @@ ipc_kmsg_copyin_body(
 		} else {
 			vm_offset_t addr;
 
-			if (sizeof(msg_align_t) > sizeof(mach_msg_type_t))
+			if (MACH_MSG_ALIGNMENT > sizeof(mach_msg_type_t))
 				saddr = msg_align(saddr);
 
 			if ((eaddr - saddr) < sizeof(vm_offset_t)) {
@@ -1407,14 +1402,27 @@ ipc_kmsg_copyin_body(
 				if (data == 0)
 					goto invalid_memory;
 
-				if (copyinmap(map, (char *) addr,
-					      (char *) data, length) ||
-				    (dealloc &&
-				     (vm_deallocate(map, addr, length) !=
-							KERN_SUCCESS))) {
+				if (sizeof(mach_port_name_t) != sizeof(mach_port_t))
+				{
+					mach_port_name_t *src = (mach_port_name_t*)addr;
+					mach_port_t *dst = (mach_port_t*)data;
+					for (int i=0; i<number; i++) {
+						if (copyin_port(src + i, dst + i)) {
+							kfree(data, length);
+							goto invalid_memory;
+						}
+					}
+				} else if (copyinmap(map, (char *) addr,
+					      (char *) data, length)) {
+					kfree(data, length);
+					goto invalid_memory;
+				}
+				if (dealloc &&
+				    (vm_deallocate(map, addr, length) != KERN_SUCCESS)) {
 					kfree(data, length);
 					goto invalid_memory;
 				}
+
 			} else {
 				vm_map_copy_t copy;
 
@@ -2443,7 +2451,7 @@ ipc_kmsg_copyout_body(
 		} else {
 			vm_offset_t data;
 
-			if (sizeof(msg_align_t) > sizeof(mach_msg_type_t))
+			if (MACH_MSG_ALIGNMENT > sizeof(mach_msg_type_t))
 				saddr = msg_align(saddr);
 
 			data = * (vm_offset_t *) saddr;
@@ -2456,8 +2464,17 @@ ipc_kmsg_copyout_body(
 			} else if (is_port) {
 				/* copyout to memory allocated above */
 
-				(void) copyoutmap(map, (char *) data,
-						  (char *) addr, length);
+				if (sizeof(mach_port_name_t) != sizeof(mach_port_t)) {
+					mach_port_t *src = (mach_port_t*)data;
+					mach_port_name_t *dst = (mach_port_name_t*)addr;
+					for (int i=0; i<number; i++) {
+						if (copyout_port(src + i, dst + i))
+							goto vm_copyout_failure;
+					}
+				} else {
+					(void) copyoutmap(map, (char *) data,
+							  (char *) addr, length);
+				}
 				kfree(data, length);
 			} else {
 				vm_map_copy_t copy = (vm_map_copy_t) data;
diff --git a/ipc/ipc_mqueue.c b/ipc/ipc_mqueue.c
index ac6bed51..44e1eb98 100644
--- a/ipc/ipc_mqueue.c
+++ b/ipc/ipc_mqueue.c
@@ -36,6 +36,7 @@
 
 #include <mach/port.h>
 #include <mach/message.h>
+#include <machine/copy_user.h>
 #include <kern/assert.h>
 #include <kern/counters.h>
 #include <kern/debug.h>
@@ -540,7 +541,7 @@ ipc_mqueue_receive(
 		if (kmsg != IKM_NULL) {
 			/* check space requirements */
 
-			if (kmsg->ikm_header.msgh_size > max_size) {
+			if (msg_usize(&kmsg->ikm_header) > max_size) {
 				* (mach_msg_size_t *) kmsgp =
 					kmsg->ikm_header.msgh_size;
 				imq_unlock(mqueue);
@@ -649,7 +650,7 @@ ipc_mqueue_receive(
 	/* we have a kmsg; unlock the msg queue */
 
 	imq_unlock(mqueue);
-	assert(kmsg->ikm_header.msgh_size <= max_size);
+	assert(msg_usize(&kmsg->ikm_header) <= max_size);
     }
 
     {
diff --git a/ipc/mach_msg.c b/ipc/mach_msg.c
index 221ea975..1a6d67d0 100644
--- a/ipc/mach_msg.c
+++ b/ipc/mach_msg.c
@@ -39,6 +39,7 @@
 #include <mach/kern_return.h>
 #include <mach/port.h>
 #include <mach/message.h>
+#include <machine/copy_user.h>
 #include <kern/assert.h>
 #include <kern/counters.h>
 #include <kern/debug.h>
@@ -241,7 +242,7 @@ mach_msg_receive(
 			return mr;
 
 		kmsg->ikm_header.msgh_seqno = seqno;
-		if (kmsg->ikm_header.msgh_size > rcv_size) {
+		if (msg_usize(&kmsg->ikm_header) > rcv_size) {
 			ipc_kmsg_copyout_dest(kmsg, space);
 			(void) ipc_kmsg_put(msg, kmsg, sizeof *msg);
 			return MACH_RCV_TOO_LARGE;
@@ -321,7 +322,7 @@ mach_msg_receive_continue(void)
 		}
 
 		kmsg->ikm_header.msgh_seqno = seqno;
-		assert(kmsg->ikm_header.msgh_size <= rcv_size);
+		assert(msg_usize(&kmsg->ikm_header) <= rcv_size);
 	} else {
 		mr = ipc_mqueue_receive(mqueue, option & MACH_RCV_TIMEOUT,
 					MACH_MSG_SIZE_MAX, time_out,
@@ -335,7 +336,7 @@ mach_msg_receive_continue(void)
 		}
 
 		kmsg->ikm_header.msgh_seqno = seqno;
-		if (kmsg->ikm_header.msgh_size > rcv_size) {
+		if (msg_usize(&kmsg->ikm_header) > rcv_size) {
 			ipc_kmsg_copyout_dest(kmsg, space);
 			(void) ipc_kmsg_put(msg, kmsg, sizeof *msg);
 			thread_syscall_return(MACH_RCV_TOO_LARGE);
@@ -450,7 +451,7 @@ mach_msg_trap(
 		 */
 
 		if ((send_size > IKM_SAVED_MSG_SIZE) ||
-		    (send_size < sizeof(mach_msg_header_t)) ||
+		    (send_size < sizeof(mach_msg_user_header_t)) ||
 		    (send_size & 3) ||
 		    ((kmsg = ikm_cache()) == IKM_NULL))
 			goto slow_get;
@@ -940,7 +941,7 @@ mach_msg_trap(
 						== dest_port);
 
 		reply_size = kmsg->ikm_header.msgh_size;
-		if (rcv_size < reply_size)
+		if (rcv_size < msg_usize(&kmsg->ikm_header))
 			goto slow_copyout;
 
 		/* optimized ipc_kmsg_copyout/ipc_kmsg_copyout_header */
@@ -1450,7 +1451,7 @@ mach_msg_trap(
 		 */
 
 		reply_size = kmsg->ikm_header.msgh_size;
-		if (rcv_size < reply_size) {
+		if (rcv_size < msg_usize(&kmsg->ikm_header)) {
 			ipc_kmsg_copyout_dest(kmsg, space);
 			(void) ipc_kmsg_put(msg, kmsg, sizeof *msg);
 			thread_syscall_return(MACH_RCV_TOO_LARGE);
@@ -1544,7 +1545,7 @@ mach_msg_trap(
 			return mr;
 
 		kmsg->ikm_header.msgh_seqno = seqno;
-		if (rcv_size < kmsg->ikm_header.msgh_size) {
+		if (rcv_size < msg_usize(&kmsg->ikm_header)) {
 			ipc_kmsg_copyout_dest(kmsg, space);
 			(void) ipc_kmsg_put(msg, kmsg, sizeof *msg);
 			return MACH_RCV_TOO_LARGE;
@@ -1628,7 +1629,7 @@ mach_msg_continue(void)
 	}
 
 	kmsg->ikm_header.msgh_seqno = seqno;
-	if (kmsg->ikm_header.msgh_size > rcv_size) {
+	if (msg_usize(&kmsg->ikm_header) > rcv_size) {
 		ipc_kmsg_copyout_dest(kmsg, space);
 		(void) ipc_kmsg_put(msg, kmsg, sizeof *msg);
 		thread_syscall_return(MACH_RCV_TOO_LARGE);
diff --git a/x86_64/Makefrag.am b/x86_64/Makefrag.am
index 1ee1092a..f5f152b8 100644
--- a/x86_64/Makefrag.am
+++ b/x86_64/Makefrag.am
@@ -92,6 +92,7 @@ libkernel_a_SOURCES += \
 	i386/i386/cpu.h \
 	i386/i386/cpu_number.h \
 	x86_64/cswitch.S \
+	x86_64/copy_user.c \
 	i386/i386/db_disasm.c \
 	i386/i386/db_interface.c \
 	i386/i386/db_interface.h \
diff --git a/x86_64/copy_user.c b/x86_64/copy_user.c
new file mode 100644
index 00000000..34fe5212
--- /dev/null
+++ b/x86_64/copy_user.c
@@ -0,0 +1,362 @@
+/*
+ *  Copyright (C) 2023 Free Software Foundation
+ *
+ * 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 2 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 the program ; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <string.h>
+
+#include <kern/debug.h>
+#include <mach/boolean.h>
+
+#include <copy_user.h>
+
+
+/* Mach field descriptors measure size in bits */
+#define descsize_to_bytes(n) (n / 8)
+#define bytes_to_descsize(n) (n * 8)
+
+
+/*
+* Helper to unpack the relevant fields of a msg type; the fields are different
+* depending on whether is long form or not.
+.*/
+static inline vm_size_t unpack_msg_type(vm_offset_t addr,
+                                        mach_msg_type_name_t *name,
+                                        mach_msg_type_size_t *size,
+                                        mach_msg_type_number_t *number,
+                                        boolean_t *is_inline)
+{
+  mach_msg_type_t* kmt = (mach_msg_type_t*)addr;
+  *is_inline = kmt->msgt_inline;
+  if (kmt->msgt_longform)
+    {
+      mach_msg_type_long_t* kmtl = (mach_msg_type_long_t*)addr;
+      *name = kmtl->msgtl_name;
+      *size = kmtl->msgtl_size;
+      *number = kmtl->msgtl_number;
+      return sizeof(mach_msg_type_long_t);
+    }
+  else
+    {
+      *name = kmt->msgt_name;
+      *size = kmt->msgt_size;
+      *number = kmt->msgt_number;
+      return sizeof(mach_msg_type_t);
+    }
+}
+
+/* Optimized version of unpack_msg_type(), including proper copyin() */
+static inline int copyin_unpack_msg_type(vm_offset_t uaddr,
+                                         vm_offset_t kaddr,
+                                         mach_msg_type_name_t *name,
+                                         mach_msg_type_size_t *size,
+                                         mach_msg_type_number_t *number,
+                                         boolean_t *is_inline,
+                                         vm_size_t *amount)
+{
+  mach_msg_type_t *kmt = (mach_msg_type_t*)kaddr;
+  if (copyin((void*)uaddr, kmt, sizeof(mach_msg_type_t)))
+    return 1;
+  *is_inline = kmt->msgt_inline;
+  if (kmt->msgt_longform)
+    {
+      mach_msg_type_long_t* kmtl = (mach_msg_type_long_t*)kaddr;
+      if (copyin((void*)uaddr, kmtl, sizeof(mach_msg_type_long_t)))
+        return 1;
+      *name = kmtl->msgtl_name;
+      *size = kmtl->msgtl_size;
+      *number = kmtl->msgtl_number;
+      *amount = sizeof(mach_msg_type_long_t);
+    }
+  else
+    {
+      *name = kmt->msgt_name;
+      *size = kmt->msgt_size;
+      *number = kmt->msgt_number;
+      *amount = sizeof(mach_msg_type_t);
+    }
+  return 0;
+}
+
+/*
+ * The msg type has a different size field depending on whether is long or not,
+ * and we also need to convert from bytes to bits
+ */
+static inline void adjust_msg_type_size(vm_offset_t addr, int amount)
+{
+  mach_msg_type_t* kmt = (mach_msg_type_t*)addr;
+  if (kmt->msgt_longform)
+    {
+      mach_msg_type_long_t* kmtl = (mach_msg_type_long_t*)addr;
+      kmtl->msgtl_size += bytes_to_descsize(amount);
+    }
+  else
+    {
+      kmt->msgt_size += bytes_to_descsize(amount);
+    }
+}
+
+/*
+ * Compute the user-space size of a message still in the kernel.
+ * The message may be originating from userspace (in which case we could
+ * optimize this by keeping the usize around) or from kernel space (we could
+ * optimize if the message structure is fixed and known in advance).
+ * For now just handle the most general case, iterating over the msg body.
+ */
+size_t msg_usize(const mach_msg_header_t *kmsg)
+{
+  size_t ksize = kmsg->msgh_size;
+  size_t usize = sizeof(mach_msg_user_header_t);
+  if (ksize > sizeof(mach_msg_header_t))
+    {
+      // iterate over body compute the user-space message size
+      vm_offset_t saddr, eaddr;
+      saddr = (vm_offset_t)(kmsg + 1);
+      eaddr = saddr + ksize - sizeof(mach_msg_header_t);
+      while (saddr < (eaddr - sizeof(mach_msg_type_t)))
+        {
+          vm_size_t amount;
+          mach_msg_type_name_t name;
+          mach_msg_type_size_t size;
+          mach_msg_type_number_t number;
+          boolean_t is_inline;
+          amount = unpack_msg_type(saddr, &name, &size, &number, &is_inline);
+          saddr += amount;
+          usize += amount;
+
+          if (is_inline)
+            {
+              if (MACH_MSG_TYPE_PORT_ANY(name))
+                {
+                  saddr += sizeof(mach_port_t) * number;
+                  usize += sizeof(mach_port_name_t) * number;
+                }
+              else
+                {
+                  size_t n = descsize_to_bytes(size);
+                  saddr += n*number;
+                  usize += n*number;
+                  saddr = msg_align(saddr);
+                  usize = msg_align(usize);
+                }
+            }
+          else
+            {
+              // advance one pointer
+              saddr += sizeof(vm_offset_t);
+              usize += sizeof(rpc_vm_offset_t);
+            }
+        }
+    }
+  return usize;
+}
+
+/*
+ * Expand the msg header and, if required, the msg body (ports, pointers)
+ *
+ * To not make the code too compicated, we use the fact that some fields of
+ * mach_msg_header have the same size in the kernel and user variant (basically
+ * all fields except ports and addresses)
+*/
+int copyinmsg (const void *userbuf, void *kernelbuf, const size_t usize)
+{
+  const mach_msg_user_header_t *umsg = userbuf;
+  mach_msg_header_t *kmsg = kernelbuf;
+
+  if (copyin(&umsg->msgh_bits, &kmsg->msgh_bits, sizeof(kmsg->msgh_bits)))
+    return 1;
+  /* kmsg->msgh_size is filled in later */
+  if (copyin_port(&umsg->msgh_remote_port, &kmsg->msgh_remote_port))
+    return 1;
+  if (copyin_port(&umsg->msgh_local_port, &kmsg->msgh_local_port))
+    return 1;
+  if (copyin(&umsg->msgh_seqno, &kmsg->msgh_seqno,
+             sizeof(kmsg->msgh_seqno) + sizeof(kmsg->msgh_id)))
+    return 1;
+
+  vm_offset_t usaddr, ueaddr, ksaddr;
+  ksaddr = (vm_offset_t)(kmsg + 1);
+  usaddr = (vm_offset_t)(umsg + 1);
+  ueaddr = (vm_offset_t)umsg + usize;
+  if (usize > sizeof(mach_msg_user_header_t))
+    {
+      /* check we have at least space for an empty descryptor */
+      while (usaddr < (ueaddr - sizeof(mach_msg_type_t)))
+        {
+          vm_size_t amount;
+          mach_msg_type_name_t name;
+          mach_msg_type_size_t size;
+          mach_msg_type_number_t number;
+          boolean_t is_inline;
+          usaddr = msg_align(usaddr);
+          ksaddr = msg_align(ksaddr);
+          if (copyin_unpack_msg_type(usaddr, ksaddr, &name, &size, &number,
+                                     &is_inline, &amount))
+            return 1;
+
+          // keep a reference to the current field descriptor, we
+          // might need to adjust it later depending on the type
+          vm_offset_t ktaddr = ksaddr;
+          usaddr += amount;
+          ksaddr += amount;
+
+          if (is_inline)
+            {
+              if (MACH_MSG_TYPE_PORT_ANY(name))
+                {
+                  if ((usaddr + sizeof(mach_port_name_t)*number) > ueaddr)
+                    return 1;
+                  adjust_msg_type_size(ktaddr, sizeof(mach_port_t) - sizeof(mach_port_name_t));
+                  for (int i=0; i<number; i++)
+                    {
+                      if (copyin_port((mach_port_name_t*)usaddr, (mach_port_t*)ksaddr))
+                        return 1;
+                      ksaddr += sizeof(mach_port_t);
+                      usaddr += sizeof(mach_port_name_t);
+                    }
+                }
+              else
+                {
+                  // type that doesn't need change
+                  size_t n = descsize_to_bytes(size);
+                  if ((usaddr + n*number) > ueaddr)
+                    return 1;
+                  if (copyin((void*)usaddr, (void*)ksaddr, n*number))
+                    return 1;
+                  usaddr += n*number;
+                  ksaddr += n*number;
+                  usaddr = msg_align(usaddr);
+                  ksaddr = msg_align(ksaddr);
+                }
+            }
+          else
+            {
+              if ((usaddr + sizeof(rpc_vm_offset_t)) > ueaddr)
+                return 1;
+
+              // out-of-line port arrays are expanded in ipc_kmsg_copyin_body()
+              if (MACH_MSG_TYPE_PORT_ANY(name))
+                adjust_msg_type_size(ktaddr, sizeof(mach_port_t) - sizeof(mach_port_name_t));
+
+              if (copyin_address((rpc_vm_offset_t*)usaddr, (vm_offset_t*)ksaddr))
+                return 1;
+              // advance one pointer
+              ksaddr += sizeof(vm_offset_t);
+              usaddr += sizeof(rpc_vm_offset_t);
+            }
+        }
+    }
+
+  kmsg->msgh_size = sizeof(mach_msg_header_t) + ksaddr - (vm_offset_t)(kmsg + 1);
+  kmsg->msgh_size = msg_align(kmsg->msgh_size);
+  return 0;
+}
+
+int copyoutmsg (const void *kernelbuf, void *userbuf, const size_t ksize)
+{
+  const mach_msg_header_t *kmsg = kernelbuf;
+  mach_msg_user_header_t *umsg = userbuf;
+
+  if (copyout(&kmsg->msgh_bits, &umsg->msgh_bits, sizeof(kmsg->msgh_bits)))
+    return 1;
+  /* umsg->msgh_size is filled in later */
+  if (copyout_port(&kmsg->msgh_remote_port, &umsg->msgh_remote_port))
+    return 1;
+  if (copyout_port(&kmsg->msgh_local_port, &umsg->msgh_local_port))
+    return 1;
+  if (copyout(&kmsg->msgh_seqno, &umsg->msgh_seqno,
+             sizeof(kmsg->msgh_seqno) + sizeof(kmsg->msgh_id)))
+    return 1;
+
+  vm_offset_t ksaddr, keaddr, usaddr;
+  ksaddr = (vm_offset_t)(kmsg + 1);
+  usaddr = (vm_offset_t)(umsg + 1);
+  keaddr = ksaddr + ksize - sizeof(mach_msg_header_t);
+
+  if (ksize > sizeof(mach_msg_user_header_t))
+    {
+      while (ksaddr < keaddr)
+        {
+          vm_size_t amount;
+          mach_msg_type_name_t name;
+          mach_msg_type_size_t size;
+          mach_msg_type_number_t number;
+          boolean_t is_inline;
+          usaddr = msg_align(usaddr);
+          ksaddr = msg_align(ksaddr);
+          amount = unpack_msg_type(ksaddr, &name, &size, &number, &is_inline);
+          // TODO: optimize and bring here type adjustment??
+          vm_offset_t utaddr=usaddr, ktaddr=ksaddr;
+          if (copyout((void*)ksaddr, (void*)usaddr, amount))
+            return 1;
+          usaddr += amount;
+          ksaddr += amount;
+
+          if (is_inline)
+            {
+              if (MACH_MSG_TYPE_PORT_ANY(name))
+                {
+                  adjust_msg_type_size(ktaddr, (int)sizeof(mach_port_name_t) - (int)sizeof(mach_port_t));
+                  if (copyout((void*)ktaddr, (void*)utaddr, amount))
+                    return 1;
+                  for (int i=0; i<number; i++)
+                    {
+                      if (copyout_port((mach_port_t*)ksaddr, (mach_port_name_t*)usaddr))
+                        return 1;
+                      ksaddr += sizeof(mach_port_t);
+                      usaddr += sizeof(mach_port_name_t);
+                    }
+                }
+              else
+                {
+                  // type that doesn't need change
+                  size_t n = descsize_to_bytes(size);
+                  if (copyout((void*)ksaddr, (void*)usaddr, n*number))
+                    return 1;
+                  usaddr += n*number;
+                  ksaddr += n*number;
+                  usaddr = msg_align(usaddr);
+                  ksaddr = msg_align(ksaddr);
+                }
+            }
+          else
+            {
+              // out-of-line port arrays are shrinked in ipc_kmsg_copyout_body()
+              if (MACH_MSG_TYPE_PORT_ANY(name))
+                {
+                  adjust_msg_type_size(ktaddr, -4);
+                  if (copyout((void*)ktaddr, (void*)utaddr, amount))
+                    return 1;
+                }
+
+              if (copyout_address((vm_offset_t*)ksaddr, (rpc_vm_offset_t*)usaddr))
+                return 1;
+              // advance one pointer
+              ksaddr += sizeof(vm_offset_t);
+              usaddr += sizeof(rpc_vm_offset_t);
+            }
+        }
+    }
+
+  mach_msg_size_t usize;
+  usize = sizeof(mach_msg_user_header_t) + usaddr - (vm_offset_t)(umsg + 1);
+  usize = msg_align(usize);
+  if (copyout(&usize, &umsg->msgh_size, sizeof(kmsg->msgh_size)))
+    return 1;
+
+  return 0;
+
+}
diff --git a/x86_64/locore.S b/x86_64/locore.S
index f81886da..21a3e373 100644
--- a/x86_64/locore.S
+++ b/x86_64/locore.S
@@ -1329,48 +1329,6 @@ copyin_fail:
 	movq	$1,%rax			/* return 1 for failure */
 	jmp	copyin_ret		/* pop frame and return */
 
-/*
- * Copy from user address space - version for copying messages.
- * arg0:	user address
- * arg1:	kernel address
- * arg2:	byte count
- */
-ENTRY(copyinmsg)
-	xchgq   %rsi,%rdi		/* Get user source and kernel destination */
-	movl	%edx,MSGH_MSGH_SIZE(%rdi)	/* set msgh_size already */
-
-/* 32 on 64 conversion */
-	subq	$32,%rdx
-	js	bogus
-
-	/* Copy msgh_bits */
-	RECOVER(copyin_fail)
-	movsl
-
-	/* Copy msgh_size */
-	RECOVER(copyin_fail)
-	lodsl
-        /* But don't actually store it: we have already set it above to what we
-           really copy */
-	addq	$4,%rdi
-
-	xorq	%rax,%rax
-	/* Copy msgh_remote_port */
-	RECOVER(copyin_fail)
-	lodsl
-	stosq
-
-	/* Copy msgh_local_port */
-	RECOVER(copyin_fail)
-	lodsl
-	stosq
-
-	/* Copy msgh_seqno and msgh_id */
-	RECOVER(copyin_fail)
-	movsq
-
-	jmp	copyin_remainder
-
 bogus:
 	ud2
 
@@ -1400,45 +1358,6 @@ copyout_fail:
 	jmp	copyout_ret		/* pop frame and return */
 
 /*
- * Copy to user address space.
- * arg0:	kernel address
- * arg1:	user address
- * arg2:	byte count
- */
-ENTRY(copyoutmsg)
-	xchgq   %rsi,%rdi		/* Get user source and kernel destination */
-
-/* 32 on 64 conversion */
-	subq	$32,%rdx
-	js	bogus
-
-	/* Copy msgh_bits */
-	RECOVER(copyout_fail)
-	movsl
-
-	/* Copy msgh_size */
-	lodsl
-	subl	$8,%eax
-	RECOVER(copyout_fail)
-	stosl
-
-	/* Copy msgh_remote_port */
-	lodsq
-	RECOVER(copyout_fail)
-	stosl
-
-	/* Copy msgh_local_port */
-	lodsq
-	RECOVER(copyout_fail)
-	stosl
-
-	/* Copy msgh_seqno and msgh_id */
-	RECOVER(copyout_fail)
-	movsq
-
-	jmp	copyin_remainder
-
-/*
  * int inst_fetch(int eip, int cs);
  *
  * Fetch instruction byte.  Return -1 if invalid address.