1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2020 Google LLC. All rights reserved.
4 //
5 // Author: Sebastiano Carlucci <scarlucci@google.com>
6
7 #include <stdint.h>
8 #include <sof/audio/component.h>
9 #include <sof/audio/format.h>
10 #include <sof/audio/dcblock/dcblock.h>
11
12 /**
13 *
14 * Genereric processing function. Input is 32 bits.
15 *
16 */
dcblock_generic(struct dcblock_state * state,int64_t R,int32_t x)17 static int32_t dcblock_generic(struct dcblock_state *state,
18 int64_t R, int32_t x)
19 {
20 /*
21 * R: Q2.30, y_prev: Q1.31
22 * R * y_prev: Q3.61
23 */
24 int64_t out = ((int64_t)x) - state->x_prev +
25 Q_SHIFT_RND(R * state->y_prev, 61, 31);
26
27 state->y_prev = sat_int32(out);
28 state->x_prev = x;
29
30 return state->y_prev;
31 }
32
33 #if CONFIG_FORMAT_S16LE
dcblock_s16_default(const struct comp_dev * dev,const struct audio_stream * source,const struct audio_stream * sink,uint32_t frames)34 static void dcblock_s16_default(const struct comp_dev *dev,
35 const struct audio_stream *source,
36 const struct audio_stream *sink,
37 uint32_t frames)
38 {
39 struct comp_data *cd = comp_get_drvdata(dev);
40 struct dcblock_state *state;
41 int16_t *x;
42 int16_t *y;
43 int32_t R;
44 int32_t tmp;
45 int idx;
46 int ch;
47 int i;
48 int nch = source->channels;
49
50 for (ch = 0; ch < nch; ch++) {
51 state = &cd->state[ch];
52 R = cd->R_coeffs[ch];
53 idx = ch;
54 for (i = 0; i < frames; i++) {
55 x = audio_stream_read_frag_s16(source, idx);
56 y = audio_stream_read_frag_s16(sink, idx);
57 tmp = dcblock_generic(state, R, *x << 16);
58 *y = sat_int16(Q_SHIFT_RND(tmp, 31, 15));
59 idx += nch;
60 }
61 }
62 }
63 #endif /* CONFIG_FORMAT_S16LE */
64
65 #if CONFIG_FORMAT_S24LE
dcblock_s24_default(const struct comp_dev * dev,const struct audio_stream * source,const struct audio_stream * sink,uint32_t frames)66 static void dcblock_s24_default(const struct comp_dev *dev,
67 const struct audio_stream *source,
68 const struct audio_stream *sink,
69 uint32_t frames)
70 {
71 struct comp_data *cd = comp_get_drvdata(dev);
72 struct dcblock_state *state;
73 int32_t *x;
74 int32_t *y;
75 int32_t R;
76 int32_t tmp;
77 int idx;
78 int ch;
79 int i;
80 int nch = source->channels;
81
82 for (ch = 0; ch < nch; ch++) {
83 state = &cd->state[ch];
84 R = cd->R_coeffs[ch];
85 idx = ch;
86 for (i = 0; i < frames; i++) {
87 x = audio_stream_read_frag_s32(source, idx);
88 y = audio_stream_read_frag_s32(sink, idx);
89 tmp = dcblock_generic(state, R, *x << 8);
90 *y = sat_int24(Q_SHIFT_RND(tmp, 31, 23));
91 idx += nch;
92 }
93 }
94 }
95 #endif /* CONFIG_FORMAT_S24LE */
96
97 #if CONFIG_FORMAT_S32LE
dcblock_s32_default(const struct comp_dev * dev,const struct audio_stream * source,const struct audio_stream * sink,uint32_t frames)98 static void dcblock_s32_default(const struct comp_dev *dev,
99 const struct audio_stream *source,
100 const struct audio_stream *sink,
101 uint32_t frames)
102 {
103 struct comp_data *cd = comp_get_drvdata(dev);
104 struct dcblock_state *state;
105 int32_t *x;
106 int32_t *y;
107 int32_t R;
108 int idx;
109 int ch;
110 int i;
111 int nch = source->channels;
112
113 for (ch = 0; ch < nch; ch++) {
114 state = &cd->state[ch];
115 R = cd->R_coeffs[ch];
116 idx = ch;
117 for (i = 0; i < frames; i++) {
118 x = audio_stream_read_frag_s32(source, idx);
119 y = audio_stream_read_frag_s32(sink, idx);
120 *y = dcblock_generic(state, R, *x);
121 idx += nch;
122 }
123 }
124 }
125 #endif /* CONFIG_FORMAT_S32LE */
126
127 const struct dcblock_func_map dcblock_fnmap[] = {
128 /* { SOURCE_FORMAT , PROCESSING FUNCTION } */
129 #if CONFIG_FORMAT_S16LE
130 { SOF_IPC_FRAME_S16_LE, dcblock_s16_default },
131 #endif /* CONFIG_FORMAT_S16LE */
132 #if CONFIG_FORMAT_S24LE
133 { SOF_IPC_FRAME_S24_4LE, dcblock_s24_default },
134 #endif /* CONFIG_FORMAT_S24LE */
135 #if CONFIG_FORMAT_S32LE
136 { SOF_IPC_FRAME_S32_LE, dcblock_s32_default },
137 #endif /* CONFIG_FORMAT_S32LE */
138 };
139
140 const size_t dcblock_fncount = ARRAY_SIZE(dcblock_fnmap);
141
