avdtp.c revision 1.4 
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
42static uint8_t transLabel = 1;
43int
44avdtpSendCommand(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
76int
77avdtpCheckResponse(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
123int
124avdtpSendCapabilitiesResponseSBC(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
162int
163avdtpSendAccept(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
175int
176avdtpSendReject(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
189int
190avdtpSendDiscResponseAudio(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
205int
206avdtpDiscover(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
232void
233avdtpGetCapabilities(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
240int
241avdtpSetConfiguration(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
267void
268avdtpOpen(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
275void
276avdtpStart(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
283void
284avdtpClose(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
291void
292avdtpSuspend(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
299void
300avdtpAbort(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
307int
308avdtpAutoConfigSBC(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
377auto_config_failed:
378	return EINVAL;
379}
380