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 */