1 /*
2 * Copyright (C) 2018 BlueKitchen GmbH
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 * 3. Neither the name of the copyright holders nor the names of
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 * 4. Any redistribution, use, or modification is done solely for
17 * personal benefit and not for any commercial purpose or for
18 * monetary gain.
19 *
20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
24 * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31 * SUCH DAMAGE.
32 *
33 * Please inquire about commercial licensing options at
34 * [email protected]
35 *
36 */
37
38 #define BTSTACK_FILE__ "btstack_resample.c"
39
40 #include "btstack_bool.h"
41 #include "btstack_debug.h"
42 #include "btstack_resample.h"
43
btstack_resample_init(btstack_resample_t * context,int num_channels)44 void btstack_resample_init(btstack_resample_t * context, int num_channels){
45 btstack_assert(num_channels <= BTSTACK_RESAMPLE_MAX_CHANNELS);
46
47 context->src_pos = 0;
48 context->src_step = 0x10000; // default resampling 1.0
49 int i;
50 for (i = 0; i < num_channels; i++){
51 context->last_sample[i] = 0;
52 }
53 context->num_channels = num_channels;
54 }
55
btstack_resample_set_factor(btstack_resample_t * context,uint32_t src_step)56 void btstack_resample_set_factor(btstack_resample_t * context, uint32_t src_step){
57 context->src_step = src_step;
58 }
59
btstack_resample_block(btstack_resample_t * context,const int16_t * input_buffer,uint32_t num_frames,int16_t * output_buffer)60 uint16_t btstack_resample_block(btstack_resample_t * context, const int16_t * input_buffer, uint32_t num_frames, int16_t * output_buffer){
61 btstack_assert(context->num_channels > 0);
62
63 uint16_t dest_frames = 0;
64 uint16_t dest_samples = 0;
65 // samples between last sample of previous block and first sample in current block
66 while (context->src_pos >= 0xffff0000){
67 const uint16_t t = context->src_pos & 0xffffu;
68 int i;
69 for (i=0;i<context->num_channels;i++){
70 int s1 = context->last_sample[i];
71 int s2 = input_buffer[i];
72 int os = ((s1*(0x10000u - t)) + (s2*t)) >> 16u;
73 output_buffer[dest_samples++] = os;
74 }
75 dest_frames++;
76 context->src_pos += context->src_step;
77 }
78 // process current block
79 while (true){
80 const uint16_t src_pos = context->src_pos >> 16;
81 const uint16_t t = context->src_pos & 0xffffu;
82 int index = src_pos * context->num_channels;
83 int i;
84 if (src_pos >= (num_frames - 1u)){
85 // store last sample
86 index = (num_frames-1u) * context->num_channels;
87 for (i=0;i<context->num_channels;i++){
88 context->last_sample[i] = input_buffer[index++];
89 }
90 // samples processed
91 context->src_pos -= num_frames << 16;
92 break;
93 }
94 for (i=0;i<context->num_channels;i++){
95 int s1 = input_buffer[index];
96 int s2 = input_buffer[index+context->num_channels];
97 int os = ((s1*(0x10000u - t)) + (s2*t)) >> 16u;
98 output_buffer[dest_samples++] = os;
99 index++;
100 }
101 dest_frames++;
102 context->src_pos += context->src_step;
103 }
104 return dest_frames;
105 }
106