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@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
avdtpSendCommand(int fd,uint8_t command,uint8_t type,uint8_t * data,size_t datasize)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
avdtpCheckResponse(int recvfd,bool * isCommand,uint8_t * trans,uint8_t * signalId,uint8_t * pkt_type,uint8_t * data,size_t * datasize,uint8_t * sep)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
avdtpSendCapabilitiesResponseSBC(int fd,int recvfd,int trans,uint8_t mySep,uint8_t bitpool,uint8_t freq,uint8_t mode,uint8_t bands,uint8_t blocks,uint8_t alloc_method)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
avdtpSendAccept(int fd,int recvfd,uint8_t trans,uint8_t myCommand)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
avdtpSendReject(int fd,int recvfd,uint8_t trans,uint8_t myCommand)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
avdtpSendDiscResponseAudio(int fd,int recvfd,uint8_t trans,uint8_t mySep,bool sink)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
avdtpDiscover(uint8_t * buffer,size_t recvsize,struct avdtp_sepInfo * sepInfo,bool sink)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
avdtpGetCapabilities(int fd,int recvfd,uint8_t sep)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
avdtpSetConfiguration(int fd,int recvfd,uint8_t sep,uint8_t * data,size_t datasize,int srcsep)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
avdtpOpen(int fd,int recvfd,uint8_t sep)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
avdtpStart(int fd,int recvfd,uint8_t sep)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
avdtpClose(int fd,int recvfd,uint8_t sep)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
avdtpSuspend(int fd,int recvfd,uint8_t sep)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
avdtpAbort(int fd,int recvfd,uint8_t sep)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
avdtpAutoConfigSBC(int fd,int recvfd,uint8_t * capabilities,size_t cap_len,uint8_t sep,uint8_t * freq,uint8_t * mode,uint8_t * alloc_method,uint8_t * bitpool,uint8_t * bands,uint8_t * blocks,uint8_t srcsep)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