diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
index 2a83c2a061ef..3e847d8821d6 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -1318,72 +1318,77 @@ notify_bulks(struct vchiq_service *service, struct vchiq_bulk_queue *queue,
 	return status;
 }
 
+static void
+poll_services_of_group(struct vchiq_state *state, int group)
+{
+	u32 flags = atomic_xchg(&state->poll_services[group], 0);
+	int i;
+
+	for (i = 0; flags; i++) {
+		if (flags & BIT(i)) {
+			struct vchiq_service *service =
+				find_service_by_port(state,
+					(group<<5) + i);
+			u32 service_flags;
+
+			flags &= ~BIT(i);
+			if (!service)
+				continue;
+			service_flags =
+				atomic_xchg(&service->poll_flags, 0);
+			if (service_flags &
+				BIT(VCHIQ_POLL_REMOVE)) {
+				vchiq_log_info(vchiq_core_log_level,
+					"%d: ps - remove %d<->%d",
+					state->id, service->localport,
+					service->remoteport);
+
+				/*
+				 * Make it look like a client, because
+				 * it must be removed and not left in
+				 * the LISTENING state.
+				 */
+				service->public_fourcc =
+					VCHIQ_FOURCC_INVALID;
+
+				if (vchiq_close_service_internal(
+					service, 0/*!close_recvd*/) !=
+					VCHIQ_SUCCESS)
+					request_poll(state, service,
+						VCHIQ_POLL_REMOVE);
+			} else if (service_flags &
+				BIT(VCHIQ_POLL_TERMINATE)) {
+				vchiq_log_info(vchiq_core_log_level,
+					"%d: ps - terminate %d<->%d",
+					state->id, service->localport,
+					service->remoteport);
+				if (vchiq_close_service_internal(
+					service, 0/*!close_recvd*/) !=
+					VCHIQ_SUCCESS)
+					request_poll(state, service,
+						VCHIQ_POLL_TERMINATE);
+			}
+			if (service_flags & BIT(VCHIQ_POLL_TXNOTIFY))
+				notify_bulks(service,
+					&service->bulk_tx,
+					1/*retry_poll*/);
+			if (service_flags & BIT(VCHIQ_POLL_RXNOTIFY))
+				notify_bulks(service,
+					&service->bulk_rx,
+					1/*retry_poll*/);
+			unlock_service(service);
+		}
+	}
+}
+
 /* Called by the slot handler thread */
 static void
 poll_services(struct vchiq_state *state)
 {
-	int group, i;
+	int group;
 
-	for (group = 0; group < BITSET_SIZE(state->unused_service); group++) {
-		u32 flags;
-
-		flags = atomic_xchg(&state->poll_services[group], 0);
-		for (i = 0; flags; i++) {
-			if (flags & BIT(i)) {
-				struct vchiq_service *service =
-					find_service_by_port(state,
-						(group<<5) + i);
-				u32 service_flags;
-
-				flags &= ~BIT(i);
-				if (!service)
-					continue;
-				service_flags =
-					atomic_xchg(&service->poll_flags, 0);
-				if (service_flags &
-					BIT(VCHIQ_POLL_REMOVE)) {
-					vchiq_log_info(vchiq_core_log_level,
-						"%d: ps - remove %d<->%d",
-						state->id, service->localport,
-						service->remoteport);
-
-					/*
-					 * Make it look like a client, because
-					 * it must be removed and not left in
-					 * the LISTENING state.
-					 */
-					service->public_fourcc =
-						VCHIQ_FOURCC_INVALID;
-
-					if (vchiq_close_service_internal(
-						service, 0/*!close_recvd*/) !=
-						VCHIQ_SUCCESS)
-						request_poll(state, service,
-							VCHIQ_POLL_REMOVE);
-				} else if (service_flags &
-					BIT(VCHIQ_POLL_TERMINATE)) {
-					vchiq_log_info(vchiq_core_log_level,
-						"%d: ps - terminate %d<->%d",
-						state->id, service->localport,
-						service->remoteport);
-					if (vchiq_close_service_internal(
-						service, 0/*!close_recvd*/) !=
-						VCHIQ_SUCCESS)
-						request_poll(state, service,
-							VCHIQ_POLL_TERMINATE);
-				}
-				if (service_flags & BIT(VCHIQ_POLL_TXNOTIFY))
-					notify_bulks(service,
-						&service->bulk_tx,
-						1/*retry_poll*/);
-				if (service_flags & BIT(VCHIQ_POLL_RXNOTIFY))
-					notify_bulks(service,
-						&service->bulk_rx,
-						1/*retry_poll*/);
-				unlock_service(service);
-			}
-		}
-	}
+	for (group = 0; group < BITSET_SIZE(state->unused_service); group++)
+		poll_services_of_group(state, group);
 }
 
 /* Called with the bulk_mutex held */