Skip to content

Commit beeed9c

Browse files
singalsusingalsu
committed
Fixup to module C code
Signed-off-by: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
1 parent 3273b52 commit beeed9c

5 files changed

Lines changed: 224 additions & 45 deletions

File tree

‎src/audio/phase_vocoder/phase_vocoder-generic.c‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -402,7 +402,7 @@ void phase_vocoder_overlap_add_ifft_buffer(struct phase_vocoder_state *state, in
402402
int i;
403403
int n;
404404
int samples_remain = fft->fft_size;
405-
int idx = fft->fft_fill_start_idx;
405+
int idx = 0;
406406

407407
while (samples_remain) {
408408
n = phase_vocoder_buffer_samples_without_wrap(obuf, w);
@@ -427,11 +427,10 @@ void phase_vocoder_overlap_add_ifft_buffer(struct phase_vocoder_state *state, in
427427
void phase_vocoder_apply_window(struct phase_vocoder_state *state)
428428
{
429429
struct phase_vocoder_fft *fft = &state->fft;
430-
int j;
431-
int i = fft->fft_fill_start_idx;
430+
int i;
432431

433432
/* Multiply Q1.31 by Q1.15 gives Q2.46, shift right by 15 to get Q2.31, no saturate need */
434-
for (j = 0; j < fft->fft_size; j++)
435-
fft->fft_buf[i + j].real = sat_int32(Q_MULTSR_32X32(
436-
(int64_t)fft->fft_buf[i + j].real, state->window[j], 31, 31, 31));
433+
for (i = 0; i < fft->fft_size; i++)
434+
fft->fft_buf[i].real = sat_int32(Q_MULTSR_32X32(
435+
(int64_t)fft->fft_buf[i].real, state->window[i], 31, 31, 31));
437436
}

‎src/audio/phase_vocoder/phase_vocoder.c‎

Lines changed: 14 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,18 @@ FILE *stft_debug_fft_out_fh;
2828
FILE *stft_debug_ifft_out_fh;
2929
#endif
3030

31+
__cold static void phase_vocoder_reset_parameters(struct processing_module *mod)
32+
{
33+
struct phase_vocoder_comp_data *cd = module_get_private_data(mod);
34+
struct module_data *md = &mod->priv;
35+
struct ipc4_base_module_cfg *base_cfg = &md->cfg.base_cfg;
36+
37+
memset(cd, 0, sizeof(*cd));
38+
cd->ibs = base_cfg->ibs;
39+
cd->enable = true; /* processing enabled by default */
40+
cd->speed = PHASE_VOCODER_SPEED_NORMAL;
41+
}
42+
3143
/**
3244
* phase_vocoder_init() - Initialize the phase_vocoder component.
3345
* @mod: Pointer to module data.
@@ -41,7 +53,6 @@ FILE *stft_debug_ifft_out_fh;
4153
__cold static int phase_vocoder_init(struct processing_module *mod)
4254
{
4355
struct module_data *md = &mod->priv;
44-
struct ipc4_base_module_cfg *base_cfg = &md->cfg.base_cfg;
4556
struct comp_dev *dev = mod->dev;
4657
struct phase_vocoder_comp_data *cd;
4758

@@ -54,9 +65,7 @@ __cold static int phase_vocoder_init(struct processing_module *mod)
5465
return -ENOMEM;
5566

5667
md->private = cd;
57-
memset(cd, 0, sizeof(*cd));
58-
cd->ibs = base_cfg->ibs;
59-
cd->enable = true; /* processing enabled by default */
68+
phase_vocoder_reset_parameters(mod);
6069

6170
#if STFT_DEBUG
6271
stft_debug_fft_in_fh = fopen("stft_debug_fft_in.txt", "w");
@@ -191,12 +200,10 @@ static int phase_vocoder_prepare(struct processing_module *mod, struct sof_sourc
191200
*/
192201
static int phase_vocoder_reset(struct processing_module *mod)
193202
{
194-
struct phase_vocoder_comp_data *cd = module_get_private_data(mod);
195-
196203
comp_dbg(mod->dev, "reset");
197204

198205
phase_vocoder_free_buffers(mod);
199-
memset(cd, 0, sizeof(*cd));
206+
phase_vocoder_reset_parameters(mod);
200207
return 0;
201208
}
202209

‎src/audio/phase_vocoder/phase_vocoder.h‎

Lines changed: 24 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,11 @@
2727
#define PHASE_VOCODER_MIN_SPEED_Q29 Q_CONVERT_FLOAT(0.5, 29)
2828
#define PHASE_VOCODER_MAX_SPEED_Q29 Q_CONVERT_FLOAT(2.0, 29)
2929
#define PHASE_VOCODER_SPEED_STEP_Q31 Q_CONVERT_FLOAT((2 - 0.5) / 15, 31)
30+
#define PHASE_VOCODER_SPEED_NORMAL Q_CONVERT_FLOAT(1.0, 29)
31+
32+
#define PHASE_VOCODER_ONE_Q29 Q_CONVERT_FLOAT(1.0, 29)
33+
#define PHASE_VOCODER_PI_Q28 843314857 /* int32(pi * 2^28) */
34+
#define PHASE_VOCODER_TWO_PI_Q28 1686629713 /* int32(2 * pi * 2^28) */
3035

3136
enum sof_phase_vocoder_fft_pad_type {
3237
STFT_PAD_END = 0,
@@ -69,30 +74,44 @@ struct phase_vocoder_buffer {
6974
struct phase_vocoder_fft {
7075
struct icomplex32 *fft_buf; /**< fft_padded_size */
7176
struct icomplex32 *fft_out; /**< fft_padded_size */
72-
struct ipolar32 *fft_polar;
7377
struct fft_plan *fft_plan;
7478
struct fft_plan *ifft_plan;
75-
int fft_fill_start_idx; /**< Set to 0 for pad left, etc. */
7679
int fft_size;
7780
int fft_hop_size;
7881
int fft_buf_size;
7982
int half_fft_size;
8083
size_t fft_buffer_size; /**< bytes */
8184
};
8285

86+
struct phase_vocoder_polar {
87+
struct ipolar32 *polar[PLATFORM_MAX_CHANNELS];
88+
struct ipolar32 *polar_prev[PLATFORM_MAX_CHANNELS];
89+
struct ipolar32 *polar_tmp;
90+
int32_t *angle_delta_prev[PLATFORM_MAX_CHANNELS];
91+
int32_t *angle_delta[PLATFORM_MAX_CHANNELS];
92+
int32_t *output_phase[PLATFORM_MAX_CHANNELS];
93+
};
94+
8395
struct phase_vocoder_state {
8496
struct phase_vocoder_buffer ibuf[PLATFORM_MAX_CHANNELS]; /**< Buffer for input data */
8597
struct phase_vocoder_buffer obuf[PLATFORM_MAX_CHANNELS]; /**< Buffer for output data */
8698
struct phase_vocoder_fft fft; /**< FFT related */
87-
int32_t *prev_data[PLATFORM_MAX_CHANNELS]; /**< prev_data_size */
99+
struct phase_vocoder_polar polar; /**< Processing in polar domain */
100+
int32_t num_input_fft_to_use;
101+
int32_t num_input_fft; /**< Total input FFTs count */
102+
int32_t num_output_ifft; /**< Total output IFFTs count */
103+
int32_t *prev_data[PLATFORM_MAX_CHANNELS]; /**< prev_data_size */
88104
int32_t *buffers;
89-
int32_t *window; /**< fft_size */
90-
int32_t gain_comp; /**< Gain to compensate window gain */
105+
int32_t *window; /**< fft_size */
106+
int32_t gain_comp; /**< Gain to compensate window gain */
107+
int32_t interpolate_fraction; /**< Q3.29 */
91108
int source_channel;
92109
int prev_data_size;
93110
int sample_rate;
94111
bool waiting_fill; /**< booleans */
95112
bool prev_samples_valid;
113+
//bool input_fft_done;
114+
bool output_ifft_done;
96115
};
97116

98117
/**
@@ -125,7 +144,6 @@ struct phase_vocoder_comp_data {
125144
int source_channel;
126145
int max_frames;
127146
int channels;
128-
bool fft_done;
129147
bool enable; /**< Processing enable flag */
130148
};
131149

‎src/audio/phase_vocoder/phase_vocoder_common.c‎

Lines changed: 137 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -42,16 +42,13 @@ LOG_MODULE_REGISTER(phase_vocoder_common, CONFIG_SOF_LOG_LEVEL);
4242
* The main processing function for PHASE_VOCODER
4343
*/
4444

45-
static int stft_prepare_fft(struct phase_vocoder_state *state, int channel)
45+
static int stft_get_num_ffts_avail(struct phase_vocoder_state *state, int channel)
4646
{
4747
struct phase_vocoder_buffer *ibuf = &state->ibuf[channel];
4848
struct phase_vocoder_fft *fft = &state->fft;
4949

5050
/* Wait for FFT hop size of new data */
51-
if (ibuf->s_avail < fft->fft_hop_size)
52-
return 0;
53-
54-
return 1;
51+
return ibuf->s_avail / fft->fft_hop_size;
5552
}
5653

5754
static void stft_do_fft(struct phase_vocoder_state *state, int ch)
@@ -98,20 +95,20 @@ static void stft_do_ifft(struct phase_vocoder_state *state, int ch)
9895
phase_vocoder_overlap_add_ifft_buffer(state, ch);
9996
}
10097

101-
static void stft_convert_to_polar(struct phase_vocoder_fft *fft)
98+
static void stft_convert_to_polar(struct phase_vocoder_fft *fft, struct ipolar32 *polar_data)
10299
{
103100
int i;
104101

105102
for (i = 0; i < fft->half_fft_size; i++)
106-
sofm_icomplex32_to_polar(&fft->fft_out[i], &fft->fft_polar[i]);
103+
sofm_icomplex32_to_polar(&fft->fft_out[i], &polar_data[i]);
107104
}
108105

109-
static void stft_convert_to_complex(struct phase_vocoder_fft *fft)
106+
static void stft_convert_to_complex(struct ipolar32 *polar_data, struct phase_vocoder_fft *fft)
110107
{
111108
int i;
112109

113110
for (i = 0; i < fft->half_fft_size; i++)
114-
sofm_ipolar32_to_complex(&fft->fft_polar[i], &fft->fft_out[i]);
111+
sofm_ipolar32_to_complex(&polar_data[i], &fft->fft_out[i]);
115112
}
116113

117114
static void stft_apply_fft_symmetry(struct phase_vocoder_fft *fft)
@@ -126,29 +123,148 @@ static void stft_apply_fft_symmetry(struct phase_vocoder_fft *fft)
126123
}
127124
}
128125

126+
static void phase_vocoder_interpolation_parameters(struct phase_vocoder_comp_data *cd)
127+
{
128+
struct phase_vocoder_state *state = &cd->state;
129+
int64_t input_frame_num_frac;
130+
int32_t input_frame_num_prev;
131+
132+
state->num_input_fft++;
133+
input_frame_num_frac = (int64_t)state->num_input_fft * cd->speed; /* Q31.29 */
134+
input_frame_num_prev = Q_SHIFT_RND(input_frame_num_frac, 29, 0);
135+
state->num_input_fft_to_use = input_frame_num_prev + 1;
136+
state->interpolate_fraction =
137+
(int32_t)(input_frame_num_frac - ((int64_t)input_frame_num_prev << 29)); /* Q3.29 */
138+
}
139+
140+
static int32_t unwrap_angle(int32_t angle)
141+
{
142+
if (angle > PHASE_VOCODER_PI_Q28)
143+
return angle - PHASE_VOCODER_TWO_PI_Q28;
144+
else if (angle < -PHASE_VOCODER_PI_Q28)
145+
return angle + PHASE_VOCODER_TWO_PI_Q28;
146+
else
147+
return angle;
148+
}
149+
129150
static void stft_do_fft_ifft(const struct processing_module *mod)
130151
{
131152
struct phase_vocoder_comp_data *cd = module_get_private_data(mod);
132153
struct phase_vocoder_state *state = &cd->state;
154+
struct phase_vocoder_polar *polar = &state->polar;
155+
struct phase_vocoder_fft *fft = &state->fft;
156+
struct ipolar32 *polar_data_prev_ch;
157+
struct ipolar32 *polar_data_ch;
158+
int32_t *angle_delta_prev_ch;
159+
int32_t *angle_delta_ch;
160+
int32_t *output_phase_ch;
161+
int32_t one_minus_frac;
162+
int32_t frac;
163+
int32_t a;
164+
const size_t polar_fft_half_bytes = sizeof(struct ipolar32) * fft->half_fft_size;
165+
const size_t int32_fft_half_bytes = sizeof(int32_t) * fft->half_fft_size;
133166
int num_fft;
134167
int ch;
168+
int i;
135169

136-
for (ch = 0; ch < cd->channels; ch++) {
137-
num_fft = stft_prepare_fft(state, ch);
138-
comp_info(mod->dev, "ch %d num_fft %d", ch, num_fft);
170+
num_fft = stft_get_num_ffts_avail(state, 0);
171+
comp_info(mod->dev, "num_fft %d", num_fft);
172+
173+
if (!num_fft)
174+
return;
175+
176+
phase_vocoder_interpolation_parameters(cd);
177+
comp_info(mod->dev, "interpolate_fraction %d", state->interpolate_fraction);
139178

140-
if (num_fft) {
179+
/* First analysis FFT */
180+
if (!state->num_input_fft) {
181+
for (ch = 0; ch < cd->channels; ch++) {
141182
stft_do_fft(state, ch);
142183

143-
/* Convert half-FFT to polar and back, and fix upper part */
144-
stft_convert_to_polar(&state->fft);
145-
stft_convert_to_complex(&state->fft);
146-
stft_apply_fft_symmetry(&state->fft);
184+
/* Convert half-FFT to polar */
185+
polar_data_ch = polar->polar[ch];
186+
angle_delta_ch = polar->angle_delta[ch];
187+
stft_convert_to_polar(&state->fft, polar_data_ch);
188+
for (i = 0; i < fft->half_fft_size; i++) {
189+
/* Calculate delta angle as Q4.28 */
190+
angle_delta_ch[i] = polar_data_ch[i].angle >> 1;
191+
}
192+
}
193+
//state->input_fft_done = true;
194+
return;
195+
}
196+
197+
while (state->num_input_fft < state->num_input_fft_to_use && num_fft > 0) {
198+
for (ch = 0; ch < cd->channels; ch++) {
199+
stft_do_fft(state, ch);
147200

148-
stft_do_ifft(state, ch);
149-
cd->fft_done = true;
201+
/* Update previous polar data */
202+
polar_data_prev_ch = polar->polar_prev[ch];
203+
polar_data_ch = polar->polar[ch];
204+
memcpy(polar_data_prev_ch, polar_data_ch, polar_fft_half_bytes);
205+
206+
/* Convert half-FFT to polar */
207+
stft_convert_to_polar(&state->fft, polar_data_ch);
208+
209+
/* Update previous delta phase data */
210+
angle_delta_ch = polar->angle_delta[ch];
211+
angle_delta_prev_ch = polar->angle_delta_prev[ch];
212+
memcpy(angle_delta_prev_ch, angle_delta_ch, int32_fft_half_bytes);
213+
214+
/* Calculate new delta phase */
215+
for (i = 0; i < fft->half_fft_size; i++) {
216+
/* Calculate as Q4.28 */
217+
a = (polar_data_ch[i].angle >> 1) - (polar_data_prev_ch[i].angle >> 1);
218+
angle_delta_ch[i] = unwrap_angle(a);
219+
}
150220
}
221+
//state->input_fft_done = true;
222+
num_fft--;
151223
}
224+
225+
if (state->num_input_fft < state->num_input_fft_to_use)
226+
return;
227+
228+
/* Interpolate IFFT frame */
229+
frac = state->interpolate_fraction;
230+
one_minus_frac = PHASE_VOCODER_ONE_Q29 - frac;
231+
232+
233+
for (ch = 0; ch < cd->channels; ch++) {
234+
polar_data_prev_ch = polar->polar_prev[ch];
235+
polar_data_ch = polar->polar[ch];
236+
angle_delta_ch = polar->angle_delta[ch];
237+
angle_delta_prev_ch = polar->angle_delta_prev[ch];
238+
output_phase_ch = polar->output_phase[ch];
239+
240+
for (i = 0; i < fft->half_fft_size; i++) {
241+
polar->polar_tmp[i].magnitude =
242+
Q_MULTSR_32X32((int64_t)one_minus_frac,
243+
polar_data_prev_ch[i].magnitude,
244+
29, 30, 30) +
245+
Q_MULTSR_32X32((int64_t)frac,
246+
polar_data_ch[i].magnitude,
247+
29, 30, 30);
248+
249+
250+
a = output_phase_ch[i];
251+
a += Q_MULTSR_32X32((int64_t)one_minus_frac,
252+
angle_delta_prev_ch[i],
253+
29, 28, 28) +
254+
Q_MULTSR_32X32((int64_t)frac,
255+
angle_delta_ch[i], 29, 28, 28);
256+
a = unwrap_angle(a);
257+
output_phase_ch[i] = a;
258+
polar->polar_tmp[i].angle = a << 1;
259+
}
260+
261+
/* Convert back to (re, im) complex, and fix upper part */
262+
stft_convert_to_complex(polar->polar_tmp, &state->fft);
263+
stft_apply_fft_symmetry(&state->fft);
264+
stft_do_ifft(state, ch);
265+
}
266+
267+
state->output_ifft_done = true;
152268
}
153269

154270
#if CONFIG_FORMAT_S32LE
@@ -203,7 +319,7 @@ static int phase_vocoder_s32(const struct processing_module *mod, struct sof_sou
203319
stft_do_fft_ifft(mod);
204320

205321
/* Get samples from source buffer */
206-
if (cd->fft_done)
322+
if (cd->state.output_ifft_done)
207323
phase_vocoder_sink_s32(cd, sink, frames);
208324
else
209325
phase_vocoder_output_zeros_s32(cd, sink, frames);
@@ -264,7 +380,7 @@ static int phase_vocoder_s16(const struct processing_module *mod, struct sof_sou
264380
stft_do_fft_ifft(mod);
265381

266382
/* Get samples from source buffer */
267-
if (cd->fft_done)
383+
if (cd->state.output_ifft_done)
268384
phase_vocoder_sink_s16(cd, sink, frames);
269385
else
270386
phase_vocoder_output_zeros_s16(cd, sink, frames);

0 commit comments

Comments
 (0)