staging: bcm2835-audio: Code refactoring of vchiq accessor codes

This is a cleanup and code refactoring in bcm2835-vchiq.c.

The major code changes are to provide local helpers for easier use of
lock / unlock, and message passing with/without response wait.  This
allows us to reduce lots of open codes.

Also, the max packet is set at opening the stream, not at each time
when the write gets called.

Signed-off-by: Takashi Iwai <tiwai@suse.de>
Tested-by: Stefan Wahren <stefan.wahren@i2se.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c

@@ -49,6 +49,7 @@ struct bcm2835_audio_instance {
 	struct mutex vchi_mutex;
 	struct bcm2835_alsa_stream *alsa_stream;
 	int result;
+	unsigned int max_packet;
 	short peer_version;
 };
 
@@ -65,16 +66,68 @@ static int bcm2835_audio_start_worker(struct bcm2835_alsa_stream *alsa_stream);
 static int bcm2835_audio_write_worker(struct bcm2835_alsa_stream *alsa_stream,
 				      unsigned int count, void *src);
 
-// Routine to send a message across a service
+static void bcm2835_audio_lock(struct bcm2835_audio_instance *instance)
-
-static int
-bcm2835_vchi_msg_queue(VCHI_SERVICE_HANDLE_T handle,
-		       void *data,
-		       unsigned int size)
 {
-	return vchi_queue_kernel_message(handle,
+	mutex_lock(&instance->vchi_mutex);
-					 data,
+	vchi_service_use(instance->vchi_handle);
-					 size);
+}
+
+static void bcm2835_audio_unlock(struct bcm2835_audio_instance *instance)
+{
+	vchi_service_release(instance->vchi_handle);
+	mutex_unlock(&instance->vchi_mutex);
+}
+
+static int bcm2835_audio_send_msg_locked(struct bcm2835_audio_instance *instance,
+					 struct vc_audio_msg *m, bool wait)
+{
+	int status;
+
+	if (wait) {
+		instance->result = -1;
+		init_completion(&instance->msg_avail_comp);
+	}
+
+	status = vchi_queue_kernel_message(instance->vchi_handle,
+					   m, sizeof(*m));
+	if (status) {
+		LOG_ERR("vchi message queue failed: %d, msg=%d\n",
+			status, m->type);
+		return -EIO;
+	}
+
+	if (wait) {
+		if (!wait_for_completion_timeout(&instance->msg_avail_comp,
+						 msecs_to_jiffies(10 * 1000))) {
+			LOG_ERR("vchi message timeout, msg=%d\n", m->type);
+			return -ETIMEDOUT;
+		} else if (instance->result) {
+			LOG_ERR("vchi message response error:%d, msg=%d\n",
+				instance->result, m->type);
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int bcm2835_audio_send_msg(struct bcm2835_audio_instance *instance,
+				  struct vc_audio_msg *m, bool wait)
+{
+	int err;
+
+	bcm2835_audio_lock(instance);
+	err = bcm2835_audio_send_msg_locked(instance, m, wait);
+	bcm2835_audio_unlock(instance);
+	return err;
+}
+
+static int bcm2835_audio_send_simple(struct bcm2835_audio_instance *instance,
+				     int type, bool wait)
+{
+	struct vc_audio_msg m = { .type = type };
+
+	return bcm2835_audio_send_msg(instance, &m, wait);
 }
 
 static const u32 BCM2835_AUDIO_WRITE_COOKIE1 = ('B' << 24 | 'C' << 16 |
@@ -283,10 +336,9 @@ static int vc_vchi_audio_deinit(struct bcm2835_audio_instance *instance)
 	int status;
 
 	mutex_lock(&instance->vchi_mutex);
-
-	/* Close all VCHI service connections */
 	vchi_service_use(instance->vchi_handle);
 
+	/* Close all VCHI service connections */
 	status = vchi_service_close(instance->vchi_handle);
 	if (status) {
 		LOG_DBG("%s: failed to close VCHI service connection (status=%d)\n",
@@ -345,12 +397,8 @@ static int bcm2835_audio_open_connection(struct bcm2835_alsa_stream *alsa_stream
 	instance = vc_vchi_audio_init(vhci_ctx->vchi_instance,
 				      vhci_ctx->vchi_connection);
 
-	if (IS_ERR(instance)) {
+	if (IS_ERR(instance))
-		LOG_ERR("%s: failed to initialize audio service\n", __func__);
-
-		/* vchi_instance is retained for use the next time. */
 		return PTR_ERR(instance);
-	}
 
 	instance->alsa_stream = alsa_stream;
 	alsa_stream->instance = instance;
@@ -361,66 +409,44 @@ static int bcm2835_audio_open_connection(struct bcm2835_alsa_stream *alsa_stream
 int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream)
 {
 	struct bcm2835_audio_instance *instance;
-	struct vc_audio_msg m;
+	int err;
-	int status;
-	int ret;
 
 	alsa_stream->my_wq = alloc_workqueue("my_queue", WQ_HIGHPRI, 1);
 	if (!alsa_stream->my_wq)
 		return -ENOMEM;
 
-	ret = bcm2835_audio_open_connection(alsa_stream);
+	err = bcm2835_audio_open_connection(alsa_stream);
-	if (ret)
+	if (err < 0)
 		goto free_wq;
 
 	instance = alsa_stream->instance;
-	LOG_DBG(" instance (%p)\n", instance);
 
-	mutex_lock(&instance->vchi_mutex);
+	err = bcm2835_audio_send_simple(instance, VC_AUDIO_MSG_TYPE_OPEN,
-	vchi_service_use(instance->vchi_handle);
+					false);
+	if (err < 0)
+		goto deinit;
 
-	m.type = VC_AUDIO_MSG_TYPE_OPEN;
+	bcm2835_audio_lock(instance);
+	vchi_get_peer_version(instance->vchi_handle, &instance->peer_version);
+	bcm2835_audio_unlock(instance);
+	if (instance->peer_version < 2 || force_bulk)
+		instance->max_packet = 0; /* bulk transfer */
+	else
+		instance->max_packet = 4000;
 
-	/* Send the message to the videocore */
+	return 0;
-	status = bcm2835_vchi_msg_queue(instance->vchi_handle,
-					&m, sizeof(m));
 
-	if (status) {
+ deinit:
-		LOG_ERR("%s: failed on vchi_msg_queue (status=%d)\n",
+	vc_vchi_audio_deinit(instance);
-			__func__, status);
+ free_wq:
-
-		ret = -1;
-		goto unlock;
-	}
-
-	ret = 0;
-
-unlock:
-	vchi_service_release(instance->vchi_handle);
-	mutex_unlock(&instance->vchi_mutex);
-
-free_wq:
-	if (ret)
 	destroy_workqueue(alsa_stream->my_wq);
-
+	return err;
-	return ret;
 }
 
 int bcm2835_audio_set_ctls(struct bcm2835_alsa_stream *alsa_stream)
 {
-	struct vc_audio_msg m;
-	struct bcm2835_audio_instance *instance = alsa_stream->instance;
 	struct bcm2835_chip *chip = alsa_stream->chip;
-	int status;
+	struct vc_audio_msg m = {};
-	int ret;
-
-	LOG_INFO(" Setting ALSA dest(%d), volume(%d)\n",
-		 chip->dest, chip->volume);
-
-	mutex_lock(&instance->vchi_mutex);
-	vchi_service_use(instance->vchi_handle);
-
-	instance->result = -1;
 
 	m.type = VC_AUDIO_MSG_TYPE_CONTROL;
 	m.u.control.dest = chip->dest;
@@ -429,289 +455,107 @@ int bcm2835_audio_set_ctls(struct bcm2835_alsa_stream *alsa_stream)
 	else
 		m.u.control.volume = alsa2chip(chip->volume);
 
-	/* Create the message available completion */
+	return bcm2835_audio_send_msg(alsa_stream->instance, &m, true);
-	init_completion(&instance->msg_avail_comp);
-
-	/* Send the message to the videocore */
-	status = bcm2835_vchi_msg_queue(instance->vchi_handle,
-					&m, sizeof(m));
-
-	if (status) {
-		LOG_ERR("%s: failed on vchi_msg_queue (status=%d)\n",
-			__func__, status);
-
-		ret = -1;
-		goto unlock;
-	}
-
-	/* We are expecting a reply from the videocore */
-	wait_for_completion(&instance->msg_avail_comp);
-
-	if (instance->result) {
-		LOG_ERR("%s: result=%d\n", __func__, instance->result);
-
-		ret = -1;
-		goto unlock;
-	}
-
-	ret = 0;
-
-unlock:
-	vchi_service_release(instance->vchi_handle);
-	mutex_unlock(&instance->vchi_mutex);
-
-	return ret;
 }
 
 int bcm2835_audio_set_params(struct bcm2835_alsa_stream *alsa_stream,
 			     unsigned int channels, unsigned int samplerate,
 			     unsigned int bps)
 {
-	struct vc_audio_msg m;
+	struct vc_audio_msg m = {
-	struct bcm2835_audio_instance *instance = alsa_stream->instance;
+		 .type = VC_AUDIO_MSG_TYPE_CONFIG,
-	int status;
+		 .u.config.channels = channels,
-	int ret;
+		 .u.config.samplerate = samplerate,
-
+		 .u.config.bps = bps,
-	LOG_INFO(" Setting ALSA channels(%d), samplerate(%d), bits-per-sample(%d)\n",
+	};
-		 channels, samplerate, bps);
+	int err;
 
 	/* resend ctls - alsa_stream may not have been open when first send */
-	ret = bcm2835_audio_set_ctls(alsa_stream);
+	err = bcm2835_audio_set_ctls(alsa_stream);
-	if (ret) {
+	if (err)
-		LOG_ERR(" Alsa controls not supported\n");
+		return err;
-		return -EINVAL;
-	}
 
-	mutex_lock(&instance->vchi_mutex);
+	return bcm2835_audio_send_msg(alsa_stream->instance, &m, true);
-	vchi_service_use(instance->vchi_handle);
-
-	instance->result = -1;
-
-	m.type = VC_AUDIO_MSG_TYPE_CONFIG;
-	m.u.config.channels = channels;
-	m.u.config.samplerate = samplerate;
-	m.u.config.bps = bps;
-
-	/* Create the message available completion */
-	init_completion(&instance->msg_avail_comp);
-
-	/* Send the message to the videocore */
-	status = bcm2835_vchi_msg_queue(instance->vchi_handle,
-					&m, sizeof(m));
-
-	if (status) {
-		LOG_ERR("%s: failed on vchi_msg_queue (status=%d)\n",
-			__func__, status);
-
-		ret = -1;
-		goto unlock;
-	}
-
-	/* We are expecting a reply from the videocore */
-	wait_for_completion(&instance->msg_avail_comp);
-
-	if (instance->result) {
-		LOG_ERR("%s: result=%d", __func__, instance->result);
-
-		ret = -1;
-		goto unlock;
-	}
-
-	ret = 0;
-
-unlock:
-	vchi_service_release(instance->vchi_handle);
-	mutex_unlock(&instance->vchi_mutex);
-
-	return ret;
 }
 
 static int bcm2835_audio_start_worker(struct bcm2835_alsa_stream *alsa_stream)
 {
-	struct vc_audio_msg m;
+	return bcm2835_audio_send_simple(alsa_stream->instance,
-	struct bcm2835_audio_instance *instance = alsa_stream->instance;
+					 VC_AUDIO_MSG_TYPE_START, false);
-	int status;
-	int ret;
-
-	mutex_lock(&instance->vchi_mutex);
-	vchi_service_use(instance->vchi_handle);
-
-	m.type = VC_AUDIO_MSG_TYPE_START;
-
-	/* Send the message to the videocore */
-	status = bcm2835_vchi_msg_queue(instance->vchi_handle,
-					&m, sizeof(m));
-
-	if (status) {
-		LOG_ERR("%s: failed on vchi_msg_queue (status=%d)\n",
-			__func__, status);
-
-		ret = -1;
-		goto unlock;
-	}
-
-	ret = 0;
-
-unlock:
-	vchi_service_release(instance->vchi_handle);
-	mutex_unlock(&instance->vchi_mutex);
-	return ret;
 }
 
 static int bcm2835_audio_stop_worker(struct bcm2835_alsa_stream *alsa_stream)
 {
-	struct vc_audio_msg m;
+	return bcm2835_audio_send_simple(alsa_stream->instance,
-	struct bcm2835_audio_instance *instance = alsa_stream->instance;
+					 VC_AUDIO_MSG_TYPE_STOP, false);
-	int status;
-	int ret;
-
-	mutex_lock(&instance->vchi_mutex);
-	vchi_service_use(instance->vchi_handle);
-
-	m.type = VC_AUDIO_MSG_TYPE_STOP;
-	m.u.stop.draining = alsa_stream->draining;
-
-	/* Send the message to the videocore */
-	status = bcm2835_vchi_msg_queue(instance->vchi_handle,
-					&m, sizeof(m));
-
-	if (status) {
-		LOG_ERR("%s: failed on vchi_msg_queue (status=%d)\n",
-			__func__, status);
-
-		ret = -1;
-		goto unlock;
-	}
-
-	ret = 0;
-
-unlock:
-	vchi_service_release(instance->vchi_handle);
-	mutex_unlock(&instance->vchi_mutex);
-	return ret;
 }
 
 int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream)
 {
-	struct vc_audio_msg m;
 	struct bcm2835_audio_instance *instance = alsa_stream->instance;
-	int status;
+	int err;
-	int ret;
 
 	my_workqueue_quit(alsa_stream);
 
-	mutex_lock(&instance->vchi_mutex);
+	err = bcm2835_audio_send_simple(alsa_stream->instance,
-	vchi_service_use(instance->vchi_handle);
+					VC_AUDIO_MSG_TYPE_CLOSE, true);
-
-	m.type = VC_AUDIO_MSG_TYPE_CLOSE;
-
-	/* Create the message available completion */
-	init_completion(&instance->msg_avail_comp);
-
-	/* Send the message to the videocore */
-	status = bcm2835_vchi_msg_queue(instance->vchi_handle,
-					&m, sizeof(m));
-
-	if (status) {
-		LOG_ERR("%s: failed on vchi_msg_queue (status=%d)\n",
-			__func__, status);
-		ret = -1;
-		goto unlock;
-	}
-
-	/* We are expecting a reply from the videocore */
-	wait_for_completion(&instance->msg_avail_comp);
-
-	if (instance->result) {
-		LOG_ERR("%s: failed result (result=%d)\n",
-			__func__, instance->result);
-
-		ret = -1;
-		goto unlock;
-	}
-
-	ret = 0;
-
-unlock:
-	vchi_service_release(instance->vchi_handle);
-	mutex_unlock(&instance->vchi_mutex);
 
 	/* Stop the audio service */
 	vc_vchi_audio_deinit(instance);
 	alsa_stream->instance = NULL;
 
-	return ret;
+	return err;
 }
 
 static int bcm2835_audio_write_worker(struct bcm2835_alsa_stream *alsa_stream,
-				      unsigned int count, void *src)
+				      unsigned int size, void *src)
 {
-	struct vc_audio_msg m;
 	struct bcm2835_audio_instance *instance = alsa_stream->instance;
-	int status;
+	struct vc_audio_msg m = {
-	int ret;
+		.type = VC_AUDIO_MSG_TYPE_WRITE,
+		.u.write.count = size,
+		.u.write.max_packet = instance->max_packet,
+		.u.write.cookie1 = BCM2835_AUDIO_WRITE_COOKIE1,
+		.u.write.cookie2 = BCM2835_AUDIO_WRITE_COOKIE2,
+	};
+	unsigned int count;
+	int err, status;
 
-	LOG_INFO(" Writing %d bytes from %p\n", count, src);
+	if (!size)
+		return 0;
 
-	mutex_lock(&instance->vchi_mutex);
+	bcm2835_audio_lock(instance);
-	vchi_service_use(instance->vchi_handle);
+	err = bcm2835_audio_send_msg_locked(instance, &m, false);
-
+	if (err < 0)
-	if (instance->peer_version == 0 &&
-	    vchi_get_peer_version(instance->vchi_handle, &instance->peer_version) == 0)
-		LOG_DBG("%s: client version %d connected\n", __func__, instance->peer_version);
-
-	m.type = VC_AUDIO_MSG_TYPE_WRITE;
-	m.u.write.count = count;
-	// old version uses bulk, new version uses control
-	m.u.write.max_packet = instance->peer_version < 2 || force_bulk ? 0 : 4000;
-	m.u.write.cookie1 = BCM2835_AUDIO_WRITE_COOKIE1;
-	m.u.write.cookie2 = BCM2835_AUDIO_WRITE_COOKIE2;
-	m.u.write.silence = src == NULL;
-
-	/* Send the message to the videocore */
-	status = bcm2835_vchi_msg_queue(instance->vchi_handle,
-					&m, sizeof(m));
-
-	if (status) {
-		LOG_ERR("%s: failed on vchi_msg_queue (status=%d)\n",
-			__func__, status);
-
-		ret = -1;
 		goto unlock;
-	}
+
-	if (!m.u.write.silence) {
+	count = size;
-		if (!m.u.write.max_packet) {
+	if (!instance->max_packet) {
 		/* Send the message to the videocore */
 		status = vchi_bulk_queue_transmit(instance->vchi_handle,
 						  src, count,
-							  0 * VCHI_FLAGS_BLOCK_UNTIL_QUEUED
+						  VCHI_FLAGS_BLOCK_UNTIL_DATA_READ,
-							  +
-							  1 * VCHI_FLAGS_BLOCK_UNTIL_DATA_READ,
 						  NULL);
 	} else {
 		while (count > 0) {
-				int bytes = min_t(int, m.u.write.max_packet, count);
+			int bytes = min(instance->max_packet, count);
 
-				status = bcm2835_vchi_msg_queue(instance->vchi_handle,
+			status = vchi_queue_kernel_message(instance->vchi_handle,
 							   src, bytes);
-				src = (char *)src + bytes;
+			src += bytes;
 			count -= bytes;
 		}
 	}
+
 	if (status) {
-			LOG_ERR("%s: failed on vchi_bulk_queue_transmit (status=%d)\n",
+		LOG_ERR("failed on %d bytes transfer (status=%d)\n",
-				__func__, status);
+			size, status);
-
+		err = -EIO;
-			ret = -1;
-			goto unlock;
 	}
-	}
-	ret = 0;
 
-unlock:
+ unlock:
-	vchi_service_release(instance->vchi_handle);
+	bcm2835_audio_unlock(instance);
-	mutex_unlock(&instance->vchi_mutex);
+	return err;
-	return ret;
 }
 
 unsigned int bcm2835_audio_retrieve_buffers(struct bcm2835_alsa_stream *alsa_stream)