staging: vchiq_core: introduce process_free_data_message

This moves the special handling for data messages from process_free_queue()
into a new function. After this process_free_queue() has less extreme
indentation and should be easier to read.
Signed-off-by: Stefan Wahren <stefan.wahren@i2se.com>
Link: https://lore.kernel.org/r/1622735405-9980-13-git-send-email-stefan.wahren@i2se.comSigned-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

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

@@ -717,59 +717,13 @@ reserve_space(struct vchiq_state *state, size_t space, int is_blocking)
 						(tx_pos & VCHIQ_SLOT_MASK));
 }
 
-/* Called by the recycle thread. */
 static void
-process_free_queue(struct vchiq_state *state, BITSET_T *service_found,
-		   size_t length)
+process_free_data_message(struct vchiq_state *state, BITSET_T *service_found,
+			  struct vchiq_header *header)
 {
-	struct vchiq_shared_state *local = state->local;
-	int slot_queue_available;
-
-	/*
-	 * Find slots which have been freed by the other side, and return them
-	 * to the available queue.
-	 */
-	slot_queue_available = state->slot_queue_available;
-
-	/*
-	 * Use a memory barrier to ensure that any state that may have been
-	 * modified by another thread is not masked by stale prefetched
-	 * values.
-	 */
-	mb();
-
-	while (slot_queue_available != local->slot_queue_recycle) {
-		unsigned int pos;
-		int slot_index = local->slot_queue[slot_queue_available &
-			VCHIQ_SLOT_QUEUE_MASK];
-		char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
-		int data_found = 0;
-
-		slot_queue_available++;
-		/*
-		 * Beware of the address dependency - data is calculated
-		 * using an index written by the other side.
-		 */
-		rmb();
-
-		vchiq_log_trace(vchiq_core_log_level, "%d: pfq %d=%pK %x %x",
-			state->id, slot_index, data,
-			local->slot_queue_recycle, slot_queue_available);
-
-		/* Initialise the bitmask for services which have used this slot */
-		memset(service_found, 0, length);
-
-		pos = 0;
-
-		while (pos < VCHIQ_SLOT_SIZE) {
-			struct vchiq_header *header =
-				(struct vchiq_header *)(data + pos);
 	int msgid = header->msgid;
-
-			if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) {
 	int port = VCHIQ_MSG_SRCPORT(msgid);
-				struct vchiq_service_quota *quota =
-					&state->service_quotas[port];
+	struct vchiq_service_quota *quota = &state->service_quotas[port];
 	int count;
 
 	spin_lock(&quota_spinlock);
@@ -800,8 +754,7 @@ process_free_queue(struct vchiq_state *state, BITSET_T *service_found,
 		spin_lock(&quota_spinlock);
 		count = quota->slot_use_count;
 		if (count > 0)
-						quota->slot_use_count =
-							count - 1;
+			quota->slot_use_count = count - 1;
 		spin_unlock(&quota_spinlock);
 
 		if (count > 0) {
@@ -810,15 +763,13 @@ process_free_queue(struct vchiq_state *state, BITSET_T *service_found,
 			 * it has dropped below its quota
 			 */
 			complete(&quota->quota_event);
-						vchiq_log_trace(
-							vchiq_core_log_level,
+			vchiq_log_trace(vchiq_core_log_level,
 				"%d: pfq:%d %x@%pK - slot_use->%d",
 				state->id, port,
 				header->size, header,
 				count - 1);
 		} else {
-						vchiq_log_error(
-							vchiq_core_log_level,
+			vchiq_log_error(vchiq_core_log_level,
 					"service %d slot_use_count=%d (header %pK, msgid %x, header->msgid %x, header->size %x)",
 				port, count, header,
 				msgid, header->msgid,
@@ -826,7 +777,60 @@ process_free_queue(struct vchiq_state *state, BITSET_T *service_found,
 			WARN(1, "bad slot use count\n");
 		}
 	}
+}
+
+/* Called by the recycle thread. */
+static void
+process_free_queue(struct vchiq_state *state, BITSET_T *service_found,
+		   size_t length)
+{
+	struct vchiq_shared_state *local = state->local;
+	int slot_queue_available;
+
+	/*
+	 * Find slots which have been freed by the other side, and return them
+	 * to the available queue.
+	 */
+	slot_queue_available = state->slot_queue_available;
 
+	/*
+	 * Use a memory barrier to ensure that any state that may have been
+	 * modified by another thread is not masked by stale prefetched
+	 * values.
+	 */
+	mb();
+
+	while (slot_queue_available != local->slot_queue_recycle) {
+		unsigned int pos;
+		int slot_index = local->slot_queue[slot_queue_available &
+			VCHIQ_SLOT_QUEUE_MASK];
+		char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
+		int data_found = 0;
+
+		slot_queue_available++;
+		/*
+		 * Beware of the address dependency - data is calculated
+		 * using an index written by the other side.
+		 */
+		rmb();
+
+		vchiq_log_trace(vchiq_core_log_level, "%d: pfq %d=%pK %x %x",
+			state->id, slot_index, data,
+			local->slot_queue_recycle, slot_queue_available);
+
+		/* Initialise the bitmask for services which have used this slot */
+		memset(service_found, 0, length);
+
+		pos = 0;
+
+		while (pos < VCHIQ_SLOT_SIZE) {
+			struct vchiq_header *header =
+				(struct vchiq_header *)(data + pos);
+			int msgid = header->msgid;
+
+			if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) {
+				process_free_data_message(state, service_found,
+							  header);
 				data_found = 1;
 			}