AudioHardware: Dispatch setParameters to input and output
[tinyalsa-audio.git] / AudioResampler.cpp
1 /*
2  * Copyright (C) 2012 Paul Kocialkowski <contact@paulk.fr>
3  *
4  * This is based on Android 4.0 resampler
5  * Copyright 2011, The Android Open-Source Project
6  *
7  * This is based on Nexus S AudioHardware implementation:
8  * Copyright 2010, The Android Open-Source Project
9  *
10  * Licensed under the Apache License, Version 2.0 (the "License");
11  * you may not use this file except in compliance with the License.
12  * You may obtain a copy of the License at
13  *
14  *     http://www.apache.org/licenses/LICENSE-2.0
15  *
16  * Unless required by applicable law or agreed to in writing, software
17  * distributed under the License is distributed on an "AS IS" BASIS,
18  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
19  * See the License for the specific language governing permissions and
20  * limitations under the License.
21  */
22
23 #define LOG_TAG "AudioResampler"
24
25 #include "AudioResampler.h"
26 #include <stdio.h>
27 #include <stdlib.h>
28 #include <stdint.h>
29 #include <unistd.h>
30 #include <ctype.h>
31
32 #include <sys/types.h>
33 #include <sys/stat.h>
34 #include <fcntl.h>
35
36 namespace android {
37
38 /*
39  * Android 4.0 resampler (begin)
40  */
41
42 struct resampler {
43     struct resampler_itfe itfe;
44     SpeexResamplerState *speex_resampler;       // handle on speex resampler
45     struct resampler_buffer_provider *provider; // buffer provider installed by client
46     uint32_t in_sample_rate;                    // input sampling rate in Hz
47     uint32_t out_sample_rate;                   // output sampling rate in Hz
48     uint32_t channel_count;                     // number of channels (interleaved)
49     int16_t *in_buf;                            // input buffer
50     size_t in_buf_size;                         // input buffer size
51     size_t frames_in;                           // number of frames in input buffer
52     size_t frames_rq;                           // cached number of output frames
53     size_t frames_needed;                       // minimum number of input frames to produce
54                                                 // frames_rq output frames
55     int32_t speex_delay_ns;                     // delay introduced by speex resampler in ns
56 };
57
58
59 //------------------------------------------------------------------------------
60 // speex based resampler
61 //------------------------------------------------------------------------------
62
63 static void resampler_reset(struct resampler_itfe *resampler)
64 {
65     struct resampler *rsmp = (struct resampler *)resampler;
66
67     rsmp->frames_in = 0;
68     rsmp->frames_rq = 0;
69
70     if (rsmp != NULL && rsmp->speex_resampler != NULL) {
71         speex_resampler_reset_mem(rsmp->speex_resampler);
72     }
73 }
74
75 static int32_t resampler_delay_ns(struct resampler_itfe *resampler)
76 {
77     struct resampler *rsmp = (struct resampler *)resampler;
78
79     int32_t delay = (int32_t)((1000000000 * (int64_t)rsmp->frames_in) / rsmp->in_sample_rate);
80     delay += rsmp->speex_delay_ns;
81
82     return delay;
83 }
84
85 // outputs a number of frames less or equal to *outFrameCount and updates *outFrameCount
86 // with the actual number of frames produced.
87 int resampler_resample_from_provider(struct resampler_itfe *resampler,
88                        int16_t *out,
89                        size_t *outFrameCount)
90 {
91     struct resampler *rsmp = (struct resampler *)resampler;
92
93     if (rsmp == NULL || out == NULL || outFrameCount == NULL) {
94         return -EINVAL;
95     }
96     if (rsmp->provider == NULL) {
97         *outFrameCount = 0;
98         return -ENOSYS;
99     }
100
101     size_t framesRq = *outFrameCount;
102     // update and cache the number of frames needed at the input sampling rate to produce
103     // the number of frames requested at the output sampling rate
104     if (framesRq != rsmp->frames_rq) {
105         rsmp->frames_needed = (framesRq * rsmp->out_sample_rate) / rsmp->in_sample_rate + 1;
106         rsmp->frames_rq = framesRq;
107     }
108
109     size_t framesWr = 0;
110     size_t inFrames = 0;
111     while (framesWr < framesRq) {
112         if (rsmp->frames_in < rsmp->frames_needed) {
113             // make sure that the number of frames present in rsmp->in_buf (rsmp->frames_in) is at
114             // least the number of frames needed to produce the number of frames requested at
115             // the output sampling rate
116             if (rsmp->in_buf_size < rsmp->frames_needed) {
117                 rsmp->in_buf_size = rsmp->frames_needed;
118                 rsmp->in_buf = (int16_t *)realloc(rsmp->in_buf,
119                                         rsmp->in_buf_size * rsmp->channel_count * sizeof(int16_t));
120             }
121             struct resampler_buffer buf;
122             buf.frame_count = rsmp->frames_needed - rsmp->frames_in;
123             rsmp->provider->get_next_buffer(rsmp->provider, &buf);
124             if (buf.raw == NULL) {
125                 break;
126             }
127             memcpy(rsmp->in_buf + rsmp->frames_in * rsmp->channel_count,
128                     buf.raw,
129                     buf.frame_count * rsmp->channel_count * sizeof(int16_t));
130             rsmp->frames_in += buf.frame_count;
131             rsmp->provider->release_buffer(rsmp->provider, &buf);
132         }
133
134         size_t outFrames = framesRq - framesWr;
135         inFrames = rsmp->frames_in;
136         if (rsmp->channel_count == 1) {
137             speex_resampler_process_int(rsmp->speex_resampler,
138                                         0,
139                                         rsmp->in_buf,
140                                         &inFrames,
141                                         out + framesWr,
142                                         &outFrames);
143         } else {
144             speex_resampler_process_interleaved_int(rsmp->speex_resampler,
145                                         rsmp->in_buf,
146                                         &inFrames,
147                                         out + framesWr * rsmp->channel_count,
148                                         &outFrames);
149         }
150         framesWr += outFrames;
151         rsmp->frames_in -= inFrames;
152         LOGW_IF((framesWr != framesRq) && (rsmp->frames_in != 0),
153                 "ReSampler::resample() remaining %d frames in and %d frames out",
154                 rsmp->frames_in, (framesRq - framesWr));
155     }
156     if (rsmp->frames_in) {
157         memmove(rsmp->in_buf,
158                 rsmp->in_buf + inFrames * rsmp->channel_count,
159                 rsmp->frames_in * rsmp->channel_count * sizeof(int16_t));
160     }
161     *outFrameCount = framesWr;
162
163     return 0;
164 }
165
166 int resampler_resample_from_input(struct resampler_itfe *resampler,
167                                   int16_t *in,
168                                   size_t *inFrameCount,
169                                   int16_t *out,
170                                   size_t *outFrameCount)
171 {
172     struct resampler *rsmp = (struct resampler *)resampler;
173
174     if (rsmp == NULL || in == NULL || inFrameCount == NULL ||
175             out == NULL || outFrameCount == NULL) {
176         return -EINVAL;
177     }
178     if (rsmp->provider != NULL) {
179         *outFrameCount = 0;
180         return -ENOSYS;
181     }
182
183     if (rsmp->channel_count == 1) {
184         speex_resampler_process_int(rsmp->speex_resampler,
185                                     0,
186                                     in,
187                                     inFrameCount,
188                                     out,
189                                     outFrameCount);
190     } else {
191         speex_resampler_process_interleaved_int(rsmp->speex_resampler,
192                                                 in,
193                                                 inFrameCount,
194                                                 out,
195                                                 outFrameCount);
196     }
197
198     LOGV("resampler_resample_from_input() DONE in %d out % d", *inFrameCount, *outFrameCount);
199
200     return 0;
201 }
202
203 int create_resampler(uint32_t inSampleRate,
204                     uint32_t outSampleRate,
205                     uint32_t channelCount,
206                     uint32_t quality,
207                     struct resampler_buffer_provider* provider,
208                     struct resampler_itfe **resampler)
209 {
210     int error;
211     struct resampler *rsmp;
212
213     LOGV("create_resampler() In SR %d Out SR %d channels %d",
214          inSampleRate, outSampleRate, channelCount);
215
216     if (resampler == NULL) {
217         return -EINVAL;
218     }
219
220     *resampler = NULL;
221
222     if (quality <= RESAMPLER_QUALITY_MIN || quality >= RESAMPLER_QUALITY_MAX) {
223         return -EINVAL;
224     }
225
226     rsmp = (struct resampler *)calloc(1, sizeof(struct resampler));
227
228     rsmp->speex_resampler = speex_resampler_init(channelCount,
229                                       inSampleRate,
230                                       outSampleRate,
231                                       quality,
232                                       &error);
233     if (rsmp->speex_resampler == NULL) {
234         LOGW("ReSampler: Cannot create speex resampler: %s", speex_resampler_strerror(error));
235         return -ENODEV;
236     }
237
238     rsmp->itfe.reset = resampler_reset;
239     rsmp->itfe.resample_from_provider = resampler_resample_from_provider;
240     rsmp->itfe.resample_from_input = resampler_resample_from_input;
241     rsmp->itfe.delay_ns = resampler_delay_ns;
242
243     rsmp->provider = provider;
244     rsmp->in_sample_rate = inSampleRate;
245     rsmp->out_sample_rate = outSampleRate;
246     rsmp->channel_count = channelCount;
247     rsmp->in_buf = NULL;
248     rsmp->in_buf_size = 0;
249
250     resampler_reset(&rsmp->itfe);
251
252     int frames = speex_resampler_get_input_latency(rsmp->speex_resampler);
253     rsmp->speex_delay_ns = (int32_t)((1000000000 * (int64_t)frames) / rsmp->in_sample_rate);
254     frames = speex_resampler_get_output_latency(rsmp->speex_resampler);
255     rsmp->speex_delay_ns += (int32_t)((1000000000 * (int64_t)frames) / rsmp->out_sample_rate);
256
257     *resampler = &rsmp->itfe;
258     LOGV("create_resampler() DONE rsmp %p &rsmp->itfe %p speex %p",
259          rsmp, &rsmp->itfe, rsmp->speex_resampler);
260     return 0;
261 }
262
263 void release_resampler(struct resampler_itfe *resampler)
264 {
265     struct resampler *rsmp = (struct resampler *)resampler;
266
267     if (rsmp == NULL) {
268         return;
269     }
270
271     free(rsmp->in_buf);
272
273     if (rsmp->speex_resampler != NULL) {
274         speex_resampler_destroy(rsmp->speex_resampler);
275     }
276     free(rsmp);
277 }
278
279 /*
280  * Android 4.0 resampler (end)
281  */
282
283 int tinyalsa_audio_resampler_get_next_buffer(
284         struct resampler_buffer_provider *buffer_provider,
285         struct resampler_buffer* buffer)
286 {
287         TinyALSAAudioResampler *r =
288                 (TinyALSAAudioResampler *) buffer_provider->pdata;
289
290         char *addr = NULL;
291         int rc;
292
293         if(r->mFramesIn == 0) {
294                 rc = pcm_read(r->mPcm, r->mPcmReadBuffer,
295                         r->mPcmReadFrames * r->mFrameSizeIn);
296
297                 r->mFramesIn = r->mPcmReadFrames;
298         }
299
300         addr = (char *) r->mPcmReadBuffer +
301                 (r->mPcmReadFrames - r->mFramesIn) * r->mFrameSizeIn;
302
303         buffer->frame_count = (buffer->frame_count > r->mFramesIn) ?
304                 r->mFramesIn : buffer->frame_count;
305         buffer->i16 = (short int *) addr;
306
307         return 0;
308 }
309
310 void tinyalsa_audio_resampler_release_buffer(
311         struct resampler_buffer_provider *buffer_provider,
312         struct resampler_buffer* buffer)
313 {
314         TinyALSAAudioResampler *r =
315                 (TinyALSAAudioResampler *) buffer_provider->pdata;
316
317         r->mFramesIn -= buffer->frame_count;
318 }
319
320 TinyALSAAudioResampler::TinyALSAAudioResampler(
321         struct pcm *Pcm, int PcmReadFrames,
322         uint32_t sampleRateIn, uint32_t channelsIn,
323         uint32_t sampleRateOut, uint32_t channelsOut) :
324         mPcm(Pcm), mPcmReadFrames(PcmReadFrames),
325         mSampleRateIn(sampleRateIn), mChannelsIn(channelsIn),
326         mSampleRateOut(sampleRateOut), mChannelsOut(channelsOut)
327 {
328         LOGD("TinyALSAAudioResampler(%d, %d, %d, %d)",
329                 sampleRateIn, channelsIn, sampleRateOut, channelsOut);
330
331         mFrameSizeIn = mChannelsIn * sizeof(int16_t);
332         mFrameSizeOut = mChannelsOut * sizeof(int16_t);
333
334         mResampledFrames = 320;
335         mFramesIn = 0;
336
337         mPcmReadBuffer = malloc(mPcmReadFrames * mFrameSizeIn);
338         mResampledBuffer = malloc(mResampledFrames * mFrameSizeIn);
339
340         if(mFrameSizeIn != mFrameSizeOut) {
341
342                 mOutBuffer = malloc(mResampledFrames * mFrameSizeOut);
343         } else {
344                 mOutBuffer = mResampledBuffer;
345         }
346
347         mResamplerProvider.get_next_buffer =
348                 tinyalsa_audio_resampler_get_next_buffer;
349         mResamplerProvider.release_buffer =
350                 tinyalsa_audio_resampler_release_buffer;
351         mResamplerProvider.pdata = (void *) this;
352
353         create_resampler(sampleRateIn, sampleRateOut, channelsIn,
354                 RESAMPLER_QUALITY_DEFAULT, &mResamplerProvider, &mResampler);
355 }
356
357 TinyALSAAudioResampler::~TinyALSAAudioResampler()
358 {
359         LOGD("~TinyALSAAudioResampler()");
360
361         release_resampler(mResampler);
362
363         if(mOutBuffer != NULL && mOutBuffer != mResampledBuffer)
364                 free(mOutBuffer);
365         if(mResampledBuffer != NULL)
366                 free(mResampledBuffer);
367         if(mPcmReadBuffer != NULL)
368                 free(mPcmReadBuffer);
369 }
370
371 void TinyALSAAudioResampler::convertChannels(void *inBuffer, void *outBuffer,
372         int inChannels, int outChannels, int frames)
373 {
374         int16_t *in = (int16_t *) inBuffer;
375         int16_t *out = (int16_t *) outBuffer;
376         int16_t d;
377         int i;
378
379         if(inChannels == 2 && outChannels == 1) {
380                 for(i=0 ; i < frames ; i++) {
381                         d = (in[i*2] + in[i*2+1]) / 2;
382                         out[i] = d;
383                 }
384         }
385 }
386
387 ssize_t TinyALSAAudioResampler::resample(void *buffer, int length)
388 {
389         size_t frames = length / mFrameSizeOut;
390         ssize_t bytes = 0;
391         int rc;
392
393         if(frames > mResampledFrames) {
394                 LOGE("TinyALSAAudioResampler::resample() failed: asked to read more frames than what the buffer can hold");
395                 return -ENOMEM;
396         }
397
398         rc = mResampler->resample_from_provider(mResampler,
399                 (int16_t *) mResampledBuffer, &frames);
400
401         if(rc < 0) {
402                 LOGE("TinyALSAAudioResampler::resample() failed");
403                 return 0;
404         }
405
406         if(mFrameSizeIn != mFrameSizeOut) {
407                 convertChannels(mResampledBuffer, mOutBuffer, mChannelsIn, mChannelsOut, frames);
408         }
409
410         bytes = frames * mFrameSizeOut > length ?
411                 length : frames * mFrameSizeOut;
412
413         memcpy(buffer, mOutBuffer, length);
414
415         return bytes;
416 }
417
418 uint32_t TinyALSAAudioResampler::sampleRate(void) const
419 {
420         return (uint32_t) mSampleRateOut;
421 }
422
423 size_t TinyALSAAudioResampler::bufferSize(void) const
424 {
425         return (size_t) mResampledFrames * mFrameSizeOut;
426 }
427
428 uint32_t TinyALSAAudioResampler::channels(void) const
429 {
430         switch(mChannelsOut) {
431                 case 1:
432                         return AudioSystem::CHANNEL_OUT_MONO;
433                 case 2:
434                         return AudioSystem::CHANNEL_OUT_STEREO;
435                 case 4:
436                         return AudioSystem::CHANNEL_OUT_QUAD;
437                 case 6:
438                         return AudioSystem::CHANNEL_OUT_5POINT1;
439                 default:
440                         return AudioSystem::CHANNEL_OUT_STEREO;
441         }
442 }
443
444
445 }; // namespace android