1 /***
2 This file is part of PulseAudio.
3
4 PulseAudio is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8
9 PulseAudio is distributed in the hope that it will be useful, but
10 WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 General Public License for more details.
13
14 You should have received a copy of the GNU Lesser General Public
15 License along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
16 ***/
17 #ifdef HAVE_CONFIG_H
18 #include <config.h>
19 #endif
20
21 #include <pulsecore/core.h>
22 #include "bt-codec-api.h"
23
24 #include "bt-codec-msbc.h"
25 #include <sbc/sbc.h>
26
27 typedef struct sbc_info {
28 sbc_t sbc; /* Codec data */
29 size_t codesize, frame_length; /* SBC Codesize, frame_length. We simply cache those values here */
30 uint8_t msbc_seq:2; /* mSBC packet sequence number, 2 bits only */
31
32 uint16_t msbc_push_offset;
33 uint8_t input_buffer[MSBC_PACKET_SIZE]; /* Codec transfer buffer */
34
35 pa_sample_spec sample_spec;
36 } sbc_info_t;
37
init(bool for_encoding,bool for_backchannel,const uint8_t * config_buffer,uint8_t config_size,pa_sample_spec * sample_spec,pa_core * core)38 static void *init(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) {
39 struct sbc_info *info;
40 int ret;
41
42 info = pa_xnew0(struct sbc_info, 1);
43
44 ret = sbc_init_msbc(&info->sbc, 0);
45 if (ret != 0) {
46 pa_xfree(info);
47 pa_log_error("mSBC initialization failed: %d", ret);
48 return NULL;
49 }
50
51 info->sbc.endian = SBC_LE;
52
53 info->codesize = sbc_get_codesize(&info->sbc);
54 info->frame_length = sbc_get_frame_length(&info->sbc);
55 pa_log_info("mSBC codesize=%d, frame_length=%d",
56 (int)info->codesize,
57 (int)info->frame_length);
58
59 info->sample_spec.format = PA_SAMPLE_S16LE;
60 info->sample_spec.channels = 1;
61 info->sample_spec.rate = 16000;
62
63 pa_assert(pa_frame_aligned(info->codesize, &info->sample_spec));
64
65 *sample_spec = info->sample_spec;
66
67 return info;
68 }
69
deinit(void * codec_info)70 static void deinit(void *codec_info) {
71 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
72
73 sbc_finish(&sbc_info->sbc);
74 pa_xfree(sbc_info);
75 }
76
reset(void * codec_info)77 static int reset(void *codec_info) {
78 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
79 int ret;
80
81 /* SBC library release 1.5 has a bug in sbc_reinit_msbc:
82 * it forgets to restore priv->msbc flag after clearing priv content.
83 * This causes decoder assertion on first call since codesize would be
84 * different from expected for mSBC configuration.
85 *
86 * Do not use sbc_reinit_msbc until it is fixed.
87 */
88
89 sbc_finish(&sbc_info->sbc);
90 ret = sbc_init_msbc(&sbc_info->sbc, 0);
91 if (ret != 0) {
92 pa_xfree(sbc_info);
93 pa_log_error("mSBC initialization failed: %d", ret);
94 return -1;
95 }
96
97 sbc_info->sbc.endian = SBC_LE;
98
99 sbc_info->msbc_seq = 0;
100 sbc_info->msbc_push_offset = 0;
101
102 return 0;
103 }
104
get_read_block_size(void * codec_info,size_t link_mtu)105 static size_t get_read_block_size(void *codec_info, size_t link_mtu) {
106 struct sbc_info *info = (struct sbc_info *) codec_info;
107 size_t block_size = info->codesize;
108
109 /* this never happens as sbc_info->codesize is always frame-aligned */
110 if (!pa_frame_aligned(block_size, &info->sample_spec)) {
111 pa_log_debug("Got invalid block size: %lu, rounding down", block_size);
112 block_size = pa_frame_align(block_size, &info->sample_spec);
113 }
114
115 /* If MTU exceeds mSBC frame size there could be up to 1 + MTU / (mSBC frame size)
116 * frames decoded for single incoming packet.
117 * See also pa_bluetooth_transport::last_read_size handling
118 * and comment about MTU size in bt_prepare_encoder_buffer()
119 */
120 if (link_mtu <= MSBC_PACKET_SIZE)
121 return block_size;
122
123 return block_size * (1 + link_mtu / MSBC_PACKET_SIZE);
124 }
125
get_write_block_size(void * codec_info,size_t link_mtu)126 static size_t get_write_block_size(void *codec_info, size_t link_mtu) {
127 struct sbc_info *info = (struct sbc_info *) codec_info;
128 return info->codesize;
129 }
130
get_encoded_block_size(void * codec_info,size_t input_size)131 static size_t get_encoded_block_size(void *codec_info, size_t input_size) {
132 struct sbc_info *info = (struct sbc_info *) codec_info;
133 size_t encoded_size = MSBC_PACKET_SIZE;
134
135 /* input size should be aligned to write block size */
136 pa_assert_fp(input_size % info->codesize == 0);
137
138 return encoded_size * (input_size / info->codesize);
139 }
140
reduce_encoder_bitrate(void * codec_info,size_t write_link_mtu)141 static size_t reduce_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
142 return 0;
143 }
144
increase_encoder_bitrate(void * codec_info,size_t write_link_mtu)145 static size_t increase_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
146 return 0;
147 }
148
encode_buffer(void * codec_info,uint32_t timestamp,const uint8_t * input_buffer,size_t input_size,uint8_t * output_buffer,size_t output_size,size_t * processed)149 static size_t encode_buffer(void *codec_info, uint32_t timestamp, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
150 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
151 struct msbc_frame *frame;
152 uint8_t seq;
153 ssize_t encoded;
154 ssize_t written;
155
156 pa_assert(input_size == sbc_info->codesize);
157
158 /* must be room to render packet */
159 pa_assert(output_size >= MSBC_PACKET_SIZE);
160
161 frame = (struct msbc_frame *)output_buffer;
162 seq = sbc_info->msbc_seq++;
163 frame->hdr.id0 = MSBC_H2_ID0;
164 frame->hdr.id1.s.id1 = MSBC_H2_ID1;
165 if (seq & 0x02)
166 frame->hdr.id1.s.sn1 = 3;
167 else
168 frame->hdr.id1.s.sn1 = 0;
169 if (seq & 0x01)
170 frame->hdr.id1.s.sn0 = 3;
171 else
172 frame->hdr.id1.s.sn0 = 0;
173
174 encoded = sbc_encode(&sbc_info->sbc,
175 input_buffer, input_size,
176 frame->payload, MSBC_FRAME_SIZE,
177 &written);
178
179 frame->padding = 0x00;
180
181 if (PA_UNLIKELY(encoded <= 0)) {
182 pa_log_error("SBC encoding error (%li) for input size %lu, SBC codesize %lu",
183 (long) encoded, input_size, sbc_get_codesize(&sbc_info->sbc));
184
185 if (encoded < 0) {
186 *processed = 0;
187 return -1;
188 } else {
189 *processed = input_size;
190 return 0;
191 }
192 }
193
194 pa_assert_fp((size_t) encoded == sbc_info->codesize);
195 pa_assert_fp((size_t) written == sbc_info->frame_length);
196
197 *processed = encoded;
198
199 return MSBC_PACKET_SIZE;
200 }
201
is_all_zero(const uint8_t * ptr,size_t len)202 static inline bool is_all_zero(const uint8_t *ptr, size_t len) {
203 size_t i;
204
205 for (i = 0; i < len; ++i)
206 if (ptr[i] != 0)
207 return false;
208
209 return true;
210 }
211
212 /*
213 * We build a msbc frame up in the sbc_info buffer until we have a whole one
214 */
msbc_find_frame(struct sbc_info * si,size_t * len,const uint8_t * buf,int * pseq)215 static struct msbc_frame *msbc_find_frame(struct sbc_info *si, size_t *len,
216 const uint8_t *buf, int *pseq)
217 {
218 size_t i;
219 uint8_t *p = si->input_buffer;
220
221 /* skip input if it has all zero bytes
222 * this could happen with older kernels inserting all-zero blocks
223 * inside otherwise valid mSBC stream */
224 if (*len > 0 && is_all_zero(buf, *len))
225 *len = 0;
226
227 for (i = 0; i < *len; i++) {
228 union msbc_h2_id1 id1;
229
230 if (si->msbc_push_offset == 0) {
231 if (buf[i] != MSBC_H2_ID0)
232 continue;
233 } else if (si->msbc_push_offset == 1) {
234 id1.b = buf[i];
235
236 if (id1.s.id1 != MSBC_H2_ID1)
237 goto error;
238 if (id1.s.sn0 != 3 && id1.s.sn0 != 0)
239 goto error;
240 if (id1.s.sn1 != 3 && id1.s.sn1 != 0)
241 goto error;
242 } else if (si->msbc_push_offset == 2) {
243 if (buf[i] != MSBC_SYNC_BYTE)
244 goto error;
245 }
246 p[si->msbc_push_offset++] = buf[i];
247
248 if (si->msbc_push_offset == MSBC_PACKET_SIZE) {
249 id1.b = p[1];
250 *pseq = (id1.s.sn0 & 0x1) | (id1.s.sn1 & 0x2);
251 si->msbc_push_offset = 0;
252 *len -= i + 1;
253 return (struct msbc_frame *)p;
254 }
255 continue;
256
257 error:
258 si->msbc_push_offset = 0;
259 }
260 *len = 0;
261 return NULL;
262 }
263
decode_buffer(void * codec_info,const uint8_t * input_buffer,size_t input_size,uint8_t * output_buffer,size_t output_size,size_t * processed)264 static size_t decode_buffer(void *codec_info, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
265 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
266 size_t save_input_size;
267 ssize_t decoded;
268 size_t written = 0;
269 size_t total_written = 0;
270 size_t total_processed = 0;
271 struct msbc_frame *frame;
272 int seq;
273
274 while (input_size > 0) {
275
276 save_input_size = input_size;
277 frame = msbc_find_frame(sbc_info, &input_size, input_buffer + total_processed, &seq);
278
279 total_processed += save_input_size - input_size;
280
281 /* Only full mSBC frame should be decoded */
282 if (!frame)
283 break;
284
285 uint8_t lost_packets = (4 + seq - sbc_info->msbc_seq++) % 4;
286
287 if (lost_packets) {
288 pa_log_debug("Lost %d input audio packet(s)", lost_packets);
289 sbc_info->msbc_seq = seq + 1;
290 }
291
292 /* pa_bt_codec::get_read_block_size must provide space for all decoded frames */
293 pa_assert_fp(output_size >= sbc_info->codesize);
294
295 decoded = sbc_decode(&sbc_info->sbc, frame->payload, MSBC_FRAME_SIZE, output_buffer, output_size, &written);
296
297 if (PA_UNLIKELY(decoded <= 0)) {
298 pa_log_error("mSBC decoding error (%li)", (long) decoded);
299 pa_silence_memory(output_buffer, sbc_info->codesize, &sbc_info->sample_spec);
300 decoded = sbc_info->frame_length;
301 written = sbc_info->codesize;
302 }
303
304 pa_assert_fp((size_t)decoded == sbc_info->frame_length);
305 pa_assert_fp((size_t)written == sbc_info->codesize);
306
307 output_buffer += written;
308 output_size -= written;
309
310 total_written += written;
311 }
312
313 *processed = total_processed;
314 return total_written;
315 }
316
317 /* Modified SBC codec for HFP Wideband Speech*/
318 const pa_bt_codec pa_bt_codec_msbc = {
319 .name = "mSBC",
320 .description = "mSBC",
321 .init = init,
322 .deinit = deinit,
323 .reset = reset,
324 .get_read_block_size = get_read_block_size,
325 .get_write_block_size = get_write_block_size,
326 .get_encoded_block_size = get_encoded_block_size,
327 .reduce_encoder_bitrate = reduce_encoder_bitrate,
328 .increase_encoder_bitrate = increase_encoder_bitrate,
329 .encode_buffer = encode_buffer,
330 .decode_buffer = decode_buffer,
331 };
332