xref: /btstack/src/classic/hfp_codec.c (revision 360d44f609bbebbf954ea85f461752a6b07644cd)
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 #endif
61 
62 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
63 #define LC3_SWB_OCTETS_PER_FRAME   58
64 static void hfp_codec_encode_lc3swb(hfp_codec_t * hfp_codec, int16_t * pcm_samples);
65 #endif
66 
67 
68 void hfp_codec_init(hfp_codec_t * hfp_codec, uint8_t codec_id){
69     memset(hfp_codec, 0, sizeof(hfp_codec_t));
70     hfp_h2_framing_init(&hfp_codec->h2_framing);
71     switch (codec_id){
72 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
73         case HFP_CODEC_MSBC:
74             hfp_codec->samples_per_frame = 120;
75             hfp_codec->encode = &hfp_codec_encode_msbc;
76             btstack_sbc_encoder_init(&hfp_codec->msbc_state, SBC_MODE_mSBC, 16, 8, SBC_ALLOCATION_METHOD_LOUDNESS, 16000, 26, SBC_CHANNEL_MODE_MONO);
77             break;
78 #endif
79 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
80         case HFP_CODEC_LC3_SWB:
81             hfp_codec->samples_per_frame = 240;
82             hfp_codec->encode = &hfp_codec_encode_lc3swb;
83             // init lc3 encoder
84             hfp_codec->lc3_encoder = btstack_lc3_encoder_google_init_instance(&hfp_codec->lc3_encoder_context);
85             hfp_codec->lc3_encoder->configure(&hfp_codec->lc3_encoder_context, 32000, BTSTACK_LC3_FRAME_DURATION_7500US, LC3_SWB_OCTETS_PER_FRAME);
86             break;
87 #endif
88         default:
89             btstack_assert(false);
90             break;
91     }
92 }
93 
94 bool hfp_codec_can_encode_audio_frame_now(const hfp_codec_t * hfp_codec){
95     return hfp_codec->write_pos <= SCO_FRAME_SIZE;
96 }
97 
98 uint16_t hfp_codec_num_audio_samples_per_frame(const hfp_codec_t * hfp_codec){
99     return hfp_codec->samples_per_frame;
100 }
101 
102 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
103 static void hfp_codec_encode_msbc(hfp_codec_t * hfp_codec, int16_t * pcm_samples){
104     // Encode SBC Frame
105     btstack_sbc_encoder_process_data(pcm_samples);
106     (void)memcpy(&hfp_codec->sco_packet[hfp_codec->write_pos], btstack_sbc_encoder_sbc_buffer(), FRAME_SIZE_MSBC);
107     hfp_codec->write_pos += FRAME_SIZE_MSBC;
108     // Final padding to use SCO_FRAME_SIZE bytes
109     hfp_codec->sco_packet[hfp_codec->write_pos++] = 0;
110 }
111 #endif
112 
113 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
114 static void hfp_codec_encode_lc3swb(hfp_codec_t * hfp_codec, int16_t * pcm_samples){
115     // Encode LC3 Frame
116     hfp_codec->lc3_encoder->encode_signed_16(&hfp_codec->lc3_encoder_context, pcm_samples, 1, &hfp_codec->sco_packet[hfp_codec->write_pos]);
117     hfp_codec->write_pos += LC3_SWB_OCTETS_PER_FRAME;
118 }
119 #endif
120 
121 void hfp_codec_encode_audio_frame(hfp_codec_t * hfp_codec, int16_t * pcm_samples){
122     btstack_assert(hfp_codec_can_encode_audio_frame_now(hfp_codec));
123     // Synchronization Header H2
124     hfp_h2_framing_add_header(&hfp_codec->h2_framing, &hfp_codec->sco_packet[hfp_codec->write_pos]);
125     hfp_codec->write_pos += 2;
126     // encode
127 #ifdef HFP_CODEC_TEST
128     // packet counter
129     static uint8_t counter = 0;
130     hfp_codec->sco_packet[hfp_codec->write_pos++] = counter++;
131     // test data
132     uint8_t i;
133     for (i=3;i<SCO_FRAME_SIZE;i++){
134         hfp_codec->sco_packet[hfp_codec->write_pos++] = i;
135     }
136 #else
137     hfp_codec->encode(hfp_codec, pcm_samples);
138 #endif
139     log_info("Encode frame, read %u, write %u", hfp_codec->read_pos, hfp_codec->write_pos);
140 }
141 
142 uint16_t hfp_codec_num_bytes_available(const hfp_codec_t * hfp_codec){
143     return hfp_codec->write_pos - hfp_codec->read_pos;
144 }
145 
146 void hfp_codec_read_from_stream(hfp_codec_t * hfp_codec, uint8_t * buf, uint16_t size){
147     uint16_t num_bytes_available = hfp_codec_num_bytes_available(hfp_codec);
148     btstack_assert(num_bytes_available >= size);
149 
150     uint16_t num_bytes_to_copy = btstack_min(num_bytes_available, size);
151 
152     memcpy(buf, &hfp_codec->sco_packet[hfp_codec->read_pos], num_bytes_to_copy);
153     hfp_codec->read_pos += num_bytes_to_copy;
154 
155     // reset buffer
156     if (hfp_codec->read_pos == hfp_codec->write_pos){
157         hfp_codec->read_pos = 0;
158         hfp_codec->write_pos = 0;
159     }
160 
161     log_info("Read %u from stream, read %u, write %u", size, hfp_codec->read_pos, hfp_codec->write_pos);
162 }
163 
164 void hfp_codec_deinit(hfp_codec_t * hfp_codec){
165     UNUSED(hfp_codec);
166 }
167