X-Git-Url: http://git.openwrt.org/?a=blobdiff_plain;f=target%2Flinux%2Fbrcm2708%2Fpatches-4.19%2F950-0393-staging-bcm2835-audio-Operate-non-atomic-PCM-ops.patch;fp=target%2Flinux%2Fbrcm2708%2Fpatches-4.19%2F950-0393-staging-bcm2835-audio-Operate-non-atomic-PCM-ops.patch;h=722804ff63a05cc09fec1fa9c48a19e7eac9b9b0;hb=c2308a7e4adbb2acc8ff149f91d1ca46801c135e;hp=0000000000000000000000000000000000000000;hpb=67dcc43f3a22dc3a7ac07a7065971b426feeb043;p=openwrt%2Fstaging%2Fdedeckeh.git

diff --git a/target/linux/brcm2708/patches-4.19/950-0393-staging-bcm2835-audio-Operate-non-atomic-PCM-ops.patch b/target/linux/brcm2708/patches-4.19/950-0393-staging-bcm2835-audio-Operate-non-atomic-PCM-ops.patch
new file mode 100644
index 0000000000..722804ff63
--- /dev/null
+++ b/target/linux/brcm2708/patches-4.19/950-0393-staging-bcm2835-audio-Operate-non-atomic-PCM-ops.patch
@@ -0,0 +1,593 @@
+From 87ba8310e9f0882e85926ac1ef91333f8906b303 Mon Sep 17 00:00:00 2001
+From: Takashi Iwai <tiwai@suse.de>
+Date: Tue, 4 Sep 2018 17:58:49 +0200
+Subject: [PATCH] staging: bcm2835-audio: Operate non-atomic PCM ops
+
+commit 5c7883e5f27e829f3f3a2ba174d4a724bfd5f026 upstream.
+
+This is the most significant part in the patch series.
+
+The bcm2835-audio driver used to queue the commands to vc04 core via
+workqueue, but basically the whole accesses to vc04 core are done in
+the sleepable context, including the callback calls.  In such a case,
+rewriting the code using non-atomic PCM ops will simplify the logic a
+lot.
+
+This patch does it: all workqueue are gone and each former-work
+implementation is now directly called from PCM ops like trigger and
+write transfer.
+
+Along with it, the DMA position updater, bcm2835_playback_fifo(), was
+also rewritten to use a simpler logic.  Now it handles the XRUN and
+draining properly by calling snd_pcm_stop() conditionally.
+
+The current position is kept in atomic_t value so that it can be read
+concurrently from the pointer callback.
+
+Also, the bcm2835_audio_instance object is allocated at the beginning
+of bcm2835_audio_open().  This makes the resource management clearer.
+
+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>
+---
+ .../vc04_services/bcm2835-audio/bcm2835-pcm.c |  74 +++---
+ .../bcm2835-audio/bcm2835-vchiq.c             | 244 +++---------------
+ .../vc04_services/bcm2835-audio/bcm2835.h     |   9 +-
+ 3 files changed, 82 insertions(+), 245 deletions(-)
+
+--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
++++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
+@@ -11,7 +11,8 @@
+ /* hardware definition */
+ static const struct snd_pcm_hardware snd_bcm2835_playback_hw = {
+ 	.info = (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER |
+-	SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID),
++		 SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID |
++		 SNDRV_PCM_INFO_DRAIN_TRIGGER),
+ 	.formats = SNDRV_PCM_FMTBIT_U8 | SNDRV_PCM_FMTBIT_S16_LE,
+ 	.rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_8000_48000,
+ 	.rate_min = 8000,
+@@ -27,7 +28,8 @@ static const struct snd_pcm_hardware snd
+ 
+ static const struct snd_pcm_hardware snd_bcm2835_playback_spdif_hw = {
+ 	.info = (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER |
+-	SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID),
++		 SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID |
++		 SNDRV_PCM_INFO_DRAIN_TRIGGER),
+ 	.formats = SNDRV_PCM_FMTBIT_S16_LE,
+ 	.rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_44100 |
+ 	SNDRV_PCM_RATE_48000,
+@@ -47,42 +49,34 @@ static void snd_bcm2835_playback_free(st
+ 	kfree(runtime->private_data);
+ }
+ 
+-void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream)
++void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream,
++			   unsigned int bytes)
+ {
+-	unsigned int consumed = 0;
+-	int new_period = 0;
++	struct snd_pcm_substream *substream = alsa_stream->substream;
++	unsigned int pos;
+ 
+-	audio_info("alsa_stream=%p substream=%p\n", alsa_stream,
+-		alsa_stream ? alsa_stream->substream : 0);
++	if (!alsa_stream->period_size)
++		return;
+ 
+-	consumed = bcm2835_audio_retrieve_buffers(alsa_stream);
+-
+-	/* We get called only if playback was triggered, So, the number of buffers we retrieve in
+-	 * each iteration are the buffers that have been played out already
+-	 */
+-
+-	if (alsa_stream->period_size) {
+-		if ((alsa_stream->pos / alsa_stream->period_size) !=
+-			((alsa_stream->pos + consumed) / alsa_stream->period_size))
+-			new_period = 1;
+-	}
+-	audio_debug("updating pos cur: %d + %d max:%d period_bytes:%d, hw_ptr: %d new_period:%d\n",
+-		alsa_stream->pos,
+-		consumed,
+-		alsa_stream->buffer_size,
+-		(int) (alsa_stream->period_size * alsa_stream->substream->runtime->periods),
+-		frames_to_bytes(alsa_stream->substream->runtime, alsa_stream->substream->runtime->status->hw_ptr),
+-		new_period);
+-	if (alsa_stream->buffer_size) {
+-		alsa_stream->pos += consumed & ~(1 << 30);
+-		alsa_stream->pos %= alsa_stream->buffer_size;
++	if (bytes >= alsa_stream->buffer_size) {
++		snd_pcm_stream_lock(substream);
++		snd_pcm_stop(substream,
++			     alsa_stream->draining ?
++			     SNDRV_PCM_STATE_SETUP :
++			     SNDRV_PCM_STATE_XRUN);
++		snd_pcm_stream_unlock(substream);
++		return;
+ 	}
+ 
+-	if (alsa_stream->substream) {
+-		if (new_period)
+-			snd_pcm_period_elapsed(alsa_stream->substream);
+-	} else {
+-		audio_warning(" unexpected NULL substream\n");
++	pos = atomic_read(&alsa_stream->pos);
++	pos += bytes;
++	pos %= alsa_stream->buffer_size;
++	atomic_set(&alsa_stream->pos, pos);
++
++	alsa_stream->period_offset += bytes;
++	if (alsa_stream->period_offset >= alsa_stream->period_size) {
++		alsa_stream->period_offset %= alsa_stream->period_size;
++		snd_pcm_period_elapsed(substream);
+ 	}
+ }
+ 
+@@ -246,7 +240,8 @@ static int snd_bcm2835_pcm_prepare(struc
+ 
+ 	alsa_stream->buffer_size = snd_pcm_lib_buffer_bytes(substream);
+ 	alsa_stream->period_size = snd_pcm_lib_period_bytes(substream);
+-	alsa_stream->pos = 0;
++	atomic_set(&alsa_stream->pos, 0);
++	alsa_stream->period_offset = 0;
+ 	alsa_stream->draining = false;
+ 
+ 	return 0;
+@@ -283,7 +278,7 @@ static int snd_bcm2835_pcm_trigger(struc
+ 		return bcm2835_audio_start(alsa_stream);
+ 	case SNDRV_PCM_TRIGGER_DRAIN:
+ 		alsa_stream->draining = true;
+-		return 0;
++		return bcm2835_audio_drain(alsa_stream);
+ 	case SNDRV_PCM_TRIGGER_STOP:
+ 		return bcm2835_audio_stop(alsa_stream);
+ 	default:
+@@ -300,7 +295,7 @@ snd_bcm2835_pcm_pointer(struct snd_pcm_s
+ 
+ 	return snd_pcm_indirect_playback_pointer(substream,
+ 		&alsa_stream->pcm_indirect,
+-		alsa_stream->pos);
++		atomic_read(&alsa_stream->pos));
+ }
+ 
+ /* operators */
+@@ -338,6 +333,7 @@ int snd_bcm2835_new_pcm(struct bcm2835_c
+ 	if (err < 0)
+ 		return err;
+ 	pcm->private_data = chip;
++	pcm->nonatomic = true;
+ 	strcpy(pcm->name, "bcm2835 ALSA");
+ 	chip->pcm = pcm;
+ 	chip->dest = AUDIO_DEST_AUTO;
+@@ -367,6 +363,7 @@ int snd_bcm2835_new_spdif_pcm(struct bcm
+ 		return err;
+ 
+ 	pcm->private_data = chip;
++	pcm->nonatomic = true;
+ 	strcpy(pcm->name, "bcm2835 IEC958/HDMI");
+ 	chip->pcm_spdif = pcm;
+ 	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK,
+@@ -395,6 +392,7 @@ int snd_bcm2835_new_simple_pcm(struct bc
+ 		return err;
+ 
+ 	pcm->private_data = chip;
++	pcm->nonatomic = true;
+ 	strcpy(pcm->name, name);
+ 	chip->pcm = pcm;
+ 	chip->dest = route;
+--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
++++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
+@@ -26,10 +26,6 @@
+ 
+ /* ---- Private Constants and Types ------------------------------------------ */
+ 
+-#define BCM2835_AUDIO_STOP           0
+-#define BCM2835_AUDIO_START          1
+-#define BCM2835_AUDIO_WRITE          2
+-
+ /* Logging macros (for remapping to other logging mechanisms, i.e., printf) */
+ #ifdef AUDIO_DEBUG_ENABLE
+ #define LOG_ERR(fmt, arg...)   pr_err("%s:%d " fmt, __func__, __LINE__, ##arg)
+@@ -55,17 +51,6 @@ struct bcm2835_audio_instance {
+ 
+ static bool force_bulk;
+ 
+-/* ---- Private Variables ---------------------------------------------------- */
+-
+-/* ---- Private Function Prototypes ------------------------------------------ */
+-
+-/* ---- Private Functions ---------------------------------------------------- */
+-
+-static int bcm2835_audio_stop_worker(struct bcm2835_alsa_stream *alsa_stream);
+-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);
+-
+ static void bcm2835_audio_lock(struct bcm2835_audio_instance *instance)
+ {
+ 	mutex_lock(&instance->vchi_mutex);
+@@ -135,108 +120,6 @@ static const u32 BCM2835_AUDIO_WRITE_COO
+ static const u32 BCM2835_AUDIO_WRITE_COOKIE2 = ('D' << 24 | 'A' << 16 |
+ 						'T' << 8  | 'A');
+ 
+-struct bcm2835_audio_work {
+-	struct work_struct my_work;
+-	struct bcm2835_alsa_stream *alsa_stream;
+-	int cmd;
+-	void *src;
+-	unsigned int count;
+-};
+-
+-static void my_wq_function(struct work_struct *work)
+-{
+-	struct bcm2835_audio_work *w =
+-		container_of(work, struct bcm2835_audio_work, my_work);
+-	int ret = -9;
+-
+-	switch (w->cmd) {
+-	case BCM2835_AUDIO_START:
+-		ret = bcm2835_audio_start_worker(w->alsa_stream);
+-		break;
+-	case BCM2835_AUDIO_STOP:
+-		ret = bcm2835_audio_stop_worker(w->alsa_stream);
+-		break;
+-	case BCM2835_AUDIO_WRITE:
+-		ret = bcm2835_audio_write_worker(w->alsa_stream, w->count,
+-						 w->src);
+-		break;
+-	default:
+-		LOG_ERR(" Unexpected work: %p:%d\n", w->alsa_stream, w->cmd);
+-		break;
+-	}
+-	kfree((void *)work);
+-}
+-
+-int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream)
+-{
+-	struct bcm2835_audio_work *work;
+-
+-	work = kmalloc(sizeof(*work), GFP_ATOMIC);
+-	/*--- Queue some work (item 1) ---*/
+-	if (!work) {
+-		LOG_ERR(" .. Error: NULL work kmalloc\n");
+-		return -ENOMEM;
+-	}
+-	INIT_WORK(&work->my_work, my_wq_function);
+-	work->alsa_stream = alsa_stream;
+-	work->cmd = BCM2835_AUDIO_START;
+-	if (!queue_work(alsa_stream->my_wq, &work->my_work)) {
+-		kfree(work);
+-		return -EBUSY;
+-	}
+-	return 0;
+-}
+-
+-int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream)
+-{
+-	struct bcm2835_audio_work *work;
+-
+-	work = kmalloc(sizeof(*work), GFP_ATOMIC);
+-	/*--- Queue some work (item 1) ---*/
+-	if (!work) {
+-		LOG_ERR(" .. Error: NULL work kmalloc\n");
+-		return -ENOMEM;
+-	}
+-	INIT_WORK(&work->my_work, my_wq_function);
+-	work->alsa_stream = alsa_stream;
+-	work->cmd = BCM2835_AUDIO_STOP;
+-	if (!queue_work(alsa_stream->my_wq, &work->my_work)) {
+-		kfree(work);
+-		return -EBUSY;
+-	}
+-	return 0;
+-}
+-
+-int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream,
+-			unsigned int count, void *src)
+-{
+-	struct bcm2835_audio_work *work;
+-
+-	work = kmalloc(sizeof(*work), GFP_ATOMIC);
+-	/*--- Queue some work (item 1) ---*/
+-	if (!work) {
+-		LOG_ERR(" .. Error: NULL work kmalloc\n");
+-		return -ENOMEM;
+-	}
+-	INIT_WORK(&work->my_work, my_wq_function);
+-	work->alsa_stream = alsa_stream;
+-	work->cmd = BCM2835_AUDIO_WRITE;
+-	work->src = src;
+-	work->count = count;
+-	if (!queue_work(alsa_stream->my_wq, &work->my_work)) {
+-		kfree(work);
+-		return -EBUSY;
+-	}
+-	return 0;
+-}
+-
+-static void my_workqueue_quit(struct bcm2835_alsa_stream *alsa_stream)
+-{
+-	flush_workqueue(alsa_stream->my_wq);
+-	destroy_workqueue(alsa_stream->my_wq);
+-	alsa_stream->my_wq = NULL;
+-}
+-
+ static void audio_vchi_callback(void *param,
+ 				const VCHI_CALLBACK_REASON_T reason,
+ 				void *msg_handle)
+@@ -249,47 +132,27 @@ static void audio_vchi_callback(void *pa
+ 	if (reason != VCHI_CALLBACK_MSG_AVAILABLE)
+ 		return;
+ 
+-	if (!instance) {
+-		LOG_ERR(" .. instance is null\n");
+-		BUG();
+-		return;
+-	}
+-	if (!instance->vchi_handle) {
+-		LOG_ERR(" .. instance->vchi_handle is null\n");
+-		BUG();
+-		return;
+-	}
+ 	status = vchi_msg_dequeue(instance->vchi_handle,
+ 				  &m, sizeof(m), &msg_len, VCHI_FLAGS_NONE);
+ 	if (m.type == VC_AUDIO_MSG_TYPE_RESULT) {
+-		LOG_DBG(" .. instance=%p, m.type=VC_AUDIO_MSG_TYPE_RESULT, success=%d\n",
+-			instance, m.u.result.success);
+ 		instance->result = m.u.result.success;
+ 		complete(&instance->msg_avail_comp);
+ 	} else if (m.type == VC_AUDIO_MSG_TYPE_COMPLETE) {
+-		struct bcm2835_alsa_stream *alsa_stream = instance->alsa_stream;
+-
+-		LOG_DBG(" .. instance=%p, m.type=VC_AUDIO_MSG_TYPE_COMPLETE, complete=%d\n",
+-			instance, m.u.complete.count);
+ 		if (m.u.complete.cookie1 != BCM2835_AUDIO_WRITE_COOKIE1 ||
+ 		    m.u.complete.cookie2 != BCM2835_AUDIO_WRITE_COOKIE2)
+-			LOG_ERR(" .. response is corrupt\n");
+-		else if (alsa_stream) {
+-			atomic_add(m.u.complete.count,
+-				   &alsa_stream->retrieved);
+-			bcm2835_playback_fifo(alsa_stream);
+-		} else {
+-			LOG_ERR(" .. unexpected alsa_stream=%p\n",
+-				alsa_stream);
+-		}
++			LOG_ERR("invalid cookie\n");
++		else
++			bcm2835_playback_fifo(instance->alsa_stream,
++					      m.u.complete.count);
+ 	} else {
+-		LOG_ERR(" .. unexpected m.type=%d\n", m.type);
++		LOG_ERR("unexpected callback type=%d\n", m.type);
+ 	}
+ }
+ 
+-static struct bcm2835_audio_instance *
++static int
+ vc_vchi_audio_init(VCHI_INSTANCE_T vchi_instance,
+-		   VCHI_CONNECTION_T *vchi_connection)
++		   VCHI_CONNECTION_T *vchi_connection,
++		   struct bcm2835_audio_instance *instance)
+ {
+ 	SERVICE_CREATION_T params = {
+ 		.version		= VCHI_VERSION_EX(VC_AUDIOSERV_VER, VC_AUDIOSERV_MIN_VER),
+@@ -298,23 +161,14 @@ vc_vchi_audio_init(VCHI_INSTANCE_T vchi_
+ 		.rx_fifo_size		= 0,
+ 		.tx_fifo_size		= 0,
+ 		.callback		= audio_vchi_callback,
++		.callback_param		= instance,
+ 		.want_unaligned_bulk_rx = 1, //TODO: remove VCOS_FALSE
+ 		.want_unaligned_bulk_tx = 1, //TODO: remove VCOS_FALSE
+ 		.want_crc		= 0
+ 	};
+-	struct bcm2835_audio_instance *instance;
+ 	int status;
+ 
+-	/* Allocate memory for this instance */
+-	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+-	if (!instance)
+-		return ERR_PTR(-ENOMEM);
+-
+-	/* Create a lock for exclusive, serialized VCHI connection access */
+-	mutex_init(&instance->vchi_mutex);
+ 	/* Open the VCHI service connections */
+-	params.callback_param = instance,
+-
+ 	status = vchi_service_open(vchi_instance, &params,
+ 				   &instance->vchi_handle);
+ 
+@@ -322,16 +176,16 @@ vc_vchi_audio_init(VCHI_INSTANCE_T vchi_
+ 		LOG_ERR("%s: failed to open VCHI service connection (status=%d)\n",
+ 			__func__, status);
+ 		kfree(instance);
+-		return ERR_PTR(-EPERM);
++		return -EPERM;
+ 	}
+ 
+ 	/* Finished with the service for now */
+ 	vchi_service_release(instance->vchi_handle);
+ 
+-	return instance;
++	return 0;
+ }
+ 
+-static int vc_vchi_audio_deinit(struct bcm2835_audio_instance *instance)
++static void vc_vchi_audio_deinit(struct bcm2835_audio_instance *instance)
+ {
+ 	int status;
+ 
+@@ -346,10 +200,6 @@ static int vc_vchi_audio_deinit(struct b
+ 	}
+ 
+ 	mutex_unlock(&instance->vchi_mutex);
+-
+-	kfree(instance);
+-
+-	return 0;
+ }
+ 
+ int bcm2835_new_vchi_ctx(struct bcm2835_vchi_ctx *vchi_ctx)
+@@ -387,39 +237,25 @@ void bcm2835_free_vchi_ctx(struct bcm283
+ 	vchi_ctx->vchi_instance = NULL;
+ }
+ 
+-static int bcm2835_audio_open_connection(struct bcm2835_alsa_stream *alsa_stream)
+-{
+-	struct bcm2835_audio_instance *instance =
+-		(struct bcm2835_audio_instance *)alsa_stream->instance;
+-	struct bcm2835_vchi_ctx *vhci_ctx = alsa_stream->chip->vchi_ctx;
+-
+-	/* Initialize an instance of the audio service */
+-	instance = vc_vchi_audio_init(vhci_ctx->vchi_instance,
+-				      vhci_ctx->vchi_connection);
+-
+-	if (IS_ERR(instance))
+-		return PTR_ERR(instance);
+-
+-	instance->alsa_stream = alsa_stream;
+-	alsa_stream->instance = instance;
+-
+-	return 0;
+-}
+-
+ int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream)
+ {
++	struct bcm2835_vchi_ctx *vchi_ctx = alsa_stream->chip->vchi_ctx;
+ 	struct bcm2835_audio_instance *instance;
+ 	int err;
+ 
+-	alsa_stream->my_wq = alloc_workqueue("my_queue", WQ_HIGHPRI, 1);
+-	if (!alsa_stream->my_wq)
++	/* Allocate memory for this instance */
++	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
++	if (!instance)
+ 		return -ENOMEM;
++	mutex_init(&instance->vchi_mutex);
++	instance->alsa_stream = alsa_stream;
++	alsa_stream->instance = instance;
+ 
+-	err = bcm2835_audio_open_connection(alsa_stream);
++	err = vc_vchi_audio_init(vchi_ctx->vchi_instance,
++				 vchi_ctx->vchi_connection,
++				 instance);
+ 	if (err < 0)
+-		goto free_wq;
+-
+-	instance = alsa_stream->instance;
++		goto free_instance;
+ 
+ 	err = bcm2835_audio_send_simple(instance, VC_AUDIO_MSG_TYPE_OPEN,
+ 					false);
+@@ -438,8 +274,9 @@ int bcm2835_audio_open(struct bcm2835_al
+ 
+  deinit:
+ 	vc_vchi_audio_deinit(instance);
+- free_wq:
+-	destroy_workqueue(alsa_stream->my_wq);
++ free_instance:
++	alsa_stream->instance = NULL;
++	kfree(instance);
+ 	return err;
+ }
+ 
+@@ -478,37 +315,46 @@ int bcm2835_audio_set_params(struct bcm2
+ 	return bcm2835_audio_send_msg(alsa_stream->instance, &m, true);
+ }
+ 
+-static int bcm2835_audio_start_worker(struct bcm2835_alsa_stream *alsa_stream)
++int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream)
+ {
+ 	return bcm2835_audio_send_simple(alsa_stream->instance,
+ 					 VC_AUDIO_MSG_TYPE_START, false);
+ }
+ 
+-static int bcm2835_audio_stop_worker(struct bcm2835_alsa_stream *alsa_stream)
++int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream)
+ {
+ 	return bcm2835_audio_send_simple(alsa_stream->instance,
+ 					 VC_AUDIO_MSG_TYPE_STOP, false);
+ }
+ 
++int bcm2835_audio_drain(struct bcm2835_alsa_stream *alsa_stream)
++{
++	struct vc_audio_msg m = {
++		.type = VC_AUDIO_MSG_TYPE_STOP,
++		.u.stop.draining = 1,
++	};
++
++	return bcm2835_audio_send_msg(alsa_stream->instance, &m, false);
++}
++
+ int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream)
+ {
+ 	struct bcm2835_audio_instance *instance = alsa_stream->instance;
+ 	int err;
+ 
+-	my_workqueue_quit(alsa_stream);
+-
+ 	err = bcm2835_audio_send_simple(alsa_stream->instance,
+ 					VC_AUDIO_MSG_TYPE_CLOSE, true);
+ 
+ 	/* Stop the audio service */
+ 	vc_vchi_audio_deinit(instance);
+ 	alsa_stream->instance = NULL;
++	kfree(instance);
+ 
+ 	return err;
+ }
+ 
+-static int bcm2835_audio_write_worker(struct bcm2835_alsa_stream *alsa_stream,
+-				      unsigned int size, void *src)
++int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream,
++			unsigned int size, void *src)
+ {
+ 	struct bcm2835_audio_instance *instance = alsa_stream->instance;
+ 	struct vc_audio_msg m = {
+@@ -558,13 +404,5 @@ static int bcm2835_audio_write_worker(st
+ 	return err;
+ }
+ 
+-unsigned int bcm2835_audio_retrieve_buffers(struct bcm2835_alsa_stream *alsa_stream)
+-{
+-	unsigned int count = atomic_read(&alsa_stream->retrieved);
+-
+-	atomic_sub(count, &alsa_stream->retrieved);
+-	return count;
+-}
+-
+ module_param(force_bulk, bool, 0444);
+ MODULE_PARM_DESC(force_bulk, "Force use of vchiq bulk for audio");
+--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
++++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
+@@ -121,13 +121,12 @@ struct bcm2835_alsa_stream {
+ 
+ 	int draining;
+ 
+-	unsigned int pos;
++	atomic_t pos;
++	unsigned int period_offset;
+ 	unsigned int buffer_size;
+ 	unsigned int period_size;
+ 
+-	atomic_t retrieved;
+ 	struct bcm2835_audio_instance *instance;
+-	struct workqueue_struct *my_wq;
+ 	int idx;
+ };
+ 
+@@ -152,11 +151,13 @@ int bcm2835_audio_set_params(struct bcm2
+ 			     unsigned int bps);
+ int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream);
+ int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream);
++int bcm2835_audio_drain(struct bcm2835_alsa_stream *alsa_stream);
+ int bcm2835_audio_set_ctls(struct bcm2835_alsa_stream *alsa_stream);
+ int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream,
+ 			unsigned int count,
+ 			void *src);
+-void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream);
++void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream,
++			   unsigned int size);
+ unsigned int bcm2835_audio_retrieve_buffers(struct bcm2835_alsa_stream *alsa_stream);
+ 
+ #endif /* __SOUND_ARM_BCM2835_H */