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