Module Name: src
Committed By: nat
Date: Sat Jan 28 16:55:54 UTC 2017
Modified Files:
src/distrib/sets/lists/base: mi
src/distrib/sets/lists/debug: mi
src/distrib/sets/lists/man: mi
src/usr.sbin: Makefile
Added Files:
src/usr.sbin/bta2dpd: Makefile
src/usr.sbin/bta2dpd/bta2dpd: Makefile avdtp.c avdtp_signal.h bta2dpd.8
bta2dpd.c sbc_encode.c sbc_encode.h
src/usr.sbin/bta2dpd/cosdata-gen: Makefile cosdata.c
src/usr.sbin/bta2dpd/sbc_crc-gen: Makefile sbc_crc.c
Log Message:
bta2dpd - Bluetooth Advand Audio Distribution (A2DP) Profile Daemon.
bta2dpd allows you to stream audio to and from bluetooth devices.
It features high quality stereo audio and is intended for use with pad(4)
device.
As discussed on current-users@ and tech-userlevel@
To generate a diff of this commit:
cvs rdiff -u -r1.1150 -r1.1151 src/distrib/sets/lists/base/mi
cvs rdiff -u -r1.193 -r1.194 src/distrib/sets/lists/debug/mi
cvs rdiff -u -r1.1546 -r1.1547 src/distrib/sets/lists/man/mi
cvs rdiff -u -r1.273 -r1.274 src/usr.sbin/Makefile
cvs rdiff -u -r0 -r1.1 src/usr.sbin/bta2dpd/Makefile
cvs rdiff -u -r0 -r1.1 src/usr.sbin/bta2dpd/bta2dpd/Makefile \
src/usr.sbin/bta2dpd/bta2dpd/avdtp.c \
src/usr.sbin/bta2dpd/bta2dpd/avdtp_signal.h \
src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.8 \
src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.c \
src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.c \
src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.h
cvs rdiff -u -r0 -r1.1 src/usr.sbin/bta2dpd/cosdata-gen/Makefile \
src/usr.sbin/bta2dpd/cosdata-gen/cosdata.c
cvs rdiff -u -r0 -r1.1 src/usr.sbin/bta2dpd/sbc_crc-gen/Makefile \
src/usr.sbin/bta2dpd/sbc_crc-gen/sbc_crc.c
Please note that diffs are not public domain; they are subject to the
copyright notices on the relevant files.
Modified files:
Index: src/distrib/sets/lists/base/mi
diff -u src/distrib/sets/lists/base/mi:1.1150 src/distrib/sets/lists/base/mi:1.1151
--- src/distrib/sets/lists/base/mi:1.1150 Fri Jan 13 11:21:47 2017
+++ src/distrib/sets/lists/base/mi Sat Jan 28 16:55:54 2017
@@ -1,4 +1,4 @@
-# $NetBSD: mi,v 1.1150 2017/01/13 11:21:47 nonaka Exp $
+# $NetBSD: mi,v 1.1151 2017/01/28 16:55:54 nat Exp $
#
# Note: Don't delete entries from here - mark them as "obsolete" instead,
# unless otherwise stated below.
@@ -1591,6 +1591,7 @@
./usr/sbin/bootpgw base-bootserver-bin
./usr/sbin/bootptest base-bootserver-bin
./usr/sbin/bpm base-obsolete obsolete
+./usr/sbin/bta2dpd base-sysutil-bin
./usr/sbin/btattach base-sysutil-bin
./usr/sbin/btconfig base-sysutil-bin
./usr/sbin/btcontrol base-obsolete obsolete
Index: src/distrib/sets/lists/debug/mi
diff -u src/distrib/sets/lists/debug/mi:1.193 src/distrib/sets/lists/debug/mi:1.194
--- src/distrib/sets/lists/debug/mi:1.193 Fri Jan 27 10:45:11 2017
+++ src/distrib/sets/lists/debug/mi Sat Jan 28 16:55:54 2017
@@ -1,4 +1,4 @@
-# $NetBSD: mi,v 1.193 2017/01/27 10:45:11 hannken Exp $
+# $NetBSD: mi,v 1.194 2017/01/28 16:55:54 nat Exp $
./etc/mtree/set.debug comp-sys-root
./usr/lib comp-sys-usr compatdir
./usr/lib/i18n/libBIG5_g.a comp-c-debuglib debuglib,compatfile
@@ -1009,6 +1009,7 @@
./usr/libdata/debug/usr/sbin/bootpef.debug comp-bootserver-debug debug
./usr/libdata/debug/usr/sbin/bootpgw.debug comp-bootserver-debug debug
./usr/libdata/debug/usr/sbin/bootptest.debug comp-bootserver-debug debug
+./usr/libdata/debug/usr/sbin/bta2dpd.debug comp-sysutil-debug debug
./usr/libdata/debug/usr/sbin/btattach.debug comp-sysutil-debug debug
./usr/libdata/debug/usr/sbin/btconfig.debug comp-sysutil-debug debug
./usr/libdata/debug/usr/sbin/btcontrol.debug comp-obsolete obsolete
Index: src/distrib/sets/lists/man/mi
diff -u src/distrib/sets/lists/man/mi:1.1546 src/distrib/sets/lists/man/mi:1.1547
--- src/distrib/sets/lists/man/mi:1.1546 Sat Jan 21 20:21:57 2017
+++ src/distrib/sets/lists/man/mi Sat Jan 28 16:55:54 2017
@@ -1,4 +1,4 @@
-# $NetBSD: mi,v 1.1546 2017/01/21 20:21:57 jdolecek Exp $
+# $NetBSD: mi,v 1.1547 2017/01/28 16:55:54 nat Exp $
#
# Note: don't delete entries from here - mark them as "obsolete" instead.
#
@@ -2366,6 +2366,7 @@
./usr/share/man/cat8/bounce.0 man-postfix-catman postfix,.cat
./usr/share/man/cat8/bozohttpd.0 man-netutil-catman .cat
./usr/share/man/cat8/brconfig.0 man-netutil-catman .cat
+./usr/share/man/cat8/bta2dpd.0 man-sysutil-catman .cat
./usr/share/man/cat8/btattach.0 man-sysutil-catman .cat
./usr/share/man/cat8/btconfig.0 man-sysutil-catman .cat
./usr/share/man/cat8/btcontrol.0 man-obsolete obsolete
@@ -5339,6 +5340,7 @@
./usr/share/man/html8/bounce.html man-postfix-htmlman postfix,html
./usr/share/man/html8/bozohttpd.html man-netutil-htmlman html
./usr/share/man/html8/brconfig.html man-netutil-htmlman html
+./usr/share/man/html8/bta2dpd.html man-sysutil-htmlman html
./usr/share/man/html8/btattach.html man-sysutil-htmlman html
./usr/share/man/html8/btconfig.html man-sysutil-htmlman html
./usr/share/man/html8/btdevctl.html man-sysutil-htmlman html
@@ -8347,6 +8349,7 @@
./usr/share/man/man8/bounce.8 man-postfix-man postfix,.man
./usr/share/man/man8/bozohttpd.8 man-netutil-man .man
./usr/share/man/man8/brconfig.8 man-netutil-man .man
+./usr/share/man/man8/bta2dpd.8 man-sysutil-man .man
./usr/share/man/man8/btattach.8 man-sysutil-man .man
./usr/share/man/man8/btconfig.8 man-sysutil-man .man
./usr/share/man/man8/btcontrol.8 man-obsolete obsolete
Index: src/usr.sbin/Makefile
diff -u src/usr.sbin/Makefile:1.273 src/usr.sbin/Makefile:1.274
--- src/usr.sbin/Makefile:1.273 Sun Dec 13 02:26:08 2015
+++ src/usr.sbin/Makefile Sat Jan 28 16:55:54 2017
@@ -1,10 +1,10 @@
-# $NetBSD: Makefile,v 1.273 2015/12/13 02:26:08 christos Exp $
+# $NetBSD: Makefile,v 1.274 2017/01/28 16:55:54 nat Exp $
# from: @(#)Makefile 5.20 (Berkeley) 6/12/93
.include <bsd.own.mk>
SUBDIR= ac accton acpitools altq apm apmd arp \
- bad144 bootp btattach btconfig btdevctl bthcid btpand catman \
+ bad144 bootp bta2dpd btattach btconfig btdevctl bthcid btpand catman \
chroot cnwctl cpuctl crash \
dev_mkdb diskpart dumpfs dumplfs \
edquota eeprom envstat eshconfig etcupdate extattrctl \
Added files:
Index: src/usr.sbin/bta2dpd/Makefile
diff -u /dev/null src/usr.sbin/bta2dpd/Makefile:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/Makefile Sat Jan 28 16:55:54 2017
@@ -0,0 +1,5 @@
+# $NetBSD: Makefile,v 1.1 2017/01/28 16:55:54 nat Exp $
+
+SUBDIR= cosdata-gen sbc_crc-gen .WAIT bta2dpd
+
+.include <bsd.subdir.mk>
Index: src/usr.sbin/bta2dpd/bta2dpd/Makefile
diff -u /dev/null src/usr.sbin/bta2dpd/bta2dpd/Makefile:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/bta2dpd/Makefile Sat Jan 28 16:55:54 2017
@@ -0,0 +1,24 @@
+# $NetBSD: Makefile,v 1.1 2017/01/28 16:55:54 nat Exp $
+
+WARNS?= 6
+
+PROG= bta2dpd
+SRCS= bta2dpd.c avdtp.c sbc_encode.c
+MAN= bta2dpd.8
+
+CFLAGS+= -I.
+DPADD+= ${LIBBLUETOOTH} ${LIBEVENT}
+LDADD+= -lbluetooth -levent
+CLEANFILES+= sbc_crc.h sbc_coeffs.h
+
+sbc_encode.c: sbc_coeffs.h sbc_crc.h
+sbc_coeffs.h:
+ ../cosdata-gen/cosdata > sbc_coeffs.h
+sbc_crc.h:
+ ../sbc_crc-gen/sbc_crc > sbc_crc.h
+
+.if exists(${.CURDIR}/../../Makefile.inc)
+.include "${.CURDIR}/../../Makefile.inc"
+.endif
+
+.include <bsd.prog.mk>
Index: src/usr.sbin/bta2dpd/bta2dpd/avdtp.c
diff -u /dev/null src/usr.sbin/bta2dpd/bta2dpd/avdtp.c:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/bta2dpd/avdtp.c Sat Jan 28 16:55:54 2017
@@ -0,0 +1,380 @@
+/* $NetBSD: avdtp.c,v 1.1 2017/01/28 16:55:54 nat Exp $ */
+
+/*-
+ * Copyright (c) 2015 - 2016 Nathanial Sloss <[email protected]>
+ * All rights reserved.
+ *
+ * This software is dedicated to the memory of -
+ * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+ *
+ * Barry was a man who loved his music.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <errno.h>
+#include <malloc.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "avdtp_signal.h"
+#include "sbc_encode.h"
+
+static uint8_t transLabel = 1;
+int
+avdtpSendCommand(int fd, uint8_t command, uint8_t type, uint8_t *data,
+ size_t datasize)
+{
+#define SINGLE_PACKET 0
+#define START_PACKET 1
+#define CONTINUE_PACKET 2
+#define END_PACKET 3
+#define signalID 3
+
+ uint8_t header[64];
+ size_t extra_size = 0;
+ const uint8_t packetType = (SINGLE_PACKET & 3) << 2;
+ const uint8_t messageType = (type & 3);
+
+ transLabel &= 0xf;
+
+ header[0] = (uint8_t)((transLabel << 4) | packetType | messageType);
+ if (command != 0)
+ header[1] = command & 0x3f;
+ else
+ header[1] = signalID & 0x3f; /* Bits 7/6 Reserved */
+
+ transLabel++;
+ if (data != NULL) {
+ extra_size = datasize;
+ memcpy(header + 2, data, extra_size);
+ }
+ write(fd, &header, extra_size + 2);
+
+ return transLabel - 1;
+}
+
+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)
+{
+ uint8_t buffer[1024];
+ size_t len;
+
+ *isCommand = false;
+ len = (size_t)read(recvfd, buffer, sizeof(buffer));
+
+ if (datasize)
+ *datasize = 0;
+
+ if (len < AVDTP_LEN_SUCCESS)
+ return ENOMEM;
+
+ *trans = (buffer[0] & TRANSACTIONLABEL) >> TRANSACTIONLABEL_S;
+ *signalId = buffer[1] & SIGNALID_MASK;
+ if ((buffer[0] & MESSAGETYPE) == COMMAND) {
+ if (datasize)
+ *datasize = 0;
+ if (sep && len > 2)
+ *sep = buffer[2] >> 2;
+ *isCommand = true;
+ }
+
+ if (len == AVDTP_LEN_ERROR)
+ return buffer[2];
+ else if ((len % AVDTP_LEN_SUCCESS) == 0 &&
+ buffer[0] & RESPONSEACCEPT) {
+ if (len == AVDTP_LEN_SUCCESS)
+ return 0;
+ }
+ if (datasize && data && len > AVDTP_LEN_SUCCESS &&
+ buffer[0] & RESPONSEACCEPT) {
+ memcpy(data, buffer + 2, len - 2);
+ *datasize = len - 2;
+
+ return 0;
+ }
+
+ if (isCommand)
+ return 0;
+
+ return EINVAL;
+}
+
+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)
+{
+ uint8_t data[12], freqmode, blk_len_sb_alloc, freq_dat, mode_dat;
+ uint8_t bands_dat, blocks_dat, alloc_dat;
+
+
+ freq_dat = (uint8_t)(freq << 4);
+ mode_dat = mode;
+ freqmode = freq_dat | mode_dat;
+
+ blocks_dat = (uint8_t)(blocks << 4);
+ bands_dat = (uint8_t)(bands << 2);
+ alloc_dat = alloc_method;
+ blk_len_sb_alloc = blocks_dat| bands_dat | alloc_dat;
+
+ data[0] = (uint8_t)(trans << 4 | RESPONSEACCEPT);
+ data[1] = AVDTP_GET_CAPABILITIES;
+ data[2] = mediaTransport;
+ data[3] = 0;
+ data[4] = mediaCodec;
+ data[5] = 0x6;
+ data[6] = mediaTypeAudio;
+ data[7] = SBC_CODEC_ID;
+ data[8] = freqmode;
+ data[9] = blk_len_sb_alloc;
+ data[10] = MIN_BITPOOL;
+ if (bitpool > MIN_BITPOOL)
+ data[11] = bitpool;
+ else
+ data[11] = DEFAULT_MAXBPOOL;
+
+ write(fd, data, sizeof(data));
+
+ return 0;
+}
+
+int
+avdtpSendAccept(int fd, int recvfd, uint8_t trans, uint8_t myCommand)
+{
+ uint8_t data[2];
+
+ data[0] = (uint8_t)(trans << 4 | RESPONSEACCEPT);
+ data[1] = myCommand;;
+
+ write(fd, data, sizeof(data));
+
+ return 0;
+}
+
+int
+avdtpSendReject(int fd, int recvfd, uint8_t trans, uint8_t myCommand)
+{
+ uint8_t data[4];
+
+ data[0] = (uint8_t)(trans << 4 | RESPONSEREJECT);
+ data[1] = myCommand;
+ data[2] = 0;
+
+ write(fd, data, sizeof(data));
+
+ return 0;
+}
+
+int
+avdtpSendDiscResponseAudio(int fd, int recvfd, uint8_t trans, uint8_t mySep,
+ bool sink)
+{
+ uint8_t data[4];
+
+ data[0] = (uint8_t)(trans << 4 | RESPONSEACCEPT);
+ data[1] = AVDTP_DISCOVER;
+ data[2] = (uint8_t)(mySep << 2);
+ data[3] = (uint8_t)((sink ? 1 : 0) << 3);
+
+ write(fd, data, sizeof(data));
+
+ return 0;
+}
+
+int
+avdtpDiscover(uint8_t *buffer, size_t recvsize, struct avdtp_sepInfo *sepInfo,
+ bool sink)
+{
+ size_t offset;
+ bool isSink;
+
+ if (recvsize >= 2) {
+ for (offset = 0;offset < recvsize;offset += 2) {
+ sepInfo->sep = buffer[offset] >> 2;
+ sepInfo->media_Type = buffer[offset+1] >> 4;
+ isSink = (buffer[offset+1] >> 3) & 1;
+ if (buffer[offset] & DISCOVER_SEP_IN_USE ||
+ isSink != sink)
+ continue;
+ else
+ break;
+ }
+ if (offset > recvsize)
+ return EINVAL;
+
+ return 0;
+ }
+
+ return EINVAL;
+}
+
+void
+avdtpGetCapabilities(int fd, int recvfd, uint8_t sep)
+{
+ uint8_t address = (uint8_t)(sep << 2);
+
+ avdtpSendCommand(fd, AVDTP_GET_CAPABILITIES, 0, &address, 1);
+}
+
+int
+avdtpSetConfiguration(int fd, int recvfd, uint8_t sep, uint8_t *data,
+ size_t datasize, int srcsep)
+{
+ uint8_t configAddresses[2];
+ uint8_t *configData;
+
+ if (data == NULL || datasize == 0)
+ return EINVAL;
+
+ configData = malloc(datasize + 2);
+ if (configData == NULL)
+ return ENOMEM;
+ configAddresses[0] = (uint8_t)(sep << 2);
+ configAddresses[1] = (uint8_t)(srcsep << 2);
+
+ memcpy(configData, configAddresses, 2);
+ memcpy(configData + 2, data, datasize);
+
+ avdtpSendCommand(fd, AVDTP_SET_CONFIGURATION, 0,
+ configData, datasize + 2);
+ free(configData);
+
+ return 0;
+
+}
+
+void
+avdtpOpen(int fd, int recvfd, uint8_t sep)
+{
+ uint8_t address = (uint8_t)(sep << 2);
+
+ avdtpSendCommand(fd, AVDTP_OPEN, 0, &address, 1);
+}
+
+void
+avdtpStart(int fd, int recvfd, uint8_t sep)
+{
+ uint8_t address = (uint8_t)(sep << 2);
+
+ avdtpSendCommand(fd, AVDTP_START, 0, &address, 1);
+}
+
+void
+avdtpClose(int fd, int recvfd, uint8_t sep)
+{
+ uint8_t address = (uint8_t)(sep << 2);
+
+ avdtpSendCommand(fd, AVDTP_CLOSE, 0, &address, 1);
+}
+
+void
+avdtpSuspend(int fd, int recvfd, uint8_t sep)
+{
+ uint8_t address = (uint8_t)(sep << 2);
+
+ avdtpSendCommand(fd, AVDTP_SUSPEND, 0, &address, 1);
+}
+
+void
+avdtpAbort(int fd, int recvfd, uint8_t sep)
+{
+ uint8_t address = (uint8_t)(sep << 2);
+
+ avdtpSendCommand(fd, AVDTP_ABORT, 0, &address, 1);
+}
+
+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)
+{
+ uint8_t freqmode, blk_len_sb_alloc, availFreqMode, availConfig;
+ uint8_t supBitpoolMin, supBitpoolMax, tmp_mask;
+ size_t i;
+
+ for (i = 0; i < cap_len; i++) {
+ if (capabilities[i] == mediaTransport &&
+ capabilities[i + 1] == 0 &&
+ capabilities[i + 2] == mediaCodec &&
+ capabilities[i + 4] == mediaTypeAudio &&
+ capabilities[i + 5] == SBC_CODEC_ID)
+ break;
+ }
+ if (i >= cap_len)
+ goto auto_config_failed;
+
+ availFreqMode = capabilities[i + 6];
+ availConfig = capabilities[i + 7];
+ supBitpoolMin = capabilities[i + 8];
+ supBitpoolMax = capabilities[i + 9];
+
+ freqmode = (uint8_t)(*freq << 4 | *mode);
+ tmp_mask = availFreqMode & freqmode;
+ *mode = (uint8_t)(1 << FLS(tmp_mask & 0xf));
+ *freq = (uint8_t)(1 << FLS(tmp_mask >> 4));
+
+ freqmode = (uint8_t)(*freq << 4 | *mode);
+ if ((availFreqMode & freqmode) != freqmode)
+ goto auto_config_failed;
+
+ blk_len_sb_alloc = (uint8_t)(*blocks << 4 | *bands << 2 |
+ *alloc_method);
+
+ tmp_mask = availConfig & blk_len_sb_alloc;
+ *blocks = (uint8_t)(1 << FLS(tmp_mask >> 4));
+ *bands = (uint8_t)(1 << FLS((tmp_mask >> 2) & 3));
+ *alloc_method = (uint8_t)(1 << FLS(tmp_mask & 3));
+
+ blk_len_sb_alloc = (uint8_t)(*blocks << 4 | *bands << 2 |
+ *alloc_method);
+
+ if ((availConfig & blk_len_sb_alloc) != blk_len_sb_alloc)
+ goto auto_config_failed;
+
+ if (*alloc_method == ALLOC_SNR)
+ supBitpoolMax &= (uint8_t)~1;
+
+ if (*mode == MODE_DUAL || *mode == MODE_MONO)
+ supBitpoolMax /= 2;
+
+ if (*bands == BANDS_4)
+ supBitpoolMax /= 2;
+
+ if (supBitpoolMax > *bitpool)
+ supBitpoolMax = *bitpool;
+ else
+ *bitpool = supBitpoolMax;
+
+ uint8_t config[] = {mediaTransport, 0x0, mediaCodec, 0x6,
+ mediaTypeAudio, SBC_CODEC_ID, freqmode, blk_len_sb_alloc,
+ supBitpoolMin, supBitpoolMax};
+
+ if (avdtpSetConfiguration(fd, fd, sep, config, sizeof(config),
+ srcsep) == 0)
+ return 0;
+
+auto_config_failed:
+ return EINVAL;
+}
+
Index: src/usr.sbin/bta2dpd/bta2dpd/avdtp_signal.h
diff -u /dev/null src/usr.sbin/bta2dpd/bta2dpd/avdtp_signal.h:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/bta2dpd/avdtp_signal.h Sat Jan 28 16:55:54 2017
@@ -0,0 +1,139 @@
+/* $NetBSD: avdtp_signal.h,v 1.1 2017/01/28 16:55:54 nat Exp $ */
+
+/*-
+ * Copyright (c) 2015 - 2016 Nathanial Sloss <[email protected]>
+ * All rights reserved.
+ *
+ * This software is dedicated to the memory of -
+ * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+ *
+ * Barry was a man who loved his music.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sys/types.h>
+#include <stdbool.h>
+/* Our endpoint. */
+#define INTSEP 8
+
+/* AVDTP signals. */
+
+#define AVDTP_DISCOVER 0x01
+#define AVDTP_GET_CAPABILITIES 0x02
+#define AVDTP_SET_CONFIGURATION 0x03
+#define AVDTP_GET_CONFIGURATION 0x04
+#define AVDTP_RECONFIGURE 0x05
+#define AVDTP_OPEN 0x06
+#define AVDTP_START 0x07
+#define AVDTP_CLOSE 0x08
+#define AVDTP_SUSPEND 0x09
+#define AVDTP_ABORT 0x0a
+#define AVDTP_SECUURITY_CONTROL 0x0b
+
+/* Signal Command & Response Header Masks. */
+
+#define TRANSACTIONLABEL 0xf0
+#define TRANSACTIONLABEL_S 4
+#define SIGNALID_MASK 0x3f
+#define PACKETTYPE 0x0c
+#define PACKETTYPE_S 0x02
+#define MESSAGETYPE 0x03
+#define SIGNALIDENTIFIER 0x3f
+#define DISCOVER_SEP_IN_USE 0x02
+
+/* Packet Types */
+#define singlePacket 0x0
+#define startPacket 0x1
+#define continuePacket 0x2
+#define endPacket 0x3
+
+/* Message Types */
+#define COMMAND 0x0
+#define RESPONSEACCEPT 0x2
+#define RESPONSEREJECT 0x3
+
+/* Response general error/success lengths */
+#define AVDTP_LEN_SUCCESS 2
+#define AVDTP_LEN_ERROR 3
+
+/* Error codes */
+#define BAD_HEADER_FORMAT 0x01
+#define BAD_LENGTH 0x11
+#define BAD_ACP_SEID 0x12
+#define SEP_IN_USE 0x13
+#define SEP_NOT_IN_USE 0x14
+#define BAD_SERV_CATAGORY 0x17
+#define BAD_PAYLOAD_FORMAT 0x18
+#define NOT_SUPPORTED_COMMAND 0x19
+#define INVALID_CAPABILITIES 0x1a
+
+#define BAD_RECOVERY_TYPE 0x22
+#define BAD_MEDIA_TRANSPORT_FORMAT 0x23
+#define BAD_RECOVERY_FORMAT 0x25
+#define BAD_ROHC_FORMAT 0x26
+#define BAD_CP_FORMAT 0x27
+#define BAD_MULTIPLEXING_FORMAT 0x28
+#define UNSUPPORTED_CONFIGURATION 0x29
+#define BAD_STATE 0x31
+
+/* Service Capabilities Field. */
+#define mediaTransport 0x1
+#define reporting 0x2
+#define recovery 0x3
+#define contentProtection 0x4
+#define headerCompression 0x5
+#define multiplexing 0x6
+#define mediaCodec 0x7
+
+/* Media Codec Capabilities */
+#define mediaType 0xf0
+
+struct avdtp_sepInfo {
+ uint8_t sep;
+ uint8_t media_Type;
+};
+
+int avdtpSendCommand(int fd, uint8_t command, uint8_t type,
+ uint8_t *data, size_t datasize);
+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);
+int avdtpDiscover(uint8_t *buffer, size_t recvsize, struct avdtp_sepInfo
+ *sepInfo, bool sink);
+void avdtpGetCapabilities(int fd, int recvfd, uint8_t sep);
+int avdtpAutoConfigSBC(int fd, int recvfd, uint8_t * capabilities, size_t
+ caplen, uint8_t sep, u_int8_t *freq, u_int8_t *mode, u_int8_t *alloc_method,
+ u_int8_t *bitpool, u_int8_t* bands, u_int8_t *blocks, u_int8_t srcsep);
+int avdtpSetConfiguration(int fd, int recvfd, uint8_t sep, uint8_t *data,
+ size_t datasize, int srcsep);
+void avdtpOpen(int fd, int recvfd, uint8_t sep);
+void avdtpStart(int fd, int recvfd, uint8_t sep);
+void avdtpClose(int fd, int recvfd, uint8_t sep);
+void avdtpSuspend(int fd, int recvfd, uint8_t sep);
+void avdtpAbort(int fd, int recvfd, uint8_t sep);
+int avdtpSendDiscResponseAudio(int, int, uint8_t, uint8_t, bool);
+int avdtpSendCapabilitiesResponseSBC(int, int, int, uint8_t, uint8_t,
+ uint8_t, uint8_t, uint8_t, uint8_t, uint8_t);
+int avdtpSendAccept(int, int, uint8_t, uint8_t);
+int avdtpSendReject(int, int, uint8_t, uint8_t);
+
Index: src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.8
diff -u /dev/null src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.8:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.8 Sat Jan 28 16:55:54 2017
@@ -0,0 +1,278 @@
+.\" $NetBSD: bta2dpd.8,v 1.1 2017/01/28 16:55:54 nat Exp $
+.\"
+.\" Copyright (c) 2015 - 2016 Nathanial Sloss <[email protected]>
+.\" All rights reserved.
+.\"
+.\" This software is dedicated to the memory of -
+.\" Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+.\"
+.\" Barry was a man who loved his music.
+.\"
+.\" Redistribution and use in source and binary forms, with or without
+.\" modification, are permitted provided that the following conditions
+.\" are met:
+.\" 1. Redistributions of source code must retain the above copyright
+.\" notice, this list of conditions and the following disclaimer.
+.\" 2. Redistributions in binary form must reproduce the above copyright
+.\" notice, this list of conditions and the following disclaimer in the
+.\" documentation and/or other materials provided with the distribution.
+.\"
+.\" THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+.\" ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+.\" TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+.\" PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+.\" BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+.\" CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+.\" SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+.\" INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+.\" CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+.\" ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+.\" POSSIBILITY OF SUCH DAMAGE.
+.\"
+.Dd March 2, 2016
+.Dt BTA2DPD 8
+.Os
+.Sh NAME
+.Nm bta2dpd
+.Nd Bluetooth Advanced Audio Distribution Profile daemon
+.Sh SYNOPSIS
+.Nm
+.Op Fl d Ar device
+.Op Fl m Ar mode
+.Op Fl B Ar bitpool
+.Op Fl V Ar volume
+.Op Fl M Ar mtu
+.Op Fl f Ar channel_mode
+.Op Fl r Ar rate
+.Op Fl b Ar blocks
+.Op Fl e Ar encoding_bands
+.Op Fl A Ar bitpool_allocation
+.Op Fl v
+.Fl a Ar address
+.Ar files...
+.Nm
+.Fl K
+.Op Fl I
+.Op Fl d Ar device
+.Op Fl m Ar mode
+.Op Fl p Ar psm
+.Op Fl B Ar bitpool
+.Op Fl f Ar channel_mode
+.Op Fl r Ar rate
+.Op Fl b Ar blocks
+.Op Fl e Ar encoding_bands
+.Op Fl A Ar bitpool_allocation
+.Op Fl v
+.Op a Ar address
+.Ar file
+.Nm
+.Fl t
+.Op Fl B Ar bitpool
+.Op Fl V Ar volume
+.Op Fl M Ar mtu
+.Op Fl f Ar channel_mode
+.Op Fl r Ar rate
+.Op Fl b Ar blocks
+.Op Fl e Ar encoding_bands
+.Op Fl A Ar bitpool_allocation
+.Ar files...
+.Nm
+.Fl t
+.Fl K
+.Ar file
+.Sh DESCRIPTION
+The
+.Nm
+daemon is used to transmit/receive audio to/from Bluetooth devices such as
+speakers or headphones, using the Advanced Audio Distribution Profile
+(A2DP)
+.Bl -tag -indent width
+.It Fl d Ar device
+Local device address.
+May be given as BDADDR or device name.
+.It Fl a Ar address
+Remote device address.
+The
+.Ar address
+may be given as BDADDR or a name.
+.Pp
+If a name was specified,
+.Nm
+attempts to resolve the name via
+.Xr bt_gethostbyname 3 .
+.It Ar files...
+Files to stream. These can be wav files or CD-Audio Red Book data or output from
+.Xr pad 4
+device.
+If not specified the default is stdin or stdout if -K applied and
+.Nm
+is used as an audio sink.
+.Pp
+When writing to a file or stdout it is raw 16-bit little endian (pulse coded
+modulation) pcm data.
+So a utility must be used to convert the file to add a header or use the
+audioplay command as stated in EXAMPLES.
+.It Fl M Ar mtu
+Allows setting the Maximum Transmission Unit (MTU) for the streaming
+connections.
+.Pp
+For most devices this would be 672 but if audio is broken or has gaps lowering
+this value may help.
+.It Fl I
+Initiate a Discover command. Useful when used as an audio sink as some
+devices need this to pair.
+.It Fl K
+Allows registering as an audio sink receiving an incoming connection.
+.It Fl m Ar mode
+Set L2CAP connection link mode.
+Supported modes are:
+.Pp
+.Bl -tag -compact
+.It auth
+require devices to be paired.
+.It encrypt
+auth, plus enable encryption.
+.It secure
+encryption, plus change of link key.
+.El
+.It Fl p Ar psm
+Allows changing the l2cap psm for an audio sink the default is 25.
+Most all devices will use 25.
+.It Fl t
+Test mode can be used with -K to decode and receive encoded music from
+stdout/stdin to file.
+.It Fl V Ar volume
+Allows shifting the volume of the music by 1 or 2 bits prior to encoding for
+increased volume.
+.Pp
+.Sy USE WITH CARE
+as this may mean the output is quite loud.
+.It Fl B Ar bitpool
+Allows setting the maximum bitpool value for your device.
+This may have to be lowered if audio is distorted.
+.Pp
+Use
+.Nm
+with only the -v option and the maximum bitpool for your device will be printed
+to stdout.
+.It Fl v
+Be verbose.
+.El
+.Ss Channel Modes
+.Bl -tag -indent width
+.It Fl f Ar channel_mode
+.Ar 0
+This specifies that all possible input formats are accepted and then one is
+chosen starting with joint stereo then stereo then 2 channel then mono.
+.Lp
+.Ar m
+This specifies that the input is monophonic.
+.Lp
+.Ar 2
+This specifies that the input is dual-channel.
+.Lp
+.Ar j
+This specifies that the input is joint-stereo.
+.Lp
+.Ar s
+This specifies that the input is stereo.
+This is the default.
+.Pp
+Specifying joint stereo as opposed to stereo would allow for a slightly higher
+quality of playback as
+.Nm
+will only encode stereo and monophonic streams.
+.It Fl r Ar rate
+The frequency of the input.
+Where rate is one of 0 16000, 32000, 44100, 48000.
+This defaults to 44.1 kHz.
+.Pp
+A value of 0 will arbitrate from the highest to lowest frequency.
+.El
+.Ss Sub Band Codec (SBC) Encoding Options
+.Bl -tag -indent width
+.It Fl b Ar blocks
+Number of blocks to use for encoding.
+Where blocks is one of 0 4, 8, 12, 16.
+This defaults to 16 blocks.
+.Pp
+In the case of 0 the number of blocks is
+negotiated starting from 16 down to 4.
+.It Fl e Ar bands
+.Ar 0
+Number of SBC bands is negotiated 8 then 4.
+.Lp
+.Ar 4
+Audio is endoded with 4 SBC bands.
+.Lp
+.Ar 8
+Audio is endoded with 8 SBC bands.
+This is the default.
+.It Fl A Ar bitpool_allocation
+.Ar 0
+Bit Allocation is negotiated starting with Loudness then SNR.
+.Lp
+.Ar S
+Audio is encoded with SNR (Signal to Noise Ratio) bit allocation.
+.Lp
+.Ar L
+Audio is encoded with Loudness bit allocation.
+This is the default.
+.El
+.Pp
+It may be necessary to use
+.Xr btconfig 1
+to set the class of your bluetooth adapter to that of headphones when
+using
+.Nm
+as an audio sink.
+i.e
+.Dl btconfig ubt0 class 0x200418
+Then start
+.Nm
+with -K before pairing.
+.Pp
+This is necessary as some devices only perform an
+.Xr sdpquery 1
+just after pairing and cache the result.
+.Pp
+It is possible to specify multiples of -r -e -b -f .
+This will mean that the specified combinations are reported as being accepted
+by the source/sink.
+.Pp
+When used as a sink -K all modes, bands, blocks and allocation modes will be
+accepted unless specified as options.
+i.e -r 44100 will only accept a connection with a rate of 44.1kHz.
+.Sh EXAMPLES
+.Dl bta2dpd -a spkr my.wav
+.Lp
+Encode and send audio
+.Pa my.wav
+to address
+.Sq spkr .
+.Pp
+.Dl bta2dpd -a spkr /dev/pad
+.Lp
+Encode and send audio from
+.Xr pad 4
+to address
+.Sq spkr .
+.Pp
+.Dl bta2dpd -K out.pcm
+.Lp
+Decode stream from any connected address
+and write it to
+.Pa out.pcm .
+.Pp
+.Dl bta2dpd -K -a phone | audioplay -f -e linear -P 16 -s 44100 -c 2 --
+.Lp
+Decode stream from address
+.Sq phone
+and send to speakers.
+.Sh SEE ALSO
+.Xr pad 4 ,
+.Xr audioplay 1 ,
+.Xr btconfig 1 ,
+.Xr sdpquery 1
+.Sh AUTHORS
+.An Nathanial Sloss
Index: src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.c
diff -u /dev/null src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.c:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/bta2dpd/bta2dpd.c Sat Jan 28 16:55:54 2017
@@ -0,0 +1,1129 @@
+/* $NetBSD: bta2dpd.c,v 1.1 2017/01/28 16:55:54 nat Exp $ */
+
+/*-
+ * Copyright (c) 2015 - 2016 Nathanial Sloss <[email protected]>
+ * All rights reserved.
+ *
+ * This software is dedicated to the memory of -
+ * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+ *
+ * Barry was a man who loved his music.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*-
+ * Copyright (c) 2006 Itronix Inc.
+ * All rights reserved.
+ *
+ * Written by Iain Hibbert for Itronix Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. The name of Itronix Inc. may not be used to endorse
+ * or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* This was based upon bthset.c */
+
+#include <sys/cdefs.h>
+__COPYRIGHT("@(#) Copyright (c) 2006 Itronix, Inc. All rights reserved.");
+__RCSID("$NetBSD");
+
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/time.h>
+
+#include <assert.h>
+#include <bluetooth.h>
+#include <err.h>
+#include <event.h>
+#include <fcntl.h>
+#include <sdp.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+
+#include "avdtp_signal.h"
+#include "sbc_encode.h"
+
+#define A2DP_SINK 0x110b
+
+/* A2DP Audio Source service record */
+static uint8_t source_data[] = {
+ 0x09, 0x00, 0x00, // uint16 ServiceRecordHandle
+ 0x0a, 0x00, 0x00, 0x00, // uint32 0x00000000
+ 0x00,
+
+ 0x09, 0x00, 0x01, // uint16 ServiceClassIDList
+ 0x35, 0x03, // seq8(3)
+ 0x19, 0x11, 0x0a, // uuid16 AudioSource
+
+ 0x09, 0x00, 0x04, // uint16 ProtocolDescriptorList
+ 0x35, 0x10, // seq8(16)
+ 0x35, 0x06, // seq8(6)
+ 0x19, 0x01, 0x00, // uuid16 L2CAP
+ 0x09, 0x00, 0x19, // uint16 %src_psm%
+ 0x35, 0x06, // seq8(6)
+ 0x19, 0x00, 0x19, // uuid16 AVDTP
+ 0x09, 0x01, 0x00, // uint16 Ver 1.0
+
+ 0x09, 0x00, 0x05, // uint16 BrowseGroupList
+ 0x35, 0x03, // seq8(3)
+ 0x19, 0x10, 0x02, // uuid16 PublicBrowseGroup
+
+ 0x09, 0x00, 0x06, // uint16 LanguageBaseAttributeIDList
+ 0x35, 0x09, // seq8(9)
+ 0x09, 0x65, 0x6e, // uint16 0x656e ("en")
+ 0x09, 0x00, 0x6a, // uint16 106 (UTF-8)
+ 0x09, 0x01, 0x00, // uint16 PrimaryLanguageBaseID
+
+ 0x09, 0x00, 0x09, // uint16 BluetoothProfileDescriptorList
+ 0x35, 0x08, // seq8(8)
+ 0x35, 0x06, // seq8(6)
+ 0x19, 0x11, 0x0d, // uuid16 A2DP
+ 0x09, 0x01, 0x02, // uint16 v1.2
+
+ 0x09, 0x01, 0x00, // uint16 PrimaryLanguageBaseID +
+ // ServiceNameOffset
+ 0x25, 0x09, // str8(9) "Audio SRC"
+ 'A', 'u', 'd', 'i', 'o', ' ', 'S', 'R', 'C',
+
+ 0x09, 0x01, 0x02, // uint16 PrimaryLanguageBaseID +
+ // ProviderName
+ 0x25, 0x06, // str8(6) "NetBSD"
+ 'N', 'e', 't', 'B', 'S', 'D',
+
+ 0x09, 0x03, 0x11, // uint16 SupportedFeatures
+ 0x09, 0x01, 0x02 // uint16 Headphone, Speaker
+};
+
+static sdp_data_t source_record = { source_data + 0, source_data + 103 };
+static sdp_data_t source_psm = { source_data + 26, source_data + 29 };
+
+
+/* A2DP Audio Sink service record */
+static uint8_t sink_data[] = {
+ 0x09, 0x00, 0x00, // uint16 ServiceRecordHandle
+ 0x0a, 0x00, 0x00, 0x00, // uint32 0x00000000
+ 0x00,
+
+ 0x09, 0x00, 0x01, // uint16 ServiceClassIDList
+ 0x35, 0x03, // seq8(3)
+ 0x19, 0x11, 0x0b, // uuid16 AudioSink
+
+ 0x09, 0x00, 0x04, // uint16 ProtocolDescriptorList
+ 0x35, 0x10, // seq8(16)
+ 0x35, 0x06, // seq8(6)
+ 0x19, 0x01, 0x00, // uuid16 L2CAP
+ 0x09, 0x00, 0x19, // uint16 %sink_psm%
+ 0x35, 0x06, // seq8(6)
+ 0x19, 0x00, 0x19, // uuid16 AVDTP
+ 0x09, 0x01, 0x00, // uint16 Ver 1.0
+
+ 0x09, 0x00, 0x05, // uint16 BrowseGroupList
+ 0x35, 0x03, // seq8(3)
+ 0x19, 0x10, 0x02, // uuid16 PublicBrowseGroup
+
+ 0x09, 0x00, 0x06, // uint16 LanguageBaseAttributeIDList
+ 0x35, 0x09, // seq8(9)
+ 0x09, 0x65, 0x6e, // uint16 0x656e ("en")
+ 0x09, 0x00, 0x6a, // uint16 106 (UTF-8)
+ 0x09, 0x01, 0x00, // uint16 PrimaryLanguageBaseID
+
+ 0x09, 0x00, 0x09, // uint16 BluetoothProfileDescriptorList
+ 0x35, 0x08, // seq8(8)
+ 0x35, 0x06, // seq8(6)
+ 0x19, 0x11, 0x0d, // uuid16 A2DP
+ 0x09, 0x01, 0x02, // uint16 v1.2
+
+ 0x09, 0x01, 0x00, // uint16 PrimaryLanguageBaseID +
+ // ServiceNameOffset
+ 0x25, 0x09, // str8(9) "Audio SNK"
+ 'A', 'u', 'd', 'i', 'o', ' ', 'S', 'N', 'K',
+
+ 0x09, 0x01, 0x02, // uint16 PrimaryLanguageBaseID +
+ // ProviderName
+ 0x25, 0x06, // str8(6) "NetBSD"
+ 'N', 'e', 't', 'B', 'S', 'D',
+
+ 0x09, 0x03, 0x11, // uint16 SupportedFeatures
+ 0x09, 0x00, 0x03 // uint16 Headphone, Speaker
+};
+
+static sdp_data_t sink_record = { sink_data + 0, sink_data + 103 };
+static sdp_data_t sink_psm = { sink_data + 26, sink_data + 29 };
+
+struct l2cap_info {
+ bdaddr_t laddr;
+ bdaddr_t raddr;
+};
+
+__dead static void usage(void);
+
+static int init_server(struct l2cap_info *);
+static int init_client(struct l2cap_info *);
+static void client_query(void);
+static void exit_func(void);
+
+static int sc; /* avdtp streaming channel */
+static int orighc; /* avdtp listening socket */
+static int hc; /* avdtp control/command channel */
+struct event_base *base;
+static struct event interrupt_ev; /* audio event */
+static struct event recv_ev; /* audio sink event */
+static struct event ctl_ev; /* avdtp ctl event */
+
+struct l2cap_info info;
+static bool asSpeaker;
+static bool initDiscover; /* initiate avdtp discover */
+static bool verbose; /* copy to stdout */
+static bool test_mode; /* copy to stdout */
+static uint8_t channel_mode = MODE_STEREO;
+static uint8_t alloc_method = ALLOC_LOUDNESS;
+static uint8_t frequency = FREQ_44_1K;
+static uint8_t freqs[4];
+static uint8_t blocks_config[4];
+static uint8_t channel_config[4];
+static uint8_t bands_config[2];
+static uint8_t alloc_config[2];
+static uint8_t bitpool = 0;
+static uint8_t bands = BANDS_8;
+static uint8_t blocks = BLOCKS_16;
+static int volume = 0;
+static int state = 0;
+static uint16_t l2cap_psm = 0;
+static int l2cap_mode;
+uint16_t mtu;
+uint16_t userset_mtu = 1000, service_class = A2DP_SINK;
+static const char* service_type = "A2DP";
+struct avdtp_sepInfo mySepInfo;
+static sdp_session_t ss; /* SDP server session */
+
+char *files2open[255];
+static int numfiles = 0;
+static int startFileInd = 0;
+int audfile;
+
+static void do_interrupt(int, short, void *);
+static void do_recv(int, short, void *);
+static void do_ctlreq(int, short, void *);
+
+#define log_err(x, ...) if (verbose) { fprintf (stderr, x "\n",\
+ __VA_ARGS__); }
+#define log_info(x, ...) if (verbose) { fprintf (stderr, x "\n",\
+ __VA_ARGS__); }
+
+int
+main(int ac, char *av[])
+{
+ int enc, i, n, m, l, j, k, o, ch, freqnum, blocksnum;
+ u_int tmpbitpool;
+ bdaddr_copy(&info.laddr, BDADDR_ANY);
+
+ sc = hc = -1;
+ verbose = asSpeaker = test_mode = initDiscover = false;
+ n = m = l = i = j = o = 0;
+ freqs[0] = frequency;
+ channel_config[0] = channel_mode;
+ blocks_config[0] = blocks;
+ bands_config[0] = bands;
+ alloc_config[0] = alloc_method;
+ channel_config[0] = channel_mode;
+
+ while ((ch = getopt(ac, av, "A:a:B:b:d:e:f:IKM:m:p:r:tV:v")) != EOF) {
+ switch (ch) {
+ case 'A':
+ for (k = 0; k < (int)strlen(optarg); k++) {
+ if (o > 1)
+ break;
+ switch (optarg[k]) {
+ case '0':
+ alloc_config[o++] = ALLOC_ANY;
+ break;
+ case 'S':
+ alloc_config[o++] = ALLOC_SNR;
+ break;
+ case 'L':
+ alloc_config[o++] = ALLOC_LOUDNESS;
+ break;
+ default:
+ usage();
+ }
+ }
+ break;
+ case 'a':
+ if (!bt_aton(optarg, &info.raddr)) {
+ struct hostent *he = NULL;
+
+ if ((he = bt_gethostbyname(optarg)) == NULL)
+ usage();
+
+ bdaddr_copy(&info.raddr,
+ (bdaddr_t *)he->h_addr);
+ }
+ break;
+ case 'B': /* Max bitpool value */
+ bitpool = (uint8_t)atoi(optarg);
+ break;
+ case 'b':
+ if (m > 3)
+ break;
+ blocksnum = atoi(optarg);
+ switch (blocksnum) {
+ case 0:
+ blocks_config[m++] = BLOCKS_ANY;
+ break;
+ case 4:
+ blocks_config[m++] = BLOCKS_4;
+ break;
+ case 8:
+ blocks_config[m++] = BLOCKS_8;
+ break;
+ case 12:
+ blocks_config[m++] = BLOCKS_12;
+ break;
+ case 16:
+ blocks_config[m++] = BLOCKS_16;
+ break;
+ default:
+ usage();
+ }
+ break;
+ case 'd': /* local device address */
+ if (!bt_devaddr(optarg, &info.laddr))
+ usage();
+ break;
+ case 'e':
+ if (l > 1)
+ break;
+ enc = atoi(optarg);
+ switch (enc) {
+ case 0:
+ bands_config[l++] = BANDS_ANY;
+ break;
+ case 4:
+ bands_config[l++] = BANDS_4;
+ break;
+ case 8:
+ bands_config[l++] = BANDS_8;
+ break;
+ default:
+ usage();
+ }
+ break;
+ case 'f':
+ for (k = 0; k < (int)strlen(optarg); k++) {
+ if (n > 3)
+ break;
+ switch (optarg[k]) {
+ case '0':
+ channel_config[n++] = MODE_ANY;
+ break;
+ case '2':
+ channel_config[n++] = MODE_DUAL;
+ break;
+ case 'j':
+ channel_config[n++] = MODE_JOINT;
+ break;
+ case 'm':
+ channel_config[n++] = MODE_MONO;
+ break;
+ case 's':
+ channel_config[n++] = MODE_STEREO;
+ break;
+ default:
+ usage();
+ }
+ }
+ break;
+ case 'I':
+ initDiscover = true;
+ break;
+ case 'K':
+ asSpeaker = true;
+ break;
+ case 'M':
+ userset_mtu = (uint16_t)atoi(optarg);
+ break;
+ case 'm': /* link mode */
+ if (strcasecmp(optarg, "auth") == 0)
+ l2cap_mode = L2CAP_LM_AUTH;
+ else if (strcasecmp(optarg, "encrypt") == 0)
+ l2cap_mode = L2CAP_LM_ENCRYPT;
+ else if (strcasecmp(optarg, "secure") == 0)
+ l2cap_mode = L2CAP_LM_SECURE;
+ else if (strcasecmp(optarg, "none"))
+ errx(EXIT_FAILURE, "%s: unknown mode", optarg);
+
+ break;
+ case 'p':
+ l2cap_psm = (uint16_t)atoi(optarg);
+ break;
+ case 'r':
+ if (i > 3)
+ break;
+ freqnum = atoi(optarg);
+ switch (freqnum) {
+ case 0:
+ freqs[i++] = FREQ_ANY;
+ break;
+ case 16000:
+ freqs[i++] = FREQ_16K;
+ break;
+ case 32000:
+ freqs[i++] = FREQ_32K;
+ break;
+ case 44100:
+ freqs[i++] = FREQ_44_1K;
+ break;
+ case 48000:
+ freqs[i++] = FREQ_48K;
+ break;
+ default:
+ usage();
+ }
+ break;
+ case 't':
+ test_mode = true;
+ break;
+ case 'V': /* Volume Multiplier */
+ volume = atoi(optarg);
+ break;
+ case 'v':
+ verbose = true;
+ break;
+ default:
+ usage();
+ }
+ }
+ av += optind;
+ ac -= optind;
+
+ numfiles = ac;
+
+ for (i = 0; i < numfiles; i++)
+ files2open[i] = av[i];
+
+ if (bdaddr_any(&info.raddr) && (!asSpeaker && !test_mode))
+ usage();
+
+ if (volume < 0 || volume > 2)
+ usage();
+
+ if (!asSpeaker && l2cap_psm)
+ usage();
+
+ if (asSpeaker) {
+ if (l == 0)
+ bands = BANDS_ANY;
+ if (m == 0)
+ blocks = BLOCKS_ANY;
+ if (n == 0)
+ channel_mode = MODE_ANY;
+ if (o == 0)
+ alloc_method = ALLOC_ANY;
+ }
+
+ if (i) {
+ frequency = 0;
+ for (j = 0; j < i; j++)
+ frequency |= freqs[j];
+ }
+ if (m) {
+ blocks = 0;
+ for (j = 0; j < m; j++)
+ blocks |= blocks_config[j];
+ }
+ if (l) {
+ bands = 0;
+ for (j = 0; j < l; j++)
+ bands |= bands_config[j];
+ }
+ if (n) {
+ channel_mode = 0;
+ for (j = 0; j < n; j++)
+ channel_mode |= channel_config[j];
+ }
+ if (o) {
+ alloc_method = 0;
+ for (j = 0; j < o; j++)
+ alloc_method |= alloc_config[j];
+ }
+
+ if (channel_mode == MODE_MONO || channel_mode == MODE_DUAL)
+ tmpbitpool = 16;
+ else
+ tmpbitpool = 32;
+
+ if (bands == BANDS_8)
+ tmpbitpool *= 8;
+ else
+ tmpbitpool *= 4;
+
+ if (tmpbitpool > DEFAULT_MAXBPOOL)
+ tmpbitpool = DEFAULT_MAXBPOOL;
+
+ if (bitpool == 0 || tmpbitpool < bitpool)
+ bitpool = (uint8_t)tmpbitpool;
+
+again:
+ base = event_init();
+
+ if (asSpeaker == 0) {
+ if (init_client(&info) < 0)
+ err(EXIT_FAILURE, "init client");
+ } else {
+ if (init_server(&info) < 0)
+ err(EXIT_FAILURE, "init server");
+ }
+
+ if (verbose) {
+ fprintf(stderr, "A2DP:\n");
+ fprintf(stderr, "\tladdr: %s\n", bt_ntoa(&info.laddr, NULL));
+ fprintf(stderr, "\traddr: %s\n", bt_ntoa(&info.raddr, NULL));
+ }
+
+ event_base_loop(base, 0);
+ event_base_free(base);
+
+ sdp_close(ss);
+
+ if (audfile != -1 && audfile != STDIN_FILENO && audfile !=
+ STDOUT_FILENO) {
+ close(audfile);
+ audfile = -1;
+ }
+
+ if (asSpeaker)
+ goto again;
+
+ return EXIT_SUCCESS;
+}
+
+static void
+usage(void)
+{
+ fprintf(stderr,
+ "usage:\t%s [-v] [-d device] [-m mode] [-r rate] [-M mtu]\n"
+ "\t\t[-V volume] [-f mode] [-b blocks] [-e bands] [-A alloc]\n"
+ "\t\t[-B bitpool] -a address files...\n"
+ "\t%s [-v] [-d device] [-m mode] [-p psm] [-B bitpool]\n"
+ "\t\t[-a address] [-M mtu] [-r rate] [-I] -K file\n"
+ "\t%s -t [-v] [-K] [-r rate] [-M mtu] [-V volume] [-f mode]\n"
+ "\t\t[-b blocks] [-e bands] [-A alloc] [-B bitpool] files...\n"
+ "Where:\n"
+ "\t-d device Local device address\n"
+ "\t-a address Remote device address\n"
+ "\tfiles... Files to read from (Defaults to stdin/stdout)\n"
+ "\t-v Verbose output\n"
+ "\t-M mtu MTU for transmission\n"
+ "\t-B bitpool Maximum bitpool value for encoding\n"
+ "\t-V volume Volume multiplier 0,1,2.\n"
+ "\t WARNING Can be VERY loud\n"
+ "\t-K Register as audio sink - Listens for incoming\n"
+ "\t connections.\n"
+ "\t-I Initiate DISCOVER - Used with audio sink\n"
+ "\t-m mode L2CAP link mode\n"
+ "\t-p psm Listens for incoming connections on psm\n"
+ "\t-t TEST MODE encodes from file to stdout or -K\n"
+ "\t read from stdin and decode to file/stdout.\n"
+ "\t Test mode does not communicate with bluetooth\n"
+ "\t only performs SBC de/encoding.\n"
+ "ENCODING OPTIONS:\n"
+ "\t-f mode\n"
+ "\t\t0 All input modes are accepted (only used with -K)\n"
+ "\t\t2 Input is dual channel\n"
+ "\t\tj Input is in joint stereo\n"
+ "\t\tm Input is mono\n"
+ "\t\ts Input is stereo (this is the default)\n"
+ "\t-r rate\n"
+ "\t\t0 All combinations of frequencies are accepted\n"
+ "\t\t16000 Encode with a rate of 16000Hz\n"
+ "\t\t32000 Encode with a rate of 32000Hz\n"
+ "\t\t44100 Encode with a rate of 44100Hz "
+ "(this is the default)\n"
+ "\t\t48000 Encode with a rate of 48000Hz\n"
+ "\t-b blocks\n"
+ "\t\t0 All combinations of blocks are accepted\n"
+ "\t\t4 Encode with 4 blocks\n"
+ "\t\t8 Encode with 8 blocks\n"
+ "\t\t12 Encode with 12 blocks\n"
+ "\t\t16 Encode with 16 blocks (this is the default)\n"
+ "\t-A alloc\n"
+ "\t\t0 All combinations of allocation methods are accepted\n"
+ "\t\tS Signal to Noise Ratio (SNR) bit allocation\n"
+ "\t\tL Loudness bit allocation (this is the default)\n"
+ "\n"
+ "\tWithout specifiying any mode rate enoding and allocation\n"
+ "\tthe channel the default is stereo, 16 blocks, 8 subbands,\n"
+ "\tloudness bit allocation, 441000 Hz.\n"
+ , getprogname(), getprogname(), getprogname());
+
+ exit(EXIT_FAILURE);
+}
+
+static void
+do_ctlreq(int fd, short ev, void *arg)
+{
+ bool isCommand;
+ uint8_t sep, signal;
+ static struct sockaddr_bt addr;
+ size_t bufflen;
+ socklen_t len, mtusize;
+ static uint8_t trans;
+ uint8_t buff[1024];
+ static size_t offset = 0;
+ int result;
+
+ if(avdtpCheckResponse(fd, &isCommand, &trans, &signal, NULL, buff,
+ &bufflen, &sep) == ENOMEM) {
+ event_del(&ctl_ev);
+ close(fd);
+
+ if (asSpeaker) {
+ close(orighc);
+ orighc = -1;
+ event_del(&recv_ev);
+ } else
+ event_del(&interrupt_ev);
+
+ if (sc != -1)
+ close(sc);
+ sc = hc = -1;
+ state = 0;
+
+ return;
+ } else if (isCommand) {
+ switch (signal) {
+ case AVDTP_ABORT:
+ case AVDTP_CLOSE:
+ avdtpSendAccept(fd, fd, trans, signal);
+ if (asSpeaker) {
+ event_del(&recv_ev);
+ } else
+ event_del(&interrupt_ev);
+
+ if (sc != -1)
+ close(sc);
+ sc = -1;
+ state = 0;
+ break;
+ case AVDTP_DISCOVER:
+ avdtpSendDiscResponseAudio(fd, fd, trans,
+ (asSpeaker ? SNK_SEP : SRC_SEP), asSpeaker);
+ state = 1;
+ break;
+ case AVDTP_GET_CAPABILITIES:
+ avdtpSendCapabilitiesResponseSBC(fd, fd, trans,
+ (asSpeaker ? SNK_SEP : SRC_SEP), bitpool,
+ frequency , channel_mode, bands, blocks,
+ alloc_method);
+ state = 2;
+ break;
+ case AVDTP_SET_CONFIGURATION:
+ avdtpSendAccept(fd, fd, trans, signal);
+ state = 3;
+ break;
+ case AVDTP_OPEN:
+ avdtpSendAccept(fd, fd, trans, signal);
+ if (state < 5)
+ state = 5;
+ break;
+ case AVDTP_SUSPEND:
+ case AVDTP_START:
+ avdtpSendAccept(fd, fd, trans, signal);
+ break;
+ default:
+ avdtpSendReject(fd, fd, trans, signal);
+ }
+ if (verbose)
+ fprintf(stderr, "Received command %d\n",signal);
+ } else {
+ switch (signal) {
+ case AVDTP_DISCOVER:
+ if (offset >= bufflen)
+ offset = 0;
+ avdtpDiscover(buff + offset, bufflen - offset,
+ &mySepInfo, !asSpeaker);
+ avdtpGetCapabilities(fd, fd, mySepInfo.sep);
+ break;
+ case AVDTP_GET_CAPABILITIES:
+ result = avdtpAutoConfigSBC(fd, fd, buff, bufflen,
+ mySepInfo.sep, &frequency, &channel_mode,
+ &alloc_method, &bitpool, &bands, &blocks,
+ (asSpeaker ? SNK_SEP : SRC_SEP));
+
+ if (result == EINVAL) {
+ offset += 2;
+ avdtpSendCommand(hc, AVDTP_DISCOVER, 0,
+ NULL, 0);
+ }
+
+ if (!result && verbose)
+ fprintf(stderr, "Bitpool value = %d\n",bitpool);
+ break;
+ case AVDTP_SET_CONFIGURATION:
+ if (state < 5 && !asSpeaker)
+ avdtpOpen(fd, fd, mySepInfo.sep);
+ break;
+ case AVDTP_OPEN:
+ if (state < 5)
+ state = 5;
+ break;
+ case AVDTP_SUSPEND:
+ case AVDTP_START:
+ break;
+ default:
+ avdtpSendReject(fd, fd, trans, signal);
+ }
+ if (verbose)
+ fprintf(stderr, "Responded to command %d\n",signal);
+ }
+
+
+ if (state != 5)
+ return;
+
+ if (asSpeaker) {
+ len = sizeof(addr);
+ if ((sc = accept(orighc,(struct sockaddr*)&addr, &len)) < 0)
+ err(EXIT_FAILURE, "stream accept");
+
+ goto opened_connection;
+ }
+
+ memset(&addr, 0, sizeof(addr));
+
+ addr.bt_len = sizeof(addr);
+ addr.bt_family = AF_BLUETOOTH;
+ bdaddr_copy(&addr.bt_bdaddr, &info.laddr);
+ addr.bt_psm = l2cap_psm;
+
+ sc = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+ if (sc < 0)
+ return;
+
+ if (bind(sc, (struct sockaddr *)&addr, sizeof(addr)) < 0)
+ return;
+
+ if (setsockopt(sc, BTPROTO_L2CAP, SO_L2CAP_LM,
+ &l2cap_mode, sizeof(l2cap_mode)) == -1) {
+ log_err("Could not set link mode (0x%4.4x)", l2cap_mode);
+ exit(EXIT_FAILURE);
+ }
+
+ bdaddr_copy(&addr.bt_bdaddr, &info.raddr);
+ if (connect(sc,(struct sockaddr*)&addr, sizeof(addr)) < 0)
+ return;
+
+opened_connection:
+ if (asSpeaker) {
+ event_set(&recv_ev, sc, EV_READ | EV_PERSIST, do_recv, NULL);
+ if (event_add(&recv_ev, NULL) < 0)
+ err(EXIT_FAILURE, "recv_ev");
+ } else {
+ event_set(&interrupt_ev, audfile, EV_READ | EV_PERSIST,
+ do_interrupt, NULL);
+ if (event_add(&interrupt_ev, NULL) < 0)
+ err(EXIT_FAILURE, "interrupt_ev");
+ }
+
+ mtusize = sizeof(uint16_t);
+ getsockopt(sc, BTPROTO_L2CAP, SO_L2CAP_OMTU, &mtu, &mtusize);
+
+ if (userset_mtu != 0 && userset_mtu > 100 && userset_mtu < mtu)
+ mtu = userset_mtu;
+ else if (userset_mtu == 0 && mtu >= 500)
+ mtu /= 2;
+
+ if (!asSpeaker)
+ avdtpStart(fd, fd, mySepInfo.sep);
+
+ state = 6;
+
+}
+
+static void
+do_recv(int fd, short ev, void *arg)
+{
+ ssize_t len;
+
+ len = recvstream(fd, audfile);
+
+ if (verbose)
+ fprintf(stderr, "Recving %zd bytes\n",len);
+
+ if (len < 0) {
+ event_del(&recv_ev);
+ close(fd);
+ fd = -1;
+ exit(1);
+ }
+}
+
+
+static void
+do_interrupt(int fd, short ev, void *arg)
+{
+ static int currentFileInd = 0;
+ ssize_t len;
+
+ if (currentFileInd == 0)
+ currentFileInd = startFileInd;
+
+ len = stream(fd, sc, channel_mode, frequency, bands, blocks,
+ alloc_method, bitpool, mtu, volume);
+
+ if (len == -1 && currentFileInd >= numfiles -1) {
+ event_del(&interrupt_ev);
+ close(fd);
+ fd = -1;
+ exit(1);
+ } else if (len == -1) {
+ close(fd);
+next_file:
+ currentFileInd++;
+ audfile = open(files2open[currentFileInd], O_RDONLY);
+ if (audfile < 0) {
+ warn("error opening file %s",
+ files2open[currentFileInd]);
+ goto next_file;
+ }
+
+ event_del(&interrupt_ev);
+ event_set(&interrupt_ev, audfile, EV_READ |
+ EV_PERSIST, do_interrupt, NULL);
+ if (event_add(&interrupt_ev, NULL) < 0)
+ err(EXIT_FAILURE, "interrupt_ev");
+ }
+
+ if (verbose)
+ fprintf(stderr, "Streaming %zd bytes\n",len);
+}
+
+/*
+ * Initialise as an audio sink
+ */
+static int
+init_server(struct l2cap_info *myInfo)
+{
+ struct sockaddr_bt addr;
+ socklen_t len;
+ int flags = O_WRONLY | O_CREAT;
+ static bool first_time = true;
+
+ if (atexit(exit_func))
+ err(EXIT_FAILURE,"atexit failed to initialize");
+
+ if (numfiles == 0)
+ audfile = STDOUT_FILENO;
+ else if (numfiles > 1)
+ usage();
+ else {
+ if (first_time)
+ flags |= O_TRUNC;
+ else
+ flags |= O_APPEND;
+
+ first_time = false;
+
+ audfile = open(files2open[0], flags, 0600);
+ if (audfile < 0) {
+ err(EXIT_FAILURE, "error opening file %s",
+ files2open[0]);
+ }
+ }
+ if (test_mode)
+ goto just_decode;
+
+ if (l2cap_psm == 0)
+ l2cap_psm = L2CAP_PSM_AVDTP;
+
+ sdp_set_uint(&sink_psm, l2cap_psm);
+
+ ss = sdp_open_local(NULL);
+ if (ss == NULL)
+ return -1;
+
+ if (!sdp_record_insert(ss, &myInfo->laddr, NULL, &sink_record)) {
+ sdp_close(ss);
+ return -1;
+ }
+
+ orighc = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+ if (orighc < 0)
+ return -1;
+
+ memset(&addr, 0, sizeof(addr));
+ addr.bt_len = sizeof(addr);
+ addr.bt_family = AF_BLUETOOTH;
+ addr.bt_psm = l2cap_psm;
+ bdaddr_copy(&addr.bt_bdaddr, &myInfo->laddr);
+
+ if (bind(orighc, (struct sockaddr *)&addr, sizeof(addr)) < 0)
+ return -1;
+
+ if (setsockopt(orighc, BTPROTO_L2CAP, SO_L2CAP_LM,
+ &l2cap_mode, sizeof(l2cap_mode)) == -1) {
+ log_err("Could not set link mode (0x%4.4x)", l2cap_mode);
+ exit(EXIT_FAILURE);
+ }
+
+ bdaddr_copy(&addr.bt_bdaddr, &myInfo->raddr);
+ if (listen(orighc,0) < 0)
+ return -1;
+
+ len = sizeof(addr);
+ if ((hc = accept(orighc,(struct sockaddr*)&addr, &len)) < 0)
+ return -1;
+
+ if (initDiscover)
+ avdtpSendCommand(hc, AVDTP_DISCOVER, 0, NULL, 0);
+
+ event_set(&ctl_ev, hc, EV_READ | EV_PERSIST, do_ctlreq, NULL);
+ if (event_add(&ctl_ev, NULL) < 0)
+ err(EXIT_FAILURE, "ctl_ev");
+
+just_decode:
+ if (test_mode) {
+ if (userset_mtu)
+ mtu = userset_mtu;
+ sc = STDIN_FILENO;
+
+ event_set(&recv_ev, sc, EV_READ | EV_PERSIST, do_recv, NULL);
+ if (event_add(&recv_ev, NULL) < 0)
+ err(EXIT_FAILURE, "recv_ev");
+ }
+
+ return 0;
+}
+
+
+/*
+ * Initialise as an audio source
+ */
+static int
+init_client(struct l2cap_info *myInfo)
+{
+ struct sockaddr_bt addr;
+ int i;
+
+ if (atexit(exit_func))
+ err(EXIT_FAILURE,"atexit failed to initialize");
+
+ if (numfiles == 0)
+ audfile = STDIN_FILENO;
+ else {
+ for (i = 0; i < numfiles; i++) {
+ audfile = open(files2open[i], O_RDONLY);
+ if (audfile < 0)
+ warn("error opening file %s",files2open[i]);
+ else
+ break;
+ }
+ startFileInd = i;
+ if (startFileInd > numfiles - 1)
+ errx(EXIT_FAILURE,"error opening file%s",
+ (numfiles > 1) ? "s":"");
+ }
+
+ if (test_mode)
+ goto just_encode;
+
+ if (l2cap_psm == 0)
+ l2cap_psm = L2CAP_PSM_AVDTP;
+
+ sdp_set_uint(&source_psm, l2cap_psm);
+
+ ss = sdp_open_local(NULL);
+ if (ss == NULL)
+ return -1;
+
+ if (!sdp_record_insert(ss, &myInfo->laddr, NULL, &source_record)) {
+ sdp_close(ss);
+ return -1;
+ }
+
+ client_query();
+
+ orighc = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+ if (orighc < 0)
+ return -1;
+
+ memset(&addr, 0, sizeof(addr));
+ addr.bt_len = sizeof(addr);
+ addr.bt_family = AF_BLUETOOTH;
+ addr.bt_psm = l2cap_psm;
+ bdaddr_copy(&addr.bt_bdaddr, &myInfo->laddr);
+
+ if (bind(orighc, (struct sockaddr *)&addr, sizeof(addr)) < 0)
+ return -1;
+
+ if (setsockopt(orighc, BTPROTO_L2CAP, SO_L2CAP_LM,
+ &l2cap_mode, sizeof(l2cap_mode)) == -1) {
+ log_err("Could not set link mode (0x%4.4x)", l2cap_mode);
+ exit(EXIT_FAILURE);
+ }
+
+ bdaddr_copy(&addr.bt_bdaddr, &myInfo->raddr);
+ if (connect(orighc,(struct sockaddr*)&addr, sizeof(addr)) < 0)
+ return -1;
+
+ hc = orighc;
+ avdtpSendCommand(hc, AVDTP_DISCOVER, 0, NULL, 0);
+
+ event_set(&ctl_ev, hc, EV_READ | EV_PERSIST, do_ctlreq, NULL);
+ if (event_add(&ctl_ev, NULL) < 0)
+ err(EXIT_FAILURE, "ctl_ev");
+
+just_encode:
+ if (test_mode) {
+ if (userset_mtu)
+ mtu = userset_mtu;
+ sc = STDOUT_FILENO;
+ event_set(&interrupt_ev, audfile, EV_READ | EV_PERSIST,
+ do_interrupt, NULL);
+ if (event_add(&interrupt_ev, NULL) < 0)
+ err(EXIT_FAILURE, "interrupt_ev");
+ }
+
+ return 0;
+}
+
+static void
+client_query(void)
+{
+ uint8_t buf[12]; /* enough for SSP and AIL both */
+ sdp_session_t myss;
+ sdp_data_t ssp, ail, rsp, rec, value, pdl, seq;
+ uintmax_t psm, ver;
+ uint16_t attr;
+ bool rv;
+
+ myss = sdp_open(&info.laddr, &info.raddr);
+ if (myss == NULL) {
+ log_err("%s: Could not open sdp session", service_type);
+ exit(EXIT_FAILURE);
+ }
+
+ log_info("Searching for %s service at %s",
+ service_type, bt_ntoa(&info.raddr, NULL));
+
+ seq.next = buf;
+ seq.end = buf + sizeof(buf);
+
+ /*
+ * build ServiceSearchPattern (9 bytes)
+ *
+ * uuid16 "service_class"
+ * uuid16 L2CAP
+ * uuid16 AVDTP
+ */
+ ssp.next = seq.next;
+ sdp_put_uuid16(&seq, service_class);
+ sdp_put_uuid16(&seq, SDP_UUID_PROTOCOL_L2CAP);
+ sdp_put_uuid16(&seq, SDP_UUID_PROTOCOL_AVDTP);
+ ssp.end = seq.next;
+
+ /*
+ * build AttributeIDList (3 bytes)
+ *
+ * uint16 ProtocolDescriptorList
+ */
+ ail.next = seq.next;
+ sdp_put_uint16(&seq, SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST);
+ ail.end = seq.next;
+
+ rv = sdp_service_search_attribute(myss, &ssp, &ail, &rsp);
+ if (!rv) {
+ log_err("%s: Required sdp record not found", service_type);
+ exit(EXIT_FAILURE);
+ }
+
+ /*
+ * we expect the response to contain a list of records
+ * containing a ProtocolDescriptorList. Find the first
+ * one containing L2CAP and AVDTP >= 1.0, and extract
+ * the PSM.
+ */
+ rv = false;
+ while (!rv && sdp_get_seq(&rsp, &rec)) {
+ if (!sdp_get_attr(&rec, &attr, &value)
+ || attr != SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST)
+ continue;
+
+ sdp_get_alt(&value, &value); /* drop any alt header */
+ while (!rv && sdp_get_seq(&value, &pdl)) {
+ if (sdp_get_seq(&pdl, &seq) &&
+ sdp_match_uuid16(&seq, SDP_UUID_PROTOCOL_L2CAP) &&
+ sdp_get_uint(&seq, &psm) &&
+ sdp_get_seq(&pdl, &seq) &&
+ sdp_match_uuid16(&seq, SDP_UUID_PROTOCOL_AVDTP) &&
+ sdp_get_uint(&seq, &ver) && ver >= 0x0100)
+ rv = true;
+ }
+ }
+
+ sdp_close(myss);
+
+ if (!rv) {
+ log_err("%s query failed", service_type);
+ exit(EXIT_FAILURE);
+ }
+
+ l2cap_psm = (uint16_t)psm;
+ log_info("Found PSM %u for service %s", l2cap_psm, service_type);
+}
+
+static void
+exit_func(void)
+{
+ avdtpAbort(hc, hc, mySepInfo.sep);
+ avdtpClose(hc, hc, mySepInfo.sep);
+ close(sc);
+ close(hc);
+ sc = hc = -1;
+}
+
Index: src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.c
diff -u /dev/null src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.c:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.c Sat Jan 28 16:55:54 2017
@@ -0,0 +1,1083 @@
+/* $NetBSD: sbc_encode.c,v 1.1 2017/01/28 16:55:54 nat Exp $ */
+
+/*-
+ * Copyright (c) 2015 - 2016 Nathanial Sloss <[email protected]>
+ * All rights reserved.
+ *
+ * This software is dedicated to the memory of -
+ * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+ *
+ * Barry was a man who loved his music.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/param.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include <sbc_coeffs.h>
+#include <sbc_crc.h>
+#include "sbc_encode.h"
+
+static uint8_t make_crc(uint8_t);
+uint8_t Crc8(uint8_t, uint8_t *, size_t, ssize_t);
+static ssize_t make_frame(uint8_t *, int16_t *);
+static ssize_t parseFrame(uint8_t *, int16_t *);
+static void calc_scalefactors(int32_t samples[16][2][8]);
+static uint8_t calc_scalefactors_joint(int32_t sb_sample[16][2][8]);
+static size_t sbc_encode(int16_t *, int32_t *);
+static void calc_bitneed(void);
+static ssize_t get_bits(uint8_t *, int, uint32_t *);
+static ssize_t move_bits(uint8_t *, int, uint32_t);
+static ssize_t move_bits_crc(uint8_t *, int, uint32_t);
+static size_t sbc_decode(int32_t *, int16_t *);
+
+uint32_t scalefactor[2][8];
+int bits[2][8];
+int global_chan = 2;
+int global_bands = 8;
+int global_blocks = 16;
+int global_volume = 0;
+uint8_t global_bitpool = 32;
+uint8_t global_mode = MODE_STEREO;
+uint8_t global_alloc = ALLOC_LOUDNESS;
+uint8_t global_freq = FREQ_44_1K;
+uint8_t global_bands_config = BANDS_8;
+uint8_t global_block_config = BLOCKS_16;
+uint8_t join = 0;
+
+#define SYNCWORD 0x9c
+
+struct a2dp_frame_header {
+ uint8_t syncword;
+ uint8_t config;
+ uint8_t bitpool;
+ uint8_t crc;
+};
+
+struct a2dp_frame_header_joint {
+ uint8_t syncword;
+ uint8_t config;
+ uint8_t bitpool;
+ uint8_t crc;
+ uint8_t joint;
+};
+
+
+struct a2dp_frame_mono {
+ struct a2dp_frame_header header;
+ uint8_t scale[4];
+ uint8_t samples[256];
+};
+
+struct a2dp_frame_joint {
+ struct a2dp_frame_header_joint header;
+ uint8_t scale[8];
+ uint8_t samples[256];
+};
+
+struct a2dp_frame {
+ struct a2dp_frame_header header;
+ uint8_t scale[8];
+ uint8_t samples[256];
+};
+
+struct rtpHeader {
+ uint8_t id; /* Just random number. */
+ uint8_t id2;
+ uint8_t seqnumMSB; /* Packet seq. number most significant byte. */
+ uint8_t seqnumLSB;
+ uint8_t ts3; /* Timestamp most significant byte. */
+ uint8_t ts2;
+ uint8_t ts1;
+ uint8_t ts0; /* Timestamp least significant byte. */
+ uint8_t reserved3;
+ uint8_t reserved2;
+ uint8_t reserved1;
+ uint8_t reserved0; /* Reseverd least significant byte set to 1. */
+ uint8_t numFrames; /* Number of sbc frames in this packet. */
+};
+
+/* Loudness offset allocations. */
+int loudnessoffset8[4][8] = {
+ { -2, 0, 0, 0, 0, 0, 0, 1 },
+ { -3, 0, 0, 0, 0, 0, 1, 2 },
+ { -4, 0, 0, 0, 0, 0, 1, 2 },
+ { -4, 0, 0, 0, 0, 0, 1, 2 },
+};
+
+int loudnessoffset4[4][4] = {
+ { -1, 0, 0, 0 },
+ { -2, 0, 0, 1 },
+ { -2, 0, 0, 1 },
+ { -2, 0, 0, 1 }
+};
+
+u_int
+FLS(uint8_t x)
+{
+ u_int numset = 0;
+ while (x) {
+ if (x & 1)
+ break;
+ x >>= 1;
+ numset++;
+ }
+ return numset;
+}
+
+uint8_t
+calc_scalefactors_joint(int32_t sb_sample[16][2][8])
+{
+ int64_t sb_j[16][2];
+ uint32_t lz, x, y;
+ int32_t ax;
+ int block, sb;
+ unsigned int joint;
+
+ joint = 0;
+ for (sb = 0;sb < global_bands - 1;sb++) {
+ for (block = 0; block < global_blocks; block++) {
+ sb_j[block][0] = (sb_sample[block][0][sb]) +
+ (sb_sample[block][1][sb]);
+ sb_j[block][1] = (sb_sample[block][0][sb]) -
+ (sb_sample[block][1][sb]);
+ }
+
+ x = 1 << 15;
+ y = 1 << 15;
+ for (block = 0; block < global_blocks; block++) {
+ ax = abs((int32_t)(sb_j[block][0] / 2));
+ if (ax)
+ x |= (uint32_t)ax;
+ ax = abs((int32_t)(sb_j[block][1] / 2));
+ if (ax)
+ y |= (uint32_t)ax;
+ }
+
+ lz = 1;
+ while (!(x & __BIT(30))) {
+ lz++;
+ x <<= 1;
+ }
+ x = 16 - lz;
+
+ lz = 1;
+ while (!(y & __BIT(30))) {
+ lz++;
+ y <<= 1;
+ }
+ y = 16 - lz;
+
+ if ((scalefactor[0][sb] + scalefactor[1][sb]) > x + y) {
+ joint |= (unsigned int)(1 << (global_bands - sb - 1));
+ scalefactor[0][sb] = x;
+ scalefactor[1][sb] = y;
+ for (block = 0; block < global_blocks; block++) {
+ sb_sample[block][0][sb] = (int32_t)
+ (sb_j[block][0] / 2);
+ sb_sample[block][1][sb] = (int32_t)
+ (sb_j[block][1] / 2);
+ }
+ }
+ }
+
+ return (uint8_t)joint;
+}
+
+void
+calc_scalefactors(int32_t samples[16][2][8])
+{
+ uint32_t lz, x;
+ int32_t ax;
+ int ch, sb, block;
+
+ for (ch = 0; ch < global_chan; ch++) {
+ for (sb = 0; sb < global_bands; sb++) {
+ x = 1 << 16;
+ for (block = 0; block < global_blocks; block++) {
+ ax = abs((int32_t)samples[block][ch][sb]);
+ if (ax)
+ x |= (uint32_t)ax;
+ }
+
+ lz = 1;
+ while (!(x & __BIT(30))) {
+ lz++;
+ x <<= 1;
+ }
+ scalefactor[ch][sb] = 16 - lz;
+ }
+ }
+}
+
+void
+calc_bitneed(void)
+{
+ int32_t bitneed[2][8];
+ int32_t max_bitneed, bitcount;
+ int32_t slicecount, bitslice;
+ int32_t loudness;
+ int ch, sb,start_chan = 0;
+
+ if (global_mode == MODE_DUAL)
+ global_chan = 1;
+next_chan:
+ max_bitneed=0;
+ bitcount=0;
+ slicecount=0;
+
+ if (global_alloc == ALLOC_SNR) {
+ for (ch = start_chan;ch < global_chan;ch++) {
+ for (sb = 0;sb < global_bands;sb++) {
+ bitneed[ch][sb] = (int32_t)scalefactor[ch][sb];
+
+ if (bitneed[ch][sb] > max_bitneed)
+ max_bitneed = bitneed[ch][sb];
+ }
+ }
+ } else {
+ for (ch = start_chan;ch < global_chan;ch++) {
+ for (sb = 0;sb < global_bands;sb++) {
+ if (scalefactor[ch][sb] == 0)
+ bitneed[ch][sb] = -5;
+ else {
+ if (global_bands == 8) {
+ loudness = (int32_t)
+ ((int)scalefactor[ch][sb]
+ - loudnessoffset8
+ [3 - FLS(global_freq)][sb]);
+ } else {
+ loudness = (int32_t)
+ ((int)scalefactor[ch][sb]
+ - loudnessoffset4
+ [3 - FLS(global_freq)][sb]);
+ }
+ if (loudness > 0)
+ bitneed[ch][sb] = loudness / 2;
+ else
+ bitneed[ch][sb] = loudness;
+ }
+ if (bitneed[ch][sb] > max_bitneed)
+ max_bitneed = bitneed[ch][sb];
+ }
+ }
+ }
+
+ slicecount = bitcount = 0;
+ bitslice = max_bitneed+1;
+ do {
+ bitslice--;
+ bitcount += slicecount;
+ slicecount = 0;
+ for (ch = start_chan;ch < global_chan;ch++) {
+ for (sb = 0;sb < global_bands;sb++) {
+ if((bitneed[ch][sb] > bitslice + 1)&&
+ (bitneed[ch][sb] < bitslice + 16))
+ slicecount++;
+ else if(bitneed[ch][sb] == bitslice + 1)
+ slicecount += 2;
+ }
+ }
+ } while (bitcount + slicecount < global_bitpool);
+ if (bitcount + slicecount == global_bitpool) {
+ bitcount += slicecount;
+ bitslice--;
+ }
+
+ for (ch = start_chan;ch < global_chan;ch++) {
+ for (sb = 0;sb < global_bands;sb++) {
+ if (bitneed[ch][sb] < bitslice + 2)
+ bits[ch][sb] = 0;
+ else {
+ bits[ch][sb] = bitneed[ch][sb] - bitslice;
+ if (bits[ch][sb] > 16)
+ bits[ch][sb] = 16;
+ }
+ }
+ }
+
+ if (global_mode == MODE_DUAL)
+ ch = start_chan;
+ else
+ ch = 0;
+ sb = 0;
+ while (bitcount < global_bitpool && sb < global_bands) {
+ if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
+ bits[ch][sb]++;
+ bitcount++;
+ } else if ((bitneed[ch][sb] == bitslice + 1) &&
+ (global_bitpool > bitcount + 1)) {
+ bits[ch][sb] = 2;
+ bitcount += 2;
+ }
+ if (global_chan == 1 || start_chan == 1)
+ sb++;
+ else if (ch == 1) {
+ ch = 0;
+ sb++;
+ } else
+ ch = 1;
+ }
+
+ if (global_mode == MODE_DUAL)
+ ch = start_chan;
+ else
+ ch = 0;
+ sb = 0;
+ while (bitcount < global_bitpool && sb < global_bands) {
+ if (bits[ch][sb] < 16) {
+ bits[ch][sb]++;
+ bitcount++;
+ }
+ if (global_chan == 1 || start_chan == 1)
+ sb++;
+ else if (ch == 1) {
+ ch = 0;
+ sb++;
+ } else
+ ch = 1;
+ }
+
+ if (global_mode == MODE_DUAL && start_chan == 0) {
+ start_chan = 1;
+ global_chan = 2;
+ goto next_chan;
+ }
+}
+
+ssize_t
+get_bits(uint8_t *data, int numbits, uint32_t *sample)
+{
+ static uint64_t cache = 0;
+ static int cache_pos = 0;
+ uint64_t tmp_cache;
+ ssize_t written = 0;
+
+ while (cache_pos < numbits) {
+ cache <<= 8;
+ cache |= *data & 0xff;
+ data++;
+ written++;
+ cache_pos += 8;
+ }
+
+ if (numbits == 0) {
+ if (cache_pos >= 8) {
+ cache_pos -= 8;
+ tmp_cache = cache >> cache_pos;
+ *sample = (uint32_t)tmp_cache;
+ written--;
+ }
+ if (cache_pos) {
+ *sample = (uint32_t)cache;
+ written--;
+ }
+ cache = 0;
+ cache_pos = 0;
+ } else {
+ cache_pos -= numbits;
+ tmp_cache = cache & __BITS((uintmax_t)(numbits + cache_pos),
+ (uintmax_t)cache_pos);
+ cache &= ~tmp_cache;
+ tmp_cache >>= cache_pos;
+ *sample = (uint32_t)tmp_cache;
+ }
+ return written;
+}
+
+ssize_t
+move_bits(uint8_t *data, int numbits, uint32_t sample)
+{
+ static uint64_t cache = 0;
+ static int cache_pos = 0;
+ uint8_t tmp_cache;
+ ssize_t written = 0;
+
+ if (numbits == 0) {
+ while (cache_pos >= 8) {
+ cache_pos -= 8;
+ tmp_cache = (uint8_t)(cache >> cache_pos);
+ *data++ = tmp_cache;
+ written++;
+ }
+ if (cache_pos)
+ *data = (uint8_t)cache;
+ cache = 0;
+ cache_pos = 0;
+ } else {
+ cache_pos += numbits;
+ cache <<= numbits;
+ cache |= sample & __BITS((uintmax_t)numbits, 0);
+ while (cache_pos >= 8) {
+ cache_pos -= 8;
+ tmp_cache = (uint8_t)(cache >> cache_pos);
+ *data++ = tmp_cache;
+ written++;
+ }
+ }
+ return written;
+}
+
+ssize_t
+move_bits_crc(uint8_t *data, int numbits, uint32_t sample)
+{
+ static uint64_t cache = 0;
+ static int cache_pos = 0;
+ uint8_t tmp_cache;
+ ssize_t written = 0;
+
+ if (numbits > 8 || numbits < 0)
+ return 0;
+
+ if (numbits == 0) {
+ while (cache_pos >= 8) {
+ cache_pos -= 8;
+ tmp_cache = (uint8_t)(cache >> cache_pos);
+ *data++ = tmp_cache;
+ written++;
+ }
+ if (cache_pos)
+ *data = (uint8_t)cache;
+ cache = 0;
+ cache_pos = 0;
+ } else {
+ cache_pos += numbits;
+ cache <<= numbits;
+ cache |= sample & __BITS((uintmax_t)numbits, 0);
+ if (cache_pos >= 8) {
+ cache_pos -= 8;
+ tmp_cache = (uint8_t)(cache >> cache_pos);
+ *data = tmp_cache;
+ written++;
+ }
+ }
+ return written;
+}
+
+size_t
+sbc_encode(int16_t *input, int32_t *samples)
+{
+ int64_t delta[2][8], levels[2][8], S[80];
+ static int32_t L[80], R[80];
+ int32_t *X, Z[80], Y[80];
+ int32_t output[16][2][8];
+ int32_t audioout;
+ int16_t left[8], right[8], *data;
+ size_t numsamples;
+ int i, k, block, chan, sb;
+
+ for (block = 0;block < global_blocks; block++) {
+
+ k = 0;
+ for (i = 0;i < global_bands;i++) {
+ left[i] = input[k++];
+ if (global_chan == 2)
+ right[i] = input[k++];
+ }
+ input += k;
+
+ for (chan = 0; chan < global_chan; chan++) {
+ if (chan == 0) {
+ X = L;
+ data = left;
+ } else {
+ X = R;
+ data = right;
+ }
+
+ for (i = (global_bands * 10) - 1;i > global_bands -1;
+ i--)
+ X[i] = X[i - global_bands];
+ k = 0;
+ for (i = global_bands - 1; i >= 0; i--)
+ X[i] = (int16_t)le16toh(data[k++]);
+ for (i = 0; i < global_bands * 10; i++) {
+ if (global_bands == 8) {
+ Z[i] = (sbc_coeffs8[i] * (X[i] <<
+ global_volume));
+ } else {
+ Z[i] = (sbc_coeffs4[i] * (X[i] <<
+ global_volume));
+ }
+ }
+ for (i = 0; i < global_bands * 2; i++) {
+ Y[i] = 0;
+ for (k = 0;k < 5;k++)
+ Y[i] += Z[i + k * global_bands * 2];
+ }
+ for (i = 0; i < global_bands; i++) {
+ S[i] = 0;
+ for (k = 0; k < global_bands * 2; k++) {
+ if (global_bands == 8) {
+ S[i] += (int64_t)cosdata8[i][k]
+ * (int64_t)Y[k];
+ } else {
+ S[i] += (int64_t)cosdata4[i][k]
+ * (int64_t)Y[k];
+ }
+ }
+ output[block][chan][i] = (int32_t)(S[i] /
+ SIMULTI);
+ }
+ }
+ }
+
+ calc_scalefactors(output);
+ if (global_mode == MODE_JOINT)
+ join = calc_scalefactors_joint(output);
+
+ calc_bitneed();
+
+ for(chan = 0;chan < global_chan;chan++) {
+ for (sb = 0; sb < global_bands;sb++) {
+ levels[chan][sb] = ((1 << bits[chan][sb]) - 1) <<
+ (15 - scalefactor[chan][sb]);
+ delta[chan][sb] = 1 << (scalefactor[chan][sb] + 16);
+ }
+ }
+
+ numsamples = 0;
+ for (block = 0; block < global_blocks; block++) {
+ for (chan = 0; chan < global_chan; chan++) {
+ for (sb = 0; sb < global_bands; sb++) {
+ if (bits[chan][sb] == 0)
+ continue;
+
+ audioout = (int32_t)((levels[chan][sb] *
+ (delta[chan][sb] + (int32_t)output[block]
+ [chan][sb])) >> 32);
+
+ samples[numsamples++] = audioout;
+ }
+ }
+ }
+ return numsamples;
+}
+
+size_t
+sbc_decode(int32_t *samples, int16_t *pcm)
+{
+ static int64_t levels[2][8], delta[2][8];
+ static int64_t S[8], V[2][160], L[160], R[160];
+ int64_t *X;
+ static int64_t U[2][160], W[2][160];
+ int64_t audioout;
+ int chan, block, sb, position, i, k;
+ size_t numsamples;
+
+ for(chan = 0;chan < global_chan;chan++) {
+ for (sb = 0; sb < global_bands;sb++) {
+ levels[chan][sb] = (1 << bits[chan][sb]) - 1;
+ delta[chan][sb] = 1 << (scalefactor[chan][sb] + 1);
+ }
+ }
+
+ numsamples = 0;
+ for (block = 0; block < global_blocks; block++) {
+ for (chan = 0; chan < global_chan; chan++) {
+ for (sb = 0; sb < global_bands; sb++) {
+ if (bits[chan][sb] == 0)
+ audioout = 0;
+ else {
+ audioout = (((((samples[numsamples]
+ * 2) + 1) * delta[chan][sb])/
+ levels[chan][sb]) -
+ (1 * delta[chan][sb]));
+ }
+ samples[numsamples] = (int32_t)audioout;
+ numsamples++;
+ }
+ }
+ }
+
+ if (global_mode == MODE_JOINT) {
+ k = 0;
+ while (k < (global_blocks * global_bands * global_chan)) {
+ for (sb = 0; sb < global_bands;sb++) {
+ if (join & 1 << (global_bands - sb - 1)) {
+ audioout = samples[k];
+ samples[k] = (2 * samples[k]) + (2 *
+ samples[k + global_bands]);
+ samples[k + global_bands] =
+ (int32_t)(2 * audioout) - (2 *
+ samples[k + global_bands]);
+ samples[k] /= 2;
+ samples[k + global_bands] /= 2;
+ }
+ k++;
+ }
+ k += global_bands;
+ }
+ }
+
+
+ position = 0;
+ for (block = 0;block < global_blocks; block++) {
+ for (chan = 0; chan < global_chan; chan++) {
+ if (chan == 0)
+ X = L;
+ else
+ X = R;
+
+ for (i = 0; i < global_bands; i++)
+ S[i] = samples[position++];
+ for (i = ((global_bands * 20) - 1); i >= (global_bands
+ * 2); i--)
+ V[chan][i] = V[chan][i - (global_bands * 2)];
+
+ for (k = 0; k < global_bands * 2; k++) {
+ V[chan][k] = 0;
+ for (i = 0; i < global_bands; i++) {
+ if (global_bands == 8) {
+ V[chan][k] += cosdecdata8[i][k]
+ * S[i];
+ } else {
+ V[chan][k] += cosdecdata4[i][k]
+ * S[i];
+ }
+ }
+ V[chan][k] /= SIMULTI;
+ }
+
+ for (i = 0; i <= 4; i++) {
+ for (k = 0; k < global_bands; k++) {
+ U[chan][(i * global_bands * 2) + k] =
+ V[chan][(i * global_bands * 4) + k];
+ U[chan][(i * global_bands
+ * 2) + global_bands + k] =
+ V[chan][(i * global_bands * 4) +
+ (global_bands * 3) + k];
+ }
+ }
+
+ for (i = 0; i < global_bands * 10; i++) {
+ if (global_bands == 4) {
+ W[chan][i] = U[chan][i] *
+ (sbc_coeffs4[i] * -4);
+ } else if (global_bands == 8) {
+ W[chan][i] = U[chan][i] *
+ (sbc_coeffs8[i] * -8);
+ }
+ }
+
+ for (k = 0; k < global_bands; k++) {
+ int offset = k + (block * global_bands);
+ X[offset] = 0;
+ for (i = 0; i < 10; i++) {
+ X[offset] += W[chan][k + (i *
+ global_bands)];
+ }
+ X[offset] /= COEFFSMULTI;
+ }
+ }
+ }
+
+ k = 0;
+ i = 0;
+ while (k < (global_blocks * global_bands)) {
+ pcm[i++] = (int16_t)L[k];
+ if (global_chan == 2)
+ pcm[i++] = (int16_t)R[k];
+ k++;
+ }
+return numsamples;
+}
+
+uint8_t
+Crc8(uint8_t inCrc, uint8_t *inData, size_t numbits, ssize_t inBytes)
+{
+ uint8_t data;
+ int i;
+
+ for (i = 0; i < (int)inBytes; i++) {
+ data = inCrc ^ inData[i];
+
+ if (numbits == 8)
+ data = sbc_crc8[data];
+ else if (numbits == 4)
+ data = sbc_crc4[data];
+
+ inCrc = data;
+ }
+ return inCrc;
+}
+
+uint8_t
+make_crc(uint8_t config)
+{
+ uint8_t crc, data[11];
+ int i, j;
+ uint8_t *dataStart = data;
+ uint8_t *crcData = data;
+
+
+ crcData += move_bits_crc(crcData, 8, config);
+ crcData += move_bits_crc(crcData, 8, global_bitpool);
+ if (global_mode == MODE_JOINT) {
+ if (global_bands == 8)
+ crcData += move_bits_crc(crcData, 8, join);
+ else
+ crcData += move_bits_crc(crcData, 4, join);
+ }
+
+ for(i = 0; i < global_chan; i++) {
+ for (j = 0; j < global_bands; j++)
+ crcData += move_bits_crc(crcData, 4, scalefactor[i][j]);
+ }
+
+ crc = Crc8(0xf, data, 8, (crcData - dataStart));
+
+ if (global_mode == MODE_JOINT && global_bands == 4) {
+ move_bits_crc(crcData, 0, 0);
+ crc = Crc8(crc, crcData, 4, 1);
+ }
+
+ return crc;
+}
+
+ssize_t
+make_frame(uint8_t *frame, int16_t *input)
+{
+ static int32_t samples[256 * 2];
+ uint8_t config, crc;
+ int block, chan, sb, j, i;
+
+ uint8_t *frameStart = frame;
+
+ config = (uint8_t)(((3 - FLS(global_freq)) << 6) |
+ ((3 - FLS(global_block_config)) << 4) | ((3 - FLS(global_mode))
+ << 2) | ((FLS(global_alloc)) << 1) |
+ (1 - FLS(global_bands_config)));
+
+ sbc_encode(input,samples);
+
+ crc = make_crc(config);
+
+ frame += move_bits(frame, 8, SYNCWORD);
+ frame += move_bits(frame, 8, config);
+ frame += move_bits(frame, 8, global_bitpool);
+ frame += move_bits(frame, 8, crc);
+
+ if (global_mode == MODE_JOINT && global_bands == 8)
+ frame += move_bits(frame, 8, join);
+ else if (global_mode == MODE_JOINT && global_bands == 4)
+ frame += move_bits(frame, 4, join);
+
+ for(i = 0; i < global_chan; i++) {
+ for (j = 0; j < global_bands; j++)
+ frame += move_bits(frame, 4, scalefactor[i][j]);
+ }
+
+ i = 0;
+ for (block = 0; block < global_blocks; block++) {
+ for (chan = 0; chan < global_chan; chan++) {
+ for (sb = 0; sb < global_bands; sb++) {
+ if (bits[chan][sb] == 0)
+ continue;
+
+ frame += move_bits(frame, bits[chan][sb],
+ (uint32_t)samples[i++]);
+ }
+ }
+ }
+ frame += move_bits(frame, 0, 0);
+
+ return frame - frameStart;
+}
+
+ssize_t
+stream(int in, int outfd, uint8_t mode, uint8_t freq, uint8_t bands, uint8_t
+ blocks, uint8_t alloc_method, uint8_t bitpool, size_t mtu, int volume)
+{
+ struct rtpHeader myHeader;
+ uint8_t *whole, *frameData;
+ int16_t music[2048];
+ ssize_t len, mySize[16], offset, next_pkt;
+ ssize_t pkt_len;
+ size_t readsize, totalSize;
+ static size_t ts = 0;
+ static uint16_t seqnumber = 0;
+ int numpkts;
+
+ global_mode = mode;
+ global_bitpool = bitpool;
+ global_alloc = alloc_method;
+ global_freq = freq;
+ global_volume = volume;
+
+ global_bands_config = bands;
+ if (bands == BANDS_8)
+ global_bands = 8;
+ else
+ global_bands = 4;
+
+ if (blocks == BLOCKS_4)
+ global_blocks = 4;
+ else if (blocks == BLOCKS_8)
+ global_blocks = 8;
+ else if (blocks == BLOCKS_12)
+ global_blocks = 12;
+ else {
+ blocks = BLOCKS_16;
+ global_blocks = 16;
+ }
+
+ global_block_config = blocks;
+
+ global_chan = 2;
+ if (global_mode == MODE_MONO)
+ global_chan = 1;
+
+ memset(&myHeader, 0, sizeof(myHeader));
+ myHeader.id = 0x80; /* RTP v2 */
+ myHeader.id2 = 0x60; /* payload type 96. */
+ myHeader.seqnumMSB = (uint8_t)(seqnumber >> 8);
+ myHeader.seqnumLSB = (uint8_t)seqnumber;
+ myHeader.ts3 = (uint8_t)(ts >> 24);
+ myHeader.ts2 = (uint8_t)(ts >> 16);
+ myHeader.ts1 = (uint8_t)(ts >> 8);
+ myHeader.ts0 = (uint8_t)ts;
+ myHeader.reserved0 = 0x01;
+
+ totalSize = sizeof(myHeader);
+
+ frameData = malloc(mtu);
+ if (frameData == NULL)
+ return -1;
+
+ readsize = (size_t)((global_blocks * global_bands * global_chan) * 2);
+
+ numpkts = 0;
+ next_pkt = 0;
+ len = 0;
+ pkt_len = 80;
+ while (totalSize + ((size_t)pkt_len * 2) <= mtu) {
+
+ len = read(in, music, readsize);
+ if (len < (int)readsize)
+ break;
+
+ pkt_len = make_frame(frameData + next_pkt, music);
+
+ mySize[numpkts] = pkt_len;
+ next_pkt += pkt_len;
+ totalSize += (size_t)mySize[numpkts];
+ numpkts++;
+
+ if (numpkts > 13)
+ break;
+ }
+
+ if (len < (int)readsize) {
+ free(frameData);
+ return -1;
+ }
+
+ myHeader.numFrames = (uint8_t)numpkts;
+ whole = malloc(totalSize);
+ if (whole == NULL)
+ return -1;
+
+ memcpy(whole, &myHeader, sizeof(myHeader));
+ offset = sizeof(myHeader);
+
+ memcpy(whole + offset, frameData, (size_t)next_pkt);
+ free(frameData);
+
+send_again:
+ len = write(outfd, whole, totalSize);
+
+ if (len == -1 && errno == EAGAIN)
+ goto send_again;
+ free(whole);
+
+ seqnumber++;
+ ts += (readsize / ((size_t)global_chan * sizeof(int16_t))) *
+ (size_t)numpkts;
+
+ return len;
+}
+
+ssize_t
+recvstream(int in, int outfd)
+{
+ struct rtpHeader myHeader;
+ static uint8_t frameData[8192];
+ uint8_t *myFrame;
+ static int16_t music[4096];
+ size_t decsize, totalSize, offset;
+ int numpkts, cur_pkt;
+ ssize_t len, res, pkt_len, next_pkt;
+ static ssize_t readlen = 0;
+ totalSize = 0;
+
+ len = read(in, frameData + readlen, (size_t)(1000 - readlen));
+ readlen += len;
+ if (readlen <= 0) {
+ readlen = 0;
+ return -1;
+ }
+
+ if (readlen < (int)sizeof(myHeader)) {
+ readlen = 0;
+ return -1;
+ }
+
+ memcpy(&myHeader, frameData, sizeof(myHeader));
+ if (myHeader.id != 0x80) {
+ return -1;
+ }
+
+ numpkts = myHeader.numFrames;
+ if (numpkts < 1 || numpkts > 13) {
+ return -1;
+ }
+
+
+ myFrame = frameData + sizeof(myHeader);
+ next_pkt = 0;
+ pkt_len = 0;
+ cur_pkt = 0;
+ offset = 0;
+ while (cur_pkt < numpkts) {
+ pkt_len = parseFrame(myFrame + next_pkt, &music[offset]);
+ decsize = (size_t)(global_blocks * global_bands * global_chan);
+
+ next_pkt += pkt_len;
+ totalSize += 2 * decsize;
+ offset += decsize;
+ cur_pkt++;
+
+ if (cur_pkt > 13)
+ break;
+ }
+
+ res = (ssize_t)(sizeof(myHeader)) + next_pkt;
+ readlen -= res;
+ if (readlen > 0)
+ memcpy(frameData, frameData + res, (size_t)readlen);
+
+send_again:
+ len = write(outfd, music, totalSize);
+
+ if (len == -1 && errno == EAGAIN)
+ goto send_again;
+
+ return len;
+}
+
+ssize_t
+parseFrame(uint8_t *myFrame, int16_t *pcm)
+{
+ uint8_t mode, bitpool, alloc_method, freq, bands, config, myCrc, blkCrc;
+ int sb, chan, block, blocks, i, j;
+ int32_t samples[768];
+
+ uint8_t *myFrame_start = myFrame;
+ if (*myFrame++ != SYNCWORD)
+ return -1;
+
+ config = *myFrame++;
+ bitpool = *myFrame++;
+ myCrc = *myFrame++;
+
+ freq = (uint8_t)(1 << (3 - ((config & 0xc0) >> 6)));
+ blocks = 1 << (3 - ((config & 0x30) >> 4));
+ mode = (uint8_t)(1 << (3 - ((config & 0x0c) >> 2)));
+ alloc_method = (uint8_t)(1 << (((config & 0x02) >> 1)));
+ bands = (uint8_t)(1 << (1 - (config & 0x01)));
+
+ global_mode = mode;
+ global_bitpool = bitpool;
+ global_alloc = alloc_method;
+ global_freq = freq;
+
+ global_bands_config = bands;
+ if (bands == BANDS_8)
+ global_bands = 8;
+ else
+ global_bands = 4;
+
+ if (blocks == BLOCKS_4)
+ global_blocks = 4;
+ else if (blocks == BLOCKS_8)
+ global_blocks = 8;
+ else if (blocks == BLOCKS_12)
+ global_blocks = 12;
+ else {
+ blocks = BLOCKS_16;
+ global_blocks = 16;
+ }
+
+ global_block_config = (uint8_t)blocks;
+
+ global_chan = 2;
+ if (global_mode == MODE_MONO)
+ global_chan = 1;
+
+ if (global_mode == MODE_JOINT && global_bands == 8)
+ myFrame += get_bits(myFrame, 8, (uint32_t *)&join);
+ else if (global_mode == MODE_JOINT && global_bands == 4)
+ myFrame += get_bits(myFrame, 4, (uint32_t *)&join);
+ else
+ join = 0;
+
+ for(i = 0; i < global_chan; i++) {
+ for (j = 0; j < global_bands; j++) {
+ myFrame += get_bits(myFrame, 4,
+ (uint32_t *)&scalefactor[i][j]);
+ }
+ }
+
+ blkCrc = make_crc(config);
+ if (blkCrc != myCrc)
+ return 0;
+
+ calc_bitneed();
+ i = 0;
+ for (block = 0; block < global_blocks; block++) {
+ for (chan = 0; chan < global_chan; chan++) {
+ for (sb = 0; sb < global_bands; sb++) {
+ samples[i] = 0;
+ if (bits[chan][sb] == 0) {
+ i++;
+ continue;
+ }
+
+ myFrame += get_bits(myFrame, bits[chan][sb],
+ (uint32_t *)&samples[i++]);
+ }
+ }
+ }
+ myFrame += get_bits(myFrame, 0, (uint32_t *)&samples[i]);
+ sbc_decode(samples, pcm);
+
+ return myFrame - myFrame_start;
+}
Index: src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.h
diff -u /dev/null src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.h:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/bta2dpd/sbc_encode.h Sat Jan 28 16:55:54 2017
@@ -0,0 +1,75 @@
+/* $NetBSD: sbc_encode.h,v 1.1 2017/01/28 16:55:54 nat Exp $ */
+
+/*-
+ * Copyright (c) 2015 - 2016 Nathanial Sloss <[email protected]>
+ * All rights reserved.
+ *
+ * This software is dedicated to the memory of -
+ * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+ *
+ * Barry was a man who loved his music.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define SRC_SEP 1
+#define SNK_SEP 2
+
+#define SBC_CODEC_ID 0x0
+#define mediaTypeAudio 0x0
+
+#define CONFIG_ANY_FREQ 0xf7
+#define CONFIG_ANY_ALLOC 0xff
+#define MIN_BITPOOL 2
+#define DEFAULT_MAXBPOOL 253
+
+#define MODE_STEREO (1 << 1)
+#define MODE_JOINT (1 << 0)
+#define MODE_DUAL (1 << 2)
+#define MODE_MONO (1 << 3)
+#define MODE_ANY 0xf
+
+#define ALLOC_LOUDNESS (1 << 0)
+#define ALLOC_SNR (1 << 1)
+#define ALLOC_ANY 3
+
+#define FREQ_16K (1 << 3)
+#define FREQ_32K (1 << 2)
+#define FREQ_44_1K (1 << 1)
+#define FREQ_48K (1 << 0)
+#define FREQ_ANY 0xf
+
+
+#define BANDS_4 (1 << 1)
+#define BANDS_8 (1 << 0)
+#define BANDS_ANY 3
+
+#define BLOCKS_4 (1 << 3)
+#define BLOCKS_8 (1 << 2)
+#define BLOCKS_12 (1 << 1)
+#define BLOCKS_16 (1 << 0)
+#define BLOCKS_ANY 0xf
+
+u_int FLS(uint8_t);
+ssize_t stream(int, int, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t,
+ size_t, int);
+ssize_t recvstream(int, int);
Index: src/usr.sbin/bta2dpd/cosdata-gen/Makefile
diff -u /dev/null src/usr.sbin/bta2dpd/cosdata-gen/Makefile:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/cosdata-gen/Makefile Sat Jan 28 16:55:54 2017
@@ -0,0 +1,11 @@
+# $NetBSD: Makefile,v 1.1 2017/01/28 16:55:54 nat Exp $
+
+WARNS?= 6
+
+HOSTPROG= cosdata
+MAN=
+
+DPADD+= ${LIBMATH}
+LDADD+= -lm
+
+.include <bsd.hostprog.mk>
Index: src/usr.sbin/bta2dpd/cosdata-gen/cosdata.c
diff -u /dev/null src/usr.sbin/bta2dpd/cosdata-gen/cosdata.c:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/cosdata-gen/cosdata.c Sat Jan 28 16:55:54 2017
@@ -0,0 +1,202 @@
+/* $NetBSD: cosdata.c,v 1.1 2017/01/28 16:55:54 nat Exp $ */
+
+/*-
+ * Copyright (c) 2015 - 2016 Nathanial Sloss <[email protected]>
+ * All rights reserved.
+ *
+ * This software is dedicated to the memory of -
+ * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+ *
+ * Barry was a man who loved his music.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <math.h>
+#include <stdio.h>
+
+static const double sbc8_coeffs[] = {
+ 0.00000000e+00, 1.56575398e-04, 3.43256425e-04, 5.54620202e-04,
+ 8.23919506e-04, 1.13992507e-03, 1.47640169e-03, 1.78371725e-03,
+ 2.01182542e-03, 2.10371989e-03, 1.99454554e-03, 1.61656283e-03,
+ 9.02154502e-04, -1.78805361e-04, -1.64973098e-03, -3.49717454e-03,
+ 5.65949473e-03, 8.02941163e-03, 1.04584443e-02, 1.27472335e-02,
+ 1.46525263e-02, 1.59045603e-02, 1.62208471e-02, 1.53184106e-02,
+ 1.29371806e-02, 8.85757540e-03, 2.92408442e-03, -4.91578024e-03,
+ -1.46404076e-02, -2.61098752e-02, -3.90751381e-02, -5.31873032e-02,
+ 6.79989431e-02, 8.29847578e-02, 9.75753918e-02, 1.11196689e-01,
+ 1.23264548e-01, 1.33264415e-01, 1.40753505e-01, 1.45389847e-01,
+ 1.46955068e-01, 1.45389847e-01, 1.40753505e-01, 1.33264415e-01,
+ 1.23264548e-01, 1.11196689e-01, 9.75753918e-02, 8.29847578e-02,
+ -6.79989431e-02, -5.31873032e-02, -3.90751381e-02, -2.61098752e-02,
+ -1.46404076e-02, -4.91578024e-03, 2.92408442e-03, 8.85757540e-03,
+ 1.29371806e-02, 1.53184106e-02, 1.62208471e-02, 1.59045603e-02,
+ 1.46525263e-02, 1.27472335e-02, 1.04584443e-02, 8.02941163e-03,
+ -5.65949473e-03, -3.49717454e-03, -1.64973098e-03, -1.78805361e-04,
+ 9.02154502e-04, 1.61656283e-03, 1.99454554e-03, 2.10371989e-03,
+ 2.01182542e-03, 1.78371725e-03, 1.47640169e-03, 1.13992507e-03,
+ 8.23919506e-04, 5.54620202e-04, 3.43256425e-04, 1.56575398e-04,
+};
+
+static const double sbc4_coeffs[] = {
+ 0.00000000e+00, 5.36548976e-04, 1.49188357e-03, 2.73370904e-03,
+ 3.83720193e-03, 3.89205149e-03, 1.86581691e-03,-3.06012286e-03,
+ 1.09137620e-02, 2.04385087e-02, 2.88757392e-02, 3.21939290e-02,
+ 2.58767811e-02, 6.13245186e-03,-2.88217274e-02,-7.76463494e-02,
+ 1.35593274e-01, 1.94987841e-01, 2.46636662e-01, 2.81828203e-01,
+ 2.94315332e-01, 2.81828203e-01, 2.46636662e-01, 1.94987841e-01,
+ -1.35593274e-01,-7.76463494e-02,-2.88217274e-02, 6.13245186e-03,
+ 2.58767811e-02, 3.21939290e-02, 2.88757392e-02, 2.04385087e-02,
+ -1.09137620e-02,-3.06012286e-03, 1.86581691e-03, 3.89205149e-03,
+ 3.83720193e-03, 2.73370904e-03, 1.49188357e-03, 5.36548976e-04,
+};
+
+#define COEFFS_MULTI (float)(1 << 12)
+#define SI_MULTI (float)512
+
+int
+main (void)
+{
+ float S[8][16], val;
+ int i,k, count = 0;
+ int vali;
+
+ printf("/* sbc_coeffs.h - Automatically generated by cosdata.c. */\n"
+ "\n");
+
+ vali = SI_MULTI;
+ printf("#define SIMULTI\t%d\n\n", vali);
+ vali = COEFFS_MULTI;
+ printf("#define COEFFSMULTI\t%d\n\n", vali);
+
+ printf("static const int32_t sbc_coeffs8[] = {\n ");
+ for (k = 0;k < (int)__arraycount(sbc8_coeffs);k++) {
+ if (count % 8 == 0 && count != 0)
+ printf("\n ");
+ val = (float)(sbc8_coeffs[k] * COEFFS_MULTI);
+ vali = (int)roundf(val);
+ printf("%d, ",vali);
+ count++;
+ }
+ printf("\n};\n");
+
+ count = 0;
+ printf("static const int32_t sbc_coeffs4[] = {\n ");
+ for (k = 0;k < (int)__arraycount(sbc4_coeffs);k++) {
+ if (count % 8 == 0 && count != 0)
+ printf("\n ");
+ val = (float)(sbc4_coeffs[k] * COEFFS_MULTI);
+ vali = (int)roundf(val);
+ printf("%d, ",vali);
+ count++;
+ }
+ printf("\n};\n");
+
+ count = 0;
+ printf("static const int32_t cosdata8[8][16] = {\n ");
+ for (i = 0; i < 8; i++) {
+ for (k = 0;k < 16;k++) {
+ S[i][k] = cosf((float)((i+0.5)*(k-4)*(M_PI_4/2)));
+
+ if (count % 8 == 0 && count != 0)
+ printf("\n ");
+ if (k == 0)
+ printf("{ ");
+ val = S[i][k] * SI_MULTI;
+ vali = (int)roundf(val);
+ printf("%d, ",vali);
+
+ if (k == 15)
+ printf("},");
+ count++;
+ }
+ }
+ printf("\n};\n");
+
+ count = 0;
+ printf("static const int32_t cosdata4[4][8] = {\n ");
+ for (i = 0; i < 4; i++) {
+ for (k = 0;k < 8;k++) {
+ S[i][k] = cosf((float)((i+0.5)*(k-2)*(M_PI_4)));
+
+ if (count % 8 == 0 && count != 0)
+ printf("\n ");
+ if (k == 0)
+ printf("{ ");
+
+ val = S[i][k] * SI_MULTI;
+ vali = (int)roundf(val);
+ printf("%d, ",vali);
+
+ if (k == 7)
+ printf("},");
+ count++;
+ }
+ }
+ printf("\n};\n");
+
+ count = 0;
+ printf("static const int32_t cosdecdata8[8][16] = {\n ");
+ for (i = 0; i < 8; i++) {
+ for (k = 0;k < 16;k++) {
+ S[i][k] = cosf((float)((i+0.5)*(k+4)*(M_PI_4/2)));
+
+ if (count % 8 == 0 && count != 0)
+ printf("\n ");
+ if (k == 0)
+ printf("{ ");
+ val = S[i][k] * SI_MULTI;
+ vali = (int)roundf(val);
+ printf("%d, ",vali);
+
+ if (k == 15)
+ printf("},");
+ count++;
+ }
+ }
+ printf("\n};\n");
+
+ count = 0;
+ printf("static const int32_t cosdecdata4[4][8] = {\n ");
+ for (i = 0; i < 4; i++) {
+ for (k = 0;k < 8;k++) {
+ S[i][k] = cosf((float)((i+0.5)*(k+2)*(M_PI_4)));
+
+ if (count % 8 == 0 && count != 0)
+ printf("\n ");
+ if (k == 0)
+ printf("{ ");
+
+ val = S[i][k] * SI_MULTI;
+ vali = (int)roundf(val);
+ printf("%d, ",vali);
+
+ if (k == 7)
+ printf("},");
+ count++;
+ }
+ }
+ printf("\n};\n");
+
+ return 0;
+}
+
Index: src/usr.sbin/bta2dpd/sbc_crc-gen/Makefile
diff -u /dev/null src/usr.sbin/bta2dpd/sbc_crc-gen/Makefile:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/sbc_crc-gen/Makefile Sat Jan 28 16:55:54 2017
@@ -0,0 +1,8 @@
+# $NetBSD: Makefile,v 1.1 2017/01/28 16:55:54 nat Exp $
+
+WARNS?= 6
+
+HOSTPROG= sbc_crc
+MAN=
+
+.include <bsd.hostprog.mk>
Index: src/usr.sbin/bta2dpd/sbc_crc-gen/sbc_crc.c
diff -u /dev/null src/usr.sbin/bta2dpd/sbc_crc-gen/sbc_crc.c:1.1
--- /dev/null Sat Jan 28 16:55:55 2017
+++ src/usr.sbin/bta2dpd/sbc_crc-gen/sbc_crc.c Sat Jan 28 16:55:54 2017
@@ -0,0 +1,71 @@
+/* $NetBSD: sbc_crc.c,v 1.1 2017/01/28 16:55:54 nat Exp $ */
+
+/*-
+ * Copyright (c) 2015 Nathanial Sloss <[email protected]>
+ * All rights reserved.
+ *
+ * This software is dedicated to the memory of -
+ * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
+ *
+ * Barry was a man who loved his music.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sys/types.h>
+#include <stdio.h>
+
+int
+main()
+{
+ int j, i, k, numbits;
+ uint8_t data;
+
+ printf("/* sbc_crc.h - Automatically generated by sbc_crc.c. */\n\n");
+
+ /* The CRC-8 polynomial this uses is 0x11D */
+
+ numbits = 8;
+ for (k = 0; k < 2; k++) {
+ printf("static const uint8_t sbc_crc%d[256] = {\n ",numbits);
+ for (i = 0; i < 256; i++) {
+ data = i;
+
+ for (j = 0; j < numbits; j++) {
+ if (data & 0x80){
+ data = data << 1;
+ data = (data ^ 0x1d);
+ } else
+ data = data << 1;
+ }
+
+ if (i % 8 == 0 && i != 0)
+ printf("\n ");
+
+ printf("0x%02x, ",data);
+ }
+ printf("\n};\n\n");
+
+ numbits = 4;
+ }
+ return 0;
+}
