1 /******************************************************************************
2 *
3 * Copyright (C) 2004-2012 Broadcom Corporation
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18
19 /******************************************************************************
20 *
21 * This module contains utility functions for dealing with SBC data frames
22 * and codec capabilities.
23 *
24 ******************************************************************************/
25
26 #include "a2d_api.h"
27 #include "a2d_sbc.h"
28 #include "bta_av_sbc.h"
29
30 typedef int (tBTA_AV_SBC_ACT)(void *p_src, void *p_dst,
31 UINT32 src_samples, UINT32 dst_samples,
32 UINT32 *p_ret);
33
34 typedef struct
35 {
36 INT32 cur_pos; /* current position */
37 UINT32 src_sps; /* samples per second (source audio data) */
38 UINT32 dst_sps; /* samples per second (converted audio data) */
39 tBTA_AV_SBC_ACT *p_act; /* the action function to do the conversion */
40 UINT16 bits; /* number of bits per pcm sample */
41 UINT16 n_channels; /* number of channels (i.e. mono(1), stereo(2)...) */
42 INT16 worker1;
43 INT16 worker2;
44 UINT8 div;
45 } tBTA_AV_SBC_UPS_CB;
46
47 tBTA_AV_SBC_UPS_CB bta_av_sbc_ups_cb;
48
49 /*******************************************************************************
50 **
51 ** Function bta_av_sbc_init_up_sample
52 **
53 ** Description initialize the up sample
54 **
55 ** src_sps: samples per second (source audio data)
56 ** dst_sps: samples per second (converted audio data)
57 ** bits: number of bits per pcm sample
58 ** n_channels: number of channels (i.e. mono(1), stereo(2)...)
59 **
60 ** Returns none
61 **
62 *******************************************************************************/
bta_av_sbc_init_up_sample(UINT32 src_sps,UINT32 dst_sps,UINT16 bits,UINT16 n_channels)63 void bta_av_sbc_init_up_sample (UINT32 src_sps, UINT32 dst_sps, UINT16 bits, UINT16 n_channels)
64 {
65 bta_av_sbc_ups_cb.cur_pos = -1;
66 bta_av_sbc_ups_cb.src_sps = src_sps;
67 bta_av_sbc_ups_cb.dst_sps = dst_sps;
68 bta_av_sbc_ups_cb.bits = bits;
69 bta_av_sbc_ups_cb.n_channels= n_channels;
70
71 if(n_channels == 1)
72 {
73 /* mono */
74 if(bits == 8)
75 {
76 bta_av_sbc_ups_cb.p_act = bta_av_sbc_up_sample_8m;
77 bta_av_sbc_ups_cb.div = 1;
78 }
79 else
80 {
81 bta_av_sbc_ups_cb.p_act = bta_av_sbc_up_sample_16m;
82 bta_av_sbc_ups_cb.div = 2;
83 }
84 }
85 else
86 {
87 /* stereo */
88 if(bits == 8)
89 {
90 bta_av_sbc_ups_cb.p_act = bta_av_sbc_up_sample_8s;
91 bta_av_sbc_ups_cb.div = 2;
92 }
93 else
94 {
95 bta_av_sbc_ups_cb.p_act = bta_av_sbc_up_sample_16s;
96 bta_av_sbc_ups_cb.div = 4;
97 }
98 }
99 }
100
101 /*******************************************************************************
102 **
103 ** Function bta_av_sbc_up_sample
104 **
105 ** Description Given the source (p_src) audio data and
106 ** source speed (src_sps, samples per second),
107 ** This function converts it to audio data in the desired format
108 **
109 ** p_src: the data buffer that holds the source audio data
110 ** p_dst: the data buffer to hold the converted audio data
111 ** src_samples: The number of source samples (number of bytes)
112 ** dst_samples: The size of p_dst (number of bytes)
113 **
114 ** Note: An AE reported an issue with this function.
115 ** When called with bta_av_sbc_up_sample(src, uint8_array_dst..)
116 ** the byte before uint8_array_dst may get overwritten.
117 ** Using uint16_array_dst avoids the problem.
118 ** This issue is related to endian-ness and is hard to resolve
119 ** in a generic manner.
120 ** **************** Please use uint16 array as dst.
121 **
122 ** Returns The number of bytes used in p_dst
123 ** The number of bytes used in p_src (in *p_ret)
124 **
125 *******************************************************************************/
bta_av_sbc_up_sample(void * p_src,void * p_dst,UINT32 src_samples,UINT32 dst_samples,UINT32 * p_ret)126 int bta_av_sbc_up_sample (void *p_src, void *p_dst,
127 UINT32 src_samples, UINT32 dst_samples,
128 UINT32 *p_ret)
129 {
130 UINT32 src;
131 UINT32 dst;
132
133 if(bta_av_sbc_ups_cb.p_act)
134 {
135 src = src_samples/bta_av_sbc_ups_cb.div;
136 dst = dst_samples/bta_av_sbc_ups_cb.div;
137 return (*bta_av_sbc_ups_cb.p_act)(p_src, p_dst, src, dst, p_ret);
138 }
139 else
140 {
141 *p_ret = 0;
142 return 0;
143 }
144 }
145
146 /*******************************************************************************
147 **
148 ** Function bta_av_sbc_up_sample_16s (16bits-stereo)
149 **
150 ** Description Given the source (p_src) audio data and
151 ** source speed (src_sps, samples per second),
152 ** This function converts it to audio data in the desired format
153 **
154 ** p_src: the data buffer that holds the source audio data
155 ** p_dst: the data buffer to hold the converted audio data
156 ** src_samples: The number of source samples (in uint of 4 bytes)
157 ** dst_samples: The size of p_dst (in uint of 4 bytes)
158 **
159 ** Returns The number of bytes used in p_dst
160 ** The number of bytes used in p_src (in *p_ret)
161 **
162 *******************************************************************************/
bta_av_sbc_up_sample_16s(void * p_src,void * p_dst,UINT32 src_samples,UINT32 dst_samples,UINT32 * p_ret)163 int bta_av_sbc_up_sample_16s (void *p_src, void *p_dst,
164 UINT32 src_samples, UINT32 dst_samples,
165 UINT32 *p_ret)
166 {
167 INT16 *p_src_tmp = (INT16 *)p_src;
168 INT16 *p_dst_tmp = (INT16 *)p_dst;
169 INT16 *p_worker1 = &bta_av_sbc_ups_cb.worker1;
170 INT16 *p_worker2 = &bta_av_sbc_ups_cb.worker2;
171 UINT32 src_sps = bta_av_sbc_ups_cb.src_sps;
172 UINT32 dst_sps = bta_av_sbc_ups_cb.dst_sps;
173
174 while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples)
175 {
176 *p_dst_tmp++ = *p_worker1;
177 *p_dst_tmp++ = *p_worker2;
178
179 bta_av_sbc_ups_cb.cur_pos -= src_sps;
180 dst_samples--;
181 }
182
183 bta_av_sbc_ups_cb.cur_pos = dst_sps;
184
185 while (src_samples-- && dst_samples)
186 {
187 *p_worker1 = *p_src_tmp++;
188 *p_worker2 = *p_src_tmp++;
189
190 do
191 {
192 *p_dst_tmp++ = *p_worker1;
193 *p_dst_tmp++ = *p_worker2;
194
195 bta_av_sbc_ups_cb.cur_pos -= src_sps;
196 dst_samples--;
197 } while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples);
198
199 bta_av_sbc_ups_cb.cur_pos += dst_sps;
200 }
201
202 if (bta_av_sbc_ups_cb.cur_pos == (INT32)dst_sps)
203 bta_av_sbc_ups_cb.cur_pos = 0;
204
205 *p_ret = ((char *)p_src_tmp - (char *)p_src);
206 return ((char *)p_dst_tmp - (char *)p_dst);
207 }
208
209 /*******************************************************************************
210 **
211 ** Function bta_av_sbc_up_sample_16m (16bits-mono)
212 **
213 ** Description Given the source (p_src) audio data and
214 ** source speed (src_sps, samples per second),
215 ** This function converts it to audio data in the desired format
216 **
217 ** p_src: the data buffer that holds the source audio data
218 ** p_dst: the data buffer to hold the converted audio data
219 ** src_samples: The number of source samples (in uint of 2 bytes)
220 ** dst_samples: The size of p_dst (in uint of 2 bytes)
221 **
222 ** Returns The number of bytes used in p_dst
223 ** The number of bytes used in p_src (in *p_ret)
224 **
225 *******************************************************************************/
bta_av_sbc_up_sample_16m(void * p_src,void * p_dst,UINT32 src_samples,UINT32 dst_samples,UINT32 * p_ret)226 int bta_av_sbc_up_sample_16m (void *p_src, void *p_dst,
227 UINT32 src_samples, UINT32 dst_samples,
228 UINT32 *p_ret)
229 {
230 INT16 *p_src_tmp = (INT16 *)p_src;
231 INT16 *p_dst_tmp = (INT16 *)p_dst;
232 INT16 *p_worker = &bta_av_sbc_ups_cb.worker1;
233 UINT32 src_sps = bta_av_sbc_ups_cb.src_sps;
234 UINT32 dst_sps = bta_av_sbc_ups_cb.dst_sps;
235
236 while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples)
237 {
238 *p_dst_tmp++ = *p_worker;
239 *p_dst_tmp++ = *p_worker;
240
241 bta_av_sbc_ups_cb.cur_pos -= src_sps;
242 dst_samples--;
243 dst_samples--;
244 }
245
246
247 bta_av_sbc_ups_cb.cur_pos = dst_sps;
248
249 while (src_samples-- && dst_samples)
250 {
251 *p_worker = *p_src_tmp++;
252
253 do
254 {
255 *p_dst_tmp++ = *p_worker;
256 *p_dst_tmp++ = *p_worker;
257
258 bta_av_sbc_ups_cb.cur_pos -= src_sps;
259 dst_samples--;
260 dst_samples--;
261
262 } while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples);
263
264 bta_av_sbc_ups_cb.cur_pos += dst_sps;
265 }
266
267 if (bta_av_sbc_ups_cb.cur_pos == (INT32)dst_sps)
268 bta_av_sbc_ups_cb.cur_pos = 0;
269
270 *p_ret = ((char *)p_src_tmp - (char *)p_src);
271 return ((char *)p_dst_tmp - (char *)p_dst);
272 }
273
274 /*******************************************************************************
275 **
276 ** Function bta_av_sbc_up_sample_8s (8bits-stereo)
277 **
278 ** Description Given the source (p_src) audio data and
279 ** source speed (src_sps, samples per second),
280 ** This function converts it to audio data in the desired format
281 **
282 ** p_src: the data buffer that holds the source audio data
283 ** p_dst: the data buffer to hold the converted audio data
284 ** src_samples: The number of source samples (in uint of 2 bytes)
285 ** dst_samples: The size of p_dst (in uint of 2 bytes)
286 **
287 ** Returns The number of bytes used in p_dst
288 ** The number of bytes used in p_src (in *p_ret)
289 **
290 *******************************************************************************/
bta_av_sbc_up_sample_8s(void * p_src,void * p_dst,UINT32 src_samples,UINT32 dst_samples,UINT32 * p_ret)291 int bta_av_sbc_up_sample_8s (void *p_src, void *p_dst,
292 UINT32 src_samples, UINT32 dst_samples,
293 UINT32 *p_ret)
294 {
295 UINT8 *p_src_tmp = (UINT8 *)p_src;
296 INT16 *p_dst_tmp = (INT16 *)p_dst;
297 INT16 *p_worker1 = &bta_av_sbc_ups_cb.worker1;
298 INT16 *p_worker2 = &bta_av_sbc_ups_cb.worker2;
299 UINT32 src_sps = bta_av_sbc_ups_cb.src_sps;
300 UINT32 dst_sps = bta_av_sbc_ups_cb.dst_sps;
301
302 while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples)
303 {
304 *p_dst_tmp++ = *p_worker1;
305 *p_dst_tmp++ = *p_worker2;
306
307 bta_av_sbc_ups_cb.cur_pos -= src_sps;
308 dst_samples--;
309 dst_samples--;
310 }
311
312 bta_av_sbc_ups_cb.cur_pos = dst_sps;
313
314 while (src_samples -- && dst_samples)
315 {
316 *p_worker1 = *(UINT8 *)p_src_tmp++;
317 *p_worker1 -= 0x80;
318 *p_worker1 <<= 8;
319 *p_worker2 = *(UINT8 *)p_src_tmp++;
320 *p_worker2 -= 0x80;
321 *p_worker2 <<= 8;
322
323 do
324 {
325 *p_dst_tmp++ = *p_worker1;
326 *p_dst_tmp++ = *p_worker2;
327
328 bta_av_sbc_ups_cb.cur_pos -= src_sps;
329 dst_samples--;
330 dst_samples--;
331 } while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples);
332
333 bta_av_sbc_ups_cb.cur_pos += dst_sps;
334 }
335
336 if (bta_av_sbc_ups_cb.cur_pos == (INT32)dst_sps)
337 bta_av_sbc_ups_cb.cur_pos = 0;
338
339 *p_ret = ((char *)p_src_tmp - (char *)p_src);
340 return ((char *)p_dst_tmp - (char *)p_dst);
341 }
342
343 /*******************************************************************************
344 **
345 ** Function bta_av_sbc_up_sample_8m (8bits-mono)
346 **
347 ** Description Given the source (p_src) audio data and
348 ** source speed (src_sps, samples per second),
349 ** This function converts it to audio data in the desired format
350 **
351 ** p_src: the data buffer that holds the source audio data
352 ** p_dst: the data buffer to hold the converted audio data
353 ** src_samples: The number of source samples (number of bytes)
354 ** dst_samples: The size of p_dst (number of bytes)
355 **
356 ** Returns The number of bytes used in p_dst
357 ** The number of bytes used in p_src (in *p_ret)
358 **
359 *******************************************************************************/
bta_av_sbc_up_sample_8m(void * p_src,void * p_dst,UINT32 src_samples,UINT32 dst_samples,UINT32 * p_ret)360 int bta_av_sbc_up_sample_8m (void *p_src, void *p_dst,
361 UINT32 src_samples, UINT32 dst_samples,
362 UINT32 *p_ret)
363 {
364 UINT8 *p_src_tmp = (UINT8 *)p_src;
365 INT16 *p_dst_tmp = (INT16 *)p_dst;
366 INT16 *p_worker = &bta_av_sbc_ups_cb.worker1;
367 UINT32 src_sps = bta_av_sbc_ups_cb.src_sps;
368 UINT32 dst_sps = bta_av_sbc_ups_cb.dst_sps;
369
370 while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples)
371 {
372 *p_dst_tmp++ = *p_worker;
373 *p_dst_tmp++ = *p_worker;
374
375 bta_av_sbc_ups_cb.cur_pos -= src_sps;
376 dst_samples -= 4;
377 }
378
379
380 bta_av_sbc_ups_cb.cur_pos = dst_sps;
381
382 while (src_samples-- && dst_samples)
383 {
384 *p_worker = *(UINT8 *)p_src_tmp++;
385 *p_worker -= 0x80;
386 *p_worker <<= 8;
387
388 do
389 {
390 *p_dst_tmp++ = *p_worker;
391 *p_dst_tmp++ = *p_worker;
392
393 bta_av_sbc_ups_cb.cur_pos -= src_sps;
394 dst_samples -= 4;
395
396 } while (bta_av_sbc_ups_cb.cur_pos > 0 && dst_samples);
397
398 bta_av_sbc_ups_cb.cur_pos += dst_sps;
399 }
400
401 if (bta_av_sbc_ups_cb.cur_pos == (INT32)dst_sps)
402 bta_av_sbc_ups_cb.cur_pos = 0;
403
404 *p_ret = ((char *)p_src_tmp - (char *)p_src);
405 return ((char *)p_dst_tmp - (char *)p_dst);
406 }
407
408 /*******************************************************************************
409 **
410 ** Function bta_av_sbc_cfg_for_cap
411 **
412 ** Description Determine the preferred SBC codec configuration for the
413 ** given codec capabilities. The function is passed the
414 ** preferred codec configuration and the peer codec
415 ** capabilities for the stream. The function attempts to
416 ** match the preferred capabilities with the configuration
417 ** as best it can. The resulting codec configuration is
418 ** returned in the same memory used for the capabilities.
419 **
420 ** Returns 0 if ok, nonzero if error.
421 ** Codec configuration in p_cap.
422 **
423 *******************************************************************************/
bta_av_sbc_cfg_for_cap(UINT8 * p_peer,tA2D_SBC_CIE * p_cap,tA2D_SBC_CIE * p_pref)424 UINT8 bta_av_sbc_cfg_for_cap(UINT8 *p_peer, tA2D_SBC_CIE *p_cap, tA2D_SBC_CIE *p_pref)
425 {
426 UINT8 status = A2D_SUCCESS;
427 tA2D_SBC_CIE peer_cie;
428
429 /* parse peer capabilities */
430 if ((status = A2D_ParsSbcInfo(&peer_cie, p_peer, TRUE)) != 0)
431 {
432 return status;
433 }
434
435 /* Check if the peer supports our channel mode */
436 if (peer_cie.ch_mode & p_pref->ch_mode)
437 {
438 peer_cie.ch_mode = p_pref->ch_mode;
439 }
440 else
441 {
442 APPL_TRACE_ERROR1("bta_av_sbc_cfg_for_cap: ch_mode(0x%02X) not supported", p_pref->ch_mode);
443 return A2D_FAIL;
444 }
445
446 /* Check if the peer supports our sampling freq */
447 if (peer_cie.samp_freq & p_pref->samp_freq)
448 {
449 peer_cie.samp_freq = p_pref->samp_freq;
450 }
451 else
452 {
453 APPL_TRACE_ERROR1("bta_av_sbc_cfg_for_cap: samp_freq(0x%02X) not supported", p_pref->samp_freq);
454 return A2D_FAIL;
455 }
456
457 /* Check if the peer supports our block len */
458 if (peer_cie.block_len & p_pref->block_len)
459 {
460 peer_cie.block_len = p_pref->block_len;
461 }
462 else
463 {
464 APPL_TRACE_ERROR1("bta_av_sbc_cfg_for_cap: block_len(0x%02X) not supported", p_pref->block_len);
465 return A2D_FAIL;
466 }
467
468 /* Check if the peer supports our num subbands */
469 if (peer_cie.num_subbands & p_pref->num_subbands)
470 {
471 peer_cie.num_subbands = p_pref->num_subbands;
472 }
473 else
474 {
475 APPL_TRACE_ERROR1("bta_av_sbc_cfg_for_cap: num_subbands(0x%02X) not supported", p_pref->num_subbands);
476 return A2D_FAIL;
477 }
478
479 /* Check if the peer supports our alloc method */
480 if (peer_cie.alloc_mthd & p_pref->alloc_mthd)
481 {
482 peer_cie.alloc_mthd = p_pref->alloc_mthd;
483 }
484 else
485 {
486 APPL_TRACE_ERROR1("bta_av_sbc_cfg_for_cap: alloc_mthd(0x%02X) not supported", p_pref->alloc_mthd);
487 return A2D_FAIL;
488 }
489
490 /* max bitpool */
491 if (p_pref->max_bitpool != 0 && p_pref->max_bitpool < peer_cie.max_bitpool)
492 {
493 peer_cie.max_bitpool = p_pref->max_bitpool;
494 }
495
496 /* min bitpool */
497 if (p_pref->min_bitpool != 0 && p_pref->min_bitpool > peer_cie.min_bitpool)
498 {
499 peer_cie.min_bitpool = p_pref->min_bitpool;
500 }
501
502 if (status == A2D_SUCCESS)
503 {
504 /* build configuration */
505 A2D_BldSbcInfo(A2D_MEDIA_TYPE_AUDIO, &peer_cie, p_peer);
506 }
507 return status;
508 }
509
510 /*******************************************************************************
511 **
512 ** Function bta_av_sbc_cfg_in_cap
513 **
514 ** Description This function checks whether an SBC codec configuration
515 ** is allowable for the given codec capabilities.
516 **
517 ** Returns 0 if ok, nonzero if error.
518 **
519 *******************************************************************************/
bta_av_sbc_cfg_in_cap(UINT8 * p_cfg,tA2D_SBC_CIE * p_cap)520 UINT8 bta_av_sbc_cfg_in_cap(UINT8 *p_cfg, tA2D_SBC_CIE *p_cap)
521 {
522 UINT8 status = 0;
523 tA2D_SBC_CIE cfg_cie;
524
525 /* parse configuration */
526 if ((status = A2D_ParsSbcInfo(&cfg_cie, p_cfg, FALSE)) != 0)
527 {
528 return status;
529 }
530
531 /* verify that each parameter is in range */
532
533 /* sampling frequency */
534 if ((cfg_cie.samp_freq & p_cap->samp_freq) == 0)
535 {
536 status = A2D_NS_SAMP_FREQ;
537 }
538 /* channel mode */
539 else if ((cfg_cie.ch_mode & p_cap->ch_mode) == 0)
540 {
541 status = A2D_NS_CH_MODE;
542 }
543 /* block length */
544 else if ((cfg_cie.block_len & p_cap->block_len) == 0)
545 {
546 status = A2D_BAD_BLOCK_LEN;
547 }
548 /* subbands */
549 else if ((cfg_cie.num_subbands & p_cap->num_subbands) == 0)
550 {
551 status = A2D_NS_SUBBANDS;
552 }
553 /* allocation method */
554 else if ((cfg_cie.alloc_mthd & p_cap->alloc_mthd) == 0)
555 {
556 status = A2D_NS_ALLOC_MTHD;
557 }
558 /* max bitpool */
559 else if (cfg_cie.max_bitpool > p_cap->max_bitpool)
560 {
561 status = A2D_NS_MAX_BITPOOL;
562 }
563 /* min bitpool */
564 else if (cfg_cie.min_bitpool < p_cap->min_bitpool)
565 {
566 status = A2D_NS_MIN_BITPOOL;
567 }
568
569 return status;
570 }
571
572 /*******************************************************************************
573 **
574 ** Function bta_av_sbc_bld_hdr
575 **
576 ** Description This function builds the packet header for MPF1.
577 **
578 ** Returns void
579 **
580 *******************************************************************************/
bta_av_sbc_bld_hdr(BT_HDR * p_buf,UINT16 fr_per_pkt)581 void bta_av_sbc_bld_hdr(BT_HDR *p_buf, UINT16 fr_per_pkt)
582 {
583 UINT8 *p;
584
585 p_buf->offset -= BTA_AV_SBC_HDR_SIZE;
586 p = (UINT8 *) (p_buf + 1) + p_buf->offset;
587 p_buf->len += BTA_AV_SBC_HDR_SIZE;
588 A2D_BldSbcMplHdr(p, FALSE, FALSE, FALSE, (UINT8) fr_per_pkt);
589 }
590
591