// SPDX-License-Identifier: BSD-3-Clause

//

// Copyright(c) 2016 Intel Corporation. All rights reserved.

//

// Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>

// Liam Girdwood <liam.r.girdwood@linux.intel.com>

// Keyon Jie <yang.jie@linux.intel.com>

#include <sof/audio/buffer.h>

#include <sof/audio/component.h>

#include <sof/audio/format.h>

#include <sof/audio/pipeline.h>

#include <sof/audio/ipc-config.h>

#include <sof/common.h>

#include <rtos/panic.h>

#include <sof/ipc/msg.h>

#include <rtos/alloc.h>

#include <rtos/init.h>

#include <sof/lib/memory.h>

#include <sof/lib/uuid.h>

#include <sof/list.h>

#include <sof/math/trig.h>

#include <sof/platform.h>

#include <rtos/string.h>

#include <sof/trace/trace.h>

#include <sof/ut.h>

#include <ipc/control.h>

#include <ipc/stream.h>

#include <ipc/topology.h>

#include <user/tone.h>

#include <user/trace.h>

#include <errno.h>

#include <stdbool.h>

#include <stddef.h>

#include <stdint.h>

/* Convert float frequency in Hz to Q16.16 fractional format */

#define TONE_FREQ(f) Q_CONVERT_FLOAT(f, 16)

/* Convert float gain to Q1.31 fractional format */

#define TONE_GAIN(v) Q_CONVERT_FLOAT(v, 31)

/* Set default tone amplitude and frequency */

#define TONE_AMPLITUDE_DEFAULT TONE_GAIN(0.1) /* -20 dB */

#define TONE_FREQUENCY_DEFAULT TONE_FREQ(997.0)

#define TONE_NUM_FS 13 /* Table size for 8-192 kHz range */

static const struct comp_driver comp_tone;

LOG_MODULE_REGISTER(tone, CONFIG_SOF_LOG_LEVEL);

/* 04e3f894-2c5c-4f2e-8dc1-694eeaab53fa */

DECLARE_SOF_RT_UUID("tone", tone_uuid, 0x04e3f894, 0x2c5c, 0x4f2e,

0x8d, 0xc1, 0x69, 0x4e, 0xea, 0xab, 0x53, 0xfa);

DECLARE_TR_CTX(tone_tr, SOF_UUID(tone_uuid), LOG_LEVEL_INFO);

/* 2*pi/Fs lookup tables in Q1.31 for each Fs */

static const int32_t tone_fs_list[TONE_NUM_FS] = {

8000, 11025, 16000, 22050, 24000, 32000, 44100, 48000,

64000, 88200, 96000, 176400, 192000

};

static const int32_t tone_pi2_div_fs[TONE_NUM_FS] = {

1686630, 1223858, 843315, 611929, 562210, 421657, 305965,

281105, 210829, 152982, 140552, 76491, 70276

};

/* tone component private data */

struct tone_state {

int mute;

int32_t a; /* Current amplitude Q1.31 */

int32_t a_target; /* Target amplitude Q1.31 */

int32_t ampl_coef; /* Amplitude multiplier Q2.30 */

int32_t c; /* Coefficient 2*pi/Fs Q1.31 */

int32_t f; /* Frequency Q16.16 */

int32_t freq_coef; /* Frequency multiplier Q2.30 */

int32_t fs; /* Sample rate in Hertz Q32.0 */

int32_t ramp_step; /* Amplitude ramp step Q1.31 */

int32_t w; /* Angle radians Q4.28 */

int32_t w_step; /* Angle step Q4.28 */

uint32_t block_count;

uint32_t repeat_count;

uint32_t repeats; /* Number of repeats for tone (sweep steps) */

uint32_t sample_count;

uint32_t samples_in_block; /* Samples in 125 us block */

uint32_t tone_length; /* Active length in 125 us blocks */

uint32_t tone_period; /* Active + idle time in 125 us blocks */

};

struct comp_data {

uint32_t period_bytes;

uint32_t channels;

uint32_t frame_bytes;

uint32_t rate;

struct tone_state sg[PLATFORM_MAX_CHANNELS];

void (*tone_func)(struct comp_dev *dev, struct audio_stream *sink,

uint32_t frames);

};

static int32_t tonegen(struct tone_state *sg);

static void tonegen_control(struct tone_state *sg);

static void tonegen_update_f(struct tone_state *sg, int32_t f);

/*

* Tone generator algorithm code

*/

static inline void tone_circ_inc_wrap(int32_t **ptr, int32_t *end, size_t size)

{

if (*ptr >= end)

*ptr = (int32_t *)((size_t)*ptr - size);

}

static void tone_s32_default(struct comp_dev *dev, struct audio_stream *sink,

uint32_t frames)

{

struct comp_data *cd = comp_get_drvdata(dev);

int32_t *dest = audio_stream_get_wptr(sink);

int i;

int n;

int n_wrap_dest;

int n_min;

int nch = cd->channels;

n = frames * nch;

while (n > 0) {

n_wrap_dest = (int32_t *)audio_stream_get_end_addr(sink) - dest;

n_min = (n < n_wrap_dest) ? n : n_wrap_dest;

/* Process until wrap or completed n */

while (n_min > 0) {

n -= nch;

n_min -= nch;

for (i = 0; i < nch; i++) {

tonegen_control(&cd->sg[i]);

*dest = tonegen(&cd->sg[i]);

dest++;

}

}

tone_circ_inc_wrap(&dest, audio_stream_get_end_addr(sink),

audio_stream_get_size(sink));

}

}

static int32_t tonegen(struct tone_state *sg)

{

int64_t sine;

int64_t w;

/* sg->w is angle in Q4.28 radians format, sin() returns Q1.31 */

/* sg->a is amplitude as Q1.31 */

sine =

q_mults_32x32(sin_fixed_32b(sg->w), sg->a,

Q_SHIFT_BITS_64(31, 31, 31));

/* Next point */

w = (int64_t)sg->w + sg->w_step;

sg->w = (w > PI_MUL2_Q4_28)

? (int32_t)(w - PI_MUL2_Q4_28) : (int32_t)w;

if (sg->mute)

return 0;

else

return (int32_t)sine; /* Q1.31 no saturation need */

}

static void tonegen_control(struct tone_state *sg)

{

int64_t a;

int64_t p;

/* Count samples, 125 us blocks */

sg->sample_count++;

if (sg->sample_count < sg->samples_in_block)

return;

sg->sample_count = 0;

if (sg->block_count < INT32_MAX)

sg->block_count++;

/* Fade-in ramp during tone */

if (sg->block_count < sg->tone_length) {

if (sg->a == 0)

sg->w = 0; /* Reset phase to have less clicky ramp */

if (sg->a > sg->a_target) {

a = (int64_t)sg->a - sg->ramp_step;

if (a < sg->a_target)

a = sg->a_target;

} else {

a = (int64_t)sg->a + sg->ramp_step;

if (a > sg->a_target)

a = sg->a_target;

}

sg->a = (int32_t)a;

}

/* Fade-out ramp after tone*/

if (sg->block_count > sg->tone_length) {

a = (int64_t)sg->a - sg->ramp_step;

if (a < 0)

a = 0;

sg->a = (int32_t)a;

}

/* New repeated tone, update for frequency or amplitude sweep */

if ((sg->block_count > sg->tone_period) &&

(sg->repeat_count + 1 < sg->repeats)) {

sg->block_count = 0;

if (sg->ampl_coef > 0) {

sg->a_target =

sat_int32(q_multsr_32x32(sg->a_target,

sg->ampl_coef, Q_SHIFT_BITS_64(31, 30, 31)));

sg->a = (sg->ramp_step > sg->a_target)

? sg->a_target : sg->ramp_step;

}

if (sg->freq_coef > 0) {

/* f is Q16.16, freq_coef is Q2.30 */

p = q_multsr_32x32(sg->f, sg->freq_coef,

Q_SHIFT_BITS_64(16, 30, 16));

tonegen_update_f(sg, (int32_t)p); /* No saturation */

}

sg->repeat_count++;

}

}

/* Set sine amplitude */

static inline void tonegen_set_a(struct tone_state *sg, int32_t a)

{

sg->a_target = a;

}

/* Repeated number of beeps */

static void tonegen_set_repeats(struct tone_state *sg, uint32_t r)

{

sg->repeats = r;

}

/* The next functions support zero as shortcut for defaults to get

* make a nicer API without need to remember the neutral steady

* non-swept tone settings.

*/

/* Multiplication factor for frequency as Q2.30 for logarithmic change */

static void tonegen_set_freq_mult(struct tone_state *sg, int32_t fm)

{

sg->freq_coef = (fm > 0) ? fm : ONE_Q2_30; /* Set freq mult to 1.0 */

}

/* Multiplication factor for amplitude as Q2.30 for logarithmic change */

static void tonegen_set_ampl_mult(struct tone_state *sg, int32_t am)

{

sg->ampl_coef = (am > 0) ? am : ONE_Q2_30; /* Set ampl mult to 1.0 */

}

/* Tone length in samples, this is the active length of tone */

static void tonegen_set_length(struct tone_state *sg, uint32_t tl)

{

sg->tone_length = (tl > 0) ? tl : INT32_MAX; /* Count rate 125 us */

}

/* Tone period in samples, this is the length including the pause after beep */

static void tonegen_set_period(struct tone_state *sg, uint32_t tp)

{

sg->tone_period = (tp > 0) ? tp : INT32_MAX; /* Count rate 125 us */

}

/* Tone ramp parameters:

* step - Value that is added or subtracted to amplitude. A zero or negative

* number disables the ramp and amplitude is immediately modified to

* final value.

*/

static inline void tonegen_set_linramp(struct tone_state *sg, int32_t step)

{

sg->ramp_step = (step > 0) ? step : INT32_MAX;

}

static inline int32_t tonegen_get_f(struct tone_state *sg)

{

return sg->f;

}

static inline int32_t tonegen_get_a(struct tone_state *sg)

{

return sg->a_target;

}

static inline void tonegen_mute(struct tone_state *sg)

{

sg->mute = 1;

}

static inline void tonegen_unmute(struct tone_state *sg)

{

sg->mute = 0;

}

static void tonegen_update_f(struct tone_state *sg, int32_t f)

{

int64_t w_tmp;

int64_t f_max;

/* Calculate Fs/2, fs is Q32.0, f is Q16.16 */

f_max = Q_SHIFT_LEFT((int64_t)sg->fs, 0, 16 - 1);

f_max = (f_max > INT32_MAX) ? INT32_MAX : f_max;

sg->f = (f > f_max) ? f_max : f;

/* Q16 x Q31 -> Q28 */

w_tmp = q_multsr_32x32(sg->f, sg->c, Q_SHIFT_BITS_64(16, 31, 28));

w_tmp = (w_tmp > PI_Q4_28) ? PI_Q4_28 : w_tmp; /* Limit to pi Q4.28 */

sg->w_step = (int32_t)w_tmp;

}

static void tonegen_reset(struct tone_state *sg)

{

sg->mute = 1;

sg->a = 0;

sg->a_target = TONE_AMPLITUDE_DEFAULT;

sg->c = 0;

sg->f = TONE_FREQUENCY_DEFAULT;

sg->w = 0;

sg->w_step = 0;

sg->block_count = 0;

sg->repeat_count = 0;

sg->repeats = 0;

sg->sample_count = 0;

sg->samples_in_block = 0;

/* Continuous tone */

sg->freq_coef = ONE_Q2_30; /* Set freq multiplier to 1.0 */

sg->ampl_coef = ONE_Q2_30; /* Set ampl multiplier to 1.0 */

sg->tone_length = INT32_MAX;

sg->tone_period = INT32_MAX;

sg->ramp_step = ONE_Q1_31; /* Set lin ramp modification to max */

}

static int tonegen_init(struct tone_state *sg, int32_t fs, int32_t f, int32_t a)

{

int idx;

int i;

sg->a_target = a;

sg->a = (sg->ramp_step > sg->a_target) ? sg->a_target : sg->ramp_step;

idx = -1;

sg->mute = 1;

sg->fs = 0;

/* Find index of current sample rate and then get from lookup table the

* corresponding 2*pi/Fs value.

*/

for (i = 0; i < TONE_NUM_FS; i++) {

if (fs == tone_fs_list[i])

idx = i;

}

if (idx < 0) {

sg->w_step = 0;

return -EINVAL;

}

sg->fs = fs;

sg->c = tone_pi2_div_fs[idx]; /* Store 2*pi/Fs */

sg->mute = 0;

tonegen_update_f(sg, f);

/* 125us as Q1.31 is 268435, calculate fs * 125e-6 in Q31.0 */

sg->samples_in_block =

(int32_t) q_multsr_32x32(fs, 268435, Q_SHIFT_BITS_64(0, 31, 0));

return 0;

}

/*

* End of algorithm code. Next the standard component methods.

*/

static struct comp_dev *tone_new(const struct comp_driver *drv,

const struct comp_ipc_config *config,

const void *spec)

{

struct comp_dev *dev;

const struct ipc_config_tone *ipc_tone = spec;

struct comp_data *cd;

int i;

comp_cl_info(&comp_tone, "tone_new()");

dev = comp_alloc(drv, sizeof(*dev));

if (!dev)

return NULL;

dev->ipc_config = *config;

cd = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, SOF_MEM_CAPS_RAM, sizeof(*cd));

if (!cd) {

rfree(dev);

return NULL;

}

comp_set_drvdata(dev, cd);

cd->tone_func = tone_s32_default;

cd->rate = ipc_tone->sample_rate;

/* Reset tone generator and set channels volumes to default */

for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)

tonegen_reset(&cd->sg[i]);

dev->state = COMP_STATE_READY;

return dev;

}

static void tone_free(struct comp_dev *dev)

{

struct tone_data *td = comp_get_drvdata(dev);

comp_info(dev, "tone_free()");

rfree(td);

rfree(dev);

}

/* set component audio stream parameters */

static int tone_params(struct comp_dev *dev,

struct sof_ipc_stream_params *params)

{

struct comp_data *cd = comp_get_drvdata(dev);

struct comp_buffer *sourceb, *sinkb;

sourceb = list_first_item(&dev->bsource_list, struct comp_buffer,

sink_list);

sinkb = list_first_item(&dev->bsink_list, struct comp_buffer,

source_list);

comp_info(dev, "tone_params(), config->frame_fmt = %u",

dev->ipc_config.frame_fmt);

/* Tone supports only S32_LE PCM format atm */

if (dev->ipc_config.frame_fmt != SOF_IPC_FRAME_S32_LE)

return -EINVAL;

audio_stream_set_frm_fmt(&sourceb->stream, dev->ipc_config.frame_fmt);

audio_stream_set_frm_fmt(&sinkb->stream, dev->ipc_config.frame_fmt);

/* calculate period size based on config */

cd->period_bytes = dev->frames *

audio_stream_frame_bytes(&sourceb->stream);

return 0;

}

static int tone_cmd_get_value(struct comp_dev *dev,

struct sof_ipc_ctrl_data *cdata, int max_size)

{

struct comp_data *cd = comp_get_drvdata(dev);

int j;

comp_info(dev, "tone_cmd_get_value()");

if (cdata->type != SOF_CTRL_TYPE_VALUE_CHAN_GET) {

comp_err(dev, "tone_cmd_get_value(): wrong cdata->type: %u",

cdata->type);

return -EINVAL;

}

if (cdata->cmd == SOF_CTRL_CMD_SWITCH) {

for (j = 0; j < cdata->num_elems; j++) {

cdata->chanv[j].channel = j;

cdata->chanv[j].value = !cd->sg[j].mute;

comp_info(dev, "tone_cmd_get_value(), j = %u, cd->sg[j].mute = %u",

j, cd->sg[j].mute);

}

}

return 0;

}

static int tone_cmd_set_value(struct comp_dev *dev,

struct sof_ipc_ctrl_data *cdata)

{

struct comp_data *cd = comp_get_drvdata(dev);

int j;

uint32_t ch;

bool val;

if (cdata->type != SOF_CTRL_TYPE_VALUE_CHAN_SET) {

comp_err(dev, "tone_cmd_set_value(): wrong cdata->type: %u",

cdata->type);

return -EINVAL;

}

if (cdata->cmd == SOF_CTRL_CMD_SWITCH) {

comp_info(dev, "tone_cmd_set_value(), SOF_CTRL_CMD_SWITCH");

for (j = 0; j < cdata->num_elems; j++) {

ch = cdata->chanv[j].channel;

val = cdata->chanv[j].value;

comp_info(dev, "tone_cmd_set_value(), SOF_CTRL_CMD_SWITCH, ch = %u, val = %u",

ch, val);

if (ch >= PLATFORM_MAX_CHANNELS) {

comp_err(dev, "tone_cmd_set_value(): ch >= PLATFORM_MAX_CHANNELS");

return -EINVAL;

}

if (val)

tonegen_unmute(&cd->sg[ch]);

else

tonegen_mute(&cd->sg[ch]);

}

} else {

comp_err(dev, "tone_cmd_set_value(): invalid cdata->cmd");

return -EINVAL;

}

return 0;

}

static int tone_cmd_set_data(struct comp_dev *dev,

struct sof_ipc_ctrl_data *cdata)

{

struct comp_data *cd = comp_get_drvdata(dev);

struct sof_ipc_ctrl_value_comp *compv;

int i;

uint32_t ch;

uint32_t val;

comp_info(dev, "tone_cmd_set_data()");

if (cdata->type != SOF_CTRL_TYPE_VALUE_COMP_SET) {

comp_err(dev, "tone_cmd_set_data(): wrong cdata->type: %u",

cdata->type);

return -EINVAL;

}

switch (cdata->cmd) {

case SOF_CTRL_CMD_ENUM:

comp_info(dev, "tone_cmd_set_data(), SOF_CTRL_CMD_ENUM, cdata->index = %u",

cdata->index);

compv = (struct sof_ipc_ctrl_value_comp *)ASSUME_ALIGNED(&cdata->data->data, 4);

for (i = 0; i < (int)cdata->num_elems; i++) {

ch = compv[i].index;

val = compv[i].svalue;

comp_info(dev, "tone_cmd_set_data(), SOF_CTRL_CMD_ENUM, ch = %u, val = %u",

ch, val);

switch (cdata->index) {

case SOF_TONE_IDX_FREQUENCY:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_FREQUENCY");

tonegen_update_f(&cd->sg[ch], val);

break;

case SOF_TONE_IDX_AMPLITUDE:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_AMPLITUDE");

tonegen_set_a(&cd->sg[ch], val);

break;

case SOF_TONE_IDX_FREQ_MULT:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_FREQ_MULT");

tonegen_set_freq_mult(&cd->sg[ch], val);

break;

case SOF_TONE_IDX_AMPL_MULT:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_AMPL_MULT");

tonegen_set_ampl_mult(&cd->sg[ch], val);

break;

case SOF_TONE_IDX_LENGTH:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_LENGTH");

tonegen_set_length(&cd->sg[ch], val);

break;

case SOF_TONE_IDX_PERIOD:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_PERIOD");

tonegen_set_period(&cd->sg[ch], val);

break;

case SOF_TONE_IDX_REPEATS:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_REPEATS");

tonegen_set_repeats(&cd->sg[ch], val);

break;

case SOF_TONE_IDX_LIN_RAMP_STEP:

comp_info(dev, "tone_cmd_set_data(), SOF_TONE_IDX_LIN_RAMP_STEP");

tonegen_set_linramp(&cd->sg[ch], val);

break;

default:

comp_err(dev, "tone_cmd_set_data(): invalid cdata->index");

return -EINVAL;

}

}

break;

default:

comp_err(dev, "tone_cmd_set_data(): invalid cdata->cmd");

return -EINVAL;

}

return 0;

}

/* used to pass standard and bespoke commands (with data) to component */

static int tone_cmd(struct comp_dev *dev, int cmd, void *data,

int max_data_size)

{

struct sof_ipc_ctrl_data *cdata = ASSUME_ALIGNED(data, 4);

int ret = 0;

comp_info(dev, "tone_cmd()");

switch (cmd) {

case COMP_CMD_SET_DATA:

ret = tone_cmd_set_data(dev, cdata);

break;

case COMP_CMD_SET_VALUE:

ret = tone_cmd_set_value(dev, cdata);

break;

case COMP_CMD_GET_VALUE:

ret = tone_cmd_get_value(dev, cdata, max_data_size);

break;

}

return ret;

}

static int tone_trigger(struct comp_dev *dev, int cmd)

{

comp_info(dev, "tone_trigger()");

return comp_set_state(dev, cmd);

}

/* copy and process stream data from source to sink buffers */

static int tone_copy(struct comp_dev *dev)

{

struct comp_buffer *sink;

struct comp_data *cd = comp_get_drvdata(dev);

uint32_t free;

int ret = 0;

comp_dbg(dev, "tone_copy()");

/* tone component sink buffer */

sink = list_first_item(&dev->bsink_list, struct comp_buffer,

source_list);

free = audio_stream_get_free_bytes(&sink->stream);

/* Test that sink has enough free frames. Then run once to maintain

* low latency and steady load for tones.

*/

if (free >= cd->period_bytes) {

/* create tone */

cd->tone_func(dev, &sink->stream, dev->frames);

buffer_stream_writeback(sink, cd->period_bytes);

/* calc new free and available */

comp_update_buffer_produce(sink, cd->period_bytes);

ret = dev->frames;

}

return ret;

}

static int tone_prepare(struct comp_dev *dev)

{

struct comp_data *cd = comp_get_drvdata(dev);

struct comp_buffer *sourceb;

int32_t f;

int32_t a;

int ret;

int i;

comp_info(dev, "tone_prepare()");

ret = comp_set_state(dev, COMP_TRIGGER_PREPARE);

if (ret < 0)

return ret;

if (ret == COMP_STATUS_STATE_ALREADY_SET)

return PPL_STATUS_PATH_STOP;

sourceb = list_first_item(&dev->bsource_list, struct comp_buffer,

sink_list);

cd->channels = audio_stream_get_channels(&sourceb->stream);

comp_info(dev, "tone_prepare(), cd->channels = %u, cd->rate = %u",

cd->channels, cd->rate);

for (i = 0; i < cd->channels; i++) {

f = tonegen_get_f(&cd->sg[i]);

a = tonegen_get_a(&cd->sg[i]);

if (tonegen_init(&cd->sg[i], cd->rate, f, a) < 0) {

comp_set_state(dev, COMP_TRIGGER_RESET);

return -EINVAL;

}

}

return 0;

}

static int tone_reset(struct comp_dev *dev)

{

struct comp_data *cd = comp_get_drvdata(dev);

int i;

comp_info(dev, "tone_reset()");

/* Initialize with the defaults */

for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)

tonegen_reset(&cd->sg[i]);

comp_set_state(dev, COMP_TRIGGER_RESET);

return 0;

}

static const struct comp_driver comp_tone = {

.type = SOF_COMP_TONE,

.uid = SOF_RT_UUID(tone_uuid),

.tctx = &tone_tr,

.ops = {

.create = tone_new,

.free = tone_free,

.params = tone_params,

.cmd = tone_cmd,

.trigger = tone_trigger,

.copy = tone_copy,

.prepare = tone_prepare,

.reset = tone_reset,

},

};

static SHARED_DATA struct comp_driver_info comp_tone_info = {

.drv = &comp_tone,

};

UT_STATIC void sys_comp_tone_init(void)

{

comp_register(platform_shared_get(&comp_tone_info,

sizeof(comp_tone_info)));

}

DECLARE_MODULE(sys_comp_tone_init);

SOF_MODULE_INIT(tone, sys_comp_tone_init);