staging: vchiq: Move message queue into struct vchiq_service

This has historically been handled by vchi, but there is no reason why
this couldn't be handled directly in vchiq.

The patch tries to avoid altering any behavior, with the exception of
the msg_queue size, which is now fixed to VCHIQ_MAX_SLOTS (it was set to
VCHIQ_MAX_SLOTS / 2). This is done to match vchiq's user_service message
queue, which could be merged with this one in the future.
Signed-off-by: Nicolas Saenz Julienne <nsaenzjulienne@suse.de>
Link: https://lore.kernel.org/r/20200629150945.10720-24-nsaenzjulienne@suse.deSigned-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

drivers/staging/vc04_services/Makefile

@@ -7,7 +7,6 @@ vchiq-objs := \
    interface/vchiq_arm/vchiq_2835_arm.o \
    interface/vchiq_arm/vchiq_debugfs.o \
    interface/vchiq_arm/vchiq_shim.o \
-   interface/vchiq_arm/vchiq_util.o \
    interface/vchiq_arm/vchiq_connected.o \
 
 obj-$(CONFIG_SND_BCM2835)		+= bcm2835-audio/

drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c

@@ -2265,6 +2265,47 @@ vchiq_init_state(struct vchiq_state *state, struct vchiq_slot_zero *slot_zero)
 	return VCHIQ_ERROR;
 }
 
+void vchiq_msg_queue_push(unsigned handle, struct vchiq_header *header)
+{
+	struct vchiq_service *service = find_service_by_handle(handle);
+	int pos;
+
+	while (service->msg_queue_write == service->msg_queue_read +
+		VCHIQ_MAX_SLOTS) {
+		if (wait_for_completion_interruptible(&service->msg_queue_pop))
+			flush_signals(current);
+	}
+
+	pos = service->msg_queue_write++ & (VCHIQ_MAX_SLOTS - 1);
+	service->msg_queue[pos] = header;
+
+	complete(&service->msg_queue_push);
+}
+EXPORT_SYMBOL(vchiq_msg_queue_push);
+
+struct vchiq_header *vchiq_msg_hold(unsigned handle)
+{
+	struct vchiq_service *service = find_service_by_handle(handle);
+	struct vchiq_header *header;
+	int pos;
+
+	if (service->msg_queue_write == service->msg_queue_read)
+		return NULL;
+
+	while (service->msg_queue_write == service->msg_queue_read) {
+		if (wait_for_completion_interruptible(&service->msg_queue_push))
+			flush_signals(current);
+	}
+
+	pos = service->msg_queue_read++ & (VCHIQ_MAX_SLOTS - 1);
+	header = service->msg_queue[pos];
+
+	complete(&service->msg_queue_pop);
+
+	return header;
+}
+EXPORT_SYMBOL(vchiq_msg_hold);
+
 static int vchiq_validate_params(const struct vchiq_service_params *params)
 {
 	if (!params->callback || !params->fourcc) {
@@ -2319,12 +2360,17 @@ vchiq_add_service_internal(struct vchiq_state *state,
 	service->state         = state;
 	service->instance      = instance;
 	service->service_use_count = 0;
+	service->msg_queue_read = 0;
+	service->msg_queue_write = 0;
 	init_bulk_queue(&service->bulk_tx);
 	init_bulk_queue(&service->bulk_rx);
 	init_completion(&service->remove_event);
 	init_completion(&service->bulk_remove_event);
+	init_completion(&service->msg_queue_pop);
+	init_completion(&service->msg_queue_push);
 	mutex_init(&service->bulk_mutex);
 	memset(&service->stats, 0, sizeof(service->stats));
+	memset(&service->msg_queue, 0, sizeof(service->msg_queue));
 
 	/* Although it is perfectly possible to use a spinlock
 	** to protect the creation of services, it is overkill as it

drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h

@@ -297,6 +297,12 @@ struct vchiq_service {
 		uint64_t bulk_tx_bytes;
 		uint64_t bulk_rx_bytes;
 	} stats;
+
+	int msg_queue_read;
+	int msg_queue_write;
+	struct completion msg_queue_pop;
+	struct completion msg_queue_push;
+	struct vchiq_header *msg_queue[VCHIQ_MAX_SLOTS];
 };
 
 /* The quota information is outside struct vchiq_service so that it can

drivers/staging/vc04_services/interface/vchiq_arm/vchiq_if.h

@@ -136,5 +136,7 @@ extern enum vchiq_status vchiq_dump_phys_mem(unsigned int service,
 
 extern enum vchiq_status vchiq_get_peer_version(unsigned int handle,
       short *peer_version);
+extern void vchiq_msg_queue_push(unsigned handle, struct vchiq_header *header);
+extern struct vchiq_header *vchiq_msg_hold(unsigned handle);
 
 #endif /* VCHIQ_IF_H */

drivers/staging/vc04_services/interface/vchiq_arm/vchiq_shim.c

@@ -13,8 +13,6 @@
 struct vchi_service {
 	unsigned int handle;
 
-	struct vchiu_queue queue;
-
 	vchi_callback callback;
 	void *callback_param;
 };
@@ -172,10 +170,9 @@ int32_t vchi_msg_hold(struct vchi_service *service, void **data,
 {
 	struct vchiq_header *header;
 
-	if (vchiu_queue_is_empty(&service->queue))
-		return -1;
-
-	header = vchiu_queue_pop(&service->queue);
+	header = vchiq_msg_hold(service->handle);
+	if (!header)
+		return -ENOENT;
 
 	*data = header->data;
 	*msg_size = header->size;
@@ -272,7 +269,7 @@ static enum vchiq_status shim_callback(enum vchiq_reason reason,
 		(struct vchi_service *)VCHIQ_GET_SERVICE_USERDATA(handle);
 
 	if (reason == VCHIQ_MESSAGE_AVAILABLE)
-		vchiu_queue_push(&service->queue, header);
+		vchiq_msg_queue_push(service->handle, header);
 
 	service->callback(service->callback_param, reason, bulk_user);
 
@@ -285,13 +282,8 @@ static struct vchi_service *service_alloc(struct vchiq_instance *instance,
 	struct vchi_service *service = kzalloc(sizeof(struct vchi_service), GFP_KERNEL);
 
 	if (service) {
-		if (!vchiu_queue_init(&service->queue, 64)) {
-			service->callback = setup->callback;
-			service->callback_param = setup->callback_param;
-		} else {
-			kfree(service);
-			service = NULL;
-		}
+		service->callback = setup->callback;
+		service->callback_param = setup->callback_param;
 	}
 
 	return service;
@@ -299,10 +291,8 @@ static struct vchi_service *service_alloc(struct vchiq_instance *instance,
 
 static void service_free(struct vchi_service *service)
 {
-	if (service) {
-		vchiu_queue_delete(&service->queue);
+	if (service)
 		kfree(service);
-	}
 }
 
 int32_t vchi_service_open(struct vchiq_instance *instance,

drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.c

-// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
-/* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
-
-#include "vchiq_util.h"
-
-static inline int is_pow2(int i)
-{
-	return i && !(i & (i - 1));
-}
-
-int vchiu_queue_init(struct vchiu_queue *queue, int size)
-{
-	WARN_ON(!is_pow2(size));
-
-	queue->size = size;
-	queue->read = 0;
-	queue->write = 0;
-	queue->initialized = 1;
-
-	init_completion(&queue->pop);
-	init_completion(&queue->push);
-
-	queue->storage = kcalloc(size, sizeof(struct vchiq_header *),
-				 GFP_KERNEL);
-	if (!queue->storage) {
-		vchiu_queue_delete(queue);
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-void vchiu_queue_delete(struct vchiu_queue *queue)
-{
-	kfree(queue->storage);
-}
-
-int vchiu_queue_is_empty(struct vchiu_queue *queue)
-{
-	return queue->read == queue->write;
-}
-
-void vchiu_queue_push(struct vchiu_queue *queue, struct vchiq_header *header)
-{
-	if (!queue->initialized)
-		return;
-
-	while (queue->write == queue->read + queue->size) {
-		if (wait_for_completion_interruptible(&queue->pop))
-			flush_signals(current);
-	}
-
-	queue->storage[queue->write & (queue->size - 1)] = header;
-	queue->write++;
-
-	complete(&queue->push);
-}
-
-struct vchiq_header *vchiu_queue_pop(struct vchiu_queue *queue)
-{
-	struct vchiq_header *header;
-
-	while (queue->write == queue->read) {
-		if (wait_for_completion_interruptible(&queue->push))
-			flush_signals(current);
-	}
-
-	header = queue->storage[queue->read & (queue->size - 1)];
-	queue->read++;
-
-	complete(&queue->pop);
-
-	return header;
-}

drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.h

@@ -24,26 +24,4 @@
 
 #include "vchiq_if.h"
 
-struct vchiu_queue {
-	int size;
-	int read;
-	int write;
-	int initialized;
-
-	struct completion pop;
-	struct completion push;
-
-	struct vchiq_header **storage;
-};
-
-extern int  vchiu_queue_init(struct vchiu_queue *queue, int size);
-extern void vchiu_queue_delete(struct vchiu_queue *queue);
-
-extern int vchiu_queue_is_empty(struct vchiu_queue *queue);
-
-extern void vchiu_queue_push(struct vchiu_queue *queue,
-			     struct vchiq_header *header);
-
-extern struct vchiq_header *vchiu_queue_pop(struct vchiu_queue *queue);
-
 #endif