From c06c6ffa81068cbd12b404f80f779755921f5edf Mon Sep 17 00:00:00 2001 From: Tobias Johansson Date: Mon, 19 Oct 2020 16:53:09 +0200 Subject: [PATCH] cxd56: add initial audio SRC implementation Add basic sample rate conversion to the CXD56 Spresense audio driver using libsamplerate. Currently conversion is only done during playback and all output is fixed at 48 kHz. Issues: - 16 kHz SRC has glitches (unless data dump is enabled) - 44.1 kHz SRC gets stuck Signed-off-by: Alin Jerpelea --- drivers/audio/Kconfig | 8 + drivers/audio/Make.defs | 3 + drivers/audio/cxd56.c | 332 ++++++++++++++++----- drivers/audio/cxd56.h | 6 + drivers/audio/cxd56_src.c | 603 ++++++++++++++++++++++++++++++++++++++ drivers/audio/cxd56_src.h | 120 ++++++++ 6 files changed, 997 insertions(+), 75 deletions(-) create mode 100644 drivers/audio/cxd56_src.c create mode 100644 drivers/audio/cxd56_src.h diff --git a/drivers/audio/Kconfig b/drivers/audio/Kconfig index 610109c96e7..a5182fa6bec 100644 --- a/drivers/audio/Kconfig +++ b/drivers/audio/Kconfig @@ -40,6 +40,14 @@ if AUDIO_CXD56 if AUDIO_DRIVER_SPECIFIC_BUFFERS +config AUDIO_CXD56_SRC + bool "CXD56 audio sample rate convertor" + select AUDIO_SRC + default n + ---help--- + Enable support for audio playback using the CXD5247 chip on the + CXD56 Spresense board with sample rate convertor. + config CXD56_AUDIO_NUM_BUFFERS int "Number of audio buffers to use" default 4 diff --git a/drivers/audio/Make.defs b/drivers/audio/Make.defs index b0c0535635e..cb8bb6e7dad 100644 --- a/drivers/audio/Make.defs +++ b/drivers/audio/Make.defs @@ -41,6 +41,9 @@ ifeq ($(CONFIG_DRIVERS_AUDIO),y) ifeq ($(CONFIG_AUDIO_CXD56),y) CSRCS += cxd56.c +ifeq ($(CONFIG_AUDIO_CXD56_SRC),y) +CSRCS += cxd56_src.c +endif endif ifeq ($(CONFIG_AUDIO_VS1053),y) diff --git a/drivers/audio/cxd56.c b/drivers/audio/cxd56.c index ea5d5582a1a..935303f26a7 100644 --- a/drivers/audio/cxd56.c +++ b/drivers/audio/cxd56.c @@ -55,6 +55,10 @@ #include "cxd56.h" +#ifdef CONFIG_AUDIO_CXD56_SRC +#include "cxd56_src.h" +#endif + /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ @@ -1272,6 +1276,157 @@ static void cxd56_reset_channel_sel(cxd56_dmahandle_t handle) } } +#ifdef CONFIG_CXD56_AUCIO_SRC +static void _process_audio_with_src(cxd56_dmahandle_t hdl, uint16_t err_code) +{ + struct audio_msg_s msg; + struct cxd56_dev_s *dev; + irqstate_t flags; + bool request_buffer = true; + int ret; + + dev = g_dev[hdl]; + + /* Trigger new DMA job */ + + flags = spin_lock_irqsave(); + + if (err_code == CXD56_AUDIO_ECODE_DMA_TRANS) + { + /* Notify end of data */ + + if (dev->state != CXD56_DEV_STATE_PAUSED + && dq_count(&dev->down_pendq) == 0) + { + msg.msg_id = AUDIO_MSG_STOP; + msg.u.data = 0; + spin_unlock_irqrestore(flags); + ret = nxmq_send(dev->mq, (FAR const char *)&msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + flags = spin_lock_irqsave(); + if (ret != OK) + { + auderr("ERROR: nxmq_send to stop failed (%d)\n", ret); + } + } + } + + if (dq_count(&dev->down_runq) > 0) + { + FAR struct ap_buffer_s *src_apb; + + src_apb = (struct ap_buffer_s *) dq_get(&dev->down_runq); + src_apb->nbytes = 0; + dq_put(&dev->down_doneq, &src_apb->dq_entry); + + if (src_apb->flags & AUDIO_APB_SRC_FINAL) + { + struct ap_buffer_s *apb; + + apb = dq_get(&dev->up_runq); + spin_unlock_irqrestore(flags); + dev->dev.upper(dev->dev.priv, AUDIO_CALLBACK_DEQUEUE, apb, OK); + flags = spin_lock_irqsave(); + + /* End of data? */ + + if ((apb->flags & AUDIO_APB_FINAL) != 0) + { + msg.msg_id = AUDIO_MSG_STOP; + msg.u.data = 0; + spin_unlock_irqrestore(flags); + ret = nxmq_send(dev->mq, (FAR const char *)&msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + flags = spin_lock_irqsave(); + if (ret != OK) + { + auderr("ERROR: nxmq_send to stop failed (%d)\n", ret); + } + + request_buffer = false; + } + } + } + + if (request_buffer && dev->mq != NULL) + { + /* Request more data */ + + msg.msg_id = AUDIO_MSG_DATA_REQUEST; + msg.u.data = 0; + spin_unlock_irqrestore(flags); + ret = nxmq_send(dev->mq, (FAR const char *) &msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + flags = spin_lock_irqsave(); + if (ret != OK) + { + auderr("ERROR: nxmq_send to request failed (%d)\n", ret); + } + } + + spin_unlock_irqrestore(flags); +} + +#else +static void _process_audio(cxd56_dmahandle_t hdl, uint16_t err_code) +{ + struct audio_msg_s msg; + struct cxd56_dev_s *dev; + irqstate_t flags; + int ret; + + dev = g_dev[hdl]; + + /* Trigger new DMA job */ + + flags = spin_lock_irqsave(); + + if (dq_count(&dev->up_runq) > 0) + { + FAR struct ap_buffer_s *apb; + + apb = (struct ap_buffer_s *) dq_get(&dev->up_runq); + spin_unlock_irqrestore(flags); + dev->dev.upper(dev->dev.priv, AUDIO_CALLBACK_DEQUEUE, apb, OK); + flags = spin_lock_irqsave(); + } + + spin_unlock_irqrestore(flags); + + if (err_code == CXD56_AUDIO_ECODE_DMA_TRANS) + { + /* Notify end of data */ + + if (dev->state != CXD56_DEV_STATE_PAUSED) + { + audinfo("DMA_TRANS up_pendq=%d \n", + dq_count(&dev->up_pendq)); + msg.msg_id = AUDIO_MSG_STOP; + msg.u.data = 0; + ret = nxmq_send(dev->mq, (FAR const char *)&msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + if (ret != OK) + { + auderr("ERROR: nxmq_send to stop failed (%d)\n", ret); + } + } + } + else if (dev->mq != NULL) + { + /* Request more data */ + + msg.msg_id = AUDIO_MSG_DATA_REQUEST; + msg.u.data = 0; + ret = nxmq_send(dev->mq, (FAR const char *) &msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + if (ret != OK) + { + auderr("ERROR: nxmq_send to request failed (%d)\n", ret); + } + } +} +#endif + static void cxd56_dma_int_handler(void) { uint16_t err_code; @@ -1360,60 +1515,11 @@ static void cxd56_dma_int_handler(void) if (err_code != CXD56_AUDIO_ECODE_DMA_HANDLE_INV) { - struct audio_msg_s msg; - struct cxd56_dev_s *dev; - irqstate_t flags; - int ret; - - dev = g_dev[hdl]; - - /* Trigger new DMA job */ - - flags = spin_lock_irqsave(); - - if (dq_count(&dev->up_runq) > 0) - { - FAR struct ap_buffer_s *apb; - - apb = (struct ap_buffer_s *) dq_get(&dev->up_runq); - spin_unlock_irqrestore(flags); - dev->dev.upper(dev->dev.priv, AUDIO_CALLBACK_DEQUEUE, apb, OK); - flags = spin_lock_irqsave(); - } - - spin_unlock_irqrestore(flags); - - if (err_code == CXD56_AUDIO_ECODE_DMA_TRANS) - { - /* Notify end of data */ - - if (dev->state != CXD56_DEV_STATE_PAUSED) - { - audinfo("DMA_TRANS up_pendq=%d \n", - dq_count(&dev->up_pendq)); - msg.msg_id = AUDIO_MSG_STOP; - msg.u.data = 0; - ret = nxmq_send(dev->mq, (FAR const char *)&msg, - sizeof(msg), CONFIG_CXD56_MSG_PRIO); - if (ret != OK) - { - auderr("ERROR: nxmq_send to stop failed (%d)\n", ret); - } - } - } - else if (dev->mq != NULL) - { - /* Request more data */ - - msg.msg_id = AUDIO_MSG_DATA_REQUEST; - msg.u.data = 0; - ret = nxmq_send(dev->mq, (FAR const char *) &msg, - sizeof(msg), CONFIG_CXD56_MSG_PRIO); - if (ret != OK) - { - auderr("ERROR: nxmq_send to request failed (%d)\n", ret); - } - } +#ifdef CONFIG_CXD56_AUCIO_SRC + _process_audio_with_src(hdl, err_code); +#else + _process_audio(hdl, err_code); +#endif } } @@ -1627,6 +1733,11 @@ static void cxd56_init_dma(FAR struct cxd56_dev_s *dev) dq_clear(&dev->up_pendq); dq_clear(&dev->up_runq); +#ifdef CONFIG_AUDIO_CXD56_SRC + dq_clear(&dev->down_pendq); + dq_clear(&dev->down_runq); + dq_clear(&dev->down_doneq); +#endif ints = CXD56_DMA_INT_DONE | CXD56_DMA_INT_ERR | CXD56_DMA_INT_CMB; @@ -2693,6 +2804,15 @@ static int cxd56_configure(FAR struct audio_lowerhalf_s *lower, priv->channels = caps->ac_channels; priv->bitwidth = caps->ac_controls.b[2]; +#ifdef CONFIG_AUDIO_CXD56_SRC + ret = cxd56_src_init(priv, &priv->down_doneq, &priv->down_pendq); + if (ret != OK) + { + auderr("ERROR: Could not initialize SRC (%d)\n", ret); + return -ENOMEM; + } + +#endif g_dev[priv->dma_handle] = priv; poweron = 1; @@ -3068,7 +3188,13 @@ static int cxd56_start_dma(FAR struct cxd56_dev_s *dev) int ret = OK; flags = spin_lock_irqsave(); +#ifdef CONFIG_AUDIO_CXD56_SRC + FAR struct ap_buffer_s *src_apb; + + if (dq_count(&dev->down_pendq) == 0) +#else if (dq_count(&dev->up_pendq) == 0) +#endif { /* Underrun occurred, stop DMA and change state for buffering */ @@ -3090,7 +3216,11 @@ static int cxd56_start_dma(FAR struct cxd56_dev_s *dev) { /* Fill up with as many DMA requests as we can */ +#ifdef CONFIG_AUDIO_CXD56_SRC + while (dq_count(&dev->down_pendq) > 0) +#else while (dq_count(&dev->up_pendq) > 0) +#endif { if (cxd56_dma_is_busy(dev->dma_handle)) { @@ -3100,9 +3230,15 @@ static int cxd56_start_dma(FAR struct cxd56_dev_s *dev) goto exit; } +#ifdef CONFIG_AUDIO_CXD56_SRC + src_apb = (struct ap_buffer_s *) dq_peek(&dev->down_pendq); + addr = ((uint32_t)src_apb->samp) & CXD56_DMA_START_ADDR_MASK; + size = (src_apb->nbytes / (dev->bitwidth / 8) / dev->channels) - 1; +#else apb = (struct ap_buffer_s *) dq_peek(&dev->up_pendq); addr = ((uint32_t)apb->samp) & CXD56_DMA_START_ADDR_MASK; size = (apb->nbytes / (dev->bitwidth / 8) / dev->channels) - 1; +#endif if (dev->dma_handle == CXD56_AUDIO_DMA_MIC) { @@ -3114,7 +3250,13 @@ static int cxd56_start_dma(FAR struct cxd56_dev_s *dev) if (dev->bitwidth == 16 && CXD56_DMA_FORMAT == CXD56_DMA_FORMAT_RL) { - cxd56_swap_buffer_rl((uint32_t)apb->samp, apb->nbytes); +#ifdef CONFIG_AUDIO_CXD56_SRC + cxd56_swap_buffer_rl((uint32_t)src_apb->samp, + src_apb->nbytes); +#else + cxd56_swap_buffer_rl((uint32_t)apb->samp, + apb->nbytes); +#endif } write_reg(REG_I2S1_OUT_START_ADR, addr); @@ -3223,10 +3365,19 @@ static int cxd56_start_dma(FAR struct cxd56_dev_s *dev) cxd56_set_dma_running(dev->dma_handle, true); } +#ifdef CONFIG_AUDIO_CXD56_SRC + dq_get(&dev->down_pendq); + dq_put(&dev->down_runq, &src_apb->dq_entry); + + apb = (struct ap_buffer_s *) dq_get(&dev->up_pendq); +#else dq_get(&dev->up_pendq); +#endif dq_put(&dev->up_runq, &apb->dq_entry); + dev->state = CXD56_DEV_STATE_STARTED; +#ifndef CONFIG_AUDIO_CXD56_SRC if ((apb->flags & AUDIO_APB_FINAL) != 0) { /* If the apb is final, send stop message */ @@ -3247,6 +3398,7 @@ static int cxd56_start_dma(FAR struct cxd56_dev_s *dev) goto exit; } } +#endif } } @@ -3269,29 +3421,40 @@ static int cxd56_enqueuebuffer(FAR struct audio_lowerhalf_s *lower, FAR struct cxd56_dev_s *priv = (FAR struct cxd56_dev_s *)lower; struct audio_msg_s msg; irqstate_t flags; + int ret; - flags = spin_lock_irqsave(); - - apb->dq_entry.flink = NULL; - dq_put(&priv->up_pendq, &apb->dq_entry); - - spin_unlock_irqrestore(flags); - - if (priv->mq != NULL) +#ifdef CONFIG_AUDIO_CXD56_SRC + ret = cxd56_src_enqueue(apb); + if (ret != OK) { - int ret; - - msg.msg_id = AUDIO_MSG_ENQUEUE; - msg.u.data = 0; - - ret = nxmq_send(priv->mq, (FAR const char *) &msg, - sizeof(msg), CONFIG_CXD56_MSG_PRIO); - if (ret != OK) - { - auderr("ERROR: nxmq_send to enqueue failed (%d)\n", ret); - return ret; - } + auderr("ERROR: SRC processing failed (%d)\n", ret); } + else + { +#endif + flags = spin_lock_irqsave(); + + apb->dq_entry.flink = NULL; + dq_put(&priv->up_pendq, &apb->dq_entry); + + spin_unlock_irqrestore(flags); + + if (priv->mq != NULL) + { + msg.msg_id = AUDIO_MSG_ENQUEUE; + msg.u.data = 0; + + ret = nxmq_send(priv->mq, (FAR const char *) &msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + if (ret != OK) + { + auderr("ERROR: nxmq_send to enqueue failed (%d)\n", ret); + return ret; + } + } +#ifdef CONFIG_AUDIO_CXD56_SRC + } +#endif return OK; } @@ -3409,6 +3572,20 @@ static void *cxd56_workerthread(pthread_addr_t pvarg) priv->running = false; } +#ifdef CONFIG_AUDIO_CXD56_SRC + ret = cxd56_src_stop(); + if (ret != OK) + { + auderr("ERROR: Could not stop SRC (%d)\n", ret); + } + + ret = cxd56_src_deinit(); + if (ret != OK) + { + auderr("ERROR: Could not deinit SRC (%d)\n", ret); + } + +#endif priv->state = CXD56_DEV_STATE_STOPPED; priv->running = false; audinfo("Workerthread stopped.\n"); @@ -3547,6 +3724,11 @@ struct audio_lowerhalf_s *cxd56_initialize( nxsem_init(&priv->pendsem, 0, 1); dq_init(&priv->up_pendq); dq_init(&priv->up_runq); +#ifdef CONFIG_AUDIO_CXD56_SRC + dq_init(&priv->down_pendq); + dq_init(&priv->down_runq); + dq_init(&priv->down_doneq); +#endif } return &priv->dev; diff --git a/drivers/audio/cxd56.h b/drivers/audio/cxd56.h index 65a385786ce..efff9d383a7 100644 --- a/drivers/audio/cxd56.h +++ b/drivers/audio/cxd56.h @@ -287,6 +287,12 @@ struct cxd56_dev_s struct dq_queue_s up_pendq; /* Pending buffers from app to process */ struct dq_queue_s up_runq; /* Buffers from app being played */ +#ifdef CONFIG_AUDIO_CXD56_SRC + struct dq_queue_s down_pendq; /* Pending SRC buffers to be DMA'd */ + struct dq_queue_s down_runq; /* SRC buffers being processed */ + struct dq_queue_s down_doneq; /* Done SRC buffers to be re-used */ +#endif + uint16_t samplerate; /* Sample rate */ #ifndef CONFIG_AUDIO_EXCLUDE_VOLUME int16_t volume; /* Output volume {0..63} */ diff --git a/drivers/audio/cxd56_src.c b/drivers/audio/cxd56_src.c new file mode 100644 index 00000000000..70ec5994d7a --- /dev/null +++ b/drivers/audio/cxd56_src.c @@ -0,0 +1,603 @@ +/**************************************************************************** + * drivers/audio/cxd56_src.c + * + * Copyright 2020 Sony Semiconductor Solutions Corporation + * + * 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. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 + * COPYRIGHT OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "cxd56.h" +#include "cxd56_src.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* For debugging: dump pre/post SRC data to sdcard */ + +/* #define DUMP_DATA */ + +/* Note: 24 bit samples not currently supported by SRC */ + +#define BUFFER_SAMPLES (CONFIG_CXD56_AUDIO_BUFFER_SIZE / 2) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +enum cxd56_srcstate_e +{ + CXD56_SRC_OFF, + CXD56_SRC_RUNNING, + CXD56_SRC_STOPPING, + CXD56_SRC_STOPPED +}; + +struct cxd56_srcdata_s +{ + enum cxd56_srcstate_e state; + + float float_in[BUFFER_SAMPLES]; + float float_out[BUFFER_SAMPLES]; + int float_in_offset; + + SRC_DATA src_data; + SRC_STATE *src_state; + + struct dq_queue_s *inq; + struct dq_queue_s *outq; + + char mqname[32]; + mqd_t mq; + sem_t pendsem; + pthread_t threadid; + + uint8_t bytewidth; + uint8_t channels; + + float buf_count; + float buf_increment; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct cxd56_srcdata_s g_src; + +#ifdef DUMP_DATA +static char *dump_name_pre = "/mnt/sd0/dump/nx_player_dump_pre.pcm"; +static char *dump_name_post = "/mnt/sd0/dump/nx_player_dump_post.pcm"; +int dump_file_pre = -1; +int dump_file_post = -1; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +extern void src_short_to_float_array (const short *in, float *out, int len); +extern void src_float_to_short_array (const float *in, short *out, int len); +extern void src_int_to_float_array (const int *in, float *out, int len); +extern void src_float_to_int_array (const float *in, int *out, int len); + +static struct ap_buffer_s *cxd56_src_get_apb() +{ + struct ap_buffer_s *src_apb; + irqstate_t flags; + + flags = spin_lock_irqsave(); + + if (dq_count(g_src.inq) == 0) + { + size_t bufsize = sizeof(struct ap_buffer_s) + + CONFIG_CXD56_AUDIO_BUFFER_SIZE; + + spin_unlock_irqrestore(flags); + + src_apb = kmm_zalloc(bufsize); + + flags = spin_lock_irqsave(); + + if (!src_apb) + { + auderr("ERROR: Couldn't allocate SRC APB (size %d)\n", bufsize); + + goto errorout_with_lock; + } + + src_apb->nmaxbytes = CONFIG_CXD56_AUDIO_BUFFER_SIZE; + src_apb->nbytes = 0; + src_apb->samp = (FAR uint8_t *)(&src_apb->samp + 1); + } + else + { + src_apb = (struct ap_buffer_s *) dq_get(g_src.inq); + } + + src_apb->flags = 0; + +errorout_with_lock: + + spin_unlock_irqrestore(flags); + + return src_apb; +} + +/* Apply SRC on incoming APB and add one or more APBs to the + * out queue accordingly. + */ + +static int cxd56_src_process(FAR struct ap_buffer_s *apb) +{ + int ret = OK; + irqstate_t flags; + struct ap_buffer_s *src_apb; + + /* audinfo("SRC: Process (size = %d)\n", apb->nbytes); */ + +#ifdef DUMP_DATA + write(dump_file_pre, + (char *) (apb->samp + apb->curbyte), + apb->nbytes - apb->curbyte); +#endif + + /* Special case of one-to-one ratio */ + + if (g_src.src_data.src_ratio == 1.0f) + { + src_apb = cxd56_src_get_apb(); + if (!src_apb) + { + ret = -ENOMEM; + goto exit; + } + + memcpy(src_apb->samp, apb->samp, apb->nbytes); + src_apb->nbytes = apb->nbytes; + src_apb->flags |= AUDIO_APB_SRC_FINAL; + + flags = spin_lock_irqsave(); + dq_put(g_src.outq, &src_apb->dq_entry); + spin_unlock_irqrestore(flags); + + goto exit; + } + + /* Perform SRC on new buffer and left overs from previous ones */ + + while (apb->curbyte < apb->nbytes) + { + int float_in_left; + int frames_in; + + short *apb_addr = (const short *)(apb->samp + apb->curbyte); + + /* Fill up incoming float buffer */ + + float_in_left = BUFFER_SAMPLES - g_src.float_in_offset; + + src_short_to_float_array(apb_addr, + (g_src.float_in + g_src.float_in_offset), + float_in_left); + g_src.src_data.output_frames = BUFFER_SAMPLES / g_src.channels; + g_src.src_data.input_frames = BUFFER_SAMPLES / g_src.channels; + + /* Incoming data larger than ingoing float buffer? */ + + frames_in = (apb->nbytes - apb->curbyte) / g_src.bytewidth; + + if (frames_in >= float_in_left || g_src.state == CXD56_SRC_STOPPING) + { + int apb_nframes; + int apb_nmaxframes; + int src_nframes; + int src_copyframes; + + float *float_out_src; + short *src_apb_dest; + + /* Run SRC */ + + g_src.src_data.data_out = g_src.float_out; + g_src.src_data.data_in = g_src.float_in; + + ret = src_process(g_src.src_state, &g_src.src_data); + if (ret != 0) + { + auderr("ERROR: SRC failed (\"%s\")\n", src_strerror(ret)); + } + + /* Move unused data to start of float_in for next round */ + + g_src.float_in_offset = + g_src.src_data.input_frames_used * g_src.channels; + memcpy((void *)g_src.float_in, + (void *)(g_src.float_in + g_src.float_in_offset), + (BUFFER_SAMPLES - g_src.float_in_offset) * sizeof(float)); + + g_src.float_in_offset = BUFFER_SAMPLES - g_src.float_in_offset; + + /* Prepare apb to dma */ + + src_apb = cxd56_src_get_apb(); + if (!src_apb) + { + ret = -ENOMEM; + goto exit; + } + + apb_nframes = + src_apb->nbytes / g_src.bytewidth / g_src.channels; + apb_nmaxframes = + src_apb->nmaxbytes / g_src.bytewidth / g_src.channels; + + src_nframes = g_src.src_data.output_frames_gen; + src_copyframes = apb_nmaxframes - apb_nframes; + + /* Generated frames will exceed apb size left */ + + if (apb_nframes + src_nframes >= apb_nmaxframes + || g_src.state == CXD56_SRC_STOPPING) + { + /* Convert SRC float data into APB to be sent */ + + float_out_src = g_src.float_out; + src_apb_dest = (short *)(src_apb->samp + src_apb->nbytes); + + src_float_to_short_array(float_out_src, src_apb_dest, + src_copyframes * g_src.channels); + src_nframes -= src_copyframes; + src_apb->nbytes = src_apb->nmaxbytes; + + /* Increase SRC buffer processing counter */ + + g_src.buf_count += g_src.buf_increment; + if (g_src.buf_count > 1.0f) + { + src_apb->flags |= AUDIO_APB_SRC_FINAL; + g_src.buf_count -= 1.0f; + } + + /* Put in out queue to be DMA'd */ + + flags = spin_lock_irqsave(); + dq_put(g_src.outq, &src_apb->dq_entry); + spin_unlock_irqrestore(flags); + +#ifdef DUMP_DATA + write(dump_file_post, src_apb->samp, src_apb->nbytes); +#endif + + /* Fetch the next APB to fill up */ + + src_apb = cxd56_src_get_apb(); + if (!src_apb) + { + ret = -ENOMEM; + goto exit; + } + + apb_nframes = + src_apb->nbytes / g_src.bytewidth / g_src.channels; + } + + /* Convert remaining SRC float data into next APB */ + + float_out_src = g_src.float_out + src_copyframes * g_src.channels; + src_apb_dest = (short *)(src_apb->samp + src_apb->nbytes); + + src_float_to_short_array(float_out_src, src_apb_dest, + src_nframes * g_src.channels); + + src_apb->nbytes += g_src.bytewidth * src_nframes * g_src.channels; + + flags = spin_lock_irqsave(); + dq_put_back(g_src.inq, &src_apb->dq_entry); + spin_unlock_irqrestore(flags); + + apb->curbyte += (float_in_left * g_src.bytewidth); + } + else + { + g_src.float_in_offset += frames_in; + apb->curbyte = apb->nbytes - 1; + + break; + } + } + +exit: + return ret; +} + +/* SRC control and processing thread */ + +static void *cxd56_src_thread(pthread_addr_t pvarg) +{ + struct audio_msg_s msg; + unsigned int prio; + int ret; + int size; + + audinfo("SRC: Thread started\n"); + + g_src.state = CXD56_SRC_RUNNING; + + while (g_src.state == CXD56_SRC_RUNNING) + { + size = nxmq_receive(g_src.mq, (FAR char *)&msg, sizeof(msg), &prio); + + /* Handle the case when we return with no message */ + + if (size == 0) + { + audinfo("SRC: Zero message, stop\n"); + g_src.state = CXD56_SRC_STOPPED; + break; + } + + /* Process the message */ + + switch (msg.msg_id) + { + case AUDIO_MSG_START: + break; + case AUDIO_MSG_STOP: + g_src.state = CXD56_SRC_STOPPED; + break; + case AUDIO_MSG_ENQUEUE: + ret = cxd56_src_process(msg.u.ptr); + if (ret != OK) + { + auderr("ERROR: SRC processing failed (%d)\n", ret); + g_src.state = CXD56_SRC_STOPPED; + } + break; + } + } + + return NULL; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cxd56_src_init + * + * Description: Initializes the SRC using audio settings set in the given + * audio device. When SRC is running the resulting buffers will + * be put into the queue "outq" for playback, and after it has + * been played it is expected to be put in the "inq" queue to be + * filled up with new data. + * + ****************************************************************************/ + +int cxd56_src_init(FAR struct cxd56_dev_s *dev, + FAR struct dq_queue_s *inq, + FAR struct dq_queue_s *outq) +{ + struct sched_param sparam; + struct mq_attr m_attr; + pthread_attr_t t_attr; + void *value; + int error; + int ret = OK; + + g_src.buf_count = 0.0f; + if (dev->samplerate < 48000) + { + g_src.buf_increment = dev->samplerate / 48000.0f; + } + + g_src.inq = inq; + g_src.outq = outq; + g_src.bytewidth = dev->bitwidth / 8; + g_src.channels = dev->channels; + g_src.float_in_offset = 0; + snprintf(g_src.mqname, sizeof(g_src.mqname), "/tmp/%X", &g_src); + + audinfo("SRC: Init (rate = %d, channels = %d, width = %d)\n", + dev->samplerate, g_src.channels, g_src.bytewidth); + + m_attr.mq_maxmsg = 16; + m_attr.mq_msgsize = sizeof(struct audio_msg_s); + m_attr.mq_curmsgs = 0; + m_attr.mq_flags = 0; + + g_src.mq = mq_open(g_src.mqname, O_RDWR | O_CREAT, 0644, &m_attr); + if (g_src.mq == NULL) + { + auderr("ERROR: Could not allocate SRC message queue.\n"); + return -ENOMEM; + } + +#ifdef DUMP_DATA + unlink(dump_name_pre); + unlink(dump_name_post); + dump_file_pre = open(dump_name_pre, O_WRONLY | O_CREAT | O_APPEND); + dump_file_post = open(dump_name_post, O_WRONLY | O_CREAT | O_APPEND); +#endif + + /* Join any old worker threads to prevent memory leaks */ + + if (g_src.threadid != 0) + { + pthread_join(g_src.threadid, &value); + } + + pthread_attr_init(&t_attr); + sparam.sched_priority = sched_get_priority_max(SCHED_FIFO) - 3; + (void)pthread_attr_setschedparam(&t_attr, &sparam); + (void)pthread_attr_setstacksize(&t_attr, + CONFIG_CXD56_AUDIO_SRC_STACKSIZE); + + ret = pthread_create(&g_src.threadid, &t_attr, cxd56_src_thread, + (pthread_addr_t)&g_src); + if (ret != OK) + { + auderr("ERROR: SRC pthread_create failed (%d)\n", ret); + return ret; + } + + pthread_setname_np(g_src.threadid, "cxd56_src"); + + /* Initialize sample rate converter */ + + g_src.src_data.src_ratio = (double) (48000.0f / dev->samplerate); + if (g_src.src_data.src_ratio == 1.0f) + { + audinfo("SRC in and out rate is the same, will copy only.\n"); + } + + g_src.src_state = src_new(SRC_LINEAR, g_src.channels, &error); + if (g_src.src_state == NULL) + { + auderr("ERROR: Could not initialize SRC (%d)\n", src_strerror(error)); + ret = error; + } + + return ret; +} + +/**************************************************************************** + * Name: cxd56_src_deinit + * + * Description: Releases the SRC instance and related resources. + * + ****************************************************************************/ + +int cxd56_src_deinit(void) +{ + struct ap_buffer_s *src_apb; + + audinfo("SRC: Deinit\n"); + + /* Free SRC buffers */ + + while (dq_count(g_src.inq)) + { + src_apb = (struct ap_buffer_s *) dq_get(g_src.inq); + kmm_free(src_apb); + } + + while (dq_count(g_src.outq)) + { + src_apb = (struct ap_buffer_s *) dq_get(g_src.outq); + kmm_free(src_apb); + } + + src_delete(g_src.src_state); + +#ifdef DUMP_DATA + if (dump_file_pre) + close(dump_file_pre); + + if (dump_file_post) + close(dump_file_post); +#endif + + return OK; +} + +/**************************************************************************** + * Name: cxd56_src_enqueue + * + * Description: Enqueues a audio buffer for SRC processing. The result will + * be put in the outgoing queue given during initialization. + * + ****************************************************************************/ + +int cxd56_src_enqueue(FAR struct ap_buffer_s *apb) +{ + int ret; + struct audio_msg_s msg; + + audinfo("SRC: Enqueue %x\n", apb); + + msg.msg_id = AUDIO_MSG_ENQUEUE; + msg.u.ptr = apb; + ret = nxmq_send(g_src.mq, (FAR const char *)&msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + if (ret != OK) + { + auderr("ERROR: SRC APB enqueue failed (%d)\n", ret); + } + + return ret; +} + +/**************************************************************************** + * Name: cxd56_src_stop + * + * Description: Stops the SRC processing thread. + * + ****************************************************************************/ + +int cxd56_src_stop(void) +{ + int ret; + void *value; + struct audio_msg_s msg; + + audinfo("SRC: Stop\n"); + + msg.msg_id = AUDIO_MSG_STOP; + msg.u.data = 0; + ret = nxmq_send(g_src.mq, (FAR const char *)&msg, + sizeof(msg), CONFIG_CXD56_MSG_PRIO); + if (ret != OK) + { + auderr("ERROR: SRC stop failed (%d)\n", ret); + } + + pthread_join(g_src.threadid, &value); + g_src.threadid = 0; + + return ret; +} diff --git a/drivers/audio/cxd56_src.h b/drivers/audio/cxd56_src.h new file mode 100644 index 00000000000..e05a190caf2 --- /dev/null +++ b/drivers/audio/cxd56_src.h @@ -0,0 +1,120 @@ +/**************************************************************************** + * drivers/audio/cxd56_src.h + * + * Copyright 2020 Sony Semiconductor Solutions Corporation + * + * 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. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 + * COPYRIGHT OWNER 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. + * + ****************************************************************************/ + +#ifndef __DRIVERS_AUDIO_CXD56_SRC_H +#define __DRIVERS_AUDIO_CXD56_SRC_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include + +#ifdef CONFIG_AUDIO + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifndef CONFIG_CXD56_AUDIO_SRC_STACKSIZE +# define CONFIG_CXD56_AUDIO_SRC_STACKSIZE 768 +#endif + +#ifndef CONFIG_CXD56_SRC_MSG_PRIO +# define CONFIG_CXD56_SRC_MSG_PRIO 1 +#endif + +#ifndef SRC_SINC_BEST_QUALITY +# define SRC_SINC_BEST_QUALITY 0 +#endif + +#ifndef SRC_SINC_MEDIUM_QUALITY +# define SRC_SINC_MEDIUM_QUALITY 1 +#endif + +#ifndef SRC_SINC_FASTEST +# define SRC_SINC_FASTEST 2 +#endif + +#ifndef SRC_ZERO_ORDER_HOLD +# define SRC_ZERO_ORDER_HOLD 3 +#endif + +#ifndef SRC_LINEAR +# define SRC_LINEAR 4 +#endif + +#define AUDIO_APB_SRC_FINAL (1 << 4) /* Last buffer in SRC processing */ + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +typedef struct SRC_STATE_TAG SRC_STATE; + +typedef struct +{ + const float *data_in; + float *data_out; + long input_frames; + long output_frames; + long input_frames_used; + long output_frames_gen; + int end_of_input; + double src_ratio; +} SRC_DATA; + +SRC_STATE *src_new (int converter_type, int channels, int *error); + +SRC_STATE *src_delete (SRC_STATE *state); + +int src_process (SRC_STATE *state, SRC_DATA *data); + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +int cxd56_src_init(FAR struct cxd56_dev_s *dev, FAR struct dq_queue_s *inq, + FAR struct dq_queue_s *outq); +int cxd56_src_deinit(void); +int cxd56_src_enqueue(FAR struct ap_buffer_s *apb); +int cxd56_src_stop(void); + +#endif /* CONFIG_AUDIO */ + +#endif /* __DRIVERS_AUDIO_CXD56_SRC_H */