Skip to content

Commit

Permalink
applications: nrf5340_audio: Add lc3_playback module
Browse files Browse the repository at this point in the history
  • Loading branch information
@mVaardal
mVaardal committed Jul 28, 2023
1 parent 3efea5f commit 8eef146
Show file tree
Hide file tree
Showing 11 changed files with 400 additions and 96 deletions.
4 changes: 4 additions & 0 deletions applications/nrf5340_audio/prj.conf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,3 +58,7 @@ CONFIG_DEVICE_SHELL=n
# Suppress err msg from sd_check_card_type. Because SPI_SDHC has no card presence method,
# assume card is in slot. Thus error message is always shown if card is not inserted
CONFIG_SD_LOG_LEVEL_OFF=y
CONFIG_LC3_DEC_CHAN_MAX=2

CONFIG_MAIN_STACK_SIZE=1800
CONFIG_LC3_PLAYBACK=y
91 changes: 48 additions & 43 deletions applications/nrf5340_audio/src/audio/audio_datapath.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include "contin_array.h"
#include "pcm_mix.h"
#include "streamctrl.h"
#include "lc3_playback.h"

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL);
Expand All @@ -42,23 +43,23 @@ LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL);

/* Total sample FIFO period in microseconds */
#define FIFO_SMPL_PERIOD_US (CONFIG_AUDIO_MAX_PRES_DLY_US * 2)
#define FIFO_NUM_BLKS NUM_BLKS(FIFO_SMPL_PERIOD_US)
#define MAX_FIFO_SIZE (FIFO_NUM_BLKS * BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ) * 2)
#define FIFO_NUM_BLKS NUM_BLKS(FIFO_SMPL_PERIOD_US)
#define MAX_FIFO_SIZE (FIFO_NUM_BLKS * BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ) * 2)

/* Number of audio blocks given a duration */
#define NUM_BLKS(d) ((d) / BLK_PERIOD_US)
#define NUM_BLKS(d) ((d) / BLK_PERIOD_US)
/* Single audio block size in number of samples (stereo) */
#define BLK_SIZE_SAMPLES(r) (((r)*BLK_PERIOD_US) / 1000000)
/* Increment sample FIFO index by one block */
#define NEXT_IDX(i) (((i) < (FIFO_NUM_BLKS - 1)) ? ((i) + 1) : 0)
#define NEXT_IDX(i) (((i) < (FIFO_NUM_BLKS - 1)) ? ((i) + 1) : 0)
/* Decrement sample FIFO index by one block */
#define PREV_IDX(i) (((i) > 0) ? ((i)-1) : (FIFO_NUM_BLKS - 1))
#define PREV_IDX(i) (((i) > 0) ? ((i)-1) : (FIFO_NUM_BLKS - 1))

#define NUM_BLKS_IN_FRAME NUM_BLKS(CONFIG_AUDIO_FRAME_DURATION_US)
#define BLK_MONO_NUM_SAMPS BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ)
#define BLK_STEREO_NUM_SAMPS (BLK_MONO_NUM_SAMPS * 2)
#define NUM_BLKS_IN_FRAME NUM_BLKS(CONFIG_AUDIO_FRAME_DURATION_US)
#define BLK_MONO_NUM_SAMPS BLK_SIZE_SAMPLES(CONFIG_AUDIO_SAMPLE_RATE_HZ)
#define BLK_STEREO_NUM_SAMPS (BLK_MONO_NUM_SAMPS * 2)
/* Number of octets in a single audio block */
#define BLK_MONO_SIZE_OCTETS (BLK_MONO_NUM_SAMPS * CONFIG_AUDIO_BIT_DEPTH_OCTETS)
#define BLK_MONO_SIZE_OCTETS (BLK_MONO_NUM_SAMPS * CONFIG_AUDIO_BIT_DEPTH_OCTETS)
#define BLK_STEREO_SIZE_OCTETS (BLK_MONO_SIZE_OCTETS * 2)
/* How many function calls before moving on with drift compensation */
#define DRIFT_COMP_WAITING_CNT (DRIFT_MEAS_PERIOD_US / BLK_PERIOD_US)
Expand All @@ -67,27 +68,27 @@ LOG_MODULE_REGISTER(audio_datapath, CONFIG_AUDIO_DATAPATH_LOG_LEVEL);

/* Audio clock - nRF5340 Analog Phase-Locked Loop (APLL) */
#define APLL_FREQ_CENTER 39854
#define APLL_FREQ_MIN 36834
#define APLL_FREQ_MAX 42874
#define APLL_FREQ_MIN 36834
#define APLL_FREQ_MAX 42874
/* Use nanoseconds to reduce rounding errors */
#define APLL_FREQ_ADJ(t) (-((t)*1000) / 331)

#define DRIFT_MEAS_PERIOD_US 100000
#define DRIFT_ERR_THRESH_LOCK 16
#define DRIFT_MEAS_PERIOD_US 100000
#define DRIFT_ERR_THRESH_LOCK 16
#define DRIFT_ERR_THRESH_UNLOCK 32

/* 3000 us to allow BLE transmission and (host -> HCI -> controller) */
#define JUST_IN_TIME_US (CONFIG_AUDIO_FRAME_DURATION_US - 3000)
#define JUST_IN_TIME_US (CONFIG_AUDIO_FRAME_DURATION_US - 3000)
#define JUST_IN_TIME_THRESHOLD_US 1500

/* How often to print underrun warning */
#define UNDERRUN_LOG_INTERVAL_BLKS 5000

enum drift_comp_state {
DRIFT_STATE_INIT, /* Waiting for data to be received */
DRIFT_STATE_CALIB, /* Calibrate and zero out local delay */
DRIFT_STATE_INIT, /* Waiting for data to be received */
DRIFT_STATE_CALIB, /* Calibrate and zero out local delay */
DRIFT_STATE_OFFSET, /* Adjust I2S offset relative to SDU Reference */
DRIFT_STATE_LOCKED /* Drift compensation locked - Minor corrections */
DRIFT_STATE_LOCKED /* Drift compensation locked - Minor corrections */
};

static const char *const drift_comp_state_names[] = {
Expand All @@ -98,9 +99,9 @@ static const char *const drift_comp_state_names[] = {
};

enum pres_comp_state {
PRES_STATE_INIT, /* Initialize presentation compensation */
PRES_STATE_MEAS, /* Measure presentation delay */
PRES_STATE_WAIT, /* Wait for some time */
PRES_STATE_INIT, /* Initialize presentation compensation */
PRES_STATE_MEAS, /* Measure presentation delay */
PRES_STATE_WAIT, /* Wait for some time */
PRES_STATE_LOCKED /* Presentation compensation locked */
};

Expand Down Expand Up @@ -137,15 +138,15 @@ static struct {
uint32_t current_pres_dly_us;

struct {
enum drift_comp_state state : 8;
enum drift_comp_state state: 8;
uint16_t ctr; /* Count func calls. Used for waiting */
uint32_t meas_start_time_us;
uint32_t center_freq;
bool enabled;
} drift_comp;

struct {
enum pres_comp_state state : 8;
enum pres_comp_state state: 8;
uint16_t ctr; /* Count func calls. Used for collecting data points and waiting */
int32_t sum_err_dly_us;
uint32_t pres_delay_us;
Expand Down Expand Up @@ -554,6 +555,8 @@ static void alt_buffer_free_both(void)
* New I2S RX data is located in rx_buf_released, and is locked into
* the in.fifo message queue.
*/

uint8_t sound_mix_buf[BLK_MONO_SIZE_OCTETS];
static void audio_datapath_i2s_blk_complete(uint32_t frame_start_ts, uint32_t *rx_buf_released,
uint32_t const *tx_buf_released)
{
Expand Down Expand Up @@ -850,18 +853,21 @@ void audio_datapath_stream_out(const uint8_t *buf, size_t size, uint32_t sdu_ref
int ret;
size_t pcm_size;

ret = sw_codec_decode(buf, size, bad_frame, &ctrl_blk.decoded_data, &pcm_size);
ret = sw_codec_decode(buf, size, bad_frame, &ctrl_blk.decoded_data, &pcm_size, true);

if (ret) {
LOG_WRN("SW codec decode error: %d", ret);
}

if (IS_ENABLED(CONFIG_LC3_PLAYBACK) && lc3_playback_is_active()) {
lc3_playback_mix_with_stream(ctrl_blk.decoded_data, pcm_size);
}

if (pcm_size != (BLK_STEREO_SIZE_OCTETS * NUM_BLKS_IN_FRAME)) {
LOG_WRN("Decoded audio has wrong size");
/* Discard frame */
return;
}

/*** Add audio data to FIFO buffer ***/

int32_t num_blks_in_fifo = ctrl_blk.out.prod_blk_idx - ctrl_blk.out.cons_blk_idx;
Expand Down Expand Up @@ -1071,24 +1077,23 @@ static int cmd_audio_pres_comp_disable(const struct shell *shell, size_t argc, c
return 0;
}

SHELL_STATIC_SUBCMD_SET_CREATE(
test_cmd,
SHELL_COND_CMD(CONFIG_SHELL, nrf_tone_start, NULL, "Start local tone from nRF5340",
cmd_i2s_tone_play),
SHELL_COND_CMD(CONFIG_SHELL, nrf_tone_stop, NULL, "Stop local tone from nRF5340",
cmd_i2s_tone_stop),
SHELL_COND_CMD(CONFIG_SHELL, pll_drift_comp_enable, NULL,
"Enable audio PLL auto drift compensation (default)",
cmd_hfclkaudio_drift_comp_enable),
SHELL_COND_CMD(CONFIG_SHELL, pll_drift_comp_disable, NULL,
"Disable audio PLL auto drift compensation",
cmd_hfclkaudio_drift_comp_disable),
SHELL_COND_CMD(CONFIG_SHELL, pll_pres_comp_enable, NULL,
"Enable audio presentation compensation (default)",
cmd_audio_pres_comp_enable),
SHELL_COND_CMD(CONFIG_SHELL, pll_pres_comp_disable, NULL,
"Disable audio presentation compensation",
cmd_audio_pres_comp_disable),
SHELL_SUBCMD_SET_END);
SHELL_STATIC_SUBCMD_SET_CREATE(test_cmd,
SHELL_COND_CMD(CONFIG_SHELL, nrf_tone_start, NULL,
"Start local tone from nRF5340", cmd_i2s_tone_play),
SHELL_COND_CMD(CONFIG_SHELL, nrf_tone_stop, NULL,
"Stop local tone from nRF5340", cmd_i2s_tone_stop),
SHELL_COND_CMD(CONFIG_SHELL, pll_drift_comp_enable, NULL,
"Enable audio PLL auto drift compensation (default)",
cmd_hfclkaudio_drift_comp_enable),
SHELL_COND_CMD(CONFIG_SHELL, pll_drift_comp_disable, NULL,
"Disable audio PLL auto drift compensation",
cmd_hfclkaudio_drift_comp_disable),
SHELL_COND_CMD(CONFIG_SHELL, pll_pres_comp_enable, NULL,
"Enable audio presentation compensation (default)",
cmd_audio_pres_comp_enable),
SHELL_COND_CMD(CONFIG_SHELL, pll_pres_comp_disable, NULL,
"Disable audio presentation compensation",
cmd_audio_pres_comp_disable),
SHELL_SUBCMD_SET_END);

SHELL_CMD_REGISTER(test, &test_cmd, "Test mode commands", NULL);
16 changes: 8 additions & 8 deletions applications/nrf5340_audio/src/audio/audio_system.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@
#include "pcm_stream_channel_modifier.h"
#include "audio_usb.h"
#include "streamctrl.h"
#include "pcm_mix.h"
#include <hal/nrf_gpio.h>

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(audio_system, CONFIG_AUDIO_SYSTEM_LOG_LEVEL);
Expand Down Expand Up @@ -90,7 +92,7 @@ static void audio_headset_configure(void)
}
#endif /* (CONFIG_STREAM_BIDIRECTIONAL) */

sw_codec_cfg.decoder.num_ch = SW_CODEC_MONO;
sw_codec_cfg.decoder.num_ch = IS_ENABLED(CONFIG_LC3_PLAYBACK) ? 2 : SW_CODEC_MONO;
sw_codec_cfg.decoder.enabled = true;
}

Expand Down Expand Up @@ -240,12 +242,11 @@ int audio_decode(void const *const encoded_data, size_t encoded_data_size, bool
}

ret = sw_codec_decode(encoded_data, encoded_data_size, bad_frame, &pcm_raw_data,
&pcm_block_size);
&pcm_block_size, true);
if (ret) {
LOG_ERR("Failed to decode");
return ret;
}

/* Split decoded frame into CONFIG_FIFO_FRAME_SPLIT_NUM blocks */
for (int i = 0; i < CONFIG_FIFO_FRAME_SPLIT_NUM; i++) {
memcpy(tmp_pcm_raw_data[i], (char *)pcm_raw_data + (i * (BLOCK_SIZE_BYTES)),
Expand Down Expand Up @@ -303,11 +304,10 @@ void audio_system_start(void)
sw_codec_cfg.initialized = true;

if (sw_codec_cfg.encoder.enabled && encoder_thread_id == NULL) {
encoder_thread_id =
k_thread_create(&encoder_thread_data, encoder_thread_stack,
CONFIG_ENCODER_STACK_SIZE, (k_thread_entry_t)encoder_thread,
NULL, NULL, NULL,
K_PRIO_PREEMPT(CONFIG_ENCODER_THREAD_PRIO), 0, K_NO_WAIT);
encoder_thread_id = k_thread_create(
&encoder_thread_data, encoder_thread_stack, CONFIG_ENCODER_STACK_SIZE,
(k_thread_entry_t)encoder_thread, NULL, NULL, NULL,
K_PRIO_PREEMPT(CONFIG_ENCODER_THREAD_PRIO), 0, K_NO_WAIT);
ret = k_thread_name_set(encoder_thread_id, "ENCODER");
ERR_CHK(ret);
}
Expand Down
82 changes: 51 additions & 31 deletions applications/nrf5340_audio/src/audio/sw_codec_select.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,17 @@
LOG_MODULE_REGISTER(sw_codec_select, CONFIG_SW_CODEC_SELECT_LOG_LEVEL);

static struct sw_codec_config m_config;
static bool initialized;

bool sw_codec_is_initialized(void)
{
return initialized;
}

int sw_codec_encode(void *pcm_data, size_t pcm_size, uint8_t **encoded_data, size_t *encoded_size)
{
/* Temp storage for split stereo PCM signal */
char pcm_data_mono[AUDIO_CH_NUM][PCM_NUM_BYTES_MONO] = { 0 };
char pcm_data_mono[AUDIO_CH_NUM][PCM_NUM_BYTES_MONO] = {0};
/* Make sure we have enough space for two frames (stereo) */
static uint8_t m_encoded_data[ENC_MAX_FRAME_SIZE * AUDIO_CH_NUM];

Expand Down Expand Up @@ -101,15 +107,15 @@ int sw_codec_encode(void *pcm_data, size_t pcm_size, uint8_t **encoded_data, siz
}

int sw_codec_decode(uint8_t const *const encoded_data, size_t encoded_size, bool bad_frame,
void **decoded_data, size_t *decoded_size)
void **decoded_data, size_t *decoded_size, bool lc3_playback_is_active)
{
if (!m_config.decoder.enabled) {
LOG_ERR("Decoder has not been initialized");
return -ENXIO;
}

int ret;
char pcm_data_mono[PCM_NUM_BYTES_MONO] = { 0 };
char pcm_data_mono[PCM_NUM_BYTES_MONO] = {0};
static char pcm_data_stereo[PCM_NUM_BYTES_STEREO];

size_t pcm_size_stereo = 0;
Expand All @@ -119,18 +125,16 @@ int sw_codec_decode(uint8_t const *const encoded_data, size_t encoded_size, bool
case SW_CODEC_LC3: {
#if (CONFIG_SW_CODEC_LC3)
/* Typically used for right channel if stereo signal */
char pcm_data_mono_right[PCM_NUM_BYTES_MONO] = { 0 };
char pcm_data_mono_right[PCM_NUM_BYTES_MONO] = {0};

switch (m_config.decoder.num_ch) {
case SW_CODEC_MONO: {
if (m_config.decoder.num_ch == SW_CODEC_MONO) {
if (bad_frame && IS_ENABLED(CONFIG_SW_CODEC_OVERRIDE_PLC)) {
memset(pcm_data_mono, 0, PCM_NUM_BYTES_MONO);
pcm_size_session = PCM_NUM_BYTES_MONO;
} else {
ret = sw_codec_lc3_dec_run(encoded_data, encoded_size,
LC3_PCM_NUM_BYTES_MONO, 0, pcm_data_mono,
(uint16_t *)&pcm_size_session,
bad_frame);
ret = sw_codec_lc3_dec_run(
encoded_data, encoded_size, LC3_PCM_NUM_BYTES_MONO, 0,
pcm_data_mono, (uint16_t *)&pcm_size_session, bad_frame);
if (ret) {
return ret;
}
Expand All @@ -146,29 +150,25 @@ int sw_codec_decode(uint8_t const *const encoded_data, size_t encoded_size, bool
if (ret) {
return ret;
}
break;
}
case SW_CODEC_STEREO: {
} else if (m_config.decoder.num_ch == SW_CODEC_STEREO && !lc3_playback_is_active) {
if (bad_frame && IS_ENABLED(CONFIG_SW_CODEC_OVERRIDE_PLC)) {
memset(pcm_data_mono, 0, PCM_NUM_BYTES_MONO);
memset(pcm_data_mono_right, 0, PCM_NUM_BYTES_MONO);
pcm_size_session = PCM_NUM_BYTES_MONO;
} else {
/* Decode left channel */
ret = sw_codec_lc3_dec_run(encoded_data, encoded_size / 2,
LC3_PCM_NUM_BYTES_MONO, AUDIO_CH_L,
pcm_data_mono,
(uint16_t *)&pcm_size_session,
bad_frame);
ret = sw_codec_lc3_dec_run(
encoded_data, encoded_size / 2, LC3_PCM_NUM_BYTES_MONO,
AUDIO_CH_L, pcm_data_mono, (uint16_t *)&pcm_size_session,
bad_frame);
if (ret) {
return ret;
}
/* Decode right channel */
ret = sw_codec_lc3_dec_run((encoded_data + (encoded_size / 2)),
encoded_size / 2, LC3_PCM_NUM_BYTES_MONO,
AUDIO_CH_R, pcm_data_mono_right,
(uint16_t *)&pcm_size_session,
bad_frame);
ret = sw_codec_lc3_dec_run(
(encoded_data + (encoded_size / 2)), encoded_size / 2,
LC3_PCM_NUM_BYTES_MONO, AUDIO_CH_R, pcm_data_mono_right,
(uint16_t *)&pcm_size_session, bad_frame);
if (ret) {
return ret;
}
Expand All @@ -179,13 +179,33 @@ int sw_codec_decode(uint8_t const *const encoded_data, size_t encoded_size, bool
if (ret) {
return ret;
}
break;
}
default:
} else if (m_config.decoder.num_ch == SW_CODEC_STEREO && lc3_playback_is_active) {
if (bad_frame && IS_ENABLED(CONFIG_SW_CODEC_OVERRIDE_PLC)) {
memset(pcm_data_mono, 0, PCM_NUM_BYTES_MONO);
pcm_size_session = PCM_NUM_BYTES_MONO;
} else {
ret = sw_codec_lc3_dec_run(
encoded_data, encoded_size, LC3_PCM_NUM_BYTES_MONO, 0,
pcm_data_mono, (uint16_t *)&pcm_size_session, bad_frame);
if (ret) {
return ret;
}
}

/* For now, i2s is only stereo, so in order to send
* just one channel, we need to insert 0 for the
* other channel
*/
ret = pscm_zero_pad(pcm_data_mono, pcm_size_session,
m_config.decoder.audio_ch, CONFIG_AUDIO_BIT_DEPTH_BITS,
pcm_data_stereo, &pcm_size_stereo);
if (ret) {
return ret;
}
} else {
LOG_ERR("Unsupported number of channels: %d", m_config.encoder.num_ch);
return -ENODEV;
}

*decoded_size = pcm_size_stereo;
*decoded_data = pcm_data_stereo;
#endif /* (CONFIG_SW_CODEC_LC3) */
Expand Down Expand Up @@ -288,10 +308,9 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg)
CONFIG_AUDIO_SAMPLE_RATE_HZ, CONFIG_AUDIO_BIT_DEPTH_BITS,
CONFIG_AUDIO_FRAME_DURATION_US, sw_codec_cfg.decoder.num_ch);

ret = sw_codec_lc3_dec_init(CONFIG_AUDIO_SAMPLE_RATE_HZ,
CONFIG_AUDIO_BIT_DEPTH_BITS,
CONFIG_AUDIO_FRAME_DURATION_US,
sw_codec_cfg.decoder.num_ch);
ret = sw_codec_lc3_dec_init(
CONFIG_AUDIO_SAMPLE_RATE_HZ, CONFIG_AUDIO_BIT_DEPTH_BITS,
CONFIG_AUDIO_FRAME_DURATION_US, sw_codec_cfg.decoder.num_ch);

if (ret) {
return ret;
Expand All @@ -308,5 +327,6 @@ int sw_codec_init(struct sw_codec_config sw_codec_cfg)
}

m_config = sw_codec_cfg;
initialized = true;
return 0;
}
Loading

0 comments on commit 8eef146

Please sign in to comment.