diff options
Diffstat (limited to 'firmware/pcm_sw_volume.c')
-rw-r--r-- | firmware/pcm_sw_volume.c | 264 |
1 files changed, 264 insertions, 0 deletions
diff --git a/firmware/pcm_sw_volume.c b/firmware/pcm_sw_volume.c new file mode 100644 index 0000000000..bcd498fe46 --- /dev/null +++ b/firmware/pcm_sw_volume.c | |||
@@ -0,0 +1,264 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2013 by Michael Sevakis | ||
11 | * | ||
12 | * This program is free software; you can redistribute it and/or | ||
13 | * modify it under the terms of the GNU General Public License | ||
14 | * as published by the Free Software Foundation; either version 2 | ||
15 | * of the License, or (at your option) any later version. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #include "config.h" | ||
22 | #include "system.h" | ||
23 | #include "pcm.h" | ||
24 | #include "pcm-internal.h" | ||
25 | #include "dsp-util.h" | ||
26 | #include "fixedpoint.h" | ||
27 | #include "pcm_sw_volume.h" | ||
28 | |||
29 | /* source buffer from client */ | ||
30 | static const void * volatile src_buf_addr = NULL; | ||
31 | static size_t volatile src_buf_rem = 0; | ||
32 | |||
33 | #define PCM_PLAY_DBL_BUF_SIZE (PCM_PLAY_DBL_BUF_SAMPLE*PCM_SAMPLE_SIZE) | ||
34 | |||
35 | /* double buffer and frame length control */ | ||
36 | static int16_t pcm_dbl_buf[2][PCM_PLAY_DBL_BUF_SAMPLES*2] | ||
37 | PCM_DBL_BUF_BSS MEM_ALIGN_ATTR; | ||
38 | static size_t pcm_dbl_buf_size[2]; | ||
39 | static int pcm_dbl_buf_num = 0; | ||
40 | static size_t frame_size; | ||
41 | static unsigned int frame_count, frame_err, frame_frac; | ||
42 | |||
43 | #ifdef AUDIOHW_HAVE_PRESCALER | ||
44 | static int32_t prescale_factor = PCM_FACTOR_UNITY; | ||
45 | static int32_t vol_factor_l = 0, vol_factor_r = 0; | ||
46 | #endif /* AUDIOHW_HAVE_PRESCALER */ | ||
47 | |||
48 | /* pcm scaling factors */ | ||
49 | static int32_t pcm_factor_l = 0, pcm_factor_r = 0; | ||
50 | |||
51 | #define PCM_FACTOR_CLIP(f) \ | ||
52 | MAX(MIN((f), PCM_FACTOR_MAX), PCM_FACTOR_MIN) | ||
53 | #define PCM_SCALE_SAMPLE(f, s) \ | ||
54 | (((f) * (s) + PCM_FACTOR_UNITY/2) >> PCM_FACTOR_BITS) | ||
55 | |||
56 | |||
57 | /* TODO: #include CPU-optimized routines and move this to /firmware/asm */ | ||
58 | static inline void pcm_copy_buffer(int16_t *dst, const int16_t *src, | ||
59 | size_t size) | ||
60 | { | ||
61 | int32_t factor_l = pcm_factor_l; | ||
62 | int32_t factor_r = pcm_factor_r; | ||
63 | |||
64 | if (LIKELY(factor_l <= PCM_FACTOR_UNITY && factor_r <= PCM_FACTOR_UNITY)) | ||
65 | { | ||
66 | /* All cut or unity */ | ||
67 | while (size) | ||
68 | { | ||
69 | *dst++ = PCM_SCALE_SAMPLE(factor_l, *src++); | ||
70 | *dst++ = PCM_SCALE_SAMPLE(factor_r, *src++); | ||
71 | size -= PCM_SAMPLE_SIZE; | ||
72 | } | ||
73 | } | ||
74 | else | ||
75 | { | ||
76 | /* Any positive gain requires clipping */ | ||
77 | while (size) | ||
78 | { | ||
79 | *dst++ = clip_sample_16(PCM_SCALE_SAMPLE(factor_l, *src++)); | ||
80 | *dst++ = clip_sample_16(PCM_SCALE_SAMPLE(factor_r, *src++)); | ||
81 | size -= PCM_SAMPLE_SIZE; | ||
82 | } | ||
83 | } | ||
84 | } | ||
85 | |||
86 | bool pcm_play_dma_complete_callback(enum pcm_dma_status status, | ||
87 | const void **addr, size_t *size) | ||
88 | { | ||
89 | /* Check status callback first if error */ | ||
90 | if (status < PCM_DMAST_OK) | ||
91 | status = pcm_play_call_status_cb(status); | ||
92 | |||
93 | size_t sz = pcm_dbl_buf_size[pcm_dbl_buf_num]; | ||
94 | |||
95 | if (status >= PCM_DMAST_OK && sz) | ||
96 | { | ||
97 | /* Do next chunk */ | ||
98 | *addr = pcm_dbl_buf[pcm_dbl_buf_num]; | ||
99 | *size = sz; | ||
100 | return true; | ||
101 | } | ||
102 | else | ||
103 | { | ||
104 | /* This is a stop chunk or error */ | ||
105 | pcm_play_stop_int(); | ||
106 | return false; | ||
107 | } | ||
108 | } | ||
109 | |||
110 | /* Equitably divide large source buffers amongst double buffer frames; | ||
111 | frames smaller than or equal to the double buffer chunk size will play | ||
112 | in one chunk */ | ||
113 | static void update_frame_params(size_t size) | ||
114 | { | ||
115 | int count = size / PCM_SAMPLE_SIZE; | ||
116 | frame_count = (count + PCM_PLAY_DBL_BUF_SAMPLES - 1) / | ||
117 | PCM_PLAY_DBL_BUF_SAMPLES; | ||
118 | int perframe = count / frame_count; | ||
119 | frame_size = perframe * PCM_SAMPLE_SIZE; | ||
120 | frame_frac = count - perframe * frame_count; | ||
121 | frame_err = 0; | ||
122 | } | ||
123 | |||
124 | /* Obtain the next buffer and prepare it for pcm driver playback */ | ||
125 | enum pcm_dma_status | ||
126 | pcm_play_dma_status_callback_int(enum pcm_dma_status status) | ||
127 | { | ||
128 | if (status != PCM_DMAST_STARTED) | ||
129 | return status; | ||
130 | |||
131 | size_t size = pcm_dbl_buf_size[pcm_dbl_buf_num]; | ||
132 | const void *addr = src_buf_addr + size; | ||
133 | |||
134 | size = src_buf_rem - size; | ||
135 | |||
136 | if (size == 0 && pcm_get_more_int(&addr, &size)) | ||
137 | { | ||
138 | update_frame_params(size); | ||
139 | pcm_play_call_status_cb(PCM_DMAST_STARTED); | ||
140 | } | ||
141 | |||
142 | src_buf_addr = addr; | ||
143 | src_buf_rem = size; | ||
144 | |||
145 | if (size != 0) | ||
146 | { | ||
147 | size = frame_size; | ||
148 | |||
149 | if ((frame_err += frame_frac) >= frame_count) | ||
150 | { | ||
151 | frame_err -= frame_count; | ||
152 | size += PCM_SAMPLE_SIZE; | ||
153 | } | ||
154 | } | ||
155 | |||
156 | pcm_dbl_buf_num ^= 1; | ||
157 | pcm_dbl_buf_size[pcm_dbl_buf_num] = size; | ||
158 | pcm_copy_buffer(pcm_dbl_buf[pcm_dbl_buf_num], addr, size); | ||
159 | |||
160 | return PCM_DMAST_OK; | ||
161 | } | ||
162 | |||
163 | /* Prefill double buffer and start pcm driver */ | ||
164 | static void start_pcm(bool reframe) | ||
165 | { | ||
166 | pcm_dbl_buf_num = 0; | ||
167 | pcm_dbl_buf_size[0] = 0; | ||
168 | |||
169 | if (reframe) | ||
170 | update_frame_params(src_buf_rem); | ||
171 | |||
172 | pcm_play_dma_status_callback(PCM_DMAST_STARTED); | ||
173 | pcm_play_dma_status_callback(PCM_DMAST_STARTED); | ||
174 | |||
175 | pcm_play_dma_start(pcm_dbl_buf[1], pcm_dbl_buf_size[1]); | ||
176 | } | ||
177 | |||
178 | void pcm_play_dma_start_int(const void *addr, size_t size) | ||
179 | { | ||
180 | src_buf_addr = addr; | ||
181 | src_buf_rem = size; | ||
182 | start_pcm(true); | ||
183 | } | ||
184 | |||
185 | void pcm_play_dma_pause_int(bool pause) | ||
186 | { | ||
187 | if (pause) | ||
188 | pcm_play_dma_pause(true); | ||
189 | else if (src_buf_rem) | ||
190 | start_pcm(false); /* Reprocess in case volume level changed */ | ||
191 | else | ||
192 | pcm_play_stop_int(); /* Playing frame was last frame */ | ||
193 | } | ||
194 | |||
195 | void pcm_play_dma_stop_int(void) | ||
196 | { | ||
197 | pcm_play_dma_stop(); | ||
198 | src_buf_addr = NULL; | ||
199 | src_buf_rem = 0; | ||
200 | } | ||
201 | |||
202 | /* Return playing buffer from the source buffer */ | ||
203 | const void * pcm_play_dma_get_peak_buffer_int(int *count) | ||
204 | { | ||
205 | const void *addr = src_buf_addr; | ||
206 | size_t size = src_buf_rem; | ||
207 | const void *addr2 = src_buf_addr; | ||
208 | |||
209 | if (addr == addr2 && size) | ||
210 | { | ||
211 | *count = size / PCM_SAMPLE_SIZE; | ||
212 | return addr; | ||
213 | } | ||
214 | |||
215 | *count = 0; | ||
216 | return NULL; | ||
217 | } | ||
218 | |||
219 | /* Return the scale factor corresponding to the centibel level */ | ||
220 | static int32_t pcm_centibels_to_factor(int volume) | ||
221 | { | ||
222 | if (volume == PCM_MUTE_LEVEL) | ||
223 | return 0; /* mute */ | ||
224 | |||
225 | /* Centibels -> fixedpoint */ | ||
226 | return fp_factor(PCM_FACTOR_UNITY*volume / 10, PCM_FACTOR_BITS); | ||
227 | } | ||
228 | |||
229 | #ifdef AUDIOHW_HAVE_PRESCALER | ||
230 | /* Produce final pcm scale factor */ | ||
231 | static void pcm_sync_prescaler(void) | ||
232 | { | ||
233 | int32_t factor_l = fp_mul(prescale_factor, vol_factor_l, PCM_FACTOR_BITS); | ||
234 | int32_t factor_r = fp_mul(prescale_factor, vol_factor_r, PCM_FACTOR_BITS); | ||
235 | pcm_factor_l = PCM_FACTOR_CLIP(factor_l); | ||
236 | pcm_factor_r = PCM_FACTOR_CLIP(factor_r); | ||
237 | } | ||
238 | |||
239 | /* Set the prescaler value for all PCM playback */ | ||
240 | void pcm_set_prescaler(int prescale) | ||
241 | { | ||
242 | prescale_factor = pcm_centibels_to_factor(-prescale); | ||
243 | pcm_sync_prescaler(); | ||
244 | } | ||
245 | |||
246 | /* Set the per-channel volume cut/gain for all PCM playback */ | ||
247 | void pcm_set_master_volume(int vol_l, int vol_r) | ||
248 | { | ||
249 | vol_factor_l = pcm_centibels_to_factor(vol_l); | ||
250 | vol_factor_r = pcm_centibels_to_factor(vol_r); | ||
251 | pcm_sync_prescaler(); | ||
252 | } | ||
253 | |||
254 | #else /* ndef AUDIOHW_HAVE_PRESCALER */ | ||
255 | |||
256 | /* Set the per-channel volume cut/gain for all PCM playback */ | ||
257 | void pcm_set_master_volume(int vol_l, int vol_r) | ||
258 | { | ||
259 | int32_t factor_l = pcm_centibels_to_factor(vol_l); | ||
260 | int32_t factor_r = pcm_centibels_to_factor(vol_r); | ||
261 | pcm_factor_l = PCM_FACTOR_CLIP(factor_l); | ||
262 | pcm_factor_r = PCM_FACTOR_CLIP(factor_r); | ||
263 | } | ||
264 | #endif /* AUDIOHW_HAVE_PRESCALER */ | ||