1 files changed, 36 insertions, 19 deletions

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 5d28fff46557..7642ced31436 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -601,6 +601,7 @@ reserve_space(VCHIQ_STATE_T *state, size_t space, int is_blocking)
 		}
 
 		if (tx_pos == (state->slot_queue_available * VCHIQ_SLOT_SIZE)) {
+			up(&state->slot_available_event);
 			pr_warn("%s: invalid tx_pos: %d\n", __func__, tx_pos);
 			return NULL;
 		}
@@ -619,10 +620,9 @@ reserve_space(VCHIQ_STATE_T *state, size_t space, int is_blocking)
 
 /* Called by the recycle thread. */
 static void
-process_free_queue(VCHIQ_STATE_T *state)
+process_free_queue(VCHIQ_STATE_T *state, BITSET_T *service_found, size_t length)
 {
 	VCHIQ_SHARED_STATE_T *local = state->local;
-	BITSET_T service_found[BITSET_SIZE(VCHIQ_MAX_SERVICES)];
 	int slot_queue_available;
 
 	/* Find slots which have been freed by the other side, and return them
@@ -655,7 +655,7 @@ process_free_queue(VCHIQ_STATE_T *state)
 
 		/* Initialise the bitmask for services which have used this
 		** slot */
-		BITSET_ZERO(service_found);
+		memset(service_found, 0, length);
 
 		pos = 0;
 
@@ -1197,8 +1197,8 @@ release_slot(VCHIQ_STATE_T *state, VCHIQ_SLOT_INFO_T *slot_info,
 			SLOT_INDEX_FROM_INFO(state, slot_info);
 		state->remote->slot_queue_recycle = slot_queue_recycle + 1;
 		vchiq_log_info(vchiq_core_log_level,
-			"%d: release_slot %d - recycle->%x",
-			state->id, SLOT_INDEX_FROM_INFO(state, slot_info),
+			"%d: %s %d - recycle->%x", state->id, __func__,
+			SLOT_INDEX_FROM_INFO(state, slot_info),
 			state->remote->slot_queue_recycle);
 
 		/* A write barrier is necessary, but remote_event_signal
@@ -2182,11 +2182,20 @@ recycle_func(void *v)
 {
 	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
 	VCHIQ_SHARED_STATE_T *local = state->local;
+	BITSET_T *found;
+	size_t length;
+
+	length = sizeof(*found) * BITSET_SIZE(VCHIQ_MAX_SERVICES);
+
+	found = kmalloc_array(BITSET_SIZE(VCHIQ_MAX_SERVICES), sizeof(*found),
+			      GFP_KERNEL);
+	if (!found)
+		return -ENOMEM;
 
 	while (1) {
 		remote_event_wait(state, &local->recycle);
 
-		process_free_queue(state);
+		process_free_queue(state, found, length);
 	}
 	return 0;
 }
@@ -2329,8 +2338,8 @@ vchiq_init_slots(void *mem_base, int mem_size)
 
 	if (num_slots < 4) {
 		vchiq_log_error(vchiq_core_log_level,
-			"vchiq_init_slots - insufficient memory %x bytes",
-			mem_size);
+			"%s - insufficient memory %x bytes",
+			__func__, mem_size);
 		return NULL;
 	}
 
@@ -2544,7 +2553,6 @@ vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
 		return VCHIQ_ERROR;
 	}
 	set_user_nice(state->slot_handler_thread, -19);
-	wake_up_process(state->slot_handler_thread);
 
 	snprintf(threadname, sizeof(threadname), "vchiq-recy/%d", state->id);
 	state->recycle_thread = kthread_create(&recycle_func,
@@ -2554,10 +2562,9 @@ vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
 		vchiq_loud_error_header();
 		vchiq_loud_error("couldn't create thread %s", threadname);
 		vchiq_loud_error_footer();
-		return VCHIQ_ERROR;
+		goto fail_free_handler_thread;
 	}
 	set_user_nice(state->recycle_thread, -19);
-	wake_up_process(state->recycle_thread);
 
 	snprintf(threadname, sizeof(threadname), "vchiq-sync/%d", state->id);
 	state->sync_thread = kthread_create(&sync_func,
@@ -2567,9 +2574,12 @@ vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
 		vchiq_loud_error_header();
 		vchiq_loud_error("couldn't create thread %s", threadname);
 		vchiq_loud_error_footer();
-		return VCHIQ_ERROR;
+		goto fail_free_recycle_thread;
 	}
 	set_user_nice(state->sync_thread, -20);
+
+	wake_up_process(state->slot_handler_thread);
+	wake_up_process(state->recycle_thread);
 	wake_up_process(state->sync_thread);
 
 	vchiq_states[0] = state;
@@ -2578,6 +2588,13 @@ vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
 	local->initialised = 1;
 
 	return status;
+
+fail_free_recycle_thread:
+	kthread_stop(state->recycle_thread);
+fail_free_handler_thread:
+	kthread_stop(state->slot_handler_thread);
+
+	return VCHIQ_ERROR;
 }
 
 /* Called from application thread when a client or server service is created. */
@@ -2861,9 +2878,9 @@ close_service_complete(VCHIQ_SERVICE_T *service, int failstate)
 		break;
 	default:
 		vchiq_log_error(vchiq_core_log_level,
-			"close_service_complete(%x) called in state %s",
+			"%s(%x) called in state %s", __func__,
 			service->handle, srvstate_names[service->srvstate]);
-		WARN(1, "close_service_complete in unexpected state\n");
+		WARN(1, "%s in unexpected state\n", __func__);
 		return VCHIQ_ERROR;
 	}
 
@@ -2915,9 +2932,9 @@ vchiq_close_service_internal(VCHIQ_SERVICE_T *service, int close_recvd)
 	case VCHIQ_SRVSTATE_CLOSEWAIT:
 		if (close_recvd)
 			vchiq_log_error(vchiq_core_log_level,
-				"vchiq_close_service_internal(1) called "
+				"%s(1) called "
 				"in state %s",
-				srvstate_names[service->srvstate]);
+				__func__, srvstate_names[service->srvstate]);
 		else if (is_server) {
 			if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) {
 				status = VCHIQ_ERROR;
@@ -3024,7 +3041,7 @@ vchiq_close_service_internal(VCHIQ_SERVICE_T *service, int close_recvd)
 
 	default:
 		vchiq_log_error(vchiq_core_log_level,
-			"vchiq_close_service_internal(%d) called in state %s",
+			"%s(%d) called in state %s", __func__,
 			close_recvd, srvstate_names[service->srvstate]);
 		break;
 	}
@@ -3145,8 +3162,8 @@ vchiq_pause_internal(VCHIQ_STATE_T *state)
 		break;
 	default:
 		vchiq_log_error(vchiq_core_log_level,
-			"vchiq_pause_internal in state %s\n",
-			conn_state_names[state->conn_state]);
+			"%s in state %s\n",
+			__func__, conn_state_names[state->conn_state]);
 		status = VCHIQ_ERROR;
 		VCHIQ_STATS_INC(state, error_count);
 		break;