1 /* $NetBSD: avdtp.c,v 1.4 2020/05/14 08:34:19 msaitoh Exp $ */ 2 3 /*- 4 * Copyright (c) 2015 - 2016 Nathanial Sloss <nathanialsloss (at) yahoo.com.au> 5 * All rights reserved. 6 * 7 * This software is dedicated to the memory of - 8 * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012. 9 * 10 * Barry was a man who loved his music. 11 * 12 * Redistribution and use in source and binary forms, with or without 13 * modification, are permitted provided that the following conditions 14 * are met: 15 * 1. Redistributions of source code must retain the above copyright 16 * notice, this list of conditions and the following disclaimer. 17 * 2. Redistributions in binary form must reproduce the above copyright 18 * notice, this list of conditions and the following disclaimer in the 19 * documentation and/or other materials provided with the distribution. 20 * 21 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS 22 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 23 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS 25 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 * POSSIBILITY OF SUCH DAMAGE. 32 */ 33 34 #include <errno.h> 35 #include <malloc.h> 36 #include <string.h> 37 #include <unistd.h> 38 39 #include "avdtp_signal.h" 40 #include "sbc_encode.h" 41 42 static uint8_t transLabel = 1; 43 int 44 avdtpSendCommand(int fd, uint8_t command, uint8_t type, uint8_t *data, 45 size_t datasize) 46 { 47 #define SINGLE_PACKET 0 48 #define START_PACKET 1 49 #define CONTINUE_PACKET 2 50 #define END_PACKET 3 51 #define signalID 3 52 53 uint8_t header[64]; 54 size_t extra_size = 0; 55 const uint8_t packetType = (SINGLE_PACKET & 3) << 2; 56 const uint8_t messageType = (type & 3); 57 58 transLabel &= 0xf; 59 60 header[0] = (uint8_t)((transLabel << 4) | packetType | messageType); 61 if (command != 0) 62 header[1] = command & 0x3f; 63 else 64 header[1] = signalID & 0x3f; /* Bits 7/6 Reserved */ 65 66 transLabel++; 67 if (data != NULL) { 68 extra_size = datasize; 69 memcpy(header + 2, data, extra_size); 70 } 71 write(fd, &header, extra_size + 2); 72 73 return transLabel - 1; 74 } 75 76 int 77 avdtpCheckResponse(int recvfd, bool *isCommand, uint8_t *trans, uint8_t 78 *signalId, uint8_t *pkt_type, uint8_t *data, size_t *datasize, uint8_t *sep) 79 { 80 uint8_t buffer[1024]; 81 size_t len; 82 83 *isCommand = false; 84 len = (size_t)read(recvfd, buffer, sizeof(buffer)); 85 86 if (datasize) 87 *datasize = 0; 88 89 if (len < AVDTP_LEN_SUCCESS) 90 return ENOMEM; 91 92 *trans = (uint8_t)((buffer[0] & TRANSACTIONLABEL) >> TRANSACTIONLABEL_S); 93 *signalId = buffer[1] & SIGNALID_MASK; 94 if ((buffer[0] & MESSAGETYPE) == COMMAND) { 95 if (datasize) 96 *datasize = 0; 97 if (sep && len > 2) 98 *sep = buffer[2] >> 2; 99 *isCommand = true; 100 } 101 102 if (len == AVDTP_LEN_ERROR) 103 return buffer[2]; 104 else if ((len % AVDTP_LEN_SUCCESS) == 0 && 105 buffer[0] & RESPONSEACCEPT) { 106 if (len == AVDTP_LEN_SUCCESS) 107 return 0; 108 } 109 if (datasize && data && len > AVDTP_LEN_SUCCESS && 110 buffer[0] & RESPONSEACCEPT) { 111 memcpy(data, buffer + 2, len - 2); 112 *datasize = len - 2; 113 114 return 0; 115 } 116 117 if (isCommand) 118 return 0; 119 120 return EINVAL; 121 } 122 123 int 124 avdtpSendCapabilitiesResponseSBC(int fd, int recvfd, int trans, uint8_t mySep, 125 uint8_t bitpool, uint8_t freq, uint8_t mode, uint8_t bands, uint8_t blocks, 126 uint8_t alloc_method) 127 { 128 uint8_t data[12], freqmode, blk_len_sb_alloc, freq_dat, mode_dat; 129 uint8_t bands_dat, blocks_dat, alloc_dat; 130 131 132 freq_dat = (uint8_t)(freq << 4); 133 mode_dat = mode; 134 freqmode = freq_dat | mode_dat; 135 136 blocks_dat = (uint8_t)(blocks << 4); 137 bands_dat = (uint8_t)(bands << 2); 138 alloc_dat = alloc_method; 139 blk_len_sb_alloc = blocks_dat| bands_dat | alloc_dat; 140 141 data[0] = (uint8_t)(trans << 4 | RESPONSEACCEPT); 142 data[1] = AVDTP_GET_CAPABILITIES; 143 data[2] = mediaTransport; 144 data[3] = 0; 145 data[4] = mediaCodec; 146 data[5] = 0x6; 147 data[6] = mediaTypeAudio; 148 data[7] = SBC_CODEC_ID; 149 data[8] = freqmode; 150 data[9] = blk_len_sb_alloc; 151 data[10] = MIN_BITPOOL; 152 if (bitpool > MIN_BITPOOL) 153 data[11] = bitpool; 154 else 155 data[11] = DEFAULT_MAXBPOOL; 156 157 write(fd, data, sizeof(data)); 158 159 return 0; 160 } 161 162 int 163 avdtpSendAccept(int fd, int recvfd, uint8_t trans, uint8_t myCommand) 164 { 165 uint8_t data[2]; 166 167 data[0] = (uint8_t)(trans << 4 | RESPONSEACCEPT); 168 data[1] = myCommand; 169 170 write(fd, data, sizeof(data)); 171 172 return 0; 173 } 174 175 int 176 avdtpSendReject(int fd, int recvfd, uint8_t trans, uint8_t myCommand) 177 { 178 uint8_t data[4]; 179 180 data[0] = (uint8_t)(trans << 4 | RESPONSEREJECT); 181 data[1] = myCommand; 182 data[2] = 0; 183 184 write(fd, data, sizeof(data)); 185 186 return 0; 187 } 188 189 int 190 avdtpSendDiscResponseAudio(int fd, int recvfd, uint8_t trans, uint8_t mySep, 191 bool sink) 192 { 193 uint8_t data[4]; 194 195 data[0] = (uint8_t)(trans << 4 | RESPONSEACCEPT); 196 data[1] = AVDTP_DISCOVER; 197 data[2] = (uint8_t)(mySep << 2); 198 data[3] = (uint8_t)((sink ? 1 : 0) << 3); 199 200 write(fd, data, sizeof(data)); 201 202 return 0; 203 } 204 205 int 206 avdtpDiscover(uint8_t *buffer, size_t recvsize, struct avdtp_sepInfo *sepInfo, 207 bool sink) 208 { 209 size_t offset; 210 bool isSink; 211 212 if (recvsize >= 2) { 213 for (offset = 0; offset < recvsize - 1; offset += 2) { 214 sepInfo->sep = buffer[offset] >> 2; 215 sepInfo->media_Type = buffer[offset+1] >> 4; 216 isSink = (buffer[offset+1] >> 3) & 1; 217 if (buffer[offset] & DISCOVER_SEP_IN_USE || 218 isSink != sink) 219 continue; 220 else 221 break; 222 } 223 if (offset > recvsize) 224 return EINVAL; 225 226 return 0; 227 } 228 229 return EINVAL; 230 } 231 232 void 233 avdtpGetCapabilities(int fd, int recvfd, uint8_t sep) 234 { 235 uint8_t address = (uint8_t)(sep << 2); 236 237 avdtpSendCommand(fd, AVDTP_GET_CAPABILITIES, 0, &address, 1); 238 } 239 240 int 241 avdtpSetConfiguration(int fd, int recvfd, uint8_t sep, uint8_t *data, 242 size_t datasize, int srcsep) 243 { 244 uint8_t configAddresses[2]; 245 uint8_t *configData; 246 247 if (data == NULL || datasize == 0) 248 return EINVAL; 249 250 configData = malloc(datasize + 2); 251 if (configData == NULL) 252 return ENOMEM; 253 configAddresses[0] = (uint8_t)(sep << 2); 254 configAddresses[1] = (uint8_t)(srcsep << 2); 255 256 memcpy(configData, configAddresses, 2); 257 memcpy(configData + 2, data, datasize); 258 259 avdtpSendCommand(fd, AVDTP_SET_CONFIGURATION, 0, 260 configData, datasize + 2); 261 free(configData); 262 263 return 0; 264 265 } 266 267 void 268 avdtpOpen(int fd, int recvfd, uint8_t sep) 269 { 270 uint8_t address = (uint8_t)(sep << 2); 271 272 avdtpSendCommand(fd, AVDTP_OPEN, 0, &address, 1); 273 } 274 275 void 276 avdtpStart(int fd, int recvfd, uint8_t sep) 277 { 278 uint8_t address = (uint8_t)(sep << 2); 279 280 avdtpSendCommand(fd, AVDTP_START, 0, &address, 1); 281 } 282 283 void 284 avdtpClose(int fd, int recvfd, uint8_t sep) 285 { 286 uint8_t address = (uint8_t)(sep << 2); 287 288 avdtpSendCommand(fd, AVDTP_CLOSE, 0, &address, 1); 289 } 290 291 void 292 avdtpSuspend(int fd, int recvfd, uint8_t sep) 293 { 294 uint8_t address = (uint8_t)(sep << 2); 295 296 avdtpSendCommand(fd, AVDTP_SUSPEND, 0, &address, 1); 297 } 298 299 void 300 avdtpAbort(int fd, int recvfd, uint8_t sep) 301 { 302 uint8_t address = (uint8_t)(sep << 2); 303 304 avdtpSendCommand(fd, AVDTP_ABORT, 0, &address, 1); 305 } 306 307 int 308 avdtpAutoConfigSBC(int fd, int recvfd, uint8_t *capabilities, size_t cap_len, 309 uint8_t sep, uint8_t *freq, uint8_t *mode, uint8_t *alloc_method, uint8_t 310 *bitpool, uint8_t* bands, uint8_t *blocks, uint8_t srcsep) 311 { 312 uint8_t freqmode, blk_len_sb_alloc, availFreqMode, availConfig; 313 uint8_t supBitpoolMin, supBitpoolMax, tmp_mask; 314 size_t i; 315 316 for (i = 0; i < cap_len - 5; i++) { 317 if (capabilities[i] == mediaTransport && 318 capabilities[i + 1] == 0 && 319 capabilities[i + 2] == mediaCodec && 320 capabilities[i + 4] == mediaTypeAudio && 321 capabilities[i + 5] == SBC_CODEC_ID) 322 break; 323 } 324 if (i >= cap_len - 9) 325 goto auto_config_failed; 326 327 availFreqMode = capabilities[i + 6]; 328 availConfig = capabilities[i + 7]; 329 supBitpoolMin = capabilities[i + 8]; 330 supBitpoolMax = capabilities[i + 9]; 331 332 freqmode = (uint8_t)(*freq << 4 | *mode); 333 tmp_mask = availFreqMode & freqmode; 334 *mode = (uint8_t)(1 << FLS(tmp_mask & 0xf)); 335 *freq = (uint8_t)(1 << FLS(tmp_mask >> 4)); 336 337 freqmode = (uint8_t)(*freq << 4 | *mode); 338 if ((availFreqMode & freqmode) != freqmode) 339 goto auto_config_failed; 340 341 blk_len_sb_alloc = (uint8_t)(*blocks << 4 | *bands << 2 | 342 *alloc_method); 343 344 tmp_mask = availConfig & blk_len_sb_alloc; 345 *blocks = (uint8_t)(1 << FLS(tmp_mask >> 4)); 346 *bands = (uint8_t)(1 << FLS((tmp_mask >> 2) & 3)); 347 *alloc_method = (uint8_t)(1 << FLS(tmp_mask & 3)); 348 349 blk_len_sb_alloc = (uint8_t)(*blocks << 4 | *bands << 2 | 350 *alloc_method); 351 352 if ((availConfig & blk_len_sb_alloc) != blk_len_sb_alloc) 353 goto auto_config_failed; 354 355 if (*alloc_method == ALLOC_SNR) 356 supBitpoolMax &= (uint8_t)~1; 357 358 if (*mode == MODE_DUAL || *mode == MODE_MONO) 359 supBitpoolMax /= 2; 360 361 if (*bands == BANDS_4) 362 supBitpoolMax /= 2; 363 364 if (supBitpoolMax > *bitpool) 365 supBitpoolMax = *bitpool; 366 else 367 *bitpool = supBitpoolMax; 368 369 uint8_t config[] = {mediaTransport, 0x0, mediaCodec, 0x6, 370 mediaTypeAudio, SBC_CODEC_ID, freqmode, blk_len_sb_alloc, 371 supBitpoolMin, supBitpoolMax}; 372 373 if (avdtpSetConfiguration(fd, fd, sep, config, sizeof(config), 374 srcsep) == 0) 375 return 0; 376 377 auto_config_failed: 378 return EINVAL; 379 } 380
Indexes created Sat Sep 19 00:25:42 UTC 2026