1 /*
2 * Squeezelite - lightweight headless squeezebox emulator
3 *
4 * (c) Adrian Smith 2012-2015, triode1@btinternet.com
5 * Ralph Irving 2015-2017, ralph_irving@hotmail.com
6 *
7 * This program is free software: you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation, either version 3 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program. If not, see <http://www.gnu.org/licenses/>.
19 *
20 */
21
22 // Scale and pack functions
23
24 #include "squeezelite.h"
25
26 #define MAX_SCALESAMPLE 0x7fffffffffffLL
27 #define MIN_SCALESAMPLE -MAX_SCALESAMPLE
28
29 // inlining these on windows prevents them being linkable...
30 #if !WIN
31 inline
32 #endif
gain(s32_t gain,s32_t sample)33 s32_t gain(s32_t gain, s32_t sample) {
34 s64_t res = (s64_t)gain * (s64_t)sample;
35 if (res > MAX_SCALESAMPLE) res = MAX_SCALESAMPLE;
36 if (res < MIN_SCALESAMPLE) res = MIN_SCALESAMPLE;
37 return (s32_t) (res >> 16);
38 }
39 #if !WIN
40 inline
41 #endif
to_gain(float f)42 s32_t to_gain(float f) {
43 return (s32_t)(f * 65536.0F);
44 }
45
_scale_and_pack_frames(void * outputptr,s32_t * inputptr,frames_t cnt,s32_t gainL,s32_t gainR,output_format format)46 void _scale_and_pack_frames(void *outputptr, s32_t *inputptr, frames_t cnt, s32_t gainL, s32_t gainR, output_format format) {
47 switch(format) {
48 #if DSD
49 case U32_LE:
50 {
51 #if SL_LITTLE_ENDIAN
52 memcpy(outputptr, inputptr, cnt * BYTES_PER_FRAME);
53 #else
54 u32_t *optr = (u32_t *)(void *)outputptr;
55 while (cnt--) {
56 s32_t lsample = *(inputptr++);
57 s32_t rsample = *(inputptr++);
58 *(optr++) =
59 (lsample & 0xff000000) >> 24 | (lsample & 0x00ff0000) >> 8 |
60 (lsample & 0x0000ff00) << 8 | (lsample & 0x000000ff) << 24;
61 *(optr++) =
62 (rsample & 0xff000000) >> 24 | (rsample & 0x00ff0000) >> 8 |
63 (rsample & 0x0000ff00) << 8 | (rsample & 0x000000ff) << 24;
64 }
65 #endif
66 }
67 break;
68 case U32_BE:
69 {
70 #if SL_LITTLE_ENDIAN
71 u32_t *optr = (u32_t *)(void *)outputptr;
72 while (cnt--) {
73 s32_t lsample = *(inputptr++);
74 s32_t rsample = *(inputptr++);
75 *(optr++) =
76 (lsample & 0xff000000) >> 24 | (lsample & 0x00ff0000) >> 8 |
77 (lsample & 0x0000ff00) << 8 | (lsample & 0x000000ff) << 24;
78 *(optr++) =
79 (rsample & 0xff000000) >> 24 | (rsample & 0x00ff0000) >> 8 |
80 (rsample & 0x0000ff00) << 8 | (rsample & 0x000000ff) << 24;
81 }
82 #else
83 memcpy(outputptr, inputptr, cnt * BYTES_PER_FRAME);
84 #endif
85 }
86 break;
87 case U16_LE:
88 {
89 u32_t *optr = (u32_t *)(void *)outputptr;
90 #if SL_LITTLE_ENDIAN
91 while (cnt--) {
92 *(optr++) = (*(inputptr) >> 16 & 0x0000ffff) | (*(inputptr + 1) & 0xffff0000);
93 inputptr += 2;
94 }
95 #else
96 while (cnt--) {
97 s32_t lsample = *(inputptr++);
98 s32_t rsample = *(inputptr++);
99 *(optr++) =
100 (lsample & 0x00ff0000) << 8 | (lsample & 0xff000000) >> 8 |
101 (rsample & 0x00ff0000) >> 8 | (rsample & 0xff000000) >> 24;
102 }
103 #endif
104 }
105 break;
106 case U16_BE:
107 {
108 u32_t *optr = (u32_t *)(void *)outputptr;
109 #if SL_LITTLE_ENDIAN
110 while (cnt--) {
111 s32_t lsample = *(inputptr++);
112 s32_t rsample = *(inputptr++);
113 *(optr++) =
114 (lsample & 0xff000000) >> 24 | (lsample & 0x00ff0000) >> 8 |
115 (rsample & 0xff000000) >> 8 | (rsample & 0x00ff0000) << 8;
116 }
117 #else
118 while (cnt--) {
119 *(optr++) = (*(inputptr) & 0xffff0000) | (*(inputptr + 1) >> 16 & 0x0000ffff);
120 inputptr += 2;
121 }
122 #endif
123 }
124 break;
125 case U8:
126 {
127 u16_t *optr = (u16_t *)(void *)outputptr;
128 #if SL_LITTLE_ENDIAN
129 while (cnt--) {
130 *(optr++) = (u16_t)((*(inputptr) >> 24 & 0x000000ff) | (*(inputptr + 1) >> 16 & 0x0000ff00));
131 inputptr += 2;
132 }
133 #else
134 while (cnt--) {
135 *(optr++) = (u16_t)((*(inputptr) >> 16 & 0x0000ff00) | (*(inputptr + 1) >> 24 & 0x000000ff));
136 inputptr += 2;
137 }
138 #endif
139 }
140 break;
141 #endif
142 case S16_LE:
143 {
144 u32_t *optr = (u32_t *)(void *)outputptr;
145 #if SL_LITTLE_ENDIAN
146 if (gainL == FIXED_ONE && gainR == FIXED_ONE) {
147 while (cnt--) {
148 *(optr++) = (*(inputptr) >> 16 & 0x0000ffff) | (*(inputptr + 1) & 0xffff0000);
149 inputptr += 2;
150 }
151 } else {
152 while (cnt--) {
153 *(optr++) = (gain(gainL, *(inputptr)) >> 16 & 0x0000ffff) | (gain(gainR, *(inputptr+1)) & 0xffff0000);
154 inputptr += 2;
155 }
156 }
157 #else
158 if (gainL == FIXED_ONE && gainR == FIXED_ONE) {
159 while (cnt--) {
160 s32_t lsample = *(inputptr++);
161 s32_t rsample = *(inputptr++);
162 *(optr++) =
163 (lsample & 0x00ff0000) << 8 | (lsample & 0xff000000) >> 8 |
164 (rsample & 0x00ff0000) >> 8 | (rsample & 0xff000000) >> 24;
165 }
166 } else {
167 while (cnt--) {
168 s32_t lsample = gain(gainL, *(inputptr++));
169 s32_t rsample = gain(gainR, *(inputptr++));
170 *(optr++) =
171 (lsample & 0x00ff0000) << 8 | (lsample & 0xff000000) >> 8 |
172 (rsample & 0x00ff0000) >> 8 | (rsample & 0xff000000) >> 24;
173 }
174 }
175 #endif
176 }
177 break;
178 case S24_LE:
179 {
180 u32_t *optr = (u32_t *)(void *)outputptr;
181 #if SL_LITTLE_ENDIAN
182 if (gainL == FIXED_ONE && gainR == FIXED_ONE) {
183 while (cnt--) {
184 *(optr++) = *(inputptr++) >> 8;
185 *(optr++) = *(inputptr++) >> 8;
186 }
187 } else {
188 while (cnt--) {
189 *(optr++) = gain(gainL, *(inputptr++)) >> 8;
190 *(optr++) = gain(gainR, *(inputptr++)) >> 8;
191 }
192 }
193 #else
194 if (gainL == FIXED_ONE && gainR == FIXED_ONE) {
195 while (cnt--) {
196 s32_t lsample = *(inputptr++);
197 s32_t rsample = *(inputptr++);
198 *(optr++) =
199 (lsample & 0xff000000) >> 16 | (lsample & 0x00ff0000) | (lsample & 0x0000ff00 << 16);
200 *(optr++) =
201 (rsample & 0xff000000) >> 16 | (rsample & 0x00ff0000) | (rsample & 0x0000ff00 << 16);
202 }
203 } else {
204 while (cnt--) {
205 s32_t lsample = gain(gainL, *(inputptr++));
206 s32_t rsample = gain(gainR, *(inputptr++));
207 *(optr++) =
208 (lsample & 0xff000000) >> 16 | (lsample & 0x00ff0000) | (lsample & 0x0000ff00 << 16);
209 *(optr++) =
210 (rsample & 0xff000000) >> 16 | (rsample & 0x00ff0000) | (rsample & 0x0000ff00 << 16);
211 }
212 }
213 #endif
214 }
215 break;
216 case S24_3LE:
217 {
218 u8_t *optr = (u8_t *)(void *)outputptr;
219 if (gainL == FIXED_ONE && gainR == FIXED_ONE) {
220 while (cnt) {
221 // attempt to do 32 bit memory accesses - move 2 frames at once: 16 bytes -> 12 bytes
222 // falls through to exception case when not aligned or if less than 2 frames to move
223 if (((uintptr_t)optr & 0x3) == 0 && cnt >= 2) {
224 u32_t *o_ptr = (u32_t *)(void *)optr;
225 while (cnt >= 2) {
226 s32_t l1 = *(inputptr++); s32_t r1 = *(inputptr++);
227 s32_t l2 = *(inputptr++); s32_t r2 = *(inputptr++);
228 #if SL_LITTLE_ENDIAN
229 *(o_ptr++) = (l1 & 0xffffff00) >> 8 | (r1 & 0x0000ff00) << 16;
230 *(o_ptr++) = (r1 & 0xffff0000) >> 16 | (l2 & 0x00ffff00) << 8;
231 *(o_ptr++) = (l2 & 0xff000000) >> 24 | (r2 & 0xffffff00);
232 #else
233 *(o_ptr++) = (l1 & 0x0000ff00) << 16 | (l1 & 0x00ff0000) | (l1 & 0xff000000) >> 16 |
234 (r1 & 0x0000ff00) >> 8;
235 *(o_ptr++) = (r1 & 0x00ff0000) << 8 | (r1 & 0xff000000) >> 8 | (l2 & 0x0000ff00) |
236 (l2 & 0x00ff0000) >> 16;
237 *(o_ptr++) = (l2 & 0xff000000) | (r2 & 0x0000ff00) << 8 | (r2 & 0x00ff0000) >> 8 |
238 (r2 & 0xff000000) >> 24;
239 #endif
240 optr += 12;
241 cnt -= 2;
242 }
243 } else {
244 s32_t lsample = *(inputptr++);
245 s32_t rsample = *(inputptr++);
246 *(optr++) = (lsample & 0x0000ff00) >> 8;
247 *(optr++) = (lsample & 0x00ff0000) >> 16;
248 *(optr++) = (lsample & 0xff000000) >> 24;
249 *(optr++) = (rsample & 0x0000ff00) >> 8;
250 *(optr++) = (rsample & 0x00ff0000) >> 16;
251 *(optr++) = (rsample & 0xff000000) >> 24;
252 cnt--;
253 }
254 }
255 } else {
256 while (cnt) {
257 // attempt to do 32 bit memory accesses - move 2 frames at once: 16 bytes -> 12 bytes
258 // falls through to exception case when not aligned or if less than 2 frames to move
259 if (((uintptr_t)optr & 0x3) == 0 && cnt >= 2) {
260 u32_t *o_ptr = (u32_t *)(void *)optr;
261 while (cnt >= 2) {
262 s32_t l1 = gain(gainL, *(inputptr++)); s32_t r1 = gain(gainR, *(inputptr++));
263 s32_t l2 = gain(gainL, *(inputptr++)); s32_t r2 = gain(gainR, *(inputptr++));
264 #if SL_LITTLE_ENDIAN
265 *(o_ptr++) = (l1 & 0xffffff00) >> 8 | (r1 & 0x0000ff00) << 16;
266 *(o_ptr++) = (r1 & 0xffff0000) >> 16 | (l2 & 0x00ffff00) << 8;
267 *(o_ptr++) = (l2 & 0xff000000) >> 24 | (r2 & 0xffffff00);
268 #else
269 *(o_ptr++) = (l1 & 0x0000ff00) << 16 | (l1 & 0x00ff0000) | (l1 & 0xff000000) >> 16 |
270 (r1 & 0x0000ff00) >> 8;
271 *(o_ptr++) = (r1 & 0x00ff0000) << 8 | (r1 & 0xff000000) >> 8 | (l2 & 0x0000ff00) |
272 (l2 & 0x00ff0000) >> 16;
273 *(o_ptr++) = (l2 & 0xff000000) | (r2 & 0x0000ff00) << 8 | (r2 & 0x00ff0000) >> 8 |
274 (r2 & 0xff000000) >> 24;
275 #endif
276 optr += 12;
277 cnt -= 2;
278 }
279 } else {
280 s32_t lsample = gain(gainL, *(inputptr++));
281 s32_t rsample = gain(gainR, *(inputptr++));
282 *(optr++) = (lsample & 0x0000ff00) >> 8;
283 *(optr++) = (lsample & 0x00ff0000) >> 16;
284 *(optr++) = (lsample & 0xff000000) >> 24;
285 *(optr++) = (rsample & 0x0000ff00) >> 8;
286 *(optr++) = (rsample & 0x00ff0000) >> 16;
287 *(optr++) = (rsample & 0xff000000) >> 24;
288 cnt--;
289 }
290 }
291 }
292 }
293 break;
294 case S32_LE:
295 {
296 u32_t *optr = (u32_t *)(void *)outputptr;
297 #if SL_LITTLE_ENDIAN
298 if (gainL == FIXED_ONE && gainR == FIXED_ONE) {
299 memcpy(outputptr, inputptr, cnt * BYTES_PER_FRAME);
300 } else {
301 while (cnt--) {
302 *(optr++) = gain(gainL, *(inputptr++));
303 *(optr++) = gain(gainR, *(inputptr++));
304 }
305 }
306 #else
307 if (gainL == FIXED_ONE && gainR == FIXED_ONE) {
308 while (cnt--) {
309 s32_t lsample = *(inputptr++);
310 s32_t rsample = *(inputptr++);
311 *(optr++) =
312 (lsample & 0xff000000) >> 24 | (lsample & 0x00ff0000) >> 8 |
313 (lsample & 0x0000ff00) << 8 | (lsample & 0x000000ff) << 24;
314 *(optr++) =
315 (rsample & 0xff000000) >> 24 | (rsample & 0x00ff0000) >> 8 |
316 (rsample & 0x0000ff00) << 8 | (rsample & 0x000000ff) << 24;
317 }
318 } else {
319 while (cnt--) {
320 s32_t lsample = gain(gainL, *(inputptr++));
321 s32_t rsample = gain(gainR, *(inputptr++));
322 *(optr++) =
323 (lsample & 0xff000000) >> 24 | (lsample & 0x00ff0000) >> 8 |
324 (lsample & 0x0000ff00) << 8 | (lsample & 0x000000ff) << 24;
325 *(optr++) =
326 (rsample & 0xff000000) >> 24 | (rsample & 0x00ff0000) >> 8 |
327 (rsample & 0x0000ff00) << 8 | (rsample & 0x000000ff) << 24;
328 }
329 }
330 #endif
331 }
332 break;
333 default:
334 break;
335 }
336 }
337
338 #if !WIN
339 inline
340 #endif
_apply_cross(struct buffer * outputbuf,frames_t out_frames,s32_t cross_gain_in,s32_t cross_gain_out,s32_t ** cross_ptr)341 void _apply_cross(struct buffer *outputbuf, frames_t out_frames, s32_t cross_gain_in, s32_t cross_gain_out, s32_t **cross_ptr) {
342 s32_t *ptr = (s32_t *)(void *)outputbuf->readp;
343 frames_t count = out_frames * 2;
344 while (count--) {
345 if (*cross_ptr > (s32_t *)outputbuf->wrap) {
346 *cross_ptr -= outputbuf->size / BYTES_PER_FRAME * 2;
347 }
348 *ptr = gain(cross_gain_out, *ptr) + gain(cross_gain_in, **cross_ptr);
349 ptr++; (*cross_ptr)++;
350 }
351 }
352
353 #if !WIN
354 inline
355 #endif
_apply_gain(struct buffer * outputbuf,frames_t count,s32_t gainL,s32_t gainR)356 void _apply_gain(struct buffer *outputbuf, frames_t count, s32_t gainL, s32_t gainR) {
357 s32_t *ptrL = (s32_t *)(void *)outputbuf->readp;
358 s32_t *ptrR = (s32_t *)(void *)outputbuf->readp + 1;
359 while (count--) {
360 *ptrL = gain(gainL, *ptrL);
361 *ptrR = gain(gainR, *ptrR);
362 ptrL += 2;
363 ptrR += 2;
364 }
365 }
366