From 778a7f1f6e4fa25ab8d04b56aa7a383d8cb5df61 Mon Sep 17 00:00:00 2001 From: Milanka Ringwald Date: Wed, 19 Oct 2016 15:07:28 +0200 Subject: [PATCH] avdtp: extracted initiator and acceptor role --- test/avdtp/.gitignore | 4 + test/avdtp/Makefile | 5 +- test/avdtp/avdtp.h | 26 +- test/avdtp/avdtp_acceptor.c | 282 ++++++++++++++ test/avdtp/avdtp_acceptor.h | 64 ++++ test/avdtp/avdtp_initiator.c | 148 ++++++++ test/avdtp/avdtp_initiator.h | 63 ++++ test/avdtp/avdtp_sink.c | 694 ++++++----------------------------- test/avdtp/avdtp_sink.h | 75 ++-- test/avdtp/avdtp_test.c | 222 ++++++++++- test/avdtp/avdtp_util.c | 71 ++++ test/avdtp/avdtp_util.h | 64 ++++ 12 files changed, 1088 insertions(+), 630 deletions(-) create mode 100644 test/avdtp/avdtp_acceptor.c create mode 100644 test/avdtp/avdtp_acceptor.h create mode 100644 test/avdtp/avdtp_initiator.c create mode 100644 test/avdtp/avdtp_initiator.h create mode 100644 test/avdtp/avdtp_util.c create mode 100644 test/avdtp/avdtp_util.h diff --git a/test/avdtp/.gitignore b/test/avdtp/.gitignore index 24e5910d1..75e10b4a4 100644 --- a/test/avdtp/.gitignore +++ b/test/avdtp/.gitignore @@ -1 +1,5 @@ avdtp_test +portaudio_test +*.sbc +*.wav + diff --git a/test/avdtp/Makefile b/test/avdtp/Makefile index df4ed8cf4..07a0117e2 100644 --- a/test/avdtp/Makefile +++ b/test/avdtp/Makefile @@ -41,6 +41,7 @@ CFLAGS += -I${BTSTACK_ROOT}/port/libusb VPATH += ${BTSTACK_ROOT}/src VPATH += ${BTSTACK_ROOT}/src/classic VPATH += ${BTSTACK_ROOT}/platform/posix +VPATH += ${BTSTACK_ROOT}/platform/libusb VPATH += ${BTSTACK_ROOT}/port/libusb VPATH += ${BTSTACK_ROOT}/3rd-party/bluedroid/decoder/srce VPATH += ${BTSTACK_ROOT}/3rd-party/bluedroid/encoder/srce @@ -71,7 +72,7 @@ SBC_ENCODER_OBJ = $(SBC_ENCODER:.c=.o) all: ${AVDTP_TESTS} -avdtp_test: ${CORE_OBJ} ${COMMON_OBJ} ${SBC_DECODER_OBJ} ${SBC_ENCODER_OBJ} btstack_ring_buffer.o avdtp_sink.o avdtp_test.o +avdtp_test: ${CORE_OBJ} ${COMMON_OBJ} ${SBC_DECODER_OBJ} ${SBC_ENCODER_OBJ} btstack_ring_buffer.o avdtp_util.o avdtp_sink.o avdtp_acceptor.o avdtp_initiator.o avdtp_test.o ${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@ portaudio_test: btstack_util.o hci_dump.o wav_util.o btstack_ring_buffer.o portaudio_test.c @@ -80,4 +81,4 @@ portaudio_test: btstack_util.o hci_dump.o wav_util.o btstack_ring_buffer.o porta test: all clean: - rm -rf *.pyc *.o $(AVDTP_TESTS) *.dSYM *_test *.wav *.sbc \ No newline at end of file + rm -rf *.pyc *.o $(AVDTP_TESTS) *.dSYM *_test *.wav *.sbc ${BTSTACK_ROOT}/port/libusb/*.o diff --git a/test/avdtp/avdtp.h b/test/avdtp/avdtp.h index 4e216fdbe..2b7e25046 100644 --- a/test/avdtp/avdtp.h +++ b/test/avdtp/avdtp.h @@ -107,19 +107,19 @@ extern "C" { #define BAD_STATE 0x31 // Signal Identifier fields typedef enum { - AVDTP_DISCOVER = 0x01, - AVDTP_GET_CAPABILITIES, - AVDTP_SET_CONFIGURATION, - AVDTP_GET_CONFIGURATION, - AVDTP_RECONFIGURE, - AVDTP_OPEN, - AVDTP_START, - AVDTP_CLOSE, - AVDTP_SUSPEND, - AVDTP_ABORT, - AVDTP_SECURITY_CONTROL, - AVDTP_GET_ALL_CAPABILITIES, - AVDTP_DELAYREPORT + AVDTP_SI_DISCOVER = 0x01, + AVDTP_SI_GET_CAPABILITIES, + AVDTP_SI_SET_CONFIGURATION, + AVDTP_SI_GET_CONFIGURATION, + AVDTP_SI_RECONFIGURE, + AVDTP_SI_OPEN, + AVDTP_SI_START, + AVDTP_SI_CLOSE, + AVDTP_SI_SUSPEND, + AVDTP_SI_ABORT, + AVDTP_SI_SECURITY_CONTROL, + AVDTP_SI_GET_ALL_CAPABILITIES, + AVDTP_SI_DELAYREPORT } avdtp_signal_identifier_t; typedef enum { diff --git a/test/avdtp/avdtp_acceptor.c b/test/avdtp/avdtp_acceptor.c new file mode 100644 index 000000000..b92eb450f --- /dev/null +++ b/test/avdtp/avdtp_acceptor.c @@ -0,0 +1,282 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * 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. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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 MATTHIAS + * RINGWALD 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. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + + +#include +#include +#include +#include +#include + +#include "btstack.h" +#include "avdtp.h" +#include "avdtp_util.h" +#include "avdtp_acceptor.h" + +static int avdtp_pack_service_capabilities(uint8_t * buffer, int size, avdtp_capabilities_t caps, avdtp_service_category_t category){ + int i; + // pos = 0 reserved for length + int pos = 1; + switch(category){ + case AVDTP_MEDIA_TRANSPORT: + case AVDTP_REPORTING: + case AVDTP_DELAY_REPORTING: + break; + case AVDTP_RECOVERY: + buffer[pos++] = caps.recovery.recovery_type; // 0x01=RFC2733 + buffer[pos++] = caps.recovery.maximum_recovery_window_size; + buffer[pos++] = caps.recovery.maximum_number_media_packets; + break; + case AVDTP_CONTENT_PROTECTION: + buffer[pos++] = caps.content_protection.cp_type_lsb; + buffer[pos++] = caps.content_protection.cp_type_msb; + for (i = 0; irecovery.recovery_type = packet[pos++]; + caps->recovery.maximum_recovery_window_size = packet[pos++]; + caps->recovery.maximum_number_media_packets = packet[pos++]; + break; + case AVDTP_CONTENT_PROTECTION: + caps->content_protection.cp_type_lsb = packet[pos++]; + caps->content_protection.cp_type_msb = packet[pos++]; + caps->content_protection.cp_type_value_len = cap_len - 2; + printf_hexdump(packet+pos, caps->content_protection.cp_type_value_len); + pos += caps->content_protection.cp_type_value_len; + break; + case AVDTP_HEADER_COMPRESSION: + caps->header_compression.back_ch = packet[pos] >> 7; + caps->header_compression.media = packet[pos] >> 6; + caps->header_compression.recovery = packet[pos] >> 5; + pos++; + break; + case AVDTP_MULTIPLEXING: + caps->multiplexing_mode.fragmentation = packet[pos++] >> 7; + for (i=0; imultiplexing_mode.transport_identifiers_num; i++){ + caps->multiplexing_mode.transport_session_identifiers[i] = packet[pos++] >> 7; + caps->multiplexing_mode.tcid[i] = packet[pos++] >> 7; + // media, reporting. recovery + } + break; + case AVDTP_MEDIA_CODEC: + caps->media_codec.media_type = packet[pos++] >> 4; + caps->media_codec.media_codec_type = packet[pos++]; + caps->media_codec.media_codec_information_len = cap_len - 2; + printf_hexdump(packet+pos, caps->media_codec.media_codec_information_len); + pos += caps->media_codec.media_codec_information_len; + break; + } + registered_service_categories = store_bit16(registered_service_categories, category, 1); + } + return registered_service_categories; +} +static int avdtp_acceptor_send_seps_response(uint16_t cid, uint8_t transaction_label, avdtp_sep_t * seps, int seps_num){ + uint8_t command[2+2*MAX_NUM_SEPS]; + int pos = 0; + command[pos++] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); + command[pos++] = (uint8_t)AVDTP_SI_DISCOVER; + int i = 0; + for (i=0; iacceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; +} + +int avdtp_acceptor_stream_config_subsm_is_done(avdtp_sink_connection_t * connection){ + return connection->acceptor_config_state == AVDTP_ACCEPTOR_STREAM_CONFIG_DONE; +} + +int avdtp_acceptor_stream_config_subsm(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size){ + int request_to_send = 1; + avdtp_signaling_packet_header_t signaling_header; + avdtp_read_signaling_header(&signaling_header, packet, size); + + // printf("SIGNALING HEADER: tr_label %d, packet_type %d, msg_type %d, signal_identifier %02x\n", + // signaling_header.transaction_label, signaling_header.packet_type, signaling_header.message_type, signaling_header.signal_identifier); + + int i = 2; + avdtp_sep_t sep; + + connection->acceptor_transaction_label = signaling_header.transaction_label; + + switch (connection->acceptor_config_state){ + case AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE: + switch (signaling_header.signal_identifier){ + case AVDTP_SI_DISCOVER: + printf(" ACP -> AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS\n"); + connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS; + break; + case AVDTP_SI_GET_CAPABILITIES: + printf(" ACP -> AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES\n"); + connection->local_seid = packet[2] >> 2; + connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES; + break; + case AVDTP_SI_SET_CONFIGURATION: + printf(" ACP -> AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION\n"); + connection->local_seid = packet[2] >> 2; + sep.seid = packet[3] >> 2; + // find or add sep + for (i=0; iremote_seps_num; i++){ + if (connection->remote_seps[i].seid == sep.seid){ + connection->remote_sep_index = i; + } + } + sep.registered_service_categories = avdtp_unpack_service_capabilities(&sep.capabilities, packet+4, size-4); + connection->remote_seps[connection->remote_sep_index] = sep; + connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION; + break; + default: + printf(" ACP -> NOT IMPLEMENTED\n"); + request_to_send = 0; + break; + } + l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); + break; + case AVDTP_ACCEPTOR_STREAM_CONFIG_DONE: + request_to_send = 0; + printf(" ACP: AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION\n"); + break; + default: + printf(" ACP -> subsm NOT IMPLEMENTED\n"); + break; + } + return request_to_send; +} + +int avdtp_acceptor_stream_config_subsm_run_for_connection(avdtp_sink_connection_t *connection, avdtp_sep_t * local_seps, uint8_t local_seps_num){ + int sent = 1; + switch (connection->acceptor_config_state){ + case AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS: + printf(" AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS -> AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE\n"); + connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; + avdtp_acceptor_send_seps_response(connection->l2cap_signaling_cid, connection->acceptor_transaction_label, local_seps, local_seps_num); + break; + case AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES: + printf(" AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES -> AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE\n"); + connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; + avdtp_acceptor_send_capabilities_response(connection->l2cap_signaling_cid, connection->acceptor_transaction_label, local_seps[connection->local_seid]); + break; + case AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION: + printf(" AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION -> AVDTP_ACCEPTOR_STREAM_CONFIG_DONE\n"); + connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_DONE; + avdtp_acceptor_send_accept_response(connection->l2cap_signaling_cid, AVDTP_SI_SET_CONFIGURATION, connection->acceptor_transaction_label); + break; + default: + printf(" ACP -> run NOT IMPLEMENTED\n"); + sent = 0; + break; + } + return sent; +} + diff --git a/test/avdtp/avdtp_acceptor.h b/test/avdtp/avdtp_acceptor.h new file mode 100644 index 000000000..f62c0ee2d --- /dev/null +++ b/test/avdtp/avdtp_acceptor.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * 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. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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 MATTHIAS + * RINGWALD 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. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + +/* + * avdtp_acp.h + * + * Audio/Video Distribution Transport Protocol - Acceptor Role + * + */ + +#ifndef __AVDTP_ACP_H +#define __AVDTP_ACP_H + +#include + +#if defined __cplusplus +extern "C" { +#endif + +void avdtp_acceptor_stream_config_subsm_init(avdtp_sink_connection_t * connection); +int avdtp_acceptor_stream_config_subsm_is_done(avdtp_sink_connection_t * connection); +int avdtp_acceptor_stream_config_subsm(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size); +int avdtp_acceptor_stream_config_subsm_run_for_connection(avdtp_sink_connection_t *connection, avdtp_sep_t * local_seps, uint8_t local_seps_num); +int avdtp_acceptor_send_accept_response(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label); + +#if defined __cplusplus +} +#endif + +#endif // __AVDTP_ACP_H \ No newline at end of file diff --git a/test/avdtp/avdtp_initiator.c b/test/avdtp/avdtp_initiator.c new file mode 100644 index 000000000..f8123a341 --- /dev/null +++ b/test/avdtp/avdtp_initiator.c @@ -0,0 +1,148 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * 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. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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 MATTHIAS + * RINGWALD 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. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + + +#include +#include +#include +#include +#include + +#include "btstack.h" +#include "avdtp.h" +#include "avdtp_util.h" +#include "avdtp_initiator.h" + +static int avdtp_initiator_send_signaling_cmd(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ + uint8_t command[2]; + command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); + command[1] = (uint8_t)identifier; + return l2cap_send(cid, command, sizeof(command)); +} + +static int avdtp_initiator_send_get_capabilities_cmd(uint16_t cid, uint8_t sep_id){ + printf("TODO: avdtp_initiator_send_get_capabilities_cmd not implemented\n"); + return 0; +} + +void avdtp_initiator_stream_config_subsm_init(avdtp_sink_connection_t * connection){ + connection->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; +} + +int avdtp_initiator_stream_config_subsm_is_done(avdtp_sink_connection_t * connection){ + return connection->initiator_config_state == AVDTP_INITIATOR_STREAM_CONFIG_DONE; +} + +int avdtp_initiator_stream_config_subsm(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size){ + if (avdtp_initiator_stream_config_subsm_run_for_connection(connection)) return 1; + int i; + int responded = 1; + avdtp_sep_t sep; + + avdtp_signaling_packet_header_t signaling_header; + avdtp_read_signaling_header(&signaling_header, packet, size); + + switch (connection->initiator_config_state){ + case AVDTP_INITIATOR_W4_SEPS_DISCOVERED: + printf(" AVDTP_INITIATOR_W4_SEPS_DISCOVERED -> AVDTP_INITIATOR_W2_GET_CAPABILITIES\n"); + + if (signaling_header.transaction_label != connection->initiator_transaction_label){ + printf("unexpected transaction label, got %d, expected %d\n", signaling_header.transaction_label, connection->initiator_transaction_label); + return 0; + } + if (signaling_header.signal_identifier != AVDTP_SI_DISCOVER) { + printf("unexpected signal identifier ...\n"); + return 0; + } + if (signaling_header.message_type != AVDTP_RESPONSE_ACCEPT_MSG){ + printf("request rejected...\n"); + return 0; + } + if (size == 3){ + printf("ERROR code %02x\n", packet[2]); + return 0; + } + for (i = 2; i> 2; + if (sep.seid < 0x01 || sep.seid > 0x3E){ + printf("invalid sep id\n"); + return 0; + } + sep.in_use = (packet[i] >> 1) & 0x01; + sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4); + sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01); + connection->remote_seps[connection->remote_seps_num++] = sep; + // printf("found sep: seid %u, in_use %d, media type %d, sep type %d (1-SNK)\n", + // sep.seid, sep.in_use, sep.media_type, sep.type); + } + connection->initiator_config_state = AVDTP_INITIATOR_W2_GET_CAPABILITIES; + connection->initiator_transaction_label++; + l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); + responded = 1; + break; + case AVDTP_INITIATOR_W4_CAPABILITIES: + printf(" AVDTP_INITIATOR_W4_CAPABILITIES -> NOT IMPLEMENTED\n"); + + default: + printf(" INT : NOT IMPLEMENTED sig. ID %02x\n", signaling_header.signal_identifier); + //printf_hexdump( packet, size ); + responded = 0; + break; + } + return responded; +} + +int avdtp_initiator_stream_config_subsm_run_for_connection(avdtp_sink_connection_t *connection){ + int sent = 1; + switch (connection->initiator_config_state){ + case AVDTP_INITIATOR_STREAM_CONFIG_IDLE: + case AVDTP_INITIATOR_W2_DISCOVER_SEPS: + printf(" AVDTP_INITIATOR_STREAM_CONFIG_IDLE | AVDTP_INITIATOR_W2_DISCOVER_SEPS -> AVDTP_INITIATOR_W4_SEPS_DISCOVERED\n"); + connection->initiator_config_state = AVDTP_INITIATOR_W4_SEPS_DISCOVERED; + avdtp_initiator_send_signaling_cmd(connection->l2cap_signaling_cid, AVDTP_SI_DISCOVER, connection->initiator_transaction_label); + break; + case AVDTP_INITIATOR_W2_GET_CAPABILITIES: + printf(" AVDTP_INitiator_W2_GET_CAPABILITIES -> AVDTP_INitiator_W4_CAPABILITIES\n"); + connection->initiator_config_state = AVDTP_INITIATOR_W4_CAPABILITIES; + avdtp_initiator_send_get_capabilities_cmd(connection->l2cap_signaling_cid, 0); + break; + default: + sent = 0; + break; + } + return sent; +} diff --git a/test/avdtp/avdtp_initiator.h b/test/avdtp/avdtp_initiator.h new file mode 100644 index 000000000..d7e222a90 --- /dev/null +++ b/test/avdtp/avdtp_initiator.h @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * 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. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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 MATTHIAS + * RINGWALD 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. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + +/* + * avdtp_int.h + * + * Audio/Video Distribution Transport Protocol - Initiator Role + * + */ + +#ifndef __AVDTP_INT_H +#define __AVDTP_INT_H + +#include + +#if defined __cplusplus +extern "C" { +#endif + +void avdtp_initiator_stream_config_subsm_init(avdtp_sink_connection_t * connection); +int avdtp_initiator_stream_config_subsm_is_done(avdtp_sink_connection_t * connection); +int avdtp_initiator_stream_config_subsm(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size); +int avdtp_initiator_stream_config_subsm_run_for_connection(avdtp_sink_connection_t *connection); + +#if defined __cplusplus +} +#endif + +#endif // __AVDTP_INT_H \ No newline at end of file diff --git a/test/avdtp/avdtp_sink.c b/test/avdtp/avdtp_sink.c index a51f3ad2c..443d142b2 100644 --- a/test/avdtp/avdtp_sink.c +++ b/test/avdtp/avdtp_sink.c @@ -45,92 +45,9 @@ #include "btstack.h" #include "avdtp.h" #include "avdtp_sink.h" - -#include "btstack_sbc.h" -#include "wav_util.h" - -#ifdef HAVE_PORTAUDIO -#include -#include "btstack_ring_buffer.h" -#endif - -#define NUM_CHANNELS 2 -#define SAMPLE_RATE 44100 - -#ifdef HAVE_POSIX_FILE_IO -#define STORE_SBC_TO_SBC_FILE -#define STORE_SBC_TO_WAV_FILE -#endif - -#ifdef HAVE_PORTAUDIO -#define PA_SAMPLE_TYPE paInt16 -#define FRAMES_PER_BUFFER 128 -#define BYTES_PER_FRAME (2*NUM_CHANNELS) -#define PREBUFFER_MS 150 -#define PREBUFFER_BYTES (PREBUFFER_MS*SAMPLE_RATE/1000*BYTES_PER_FRAME) -static uint8_t ring_buffer_storage[2*PREBUFFER_BYTES]; -static btstack_ring_buffer_t ring_buffer; - -static PaStream * stream; -static uint8_t pa_stream_started = 0; - -static int patestCallback( const void *inputBuffer, void *outputBuffer, - unsigned long framesPerBuffer, - const PaStreamCallbackTimeInfo* timeInfo, - PaStreamCallbackFlags statusFlags, - void *userData ) { - (void) timeInfo; /* Prevent unused variable warnings. */ - (void) statusFlags; - (void) inputBuffer; - - uint32_t bytes_read = 0; - int bytes_per_buffer = framesPerBuffer * BYTES_PER_FRAME; - - if (btstack_ring_buffer_bytes_available(&ring_buffer) >= bytes_per_buffer){ - btstack_ring_buffer_read(&ring_buffer, outputBuffer, bytes_per_buffer, &bytes_read); - } else { - printf("NOT ENOUGH DATA!\n"); - memset(outputBuffer, 0, bytes_per_buffer); - } - printf("bytes avail after read: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); - return 0; -} -#endif - -#ifdef STORE_SBC_TO_WAV_FILE -// store sbc as wav: -static btstack_sbc_decoder_state_t state; -static int total_num_samples = 0; -static int frame_count = 0; -static char * wav_filename = "avdtp_sink.wav"; -static btstack_sbc_mode_t mode = SBC_MODE_STANDARD; - -static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ - wav_writer_write_int16(num_samples*num_channels, data); - total_num_samples+=num_samples*num_channels; - frame_count++; - -#ifdef HAVE_PORTAUDIO - if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= PREBUFFER_BYTES){ - /* -- start stream -- */ - PaError err = Pa_StartStream(stream); - if (err != paNoError){ - printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - pa_stream_started = 1; - } - btstack_ring_buffer_write(&ring_buffer, (uint8_t *)data, num_samples*num_channels*2); - printf("bytes avail after write: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); -#endif -} -#endif - -#ifdef STORE_SBC_TO_SBC_FILE -static FILE * sbc_file; -static char * sbc_filename = "avdtp_sink.sbc"; -#endif - +#include "avdtp_util.h" +#include "avdtp_initiator.h" +#include "avdtp_acceptor.h" static const char * default_avdtp_sink_service_name = "BTstack AVDTP Sink Service"; static const char * default_avdtp_sink_service_provider_name = "BTstack AVDTP Sink Service Provider"; @@ -144,6 +61,7 @@ static btstack_packet_handler_t avdtp_sink_callback; static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); static void avdtp_sink_run_for_connection(avdtp_sink_connection_t *connection); +static void (*handle_media_data)(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size); void a2dp_sink_create_sdp_record(uint8_t * service, uint32_t service_record_handle, uint16_t supported_features, const char * service_name, const char * service_provider_name){ uint8_t* attribute; @@ -242,18 +160,6 @@ uint8_t avdtp_sink_register_stream_end_point(avdtp_sep_type_t sep_type, avdtp_me return seid; } -static int get_bit16(uint16_t bitmap, int position){ - return (bitmap >> position) & 1; -} - -static uint8_t store_bit16(uint16_t bitmap, int position, uint8_t value){ - if (value){ - bitmap |= 1 << position; - } else { - bitmap &= ~ (1 << position); - } - return bitmap; -} void avdtp_sink_register_media_transport_category(uint8_t seid){ if (seid >= local_seps_num){ @@ -348,19 +254,18 @@ void avdtp_sink_register_multiplexing_category(uint8_t seid, uint8_t fragmentati // } -static void audio_sink_generate_next_transaction_label(avdtp_sink_connection_t * connection){ - connection->local_transaction_label++; -} static avdtp_sink_connection_t * create_avdtp_sink_connection_context(bd_addr_t bd_addr){ - avdtp_sink_connection_t * avdtp_connection = btstack_memory_avdtp_sink_connection_get(); - if (!avdtp_connection) return NULL; - memset(avdtp_connection,0, sizeof(avdtp_sink_connection_t)); - memcpy(avdtp_connection->remote_addr, bd_addr, 6); - avdtp_connection->local_state = AVDTP_SINK_IDLE; + avdtp_sink_connection_t * connection = btstack_memory_avdtp_sink_connection_get(); + if (!connection) return NULL; + memset(connection,0, sizeof(avdtp_sink_connection_t)); + memcpy(connection->remote_addr, bd_addr, 6); + connection->avdtp_state = AVDTP_IDLE; + connection->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; + connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; - btstack_linked_list_add(&avdtp_sink_connections, (btstack_linked_item_t*)avdtp_connection); - return avdtp_connection; + btstack_linked_list_add(&avdtp_sink_connections, (btstack_linked_item_t*)connection); + return connection; } static avdtp_sink_connection_t * get_avdtp_sink_connection_context_for_bd_addr(bd_addr_t bd_addr){ @@ -397,364 +302,65 @@ static void avdtp_sink_remove_connection_context(avdtp_sink_connection_t * conne btstack_linked_list_remove(&avdtp_sink_connections, (btstack_linked_item_t*) connection); } -static inline uint8_t avdtp_header(uint8_t tr_label, avdtp_packet_type_t packet_type, avdtp_message_type_t msg_type){ - return (tr_label<<4) | ((uint8_t)packet_type<<2) | (uint8_t)msg_type; -} - -static int avdtp_sink_send_signaling_cmd(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ - uint8_t command[2]; - command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); - command[1] = (uint8_t)identifier; - return l2cap_send(cid, command, sizeof(command)); -} - -static int avdtp_sink_send_seps_response(uint16_t cid, uint8_t transaction_label, avdtp_sep_t * seps, int seps_num){ - uint8_t command[2+2*MAX_NUM_SEPS]; - int pos = 0; - command[pos++] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); - command[pos++] = (uint8_t)AVDTP_DISCOVER; - int i = 0; - for (i=0; irecovery.recovery_type = packet[pos++]; - caps->recovery.maximum_recovery_window_size = packet[pos++]; - caps->recovery.maximum_number_media_packets = packet[pos++]; - break; - case AVDTP_CONTENT_PROTECTION: - caps->content_protection.cp_type_lsb = packet[pos++]; - caps->content_protection.cp_type_msb = packet[pos++]; - caps->content_protection.cp_type_value_len = cap_len - 2; - printf_hexdump(packet+pos, caps->content_protection.cp_type_value_len); - pos += caps->content_protection.cp_type_value_len; - break; - case AVDTP_HEADER_COMPRESSION: - caps->header_compression.back_ch = packet[pos] >> 7; - caps->header_compression.media = packet[pos] >> 6; - caps->header_compression.recovery = packet[pos] >> 5; - pos++; - break; - case AVDTP_MULTIPLEXING: - caps->multiplexing_mode.fragmentation = packet[pos++] >> 7; - for (i=0; imultiplexing_mode.transport_identifiers_num; i++){ - caps->multiplexing_mode.transport_session_identifiers[i] = packet[pos++] >> 7; - caps->multiplexing_mode.tcid[i] = packet[pos++] >> 7; - // media, reporting. recovery - } - break; - case AVDTP_MEDIA_CODEC: - caps->media_codec.media_type = packet[pos++] >> 4; - caps->media_codec.media_codec_type = packet[pos++]; - caps->media_codec.media_codec_information_len = cap_len - 2; - printf_hexdump(packet+pos, caps->media_codec.media_codec_information_len); - pos += caps->media_codec.media_codec_information_len; - break; - } - registered_service_categories = store_bit16(registered_service_categories, category, 1); - } - return registered_service_categories; -} - -static int avdtp_sink_send_capabilities_response(uint16_t cid, uint8_t transaction_label, avdtp_sep_t sep){ - uint8_t command[100]; - int pos = 0; - command[pos++] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); - command[pos++] = (uint8_t)AVDTP_GET_CAPABILITIES; - int i = 0; - for (i = 1; i < 9; i++){ - if (get_bit16(sep.registered_service_categories, i)){ - // service category - command[pos++] = i; - pos += avdtp_pack_service_capabilities(command+pos, sizeof(command)-pos, sep.capabilities, (avdtp_service_category_t)i); - } - } - return l2cap_send(cid, command, pos); -} - -static void handle_l2cap_media_data_packet(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size){ - int pos = 0; - - avdtp_media_packet_header_t media_header; - media_header.version = packet[pos] & 0x03; - media_header.padding = get_bit16(packet[pos],2); - media_header.extension = get_bit16(packet[pos],3); - media_header.csrc_count = (packet[pos] >> 4) & 0x0F; - - pos++; - - media_header.marker = get_bit16(packet[pos],0); - media_header.payload_type = (packet[pos] >> 1) & 0x7F; - pos++; - - media_header.sequence_number = big_endian_read_16(packet, pos); - pos+=2; - - media_header.timestamp = big_endian_read_32(packet, pos); - pos+=4; - - media_header.synchronization_source = big_endian_read_32(packet, pos); - pos+=4; - - // TODO: read csrc list - - // printf_hexdump( packet, pos ); - // printf("MEDIA HEADER: %u timestamp, version %u, padding %u, extension %u, csrc_count %u\n", - // media_header.timestamp, media_header.version, media_header.padding, media_header.extension, media_header.csrc_count); - // printf("MEDIA HEADER: marker %02x, payload_type %02x, sequence_number %u, synchronization_source %u\n", - // media_header.marker, media_header.payload_type, media_header.sequence_number, media_header.synchronization_source); - - avdtp_sbc_codec_header_t sbc_header; - sbc_header.fragmentation = get_bit16(packet[pos], 7); - sbc_header.starting_packet = get_bit16(packet[pos], 6); - sbc_header.last_packet = get_bit16(packet[pos], 5); - sbc_header.num_frames = packet[pos] & 0x0f; - pos++; - - // printf("SBC HEADER: num_frames %u, fragmented %u, start %u, stop %u\n", sbc_header.num_frames, sbc_header.fragmentation, sbc_header.starting_packet, sbc_header.last_packet); - // printf_hexdump( packet+pos, size-pos ); -#ifdef STORE_SBC_TO_WAV_FILE - btstack_sbc_decoder_process_data(&state, 0, packet+pos, size-pos); -#endif - -#ifdef STORE_SBC_TO_SBC_FILE - fwrite(packet+pos, size-pos, 1, sbc_file); -#endif -} static void handle_l2cap_signaling_data_packet(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size){ if (size < 2) { log_error("l2cap data packet too small"); return; } + avdtp_signaling_packet_header_t signaling_header; + avdtp_read_signaling_header(&signaling_header, packet, size); - signaling_header.transaction_label = packet[0] >> 4; - signaling_header.packet_type = (avdtp_packet_type_t)((packet[0] >> 2) & 0x03); - signaling_header.message_type = (avdtp_message_type_t) (packet[0] & 0x03); - signaling_header.signal_identifier = packet[1] & 0x3f; + // printf("SIGNALING HEADER: tr_label %d, packet_type %d, msg_type %d, signal_identifier %02x\n", + // signaling_header.transaction_label, signaling_header.packet_type, signaling_header.message_type, signaling_header.signal_identifier); - printf("SIGNALING HEADER: tr_label %d, packet_type %d, msg_type %d, signal_identifier %02x\n", - signaling_header.transaction_label, signaling_header.packet_type, signaling_header.message_type, signaling_header.signal_identifier); - - int i = 2; - avdtp_sep_t sep; - - if (signaling_header.message_type == AVDTP_CMD_MSG){ - // remote - printf("command from remote: "); - connection->remote_transaction_label = signaling_header.transaction_label; - switch (signaling_header.signal_identifier){ - case AVDTP_DISCOVER: - if (connection->remote_state != AVDTP_REMOTE_IDLE) return; - printf("AVDTP_DISCOVER\n"); - connection->remote_state = AVDTP_REMOTE_W2_ANSWER_DISCOVER_SEPS; - l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); - break; - case AVDTP_GET_CAPABILITIES: - if (connection->remote_state != AVDTP_REMOTE_IDLE) return; - printf("AVDTP_GET_CAPABILITIES\n"); - connection->local_seid = packet[2] >> 2; - connection->remote_state = AVDTP_REMOTE_W2_ANSWER_GET_CAPABILITIES; - l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); - break; - case AVDTP_SET_CONFIGURATION: - if (connection->remote_state != AVDTP_REMOTE_IDLE) return; - printf("AVDTP_SET_CONFIGURATION\n"); - connection->local_seid = packet[2] >> 2; - sep.seid = packet[3] >> 2; - // find or add sep - for (i=0; iremote_seps_num; i++){ - if (connection->remote_seps[i].seid == sep.seid){ - connection->remote_sep_index = i; - } - } - sep.registered_service_categories = avdtp_unpack_service_capabilities(&sep.capabilities, packet+4, size-4); - connection->remote_seps[connection->remote_sep_index] = sep; - connection->remote_state = AVDTP_REMOTE_W2_ANSWER_SET_CONFIGURATION; - l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); - break; - case AVDTP_OPEN: - if (connection->remote_state != AVDTP_REMOTE_IDLE) return; - printf("AVDTP_OPEN\n"); - connection->local_seid = packet[2] >> 2; - connection->remote_state = AVDTP_REMOTE_W2_ANSWER_OPEN_STREAM; - l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); - break; - case AVDTP_START: - if (connection->remote_state != AVDTP_REMOTE_OPEN) return; - printf("AVDTP_START\n"); - connection->local_seid = packet[2] >> 2; - connection->remote_state = AVDTP_REMOTE_W2_ANSWER_START_SINGLE_STREAM; - l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); - break; - case AVDTP_CLOSE: - - break; - - default:{ - printf("NOT IMPLEMENTED signal_identifier %d\n", signaling_header.signal_identifier); - printf_hexdump( packet, size ); - #ifdef STORE_SBC_TO_WAV_FILE - printf(" Close wav writer\n"); - wav_writer_close(); - int total_frames_nr = state.good_frames_nr + state.bad_frames_nr + state.zero_frames_nr; - - printf("\nDecoding done. Processed totaly %d frames:\n - %d good\n - %d bad\n - %d zero frames\n", total_frames_nr, state.good_frames_nr, state.bad_frames_nr, state.zero_frames_nr); - printf("Write %d frames to wav file: %s\n\n", frame_count, wav_filename); -#endif -#ifdef STORE_SBC_TO_SBC_FILE - fclose(sbc_file); -#endif - -#ifdef HAVE_PORTAUDIO - PaError err = Pa_StopStream(stream); - if (err != paNoError){ - printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - pa_stream_started = 0; - err = Pa_CloseStream(stream); - if (err != paNoError){ - printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - - err = Pa_Terminate(); - if (err != paNoError){ - printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); - return; - } -#endif - break; - } + if (connection->avdtp_state == AVDTP_CONFIGURATION_SUBSTATEMACHINE){ + if (avdtp_initiator_stream_config_subsm_is_done(connection) || avdtp_acceptor_stream_config_subsm_is_done(connection)){ + printf("AVDTP_CONFIGURATION_SUBSTATEMACHINE -> AVDTP_CONFIGURED\n"); + connection->avdtp_state = AVDTP_CONFIGURED; } - return; } - printf("send to remote: "); - switch (connection->local_state){ - case AVDTP_SINK_W4_SEPS_DISCOVERED: - printf("AVDTP_SINK_W4_SEPS_DISCOVERED\n"); - if (signaling_header.transaction_label != connection->local_transaction_label){ - log_error("unexpected transaction label, got %d, expected %d", signaling_header.transaction_label, connection->local_transaction_label); - return; + switch (connection->avdtp_state){ + case AVDTP_CONFIGURATION_SUBSTATEMACHINE: + if (signaling_header.message_type == AVDTP_CMD_MSG){ + avdtp_acceptor_stream_config_subsm(connection, packet, size); + break; + } + avdtp_initiator_stream_config_subsm(connection, packet, size); + break; + case AVDTP_CONFIGURED: + switch (signaling_header.signal_identifier){ + case AVDTP_SI_OPEN: + printf("AVDTP_CONFIGURED -> AVDTP_W2_ANSWER_OPEN_STREAM %d\n", signaling_header.transaction_label); + connection->local_seid = packet[2] >> 2; + connection->avdtp_state = AVDTP_W2_ANSWER_OPEN_STREAM; + connection->acceptor_transaction_label = signaling_header.transaction_label; + l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); + break; + default: + printf("AVDTP_CONFIGURED -> NOT IMPLEMENTED signal_identifier %d, state %d\n", signaling_header.signal_identifier, connection->avdtp_state); + break; } - if (signaling_header.signal_identifier != AVDTP_DISCOVER) { - log_error("unexpected signal identifier ..."); - return; + break; + case AVDTP_OPEN: + switch (signaling_header.signal_identifier){ + case AVDTP_SI_START: + printf("AVDTP_OPEN -> AVDTP_W2_ANSWER_START_SINGLE_STREA %d\n", signaling_header.transaction_label); + connection->local_seid = packet[2] >> 2; + connection->avdtp_state = AVDTP_W2_ANSWER_START_SINGLE_STREAM; + connection->acceptor_transaction_label = signaling_header.transaction_label; + l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); + break; + default: + printf("AVDTP_OPEN -> NOT IMPLEMENTED signal_identifier %d\n", signaling_header.signal_identifier); + break; } - if (signaling_header.message_type != AVDTP_RESPONSE_ACCEPT_MSG){ - log_error("request rejected..."); - return; - } - if (size == 3){ - printf("ERROR code %02x\n", packet[2]); - return; - } - for (i = 2; i> 2; - if (sep.seid < 0x01 || sep.seid > 0x3E){ - log_error("invalid sep id"); - return; - } - sep.in_use = (packet[i] >> 1) & 0x01; - sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4); - sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01); - connection->remote_seps[connection->remote_seps_num++] = sep; - printf("found sep: seid %u, in_use %d, media type %d, sep type %d (1-SNK)\n", - sep.seid, sep.in_use, sep.media_type, sep.type); - } - // connection->local_state = AVDTP_SINK_W2_GET_CAPABILITIES; - // connection->local_transaction_label++; - // l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); break; default: - printf("NOT IMPLEMENTED\n"); - printf_hexdump( packet, size ); + printf("handle_l2cap_signaling_data_packet: state %d -> NOT IMPLEMENTED signal_identifier %d\n", connection->avdtp_state, signaling_header.signal_identifier); + // printf_hexdump( packet, size ); break; } } @@ -777,7 +383,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe if (channel == connection->l2cap_signaling_cid){ handle_l2cap_signaling_data_packet(connection, packet, size); } else if (channel == connection->l2cap_media_cid){ - handle_l2cap_media_data_packet(connection, packet, size); + (*handle_media_data)(connection, packet, size); } else if (channel == connection->l2cap_reporting_cid){ // TODO } else { @@ -789,7 +395,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe switch (hci_event_packet_get_type(packet)) { case L2CAP_EVENT_INCOMING_CONNECTION: l2cap_event_incoming_connection_get_address(packet, event_addr); - connection = get_avdtp_sink_connection_context_for_bd_addr(event_addr); + connection = create_avdtp_sink_connection_context(event_addr); if (!connection){ log_error("avdtp packet handler L2CAP_EVENT_INCOMING_CONNECTION: connection for bd address %s not found", bd_addr_to_str(event_addr)); break; @@ -818,7 +424,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe } if (l2cap_event_channel_opened_get_status(packet)){ - printf("L2CAP connection to device %s failed. status code 0x%02x\n", + log_error("L2CAP connection to device %s failed. status code 0x%02x", bd_addr_to_str(event_addr), l2cap_event_channel_opened_get_status(packet)); break; } @@ -827,57 +433,51 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe log_error("unexpected PSM - Not implemented yet, avdtp sink: L2CAP_EVENT_CHANNEL_OPENED"); return; } - printf("L2CAP_EVENT_CHANNEL_OPENED cid %02x, context %p, local state %d, remote state %d\n", - l2cap_event_channel_opened_get_local_cid(packet), connection, connection->local_state, connection->remote_state); - if (connection->local_state == AVDTP_SINK_W4_L2CAP_CONNECTED){ + + con_handle = l2cap_event_channel_opened_get_handle(packet); + printf("avdtp_sink: Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x, context %p\n", + bd_addr_to_str(event_addr), con_handle, psm, connection->l2cap_signaling_cid, l2cap_event_channel_opened_get_remote_cid(packet), + connection); + + if (connection->avdtp_state == AVDTP_W4_L2CAP_CONNECTED){ + printf("AVDTP_W4_L2CAP_CONNECTED -> AVDTP_CONFIGURATION_SUBSTATEMACHINE\n"); + connection->avdtp_state = AVDTP_CONFIGURATION_SUBSTATEMACHINE; + avdtp_initiator_stream_config_subsm_init(connection); + avdtp_acceptor_stream_config_subsm_init(connection); + connection->l2cap_signaling_cid = l2cap_event_channel_opened_get_local_cid(packet); - connection->local_state = AVDTP_SINK_W2_DISCOVER_SEPS; - audio_sink_generate_next_transaction_label(connection); + connection->initiator_transaction_label++; l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); - printf("L2CAP_EVENT_CHANNEL_OPENED: Signaling "); - } else if (connection->remote_state <= AVDTP_REMOTE_W4_STREAMING_CONNECTION_OPEN){ + } else if (connection->avdtp_state <= AVDTP_W4_STREAMING_CONNECTION_OPEN){ + printf("AVDTP_W4_STREAMING_CONNECTION_OPEN -> AVDTP_STREAMING\n"); + connection->avdtp_state = AVDTP_STREAMING; connection->l2cap_media_cid = l2cap_event_channel_opened_get_local_cid(packet); - connection->remote_state = AVDTP_REMOTE_STREAMING; // l2cap_request_can_send_now_event(connection->l2cap_media_cid); - printf("L2CAP_EVENT_CHANNEL_OPENED: Media "); + printf("avdtp_sink: L2CAP_EVENT_CHANNEL_OPENED: Media \n"); } else { - printf("unexpected connection state: Not implemented yet remote state %d\n", connection->remote_state); + printf("avdtp_sink: unexpected connection state: Not implemented yet remote state %d\n", connection->acceptor_config_state); return; } - con_handle = l2cap_event_channel_opened_get_handle(packet); - printf("Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x, context %p\n", - bd_addr_to_str(event_addr), con_handle, psm, connection->l2cap_signaling_cid, l2cap_event_channel_opened_get_remote_cid(packet), - connection); break; case L2CAP_EVENT_CHANNEL_CLOSED: // data: event (8), len(8), channel (16) local_cid = l2cap_event_channel_closed_get_local_cid(packet); connection = get_avdtp_sink_connection_context_for_l2cap_cid(local_cid); - if (!connection || connection->local_state != AVDTP_SINK_W4_L2CAP_DISCONNECTED) return; + if (!connection || connection->avdtp_state != AVDTP_W4_L2CAP_DISCONNECTED) return; log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x", local_cid); avdtp_sink_remove_connection_context(connection); - connection->local_state = AVDTP_SINK_IDLE; + connection->avdtp_state = AVDTP_IDLE; break; - case HCI_EVENT_DISCONNECTION_COMPLETE:{ - -#ifdef STORE_SBC_TO_WAV_FILE - printf(" Close wav writer\n"); - wav_writer_close(); - int total_frames_nr = state.good_frames_nr + state.bad_frames_nr + state.zero_frames_nr; - - printf("\nDecoding done. Processed totaly %d frames:\n - %d good\n - %d bad\n - %d zero frames\n", total_frames_nr, state.good_frames_nr, state.bad_frames_nr, state.zero_frames_nr); - printf("Write %d frames to wav file: %s\n\n", frame_count, wav_filename); -#endif -#ifdef STORE_SBC_TO_SBC_FILE - fclose(sbc_file); -#endif - } + case HCI_EVENT_DISCONNECTION_COMPLETE: + break; + case L2CAP_EVENT_CAN_SEND_NOW: + break; default: - printf("unknown packet %02x\n", hci_event_packet_get_type(packet)); + printf("unknown HCI event type %02x\n", hci_event_packet_get_type(packet)); break; } break; @@ -891,51 +491,15 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe // TODO: find out which security level is needed, and replace LEVEL_0 in avdtp_sink_init void avdtp_sink_init(void){ -#ifdef STORE_SBC_TO_WAV_FILE - btstack_sbc_decoder_init(&state, mode, &handle_pcm_data, NULL); - wav_writer_open(wav_filename, NUM_CHANNELS, SAMPLE_RATE); -#endif + l2cap_register_service(&packet_handler, PSM_AVDTP, 0xffff, LEVEL_0); +} -#ifdef STORE_SBC_TO_SBC_FILE - sbc_file = fopen(sbc_filename, "wb"); -#endif - -#ifdef HAVE_PORTAUDIO - PaError err; - PaStreamParameters outputParameters; - - /* -- initialize PortAudio -- */ - err = Pa_Initialize(); - if (err != paNoError){ - printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - /* -- setup input and output -- */ - outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ - outputParameters.channelCount = NUM_CHANNELS; - outputParameters.sampleFormat = PA_SAMPLE_TYPE; - outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; - outputParameters.hostApiSpecificStreamInfo = NULL; - /* -- setup stream -- */ - err = Pa_OpenStream( - &stream, - NULL, /* &inputParameters */ - &outputParameters, - SAMPLE_RATE, - FRAMES_PER_BUFFER, - paClipOff, /* we won't output out of range samples so don't bother clipping them */ - patestCallback, /* use callback */ - NULL ); - - if (err != paNoError){ - printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); +void avdtp_sink_register_media_handler(void (*callback)(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size)){ + if (callback == NULL){ + log_error("avdtp_sink_register_media_handler called with NULL callback"); return; } - memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); - btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); - pa_stream_started = 0; -#endif - l2cap_register_service(&packet_handler, PSM_AVDTP, 0xffff, LEVEL_0); + handle_media_data = callback; } void avdtp_sink_register_packet_handler(btstack_packet_handler_t callback){ @@ -950,11 +514,11 @@ void avdtp_sink_register_packet_handler(btstack_packet_handler_t callback){ static void avdtp_sink_run_for_connection(avdtp_sink_connection_t *connection){ if (!connection) return; if (connection->release_l2cap_connection){ - if (connection->local_state > AVDTP_SINK_W4_L2CAP_CONNECTED || - connection->local_state < AVDTP_SINK_W4_L2CAP_DISCONNECTED){ + if (connection->avdtp_state > AVDTP_W4_L2CAP_CONNECTED || + connection->avdtp_state < AVDTP_W4_L2CAP_DISCONNECTED){ connection->release_l2cap_connection = 0; - connection->local_state = AVDTP_SINK_W4_L2CAP_DISCONNECTED; + connection->avdtp_state = AVDTP_W4_L2CAP_DISCONNECTED; l2cap_disconnect(connection->l2cap_signaling_cid, 0); return; } @@ -964,59 +528,35 @@ static void avdtp_sink_run_for_connection(avdtp_sink_connection_t *connection){ log_info("avdtp_sink_run_for_connection: request cannot send for 0x%02x", connection->l2cap_signaling_cid); return; } - printf("avdtp_sink_run_for_connection: \n"); - int sent = 1; - switch (connection->remote_state){ - case AVDTP_REMOTE_W2_ANSWER_DISCOVER_SEPS: - connection->remote_state = AVDTP_REMOTE_IDLE; - avdtp_sink_send_seps_response(connection->l2cap_signaling_cid, connection->remote_transaction_label, local_seps, local_seps_num); + if (connection->avdtp_state == AVDTP_CONFIGURATION_SUBSTATEMACHINE){ + if (avdtp_initiator_stream_config_subsm_is_done(connection) || avdtp_acceptor_stream_config_subsm_is_done(connection)){ + printf("AVDTP_CONFIGURATION_SUBSTATEMACHINE -> AVDTP_CONFIGURED\n"); + connection->avdtp_state = AVDTP_CONFIGURED; + } + } + + switch (connection->avdtp_state){ + case AVDTP_CONFIGURATION_SUBSTATEMACHINE: + if (!avdtp_initiator_stream_config_subsm_run_for_connection(connection)) { + avdtp_acceptor_stream_config_subsm_run_for_connection(connection, local_seps, local_seps_num); + } break; - case AVDTP_REMOTE_W2_ANSWER_GET_CAPABILITIES: - connection->remote_state = AVDTP_REMOTE_IDLE; - avdtp_sink_send_capabilities_response(connection->l2cap_signaling_cid, connection->remote_transaction_label, local_seps[connection->local_seid]); + case AVDTP_W2_ANSWER_OPEN_STREAM: + printf("AVDTP_W2_ANSWER_OPEN_STREAM -> AVDTP_OPEN use %d\n", connection->acceptor_transaction_label); + connection->avdtp_state = AVDTP_OPEN; + avdtp_acceptor_send_accept_response(connection->l2cap_signaling_cid, AVDTP_SI_OPEN, connection->acceptor_transaction_label); break; - case AVDTP_REMOTE_W2_ANSWER_SET_CONFIGURATION: - connection->remote_state = AVDTP_REMOTE_IDLE; - avdtp_sink_send_accept_response(connection->l2cap_signaling_cid, AVDTP_SET_CONFIGURATION, connection->remote_transaction_label); - break; - case AVDTP_REMOTE_W2_ANSWER_OPEN_STREAM: - connection->remote_state = AVDTP_REMOTE_OPEN; - avdtp_sink_send_accept_response(connection->l2cap_signaling_cid, AVDTP_OPEN, connection->remote_transaction_label); - break; - case AVDTP_REMOTE_W2_ANSWER_START_SINGLE_STREAM: - connection->remote_state = AVDTP_REMOTE_W4_STREAMING_CONNECTION_OPEN; - avdtp_sink_send_accept_response(connection->l2cap_signaling_cid, AVDTP_START, connection->remote_transaction_label); + case AVDTP_W2_ANSWER_START_SINGLE_STREAM: + printf("AVDTP_W2_ANSWER_START_SINGLE_STREAM -> AVDTP_W4_STREAMING_CONNECTION_OPEN use %d\n", connection->acceptor_transaction_label); + connection->avdtp_state = AVDTP_W4_STREAMING_CONNECTION_OPEN; + avdtp_acceptor_send_accept_response(connection->l2cap_signaling_cid, AVDTP_SI_START, connection->acceptor_transaction_label); break; default: - sent = 0; + printf("avdtp_sink_run_for_connection: state %d -> NOT IMPLEMENTED\n", connection->avdtp_state); break; } - if (sent == 1){ - printf("answered to remote\n"); - return; - } - - sent = 1; - switch (connection->local_state){ - case AVDTP_SINK_W2_DISCOVER_SEPS: - connection->local_state = AVDTP_SINK_W4_SEPS_DISCOVERED; - avdtp_sink_send_signaling_cmd(connection->l2cap_signaling_cid, AVDTP_DISCOVER, connection->local_transaction_label); - break; - case AVDTP_SINK_W2_GET_CAPABILITIES: - connection->local_state = AVDTP_SINK_W4_CAPABILITIES; - avdtp_sink_send_get_capabilities_cmd(connection->l2cap_signaling_cid, 0); - break; - default: - sent = 0; - break; - } - - if (sent){ - printf("sent request to remote\n"); - return; - } } void avdtp_sink_connect(bd_addr_t bd_addr){ @@ -1031,7 +571,7 @@ void avdtp_sink_connect(bd_addr_t bd_addr){ return; } - connection->local_state = AVDTP_SINK_W4_L2CAP_CONNECTED; + connection->avdtp_state = AVDTP_W4_L2CAP_CONNECTED; l2cap_create_channel(packet_handler, connection->remote_addr, PSM_AVDTP, 0xffff, NULL); } @@ -1042,8 +582,8 @@ void avdtp_sink_disconnect(uint16_t l2cap_cid){ return; } - if (connection->local_state == AVDTP_SINK_IDLE) return; - if (connection->local_state == AVDTP_SINK_W4_L2CAP_DISCONNECTED) return; + if (connection->avdtp_state == AVDTP_IDLE) return; + if (connection->avdtp_state == AVDTP_W4_L2CAP_DISCONNECTED) return; connection->release_l2cap_connection = 1; avdtp_sink_run_for_connection(connection); diff --git a/test/avdtp/avdtp_sink.h b/test/avdtp/avdtp_sink.h index 83a2672fc..95ea54dfb 100644 --- a/test/avdtp/avdtp_sink.h +++ b/test/avdtp/avdtp_sink.h @@ -57,40 +57,42 @@ extern "C" { #define MAX_NUM_SEPS 10 typedef enum { - AVDTP_SINK_IDLE, + AVDTP_IDLE, + AVDTP_W4_L2CAP_CONNECTED, + AVDTP_CONFIGURATION_SUBSTATEMACHINE, + AVDTP_CONFIGURED, + + AVDTP_W2_ANSWER_OPEN_STREAM, + AVDTP_OPEN, + AVDTP_W2_ANSWER_START_SINGLE_STREAM, + AVDTP_W4_STREAMING_CONNECTION_OPEN, + AVDTP_STREAMING, - AVDTP_SINK_W4_L2CAP_CONNECTED, - AVDTP_SINK_W2_DISCOVER_SEPS, - AVDTP_SINK_W4_SEPS_DISCOVERED, - AVDTP_SINK_W2_GET_CAPABILITIES, - AVDTP_SINK_W4_CAPABILITIES, - - AVDTP_SINK_W2_SET_CONFIGURATION, - AVDTP_SINK_W4_CONFIGURATION_SET, - - AVDTP_SINK_W2_GET_CONFIGURATION, - AVDTP_SINK_W4_CONFIGURATION_RECEIVED, - - AVDTP_SINK_CONFIGURED, - AVDTP_SINK_OPEN, - AVDTP_SINK_STREAMING, - AVDTP_SINK_CLOSING, - AVDTP_SINK_ABORTING, - AVDTP_SINK_W4_L2CAP_DISCONNECTED -} avdtp_sink_state_t; - + AVDTP_CLOSING, + AVDTP_ABORTING, + AVDTP_W4_L2CAP_DISCONNECTED +} avdtp_state_t; typedef enum { - AVDTP_REMOTE_IDLE, - AVDTP_REMOTE_W2_ANSWER_DISCOVER_SEPS, - AVDTP_REMOTE_W2_ANSWER_GET_CAPABILITIES, - AVDTP_REMOTE_W2_ANSWER_SET_CONFIGURATION, - AVDTP_REMOTE_W2_ANSWER_OPEN_STREAM, - AVDTP_REMOTE_OPEN, - AVDTP_REMOTE_W2_ANSWER_START_SINGLE_STREAM, - AVDTP_REMOTE_W4_STREAMING_CONNECTION_OPEN, - AVDTP_REMOTE_STREAMING -} avdtp_source_state_t; + AVDTP_INITIATOR_STREAM_CONFIG_IDLE, + AVDTP_INITIATOR_W2_DISCOVER_SEPS, + AVDTP_INITIATOR_W4_SEPS_DISCOVERED, + AVDTP_INITIATOR_W2_GET_CAPABILITIES, + AVDTP_INITIATOR_W4_CAPABILITIES, + AVDTP_INITIATOR_W2_SET_CONFIGURATION, + AVDTP_INITIATOR_W4_CONFIGURATION_SET, + AVDTP_INITIATOR_W2_GET_CONFIGURATION, + AVDTP_INITIATOR_W4_CONFIGURATION_RECEIVED, + AVDTP_INITIATOR_STREAM_CONFIG_DONE +} avdtp_initiator_stream_config_state_t; + +typedef enum { + AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE, + AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS, + AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES, + AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION, + AVDTP_ACCEPTOR_STREAM_CONFIG_DONE +} avdtp_acceptor_stream_config_state_t; typedef struct avdtp_sink_connection { @@ -101,11 +103,12 @@ typedef struct avdtp_sink_connection { uint16_t l2cap_media_cid; uint16_t l2cap_reporting_cid; - avdtp_sink_state_t local_state; - avdtp_source_state_t remote_state; + avdtp_state_t avdtp_state; + avdtp_initiator_stream_config_state_t initiator_config_state; + avdtp_acceptor_stream_config_state_t acceptor_config_state; - uint8_t local_transaction_label; - uint8_t remote_transaction_label; + uint8_t initiator_transaction_label; + uint8_t acceptor_transaction_label; // store remote seps avdtp_sep_t remote_seps[MAX_NUM_SEPS]; @@ -163,6 +166,8 @@ void avdtp_sink_register_packet_handler(btstack_packet_handler_t callback); */ void avdtp_sink_connect(bd_addr_t bd_addr); +// TODO: per connectio? +void avdtp_sink_register_media_handler(void (*callback)(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size)); /** * @brief Disconnect from device with connection handle. * @param l2cap_cid diff --git a/test/avdtp/avdtp_test.c b/test/avdtp/avdtp_test.c index 6eefe76a2..4a0789f5f 100644 --- a/test/avdtp/avdtp_test.c +++ b/test/avdtp/avdtp_test.c @@ -54,12 +54,148 @@ #include "stdin_support.h" #include "avdtp_sink.h" +#include "btstack_sbc.h" +#include "wav_util.h" +#include "avdtp_util.h" + +#define NUM_CHANNELS 2 +#define SAMPLE_RATE 44100 + +#ifdef HAVE_PORTAUDIO +#include +#include "btstack_ring_buffer.h" +#endif + +#ifdef HAVE_POSIX_FILE_IO +#define STORE_SBC_TO_SBC_FILE +#define STORE_SBC_TO_WAV_FILE +#endif + +#ifdef HAVE_PORTAUDIO +#define PA_SAMPLE_TYPE paInt16 +#define FRAMES_PER_BUFFER 128 +#define BYTES_PER_FRAME (2*NUM_CHANNELS) +#define PREBUFFER_MS 150 +#define PREBUFFER_BYTES (PREBUFFER_MS*SAMPLE_RATE/1000*BYTES_PER_FRAME) +static uint8_t ring_buffer_storage[2*PREBUFFER_BYTES]; +static btstack_ring_buffer_t ring_buffer; + +static PaStream * stream; +static uint8_t pa_stream_started = 0; + +static int patestCallback( const void *inputBuffer, void *outputBuffer, + unsigned long framesPerBuffer, + const PaStreamCallbackTimeInfo* timeInfo, + PaStreamCallbackFlags statusFlags, + void *userData ) { + (void) timeInfo; /* Prevent unused variable warnings. */ + (void) statusFlags; + (void) inputBuffer; + + uint32_t bytes_read = 0; + int bytes_per_buffer = framesPerBuffer * BYTES_PER_FRAME; + + if (btstack_ring_buffer_bytes_available(&ring_buffer) >= bytes_per_buffer){ + btstack_ring_buffer_read(&ring_buffer, outputBuffer, bytes_per_buffer, &bytes_read); + } else { + memset(outputBuffer, 0, bytes_per_buffer); + } + // printf("bytes avail after read: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); + return 0; +} +#endif + +#ifdef STORE_SBC_TO_WAV_FILE +// store sbc as wav: +static btstack_sbc_decoder_state_t state; +static int total_num_samples = 0; +static int frame_count = 0; +static char * wav_filename = "avdtp_sink.wav"; +static btstack_sbc_mode_t mode = SBC_MODE_STANDARD; + +static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ + wav_writer_write_int16(num_samples*num_channels, data); + total_num_samples+=num_samples*num_channels; + frame_count++; + +#ifdef HAVE_PORTAUDIO + if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= PREBUFFER_BYTES){ + /* -- start stream -- */ + PaError err = Pa_StartStream(stream); + if (err != paNoError){ + printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); + return; + } + pa_stream_started = 1; + } + btstack_ring_buffer_write(&ring_buffer, (uint8_t *)data, num_samples*num_channels*2); + // printf("bytes avail after write: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); +#endif +} +#endif + +#ifdef STORE_SBC_TO_SBC_FILE +static FILE * sbc_file; +static char * sbc_filename = "avdtp_sink.sbc"; +#endif + static bd_addr_t remote = {0x04, 0x0C, 0xCE, 0xE4, 0x85, 0xD3}; static uint8_t sdp_avdtp_sink_service_buffer[150]; uint16_t local_cid = 0; static btstack_packet_callback_registration_t hci_event_callback_registration; + +static void handle_l2cap_media_data_packet(avdtp_sink_connection_t * connection, uint8_t *packet, uint16_t size){ + int pos = 0; + + avdtp_media_packet_header_t media_header; + media_header.version = packet[pos] & 0x03; + media_header.padding = get_bit16(packet[pos],2); + media_header.extension = get_bit16(packet[pos],3); + media_header.csrc_count = (packet[pos] >> 4) & 0x0F; + + pos++; + + media_header.marker = get_bit16(packet[pos],0); + media_header.payload_type = (packet[pos] >> 1) & 0x7F; + pos++; + + media_header.sequence_number = big_endian_read_16(packet, pos); + pos+=2; + + media_header.timestamp = big_endian_read_32(packet, pos); + pos+=4; + + media_header.synchronization_source = big_endian_read_32(packet, pos); + pos+=4; + + // TODO: read csrc list + + // printf_hexdump( packet, pos ); + // printf("MEDIA HEADER: %u timestamp, version %u, padding %u, extension %u, csrc_count %u\n", + // media_header.timestamp, media_header.version, media_header.padding, media_header.extension, media_header.csrc_count); + // printf("MEDIA HEADER: marker %02x, payload_type %02x, sequence_number %u, synchronization_source %u\n", + // media_header.marker, media_header.payload_type, media_header.sequence_number, media_header.synchronization_source); + + avdtp_sbc_codec_header_t sbc_header; + sbc_header.fragmentation = get_bit16(packet[pos], 7); + sbc_header.starting_packet = get_bit16(packet[pos], 6); + sbc_header.last_packet = get_bit16(packet[pos], 5); + sbc_header.num_frames = packet[pos] & 0x0f; + pos++; + + // printf("SBC HEADER: num_frames %u, fragmented %u, start %u, stop %u\n", sbc_header.num_frames, sbc_header.fragmentation, sbc_header.starting_packet, sbc_header.last_packet); + // printf_hexdump( packet+pos, size-pos ); +#ifdef STORE_SBC_TO_WAV_FILE + btstack_sbc_decoder_process_data(&state, 0, packet+pos, size-pos); +#endif + +#ifdef STORE_SBC_TO_SBC_FILE + fwrite(packet+pos, size-pos, 1, sbc_file); +#endif +} + static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ bd_addr_t event_addr; @@ -88,8 +224,38 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe case HCI_EVENT_DISCONNECTION_COMPLETE: // connection closed -> quit tes app - printf("HCI_EVENT_DISCONNECTION_COMPLETE\n"); - + printf("\n --- avdtp_test: HCI_EVENT_DISCONNECTION_COMPLETE ---\n"); +#ifdef STORE_SBC_TO_WAV_FILE + printf(" Close wav writer.\n"); + wav_writer_close(); + int total_frames_nr = state.good_frames_nr + state.bad_frames_nr + state.zero_frames_nr; + + printf(" Decoding done. Processed totaly %d frames:\n - %d good\n - %d bad\n - %d zero frames\n", total_frames_nr, state.good_frames_nr, state.bad_frames_nr, state.zero_frames_nr); + printf(" Written %d frames to wav file: %s\n\n", frame_count, wav_filename); +#endif +#ifdef STORE_SBC_TO_SBC_FILE + fclose(sbc_file); +#endif + +#ifdef HAVE_PORTAUDIO + PaError err = Pa_StopStream(stream); + if (err != paNoError){ + printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); + return; + } + pa_stream_started = 0; + err = Pa_CloseStream(stream); + if (err != paNoError){ + printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); + return; + } + + err = Pa_Terminate(); + if (err != paNoError){ + printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); + return; + } +#endif // exit(0); break; @@ -153,18 +319,68 @@ int btstack_main(int argc, const char * argv[]){ // Initialize AVDTP Sink avdtp_sink_init(); avdtp_sink_register_packet_handler(&packet_handler); + uint8_t seid = avdtp_sink_register_stream_end_point(AVDTP_SINK, AVDTP_AUDIO); avdtp_sink_register_media_transport_category(seid); - avdtp_sink_register_media_codec_category(seid, AVDTP_AUDIO, AVDTP_CODEC_SBC, media_sbc_codec_info, sizeof(media_sbc_codec_info)); + avdtp_sink_register_media_handler(&handle_l2cap_media_data_packet); // Initialize SDP sdp_init(); memset(sdp_avdtp_sink_service_buffer, 0, sizeof(sdp_avdtp_sink_service_buffer)); a2dp_sink_create_sdp_record(sdp_avdtp_sink_service_buffer, 0x10001, 0, NULL, NULL); sdp_register_service(sdp_avdtp_sink_service_buffer); + + gap_set_local_name("BTstack AVDTP Test"); + gap_discoverable_control(1); + gap_set_class_of_device(0x200408); +#ifdef STORE_SBC_TO_WAV_FILE + btstack_sbc_decoder_init(&state, mode, handle_pcm_data, NULL); + wav_writer_open(wav_filename, NUM_CHANNELS, SAMPLE_RATE); +#endif + +#ifdef STORE_SBC_TO_SBC_FILE + sbc_file = fopen(sbc_filename, "wb"); +#endif + +#ifdef HAVE_PORTAUDIO + PaError err; + PaStreamParameters outputParameters; + + /* -- initialize PortAudio -- */ + err = Pa_Initialize(); + if (err != paNoError){ + printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); + return err; + } + /* -- setup input and output -- */ + outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ + outputParameters.channelCount = NUM_CHANNELS; + outputParameters.sampleFormat = PA_SAMPLE_TYPE; + outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; + outputParameters.hostApiSpecificStreamInfo = NULL; + /* -- setup stream -- */ + err = Pa_OpenStream( + &stream, + NULL, /* &inputParameters */ + &outputParameters, + SAMPLE_RATE, + FRAMES_PER_BUFFER, + paClipOff, /* we won't output out of range samples so don't bother clipping them */ + patestCallback, /* use callback */ + NULL ); + + if (err != paNoError){ + printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); + return err; + } + memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); + btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); + pa_stream_started = 0; +#endif + // turn on! hci_power_control(HCI_POWER_ON); diff --git a/test/avdtp/avdtp_util.c b/test/avdtp/avdtp_util.c new file mode 100644 index 000000000..1e9d4abee --- /dev/null +++ b/test/avdtp/avdtp_util.c @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * 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. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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 MATTHIAS + * RINGWALD 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. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + + +#include +#include +#include +#include +#include + +#include "avdtp.h" +#include "avdtp_util.h" + +inline uint8_t avdtp_header(uint8_t tr_label, avdtp_packet_type_t packet_type, avdtp_message_type_t msg_type){ + return (tr_label<<4) | ((uint8_t)packet_type<<2) | (uint8_t)msg_type; +} + +int get_bit16(uint16_t bitmap, int position){ + return (bitmap >> position) & 1; +} + +uint8_t store_bit16(uint16_t bitmap, int position, uint8_t value){ + if (value){ + bitmap |= 1 << position; + } else { + bitmap &= ~ (1 << position); + } + return bitmap; +} + +void avdtp_read_signaling_header(avdtp_signaling_packet_header_t * signaling_header, uint8_t * packet, uint16_t size){ + if (size < 2) return; + signaling_header->transaction_label = packet[0] >> 4; + signaling_header->packet_type = (avdtp_packet_type_t)((packet[0] >> 2) & 0x03); + signaling_header->message_type = (avdtp_message_type_t) (packet[0] & 0x03); + signaling_header->signal_identifier = packet[1] & 0x3f; +} diff --git a/test/avdtp/avdtp_util.h b/test/avdtp/avdtp_util.h new file mode 100644 index 000000000..d0f2190af --- /dev/null +++ b/test/avdtp/avdtp_util.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * 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. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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 MATTHIAS + * RINGWALD 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. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + +/* + * avdtp_util.h + * + * Audio/Video Distribution Transport Protocol - Utils + * + */ + +#ifndef __AVDTP_UITL_H +#define __AVDTP_UITL_H + +#include + +#if defined __cplusplus +extern "C" { +#endif + +uint8_t avdtp_header(uint8_t tr_label, avdtp_packet_type_t packet_type, avdtp_message_type_t msg_type); +void avdtp_read_signaling_header(avdtp_signaling_packet_header_t * signaling_header, uint8_t * packet, uint16_t size); + +uint8_t store_bit16(uint16_t bitmap, int position, uint8_t value); +int get_bit16(uint16_t bitmap, int position); + +#if defined __cplusplus +} +#endif + +#endif // __AVDTP_UITL_H \ No newline at end of file