1 /*
2 * Copyright (C) 2023 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__ "hfp_codec.c"
39
40 // *****************************************************************************
41 //
42 // HFP Codec
43 //
44 // *****************************************************************************
45
46 #include "btstack_config.h"
47
48 #include <string.h>
49
50 #include "hfp_codec.h"
51 #include "btstack_debug.h"
52
53 // enable to send test data
54 // #define HFP_CODEC_TEST
55
56 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
57 #include "btstack_sbc.h"
58 #define FRAME_SIZE_MSBC 57
59 static void hfp_codec_encode_msbc(hfp_codec_t * hfp_codec, int16_t * pcm_samples);
60 static void hfp_codec_encode_msbc_with_codec(hfp_codec_t * hfp_codec, int16_t * pcm_samples);
61 #endif
62
63 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
64 #define LC3_SWB_OCTETS_PER_FRAME 58
65 static void hfp_codec_encode_lc3swb(hfp_codec_t * hfp_codec, int16_t * pcm_samples);
66 #endif
67
68 // HFP H2 Framing - might get moved into a hfp_h2.c
69
70 // const
71 static const uint8_t hfp_h2_header_h2_byte_0 = 1;
72 static const uint8_t hfp_h2_header_h2_byte_1_table[] = {0x08, 0x38, 0xc8, 0xf8 };
73
hfp_h2_framing_init(hfp_h2_framing_t * hfp_h2_framing)74 void hfp_h2_framing_init(hfp_h2_framing_t * hfp_h2_framing){
75 hfp_h2_framing->sequence_number = 0;
76 }
77
78 /**
79 * @brief Add next H2 Header
80 * @param hfp_h2_framing
81 * @param buffer [2]
82 */
hfp_h2_framing_add_header(hfp_h2_framing_t * hfp_h2_framing,uint8_t * buffer)83 void hfp_h2_framing_add_header(hfp_h2_framing_t * hfp_h2_framing, uint8_t * buffer){
84 // Synchronization Header H2
85 buffer[0] = hfp_h2_header_h2_byte_0;
86 buffer[1] = hfp_h2_header_h2_byte_1_table[hfp_h2_framing->sequence_number];
87 hfp_h2_framing->sequence_number = (hfp_h2_framing->sequence_number + 1) & 3;
88 }
89
hfp_codec_reset_sco_buffer(hfp_codec_t * hfp_codec)90 static void hfp_codec_reset_sco_buffer(hfp_codec_t *hfp_codec) {
91 hfp_codec->read_pos = 0;
92 hfp_codec->write_pos = 0;
93 }
94
95 #if defined(ENABLE_HFP_WIDE_BAND_SPEECH) || defined(ENABLE_HFP_SUPER_WIDE_BAND_SPEECH)
hfp_codec_init_helper(hfp_codec_t * hfp_codec,void (* encode)(struct hfp_codec *,int16_t *),uint16_t samples_per_buffer)96 static void hfp_codec_init_helper(hfp_codec_t *hfp_codec, void (*encode)(struct hfp_codec *, int16_t *),
97 uint16_t samples_per_buffer) {
98 memset(hfp_codec, 0, sizeof(hfp_codec_t));
99 hfp_h2_framing_init(&hfp_codec->h2_framing);
100 hfp_codec_reset_sco_buffer(hfp_codec);
101 hfp_codec->samples_per_frame = samples_per_buffer;
102 hfp_codec->encode = encode;
103 }
104 #endif
105
106 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
107 // old
hfp_codec_init_msbc(hfp_codec_t * hfp_codec,btstack_sbc_encoder_state_t * msbc_encoder_context)108 void hfp_codec_init_msbc(hfp_codec_t * hfp_codec, btstack_sbc_encoder_state_t * msbc_encoder_context){
109 hfp_codec_init_helper(hfp_codec, &hfp_codec_encode_msbc, 120);
110 hfp_codec->msbc_encoder_context = msbc_encoder_context;
111 btstack_sbc_encoder_init(hfp_codec->msbc_encoder_context, SBC_MODE_mSBC, 16, 8, SBC_ALLOCATION_METHOD_LOUDNESS, 16000, 26, SBC_CHANNEL_MODE_MONO);
112 }
113 // new
hfp_codec_init_msbc_with_codec(hfp_codec_t * hfp_codec,const btstack_sbc_encoder_t * sbc_encoder,void * sbc_encoder_context)114 void hfp_codec_init_msbc_with_codec(hfp_codec_t * hfp_codec, const btstack_sbc_encoder_t * sbc_encoder, void * sbc_encoder_context){
115 hfp_codec_init_helper(hfp_codec, &hfp_codec_encode_msbc_with_codec, 120);
116 // init sbc encoder
117 hfp_codec->sbc_encoder_instance = sbc_encoder;
118 hfp_codec->sbc_encoder_context = sbc_encoder_context;
119 hfp_codec->sbc_encoder_instance->configure(hfp_codec->sbc_encoder_context, SBC_MODE_mSBC, 16, 8, SBC_ALLOCATION_METHOD_LOUDNESS, 16000, 26, SBC_CHANNEL_MODE_MONO);
120 }
121 #endif
122
123 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
hfp_codec_init_lc3_swb(hfp_codec_t * hfp_codec,const btstack_lc3_encoder_t * lc3_encoder,void * lc3_encoder_context)124 void hfp_codec_init_lc3_swb(hfp_codec_t * hfp_codec, const btstack_lc3_encoder_t * lc3_encoder, void * lc3_encoder_context){
125 hfp_codec_init_helper(hfp_codec, &hfp_codec_encode_lc3swb, 240);
126 // init lc3 encoder
127 hfp_codec->lc3_encoder = lc3_encoder;
128 hfp_codec->lc3_encoder_context = lc3_encoder_context;
129 hfp_codec->lc3_encoder->configure(hfp_codec->lc3_encoder_context, 32000, BTSTACK_LC3_FRAME_DURATION_7500US, LC3_SWB_OCTETS_PER_FRAME);
130 }
131 #endif
132
hfp_codec_can_encode_audio_frame_now(const hfp_codec_t * hfp_codec)133 bool hfp_codec_can_encode_audio_frame_now(const hfp_codec_t * hfp_codec){
134 return hfp_codec->write_pos <= SCO_FRAME_SIZE;
135 }
136
hfp_codec_num_audio_samples_per_frame(const hfp_codec_t * hfp_codec)137 uint16_t hfp_codec_num_audio_samples_per_frame(const hfp_codec_t * hfp_codec){
138 return hfp_codec->samples_per_frame;
139 }
140
141 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
142 // old
hfp_codec_encode_msbc(hfp_codec_t * hfp_codec,int16_t * pcm_samples)143 static void hfp_codec_encode_msbc(hfp_codec_t * hfp_codec, int16_t * pcm_samples){
144 // Encode SBC Frame
145 btstack_sbc_encoder_process_data(pcm_samples);
146 (void)memcpy(&hfp_codec->sco_packet[hfp_codec->write_pos], btstack_sbc_encoder_sbc_buffer(), FRAME_SIZE_MSBC);
147 hfp_codec->write_pos += FRAME_SIZE_MSBC;
148 // Final padding to use SCO_FRAME_SIZE bytes
149 hfp_codec->sco_packet[hfp_codec->write_pos++] = 0;
150 }
151 // new
hfp_codec_encode_msbc_with_codec(hfp_codec_t * hfp_codec,int16_t * pcm_samples)152 static void hfp_codec_encode_msbc_with_codec(hfp_codec_t * hfp_codec, int16_t * pcm_samples){
153 // Encode SBC Frame
154 hfp_codec->sbc_encoder_instance->encode_signed_16(hfp_codec->sbc_encoder_context, pcm_samples, &hfp_codec->sco_packet[hfp_codec->write_pos]);
155 hfp_codec->write_pos += FRAME_SIZE_MSBC;
156 // Final padding to use SCO_FRAME_SIZE bytes
157 hfp_codec->sco_packet[hfp_codec->write_pos++] = 0;
158 }
159 #endif
160
161 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
hfp_codec_encode_lc3swb(hfp_codec_t * hfp_codec,int16_t * pcm_samples)162 static void hfp_codec_encode_lc3swb(hfp_codec_t * hfp_codec, int16_t * pcm_samples){
163 // Encode LC3 Frame
164 hfp_codec->lc3_encoder->encode_signed_16(hfp_codec->lc3_encoder_context, pcm_samples, 1, &hfp_codec->sco_packet[hfp_codec->write_pos]);
165 hfp_codec->write_pos += LC3_SWB_OCTETS_PER_FRAME;
166 }
167 #endif
168
hfp_codec_encode_audio_frame(hfp_codec_t * hfp_codec,int16_t * pcm_samples)169 void hfp_codec_encode_audio_frame(hfp_codec_t * hfp_codec, int16_t * pcm_samples){
170 btstack_assert(hfp_codec_can_encode_audio_frame_now(hfp_codec));
171 // Synchronization Header H2
172 hfp_h2_framing_add_header(&hfp_codec->h2_framing, &hfp_codec->sco_packet[hfp_codec->write_pos]);
173 hfp_codec->write_pos += 2;
174 // encode
175 #ifdef HFP_CODEC_TEST
176 // packet counter
177 static uint8_t counter = 0;
178 hfp_codec->sco_packet[hfp_codec->write_pos++] = counter++;
179 // test data
180 uint8_t i;
181 for (i=3;i<SCO_FRAME_SIZE;i++){
182 hfp_codec->sco_packet[hfp_codec->write_pos++] = i;
183 }
184 #else
185 hfp_codec->encode(hfp_codec, pcm_samples);
186 #endif
187 log_debug("Encode frame, read %u, write %u", hfp_codec->read_pos, hfp_codec->write_pos);
188 }
189
hfp_codec_num_bytes_available(const hfp_codec_t * hfp_codec)190 uint16_t hfp_codec_num_bytes_available(const hfp_codec_t * hfp_codec){
191 return hfp_codec->write_pos - hfp_codec->read_pos;
192 }
193
hfp_codec_read_from_stream(hfp_codec_t * hfp_codec,uint8_t * buf,uint16_t size)194 void hfp_codec_read_from_stream(hfp_codec_t * hfp_codec, uint8_t * buf, uint16_t size){
195 uint16_t num_bytes_available = hfp_codec_num_bytes_available(hfp_codec);
196 btstack_assert(num_bytes_available >= size);
197
198 uint16_t num_bytes_to_copy = btstack_min(num_bytes_available, size);
199
200 memcpy(buf, &hfp_codec->sco_packet[hfp_codec->read_pos], num_bytes_to_copy);
201 hfp_codec->read_pos += num_bytes_to_copy;
202
203 // reset buffer
204 if (hfp_codec->read_pos == hfp_codec->write_pos){
205 hfp_codec_reset_sco_buffer(hfp_codec);
206 }
207
208 log_debug("Read %u from stream, read %u, write %u", size, hfp_codec->read_pos, hfp_codec->write_pos);
209 }
210
hfp_codec_deinit(hfp_codec_t * hfp_codec)211 void hfp_codec_deinit(hfp_codec_t * hfp_codec){
212 UNUSED(hfp_codec);
213 }
214