1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2021 Google LLC. All rights reserved.
4 //
5 // Author: Pin-chih Lin <johnylin@google.com>
6
7 #include <sof/audio/drc/drc_math.h>
8 #include <sof/math/numbers.h>
9
10 #if DRC_HIFI3
11
12 #include <xtensa/tie/xt_hifi3.h>
13
14 #define ONE_OVER_SQRT2_Q30 759250112 /* Q_CONVERT_FLOAT(0.70710678118654752f, 30); 1/sqrt(2) */
15 #define LOG10_FUNC_A5_Q26 75959200 /* Q_CONVERT_FLOAT(1.131880283355712890625f, 26) */
16 #define LOG10_FUNC_A4_Q26 -285795039 /* Q_CONVERT_FLOAT(-4.258677959442138671875f, 26) */
17 #define LOG10_FUNC_A3_Q26 457435200 /* Q_CONVERT_FLOAT(6.81631565093994140625f, 26) */
18 #define LOG10_FUNC_A2_Q26 -410610303 /* Q_CONVERT_FLOAT(-6.1185703277587890625f, 26) */
19 #define LOG10_FUNC_A1_Q26 244982704 /* Q_CONVERT_FLOAT(3.6505267620086669921875f, 26) */
20 #define LOG10_FUNC_A0_Q26 -81731487 /* Q_CONVERT_FLOAT(-1.217894077301025390625f, 26) */
21 #define HALF_Q25 16777216 /* Q_CONVERT_FLOAT(0.5, 25) */
22 #define LOG10_2_Q26 20201782 /* Q_CONVERT_FLOAT(0.301029995663981195214f, 26) */
23 #define NEG_1K_Q21 -2097151999 /* Q_CONVERT_FLOAT(-1000.0f, 21) */
24 #define LOG_10_Q29 1236190976 /* Q_CONVERT_FLOAT(2.3025850929940457f, 29) */
25 #define NEG_30_Q26 -2013265919 /* Q_CONVERT_FLOAT(-30.0f, 26) */
26 #define ASIN_FUNC_A7L_Q30 126897672 /* Q_CONVERT_FLOAT(0.1181826665997505187988281f, 30) */
27 #define ASIN_FUNC_A5L_Q30 43190596 /* Q_CONVERT_FLOAT(4.0224377065896987915039062e-2f, 30) */
28 #define ASIN_FUNC_A3L_Q30 184887136 /* Q_CONVERT_FLOAT(0.1721895635128021240234375f, 30) */
29 #define ASIN_FUNC_A1L_Q30 1073495040 /* Q_CONVERT_FLOAT(0.99977016448974609375f, 30) */
30 #define ASIN_FUNC_A7H_Q26 948097024 /* Q_CONVERT_FLOAT(14.12774658203125f, 26) */
31 #define ASIN_FUNC_A5H_Q26 -2024625535 /* Q_CONVERT_FLOAT(-30.1692714691162109375f, 26) */
32 #define ASIN_FUNC_A3H_Q26 1441234048 /* Q_CONVERT_FLOAT(21.4760608673095703125f, 26) */
33 #define ASIN_FUNC_A1H_Q26 -261361631 /* Q_CONVERT_FLOAT(-3.894591808319091796875f, 26) */
34 #define SQRT2_Q30 1518500224 /* Q_CONVERT_FLOAT(1.4142135623730950488f, 30); sqrt(2) */
35 #define INV_FUNC_A5_Q25 -92027983 /* Q_CONVERT_FLOAT(-2.742647647857666015625f, 25) */
36 #define INV_FUNC_A4_Q25 470207584 /* Q_CONVERT_FLOAT(14.01327800750732421875f, 25) */
37 #define INV_FUNC_A3_Q25 -998064895 /* Q_CONVERT_FLOAT(-29.74465179443359375f, 25) */
38 #define INV_FUNC_A2_Q25 1126492160 /* Q_CONVERT_FLOAT(33.57208251953125f, 25) */
39 #define INV_FUNC_A1_Q25 -713042175 /* Q_CONVERT_FLOAT(-21.25031280517578125f, 25) */
40 #define INV_FUNC_A0_Q25 239989712 /* Q_CONVERT_FLOAT(7.152250766754150390625f, 25) */
41
42 /*
43 * Input depends on precision_x
44 * Output range [0.5, 1); regulated to Q2.30
45 */
rexp_fixed(ae_f32 x,int32_t precision_x,int32_t * e)46 static inline ae_f32 rexp_fixed(ae_f32 x, int32_t precision_x, int32_t *e)
47 {
48 int32_t bit = 31 - AE_NSAZ32_L(x);
49
50 *e = bit - precision_x;
51
52 return AE_SRAA32(x, bit - 30);
53 }
54
55 /*
56 * Input is Q6.26: max 32.0
57 * Output range ~ (-inf, 1.505); regulated to Q6.26: (-32.0, 32.0)
58 */
log10_fixed(ae_f32 x)59 static inline ae_f32 log10_fixed(ae_f32 x)
60 {
61 /* Coefficients obtained from:
62 * fpminimax(log10(x), 5, [|SG...|], [1/2;sqrt(2)/2], absolute);
63 * max err ~= 6.088e-8
64 */
65 int32_t e;
66 int32_t lshift;
67 ae_f32 exp; /* Q7.25 */
68 ae_f32 acc; /* Q6.26 */
69 ae_f32 tmp; /* Q6.26 */
70
71 x = rexp_fixed(x, 26, &e); /* Q2.30 */
72 exp = e << 25; /* Q_CONVERT_FLOAT(e, 25) */
73
74 if ((int32_t)x > (int32_t)ONE_OVER_SQRT2_Q30) {
75 lshift = drc_get_lshift(30, 30, 30);
76 x = drc_mult_lshift(x, ONE_OVER_SQRT2_Q30, lshift);
77 exp = AE_ADD32(exp, HALF_Q25);
78 }
79
80 lshift = drc_get_lshift(26, 30, 26);
81 acc = drc_mult_lshift(LOG10_FUNC_A5_Q26, x, lshift);
82 acc = AE_ADD32(acc, LOG10_FUNC_A4_Q26);
83 acc = drc_mult_lshift(acc, x, lshift);
84 acc = AE_ADD32(acc, LOG10_FUNC_A3_Q26);
85 acc = drc_mult_lshift(acc, x, lshift);
86 acc = AE_ADD32(acc, LOG10_FUNC_A2_Q26);
87 acc = drc_mult_lshift(acc, x, lshift);
88 acc = AE_ADD32(acc, LOG10_FUNC_A1_Q26);
89 acc = drc_mult_lshift(acc, x, lshift);
90 acc = AE_ADD32(acc, LOG10_FUNC_A0_Q26);
91
92 lshift = drc_get_lshift(25, 26, 26);
93 tmp = drc_mult_lshift(exp, LOG10_2_Q26, lshift);
94 acc = AE_ADD32(acc, tmp);
95
96 return acc;
97 }
98
99 /*
100 * Input is Q6.26: max 32.0
101 * Output range ~ (-inf, 30.1030); regulated to Q11.21: (-1024.0, 1024.0)
102 */
drc_lin2db_fixed(int32_t linear)103 inline int32_t drc_lin2db_fixed(int32_t linear)
104 {
105 ae_f32 log10_linear;
106
107 /* For negative or zero, just return a very small dB value. */
108 if (linear <= 0)
109 return NEG_1K_Q21;
110
111 log10_linear = log10_fixed(linear); /* Q6.26 */
112 return drc_mult_lshift(20 << 26, log10_linear, drc_get_lshift(26, 26, 21));
113 }
114
115 /*
116 * Input is Q6.26: max 32.0
117 * Output range ~ (-inf, 3.4657); regulated to Q6.26: (-32.0, 32.0)
118 */
drc_log_fixed(int32_t x)119 inline int32_t drc_log_fixed(int32_t x)
120 {
121 ae_f32 log10_x;
122
123 if (x <= 0)
124 return NEG_30_Q26;
125
126 /* log(x) = log(10) * log10(x) */
127 log10_x = log10_fixed(x); /* Q6.26 */
128 return drc_mult_lshift(LOG_10_Q29, log10_x, drc_get_lshift(29, 26, 26));
129 }
130
131 #ifndef DRC_USE_CORDIC_ASIN
132 /*
133 * Input is Q2.30; valid range: [-1.0, 1.0]
134 * Output range: [-1.0, 1.0]; regulated to Q2.30: (-2.0, 2.0)
135 */
drc_asin_fixed(int32_t x)136 inline int32_t drc_asin_fixed(int32_t x)
137 {
138 /* Coefficients obtained from:
139 * If x <= 1/sqrt(2), then
140 * fpminimax(asin(x), [|1,3,5,7|], [|SG...|], [-1e-30;1/sqrt(2)], absolute)
141 * max err ~= 1.89936e-5
142 * Else then
143 * fpminimax(asin(x), [|1,3,5,7|], [|SG...|], [1/sqrt(2);1], absolute)
144 * max err ~= 3.085226e-2
145 */
146 int32_t lshift;
147 ae_f32 in = x; /* Q2.30 */
148 ae_f32 in2; /* Q2.30 */
149 ae_f32 A7, A5, A3, A1;
150 int32_t qc;
151 ae_f32 acc;
152
153 lshift = drc_get_lshift(30, 30, 30);
154 in2 = drc_mult_lshift(in, in, lshift);
155
156 if (ABS((int32_t)in) <= ONE_OVER_SQRT2_Q30) {
157 A7 = ASIN_FUNC_A7L_Q30;
158 A5 = ASIN_FUNC_A5L_Q30;
159 A3 = ASIN_FUNC_A3L_Q30;
160 A1 = ASIN_FUNC_A1L_Q30;
161 qc = 30;
162 } else {
163 A7 = ASIN_FUNC_A7H_Q26;
164 A5 = ASIN_FUNC_A5H_Q26;
165 A3 = ASIN_FUNC_A3H_Q26;
166 A1 = ASIN_FUNC_A1H_Q26;
167 qc = 26;
168 }
169
170 lshift = drc_get_lshift(qc, 30, qc);
171 acc = drc_mult_lshift(A7, in2, lshift);
172 acc = AE_ADD32(acc, A5);
173 acc = drc_mult_lshift(acc, in2, lshift);
174 acc = AE_ADD32(acc, A3);
175 acc = drc_mult_lshift(acc, in2, lshift);
176 acc = AE_ADD32(acc, A1);
177 acc = drc_mult_lshift(acc, in, lshift);
178 lshift = drc_get_lshift(qc, 30, 30);
179 return drc_mult_lshift(acc, TWO_OVER_PI_Q30, lshift);
180 }
181 #endif /* !DRC_USE_CORDIC_ASIN */
182
183 /*
184 * Input depends on precision_x
185 * Output depends on precision_y
186 */
drc_inv_fixed(int32_t x,int32_t precision_x,int32_t precision_y)187 inline int32_t drc_inv_fixed(int32_t x, int32_t precision_x, int32_t precision_y)
188 {
189 /* Coefficients obtained from:
190 * fpminimax(1/x, 5, [|SG...|], [sqrt(2)/2;1], absolute);
191 * max err ~= 1.00388e-6
192 */
193 ae_f32 in;
194 int32_t lshift;
195 int32_t e;
196 int32_t precision_inv;
197 int32_t sqrt2_extracted = 0;
198 ae_f32 acc;
199
200 in = rexp_fixed(x, precision_x, &e); /* Q2.30 */
201
202 if (ABS((int32_t)in) < ONE_OVER_SQRT2_Q30) {
203 lshift = drc_get_lshift(30, 30, 30);
204 in = drc_mult_lshift(in, SQRT2_Q30, lshift);
205 sqrt2_extracted = 1;
206 }
207
208 lshift = drc_get_lshift(25, 30, 25);
209 acc = drc_mult_lshift(INV_FUNC_A5_Q25, in, lshift);
210 acc = AE_ADD32(acc, INV_FUNC_A4_Q25);
211 acc = drc_mult_lshift(acc, in, lshift);
212 acc = AE_ADD32(acc, INV_FUNC_A3_Q25);
213 acc = drc_mult_lshift(acc, in, lshift);
214 acc = AE_ADD32(acc, INV_FUNC_A2_Q25);
215 acc = drc_mult_lshift(acc, in, lshift);
216 acc = AE_ADD32(acc, INV_FUNC_A1_Q25);
217 acc = drc_mult_lshift(acc, in, lshift);
218 acc = AE_ADD32(acc, INV_FUNC_A0_Q25);
219
220 if (sqrt2_extracted)
221 acc = drc_mult_lshift(acc, SQRT2_Q30, lshift);
222
223 precision_inv = e + 25;
224 acc = AE_SLAA32S(acc, precision_y - precision_inv);
225 return acc;
226 }
227
228 #endif /* DRC_HIFI3 */
229
