[PATCH V2 5/7] staging: vchiq_arm: Add compat ioctl for dequeue message

Michael Zoran mzoran at crowfest.net
Sat Jan 21 09:48:14 PST 2017


Add compat ioctl for dequeue message

Signed-off-by: Michael Zoran <mzoran at crowfest.net>
---
 .../vc04_services/interface/vchiq_arm/vchiq_arm.c  | 221 +++++++++++++--------
 1 file changed, 137 insertions(+), 84 deletions(-)

diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
index 8e8162d4b2e4..961a10eee525 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
@@ -1355,6 +1355,137 @@ vchiq_ioctl_compat_await_completion(struct vchiq_ioctl_ctxt *ctxt)
 
 #endif
 
+static long
+vchiq_ioctl_do_dequeue_message(struct vchiq_ioctl_ctxt *ctxt)
+{
+	VCHIQ_DEQUEUE_MESSAGE_T *args = ctxt->args;
+	USER_SERVICE_T *user_service;
+	VCHIQ_HEADER_T *header;
+	long ret = 0;
+
+	DEBUG_INITIALISE(g_state.local)
+
+	ctxt->service = find_service_for_instance(ctxt->instance, args->handle);
+	if (!ctxt->service)
+		return -EINVAL;
+
+	user_service = (USER_SERVICE_T *)(ctxt->service)->base.userdata;
+	if (!user_service->is_vchi)
+		return -EINVAL;
+
+	spin_lock(&msg_queue_spinlock);
+	if (user_service->msg_remove == user_service->msg_insert) {
+		if (!args->blocking) {
+			spin_unlock(&msg_queue_spinlock);
+			DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+			return -EWOULDBLOCK;
+		}
+
+		user_service->dequeue_pending = 1;
+		do {
+			spin_unlock(&msg_queue_spinlock);
+			DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+			if (down_interruptible(&user_service->insert_event)) {
+				vchiq_log_info(vchiq_arm_log_level,
+					       "DEQUEUE_MESSAGE interrupted");
+				return -EINTR;
+			}
+			spin_lock(&msg_queue_spinlock);
+		} while (user_service->msg_remove == user_service->msg_insert);
+	}
+
+	BUG_ON((int)(user_service->msg_insert - user_service->msg_remove) < 0);
+
+	header = user_service->msg_queue[user_service->msg_remove &
+		(MSG_QUEUE_SIZE - 1)];
+	user_service->msg_remove++;
+	spin_unlock(&msg_queue_spinlock);
+
+	up(&user_service->remove_event);
+
+	if (!header)
+		return -ENOTCONN;
+
+	if (header->size > args->bufsize) {
+		vchiq_log_error(vchiq_arm_log_level,
+				"header %pK: bufsize %x < size %x",
+				header, args->bufsize, header->size);
+		WARN(1, "invalid size\n");
+		return -EMSGSIZE;
+	}
+
+	if (args->buf)
+		if (copy_to_user(args->buf,
+				 header->data,
+				 header->size))
+			return -EFAULT;
+
+	ret = header->size;
+	vchiq_release_message(ctxt->service->handle, header);
+
+	DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+
+	return ret;
+}
+
+static long
+vchiq_ioctl_dequeue_message(struct vchiq_ioctl_ctxt *ctxt)
+{
+	VCHIQ_DEQUEUE_MESSAGE_T __user *puargs =
+		(VCHIQ_DEQUEUE_MESSAGE_T __user *)ctxt->arg;
+	VCHIQ_DEQUEUE_MESSAGE_T args;
+
+	DEBUG_INITIALISE(g_state.local)
+
+	DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+
+	if (copy_from_user(&args, puargs, sizeof(args)))
+		return -EFAULT;
+
+	ctxt->args = &args;
+
+	return vchiq_ioctl_do_dequeue_message(ctxt);
+}
+
+#if defined(CONFIG_COMPAT)
+
+struct vchiq_dequeue_message32 {
+	unsigned int handle;
+	int blocking;
+	unsigned int bufsize;
+	compat_uptr_t buf;
+};
+
+#define VCHIQ_IOC_DEQUEUE_MESSAGE32 \
+	_IOWR(VCHIQ_IOC_MAGIC, 8, struct vchiq_dequeue_message32)
+
+static long
+vchiq_ioctl_compat_dequeue_message(struct vchiq_ioctl_ctxt *ctxt)
+{
+	struct vchiq_dequeue_message32 __user *puargs32 =
+		(struct vchiq_dequeue_message32 __user *)ctxt->arg;
+	struct vchiq_dequeue_message32 args32;
+	VCHIQ_DEQUEUE_MESSAGE_T args;
+
+	DEBUG_INITIALISE(g_state.local)
+
+	DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
+
+	if (copy_from_user(&args32, puargs32, sizeof(args32)))
+		return -EFAULT;
+
+	args.handle = args32.handle;
+	args.blocking = args32.blocking;
+	args.bufsize = args32.bufsize;
+	args.buf = compat_ptr(args32.buf);
+
+	ctxt->args = &args;
+
+	return vchiq_ioctl_do_dequeue_message(ctxt);
+}
+
+#endif
+
 /****************************************************************************
 *
 *   vchiq_ioctl
@@ -1368,7 +1499,6 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	VCHIQ_SERVICE_T *service = NULL;
 	long ret = 0;
 	int i, rc;
-	DEBUG_INITIALISE(g_state.local)
 
 	switch (cmd) {
 	case VCHIQ_IOC_CREATE_SERVICE:
@@ -1384,6 +1514,9 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	case VCHIQ_IOC_AWAIT_COMPLETION:
 		return vchiq_dispatch_ioctl(vchiq_ioctl_await_completion,
 					    file, cmd, arg);
+	case VCHIQ_IOC_DEQUEUE_MESSAGE:
+		return vchiq_dispatch_ioctl(vchiq_ioctl_dequeue_message,
+					    file, cmd, arg);
 	default:
 		break;
 	}
@@ -1522,89 +1655,6 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			ret = -EINVAL;
 	} break;
 
-	case VCHIQ_IOC_DEQUEUE_MESSAGE: {
-		VCHIQ_DEQUEUE_MESSAGE_T args;
-		USER_SERVICE_T *user_service;
-		VCHIQ_HEADER_T *header;
-
-		DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
-		if (copy_from_user
-			 (&args, (const void __user *)arg,
-			  sizeof(args)) != 0) {
-			ret = -EFAULT;
-			break;
-		}
-		service = find_service_for_instance(instance, args.handle);
-		if (!service) {
-			ret = -EINVAL;
-			break;
-		}
-		user_service = (USER_SERVICE_T *)service->base.userdata;
-		if (user_service->is_vchi == 0) {
-			ret = -EINVAL;
-			break;
-		}
-
-		spin_lock(&msg_queue_spinlock);
-		if (user_service->msg_remove == user_service->msg_insert) {
-			if (!args.blocking) {
-				spin_unlock(&msg_queue_spinlock);
-				DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
-				ret = -EWOULDBLOCK;
-				break;
-			}
-			user_service->dequeue_pending = 1;
-			do {
-				spin_unlock(&msg_queue_spinlock);
-				DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
-				if (down_interruptible(
-					&user_service->insert_event) != 0) {
-					vchiq_log_info(vchiq_arm_log_level,
-						"DEQUEUE_MESSAGE interrupted");
-					ret = -EINTR;
-					break;
-				}
-				spin_lock(&msg_queue_spinlock);
-			} while (user_service->msg_remove ==
-				user_service->msg_insert);
-
-			if (ret)
-				break;
-		}
-
-		BUG_ON((int)(user_service->msg_insert -
-			user_service->msg_remove) < 0);
-
-		header = user_service->msg_queue[user_service->msg_remove &
-			(MSG_QUEUE_SIZE - 1)];
-		user_service->msg_remove++;
-		spin_unlock(&msg_queue_spinlock);
-
-		up(&user_service->remove_event);
-		if (header == NULL)
-			ret = -ENOTCONN;
-		else if (header->size <= args.bufsize) {
-			/* Copy to user space if msgbuf is not NULL */
-			if ((args.buf == NULL) ||
-				(copy_to_user((void __user *)args.buf,
-				header->data,
-				header->size) == 0)) {
-				ret = header->size;
-				vchiq_release_message(
-					service->handle,
-					header);
-			} else
-				ret = -EFAULT;
-		} else {
-			vchiq_log_error(vchiq_arm_log_level,
-				"header %pK: bufsize %x < size %x",
-				header, args.bufsize, header->size);
-			WARN(1, "invalid size\n");
-			ret = -EMSGSIZE;
-		}
-		DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
-	} break;
-
 	case VCHIQ_IOC_GET_CLIENT_ID: {
 		VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg;
 
@@ -1743,6 +1793,9 @@ vchiq_ioctl_compat(struct file *file, unsigned int cmd, unsigned long arg)
 	case VCHIQ_IOC_AWAIT_COMPLETION32:
 		return vchiq_dispatch_ioctl(vchiq_ioctl_compat_await_completion,
 					    file, cmd, arg);
+	case VCHIQ_IOC_DEQUEUE_MESSAGE32:
+		return vchiq_dispatch_ioctl(vchiq_ioctl_compat_dequeue_message,
+					    file, cmd, arg);
 	default:
 		return vchiq_ioctl(file, cmd, arg);
 	}
-- 
2.11.0




More information about the linux-rpi-kernel mailing list