- Sound-open-firmware - mailman.alsa-project.org

[Sound-open-firmware] [PATCH V2] SRC: Use a multi-channel FIR core to optimize speed plus other cleanup
by Seppo Ingalsuo 20 Oct '17

20 Oct '17

This patch changes the sample rate conversion processing to use a single multi-channel filter instead of per channel called mono filter instances. The filter output is now rounded with 1/2 LSB add. The polyphase filter input input block sizes are multiplied in SRC initialization to reach near to or exacly the period length to reduce polyphase filter call overhead. The polyphase filter is now called via function pointer for more flexibility with audio data formats. Currently S32_LE and S24_4LE are supported. Code cleanup includes removal of redundant variables and some debug print code. Signed-off-by: Seppo Ingalsuo <seppo.ingalsuo(a)linux.intel.com> --- src/audio/src.c | 200 +++++++++++------------ src/audio/src_core.c | 448 +++++++++++++++++++++++++++------------------------ src/audio/src_core.h | 42 +---- 3 files changed, 329 insertions(+), 361 deletions(-) diff --git a/src/audio/src.c b/src/audio/src.c index 4c4bc6e..9fe81d3 100644 --- a/src/audio/src.c +++ b/src/audio/src.c @@ -55,7 +55,7 @@ /* src component private data */ struct comp_data { - struct polyphase_src src[PLATFORM_MAX_CHANNELS]; + struct polyphase_src src; struct src_param param; int32_t *delay_lines; uint32_t sink_rate; @@ -63,16 +63,16 @@ struct comp_data { int32_t *sbuf_w_ptr; int32_t *sbuf_r_ptr; int sbuf_avail; - int sign_extend_s24; /* Set if need to copy sign bit to b24..b31 */ void (* src_func)(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *consumed, size_t *produced); + void (* polyphase_func)(struct src_stage_prm *s); }; /* Fallback function */ -static void fallback_s32(struct comp_dev *dev, struct comp_buffer *source, +static void src_fallback(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written) { *bytes_read = 0; @@ -84,101 +84,110 @@ static void src_2s_s32_default(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written) { - struct polyphase_src *s; struct src_stage_prm s1; struct src_stage_prm s2; - int j; + int s1_blk_in; + int s1_blk_out; + int s2_blk_in; + int s2_blk_out; struct comp_data *cd = comp_get_drvdata(dev); int32_t *dest = (int32_t *) sink->w_ptr; int32_t *src = (int32_t *) source->r_ptr; - int32_t *sbuf_addr = cd->delay_lines; int32_t *sbuf_end_addr = &cd->delay_lines[cd->param.sbuf_length]; int32_t sbuf_size = cd->param.sbuf_length * sizeof(int32_t); - const int nch = dev->params.channels; - const int s1_blk_in = cd->src[0].stage1->blk_in * nch; - const int s1_blk_out = cd->src[0].stage1->blk_out * nch; - const int s2_blk_in = cd->src[0].stage2->blk_in * nch; - const int s2_blk_out = cd->src[0].stage2->blk_out * nch; + int nch = dev->params.channels; int sbuf_free = cd->param.sbuf_length - cd->sbuf_avail; - int source_check = (source->avail >> 2) - s1_blk_in; /* for int32_t */ - int sink_check = (sink->free >> 2) - s2_blk_out; /* For int32_t */ int n_read = 0; int n_written = 0; int n1 = 0; int n2 = 0; + int avail_b = source->avail; + int free_b = sink->free; + int sz = sizeof(int32_t); - s1.times = 1; s1.x_end_addr = source->end_addr; s1.x_size = source->size; - s1.x_inc = nch; s1.y_end_addr = sbuf_end_addr; s1.y_size = sbuf_size; - s1.y_inc = nch; + s1.state = &cd->src.state1; + s1.stage = cd->src.stage1; + s1.x_rptr = src; + s1.y_wptr = cd->sbuf_w_ptr; + s1.nch = nch; - s2.times = 1; s2.x_end_addr = sbuf_end_addr; s2.x_size = sbuf_size; - s2.x_inc = nch; s2.y_end_addr = sink->end_addr; s2.y_size = sink->size; - s2.y_inc = nch; + s2.state = &cd->src.state2; + s2.stage = cd->src.stage2; + s2.x_rptr = cd->sbuf_r_ptr; + s2.y_wptr = dest; + s2.nch = nch; + - /* 1st stage runs once a long multiplied length block. - * The stage buffer much be large enough to fit one s1 output block - * plus one s2 input block plus jitter in s2 consumption. + /* Test if 1st stage can be run with default block length to reach + * the period length or just under it. */ - while ((n1 < cd->param.stage1_times_max) - && (n_read <= source_check) - && (sbuf_free >= s1_blk_out)) { - for (j = 0; j < nch; j++) { - s = &cd->src[j]; /* Point to src[] for this channel */ - s1.x_rptr = src++; - s1.y_wptr = cd->sbuf_w_ptr++; - src_circ_inc_wrap(&src, source->end_addr, source->size); - src_circ_inc_wrap(&cd->sbuf_w_ptr, sbuf_end_addr, sbuf_size); - s1.state = &s->state1; - s1.stage = s->stage1; - if (cd->sign_extend_s24) - src_polyphase_stage_cir_s24(&s1); - else - src_polyphase_stage_cir(&s1); - - } + s1.times = cd->param.stage1_times; + s1_blk_in = s1.times * cd->src.stage1->blk_in * nch; + s1_blk_out = s1.times * cd->src.stage1->blk_out * nch; + if ((avail_b >= s1_blk_in * sz) && (sbuf_free >= s1_blk_out)) { + cd->polyphase_func(&s1); + + cd->sbuf_w_ptr = s1.y_wptr; + cd->sbuf_avail += s1_blk_out; n_read += s1_blk_in; + avail_b -= s1_blk_in * sz; + sbuf_free -= s1_blk_out; + n1 = s1.times; + } + + /* Run one block at time the remaining data for 1st stage. */ + s1.times = 1; + s1_blk_in = cd->src.stage1->blk_in * nch; + s1_blk_out = cd->src.stage1->blk_out * nch; + while ((n1 < cd->param.stage1_times_max) && (avail_b >= s1_blk_in * sz) + && (sbuf_free >= s1_blk_out)) { + cd->polyphase_func(&s1); + + cd->sbuf_w_ptr = s1.y_wptr; cd->sbuf_avail += s1_blk_out; + n_read += s1_blk_in; + avail_b -= s1_blk_in * sz; sbuf_free -= s1_blk_out; - src = s1.x_rptr - nch + 1; - cd->sbuf_w_ptr = s1.y_wptr - nch + 1; - src_circ_dec_wrap(&src, source->addr, source->size); - src_circ_dec_wrap(&cd->sbuf_w_ptr, sbuf_addr, sbuf_size); - n1++; + n1 += s1.times; + } + + /* Test if 2nd stage can be run with default block length. */ + s2.times = cd->param.stage2_times; + s2_blk_in = s2.times * cd->src.stage2->blk_in * nch; + s2_blk_out = s2.times * cd->src.stage2->blk_out * nch; + if ((cd->sbuf_avail >= s2_blk_in) && (free_b >= s2_blk_out * sz)) { + cd->polyphase_func(&s2); + + cd->sbuf_r_ptr = s2.x_rptr; + cd->sbuf_avail -= s2_blk_in; + free_b -= s2_blk_out * sz; + n_written += s2_blk_out; + n2 = s2.times; } - /* 2nd stage runs as many min size blocks as buffers allow */ + + /* Run one block at time the remaining 2nd stage output */ + s2.times = 1; + s2_blk_in = cd->src.stage2->blk_in * nch; + s2_blk_out = cd->src.stage2->blk_out * nch; while ((n2 < cd->param.stage2_times_max) && (cd->sbuf_avail >= s2_blk_in) - && (n_written <= sink_check)) { - for (j = 0; j < nch; j++) { - s2.x_rptr = cd->sbuf_r_ptr++; - s2.y_wptr = dest++; - src_circ_inc_wrap(&cd->sbuf_r_ptr, sbuf_end_addr, sbuf_size); - src_circ_inc_wrap(&dest, sink->end_addr, sink->size); - s = &cd->src[j]; /* Point to src[] for this channel */ - s2.state = &s->state2; - s2.stage = s->stage2; - if (cd->sign_extend_s24) - src_polyphase_stage_cir_s24(&s2); - else - src_polyphase_stage_cir(&s2); - - } - cd->sbuf_r_ptr = s2.x_rptr - nch + 1; - dest = s2.y_wptr - nch + 1; - src_circ_dec_wrap(&cd->sbuf_r_ptr, sbuf_addr, sbuf_size); - src_circ_dec_wrap(&dest, sink->addr, sink->size); - n_written += s2_blk_out; + && (free_b >= s2_blk_out * sz)) { + cd->polyphase_func(&s2); + + cd->sbuf_r_ptr = s2.x_rptr; cd->sbuf_avail -= s2_blk_in; - n2++; + free_b -= s2_blk_out * sz; + n_written += s2_blk_out; + n2 += s2.times; } *bytes_read = sizeof(int32_t) * n_read; *bytes_written = sizeof(int32_t) * n_written; @@ -189,42 +198,27 @@ static void src_1s_s32_default(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written) { - struct polyphase_src *s; struct src_stage_prm s1; - int j; struct comp_data *cd = comp_get_drvdata(dev); - int32_t *dest = (int32_t *) sink->w_ptr; - int32_t *src = (int32_t *) source->r_ptr; int nch = dev->params.channels; int n_read = 0; int n_written = 0; s1.times = cd->param.stage1_times; + s1.x_rptr = (int32_t *) source->r_ptr; s1.x_end_addr = source->end_addr; s1.x_size = source->size; - s1.x_inc = nch; + s1.y_wptr = (int32_t *) sink->w_ptr; s1.y_end_addr = sink->end_addr; s1.y_size = sink->size; - s1.y_inc = nch; - s1.x_rptr = src + nch - 1; - s1.y_wptr = dest + nch - 1; - - for (j = 0; j < nch; j++) { - s = &cd->src[j]; /* Point to src for this channel */ - s1.x_rptr = src++; - s1.y_wptr = dest++; - src_circ_inc_wrap(&src, source->end_addr, source->size); - src_circ_inc_wrap(&dest, sink->end_addr, sink->size); - s1.state = &s->state1; - s1.stage = s->stage1; - if (cd->sign_extend_s24) - src_polyphase_stage_cir_s24(&s1); - else - src_polyphase_stage_cir(&s1); - - n_read += cd->param.blk_in; - n_written += cd->param.blk_out; - } + s1.state = &cd->src.state1; + s1.stage = cd->src.stage1; + s1.nch = dev->params.channels; + + cd->polyphase_func(&s1); + + n_read += nch * cd->param.blk_in; + n_written += nch * cd->param.blk_out; *bytes_read = n_read * sizeof(int32_t); *bytes_written = n_written * sizeof(int32_t); } @@ -271,7 +265,6 @@ static struct comp_dev *src_new(struct sof_ipc_comp *comp) struct sof_ipc_comp_src *src; struct sof_ipc_comp_src *ipc_src = (struct sof_ipc_comp_src *) comp; struct comp_data *cd; - int i; trace_src("new"); @@ -299,8 +292,8 @@ static struct comp_dev *src_new(struct sof_ipc_comp *comp) cd->delay_lines = NULL; cd->src_func = src_2s_s32_default; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) - src_polyphase_reset(&cd->src[i]); + cd->polyphase_func = src_polyphase_stage_cir; + src_polyphase_reset(&cd->src); dev->state = COMP_STATE_READY; return dev; @@ -334,10 +327,8 @@ static int src_params(struct comp_dev *dev) uint32_t sink_rate; int32_t *buffer_start; int n = 0; - int i; int err; int frames_is_for_source; - int nch; int q; trace_src("par"); @@ -345,10 +336,10 @@ static int src_params(struct comp_dev *dev) /* SRC supports S24_4LE and S32_LE formats */ switch (config->frame_fmt) { case SOF_IPC_FRAME_S24_4LE: - cd->sign_extend_s24 = 1; + cd->polyphase_func = src_polyphase_stage_cir_s24; break; case SOF_IPC_FRAME_S32_LE: - cd->sign_extend_s24 = 0; + cd->polyphase_func = src_polyphase_stage_cir; break; default: trace_src_error("sr0"); @@ -407,11 +398,7 @@ static int src_params(struct comp_dev *dev) buffer_start = cd->delay_lines + cd->param.sbuf_length; /* Initize SRC for actual sample rate */ - nch = MIN(params->channels, PLATFORM_MAX_CHANNELS); - for (i = 0; i < nch; i++) { - n = src_polyphase_init(&cd->src[i], &cd->param, buffer_start); - buffer_start += cd->param.single_src; - } + n = src_polyphase_init(&cd->src, &cd->param, buffer_start); /* Reset stage buffer */ cd->sbuf_r_ptr = cd->delay_lines; @@ -434,7 +421,7 @@ static int src_params(struct comp_dev *dev) * muted if copy() is run. */ trace_src("SFa"); - cd->src_func = fallback_s32; + cd->src_func = src_fallback; return -EINVAL; break; } @@ -532,7 +519,6 @@ static int src_copy(struct comp_dev *dev) /* Run SRC function if buffers avail and free allow */ if (((int) source->avail >= need_source) && ((int) sink->free >= need_sink)) { - /* Run src */ cd->src_func(dev, source, sink, &consumed, &produced); /* Calc new free and available if data was processed. These @@ -561,14 +547,12 @@ static int src_preload(struct comp_dev *dev) static int src_reset(struct comp_dev *dev) { - int i; struct comp_data *cd = comp_get_drvdata(dev); trace_src("SRe"); cd->src_func = src_2s_s32_default; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) - src_polyphase_reset(&cd->src[i]); + src_polyphase_reset(&cd->src); comp_set_state(dev, COMP_CMD_RESET); return 0; diff --git a/src/audio/src_core.c b/src/audio/src_core.c index 4ee2efd..080256c 100644 --- a/src/audio/src_core.c +++ b/src/audio/src_core.c @@ -45,12 +45,20 @@ #include "src_core.h" #include "src_config.h" +#define trace_src(__e) trace_event(TRACE_CLASS_SRC, __e) +#define tracev_src(__e) tracev_event(TRACE_CLASS_SRC, __e) +#define trace_src_error(__e) trace_error(TRACE_CLASS_SRC, __e) + /* TODO: These should be defined somewhere else. */ #define SOF_RATES_LENGTH 15 int sof_rates[SOF_RATES_LENGTH] = {8000, 11025, 12000, 16000, 18900, 22050, 24000, 32000, 44100, 48000, 64000, 88200, 96000, 176400, 192000}; +/* The FIR maximum lenghts are per channel so need to multiply them */ +#define MAX_FIR_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_FIR_DELAY_SIZE) +#define MAX_OUT_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_OUT_DELAY_SIZE) + /* Calculate ceil() for integer division */ int src_ceil_divide(int a, int b) { @@ -131,6 +139,13 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, int num; int frames2; + if (nch > PLATFORM_MAX_CHANNELS) { + trace_src_error("che"); + tracev_value(nch); + return -EINVAL; + } + + a->nch = nch; a->idx_in = src_find_fs(src_in_fs, NUM_IN_FS, fs_in); a->idx_out = src_find_fs(src_out_fs, NUM_OUT_FS, fs_out); @@ -146,8 +161,8 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, stage1 = src_table1[a->idx_out][a->idx_in]; stage2 = src_table2[a->idx_out][a->idx_in]; - a->fir_s1 = src_fir_delay_length(stage1); - a->out_s1 = src_out_delay_length(stage1); + a->fir_s1 = nch * src_fir_delay_length(stage1); + a->out_s1 = nch * src_out_delay_length(stage1); /* Find out how many additional times the SRC can be executed while having block size less or equal to max_frames. @@ -191,8 +206,8 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, a->stage2_times_max = 0; a->sbuf_length = 0; } else { - a->fir_s2 = src_fir_delay_length(stage2); - a->out_s2 = src_out_delay_length(stage2); + a->fir_s2 = nch * src_fir_delay_length(stage2); + a->out_s2 = nch * src_out_delay_length(stage2); /* 2x is an empirically tested length. Since the sink buffer * capability to receive samples varies a shorter stage 2 output * block will create a peak in internal buffer usage. @@ -200,8 +215,8 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, a->sbuf_length = 2 * nch * stage1->blk_out * a->stage1_times_max; } - a->single_src = a->fir_s1 + a->fir_s2 + a->out_s1 + a->out_s2; - a->total = a->sbuf_length + nch * a->single_src; + a->src_multich = a->fir_s1 + a->fir_s2 + a->out_s1 + a->out_s2; + a->total = a->sbuf_length + a->src_multich; return 0; } @@ -212,8 +227,6 @@ static void src_state_reset(struct src_state *state) state->fir_delay_size = 0; state->out_delay_size = 0; state->fir_wi = 0; - state->fir_ri = 0; - state->out_wi = 0; state->out_ri = 0; } @@ -229,17 +242,8 @@ static int init_stages( src->number_of_stages = n; src->stage1 = stage1; src->stage2 = stage2; - src->blk_in = p->blk_in; - src->blk_out = p->blk_out; - if (n == 1) { - src->stage1_times = p->stage1_times; - src->stage2_times = 0; - if (stage1->blk_out == 0) - return -EINVAL; - } else { - src->stage1_times = p->stage1_times; - src->stage2_times = p->stage2_times; - } + if ((n == 1) && (stage1->blk_out == 0)) + return -EINVAL; /* Delay line sizes */ src->state1.fir_delay_size = p->fir_s1; @@ -262,10 +266,10 @@ static int init_stages( } /* Check the sizes are less than MAX */ - if ((src->state1.fir_delay_size > MAX_FIR_DELAY_SIZE) - || (src->state1.out_delay_size > MAX_OUT_DELAY_SIZE) - || (src->state2.fir_delay_size > MAX_FIR_DELAY_SIZE) - || (src->state2.out_delay_size > MAX_OUT_DELAY_SIZE)) { + if ((src->state1.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH) + || (src->state1.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH) + || (src->state2.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH) + || (src->state2.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH)) { src->state1.fir_delay = NULL; src->state1.out_delay = NULL; src->state2.fir_delay = NULL; @@ -279,12 +283,7 @@ static int init_stages( void src_polyphase_reset(struct polyphase_src *src) { - src->mute = 0; src->number_of_stages = 0; - src->blk_in = 0; - src->blk_out = 0; - src->stage1_times = 0; - src->stage2_times = 0; src->stage1 = NULL; src->stage2 = NULL; src_state_reset(&src->state1); @@ -294,14 +293,12 @@ void src_polyphase_reset(struct polyphase_src *src) int src_polyphase_init(struct polyphase_src *src, struct src_param *p, int32_t *delay_lines_start) { - int n_stages; - int ret; struct src_stage *stage1; struct src_stage *stage2; + int n_stages; + int ret; if ((p->idx_in < 0) || (p->idx_out < 0)) { - src->blk_in = p->blk_in; - src->blk_out = p->blk_out; return -EINVAL; } @@ -335,109 +332,157 @@ int src_polyphase_init(struct polyphase_src *src, struct src_param *p, #if SRC_SHORT == 1 /* Calculate a FIR filter part that does not need circular modification */ -static inline void fir_part(int64_t *y, int ntaps, const int16_t c[], int *ic, - int32_t d[], int *id) + +static inline void fir_part(int64_t y[], int *id, int *ic, + const int32_t data[], const int16_t coef[], int nch_x_taps, int nch) { + int64_t tap0; + int64_t tap1; int n; int64_t a = 0; + int64_t b = 0; + int c = *ic; + int d = *id; + int d_end = d - nch_x_taps; /* Data is Q1.31, coef is Q1.15, product is Q2.46 */ - for (n = 0; n < (ntaps >> 1); n++) { - a += (int64_t) c[*ic] * d[*id] - + (int64_t) c[*ic + 1] * d[*id - 1]; - *ic += 2; - *id -= 2; + if (nch == 2) { + for (n = 0; n < (nch_x_taps >> 2); n++) { + tap0 = coef[c++]; + tap1 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + b += data[d--] * tap1; + a += data[d--] * tap1; + } + if (d > d_end) { + tap0 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + } + y[1] += b; + y[0] += a; + } else { + while (d > d_end) { + tap0 = coef[c++]; + for (n = nch - 1; n >= 0; n--) + y[n] += data[d--] * tap0; + } } - if (ntaps & 1) - a += (int64_t) c[(*ic)++] * d[(*id)--]; - - *y += a; + *ic = c; + *id = d; } + #else -/* Calculate a FIR filter part that does not need circular modification */ -static inline void fir_part(int64_t *y, int ntaps, const int32_t c[], int *ic, - int32_t d[], int *id) +static inline void fir_part(int64_t y[], int *id, int *ic, + const int32_t data[], const int32_t coef[], int nch_x_taps, int nch) { + int64_t tap0; + int64_t tap1; int n; int64_t a = 0; - - /* Data is Q8.24, coef is Q1.23, product is Q9.47 */ - for (n = 0; n < (ntaps >> 1); n++) { - a += (int64_t) c[*ic] * d[*id] - + (int64_t) c[*ic + 1] * d[*id - 1]; - *ic += 2; - *id -= 2; + int64_t b = 0; + int c = *ic; + int d = *id; + int d_end = d - nch_x_taps; + + /* Data is Q1.31, coef is Q1.23, product is Q2.54 */ + if (nch == 2) { + for (n = 0; n < (nch_x_taps >> 2); n++) { + tap0 = coef[c++]; + tap1 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + b += data[d--] * tap1; + a += data[d--] * tap1; + } + if (d > d_end) { + tap0 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + } + y[1] += b; + y[0] += a; + } else { + while (d > d_end) { + tap0 = coef[c++]; + for (n = nch - 1; n >= 0; n--) + y[n] += data[d--] * tap0; + } } - if (ntaps & 1) - a += (int64_t) c[(*ic)++] * d[(*id)--]; - - *y += a; + *ic = c; + *id = d; } + #endif #if SRC_SHORT == 1 -static inline int32_t fir_filter( - struct src_state *fir, const int16_t coefs[], - int *coefi, int filter_length, int shift) +static void fir_filter(int ri0, int *ci, int wi0, int32_t in_delay[], + int32_t out_delay[], const int16_t coefs[], int dsm1, int taps, + int shift, int nch) { - int64_t y = 0; - int n1; int n2; - - n1 = fir->fir_ri + 1; - if (n1 > filter_length) { - /* No need to un-wrap fir read index, make sure fir_fi - * is ge 0 after FIR computation. - */ - fir_part(&y, filter_length, coefs, coefi, fir->fir_delay, - &fir->fir_ri); + int i; + int64_t y[PLATFORM_MAX_CHANNELS]; + int ri = ri0; + int wi = wi0; + int n1 = ri0 + 1; /* Convert to number of sequential frames */ + int qshift = 15 + shift; /* Q2.46 -> Q2.31 */ + int32_t rnd = 1 << (qshift - 1); /* Half LSB */ + int nch_x_taps = nch * taps; + + /* Initialize to half LSB for rounding */ + for (i = 0; i < nch; i++) + y[i] = rnd; + + if (n1 >= nch_x_taps) { + fir_part(y, &ri, ci, in_delay, coefs, nch_x_taps, nch); } else { - n2 = filter_length - n1; - /* Part 1, loop n1 times, fir_ri becomes -1 */ - fir_part(&y, n1, coefs, coefi, fir->fir_delay, &fir->fir_ri); - - /* Part 2, unwrap fir_ri, continue rest of filter */ - fir->fir_ri = fir->fir_delay_size - 1; - fir_part(&y, n2, coefs, coefi, fir->fir_delay, &fir->fir_ri); + n2 = nch_x_taps - n1; + fir_part(y, &ri, ci, in_delay, coefs, n1, nch); + ri = dsm1; + fir_part(y, &ri, ci, in_delay, coefs, n2, nch); } - /* Q2.46 -> Q2.31, saturate to Q1.31 */ - y = y >> (15 + shift); - return(int32_t) sat_int32(y); + for (i = 0; i < nch; i++) + out_delay[wi++] = sat_int32(y[i] >> qshift); } #else -static inline int32_t fir_filter( - struct src_state *fir, const int32_t coefs[], - int *coefi, int filter_length, int shift) +static void fir_filter(int ri0, int *ci, int wi0, int32_t in_delay[], + int32_t out_delay[], const int32_t coefs[], int dsm1, int taps, + int shift, int nch) { - int64_t y = 0; - int n1; int n2; - - n1 = fir->fir_ri + 1; - if (n1 > filter_length) { - /* No need to un-wrap fir read index, make sure fir_fi - * is ge 0 after FIR computation. - */ - fir_part(&y, filter_length, coefs, coefi, fir->fir_delay, - &fir->fir_ri); + int i; + int64_t y[PLATFORM_MAX_CHANNELS]; + int ri = ri0; + int wi = wi0; + int n1 = ri0 + 1; /* Convert to number of sequential frames */ + int qshift = 23 + shift; /* Q2.54 -> Q2.31 */ + int32_t rnd = 1 << (qshift - 1); /* Half LSB */ + int nch_x_taps = nch * taps; + + /* Initialize to half LSB for rounding */ + for (i = 0; i < nch; i++) + y[i] = rnd; + + if (n1 >= nch_x_taps) { + fir_part(y, &ri, ci, in_delay, coefs, nch_x_taps, nch); } else { - n2 = filter_length - n1; - /* Part 1, loop n1 times, fir_ri becomes -1 */ - fir_part(&y, n1, coefs, coefi, fir->fir_delay, &fir->fir_ri); - - /* Part 2, unwrap fir_ri, continue rest of filter */ - fir->fir_ri = fir->fir_delay_size - 1; - fir_part(&y, n2, coefs, coefi, fir->fir_delay, &fir->fir_ri); + n2 = nch_x_taps - n1; + fir_part(y, &ri, ci, in_delay, coefs, n1, nch); + ri = dsm1; + fir_part(y, &ri, ci, in_delay, coefs, n2, nch); } - /* Q9.47 -> Q9.24, saturate to Q8.24 */ - y = y >> (23 + shift); - return(int32_t) sat_int32(y); + for (i = 0; i < nch; i++) + out_delay[wi++] = sat_int32(y[i] >> qshift); + } + #endif void src_polyphase_stage_cir(struct src_stage_prm * s) @@ -447,29 +492,42 @@ void src_polyphase_stage_cir(struct src_stage_prm * s) int n; int m; int f; - int c; - int r; + int ci; + int ri; int n_wrap_fir; int n_wrap_buf; int n_wrap_min; int n_min; - int32_t z; + int wi; + const void *coef = cfg->coefs; + int32_t *in_delay = fir->fir_delay; + int32_t *out_delay = fir->out_delay; + int dsm1 = fir->fir_delay_size - 1; + int shift = cfg->shift; + int nch = s->nch; + int rewind = -nch * (cfg->blk_in + + (cfg->num_of_subfilters - 1) * cfg->idm) + nch - 1; + int nch_x_idm = cfg->idm * nch; + int nch_x_odm = cfg->odm * nch; + size_t sz = sizeof(int32_t); + int blk_in_bytes = nch * cfg->blk_in * sz; + int blk_out_bytes = nch * cfg->num_of_subfilters * sz; + for (n = 0; n < s->times; n++) { /* Input data */ - m = s->x_inc * cfg->blk_in; + m = blk_in_bytes; while (m > 0) { - n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) - * s->x_inc; + n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) * sz; n_wrap_buf = s->x_end_addr - s->x_rptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { fir->fir_delay[fir->fir_wi++] = *s->x_rptr; - s->x_rptr += s->x_inc; - n_min -= s->x_inc; - m -= s->x_inc; + s->x_rptr++; + n_min -= sz; + m -= sz; } /* Check for wrap */ src_circ_inc_wrap(&s->x_rptr, s->x_end_addr, s->x_size); @@ -478,41 +536,38 @@ void src_polyphase_stage_cir(struct src_stage_prm * s) } /* Filter */ - c = 0; - r = fir->fir_wi - cfg->blk_in - - (cfg->num_of_subfilters - 1) * cfg->idm; - if (r < 0) - r += fir->fir_delay_size; + ci = 0; /* Reset to 1st coefficient */ + ri = fir->fir_wi + rewind; /* Newest data for last subfilter */ + if (ri < 0) + ri += fir->fir_delay_size; - fir->out_wi = fir->out_ri; + wi = fir->out_ri; for (f = 0; f < cfg->num_of_subfilters; f++) { - fir->fir_ri = r; - z = fir_filter(fir, cfg->coefs, &c, - cfg->subfilter_length, cfg->shift); - r += cfg->idm; - if (r >= fir->fir_delay_size) - r -= fir->fir_delay_size; - - fir->out_delay[fir->out_wi] = z; - fir->out_wi += cfg->odm; - if (fir->out_wi >= fir->out_delay_size) - fir->out_wi -= fir->out_delay_size; + fir_filter(ri, &ci, wi, in_delay, out_delay, coef, + dsm1, cfg->subfilter_length, shift, nch); + + wi += nch_x_odm; + if (wi >= fir->out_delay_size) + wi -= fir->out_delay_size; + + ri += nch_x_idm; /* Next sub-filter start */ + if (ri >= fir->fir_delay_size) + ri -= fir->fir_delay_size; } /* Output */ - m = s->y_inc * cfg->num_of_subfilters; + m = blk_out_bytes; while (m > 0) { - n_wrap_fir = (fir->out_delay_size - fir->out_ri) - * s->y_inc; + n_wrap_fir = (fir->out_delay_size - fir->out_ri) * sz; n_wrap_buf = s->y_end_addr - s->y_wptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { *s->y_wptr = fir->out_delay[fir->out_ri++]; - s->y_wptr += s->y_inc; - n_min -= s->y_inc; - m -= s->y_inc; + s->y_wptr++; + n_min -= sz; + m -= sz; } /* Check wrap */ src_circ_inc_wrap(&s->y_wptr, s->y_end_addr, s->y_size); @@ -522,38 +577,48 @@ void src_polyphase_stage_cir(struct src_stage_prm * s) } } -void src_polyphase_stage_cir_s24(struct src_stage_prm * s) +void src_polyphase_stage_cir_s24(struct src_stage_prm *s) { struct src_state *fir = s->state; struct src_stage *cfg = s->stage; int n; int m; int f; - int c; - int r; + int ci; + int ri; int n_wrap_fir; int n_wrap_buf; int n_wrap_min; int n_min; - int32_t z; - int32_t se; + int wi; + const void *coef = cfg->coefs; + int32_t *in_delay = fir->fir_delay; + int32_t *out_delay = fir->out_delay; + int dsm1 = fir->fir_delay_size - 1; + int shift = cfg->shift; + int nch = s->nch; + int rewind = -nch * (cfg->blk_in + + (cfg->num_of_subfilters - 1) * cfg->idm) + nch - 1; + int nch_x_idm = cfg->idm * nch; + int nch_x_odm = cfg->odm * nch; + size_t sz = sizeof(int32_t); + int blk_in_bytes = nch * cfg->blk_in * sz; + int blk_out_bytes = nch * cfg->num_of_subfilters * sz; for (n = 0; n < s->times; n++) { /* Input data */ - m = s->x_inc * cfg->blk_in; + m = blk_in_bytes; while (m > 0) { - n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) - * s->x_inc; + n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) * sz; n_wrap_buf = s->x_end_addr - s->x_rptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { - se = *s->x_rptr << 8; - fir->fir_delay[fir->fir_wi++] = se >> 8; - s->x_rptr += s->x_inc; - n_min -= s->x_inc; - m -= s->x_inc; + fir->fir_delay[fir->fir_wi++] = *s->x_rptr << 8; + s->x_rptr++; + n_min -= sz; + m -= sz; } /* Check for wrap */ src_circ_inc_wrap(&s->x_rptr, s->x_end_addr, s->x_size); @@ -562,41 +627,38 @@ void src_polyphase_stage_cir_s24(struct src_stage_prm * s) } /* Filter */ - c = 0; - r = fir->fir_wi - cfg->blk_in - - (cfg->num_of_subfilters - 1) * cfg->idm; - if (r < 0) - r += fir->fir_delay_size; + ci = 0; /* Reset to 1st coefficient */ + ri = fir->fir_wi + rewind; /* Newest data for last subfilter */ + if (ri < 0) + ri += fir->fir_delay_size; - fir->out_wi = fir->out_ri; + wi = fir->out_ri; for (f = 0; f < cfg->num_of_subfilters; f++) { - fir->fir_ri = r; - z = fir_filter(fir, cfg->coefs, &c, - cfg->subfilter_length, cfg->shift); - r += cfg->idm; - if (r >= fir->fir_delay_size) - r -= fir->fir_delay_size; - - fir->out_delay[fir->out_wi] = z; - fir->out_wi += cfg->odm; - if (fir->out_wi >= fir->out_delay_size) - fir->out_wi -= fir->out_delay_size; + fir_filter(ri, &ci, wi, in_delay, out_delay, coef, + dsm1, cfg->subfilter_length, shift, nch); + + wi += nch_x_odm; + if (wi >= fir->out_delay_size) + wi -= fir->out_delay_size; + + ri += nch_x_idm; /* Next sub-filter start */ + if (ri >= fir->fir_delay_size) + ri -= fir->fir_delay_size; } /* Output */ - m = s->y_inc * cfg->num_of_subfilters; + m = blk_out_bytes; while (m > 0) { - n_wrap_fir = (fir->out_delay_size - fir->out_ri) - * s->y_inc; + n_wrap_fir = (fir->out_delay_size - fir->out_ri) * sz; n_wrap_buf = s->y_end_addr - s->y_wptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { - *s->y_wptr = fir->out_delay[fir->out_ri++]; - s->y_wptr += s->y_inc; - n_min -= s->y_inc; - m -= s->y_inc; + *s->y_wptr = fir->out_delay[fir->out_ri++] >> 8; + s->y_wptr++; + n_min -= sz; + m -= sz; } /* Check wrap */ src_circ_inc_wrap(&s->y_wptr, s->y_end_addr, s->y_size); @@ -604,47 +666,5 @@ void src_polyphase_stage_cir_s24(struct src_stage_prm * s) fir->out_ri = 0; } } -} - -#ifdef MODULE_TEST - -void src_print_info(struct polyphase_src * src) -{ - - int n1; - int n2; - - n1 = src->stage1->filter_length; - n2 = src->stage2->filter_length; - printf("SRC stages %d\n", src->number_of_stages); - printf("SRC input blk %d\n", src->blk_in); - printf("SRC output blk %d\n", src->blk_out); - printf("SRC stage1 %d times\n", src->stage1_times); - printf("SRC stage2 %d times\n", src->stage2_times); - - printf("SRC1 filter length %d\n", n1); - printf("SRC1 subfilter length %d\n", src->stage1->subfilter_length); - printf("SRC1 number of subfilters %d\n", - src->stage1->num_of_subfilters); - printf("SRC1 idm %d\n", src->stage1->idm); - printf("SRC1 odm %d\n", src->stage1->odm); - printf("SRC1 input blk %d\n", src->stage1->blk_in); - printf("SRC1 output blk %d\n", src->stage1->blk_out); - printf("SRC1 halfband %d\n", src->stage1->halfband); - printf("SRC1 FIR delay %d\n", src->state1.fir_delay_size); - printf("SRC1 out delay %d\n", src->state1.out_delay_size); - - printf("SRC2 filter length %d\n", n2); - printf("SRC2 subfilter length %d\n", src->stage2->subfilter_length); - printf("SRC2 number of subfilters %d\n", - src->stage2->num_of_subfilters); - printf("SRC2 idm %d\n", src->stage2->idm); - printf("SRC2 odm %d\n", src->stage2->odm); - printf("SRC2 input blk %d\n", src->stage2->blk_in); - printf("SRC2 output blk %d\n", src->stage2->blk_out); - printf("SRC2 halfband %d\n", src->stage2->halfband); - printf("SRC2 FIR delay %d\n", src->state2.fir_delay_size); - printf("SRC2 out delay %d\n", src->state2.out_delay_size); } -#endif diff --git a/src/audio/src_core.h b/src/audio/src_core.h index 3859e6f..3ea6028 100644 --- a/src/audio/src_core.h +++ b/src/audio/src_core.h @@ -41,7 +41,7 @@ struct src_param { int out_s1; int out_s2; int sbuf_length; - int single_src; + int src_multich; int total; int blk_in; int blk_out; @@ -51,6 +51,7 @@ struct src_param { int stage2_times_max; int idx_in; int idx_out; + int nch; }; struct src_stage { @@ -70,20 +71,13 @@ struct src_state { int fir_delay_size; int out_delay_size; int fir_wi; - int fir_ri; - int out_wi; int out_ri; int32_t *fir_delay; int32_t *out_delay; }; struct polyphase_src { - int mute; int number_of_stages; - int blk_in; - int blk_out; - int stage1_times; - int stage2_times; struct src_stage *stage1; struct src_stage *stage2; struct src_state state1; @@ -91,15 +85,14 @@ struct polyphase_src { }; struct src_stage_prm { + int nch; int times; int32_t *x_rptr; int32_t *x_end_addr; size_t x_size; - int x_inc; int32_t *y_wptr; int32_t *y_end_addr; size_t y_size; - int y_inc; struct src_state *state; struct src_stage *stage; }; @@ -116,31 +109,6 @@ static inline void src_circ_dec_wrap(int32_t **ptr, int32_t *addr, size_t size) *ptr = (int32_t *) ((size_t) * ptr + size); } -static inline void src_polyphase_mute(struct polyphase_src *src) -{ - src->mute = 1; -} - -static inline void src_polyphase_unmute(struct polyphase_src *src) -{ - src->mute = 0; -} - -static inline int src_polyphase_getmute(struct polyphase_src *src) -{ - return src->mute; -} - -static inline int src_polyphase_get_blk_in(struct polyphase_src *src) -{ - return src->blk_in; -} - -static inline int src_polyphase_get_blk_out(struct polyphase_src *src) -{ - return src->blk_out; -} - void src_polyphase_reset(struct polyphase_src *src); int src_polyphase_init(struct polyphase_src *src, struct src_param *p, @@ -162,8 +130,4 @@ int32_t src_output_rates(void); int src_ceil_divide(int a, int b); -#ifdef MODULE_TEST -void src_print_info(struct polyphase_src *src); -#endif - #endif -- 2.11.0

1 0

[Sound-open-firmware] [PATCH 2/2] Move DMA tracing intialization calling.
by yan.wang＠linux.intel.com 20 Oct '17

20 Oct '17

From: Yan Wang <yan.wang(a)linux.intel.com> Move it from ipc handler into platform_init(). Signed-off-by: Yan Wang <yan.wang(a)linux.intel.com> --- src/ipc/intel-ipc.c | 7 ------- src/platform/baytrail/platform.c | 4 ++++ 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/ipc/intel-ipc.c b/src/ipc/intel-ipc.c index f6d88ef..902e5a6 100644 --- a/src/ipc/intel-ipc.c +++ b/src/ipc/intel-ipc.c @@ -583,13 +583,6 @@ static int ipc_dma_trace_config(uint32_t header) trace_ipc_error("DA1"); - /* Initialize DMA for Trace*/ - err = dma_trace_init(&_ipc->dmat); - if (err < 0) { - trace_ipc_error("eIP"); - goto error; - } - /* use DMA to read in compressed page table ringbuffer from host */ err = get_page_descriptors(iipc, &params->buffer); if (err < 0) { diff --git a/src/platform/baytrail/platform.c b/src/platform/baytrail/platform.c index b701196..61596bb 100644 --- a/src/platform/baytrail/platform.c +++ b/src/platform/baytrail/platform.c @@ -46,6 +46,7 @@ #include <reef/clock.h> #include <reef/ipc.h> #include <reef/trace.h> +#include <reef/dma-trace.h> #include <reef/audio/component.h> #include <config.h> #include <string.h> @@ -296,6 +297,9 @@ int platform_init(struct reef *reef) trace_point(TRACE_BOOT_PLATFORM_IPC); ipc_init(reef); + /* Initialize DMA for Trace*/ + dma_trace_init(&reef->ipc->dmat); + /* init DMACs */ trace_point(TRACE_BOOT_PLATFORM_DMA); dmac0 = dma_get(DMA_ID_DMAC0); -- 2.7.4

1 0

[Sound-open-firmware] [PATCH 1/2] Move DMA tracing implementaion from src/audio to src/lib.
by yan.wang＠linux.intel.com 20 Oct '17

20 Oct '17

From: Yan Wang <yan.wang(a)linux.intel.com> 1. Move source and header file. 2. Change src/lib/Makefile.am and src/audio/Makefile.am. 3. Chnage header file path. Signed-off-by: Yan Wang <yan.wang(a)linux.intel.com> --- src/audio/Makefile.am | 1 - src/include/reef/{audio => }/dma-trace.h | 0 src/include/reef/ipc.h | 2 +- src/lib/Makefile.am | 1 + src/{audio => lib}/dma-trace.c | 3 +-- src/lib/trace.c | 2 +- 6 files changed, 4 insertions(+), 5 deletions(-) rename src/include/reef/{audio => }/dma-trace.h (100%) rename src/{audio => lib}/dma-trace.c (98%) diff --git a/src/audio/Makefile.am b/src/audio/Makefile.am index 7ffe01f..285422e 100644 --- a/src/audio/Makefile.am +++ b/src/audio/Makefile.am @@ -17,7 +17,6 @@ libaudio_a_SOURCES = \ pipeline.c \ pipeline_static.c \ component.c \ - dma-trace.c \ buffer.c libaudio_a_CFLAGS = \ diff --git a/src/include/reef/audio/dma-trace.h b/src/include/reef/dma-trace.h similarity index 100% rename from src/include/reef/audio/dma-trace.h rename to src/include/reef/dma-trace.h diff --git a/src/include/reef/ipc.h b/src/include/reef/ipc.h index a552626..472aca8 100644 --- a/src/include/reef/ipc.h +++ b/src/include/reef/ipc.h @@ -40,7 +40,7 @@ #include <uapi/ipc.h> #include <reef/audio/pipeline.h> #include <reef/audio/component.h> -#include <reef/audio/dma-trace.h> +#include <reef/dma-trace.h> struct reef; struct dai_config; diff --git a/src/lib/Makefile.am b/src/lib/Makefile.am index acdbd85..b4521df 100644 --- a/src/lib/Makefile.am +++ b/src/lib/Makefile.am @@ -6,6 +6,7 @@ libcore_a_SOURCES = \ work.c \ notifier.c \ trace.c \ + dma-trace.c \ schedule.c libcore_a_CFLAGS = \ diff --git a/src/audio/dma-trace.c b/src/lib/dma-trace.c similarity index 98% rename from src/audio/dma-trace.c rename to src/lib/dma-trace.c index b0267fa..5885037 100644 --- a/src/audio/dma-trace.c +++ b/src/lib/dma-trace.c @@ -29,7 +29,7 @@ */ #include <reef/trace.h> -#include <reef/audio/dma-trace.h> +#include <reef/dma-trace.h> #include <reef/ipc.h> #include <reef/reef.h> #include <reef/alloc.h> @@ -172,7 +172,6 @@ int dma_trace_enable(struct dma_trace_data *d) return -ENODEV; } - /* TODO: fix crash when enabled */ d->enabled = 1; work_schedule_default(&d->dmat_work, DMA_TRACE_US); return 0; diff --git a/src/lib/trace.c b/src/lib/trace.c index c0abf7c..eec93b5 100644 --- a/src/lib/trace.c +++ b/src/lib/trace.c @@ -34,7 +34,7 @@ #include <arch/cache.h> #include <platform/timer.h> #include <reef/lock.h> -#include <reef/audio/dma-trace.h> +#include <reef/dma-trace.h> #include <stdint.h> struct trace { -- 2.7.4

1 0

[Sound-open-firmware] [PATCH v5] volume: fix for DC offset and handling of sign extension for 24-bit samples
by Ranjani Sridharan 20 Oct '17

20 Oct '17

This patch fixes the DC offsets introduced in the volume component due to shifts and handles sign extension for 24-bit input samples Signed-off-by: Ranjani Sridharan <ranjani.sridharan(a)linux.intel.com> --- src/audio/tone.c | 19 +++++++---- src/audio/volume.c | 71 +++++++++++++++++++++++++++-------------- src/include/reef/audio/format.h | 50 ++++++++++++++++++++++------- src/math/trig.c | 3 +- 4 files changed, 99 insertions(+), 44 deletions(-) diff --git a/src/audio/tone.c b/src/audio/tone.c index 0baf247..1fde455 100644 --- a/src/audio/tone.c +++ b/src/audio/tone.c @@ -149,7 +149,9 @@ static int32_t tonegen(struct tone_state *sg) /* 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(sg->w), sg->a, 31, 31, 31); + sine = + q_mults_32x32(sin_fixed(sg->w), sg->a, + Q_SHIFT_BITS_64(31, 31, 31)); /* Next point */ w = (int64_t) sg->w + sg->w_step; @@ -208,15 +210,16 @@ static void tonegen_control(struct tone_state *sg) && (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, 31, 30, 31)); + 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, 16, 30, 16); + 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++; @@ -297,7 +300,8 @@ static void tonegen_update_f(struct tone_state *sg, int32_t f) f_max = Q_SHIFT_LEFT((int64_t) sg->fs, 0, 16 - 1); f_max = (f_max > INT32_MAXVALUE) ? INT32_MAXVALUE : f_max; sg->f = (f > f_max) ? f_max : f; - w_tmp = q_multsr_32x32(sg->f, sg->c, 16, 31, 28); /* Q16 x Q31 -> Q28 */ + /* 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; @@ -362,7 +366,8 @@ static int tonegen_init(struct tone_state *sg, int32_t fs, int32_t f, int32_t a) 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, 0, 31, 0); + sg->samples_in_block = + (int32_t) q_multsr_32x32(fs, 268435, Q_SHIFT_BITS_64(0, 31, 0)); return 0; } diff --git a/src/audio/volume.c b/src/audio/volume.c index 472aab9..137be3c 100644 --- a/src/audio/volume.c +++ b/src/audio/volume.c @@ -99,8 +99,8 @@ static void vol_s16_to_s32(struct comp_dev *dev, struct comp_buffer *sink, int32_t *dest = (int32_t *) sink->w_ptr; int32_t i; - /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 --> Q1.31 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { dest[i] = (int32_t)src[i] * cd->volume[0]; dest[i + 1] = (int32_t)src[i + 1] * cd->volume[1]; @@ -117,9 +117,12 @@ static void vol_s32_to_s16(struct comp_dev *dev, struct comp_buffer *sink, int32_t i; /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.15 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = (((int32_t)src[i] >> 16) * cd->volume[0]) >> 16; - dest[i + 1] = (((int32_t)src[i + 1] >> 16) * cd->volume[1]) >> 16; + dest[i] = (int16_t)q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 15)); + dest[i + 1] = (int16_t)q_multsr_sat_32x32( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 15)); } } @@ -133,9 +136,12 @@ static void vol_s32_to_s32(struct comp_dev *dev, struct comp_buffer *sink, int32_t i; /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.31 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = ((int64_t)src[i] * cd->volume[0]) >> 16; - dest[i + 1] = ((int64_t)src[i + 1] * cd->volume[1]) >> 16; + dest[i] = q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 31)); + dest[i + 1] = q_multsr_sat_32x32( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 31)); } } @@ -149,9 +155,12 @@ static void vol_s16_to_s16(struct comp_dev *dev, struct comp_buffer *sink, int32_t i; /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 --> Q1.15 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = ((int32_t)src[i] * cd->volume[0]) >> 16; - dest[i + 1] = ((int32_t)src[i + 1] * cd->volume[1]) >> 16; + dest[i] = q_multsr_sat_16x16( + src[i], cd->volume[0], Q_SHIFT_BITS_32(15, 16, 15)); + dest[i + 1] = q_multsr_sat_16x16( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_32(15, 16, 15)); } } @@ -165,9 +174,12 @@ static void vol_s16_to_s24(struct comp_dev *dev, struct comp_buffer *sink, int32_t i; /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = ((int32_t)src[i] * cd->volume[0]) >> 8; - dest[i + 1] = ((int32_t)src[i + 1] * cd->volume[1]) >> 8; + dest[i] = q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(15, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(15, 16, 23)); } } @@ -181,11 +193,14 @@ static void vol_s24_to_s16(struct comp_dev *dev, struct comp_buffer *sink, int32_t i; /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.15 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = (int16_t)((((int32_t)src[i] >> 8) * - cd->volume[0]) >> 16); - dest[i + 1] = (int16_t)((((int32_t)src[i + 1] >> 8) * - cd->volume[1]) >> 16); + dest[i] = (int16_t)q_multsr_sat_32x32( + sign_extend_s24(src[i]), cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 15)); + dest[i + 1] = (int16_t)q_multsr_sat_32x32( + sign_extend_s24(src[i + 1]), cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 15)); } } @@ -199,9 +214,12 @@ static void vol_s32_to_s24(struct comp_dev *dev, struct comp_buffer *sink, int32_t i; /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.23 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = ((int64_t)src[i] * cd->volume[0]) >> 24; - dest[i + 1] = ((int64_t)src[i + 1] * cd->volume[1]) >> 24; + dest[i] = q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 23)); } } @@ -215,11 +233,14 @@ static void vol_s24_to_s32(struct comp_dev *dev, struct comp_buffer *sink, int32_t i; /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.31 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = (int32_t)(((int64_t)src[i] * - cd->volume[0]) >> 8); - dest[i + 1] = (int32_t)(((int64_t)src[i + 1] * - cd->volume[1]) >> 8); + dest[i] = q_multsr_sat_32x32( + sign_extend_s24(src[i]), cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 31)); + dest[i + 1] = q_multsr_sat_32x32( + sign_extend_s24(src[i + 1]), cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 31)); } } @@ -234,12 +255,14 @@ static void vol_s24_to_s24(struct comp_dev *dev, struct comp_buffer *sink, int32_t *dest = (int32_t*) sink->w_ptr; /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.23 and volume is Q1.16 */ + /* Samples are Q1.23 --> Q1.23 and volume is Q1.16 */ for (i = 0; i < frames * 2; i += 2) { - dest[i] = (int32_t)(Q_MULTS_32X32( - (int64_t)src[i], cd->volume[0], 23, 16, 23)); - dest[i + 1] = (int32_t)(Q_MULTS_32X32( - (int64_t)src[i+1], cd->volume[1], 23, 16, 23)); + dest[i] = q_multsr_sat_32x32( + sign_extend_s24(src[i]), cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32( + sign_extend_s24(src[i + 1]), cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 23)); } } diff --git a/src/include/reef/audio/format.h b/src/include/reef/audio/format.h index 945a968..b9263f1 100644 --- a/src/include/reef/audio/format.h +++ b/src/include/reef/audio/format.h @@ -56,6 +56,18 @@ #define MINUS_80DB_Q1_31 214748 /* 10^(-80/20) */ #define MINUS_90DB_Q1_31 67909 /* 10^(-90/20) */ +/* Compute the number of shifts + * This will result in a compiler overflow error if shift bits are out of + * range as INT64_MAX/MIN is greater than 32 bit Q shift parameter + */ +#define Q_SHIFT_BITS_64(qx, qy, qz) \ + ((qx + qy - qz) <= 63 ? (((qx + qy - qz) >= 0) ? \ + (qx + qy - qz) : INT64_MIN) : INT64_MAX) + +#define Q_SHIFT_BITS_32(qx, qy, qz) \ + ((qx + qy - qz) <= 31 ? (((qx + qy - qz) >= 0) ? \ + (qx + qy - qz) : INT32_MIN) : INT32_MAX) + /* Convert a float number to fractional Qnx.ny format. Note that there is no * check for nx+ny number of bits to fit the word length of int. */ @@ -82,28 +94,24 @@ #define SATP_INT32(x) (((x) > INT32_MAXVALUE) ? INT32_MAXVALUE : (x)) #define SATM_INT32(x) (((x) < INT32_MINVALUE) ? INT32_MINVALUE : (x)) -static inline int64_t q_mults_32x32(int32_t x, int32_t y, - const int qx, const int qy, const int qp) +static inline int64_t q_mults_32x32(int32_t x, int32_t y, const int shift_bits) { - return ((int64_t)x * y) >> (qx+qy-qp); + return ((int64_t)x * y) >> shift_bits; } -static inline int64_t q_multsr_32x32(int32_t x, int32_t y, - const int qx, const int qy, const int qp) +static inline int64_t q_multsr_32x32(int32_t x, int32_t y, const int shift_bits) { - return ((((int64_t)x * y) >> (qx+qy-qp-1)) + 1) >> 1; + return ((((int64_t)x * y) >> (shift_bits - 1)) + 1) >> 1; } -static inline int32_t q_mults_16x16(int16_t x, int16_t y, - const int qx, const int qy, const int qp) +static inline int32_t q_mults_16x16(int16_t x, int32_t y, const int shift_bits) { - return ((int32_t)x * y) >> (qx+qy-qp); + return ((int32_t)x * y) >> shift_bits; } -static inline int16_t q_multsr_16x16(int16_t x, int16_t y, - const int qx, const int qy, const int qp) +static inline int16_t q_multsr_16x16(int16_t x, int32_t y, const int shift_bits) { - return ((((int32_t)x * y) >> (qx+qy-qp-1)) + 1) >> 1; + return ((((int32_t)x * y) >> (shift_bits - 1)) + 1) >> 1; } /* Saturation inline functions */ @@ -147,4 +155,22 @@ static inline int16_t sat_int16(int32_t x) return (int16_t)x; } +/* Fractional multiplication with shift and saturation */ +static inline int32_t q_multsr_sat_32x32(int32_t x, int32_t y, + const int shift_bits) +{ + return sat_int32(((((int64_t)x * y) >> (shift_bits - 1)) + 1) >> 1); +} + +static inline int16_t q_multsr_sat_16x16(int16_t x, int32_t y, + const int shift_bits) +{ + return sat_int16(((((int32_t)x * y) >> (shift_bits - 1)) + 1) >> 1); +} + +static inline int32_t sign_extend_s24(int32_t x) +{ + return (x << 8) >> 8; +} + #endif diff --git a/src/math/trig.c b/src/math/trig.c index 79eb480..70593e4 100644 --- a/src/math/trig.c +++ b/src/math/trig.c @@ -594,6 +594,7 @@ int32_t sin_fixed(int32_t w) { delta = s1 - s0; /* Q1.31 */ //sine = (int64_t) frac*delta; /* Q1.31 x Q1.31 -> Q2.62 */ //sine = (sine >> 31) + s0; /* Q2.31 */ - sine = s0 + q_mults_32x32(frac, delta, 31, 31, 31); /* All Q1.31 */ + /* All Q1.31 */ + sine = s0 + q_mults_32x32(frac, delta, Q_SHIFT_BITS_64(31, 31, 31)); return (int32_t) sine; } -- 2.11.0

2 1

[Sound-open-firmware] [PATCH 1/2] comp: remove unused list in struct component.
by Liam Girdwood 20 Oct '17

20 Oct '17

Not used so remove. Signed-off-by: Liam Girdwood <liam.r.girdwood(a)linux.intel.com> --- src/include/reef/audio/component.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/include/reef/audio/component.h b/src/include/reef/audio/component.h index db5f35b..8f5a508 100644 --- a/src/include/reef/audio/component.h +++ b/src/include/reef/audio/component.h @@ -186,7 +186,6 @@ struct comp_dev { struct comp_driver *drv; /* lists */ - struct list_item list; /* list in components */ struct list_item bsource_list; /* list of source buffers */ struct list_item bsink_list; /* list of sink buffers */ -- 2.11.0

1 0

[Sound-open-firmware] [PATCH] rmbox: use 64bit trace data
by Liam Girdwood 20 Oct '17

20 Oct '17

Update trace to align with FW and use 64bit data size. Signed-off-by: Liam Girdwood <liam.r.girdwood(a)linux.intel.com> --- rmbox/rmbox.c | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/rmbox/rmbox.c b/rmbox/rmbox.c index 9313f4b..b872694 100644 --- a/rmbox/rmbox.c +++ b/rmbox/rmbox.c @@ -129,16 +129,28 @@ static void show_trace(uint64_t val, uint64_t addr, uint64_t *timestamp, double /* 64-bit timestamp */ us = to_usecs(val, clk); - printf("[%6.6f]\tdelta [%6.6f]\t", + /* empty data ? */ + if (val == 0) { + *timestamp = 0; + return; + } + + /* detect wrap around */ + if (fdelta < 1000.0 * 1000.0 * 1000.0) + printf("0x%lx [%6.6f]\tdelta [%6.6f]\t", addr, us / 1000000.0 , fdelta / 1000000.0); + else + printf("0x%lx [%6.6f]\tdelta [********]\t", addr, + us / 1000000.0); *timestamp = val; return; - } + } else if (*timestamp == 0) + return; /* check for printable values - otherwise it's a value */ if (!isprint((char)(val >> 16)) || !isprint((char)(val >> 8)) || !isprint((char)val)) { - printf("value 0x%8.8x\n", (uint32_t)val); + printf("value 0x%16.16lx\n", val); return; } @@ -237,9 +249,6 @@ trace: tmp[TRACE_BLOCK_SIZE - i - 1] = c; } - if (val == 0) - break; - show_trace(val, addr, &timestamp, clk); if (out_fd) { -- 1.9.1

1 0

[Sound-open-firmware] [PATCH] SRC: Use a multi-channel FIR core to optimize speed plus other cleanup
by Seppo Ingalsuo 20 Oct '17

20 Oct '17

This patch changes the sample rate conversion processing to use a single multi-channel filter instead of per channel called mono filter instances. The filter output is now rounded with 1/2 LSB add. The polyphase filter input input block sizes are multiplied in SRC initialization to reach near to or exacly the period length to reduce polyphase filter call overhead. The polyphase filter is now called via function pointer for more flexibility with audio data formats. Currently S32_LE and S24_4LE are supported. Code cleanup includes removal of redundant variables and some debug print code. Signed-off-by: Seppo Ingalsuo <seppo.ingalsuo(a)linux.intel.com> --- src/audio/src.c | 198 ++++++++++------------- src/audio/src_core.c | 447 ++++++++++++++++++++++++++------------------------- src/audio/src_core.h | 42 +---- 3 files changed, 323 insertions(+), 364 deletions(-) diff --git a/src/audio/src.c b/src/audio/src.c index 4c4bc6e..854cb82 100644 --- a/src/audio/src.c +++ b/src/audio/src.c @@ -55,7 +55,7 @@ /* src component private data */ struct comp_data { - struct polyphase_src src[PLATFORM_MAX_CHANNELS]; + struct polyphase_src src; struct src_param param; int32_t *delay_lines; uint32_t sink_rate; @@ -63,16 +63,16 @@ struct comp_data { int32_t *sbuf_w_ptr; int32_t *sbuf_r_ptr; int sbuf_avail; - int sign_extend_s24; /* Set if need to copy sign bit to b24..b31 */ void (* src_func)(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *consumed, size_t *produced); + void (* polyphase_func)(struct src_stage_prm *s); }; /* Fallback function */ -static void fallback_s32(struct comp_dev *dev, struct comp_buffer *source, +static void src_fallback(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written) { *bytes_read = 0; @@ -84,101 +84,105 @@ static void src_2s_s32_default(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written) { - struct polyphase_src *s; struct src_stage_prm s1; struct src_stage_prm s2; - int j; + int s1_blk_in; + int s1_blk_out; + int s2_blk_in; + int s2_blk_out; struct comp_data *cd = comp_get_drvdata(dev); int32_t *dest = (int32_t *) sink->w_ptr; int32_t *src = (int32_t *) source->r_ptr; - int32_t *sbuf_addr = cd->delay_lines; int32_t *sbuf_end_addr = &cd->delay_lines[cd->param.sbuf_length]; int32_t sbuf_size = cd->param.sbuf_length * sizeof(int32_t); - const int nch = dev->params.channels; - const int s1_blk_in = cd->src[0].stage1->blk_in * nch; - const int s1_blk_out = cd->src[0].stage1->blk_out * nch; - const int s2_blk_in = cd->src[0].stage2->blk_in * nch; - const int s2_blk_out = cd->src[0].stage2->blk_out * nch; + int nch = dev->params.channels; int sbuf_free = cd->param.sbuf_length - cd->sbuf_avail; - int source_check = (source->avail >> 2) - s1_blk_in; /* for int32_t */ - int sink_check = (sink->free >> 2) - s2_blk_out; /* For int32_t */ int n_read = 0; int n_written = 0; int n1 = 0; int n2 = 0; + int avail = source->avail >> 2; /* For int32_t */ + int free = sink->free >> 2; /* For int32_t */ - s1.times = 1; s1.x_end_addr = source->end_addr; s1.x_size = source->size; - s1.x_inc = nch; s1.y_end_addr = sbuf_end_addr; s1.y_size = sbuf_size; - s1.y_inc = nch; + s1.state = &cd->src.state1; + s1.stage = cd->src.stage1; + s1.x_rptr = src; + s1.y_wptr = cd->sbuf_w_ptr; + s1.nch = nch; - s2.times = 1; s2.x_end_addr = sbuf_end_addr; s2.x_size = sbuf_size; - s2.x_inc = nch; s2.y_end_addr = sink->end_addr; s2.y_size = sink->size; - s2.y_inc = nch; + s2.state = &cd->src.state2; + s2.stage = cd->src.stage2; + s2.x_rptr = cd->sbuf_r_ptr; + s2.y_wptr = dest; + s2.nch = nch; + - /* 1st stage runs once a long multiplied length block. - * The stage buffer much be large enough to fit one s1 output block - * plus one s2 input block plus jitter in s2 consumption. + /* Test if 1st stage can be run with default block length to reach + * the period length or just under it. */ - while ((n1 < cd->param.stage1_times_max) - && (n_read <= source_check) - && (sbuf_free >= s1_blk_out)) { - for (j = 0; j < nch; j++) { - s = &cd->src[j]; /* Point to src[] for this channel */ - s1.x_rptr = src++; - s1.y_wptr = cd->sbuf_w_ptr++; - src_circ_inc_wrap(&src, source->end_addr, source->size); - src_circ_inc_wrap(&cd->sbuf_w_ptr, sbuf_end_addr, sbuf_size); - s1.state = &s->state1; - s1.stage = s->stage1; - if (cd->sign_extend_s24) - src_polyphase_stage_cir_s24(&s1); - else - src_polyphase_stage_cir(&s1); - - } + s1.times = cd->param.stage1_times; + s1_blk_in = s1.times * cd->src.stage1->blk_in * nch; + s1_blk_out = s1.times * cd->src.stage1->blk_out * nch; + if ((avail >= s1_blk_in) && (sbuf_free >= s1_blk_out)) { + cd->polyphase_func(&s1); + cd->sbuf_w_ptr = s1.y_wptr; + cd->sbuf_avail += s1_blk_out; n_read += s1_blk_in; + avail -= s1_blk_in; + sbuf_free -= s1_blk_out; + n1 = s1.times; + } + + /* Run one block at time the remaining data for 1st stage. */ + s1.times = 1; + s1_blk_in = cd->src.stage1->blk_in * nch; + s1_blk_out = cd->src.stage1->blk_out * nch; + while ((n1 < cd->param.stage1_times_max) && (avail >= s1_blk_in) + && (sbuf_free >= s1_blk_out)) { + cd->polyphase_func(&s1); + cd->sbuf_w_ptr = s1.y_wptr; cd->sbuf_avail += s1_blk_out; + n_read += s1_blk_in; + avail -= s1_blk_in; sbuf_free -= s1_blk_out; - src = s1.x_rptr - nch + 1; - cd->sbuf_w_ptr = s1.y_wptr - nch + 1; - src_circ_dec_wrap(&src, source->addr, source->size); - src_circ_dec_wrap(&cd->sbuf_w_ptr, sbuf_addr, sbuf_size); - n1++; + n1 += s1.times; + } + + /* Test if 2nd stage can be run with default block length. */ + s2.times = cd->param.stage2_times; + s2_blk_in = s2.times * cd->src.stage2->blk_in * nch; + s2_blk_out = s2.times * cd->src.stage2->blk_out * nch; + if ((cd->sbuf_avail >= s2_blk_in) && (free >= s2_blk_out)) { + cd->polyphase_func(&s2); + cd->sbuf_r_ptr = s2.x_rptr; + cd->sbuf_avail -= s2_blk_in; + free -= s2_blk_out; + n_written += s2_blk_out; + n2 = s2.times; } - /* 2nd stage runs as many min size blocks as buffers allow */ + + /* Run one block at time the remaining 2nd stage output */ + s2.times = 1; + s2_blk_in = cd->src.stage2->blk_in * nch; + s2_blk_out = cd->src.stage2->blk_out * nch; while ((n2 < cd->param.stage2_times_max) && (cd->sbuf_avail >= s2_blk_in) - && (n_written <= sink_check)) { - for (j = 0; j < nch; j++) { - s2.x_rptr = cd->sbuf_r_ptr++; - s2.y_wptr = dest++; - src_circ_inc_wrap(&cd->sbuf_r_ptr, sbuf_end_addr, sbuf_size); - src_circ_inc_wrap(&dest, sink->end_addr, sink->size); - s = &cd->src[j]; /* Point to src[] for this channel */ - s2.state = &s->state2; - s2.stage = s->stage2; - if (cd->sign_extend_s24) - src_polyphase_stage_cir_s24(&s2); - else - src_polyphase_stage_cir(&s2); - - } - cd->sbuf_r_ptr = s2.x_rptr - nch + 1; - dest = s2.y_wptr - nch + 1; - src_circ_dec_wrap(&cd->sbuf_r_ptr, sbuf_addr, sbuf_size); - src_circ_dec_wrap(&dest, sink->addr, sink->size); - n_written += s2_blk_out; + && (free >= s2_blk_out)) { + cd->polyphase_func(&s2); + cd->sbuf_r_ptr = s2.x_rptr; cd->sbuf_avail -= s2_blk_in; - n2++; + free -= s2_blk_out; + n_written += s2_blk_out; + n2 += s2.times; } *bytes_read = sizeof(int32_t) * n_read; *bytes_written = sizeof(int32_t) * n_written; @@ -189,42 +193,27 @@ static void src_1s_s32_default(struct comp_dev *dev, struct comp_buffer *source, struct comp_buffer *sink, size_t *bytes_read, size_t *bytes_written) { - struct polyphase_src *s; struct src_stage_prm s1; - int j; struct comp_data *cd = comp_get_drvdata(dev); - int32_t *dest = (int32_t *) sink->w_ptr; - int32_t *src = (int32_t *) source->r_ptr; int nch = dev->params.channels; int n_read = 0; int n_written = 0; s1.times = cd->param.stage1_times; + s1.x_rptr = (int32_t *) source->r_ptr; s1.x_end_addr = source->end_addr; s1.x_size = source->size; - s1.x_inc = nch; + s1.y_wptr = (int32_t *) sink->w_ptr; s1.y_end_addr = sink->end_addr; s1.y_size = sink->size; - s1.y_inc = nch; - s1.x_rptr = src + nch - 1; - s1.y_wptr = dest + nch - 1; - - for (j = 0; j < nch; j++) { - s = &cd->src[j]; /* Point to src for this channel */ - s1.x_rptr = src++; - s1.y_wptr = dest++; - src_circ_inc_wrap(&src, source->end_addr, source->size); - src_circ_inc_wrap(&dest, sink->end_addr, sink->size); - s1.state = &s->state1; - s1.stage = s->stage1; - if (cd->sign_extend_s24) - src_polyphase_stage_cir_s24(&s1); - else - src_polyphase_stage_cir(&s1); - - n_read += cd->param.blk_in; - n_written += cd->param.blk_out; - } + s1.state = &cd->src.state1; + s1.stage = cd->src.stage1; + s1.nch = dev->params.channels; + + cd->polyphase_func(&s1); + + n_read += nch * cd->param.blk_in; + n_written += nch * cd->param.blk_out; *bytes_read = n_read * sizeof(int32_t); *bytes_written = n_written * sizeof(int32_t); } @@ -271,7 +260,6 @@ static struct comp_dev *src_new(struct sof_ipc_comp *comp) struct sof_ipc_comp_src *src; struct sof_ipc_comp_src *ipc_src = (struct sof_ipc_comp_src *) comp; struct comp_data *cd; - int i; trace_src("new"); @@ -299,8 +287,8 @@ static struct comp_dev *src_new(struct sof_ipc_comp *comp) cd->delay_lines = NULL; cd->src_func = src_2s_s32_default; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) - src_polyphase_reset(&cd->src[i]); + cd->polyphase_func = src_polyphase_stage_cir; + src_polyphase_reset(&cd->src); dev->state = COMP_STATE_READY; return dev; @@ -334,10 +322,8 @@ static int src_params(struct comp_dev *dev) uint32_t sink_rate; int32_t *buffer_start; int n = 0; - int i; int err; int frames_is_for_source; - int nch; int q; trace_src("par"); @@ -345,10 +331,10 @@ static int src_params(struct comp_dev *dev) /* SRC supports S24_4LE and S32_LE formats */ switch (config->frame_fmt) { case SOF_IPC_FRAME_S24_4LE: - cd->sign_extend_s24 = 1; + cd->polyphase_func = src_polyphase_stage_cir_s24; break; case SOF_IPC_FRAME_S32_LE: - cd->sign_extend_s24 = 0; + cd->polyphase_func = src_polyphase_stage_cir; break; default: trace_src_error("sr0"); @@ -407,11 +393,7 @@ static int src_params(struct comp_dev *dev) buffer_start = cd->delay_lines + cd->param.sbuf_length; /* Initize SRC for actual sample rate */ - nch = MIN(params->channels, PLATFORM_MAX_CHANNELS); - for (i = 0; i < nch; i++) { - n = src_polyphase_init(&cd->src[i], &cd->param, buffer_start); - buffer_start += cd->param.single_src; - } + n = src_polyphase_init(&cd->src, &cd->param, buffer_start); /* Reset stage buffer */ cd->sbuf_r_ptr = cd->delay_lines; @@ -434,7 +416,7 @@ static int src_params(struct comp_dev *dev) * muted if copy() is run. */ trace_src("SFa"); - cd->src_func = fallback_s32; + cd->src_func = src_fallback; return -EINVAL; break; } @@ -532,7 +514,6 @@ static int src_copy(struct comp_dev *dev) /* Run SRC function if buffers avail and free allow */ if (((int) source->avail >= need_source) && ((int) sink->free >= need_sink)) { - /* Run src */ cd->src_func(dev, source, sink, &consumed, &produced); /* Calc new free and available if data was processed. These @@ -561,14 +542,12 @@ static int src_preload(struct comp_dev *dev) static int src_reset(struct comp_dev *dev) { - int i; struct comp_data *cd = comp_get_drvdata(dev); trace_src("SRe"); cd->src_func = src_2s_s32_default; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) - src_polyphase_reset(&cd->src[i]); + src_polyphase_reset(&cd->src); comp_set_state(dev, COMP_CMD_RESET); return 0; @@ -576,7 +555,8 @@ static int src_reset(struct comp_dev *dev) struct comp_driver comp_src = { .type = SOF_COMP_SRC, - .ops = { + .ops = + { .new = src_new, .free = src_free, .params = src_params, diff --git a/src/audio/src_core.c b/src/audio/src_core.c index 4ee2efd..0d8cc17 100644 --- a/src/audio/src_core.c +++ b/src/audio/src_core.c @@ -51,6 +51,10 @@ int sof_rates[SOF_RATES_LENGTH] = {8000, 11025, 12000, 16000, 18900, 22050, 24000, 32000, 44100, 48000, 64000, 88200, 96000, 176400, 192000}; +/* The FIR maximum lenghts are per channel so need to multiply them */ +#define MAX_FIR_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_FIR_DELAY_SIZE) +#define MAX_OUT_DELAY_SIZE_XNCH (PLATFORM_MAX_CHANNELS * MAX_OUT_DELAY_SIZE) + /* Calculate ceil() for integer division */ int src_ceil_divide(int a, int b) { @@ -131,6 +135,10 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, int num; int frames2; + if (nch > PLATFORM_MAX_CHANNELS) + return -EINVAL; + + a->nch = nch; a->idx_in = src_find_fs(src_in_fs, NUM_IN_FS, fs_in); a->idx_out = src_find_fs(src_out_fs, NUM_OUT_FS, fs_out); @@ -146,8 +154,8 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, stage1 = src_table1[a->idx_out][a->idx_in]; stage2 = src_table2[a->idx_out][a->idx_in]; - a->fir_s1 = src_fir_delay_length(stage1); - a->out_s1 = src_out_delay_length(stage1); + a->fir_s1 = nch * src_fir_delay_length(stage1); + a->out_s1 = nch * src_out_delay_length(stage1); /* Find out how many additional times the SRC can be executed while having block size less or equal to max_frames. @@ -191,8 +199,8 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, a->stage2_times_max = 0; a->sbuf_length = 0; } else { - a->fir_s2 = src_fir_delay_length(stage2); - a->out_s2 = src_out_delay_length(stage2); + a->fir_s2 = nch * src_fir_delay_length(stage2); + a->out_s2 = nch * src_out_delay_length(stage2); /* 2x is an empirically tested length. Since the sink buffer * capability to receive samples varies a shorter stage 2 output * block will create a peak in internal buffer usage. @@ -200,8 +208,8 @@ int src_buffer_lengths(struct src_param *a, int fs_in, int fs_out, int nch, a->sbuf_length = 2 * nch * stage1->blk_out * a->stage1_times_max; } - a->single_src = a->fir_s1 + a->fir_s2 + a->out_s1 + a->out_s2; - a->total = a->sbuf_length + nch * a->single_src; + a->src_multich = a->fir_s1 + a->fir_s2 + a->out_s1 + a->out_s2; + a->total = a->sbuf_length + a->src_multich; return 0; } @@ -212,8 +220,6 @@ static void src_state_reset(struct src_state *state) state->fir_delay_size = 0; state->out_delay_size = 0; state->fir_wi = 0; - state->fir_ri = 0; - state->out_wi = 0; state->out_ri = 0; } @@ -229,17 +235,8 @@ static int init_stages( src->number_of_stages = n; src->stage1 = stage1; src->stage2 = stage2; - src->blk_in = p->blk_in; - src->blk_out = p->blk_out; - if (n == 1) { - src->stage1_times = p->stage1_times; - src->stage2_times = 0; - if (stage1->blk_out == 0) - return -EINVAL; - } else { - src->stage1_times = p->stage1_times; - src->stage2_times = p->stage2_times; - } + if ((n == 1) && (stage1->blk_out == 0)) + return -EINVAL; /* Delay line sizes */ src->state1.fir_delay_size = p->fir_s1; @@ -262,10 +259,10 @@ static int init_stages( } /* Check the sizes are less than MAX */ - if ((src->state1.fir_delay_size > MAX_FIR_DELAY_SIZE) - || (src->state1.out_delay_size > MAX_OUT_DELAY_SIZE) - || (src->state2.fir_delay_size > MAX_FIR_DELAY_SIZE) - || (src->state2.out_delay_size > MAX_OUT_DELAY_SIZE)) { + if ((src->state1.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH) + || (src->state1.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH) + || (src->state2.fir_delay_size > MAX_FIR_DELAY_SIZE_XNCH) + || (src->state2.out_delay_size > MAX_OUT_DELAY_SIZE_XNCH)) { src->state1.fir_delay = NULL; src->state1.out_delay = NULL; src->state2.fir_delay = NULL; @@ -279,12 +276,7 @@ static int init_stages( void src_polyphase_reset(struct polyphase_src *src) { - src->mute = 0; src->number_of_stages = 0; - src->blk_in = 0; - src->blk_out = 0; - src->stage1_times = 0; - src->stage2_times = 0; src->stage1 = NULL; src->stage2 = NULL; src_state_reset(&src->state1); @@ -300,8 +292,6 @@ int src_polyphase_init(struct polyphase_src *src, struct src_param *p, struct src_stage *stage2; if ((p->idx_in < 0) || (p->idx_out < 0)) { - src->blk_in = p->blk_in; - src->blk_out = p->blk_out; return -EINVAL; } @@ -335,141 +325,203 @@ int src_polyphase_init(struct polyphase_src *src, struct src_param *p, #if SRC_SHORT == 1 /* Calculate a FIR filter part that does not need circular modification */ -static inline void fir_part(int64_t *y, int ntaps, const int16_t c[], int *ic, - int32_t d[], int *id) + +static inline void fir_part(int64_t y[], int *id, int *ic, + const int32_t data[], const int16_t coef[], int nch_x_taps, int nch) { + int64_t tap0; + int64_t tap1; int n; int64_t a = 0; + int64_t b = 0; + int c = *ic; + int d = *id; + int d_end = d - nch_x_taps; /* Data is Q1.31, coef is Q1.15, product is Q2.46 */ - for (n = 0; n < (ntaps >> 1); n++) { - a += (int64_t) c[*ic] * d[*id] - + (int64_t) c[*ic + 1] * d[*id - 1]; - *ic += 2; - *id -= 2; + if (nch == 2) { + for (n = 0; n < (nch_x_taps >> 2); n++) { + tap0 = coef[c++]; + tap1 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + b += data[d--] * tap1; + a += data[d--] * tap1; + } + if (d > d_end) { + tap0 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + } + y[1] += b; + y[0] += a; + } else { + while (d > d_end) { + tap0 = coef[c++]; + for (n = nch - 1; n >= 0; n--) + y[n] += data[d--] * tap0; + } } - if (ntaps & 1) - a += (int64_t) c[(*ic)++] * d[(*id)--]; - - *y += a; + *ic = c; + *id = d; } + #else -/* Calculate a FIR filter part that does not need circular modification */ -static inline void fir_part(int64_t *y, int ntaps, const int32_t c[], int *ic, - int32_t d[], int *id) +static inline void fir_part(int64_t y[], int *id, int *ic, + const int32_t data[], const int32_t coef[], int nch_x_taps, int nch) { + int64_t tap0; + int64_t tap1; int n; int64_t a = 0; - - /* Data is Q8.24, coef is Q1.23, product is Q9.47 */ - for (n = 0; n < (ntaps >> 1); n++) { - a += (int64_t) c[*ic] * d[*id] - + (int64_t) c[*ic + 1] * d[*id - 1]; - *ic += 2; - *id -= 2; + int64_t b = 0; + int c = *ic; + int d = *id; + int d_end = d - nch_x_taps; + + /* Data is Q1.31, coef is Q1.23, product is Q2.54 */ + if (nch == 2) { + for (n = 0; n < (nch_x_taps >> 2); n++) { + tap0 = coef[c++]; + tap1 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + b += data[d--] * tap1; + a += data[d--] * tap1; + } + if (d > d_end) { + tap0 = coef[c++]; + b += data[d--] * tap0; + a += data[d--] * tap0; + } + y[1] += b; + y[0] += a; + } else { + while (d > d_end) { + tap0 = coef[c++]; + for (n = nch - 1; n >= 0; n--) + y[n] += data[d--] * tap0; + } } - if (ntaps & 1) - a += (int64_t) c[(*ic)++] * d[(*id)--]; - - *y += a; + *ic = c; + *id = d; } + #endif #if SRC_SHORT == 1 -static inline int32_t fir_filter( - struct src_state *fir, const int16_t coefs[], - int *coefi, int filter_length, int shift) +static void fir_filter(int ri0, int *ci, int wi0, int32_t in_delay[], + int32_t out_delay[], const int16_t coefs[], int dsm1, int taps, + int shift, int nch) { - int64_t y = 0; - int n1; int n2; - - n1 = fir->fir_ri + 1; - if (n1 > filter_length) { - /* No need to un-wrap fir read index, make sure fir_fi - * is ge 0 after FIR computation. - */ - fir_part(&y, filter_length, coefs, coefi, fir->fir_delay, - &fir->fir_ri); + int i; + int64_t y[PLATFORM_MAX_CHANNELS]; + int ri = ri0; + int wi = wi0; + int n1 = ri0 + 1; /* Convert to number of sequential frames */ + int qshift = 15 + shift; /* Q2.46 -> Q2.31 */ + int32_t rnd = 1 << (qshift - 1); /* Half LSB */ + int nch_x_taps = nch * taps; + + /* Initialize to half LSB for rounding */ + for (i = 0; i < nch; i++) + y[i] = rnd; + + if (n1 >= nch_x_taps) { + fir_part(y, &ri, ci, in_delay, coefs, nch_x_taps, nch); } else { - n2 = filter_length - n1; - /* Part 1, loop n1 times, fir_ri becomes -1 */ - fir_part(&y, n1, coefs, coefi, fir->fir_delay, &fir->fir_ri); - - /* Part 2, unwrap fir_ri, continue rest of filter */ - fir->fir_ri = fir->fir_delay_size - 1; - fir_part(&y, n2, coefs, coefi, fir->fir_delay, &fir->fir_ri); + n2 = nch_x_taps - n1; + fir_part(y, &ri, ci, in_delay, coefs, n1, nch); + ri = dsm1; + fir_part(y, &ri, ci, in_delay, coefs, n2, nch); } - /* Q2.46 -> Q2.31, saturate to Q1.31 */ - y = y >> (15 + shift); - return(int32_t) sat_int32(y); + for (i = 0; i < nch; i++) + out_delay[wi++] = sat_int32(y[i] >> qshift); } #else -static inline int32_t fir_filter( - struct src_state *fir, const int32_t coefs[], - int *coefi, int filter_length, int shift) +static void fir_filter(int ri0, int *ci, int wi0, int32_t in_delay[], + int32_t out_delay[], const int32_t coefs[], int dsm1, int taps, + int shift, int nch) { - int64_t y = 0; - int n1; int n2; - - n1 = fir->fir_ri + 1; - if (n1 > filter_length) { - /* No need to un-wrap fir read index, make sure fir_fi - * is ge 0 after FIR computation. - */ - fir_part(&y, filter_length, coefs, coefi, fir->fir_delay, - &fir->fir_ri); + int i; + int64_t y[PLATFORM_MAX_CHANNELS]; + int ri = ri0; + int wi = wi0; + int n1 = ri0 + 1; /* Convert to number of sequential frames */ + int qshift = 23 + shift; /* Q2.54 -> Q2.31 */ + int32_t rnd = 1 << (qshift - 1); /* Half LSB */ + int nch_x_taps = nch * taps; + + /* Initialize to half LSB for rounding */ + for (i = 0; i < nch; i++) + y[i] = rnd; + + if (n1 >= nch_x_taps) { + fir_part(y, &ri, ci, in_delay, coefs, nch_x_taps, nch); } else { - n2 = filter_length - n1; - /* Part 1, loop n1 times, fir_ri becomes -1 */ - fir_part(&y, n1, coefs, coefi, fir->fir_delay, &fir->fir_ri); - - /* Part 2, unwrap fir_ri, continue rest of filter */ - fir->fir_ri = fir->fir_delay_size - 1; - fir_part(&y, n2, coefs, coefi, fir->fir_delay, &fir->fir_ri); + n2 = nch_x_taps - n1; + fir_part(y, &ri, ci, in_delay, coefs, n1, nch); + ri = dsm1; + fir_part(y, &ri, ci, in_delay, coefs, n2, nch); } - /* Q9.47 -> Q9.24, saturate to Q8.24 */ - y = y >> (23 + shift); - return(int32_t) sat_int32(y); + for (i = 0; i < nch; i++) + out_delay[wi++] = sat_int32(y[i] >> qshift); + } + #endif void src_polyphase_stage_cir(struct src_stage_prm * s) { - struct src_state *fir = s->state; - struct src_stage *cfg = s->stage; int n; int m; int f; - int c; - int r; + int ci; + int ri; int n_wrap_fir; int n_wrap_buf; int n_wrap_min; int n_min; - int32_t z; + int wi; + + struct src_state *fir = s->state; + struct src_stage *cfg = s->stage; + const void *coef = cfg->coefs; + int32_t *in_delay = fir->fir_delay; + int32_t *out_delay = fir->out_delay; + int dsm1 = fir->fir_delay_size - 1; + int shift = cfg->shift; + int nch = s->nch; + int rewind = -nch * (cfg->blk_in + + (cfg->num_of_subfilters - 1) * cfg->idm) + nch - 1; + int nch_x_idm = cfg->idm * nch; + int nch_x_odm = cfg->odm * nch; + size_t sz = sizeof(int32_t); + int blk_in_bytes = nch * cfg->blk_in * sz; + int blk_out_bytes = nch * cfg->num_of_subfilters * sz; + for (n = 0; n < s->times; n++) { /* Input data */ - m = s->x_inc * cfg->blk_in; + m = blk_in_bytes; while (m > 0) { - n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) - * s->x_inc; + n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) * sz; n_wrap_buf = s->x_end_addr - s->x_rptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { fir->fir_delay[fir->fir_wi++] = *s->x_rptr; - s->x_rptr += s->x_inc; - n_min -= s->x_inc; - m -= s->x_inc; + s->x_rptr++; + n_min -= sz; + m -= sz; } /* Check for wrap */ src_circ_inc_wrap(&s->x_rptr, s->x_end_addr, s->x_size); @@ -478,41 +530,38 @@ void src_polyphase_stage_cir(struct src_stage_prm * s) } /* Filter */ - c = 0; - r = fir->fir_wi - cfg->blk_in - - (cfg->num_of_subfilters - 1) * cfg->idm; - if (r < 0) - r += fir->fir_delay_size; + ci = 0; /* Reset to 1st coefficient */ + ri = fir->fir_wi + rewind; /* Newest data for last subfilter */ + if (ri < 0) + ri += fir->fir_delay_size; - fir->out_wi = fir->out_ri; + wi = fir->out_ri; for (f = 0; f < cfg->num_of_subfilters; f++) { - fir->fir_ri = r; - z = fir_filter(fir, cfg->coefs, &c, - cfg->subfilter_length, cfg->shift); - r += cfg->idm; - if (r >= fir->fir_delay_size) - r -= fir->fir_delay_size; - - fir->out_delay[fir->out_wi] = z; - fir->out_wi += cfg->odm; - if (fir->out_wi >= fir->out_delay_size) - fir->out_wi -= fir->out_delay_size; + fir_filter(ri, &ci, wi, in_delay, out_delay, coef, + dsm1, cfg->subfilter_length, shift, nch); + + wi += nch_x_odm; + if (wi >= fir->out_delay_size) + wi -= fir->out_delay_size; + + ri += nch_x_idm; /* Next sub-filter start */ + if (ri >= fir->fir_delay_size) + ri -= fir->fir_delay_size; } /* Output */ - m = s->y_inc * cfg->num_of_subfilters; + m = blk_out_bytes; while (m > 0) { - n_wrap_fir = (fir->out_delay_size - fir->out_ri) - * s->y_inc; + n_wrap_fir = (fir->out_delay_size - fir->out_ri) * sz; n_wrap_buf = s->y_end_addr - s->y_wptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { *s->y_wptr = fir->out_delay[fir->out_ri++]; - s->y_wptr += s->y_inc; - n_min -= s->y_inc; - m -= s->y_inc; + s->y_wptr++; + n_min -= sz; + m -= sz; } /* Check wrap */ src_circ_inc_wrap(&s->y_wptr, s->y_end_addr, s->y_size); @@ -522,38 +571,49 @@ void src_polyphase_stage_cir(struct src_stage_prm * s) } } -void src_polyphase_stage_cir_s24(struct src_stage_prm * s) +void src_polyphase_stage_cir_s24(struct src_stage_prm *s) { - struct src_state *fir = s->state; - struct src_stage *cfg = s->stage; int n; int m; int f; - int c; - int r; + int ci; + int ri; int n_wrap_fir; int n_wrap_buf; int n_wrap_min; int n_min; - int32_t z; - int32_t se; + int wi; + + struct src_state *fir = s->state; + struct src_stage *cfg = s->stage; + const void *coef = cfg->coefs; + int32_t *in_delay = fir->fir_delay; + int32_t *out_delay = fir->out_delay; + int dsm1 = fir->fir_delay_size - 1; + int shift = cfg->shift; + int nch = s->nch; + int rewind = -nch * (cfg->blk_in + + (cfg->num_of_subfilters - 1) * cfg->idm) + nch - 1; + int nch_x_idm = cfg->idm * nch; + int nch_x_odm = cfg->odm * nch; + size_t sz = sizeof(int32_t); + int blk_in_bytes = nch * cfg->blk_in * sz; + int blk_out_bytes = nch * cfg->num_of_subfilters * sz; for (n = 0; n < s->times; n++) { /* Input data */ - m = s->x_inc * cfg->blk_in; + m = blk_in_bytes; while (m > 0) { - n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) - * s->x_inc; + n_wrap_fir = (fir->fir_delay_size - fir->fir_wi) * sz; n_wrap_buf = s->x_end_addr - s->x_rptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { - se = *s->x_rptr << 8; - fir->fir_delay[fir->fir_wi++] = se >> 8; - s->x_rptr += s->x_inc; - n_min -= s->x_inc; - m -= s->x_inc; + fir->fir_delay[fir->fir_wi++] = *s->x_rptr << 8; + s->x_rptr++; + n_min -= sz; + m -= sz; } /* Check for wrap */ src_circ_inc_wrap(&s->x_rptr, s->x_end_addr, s->x_size); @@ -562,41 +622,38 @@ void src_polyphase_stage_cir_s24(struct src_stage_prm * s) } /* Filter */ - c = 0; - r = fir->fir_wi - cfg->blk_in - - (cfg->num_of_subfilters - 1) * cfg->idm; - if (r < 0) - r += fir->fir_delay_size; + ci = 0; /* Reset to 1st coefficient */ + ri = fir->fir_wi + rewind; /* Newest data for last subfilter */ + if (ri < 0) + ri += fir->fir_delay_size; - fir->out_wi = fir->out_ri; + wi = fir->out_ri; for (f = 0; f < cfg->num_of_subfilters; f++) { - fir->fir_ri = r; - z = fir_filter(fir, cfg->coefs, &c, - cfg->subfilter_length, cfg->shift); - r += cfg->idm; - if (r >= fir->fir_delay_size) - r -= fir->fir_delay_size; - - fir->out_delay[fir->out_wi] = z; - fir->out_wi += cfg->odm; - if (fir->out_wi >= fir->out_delay_size) - fir->out_wi -= fir->out_delay_size; + fir_filter(ri, &ci, wi, in_delay, out_delay, coef, + dsm1, cfg->subfilter_length, shift, nch); + + wi += nch_x_odm; + if (wi >= fir->out_delay_size) + wi -= fir->out_delay_size; + + ri += nch_x_idm; /* Next sub-filter start */ + if (ri >= fir->fir_delay_size) + ri -= fir->fir_delay_size; } /* Output */ - m = s->y_inc * cfg->num_of_subfilters; + m = blk_out_bytes; while (m > 0) { - n_wrap_fir = (fir->out_delay_size - fir->out_ri) - * s->y_inc; + n_wrap_fir = (fir->out_delay_size - fir->out_ri) * sz; n_wrap_buf = s->y_end_addr - s->y_wptr; n_wrap_min = (n_wrap_fir < n_wrap_buf) ? n_wrap_fir : n_wrap_buf; n_min = (m < n_wrap_min) ? m : n_wrap_min; while (n_min > 0) { - *s->y_wptr = fir->out_delay[fir->out_ri++]; - s->y_wptr += s->y_inc; - n_min -= s->y_inc; - m -= s->y_inc; + *s->y_wptr = fir->out_delay[fir->out_ri++] >> 8; + s->y_wptr++; + n_min -= sz; + m -= sz; } /* Check wrap */ src_circ_inc_wrap(&s->y_wptr, s->y_end_addr, s->y_size); @@ -604,47 +661,5 @@ void src_polyphase_stage_cir_s24(struct src_stage_prm * s) fir->out_ri = 0; } } -} - -#ifdef MODULE_TEST - -void src_print_info(struct polyphase_src * src) -{ - - int n1; - int n2; - - n1 = src->stage1->filter_length; - n2 = src->stage2->filter_length; - printf("SRC stages %d\n", src->number_of_stages); - printf("SRC input blk %d\n", src->blk_in); - printf("SRC output blk %d\n", src->blk_out); - printf("SRC stage1 %d times\n", src->stage1_times); - printf("SRC stage2 %d times\n", src->stage2_times); - - printf("SRC1 filter length %d\n", n1); - printf("SRC1 subfilter length %d\n", src->stage1->subfilter_length); - printf("SRC1 number of subfilters %d\n", - src->stage1->num_of_subfilters); - printf("SRC1 idm %d\n", src->stage1->idm); - printf("SRC1 odm %d\n", src->stage1->odm); - printf("SRC1 input blk %d\n", src->stage1->blk_in); - printf("SRC1 output blk %d\n", src->stage1->blk_out); - printf("SRC1 halfband %d\n", src->stage1->halfband); - printf("SRC1 FIR delay %d\n", src->state1.fir_delay_size); - printf("SRC1 out delay %d\n", src->state1.out_delay_size); - - printf("SRC2 filter length %d\n", n2); - printf("SRC2 subfilter length %d\n", src->stage2->subfilter_length); - printf("SRC2 number of subfilters %d\n", - src->stage2->num_of_subfilters); - printf("SRC2 idm %d\n", src->stage2->idm); - printf("SRC2 odm %d\n", src->stage2->odm); - printf("SRC2 input blk %d\n", src->stage2->blk_in); - printf("SRC2 output blk %d\n", src->stage2->blk_out); - printf("SRC2 halfband %d\n", src->stage2->halfband); - printf("SRC2 FIR delay %d\n", src->state2.fir_delay_size); - printf("SRC2 out delay %d\n", src->state2.out_delay_size); } -#endif diff --git a/src/audio/src_core.h b/src/audio/src_core.h index 3859e6f..3ea6028 100644 --- a/src/audio/src_core.h +++ b/src/audio/src_core.h @@ -41,7 +41,7 @@ struct src_param { int out_s1; int out_s2; int sbuf_length; - int single_src; + int src_multich; int total; int blk_in; int blk_out; @@ -51,6 +51,7 @@ struct src_param { int stage2_times_max; int idx_in; int idx_out; + int nch; }; struct src_stage { @@ -70,20 +71,13 @@ struct src_state { int fir_delay_size; int out_delay_size; int fir_wi; - int fir_ri; - int out_wi; int out_ri; int32_t *fir_delay; int32_t *out_delay; }; struct polyphase_src { - int mute; int number_of_stages; - int blk_in; - int blk_out; - int stage1_times; - int stage2_times; struct src_stage *stage1; struct src_stage *stage2; struct src_state state1; @@ -91,15 +85,14 @@ struct polyphase_src { }; struct src_stage_prm { + int nch; int times; int32_t *x_rptr; int32_t *x_end_addr; size_t x_size; - int x_inc; int32_t *y_wptr; int32_t *y_end_addr; size_t y_size; - int y_inc; struct src_state *state; struct src_stage *stage; }; @@ -116,31 +109,6 @@ static inline void src_circ_dec_wrap(int32_t **ptr, int32_t *addr, size_t size) *ptr = (int32_t *) ((size_t) * ptr + size); } -static inline void src_polyphase_mute(struct polyphase_src *src) -{ - src->mute = 1; -} - -static inline void src_polyphase_unmute(struct polyphase_src *src) -{ - src->mute = 0; -} - -static inline int src_polyphase_getmute(struct polyphase_src *src) -{ - return src->mute; -} - -static inline int src_polyphase_get_blk_in(struct polyphase_src *src) -{ - return src->blk_in; -} - -static inline int src_polyphase_get_blk_out(struct polyphase_src *src) -{ - return src->blk_out; -} - void src_polyphase_reset(struct polyphase_src *src); int src_polyphase_init(struct polyphase_src *src, struct src_param *p, @@ -162,8 +130,4 @@ int32_t src_output_rates(void); int src_ceil_divide(int a, int b); -#ifdef MODULE_TEST -void src_print_info(struct polyphase_src *src); -#endif - #endif -- 2.11.0

2 1

[Sound-open-firmware] [PATCH] trace: dma: Add atomic and nowait DMA tracing support.
by Liam Girdwood 19 Oct '17

19 Oct '17

Add support for DMA trace to run in atomic and IRQ contexts. Currently DMA trace would sleep between DMA copies and enter atomic state when inserting new tarce data into the trace buffer. This patch adds new DMA _nowait() APIs that dont sleep and _atomic() APIs for trace logging that can be safely called in atomic context. Signed-off-by: Liam Girdwood <liam.r.girdwood(a)linux.intel.com> --- src/audio/dma-trace.c | 144 ++++++++++++++++++++-------------- src/include/reef/audio/dma-trace.h | 1 + src/include/reef/dma.h | 4 + src/include/reef/trace.h | 12 +++ src/ipc/dma-copy.c | 157 ++++++++++++++++++++++++++++++++++--- src/lib/trace.c | 56 ++++++++++++- 6 files changed, 301 insertions(+), 73 deletions(-) diff --git a/src/audio/dma-trace.c b/src/audio/dma-trace.c index 9704733..b0267fa 100644 --- a/src/audio/dma-trace.c +++ b/src/audio/dma-trace.c @@ -47,9 +47,7 @@ static uint64_t trace_work(void *data, uint64_t delay) struct dma_trace_buf *buffer = &d->dmatb; struct dma_sg_config *config = &d->config; unsigned long flags; - int32_t offset = 0; uint32_t avail = buffer->avail; - uint32_t bytes_copied = 0; uint32_t size; uint32_t hsize; uint32_t lsize; @@ -58,53 +56,51 @@ static uint64_t trace_work(void *data, uint64_t delay) if (avail == 0) return DMA_TRACE_US; + /* make sure we dont write more than buffer */ + if (avail > DMA_TRACE_LOCAL_SIZE) + avail = DMA_TRACE_LOCAL_SIZE; + /* copy to host in sections if we wrap */ - while (avail > 0) { - - lsize = hsize = avail; - - /* host buffer wrap ? */ - if (d->host_offset + buffer->avail > d->host_size) - hsize = d->host_offset + buffer->avail - d->host_size; - - /* local buffer wrap ? */ - if (buffer->r_ptr > buffer->w_ptr) - lsize = buffer->end_addr - buffer->r_ptr; - - /* get smallest size */ - if (hsize < lsize) - size = hsize; - else - size = lsize; - - /* writeback trace data */ - dcache_writeback_region((void*)buffer->r_ptr, size); - - /* copy this section to host */ - offset = dma_copy_to_host(&d->dc, config, d->host_offset, - buffer->r_ptr, size); - if (offset < 0) { - trace_buffer_error("ebb"); - goto out; - } - - /* update host pointer and check for wrap */ - d->host_offset += size; - if (d->host_offset + size >= d->host_size) - d->host_offset = 0; - - /* update local pointer and check for wrap */ - buffer->r_ptr += size; - if (buffer->r_ptr >= buffer->end_addr) - buffer->r_ptr = buffer->addr; - - avail -= size; - bytes_copied += size; + lsize = hsize = avail; + + /* host buffer wrap ? */ + if (d->host_offset + avail > d->host_size) + hsize = d->host_size - d->host_offset; + + /* local buffer wrap ? */ + if (buffer->r_ptr + avail > buffer->end_addr) + lsize = buffer->end_addr - buffer->r_ptr; + + /* get smallest size */ + if (hsize < lsize) + size = hsize; + else + size = lsize; + + /* writeback trace data */ + dcache_writeback_region((void*)buffer->r_ptr, size); + + /* copy this section to host */ + size = dma_copy_to_host_nowait(&d->dc, config, d->host_offset, + buffer->r_ptr, size); + if (size < 0) { + trace_buffer_error("ebb"); + goto out; } + /* update host pointer and check for wrap */ + d->host_offset += size; + if (d->host_offset + size >= d->host_size) + d->host_offset = 0; + + /* update local pointer and check for wrap */ + buffer->r_ptr += size; + if (buffer->r_ptr >= buffer->end_addr) + buffer->r_ptr = buffer->addr; + out: spin_lock_irq(&d->lock, flags); - buffer->avail -= bytes_copied; + buffer->avail -= size; spin_unlock_irq(&d->lock, flags); /* reschedule the trace copying work */ @@ -172,32 +168,28 @@ int dma_trace_enable(struct dma_trace_data *d) { /* validate DMA context */ if (d->dc.dmac == NULL || d->dc.chan < 0) { - trace_buffer_error("eem"); + trace_error_atomic(TRACE_CLASS_BUFFER, "eem"); return -ENODEV; } /* TODO: fix crash when enabled */ - //d->enabled = 1; + d->enabled = 1; work_schedule_default(&d->dmat_work, DMA_TRACE_US); return 0; } -void dtrace_event(const char *e, uint32_t length) +static void dtrace_add_event(const char *e, uint32_t length) { - struct dma_trace_buf *buffer = NULL; - int margin = 0; - unsigned long flags; + struct dma_trace_buf *buffer = &trace_data->dmatb; + int margin; - if (trace_data == NULL || length == 0) - return; + margin = buffer->end_addr - buffer->w_ptr; - buffer = &trace_data->dmatb; - if (buffer == NULL) + /* validate */ + if (margin <= 0) { + trace_buffer_error("emm"); return; - - spin_lock_irq(&trace_data->lock, flags); - - margin = buffer->end_addr - buffer->w_ptr; + } /* check for buffer wrap */ if (margin > length) { @@ -216,9 +208,45 @@ void dtrace_event(const char *e, uint32_t length) } buffer->avail += length; +} + +void dtrace_event(const char *e, uint32_t length) +{ + struct dma_trace_buf *buffer = NULL; + unsigned long flags; + + if (trace_data == NULL || length == 0) + return; + + if (!trace_data->enabled) + return; + + buffer = &trace_data->dmatb; + if (buffer == NULL) + return; + + spin_lock_irq(&trace_data->lock, flags); + dtrace_add_event(e, length); spin_unlock_irq(&trace_data->lock, flags); /* schedule copy now if buffer > 50% full */ if (trace_data->enabled && buffer->avail >= (DMA_TRACE_LOCAL_SIZE / 2)) work_reschedule_default(&trace_data->dmat_work, 100); } + +void dtrace_event_atomic(const char *e, uint32_t length) +{ + struct dma_trace_buf *buffer = NULL; + + if (trace_data == NULL || length == 0) + return; + + if (!trace_data->enabled) + return; + + buffer = &trace_data->dmatb; + if (buffer == NULL) + return; + + dtrace_add_event(e, length); +} diff --git a/src/include/reef/audio/dma-trace.h b/src/include/reef/audio/dma-trace.h index 83db484..12d0956 100644 --- a/src/include/reef/audio/dma-trace.h +++ b/src/include/reef/audio/dma-trace.h @@ -69,5 +69,6 @@ int dma_trace_host_buffer(struct dma_trace_data *d, struct dma_sg_elem *elem, int dma_trace_enable(struct dma_trace_data *d); void dtrace_event(const char *e, uint32_t size); +void dtrace_event_atomic(const char *e, uint32_t length); #endif diff --git a/src/include/reef/dma.h b/src/include/reef/dma.h index 33d1c9a..8dd4adf 100644 --- a/src/include/reef/dma.h +++ b/src/include/reef/dma.h @@ -249,9 +249,13 @@ static inline void dma_copy_free(struct dma_copy *dc) /* DMA copy data from host to DSP */ int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, int32_t host_offset, void *local_ptr, int32_t size); +int dma_copy_from_host_nowait(struct dma_copy *dc, struct dma_sg_config *host_sg, + int32_t host_offset, void *local_ptr, int32_t size); /* DMA copy data from DSP to host */ int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, int32_t host_offset, void *local_ptr, int32_t size); +int dma_copy_to_host_nowait(struct dma_copy *dc, struct dma_sg_config *host_sg, + int32_t host_offset, void *local_ptr, int32_t size); #endif diff --git a/src/include/reef/trace.h b/src/include/reef/trace.h index cdd8158..17dff5c 100644 --- a/src/include/reef/trace.h +++ b/src/include/reef/trace.h @@ -95,6 +95,8 @@ void _trace_event(uint32_t event); void _trace_error(uint32_t event); +void _trace_event_atomic(uint32_t event); +void _trace_error_atomic(uint32_t event); void trace_off(void); void trace_init(struct reef * reef); @@ -102,8 +104,11 @@ void trace_init(struct reef * reef); #define trace_event(__c, __e) \ _trace_event(__c | (__e[0] << 16) | (__e[1] <<8) | __e[2]) +#define trace_event_atomic(__c, __e) \ + _trace_event_atomic(__c | (__e[0] << 16) | (__e[1] <<8) | __e[2]) #define trace_value(x) _trace_event(x) +#define trace_value_atomic(x) _trace_event_atomic(x) #define trace_point(x) platform_trace_point(x) @@ -111,17 +116,24 @@ void trace_init(struct reef * reef); #if TRACEV #define tracev_event(__c, __e) trace_event(__c, __e) #define tracev_value(x) _trace_event(x) +#define tracev_event_atomic(__c, __e) trace_event_atomic(__c, __e) +#define tracev_value_atomic(x) _trace_event_atomic(x) #else #define tracev_event(__c, __e) #define tracev_value(x) +#define tracev_event_atomic(__c, __e) +#define tracev_value_atomic(x) #endif /* error tracing */ #if TRACEE #define trace_error(__c, __e) \ _trace_error(__c | (__e[0] << 16) | (__e[1] <<8) | __e[2]) +#define trace_error_atomic(__c, __e) \ + _trace_error_atomic(__c | (__e[0] << 16) | (__e[1] <<8) | __e[2]) #else #define trace_error(__c, __e) +#define trace_error_atomic(__c, __e) #endif #else diff --git a/src/ipc/dma-copy.c b/src/ipc/dma-copy.c index 75dd3bd..5c9ff97 100644 --- a/src/ipc/dma-copy.c +++ b/src/ipc/dma-copy.c @@ -75,8 +75,14 @@ static void dma_complete(void *data, uint32_t type, struct dma_sg_elem *next) if (type == DMA_IRQ_TYPE_LLIST) wait_completed(comp); + + next->size = DMA_RELOAD_END; } +/* Copy DSP memory to host memory. + * copies DSP memory to host in PAGE_SIZE or smaller blocks and waits/sleeps + * between blocks. Cant be used in IRQ context. + */ int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, int32_t host_offset, void *local_ptr, int32_t size) { @@ -85,6 +91,7 @@ int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, struct dma_sg_elem local_sg_elem; int32_t err; int32_t offset = host_offset; + int32_t bytes_copied = 0; if (size <= 0) return 0; @@ -104,7 +111,11 @@ int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, /* configure local DMA elem */ local_sg_elem.dest = host_sg_elem->dest + offset; local_sg_elem.src = (uint32_t)local_ptr; - local_sg_elem.size = HOST_PAGE_SIZE - offset; + if (size >= HOST_PAGE_SIZE - offset) + local_sg_elem.size = HOST_PAGE_SIZE - offset; + else + local_sg_elem.size = size; + list_item_prepend(&local_sg_elem.list, &config.elem_list); /* transfer max PAGE size at a time to SG buffer */ @@ -112,8 +123,13 @@ int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, /* start the DMA */ wait_init(&dc->complete); - dma_set_config(dc->dmac, dc->chan, &config); - dma_start(dc->dmac, dc->chan); + err = dma_set_config(dc->dmac, dc->chan, &config); + if (err < 0) + return err; + + err = dma_start(dc->dmac, dc->chan); + if (err < 0) + return err; /* wait for DMA to complete */ err = wait_for_completion_timeout(&dc->complete); @@ -127,12 +143,15 @@ int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, host_offset += local_sg_elem.size; /* next dest host address is in next host elem */ + host_sg_elem = list_next_item(host_sg_elem, list); local_sg_elem.dest = host_sg_elem->dest; /* local address is continuous */ local_sg_elem.src = (uint32_t)local_ptr + local_sg_elem.size; + bytes_copied += local_sg_elem.size; + /* do we have less than 1 PAGE to copy ? */ if (size >= HOST_PAGE_SIZE) local_sg_elem.size = HOST_PAGE_SIZE; @@ -140,10 +159,65 @@ int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, local_sg_elem.size = size; } - /* new host offset in SG buffer */ - return host_offset; + /* bytes copied */ + return bytes_copied; +} + +/* Copy DSP memory to host memory. + * Copies DSP memory to host in a single PAGE_SIZE or smaller block. Does not + * waits/sleeps and can be used in IRQ context. + */ +int dma_copy_to_host_nowait(struct dma_copy *dc, struct dma_sg_config *host_sg, + int32_t host_offset, void *local_ptr, int32_t size) +{ + struct dma_sg_config config; + struct dma_sg_elem *host_sg_elem; + struct dma_sg_elem local_sg_elem; + int32_t err; + int32_t offset = host_offset; + + if (size <= 0) + return 0; + + /* find host element with host_offset */ + host_sg_elem = sg_get_elem_at(host_sg, &offset); + if (host_sg_elem == NULL) + return -EINVAL; + + /* set up DMA configuration */ + config.direction = DMA_DIR_LMEM_TO_HMEM; + config.src_width = sizeof(uint32_t); + config.dest_width = sizeof(uint32_t); + config.cyclic = 0; + list_init(&config.elem_list); + + /* configure local DMA elem */ + local_sg_elem.dest = host_sg_elem->dest + offset; + local_sg_elem.src = (uint32_t)local_ptr; + if (size >= HOST_PAGE_SIZE - offset) + local_sg_elem.size = HOST_PAGE_SIZE - offset; + else + local_sg_elem.size = size; + + list_item_prepend(&local_sg_elem.list, &config.elem_list); + + /* start the DMA */ + err = dma_set_config(dc->dmac, dc->chan, &config); + if (err < 0) + return err; + + err = dma_start(dc->dmac, dc->chan); + if (err < 0) + return err; + + /* bytes copied */ + return local_sg_elem.size; } +/* Copy host memory to DSP memory. + * Copies host memory to host in PAGE_SIZE or smaller blocks and waits/sleeps + * between blocks. Cant be used in IRQ context. + */ int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, int32_t host_offset, void *local_ptr, int32_t size) { @@ -152,6 +226,7 @@ int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, struct dma_sg_elem local_sg_elem; int32_t err; int32_t offset = host_offset; + int32_t bytes_copied = 0; if (size <= 0) return 0; @@ -171,7 +246,10 @@ int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, /* configure local DMA elem */ local_sg_elem.dest = (uint32_t)local_ptr; local_sg_elem.src = host_sg_elem->src + offset; - local_sg_elem.size = HOST_PAGE_SIZE - offset; + if (size >= HOST_PAGE_SIZE - offset) + local_sg_elem.size = HOST_PAGE_SIZE - offset; + else + local_sg_elem.size = size; list_item_prepend(&local_sg_elem.list, &config.elem_list); /* transfer max PAGE size at a time to SG buffer */ @@ -179,9 +257,14 @@ int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, /* start the DMA */ wait_init(&dc->complete); - dma_set_config(dc->dmac, dc->chan, &config); - dma_start(dc->dmac, dc->chan); - + err = dma_set_config(dc->dmac, dc->chan, &config); + if (err < 0) + return err; + + err = dma_start(dc->dmac, dc->chan); + if (err < 0) + return err; + /* wait for DMA to complete */ err = wait_for_completion_timeout(&dc->complete); if (err < 0) { @@ -200,6 +283,8 @@ int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, /* local address is continuous */ local_sg_elem.dest = (uint32_t)local_ptr + local_sg_elem.size; + bytes_copied += local_sg_elem.size; + /* do we have less than 1 PAGE to copy ? */ if (size >= HOST_PAGE_SIZE) local_sg_elem.size = HOST_PAGE_SIZE; @@ -207,8 +292,58 @@ int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, local_sg_elem.size = size; } - /* new host offset in SG buffer */ - return host_offset; + /* bytes copied */ + return bytes_copied; +} + +/* Copy host memory to DSP memory. + * Copies host memory to DSP in a single PAGE_SIZE or smaller block. Does not + * waits/sleeps and can be used in IRQ context. + */ +int dma_copy_from_host_nowait(struct dma_copy *dc, struct dma_sg_config *host_sg, + int32_t host_offset, void *local_ptr, int32_t size) +{ + struct dma_sg_config config; + struct dma_sg_elem *host_sg_elem; + struct dma_sg_elem local_sg_elem; + int32_t err; + int32_t offset = host_offset; + + if (size <= 0) + return 0; + + /* find host element with host_offset */ + host_sg_elem = sg_get_elem_at(host_sg, &offset); + if (host_sg_elem == NULL) + return -EINVAL; + + /* set up DMA configuration */ + config.direction = DMA_DIR_HMEM_TO_LMEM; + config.src_width = sizeof(uint32_t); + config.dest_width = sizeof(uint32_t); + config.cyclic = 0; + list_init(&config.elem_list); + + /* configure local DMA elem */ + local_sg_elem.dest = (uint32_t)local_ptr; + local_sg_elem.src = host_sg_elem->src + offset; + if (size >= HOST_PAGE_SIZE - offset) + local_sg_elem.size = HOST_PAGE_SIZE - offset; + else + local_sg_elem.size = size; + list_item_prepend(&local_sg_elem.list, &config.elem_list); + + /* start the DMA */ + err = dma_set_config(dc->dmac, dc->chan, &config); + if (err < 0) + return err; + + err = dma_start(dc->dmac, dc->chan); + if (err < 0) + return err; + + /* bytes copied */ + return local_sg_elem.size; } int dma_copy_new(struct dma_copy *dc, int dmac) diff --git a/src/lib/trace.c b/src/lib/trace.c index 6f707c3..c0abf7c 100644 --- a/src/lib/trace.c +++ b/src/lib/trace.c @@ -49,30 +49,66 @@ void _trace_error(uint32_t event) { unsigned long flags; volatile uint64_t *t; + uint64_t dt[2]; + uint64_t time; if (!trace.enable) return; + time = platform_timer_get(platform_timer); + /* save event to DMA tracing buffer */ - _trace_event(event); + dt[0] = time; + dt[1] = event; + dtrace_event((const char*)dt, sizeof(uint64_t) * 2); /* send event by mail box too. */ spin_lock_irq(&trace.lock, flags); /* write timestamp and event to trace buffer */ t = (volatile uint64_t*)(MAILBOX_TRACE_BASE + trace.pos); - t[0] = platform_timer_get(platform_timer); + trace.pos += (sizeof(uint64_t) << 1); + + if (trace.pos > MAILBOX_TRACE_SIZE - sizeof(uint64_t) * 2) + trace.pos = 0; + + spin_unlock_irq(&trace.lock, flags); + + t[0] = time; t[1] = event; /* writeback trace data */ dcache_writeback_region((void*)t, sizeof(uint64_t) * 2); +} + +void _trace_error_atomic(uint32_t event) +{ + volatile uint64_t *t; + uint64_t dt[2]; + uint64_t time; + + if (!trace.enable) + return; + + time = platform_timer_get(platform_timer); + + /* save event to DMA tracing buffer */ + dt[0] = time; + dt[1] = event; + dtrace_event_atomic((const char*)dt, sizeof(uint64_t) * 2); + /* write timestamp and event to trace buffer */ + t = (volatile uint64_t*)(MAILBOX_TRACE_BASE + trace.pos); trace.pos += (sizeof(uint64_t) << 1); if (trace.pos > MAILBOX_TRACE_SIZE - sizeof(uint64_t) * 2) trace.pos = 0; - spin_unlock_irq(&trace.lock, flags); + t[0] = time; + t[1] = event; + + /* writeback trace data */ + dcache_writeback_region((void*)t, sizeof(uint64_t) * 2); } void _trace_event(uint32_t event) @@ -87,10 +123,22 @@ void _trace_event(uint32_t event) dtrace_event((const char*)dt, sizeof(uint64_t) * 2); } +void _trace_event_atomic(uint32_t event) +{ + uint64_t dt[2]; + + if (!trace.enable) + return; + + dt[0] = platform_timer_get(platform_timer); + dt[1] = event; + dtrace_event_atomic((const char*)dt, sizeof(uint64_t) * 2); +} + void trace_off(void) { trace.enable = 0; -}; +} void trace_init(struct reef *reef) { -- 2.11.0

1 5

Re: [Sound-open-firmware] Support for Tangier SoC
by Liam Girdwood 16 Oct '17

16 Oct '17

On Sun, 2017-10-15 at 19:51 +0200, Sven Schwermer wrote: > Hi. > > I was wondering if the Tangier SoC (Merrifield platform) is or will be > supported by SOF? I have the Intel Edison compute module in mind. > Yes, it _should_ work on Merrifield, but I don't have Edison HW so some things on the driver side might need some work. The 1.0-dev branch and sof-v4.13 kernel will probably be better for Edison than the stable 0.95 branch atm (mainly due to driver updates), but 1.0-dev is still has a few issues prior to doing an rc1 tag. Liam --------------------------------------------------------------------- Intel Corporation (UK) Limited Registered No. 1134945 (England) Registered Office: Pipers Way, Swindon SN3 1RJ VAT No: 860 2173 47 This e-mail and any attachments may contain confidential material for the sole use of the intended recipient(s). Any review or distribution by others is strictly prohibited. If you are not the intended recipient, please contact the sender and delete all copies.

2 1

[Sound-open-firmware] [PATCH] dma copy: trace: Fix locking with DMA trace and refactor API usage.
by Liam Girdwood 16 Oct '17

16 Oct '17

Fix locking in DMA trace. Also run the DMA trace as delayed work so we dont block callers. Refactor API so that DMAC and channel are pre-allocated. Signed-off-by: Liam Girdwood <liam.r.girdwood(a)linux.intel.com> --- src/audio/dma-trace.c | 101 ++++++++++------------ src/include/reef/alloc.h | 4 +- src/include/reef/audio/dma-trace.h | 5 +- src/include/reef/dma.h | 22 ++++- src/ipc/dma-copy.c | 95 ++++++++++---------- src/ipc/intel-ipc.c | 4 +- src/ipc/ipc.c | 1 - src/lib/alloc.c | 12 +-- src/lib/trace.c | 6 +- src/platform/baytrail/include/platform/platform.h | 3 + 10 files changed, 131 insertions(+), 122 deletions(-) diff --git a/src/audio/dma-trace.c b/src/audio/dma-trace.c index 00a29b4..9704733 100644 --- a/src/audio/dma-trace.c +++ b/src/audio/dma-trace.c @@ -41,51 +41,22 @@ static struct dma_trace_data *trace_data = NULL; -static int dma_trace_new_buffer(struct dma_trace_data *d, uint32_t buffer_size) -{ - struct dma_trace_buf *buffer = &d->dmatb; - - trace_buffer("nlb"); - - /* validate request */ - if (buffer_size == 0 || buffer_size > HEAP_BUFFER_SIZE) { - trace_buffer_error("ebg"); - trace_value(buffer_size); - return -ENOMEM; - } - - /* allocate new buffer */ - buffer->addr = rballoc(RZONE_RUNTIME, RFLAGS_NONE, buffer_size); - if (buffer->addr == NULL) { - trace_buffer_error("ebm"); - return -ENOMEM; - } - - bzero(buffer->addr, buffer_size); - - buffer->size = buffer_size; - buffer->w_ptr = buffer->r_ptr = buffer->addr; - buffer->end_addr = buffer->addr + buffer->size; - buffer->avail = 0; - - return 0; -} - -static void trace_send(struct dma_trace_data *d) +static uint64_t trace_work(void *data, uint64_t delay) { + struct dma_trace_data *d = (struct dma_trace_data *)data; struct dma_trace_buf *buffer = &d->dmatb; struct dma_sg_config *config = &d->config; unsigned long flags; int32_t offset = 0; uint32_t avail = buffer->avail; - uint32_t bytes_copied = avail; + uint32_t bytes_copied = 0; uint32_t size; uint32_t hsize; uint32_t lsize; /* any data to copy ? */ if (avail == 0) - return; + return DMA_TRACE_US; /* copy to host in sections if we wrap */ while (avail > 0) { @@ -106,12 +77,15 @@ static void trace_send(struct dma_trace_data *d) else size = lsize; + /* writeback trace data */ + dcache_writeback_region((void*)buffer->r_ptr, size); + /* copy this section to host */ - offset = dma_copy_to_host(config, d->host_offset, + offset = dma_copy_to_host(&d->dc, config, d->host_offset, buffer->r_ptr, size); if (offset < 0) { trace_buffer_error("ebb"); - return; + goto out; } /* update host pointer and check for wrap */ @@ -125,18 +99,13 @@ static void trace_send(struct dma_trace_data *d) buffer->r_ptr = buffer->addr; avail -= size; + bytes_copied += size; } +out: spin_lock_irq(&d->lock, flags); buffer->avail -= bytes_copied; spin_unlock_irq(&d->lock, flags); -} - -static uint64_t trace_work(void *data, uint64_t delay) -{ - struct dma_trace_data *d = (struct dma_trace_data *)data; - - trace_send(d); /* reschedule the trace copying work */ return DMA_TRACE_US; @@ -144,25 +113,41 @@ static uint64_t trace_work(void *data, uint64_t delay) int dma_trace_init(struct dma_trace_data *d) { - int err; + struct dma_trace_buf *buffer = &d->dmatb; + int ret; trace_buffer("dtn"); - /* init buffer elems */ - list_init(&d->config.elem_list); + /* allocate new buffer */ + buffer->addr = rballoc(RZONE_RUNTIME, RFLAGS_NONE, DMA_TRACE_LOCAL_SIZE); + if (buffer->addr == NULL) { + trace_buffer_error("ebm"); + return -ENOMEM; + } - /* allocate local DMA buffer */ - err = dma_trace_new_buffer(d, DMA_TRACE_LOCAL_SIZE); - if (err < 0) { - trace_buffer_error("ePb"); - return err; + /* init DMA copy context */ + ret = dma_copy_new(&d->dc, PLATFORM_TRACE_DMAC); + if (ret < 0) { + trace_buffer_error("edm"); + rfree(buffer->addr); + return ret; } + bzero(buffer->addr, DMA_TRACE_LOCAL_SIZE); + + /* initialise the DMA buffer */ + buffer->size = DMA_TRACE_LOCAL_SIZE; + buffer->w_ptr = buffer->r_ptr = buffer->addr; + buffer->end_addr = buffer->addr + buffer->size; + buffer->avail = 0; d->host_offset = 0; - trace_data = d; + d->enabled = 0; + list_init(&d->config.elem_list); work_init(&d->dmat_work, trace_work, d, WORK_ASYNC); spinlock_init(&d->lock); + trace_data = d; + return 0; } @@ -183,10 +168,18 @@ int dma_trace_host_buffer(struct dma_trace_data *d, struct dma_sg_elem *elem, return 0; } -void dma_trace_config_ready(struct dma_trace_data *d) +int dma_trace_enable(struct dma_trace_data *d) { + /* validate DMA context */ + if (d->dc.dmac == NULL || d->dc.chan < 0) { + trace_buffer_error("eem"); + return -ENODEV; + } + + /* TODO: fix crash when enabled */ + //d->enabled = 1; work_schedule_default(&d->dmat_work, DMA_TRACE_US); - d->ready = 1; + return 0; } void dtrace_event(const char *e, uint32_t length) @@ -226,6 +219,6 @@ void dtrace_event(const char *e, uint32_t length) spin_unlock_irq(&trace_data->lock, flags); /* schedule copy now if buffer > 50% full */ - if (trace_data->ready && buffer->avail >= (DMA_TRACE_LOCAL_SIZE / 2)) + if (trace_data->enabled && buffer->avail >= (DMA_TRACE_LOCAL_SIZE / 2)) work_reschedule_default(&trace_data->dmat_work, 100); } diff --git a/src/include/reef/alloc.h b/src/include/reef/alloc.h index 4e8a630..982af90 100644 --- a/src/include/reef/alloc.h +++ b/src/include/reef/alloc.h @@ -86,8 +86,8 @@ int rstrlen(const char *s); /* Heap save/restore contents and context for PM D0/D3 events */ uint32_t mm_pm_context_size(void); -int mm_pm_context_save(struct dma_sg_config *sg); -int mm_pm_context_restore(struct dma_sg_config *sg); +int mm_pm_context_save(struct dma_copy *dc, struct dma_sg_config *sg); +int mm_pm_context_restore(struct dma_copy *dc, struct dma_sg_config *sg); /* heap initialisation */ void init_heap(struct reef *reef); diff --git a/src/include/reef/audio/dma-trace.h b/src/include/reef/audio/dma-trace.h index 20f2e18..83db484 100644 --- a/src/include/reef/audio/dma-trace.h +++ b/src/include/reef/audio/dma-trace.h @@ -55,17 +55,18 @@ struct dma_trace_buf { struct dma_trace_data { struct dma_sg_config config; struct dma_trace_buf dmatb; + struct dma_copy dc; int32_t host_offset; uint32_t host_size; struct work dmat_work; - uint32_t ready; + uint32_t enabled; spinlock_t lock; }; int dma_trace_init(struct dma_trace_data *d); int dma_trace_host_buffer(struct dma_trace_data *d, struct dma_sg_elem *elem, uint32_t host_size); -void dma_trace_config_ready(struct dma_trace_data *d); +int dma_trace_enable(struct dma_trace_data *d); void dtrace_event(const char *e, uint32_t size); diff --git a/src/include/reef/dma.h b/src/include/reef/dma.h index 881332f..33d1c9a 100644 --- a/src/include/reef/dma.h +++ b/src/include/reef/dma.h @@ -36,6 +36,7 @@ #include <reef/list.h> #include <reef/lock.h> #include <reef/reef.h> +#include <reef/wait.h> /* DMA directions */ #define DMA_DIR_MEM_TO_MEM 0 /* local memcpy */ @@ -228,12 +229,29 @@ static inline uint32_t dma_sg_get_size(struct dma_sg_config *sg) return size; } +/* generic DMA DSP <-> Host copier */ + +struct dma_copy { + int chan; + struct dma *dmac; + completion_t complete; +}; + +/* init dma copy context */ +int dma_copy_new(struct dma_copy *dc, int dmac); + +/* free dma copy context resources */ +static inline void dma_copy_free(struct dma_copy *dc) +{ + dma_channel_put(dc->dmac, dc->chan); +} + /* DMA copy data from host to DSP */ -int dma_copy_from_host(struct dma_sg_config *host_sg, +int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, int32_t host_offset, void *local_ptr, int32_t size); /* DMA copy data from DSP to host */ -int dma_copy_to_host(struct dma_sg_config *host_sg, +int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, int32_t host_offset, void *local_ptr, int32_t size); #endif diff --git a/src/ipc/dma-copy.c b/src/ipc/dma-copy.c index b345ac9..75dd3bd 100644 --- a/src/ipc/dma-copy.c +++ b/src/ipc/dma-copy.c @@ -38,6 +38,11 @@ #include <reef/wait.h> #include <platform/dma.h> +/* tracing */ +#define trace_dma(__e) trace_event(TRACE_CLASS_DMA, __e) +#define trace_dma_error(__e) trace_error(TRACE_CLASS_DMA, __e) +#define tracev_dma(__e) tracev_event(TRACE_CLASS_DMA, __e) + static struct dma_sg_elem *sg_get_elem_at(struct dma_sg_config *host_sg, int32_t *offset) { @@ -60,7 +65,7 @@ static struct dma_sg_elem *sg_get_elem_at(struct dma_sg_config *host_sg, } /* host offset in beyond end of SG buffer */ - //trace_mem_error("eMs"); + trace_dma_error("ex0"); return NULL; } @@ -72,27 +77,17 @@ static void dma_complete(void *data, uint32_t type, struct dma_sg_elem *next) wait_completed(comp); } -int dma_copy_to_host(struct dma_sg_config *host_sg, int32_t host_offset, - void *local_ptr, int32_t size) +int dma_copy_to_host(struct dma_copy *dc, struct dma_sg_config *host_sg, + int32_t host_offset, void *local_ptr, int32_t size) { struct dma_sg_config config; struct dma_sg_elem *host_sg_elem; struct dma_sg_elem local_sg_elem; - struct dma *dma = dma_get(DMA_ID_DMAC0); - completion_t complete; int32_t err; int32_t offset = host_offset; - int32_t chan; - if (dma == NULL) - return -ENODEV; - - /* get DMA channel from DMAC0 */ - chan = dma_channel_get(dma); - if (chan < 0) { - //trace_ipc_error("ePC"); - return chan; - } + if (size <= 0) + return 0; /* find host element with host_offset */ host_sg_elem = sg_get_elem_at(host_sg, &offset); @@ -100,7 +95,6 @@ int dma_copy_to_host(struct dma_sg_config *host_sg, int32_t host_offset, return -EINVAL; /* set up DMA configuration */ - complete.timeout = 100; /* wait 100 usecs for DMA to finish */ config.direction = DMA_DIR_LMEM_TO_HMEM; config.src_width = sizeof(uint32_t); config.dest_width = sizeof(uint32_t); @@ -113,21 +107,18 @@ int dma_copy_to_host(struct dma_sg_config *host_sg, int32_t host_offset, local_sg_elem.size = HOST_PAGE_SIZE - offset; list_item_prepend(&local_sg_elem.list, &config.elem_list); - dma_set_cb(dma, chan, DMA_IRQ_TYPE_LLIST, dma_complete, &complete); - /* transfer max PAGE size at a time to SG buffer */ while (size > 0) { /* start the DMA */ - wait_init(&complete); - dma_set_config(dma, chan, &config); - dma_start(dma, chan); + wait_init(&dc->complete); + dma_set_config(dc->dmac, dc->chan, &config); + dma_start(dc->dmac, dc->chan); /* wait for DMA to complete */ - err = wait_for_completion_timeout(&complete); + err = wait_for_completion_timeout(&dc->complete); if (err < 0) { - //trace_comp_error("eAp"); - dma_channel_put(dma, chan); + trace_dma_error("ex1"); return -EIO; } @@ -150,31 +141,20 @@ int dma_copy_to_host(struct dma_sg_config *host_sg, int32_t host_offset, } /* new host offset in SG buffer */ - dma_channel_put(dma, chan); return host_offset; } -int dma_copy_from_host(struct dma_sg_config *host_sg, int32_t host_offset, - void *local_ptr, int32_t size) +int dma_copy_from_host(struct dma_copy *dc, struct dma_sg_config *host_sg, + int32_t host_offset, void *local_ptr, int32_t size) { struct dma_sg_config config; struct dma_sg_elem *host_sg_elem; struct dma_sg_elem local_sg_elem; - struct dma *dma = dma_get(DMA_ID_DMAC0); - completion_t complete; int32_t err; int32_t offset = host_offset; - int32_t chan; - if (dma == NULL) - return -ENODEV; - - /* get DMA channel from DMAC0 */ - chan = dma_channel_get(dma); - if (chan < 0) { - //trace_ipc_error("ePC"); - return chan; - } + if (size <= 0) + return 0; /* find host element with host_offset */ host_sg_elem = sg_get_elem_at(host_sg, &offset); @@ -182,7 +162,6 @@ int dma_copy_from_host(struct dma_sg_config *host_sg, int32_t host_offset, return -EINVAL; /* set up DMA configuration */ - complete.timeout = 100; /* wait 100 usecs for DMA to finish */ config.direction = DMA_DIR_HMEM_TO_LMEM; config.src_width = sizeof(uint32_t); config.dest_width = sizeof(uint32_t); @@ -195,21 +174,18 @@ int dma_copy_from_host(struct dma_sg_config *host_sg, int32_t host_offset, local_sg_elem.size = HOST_PAGE_SIZE - offset; list_item_prepend(&local_sg_elem.list, &config.elem_list); - dma_set_cb(dma, chan, DMA_IRQ_TYPE_LLIST, dma_complete, &complete); - /* transfer max PAGE size at a time to SG buffer */ while (size > 0) { /* start the DMA */ - wait_init(&complete); - dma_set_config(dma, chan, &config); - dma_start(dma, chan); + wait_init(&dc->complete); + dma_set_config(dc->dmac, dc->chan, &config); + dma_start(dc->dmac, dc->chan); /* wait for DMA to complete */ - err = wait_for_completion_timeout(&complete); + err = wait_for_completion_timeout(&dc->complete); if (err < 0) { - //trace_comp_error("eAp"); - dma_channel_put(dma, chan); + trace_dma_error("ex2"); return -EIO; } @@ -232,6 +208,27 @@ int dma_copy_from_host(struct dma_sg_config *host_sg, int32_t host_offset, } /* new host offset in SG buffer */ - dma_channel_put(dma, chan); return host_offset; } + +int dma_copy_new(struct dma_copy *dc, int dmac) +{ + dc->dmac = dma_get(dmac); + if (dc->dmac == NULL) { + trace_dma_error("ec0"); + return -ENODEV; + } + + /* get DMA channel from DMAC0 */ + dc->chan = dma_channel_get(dc->dmac); + if (dc->chan < 0) { + trace_dma_error("ec1"); + return dc->chan; + } + + dc->complete.timeout = 100; /* wait 100 usecs for DMA to finish */ + dma_set_cb(dc->dmac, dc->chan, DMA_IRQ_TYPE_LLIST, dma_complete, + &dc->complete); + return 0; +} + diff --git a/src/ipc/intel-ipc.c b/src/ipc/intel-ipc.c index 4b56993..9b47ca9 100644 --- a/src/ipc/intel-ipc.c +++ b/src/ipc/intel-ipc.c @@ -629,7 +629,9 @@ static int ipc_dma_trace_config(uint32_t header) trace_ipc("DAp"); - dma_trace_config_ready(&_ipc->dmat); + err = dma_trace_enable(&_ipc->dmat); + if (err < 0) + goto error; /* write component values to the outbox */ reply.hdr.size = sizeof(reply); diff --git a/src/ipc/ipc.c b/src/ipc/ipc.c index 05de838..6c40d10 100644 --- a/src/ipc/ipc.c +++ b/src/ipc/ipc.c @@ -350,7 +350,6 @@ int ipc_init(struct reef *reef) /* init ipc data */ reef->ipc = rzalloc(RZONE_SYS, RFLAGS_NONE, sizeof(*reef->ipc)); reef->ipc->comp_data = rzalloc(RZONE_SYS, RFLAGS_NONE, SOF_IPC_MSG_MAX_SIZE); - reef->ipc->dmat.ready = 0; list_init(&reef->ipc->comp_list); diff --git a/src/lib/alloc.c b/src/lib/alloc.c index 0d9b07c..63f92de 100644 --- a/src/lib/alloc.c +++ b/src/lib/alloc.c @@ -573,7 +573,7 @@ uint32_t mm_pm_context_size(void) * must be disabled before calling this functions. No allocations are permitted after * calling this and before calling restore. */ -int mm_pm_context_save(struct dma_sg_config *sg) +int mm_pm_context_save(struct dma_copy *dc, struct dma_sg_config *sg) { uint32_t used; int32_t offset = 0; @@ -585,13 +585,13 @@ int mm_pm_context_save(struct dma_sg_config *sg) return -EINVAL; /* copy memory maps to SG */ - ret = dma_copy_to_host(sg, offset, + ret = dma_copy_to_host(dc, sg, offset, (void *)&memmap, sizeof(memmap)); if (ret < 0) return ret; /* copy system memory contents to SG */ - ret = dma_copy_to_host(sg, offset + ret, + ret = dma_copy_to_host(dc, sg, offset + ret, (void *)memmap.system.heap, (int32_t)(memmap.system.size)); if (ret < 0) return ret; @@ -611,19 +611,19 @@ int mm_pm_context_save(struct dma_sg_config *sg) * Restore the DSP memories to modules abd the system. This must be called immediately * after booting before any pipeline work. */ -int mm_pm_context_restore(struct dma_sg_config *sg) +int mm_pm_context_restore(struct dma_copy *dc, struct dma_sg_config *sg) { int32_t offset = 0; int32_t ret; /* copy memory maps from SG */ - ret = dma_copy_from_host(sg, offset, + ret = dma_copy_from_host(dc, sg, offset, (void *)&memmap, sizeof(memmap)); if (ret < 0) return ret; /* copy system memory contents from SG */ - ret = dma_copy_to_host(sg, offset + ret, + ret = dma_copy_to_host(dc, sg, offset + ret, (void *)memmap.system.heap, (int32_t)(memmap.system.size)); if (ret < 0) return ret; diff --git a/src/lib/trace.c b/src/lib/trace.c index bdd50ae..6f707c3 100644 --- a/src/lib/trace.c +++ b/src/lib/trace.c @@ -77,15 +77,11 @@ void _trace_error(uint32_t event) void _trace_event(uint32_t event) { - volatile uint64_t dt[2]; - uint32_t et = (event & 0xff000000); + uint64_t dt[2]; if (!trace.enable) return; - if (et == TRACE_CLASS_DMA) - return; - dt[0] = platform_timer_get(platform_timer); dt[1] = event; dtrace_event((const char*)dt, sizeof(uint64_t) * 2); diff --git a/src/platform/baytrail/include/platform/platform.h b/src/platform/baytrail/include/platform/platform.h index ff3a117..4d63f7b 100644 --- a/src/platform/baytrail/include/platform/platform.h +++ b/src/platform/baytrail/include/platform/platform.h @@ -82,6 +82,9 @@ struct reef; /* the interval of DMA trace copying */ #define DMA_TRACE_US 500000 +/* DMAC used for trace DMA */ +#define PLATFORM_TRACE_DMAC DMA_ID_DMAC0 + /* Platform defined panic code */ #define platform_panic(__x) \ shim_write(SHIM_IPCXL, ((shim_read(SHIM_IPCXL) & 0xc0000000) |\ -- 1.9.1

1 0