bcm2835_audio.c

/*-
 * Copyright (c) 2015 Oleksandr Tymoshenko <[email protected]>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#ifdef HAVE_KERNEL_OPTION_HEADERS
#include "opt_snd.h"
#endif

#include <dev/sound/pcm/sound.h>

#include "mixer_if.h"

#include "interface/compat/vchi_bsd.h"
#include "interface/vchi/vchi.h"
#include "interface/vchiq_arm/vchiq.h"

#include "vc_vchi_audioserv_defs.h"

/* Audio destination */
#define	DEST_AUTO		0
#define	DEST_HEADPHONES		1
#define	DEST_HDMI		2

/* Playback state */
#define	PLAYBACK_IDLE		0
#define	PLAYBACK_PLAYING	1
#define	PLAYBACK_STOPPING	2

/* Worker thread state */
#define	WORKER_RUNNING		0
#define	WORKER_STOPPING		1
#define	WORKER_STOPPED		2

/*
 * Worker thread flags, set to 1 in flags_pending
 * when driver requests one or another operation
 * from worker. Cleared to 0 once worker performs
 * the operations.
 */
#define	AUDIO_PARAMS		(1 << 0)
#define	AUDIO_PLAY		(1 << 1)
#define	AUDIO_STOP		(1 << 2)

#define	VCHIQ_AUDIO_PACKET_SIZE	4000
#define	VCHIQ_AUDIO_BUFFER_SIZE	10*VCHIQ_AUDIO_PACKET_SIZE

#define	VCHIQ_AUDIO_MAX_VOLUME	
/* volume in terms of 0.01dB */
#define VCHIQ_AUDIO_VOLUME_MIN -10239
#define VCHIQ_AUDIO_VOLUME(db100) (uint32_t)(-((db100) << 8)/100)

/* dB levels with 5% volume step */
static int db_levels[] = {
	VCHIQ_AUDIO_VOLUME_MIN, -4605, -3794, -3218, -2772,
	-2407, -2099, -1832, -1597, -1386,
	-1195, -1021, -861, -713, -575,
	-446, -325, -210, -102, 0,
};

static uint32_t bcm2835_audio_playfmt[] = {
	SND_FORMAT(AFMT_U8, 1, 0),
	SND_FORMAT(AFMT_U8, 2, 0),
	SND_FORMAT(AFMT_S8, 1, 0),
	SND_FORMAT(AFMT_S8, 2, 0),
	SND_FORMAT(AFMT_S16_LE, 1, 0),
	SND_FORMAT(AFMT_S16_LE, 2, 0),
	SND_FORMAT(AFMT_U16_LE, 1, 0),
	SND_FORMAT(AFMT_U16_LE, 2, 0),
	0
};

static struct pcmchan_caps bcm2835_audio_playcaps = {8000, 48000, bcm2835_audio_playfmt, 0};

struct bcm2835_audio_info;

struct bcm2835_audio_chinfo {
	struct bcm2835_audio_info *parent;
	struct pcm_channel *channel;
	struct snd_dbuf *buffer;
	uint32_t fmt, spd, blksz;

	/* Pointer to first unsubmitted sample */
	uint32_t unsubmittedptr;
	/*
	 * Number of bytes in "submitted but not played"
	 * pseudo-buffer
	 */
	int available_space;
	int playback_state;
	uint64_t callbacks;
	uint64_t submitted_samples;
	uint64_t retrieved_samples;
	uint64_t underruns;
	int starved;
	struct bcm_log_vars {
		unsigned int bsize ;
		int slept_for_lack_of_space ;
	} log_vars;
#define DEFAULT_LOG_VALUES \
	((struct bcm_log_vars) { .bsize = 0 , .slept_for_lack_of_space = 0 })
};

struct bcm2835_audio_info {
	device_t dev;
	unsigned int bufsz;
    	struct bcm2835_audio_chinfo pch;
	uint32_t dest, volume;
	struct intr_config_hook intr_hook;

	/* VCHI data */
	VCHI_INSTANCE_T vchi_instance;
	VCHI_CONNECTION_T *vchi_connection;
	VCHI_SERVICE_HANDLE_T vchi_handle;

	struct mtx lock;
	struct cv worker_cv;

	uint32_t flags_pending;

	int verbose_trace;
	/* Worker thread state */
	int worker_state;
};

#define BCM2835_AUDIO_LOCK(sc)		mtx_lock(&(sc)->lock)
#define BCM2835_AUDIO_LOCKED(sc)	mtx_assert(&(sc)->lock, MA_OWNED)
#define BCM2835_AUDIO_UNLOCK(sc)	mtx_unlock(&(sc)->lock)

#define BCM2835_LOG_ERROR(sc,...)				\
	do {							\
		device_printf((sc)->dev, __VA_ARGS__);		\
	} while(0)

#define BCM2835_LOG_INFO(sc,...)				\
	do {							\
		if (sc->verbose_trace > 0)			\
			device_printf((sc)->dev, __VA_ARGS__);	\
	} while(0)

#define BCM2835_LOG_WARN(sc,...) \
	do {							\
		if (sc->verbose_trace > 1)			\
			device_printf((sc)->dev, __VA_ARGS__);	\
	} while(0)

#define BCM2835_LOG_TRACE(sc,...)				\
	do {							\
		if(sc->verbose_trace > 2)			\
			device_printf((sc)->dev, __VA_ARGS__);	\
	} while(0)

/* Useful for circular buffer calcs */
#define MOD_DIFF(front,rear,mod) (((mod) + (front) - (rear)) % (mod))


static const char *
dest_description(uint32_t dest)
{
	switch (dest) {
		case DEST_AUTO:
			return "AUTO";
			break;

		case DEST_HEADPHONES:
			return "HEADPHONES";
			break;

		case DEST_HDMI:
			return "HDMI";
			break;
		default:
			return "UNKNOWN";
			break;
	}
}

static void
bcm2835_worker_update_params(struct bcm2835_audio_info *sc)
{

	BCM2835_AUDIO_LOCKED(sc);

	sc->flags_pending |= AUDIO_PARAMS;
	cv_signal(&sc->worker_cv);
}

static void
bcm2835_worker_play_start(struct bcm2835_audio_info *sc)
{
	BCM2835_AUDIO_LOCK(sc);
	sc->flags_pending &= ~(AUDIO_STOP);
	sc->flags_pending |= AUDIO_PLAY;
	cv_signal(&sc->worker_cv);
	BCM2835_AUDIO_UNLOCK(sc);
}

static void
bcm2835_worker_play_stop(struct bcm2835_audio_info *sc)
{
	BCM2835_AUDIO_LOCK(sc);
	sc->flags_pending &= ~(AUDIO_PLAY);
	sc->flags_pending |= AUDIO_STOP;
	cv_signal(&sc->worker_cv);
	BCM2835_AUDIO_UNLOCK(sc);
}

static void
bcm2835_audio_callback(void *param, const VCHI_CALLBACK_REASON_T reason, void *msg_handle)
{
	struct bcm2835_audio_info *sc = (struct bcm2835_audio_info *)param;
	int32_t status;
	uint32_t msg_len;
	VC_AUDIO_MSG_T m;

	if (reason != VCHI_CALLBACK_MSG_AVAILABLE)
		return;

	status = vchi_msg_dequeue(sc->vchi_handle,
	    &m, sizeof m, &msg_len, VCHI_FLAGS_NONE);
	if (status != 0)
		return;
	if (m.type == VC_AUDIO_MSG_TYPE_RESULT) {
		if (m.u.result.success) {
			device_printf(sc->dev,
			    "msg type %08x failed\n",
			    m.type);
		}
	} else if (m.type == VC_AUDIO_MSG_TYPE_COMPLETE) {
		unsigned int signaled = 0;
		struct bcm2835_audio_chinfo *ch ;
#if defined(__aarch64__)
		ch = (void *) ((((size_t)m.u.complete.callback) << 32)
		    | ((size_t)m.u.complete.cookie));
#else
		ch = (void *) (m.u.complete.cookie);
#endif

		int count = m.u.complete.count & 0xffff;
		int perr = (m.u.complete.count & (1U << 30)) != 0;

		BCM2835_LOG_TRACE(sc, "in:: count:0x%x perr:%d\n",
		    m.u.complete.count, perr);

		ch->callbacks++;
		if (perr)
			ch->underruns++;

		BCM2835_AUDIO_LOCK(sc);
		if (ch->playback_state != PLAYBACK_IDLE) {
			/* Prevent LOR */
			BCM2835_AUDIO_UNLOCK(sc);
			chn_intr(sc->pch.channel);
			BCM2835_AUDIO_LOCK(sc);
		}
		/* We should check again, state might have changed */
		if (ch->playback_state != PLAYBACK_IDLE) {
			if (!perr) {
				if ((ch->available_space + count)> VCHIQ_AUDIO_BUFFER_SIZE) {
					device_printf(sc->dev, "inconsistent data in callback:\n");
					device_printf(sc->dev, "available_space == %d, count = %d, perr=%d\n",
					    ch->available_space, count, perr);
					device_printf(sc->dev,
					    "retrieved_samples = %ju, submitted_samples = %ju\n",
					    (uintmax_t)ch->retrieved_samples,
					    (uintmax_t)ch->submitted_samples);
				}
			}
			ch->available_space += count;
			ch->retrieved_samples += count;
			/*
			 *  XXXMDC
			 *  Experimental: if VC says it's empty, believe it
			 *  Has to come after the usual adjustments
			 */
			if(perr){
				ch->available_space = VCHIQ_AUDIO_BUFFER_SIZE;
				perr = ch->retrieved_samples; // shd be != 0
			}

			if ((ch->available_space >= 1*VCHIQ_AUDIO_PACKET_SIZE)){
					cv_signal(&sc->worker_cv);
				signaled = 1;
			}
		}
		BCM2835_AUDIO_UNLOCK(sc);
		if(perr){
			BCM2835_LOG_WARN(sc,
			    "VC starved; reported %u for a total of %u\n"
			    "worker %s\n", count, perr,
			    (signaled ? "signaled": "not signaled"));
		}
	} else
		BCM2835_LOG_WARN(sc, "%s: unknown m.type: %d\n", __func__,
		    m.type);
}

/* VCHIQ stuff */
static void
bcm2835_audio_init(struct bcm2835_audio_info *sc)
{
	int status;

	/* Initialize and create a VCHI connection */
	status = vchi_initialise(&sc->vchi_instance);
	if (status != 0) {
		BCM2835_LOG_ERROR(sc, "vchi_initialise failed: %d\n", status);
		return;
	}

	status = vchi_connect(NULL, 0, sc->vchi_instance);
	if (status != 0) {
		BCM2835_LOG_ERROR(sc, "vchi_connect failed: %d\n", status);
		return;
	}

	SERVICE_CREATION_T params = {
	    VCHI_VERSION_EX(VC_AUDIOSERV_VER, VC_AUDIOSERV_MIN_VER),
	    VC_AUDIO_SERVER_NAME,   /* 4cc service code */
	    sc->vchi_connection,    /* passed in fn pointers */
	    0,  /* rx fifo size */
	    0,  /* tx fifo size */
	    bcm2835_audio_callback,    /* service callback */
	    sc,   /* service callback parameter */
	    1,
	    1,
	    0   /* want crc check on bulk transfers */
	};

	status = vchi_service_open(sc->vchi_instance, &params,
	    &sc->vchi_handle);

	if (status != 0)
		sc->vchi_handle = VCHIQ_SERVICE_HANDLE_INVALID;
}

static void
bcm2835_audio_release(struct bcm2835_audio_info *sc)
{
	int success;

	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
		success = vchi_service_close(sc->vchi_handle);
		if (success != 0)
			BCM2835_LOG_ERROR(sc, "vchi_service_close failed: %d\n",
			    success);
		vchi_service_release(sc->vchi_handle);
		sc->vchi_handle = VCHIQ_SERVICE_HANDLE_INVALID;
	}

	vchi_disconnect(sc->vchi_instance);
}

static void
bcm2835_audio_reset_channel(struct bcm2835_audio_chinfo *ch)
{

	ch->available_space = VCHIQ_AUDIO_BUFFER_SIZE;
	ch->unsubmittedptr = 0;
	sndbuf_reset(ch->buffer);
}

static void
bcm2835_audio_start(struct bcm2835_audio_chinfo *ch)
{
	VC_AUDIO_MSG_T m;
	int ret;
	struct bcm2835_audio_info *sc = ch->parent;

	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
		m.type = VC_AUDIO_MSG_TYPE_START;
		ret = vchi_msg_queue(sc->vchi_handle,
		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);

		if (ret != 0)
			BCM2835_LOG_ERROR(sc,
			    "%s: vchi_msg_queue failed (err %d)\n", __func__,
			    ret);
	}
}

static void
bcm2835_audio_stop(struct bcm2835_audio_chinfo *ch)
{
	VC_AUDIO_MSG_T m;
	int ret;
	struct bcm2835_audio_info *sc = ch->parent;

	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
		m.type = VC_AUDIO_MSG_TYPE_STOP;
		m.u.stop.draining = 0;

		BCM2835_LOG_INFO(sc,"sending stop\n");
		ret = vchi_msg_queue(sc->vchi_handle,
		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);

		if (ret != 0)
			BCM2835_LOG_ERROR(sc,
			    "%s: vchi_msg_queue failed (err %d)\n", __func__,
			    ret);
	}
}

static void
bcm2835_audio_open(struct bcm2835_audio_info *sc)
{
	VC_AUDIO_MSG_T m;
	int ret;

	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
		m.type = VC_AUDIO_MSG_TYPE_OPEN;
		ret = vchi_msg_queue(sc->vchi_handle,
		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);

		if (ret != 0)
			BCM2835_LOG_ERROR(sc,
			    "%s: vchi_msg_queue failed (err %d)\n", __func__,
			    ret);
	}
}

static void
bcm2835_audio_update_controls(struct bcm2835_audio_info *sc, uint32_t volume, uint32_t dest)
{
	VC_AUDIO_MSG_T m;
	int ret, db;

	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
		m.type = VC_AUDIO_MSG_TYPE_CONTROL;
		m.u.control.dest = dest;
		if (volume > 99)
			volume = 99;
		db = db_levels[volume/5];
		m.u.control.volume = VCHIQ_AUDIO_VOLUME(db);

		ret = vchi_msg_queue(sc->vchi_handle,
		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);

		if (ret != 0)
			BCM2835_LOG_ERROR(sc,
			    "%s: vchi_msg_queue failed (err %d)\n", __func__,
			    ret);
	}
}

static void
bcm2835_audio_update_params(struct bcm2835_audio_info *sc, uint32_t fmt, uint32_t speed)
{
	VC_AUDIO_MSG_T m;
	int ret;

	if (sc->vchi_handle != VCHIQ_SERVICE_HANDLE_INVALID) {
		m.type = VC_AUDIO_MSG_TYPE_CONFIG;
		m.u.config.channels = AFMT_CHANNEL(fmt);
		m.u.config.samplerate = speed;
		m.u.config.bps = AFMT_BIT(fmt);

		ret = vchi_msg_queue(sc->vchi_handle,
		    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);

		if (ret != 0)
			BCM2835_LOG_ERROR(sc,
			    "%s: vchi_msg_queue failed (err %d)\n", __func__,
			    ret);
	}
}

static bool
bcm2835_audio_buffer_should_sleep(struct bcm2835_audio_chinfo *ch)
{

	ch->log_vars.slept_for_lack_of_space = 0;
	if (ch->playback_state != PLAYBACK_PLAYING)
		return (true);

	/* Not enough data */
	/* XXXMDC Take unsubmitted stuff into account */
	if (sndbuf_getready(ch->buffer)
			- MOD_DIFF(
				ch->unsubmittedptr,
				sndbuf_getreadyptr(ch->buffer),
				ch->buffer->bufsize
			) < VCHIQ_AUDIO_PACKET_SIZE) {
		ch->starved++;
		return (true);
	}

	/* Not enough free space */
	if (ch->available_space < VCHIQ_AUDIO_PACKET_SIZE) {
		ch->log_vars.slept_for_lack_of_space = 1;
		return (true);
	}

	return (false);
}

static void
bcm2835_audio_write_samples(struct bcm2835_audio_chinfo *ch, void *buf, uint32_t count)
{
	struct bcm2835_audio_info *sc = ch->parent;
	VC_AUDIO_MSG_T m;
	int ret;

	if (sc->vchi_handle == VCHIQ_SERVICE_HANDLE_INVALID) {
		return;
	}

	m.type = VC_AUDIO_MSG_TYPE_WRITE;
	m.u.write.count = count;
	m.u.write.max_packet = VCHIQ_AUDIO_PACKET_SIZE;
#if defined(__aarch64__)
	m.u.write.callback = (uint32_t)(((size_t) ch) >> 32) & 0xffffffff;
	m.u.write.cookie = (uint32_t)(((size_t) ch) & 0xffffffff);
#else
	m.u.write.callback = (uint32_t) NULL;
	m.u.write.cookie = (uint32_t) ch;
#endif
	m.u.write.silence = 0;

	ret = vchi_msg_queue(sc->vchi_handle,
	    &m, sizeof m, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);

	if (ret != 0)
		BCM2835_LOG_ERROR(sc, "%s: vchi_msg_queue failed (err %d)\n",
		    __func__, ret);

	while (count > 0) {
		int bytes = MIN((int)m.u.write.max_packet, (int)count);
		ret = vchi_msg_queue(sc->vchi_handle,
		    buf, bytes, VCHI_FLAGS_BLOCK_UNTIL_QUEUED, NULL);
		if (ret != 0)
			BCM2835_LOG_ERROR(sc, "%s: vchi_msg_queue failed: %d\n",
			    __func__, ret);
		buf = (char *)buf + bytes;
		count -= bytes;
	}
}

static void
bcm2835_audio_worker(void *data)
{
	struct bcm2835_audio_info *sc = (struct bcm2835_audio_info *)data;
	struct bcm2835_audio_chinfo *ch = &sc->pch;
	uint32_t speed, format;
	uint32_t volume, dest;
	uint32_t flags;
	uint32_t count, size, readyptr;
	uint8_t *buf;

	ch->playback_state = PLAYBACK_IDLE;

	while (1) {
		if (sc->worker_state != WORKER_RUNNING)
			break;

		BCM2835_AUDIO_LOCK(sc);
		/*
		 * wait until there are flags set or buffer is ready
		 * to consume more samples
		 */
		while ((sc->flags_pending == 0) &&
		    bcm2835_audio_buffer_should_sleep(ch)) {
			cv_wait_sig(&sc->worker_cv, &sc->lock);
			if ((sc->flags_pending == 0) &&
			    (ch->log_vars.slept_for_lack_of_space)) {
				BCM2835_LOG_TRACE(sc,
				    "slept for lack of space\n");
			}
		}
		flags = sc->flags_pending;
		/* Clear pending flags */
		sc->flags_pending = 0;
		BCM2835_AUDIO_UNLOCK(sc);

		/* Requested to change parameters */
		if (flags & AUDIO_PARAMS) {
			BCM2835_AUDIO_LOCK(sc);
			speed = ch->spd;
			format = ch->fmt;
			volume = sc->volume;
			dest = sc->dest;
			BCM2835_AUDIO_UNLOCK(sc);
			if (ch->playback_state == PLAYBACK_IDLE)
				bcm2835_audio_update_params(sc, format, speed);
			bcm2835_audio_update_controls(sc, volume, dest);
		}

		/* Requested to stop playback */
		if ((flags & AUDIO_STOP) &&
		    (ch->playback_state == PLAYBACK_PLAYING)) {
			bcm2835_audio_stop(ch);
			BCM2835_AUDIO_LOCK(sc);
			bcm2835_audio_reset_channel(&sc->pch);
			ch->playback_state = PLAYBACK_IDLE;
			long sub_total = ch->submitted_samples;
			long retd = ch->retrieved_samples;
			BCM2835_AUDIO_UNLOCK(sc);
			BCM2835_LOG_INFO(sc,
			    "stopped audio. submitted a total of %lu "
			    "having been acked %lu\n", sub_total, retd);
			continue;
		}

		/* Requested to start playback */
		if ((flags & AUDIO_PLAY) &&
		    (ch->playback_state == PLAYBACK_IDLE)) {
			BCM2835_LOG_INFO(sc, "starting audio\n");
			unsigned int bsize = ch->buffer->bufsize;
			BCM2835_AUDIO_LOCK(sc);
			ch->playback_state = PLAYBACK_PLAYING;
			ch->log_vars.bsize = bsize;
			BCM2835_AUDIO_UNLOCK(sc);
			BCM2835_LOG_INFO(sc, "buffer size is %u\n", bsize);
			bcm2835_audio_start(ch);
		}

		if (ch->playback_state == PLAYBACK_IDLE)
			continue;

		if (sndbuf_getready(ch->buffer) == 0)
			continue;

		uint32_t i_count;

		/* XXXMDC Take unsubmitted stuff into account */
		count = i_count = sndbuf_getready(ch->buffer)
		    - MOD_DIFF(ch->unsubmittedptr,
		     sndbuf_getreadyptr(ch->buffer),
		     ch->buffer->bufsize);
		size = ch->buffer->bufsize;
		readyptr = ch->unsubmittedptr;

		int size_changed = 0;
		unsigned int available;

		BCM2835_AUDIO_LOCK(sc);
		if (size != ch->log_vars.bsize) {
			ch->log_vars.bsize = size;
			size_changed = 1;
		}
		available = ch->available_space;
		/*
		 *  XXXMDC
		 *
		 *  On arm64, got into situations where
		 *  readyptr was less than a packet away
		 *  from the end of the buffer, which led
		 *  to count being set to 0 and, inexorably, starvation.
		 *  Code below tries to take that into account.
		 *  The problem might have been fixed with some of the
		 *  other changes that were made in the meantime,
		 *  but for now this works fine.
		 */
		if (readyptr + count > size) {
			count = size - readyptr;
		}
		if(count > ch->available_space){
			count = ch->available_space;
			count -= (count % VCHIQ_AUDIO_PACKET_SIZE);
		}else if (count > VCHIQ_AUDIO_PACKET_SIZE){
			count -= (count % VCHIQ_AUDIO_PACKET_SIZE);
		}else if (size > count + readyptr) {
			count = 0;
		}
		BCM2835_AUDIO_UNLOCK(sc);

		if (count % VCHIQ_AUDIO_PACKET_SIZE != 0) {
			BCM2835_LOG_WARN(sc, "count: %u  initial count: %u  "
			    "size: %u  readyptr: %u  available: %u\n", count,
			    i_count,size,readyptr,available);
		}
		if (size_changed)
		    BCM2835_LOG_INFO(sc, "bsize changed to %u\n", size);

		if (count == 0) {
			BCM2835_LOG_WARN(sc,
			    "not enough room for a packet: count %d,"
			    " i_count %d, rptr %d, size %d\n",
			    count, i_count, readyptr, size);
			continue;
		}

		buf = ch->buffer->buf + readyptr;

		bcm2835_audio_write_samples(ch, buf, count);
		BCM2835_AUDIO_LOCK(sc);
		ch->unsubmittedptr = (ch->unsubmittedptr + count) %
		    ch->buffer->bufsize;
		ch->available_space -= count;
		ch->submitted_samples += count;
		long sub = count;
		long sub_total = ch->submitted_samples;
		long retd = ch->retrieved_samples;
		KASSERT(ch->available_space >= 0, ("ch->available_space == %d\n", ch->available_space));
		BCM2835_AUDIO_UNLOCK(sc);

		BCM2835_LOG_TRACE(sc,
		    "submitted %lu for a total of %lu having been acked %lu; "
		    "rptr %d, had %u available\n", sub, sub_total, retd,
		    readyptr, available);
	}

	BCM2835_AUDIO_LOCK(sc);
	sc->worker_state = WORKER_STOPPED;
	cv_signal(&sc->worker_cv);
	BCM2835_AUDIO_UNLOCK(sc);

	kproc_exit(0);
}

static void
bcm2835_audio_create_worker(struct bcm2835_audio_info *sc)
{
	struct proc *newp;

	sc->worker_state = WORKER_RUNNING;
	if (kproc_create(bcm2835_audio_worker, (void*)sc, &newp, 0, 0,
	    "bcm2835_audio_worker") != 0) {
		BCM2835_LOG_ERROR(sc,
		    "failed to create bcm2835_audio_worker\n");
	}
}

/* -------------------------------------------------------------------- */
/* channel interface for VCHI audio */
static void *
bcmchan_init(kobj_t obj, void *devinfo, struct snd_dbuf *b, struct pcm_channel *c, int dir)
{
	struct bcm2835_audio_info *sc = devinfo;
	struct bcm2835_audio_chinfo *ch = &sc->pch;
	void *buffer;

	if (dir == PCMDIR_REC)
		return NULL;

	ch->parent = sc;
	ch->channel = c;
	ch->buffer = b;

	/* default values */
	ch->spd = 44100;
	ch->fmt = SND_FORMAT(AFMT_S16_LE, 2, 0);
	ch->blksz = VCHIQ_AUDIO_PACKET_SIZE;

	buffer = malloc(sc->bufsz, M_DEVBUF, M_WAITOK | M_ZERO);

	if (sndbuf_setup(ch->buffer, buffer, sc->bufsz) != 0) {
		device_printf(sc->dev, "sndbuf_setup failed\n");
		free(buffer, M_DEVBUF);
		return NULL;
	}

	ch->log_vars = DEFAULT_LOG_VALUES;

	BCM2835_AUDIO_LOCK(sc);
	bcm2835_worker_update_params(sc);
	BCM2835_AUDIO_UNLOCK(sc);

	return ch;
}

static int
bcmchan_free(kobj_t obj, void *data)
{
	struct bcm2835_audio_chinfo *ch = data;
	void *buffer;

	buffer = ch->buffer->buf;
	if (buffer)
		free(buffer, M_DEVBUF);

	return (0);
}

static int
bcmchan_setformat(kobj_t obj, void *data, uint32_t format)
{
	struct bcm2835_audio_chinfo *ch = data;
	struct bcm2835_audio_info *sc = ch->parent;

	BCM2835_AUDIO_LOCK(sc);
	ch->fmt = format;
	bcm2835_worker_update_params(sc);
	BCM2835_AUDIO_UNLOCK(sc);

	return 0;
}

static uint32_t
bcmchan_setspeed(kobj_t obj, void *data, uint32_t speed)
{
	struct bcm2835_audio_chinfo *ch = data;
	struct bcm2835_audio_info *sc = ch->parent;

	BCM2835_AUDIO_LOCK(sc);
	ch->spd = speed;
	bcm2835_worker_update_params(sc);
	BCM2835_AUDIO_UNLOCK(sc);

	return ch->spd;
}

static uint32_t
bcmchan_setblocksize(kobj_t obj, void *data, uint32_t blocksize)
{
	struct bcm2835_audio_chinfo *ch = data;

	return ch->blksz;
}

static int
bcmchan_trigger(kobj_t obj, void *data, int go)
{
	struct bcm2835_audio_chinfo *ch = data;
	struct bcm2835_audio_info *sc = ch->parent;

	if (!PCMTRIG_COMMON(go))
		return (0);

	switch (go) {
	case PCMTRIG_START:
		/* kickstart data flow */
		chn_intr(sc->pch.channel);
		ch->submitted_samples = 0;
		ch->retrieved_samples = 0;
		bcm2835_worker_play_start(sc);
		break;

	case PCMTRIG_STOP:
	case PCMTRIG_ABORT:
		bcm2835_worker_play_stop(sc);
		break;

	default:
		break;
	}
	return 0;
}

static uint32_t
bcmchan_getptr(kobj_t obj, void *data)
{
	struct bcm2835_audio_chinfo *ch = data;
	struct bcm2835_audio_info *sc = ch->parent;
	uint32_t ret;

	BCM2835_AUDIO_LOCK(sc);
	ret = ch->unsubmittedptr;
	BCM2835_AUDIO_UNLOCK(sc);

	return ret;
}

static struct pcmchan_caps *
bcmchan_getcaps(kobj_t obj, void *data)
{

	return &bcm2835_audio_playcaps;
}

static kobj_method_t bcmchan_methods[] = {
    	KOBJMETHOD(channel_init,		bcmchan_init),
    	KOBJMETHOD(channel_free,		bcmchan_free),
    	KOBJMETHOD(channel_setformat,		bcmchan_setformat),
    	KOBJMETHOD(channel_setspeed,		bcmchan_setspeed),
    	KOBJMETHOD(channel_setblocksize,	bcmchan_setblocksize),
    	KOBJMETHOD(channel_trigger,		bcmchan_trigger),
    	KOBJMETHOD(channel_getptr,		bcmchan_getptr),
    	KOBJMETHOD(channel_getcaps,		bcmchan_getcaps),
	KOBJMETHOD_END
};
CHANNEL_DECLARE(bcmchan);

/************************************************************/

static int
bcmmix_init(struct snd_mixer *m)
{

	mix_setdevs(m, SOUND_MASK_VOLUME);

	return (0);
}

static int
bcmmix_set(struct snd_mixer *m, unsigned dev, unsigned left, unsigned right)
{
    	struct bcm2835_audio_info *sc = mix_getdevinfo(m);

	switch (dev) {
	case SOUND_MIXER_VOLUME:
		BCM2835_AUDIO_LOCK(sc);
		sc->volume = left;
		bcm2835_worker_update_params(sc);
		BCM2835_AUDIO_UNLOCK(sc);

		break;

	default:
		break;
	}

    	return left | (left << 8);
}

static kobj_method_t bcmmixer_methods[] = {
    	KOBJMETHOD(mixer_init,		bcmmix_init),
    	KOBJMETHOD(mixer_set,		bcmmix_set),
	KOBJMETHOD_END
};

MIXER_DECLARE(bcmmixer);

static int
sysctl_bcm2835_audio_dest(SYSCTL_HANDLER_ARGS)
{
	struct bcm2835_audio_info *sc = arg1;
	int val;
	int err;

	val = sc->dest;
	err = sysctl_handle_int(oidp, &val, 0, req);
	if (err || !req->newptr) /* error || read request */
		return (err);

	if ((val < 0) || (val > 2))
		return (EINVAL);

	BCM2835_AUDIO_LOCK(sc);
	sc->dest = val;
	bcm2835_worker_update_params(sc);
	BCM2835_AUDIO_UNLOCK(sc);

	if (bootverbose)
		device_printf(sc->dev, "destination set to %s\n", dest_description(val));

	return (0);
}

static void
vchi_audio_sysctl_init(struct bcm2835_audio_info *sc)
{
	struct sysctl_ctx_list *ctx;
	struct sysctl_oid *tree_node;
	struct sysctl_oid_list *tree;

	/*
	 * Add system sysctl tree/handlers.
	 */
	ctx = device_get_sysctl_ctx(sc->dev);
	tree_node = device_get_sysctl_tree(sc->dev);
	tree = SYSCTL_CHILDREN(tree_node);
	SYSCTL_ADD_PROC(ctx, tree, OID_AUTO, "dest",
	    CTLFLAG_RW | CTLTYPE_UINT | CTLFLAG_NEEDGIANT, sc, sizeof(*sc),
	    sysctl_bcm2835_audio_dest, "IU", "audio destination, "
	    "0 - auto, 1 - headphones, 2 - HDMI");
	SYSCTL_ADD_UQUAD(ctx, tree, OID_AUTO, "callbacks",
			CTLFLAG_RD, &sc->pch.callbacks,
			"callbacks total");
	SYSCTL_ADD_UQUAD(ctx, tree, OID_AUTO, "submitted",
			CTLFLAG_RD, &sc->pch.submitted_samples,
			"last play submitted samples");
	SYSCTL_ADD_UQUAD(ctx, tree, OID_AUTO, "retrieved",
			CTLFLAG_RD, &sc->pch.retrieved_samples,
			"last play retrieved samples");
	SYSCTL_ADD_UQUAD(ctx, tree, OID_AUTO, "underruns",
			CTLFLAG_RD, &sc->pch.underruns,
			"callback underruns");
	SYSCTL_ADD_INT(ctx, tree, OID_AUTO, "freebuffer",
			CTLFLAG_RD, &sc->pch.available_space,
			sc->pch.available_space, "callbacks total");
	SYSCTL_ADD_INT(ctx, tree, OID_AUTO, "starved",
			CTLFLAG_RD, &sc->pch.starved,
			sc->pch.starved, "number of starved conditions");
	SYSCTL_ADD_INT(ctx, tree, OID_AUTO, "trace",
			CTLFLAG_RW, &sc->verbose_trace,
			sc->verbose_trace, "enable tracing of transfers");
}

static void
bcm2835_audio_identify(driver_t *driver, device_t parent)
{

	BUS_ADD_CHILD(parent, 0, "pcm", 0);
}

static int
bcm2835_audio_probe(device_t dev)
{

	device_set_desc(dev, "VCHIQ audio");
	return (BUS_PROBE_DEFAULT);
}

static void
bcm2835_audio_delayed_init(void *xsc)
{
    	struct bcm2835_audio_info *sc;
    	char status[SND_STATUSLEN];

	sc = xsc;

	config_intrhook_disestablish(&sc->intr_hook);

	bcm2835_audio_init(sc);
	bcm2835_audio_open(sc);
	sc->volume = 75;
	sc->dest = DEST_AUTO;
	sc->verbose_trace = 0;

    	if (mixer_init(sc->dev, &bcmmixer_class, sc)) {
		device_printf(sc->dev, "mixer_init failed\n");
		goto no;
	}

	pcm_init(sc->dev, sc);

	pcm_addchan(sc->dev, PCMDIR_PLAY, &bcmchan_class, sc);
    	snprintf(status, SND_STATUSLEN, "at VCHIQ");
	if (pcm_register(sc->dev, status)) {
		device_printf(sc->dev, "pcm_register failed\n");
		goto no;
	}

	bcm2835_audio_reset_channel(&sc->pch);
	bcm2835_audio_create_worker(sc);

	vchi_audio_sysctl_init(sc);

no:
	;
}

static int
bcm2835_audio_attach(device_t dev)
{
    	struct bcm2835_audio_info *sc;

	sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK | M_ZERO);

	sc->dev = dev;
	sc->bufsz = VCHIQ_AUDIO_BUFFER_SIZE;

	mtx_init(&sc->lock, device_get_nameunit(dev),
	    "bcm_audio_lock", MTX_DEF);
	cv_init(&sc->worker_cv, "worker_cv");
	sc->vchi_handle = VCHIQ_SERVICE_HANDLE_INVALID;

	/*
	 * We need interrupts enabled for VCHI to work properly,
	 * so delay initialization until it happens.
	 */
	sc->intr_hook.ich_func = bcm2835_audio_delayed_init;
	sc->intr_hook.ich_arg = sc;

	if (config_intrhook_establish(&sc->intr_hook) != 0)
		goto no;

    	return 0;

no:
    	return ENXIO;
}

static int
bcm2835_audio_detach(device_t dev)
{
	int r;
	struct bcm2835_audio_info *sc;
	sc = pcm_getdevinfo(dev);

	/* Stop worker thread */
	BCM2835_AUDIO_LOCK(sc);
	sc->worker_state = WORKER_STOPPING;
	cv_signal(&sc->worker_cv);
	/* Wait for thread to exit */
	while (sc->worker_state != WORKER_STOPPED)
		cv_wait_sig(&sc->worker_cv, &sc->lock);
	BCM2835_AUDIO_UNLOCK(sc);

	r = pcm_unregister(dev);
	if (r)
		return r;

	mtx_destroy(&sc->lock);
	cv_destroy(&sc->worker_cv);

	bcm2835_audio_release(sc);

    	free(sc, M_DEVBUF);

	return 0;
}

static device_method_t bcm2835_audio_methods[] = {
	/* Device interface */
	DEVMETHOD(device_identify,	bcm2835_audio_identify),
	DEVMETHOD(device_probe,		bcm2835_audio_probe),
	DEVMETHOD(device_attach,	bcm2835_audio_attach),
	DEVMETHOD(device_detach,	bcm2835_audio_detach),
	{ 0, 0 }
};

static driver_t bcm2835_audio_driver = {
	"pcm",
	bcm2835_audio_methods,
	PCM_SOFTC_SIZE,
};

DRIVER_MODULE(bcm2835_audio, vchiq, bcm2835_audio_driver, 0, 0);
MODULE_DEPEND(bcm2835_audio, sound, SOUND_MINVER, SOUND_PREFVER, SOUND_MAXVER);
MODULE_DEPEND(bcm2835_audio, vchiq, 1, 1, 1);
MODULE_VERSION(bcm2835_audio, 1);