static void sdp_client_parse_service_search_response(uint8_t* packet){
uint16_t offset = 3;
uint16_t parameterLength = big_endian_read_16(packet,offset);
offset+=2;
uint16_t totalServiceRecordCount = big_endian_read_16(packet,offset);
offset+=2;
uint16_t currentServiceRecordCount = big_endian_read_16(packet,offset);
offset+=2;
if (currentServiceRecordCount > totalServiceRecordCount){
log_error("CurrentServiceRecordCount is larger then TotalServiceRecordCount.");
return;
}
sdp_client_parse_service_record_handle_list(packet+offset, totalServiceRecordCount, currentServiceRecordCount);
offset+=(currentServiceRecordCount * 4);
continuationStateLen = packet[offset];
offset++;
if (continuationStateLen > 16){
log_error("Error parsing ServiceSearchResponse: Number of bytes in continuation state exceedes 16.");
return;
}
memcpy(continuationState, packet+offset, continuationStateLen);
offset+=continuationStateLen;
if (parameterLength != offset - 5){
log_error("Error parsing ServiceSearchResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset);
}
}
static void sdp_client_parse_service_attribute_response(uint8_t* packet){
uint16_t offset = 3;
uint16_t parameterLength = big_endian_read_16(packet,offset);
offset+=2;
// AttributeListByteCount <= mtu
uint16_t attributeListByteCount = big_endian_read_16(packet,offset);
offset+=2;
if (attributeListByteCount > mtu){
log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in found attribute list is larger then the MaximumAttributeByteCount.");
return;
}
// AttributeLists
sdp_client_parse_attribute_lists(packet+offset, attributeListByteCount);
offset+=attributeListByteCount;
continuationStateLen = packet[offset];
offset++;
if (continuationStateLen > 16){
log_error("Error parsing ServiceAttributeResponse: Number of bytes in continuation state exceedes 16.");
return;
}
memcpy(continuationState, packet+offset, continuationStateLen);
offset+=continuationStateLen;
if (parameterLength != offset - 5){
log_error("Error parsing ServiceAttributeResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset);
}
}
static void handle_l2cap_media_data_packet(uint8_t seid, uint8_t *packet, uint16_t size){
UNUSED(seid);
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;
UNUSED(media_header);
// 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++;
UNUSED(sbc_header);
// 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 DECODE_SBC
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
#ifdef HAVE_PORTAUDIO
log_info("PA: bytes avail after recv: %d", btstack_ring_buffer_bytes_available(&ring_buffer));
#endif
}
static uint16_t calc_internet_checksum(uint8_t * data, int size){
uint32_t checksum = 0;
while (size){
// add 16-bit value
checksum = sum_ones_complement(checksum, big_endian_read_16(data, 0));
data += 2;
size -= 2;
}
return checksum;
}
/* decode and store received TLM */
static esp_err_t esp_eddystone_tlm_received(const uint8_t* buf, uint8_t len, esp_eddystone_result_t* res)
{
uint8_t pos = 0;
if(len+4 > EDDYSTONE_TLM_FRAME_LEN) {
//ERROR:TLM too long
return -1;
}
res->inform.tlm.version = buf[pos++];
res->inform.tlm.battery_voltage = big_endian_read_16(buf, pos);
pos += 2;
uint16_t temp = big_endian_read_16(buf, pos);
int8_t temp_integral = (int8_t)((temp >> 8) & 0xff);
float temp_decimal = (temp & 0xff) / 256.0;
res->inform.tlm.temperature = temp_integral + temp_decimal;
pos += 2;
res->inform.tlm.adv_count = big_endian_read_32(buf, pos);
pos += 4;
res->inform.tlm.time = big_endian_read_32(buf, pos);
return 0;
}
static char * get_string_from_data_element(uint8_t * element){
de_size_t de_size = de_get_size_type(element);
int pos = de_get_header_size(element);
int len = 0;
switch (de_size){
case DE_SIZE_VAR_8:
len = element[1];
break;
case DE_SIZE_VAR_16:
len = big_endian_read_16(element, 1);
break;
default:
break;
}
char * str = (char*)malloc(len+1);
memcpy(str, &element[pos], len);
str[len] ='\0';
return str;
}
static int read_media_data_header(uint8_t *packet, int size, int *offset, avdtp_media_packet_header_t *media_header){
int media_header_len = 12; // without crc
int pos = *offset;
if (size - pos < media_header_len){
printf("Not enough data to read media packet header, expected %d, received %d\n", media_header_len, size-pos);
return 0;
}
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;
*offset = pos;
// TODO: read csrc list
// printf_hexdump( packet, pos );
if (!is_media_header_reported_once){
is_media_header_reported_once = 1;
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);
}
return 1;
}
void sdp_client_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
UNUSED(size);
// uint16_t handle;
if (packet_type == L2CAP_DATA_PACKET){
uint16_t responseTransactionID = big_endian_read_16(packet,1);
if (responseTransactionID != transactionID){
log_error("Mismatching transaction ID, expected %u, found %u.", transactionID, responseTransactionID);
return;
}
if (packet[0] == SDP_ErrorResponse){
log_error("Received error response with code %u, disconnecting", packet[2]);
l2cap_disconnect(sdp_cid, 0);
return;
}
if (packet[0] != SDP_ServiceSearchAttributeResponse
&& packet[0] != SDP_ServiceSearchResponse
&& packet[0] != SDP_ServiceAttributeResponse){
log_error("Not a valid PDU ID, expected %u, %u or %u, found %u.", SDP_ServiceSearchResponse,
SDP_ServiceAttributeResponse, SDP_ServiceSearchAttributeResponse, packet[0]);
return;
}
PDU_ID = (SDP_PDU_ID_t)packet[0];
log_debug("SDP Client :: PDU ID. %u ,%u", PDU_ID, packet[0]);
switch (PDU_ID){
#ifdef ENABLE_SDP_EXTRA_QUERIES
case SDP_ServiceSearchResponse:
sdp_client_parse_service_search_response(packet);
break;
case SDP_ServiceAttributeResponse:
sdp_client_parse_service_attribute_response(packet);
break;
#endif
case SDP_ServiceSearchAttributeResponse:
sdp_client_parse_service_search_attribute_response(packet);
break;
default:
log_error("SDP Client :: PDU ID invalid. %u ,%u", PDU_ID, packet[0]);
return;
}
// continuation set or DONE?
if (continuationStateLen == 0){
log_debug("SDP Client Query DONE! ");
sdp_client_state = QUERY_COMPLETE;
l2cap_disconnect(sdp_cid, 0);
return;
}
// prepare next request and send
sdp_client_state = W2_SEND;
l2cap_request_can_send_now_event(sdp_cid);
return;
}
if (packet_type != HCI_EVENT_PACKET) return;
switch(hci_event_packet_get_type(packet)){
case L2CAP_EVENT_CHANNEL_OPENED:
if (sdp_client_state != W4_CONNECT) break;
// data: event (8), len(8), status (8), address(48), handle (16), psm (16), local_cid(16), remote_cid (16), local_mtu(16), remote_mtu(16)
if (packet[2]) {
log_info("SDP Client Connection failed, status 0x%02x.", packet[2]);
sdp_client_state = INIT;
sdp_parser_handle_done(packet[2]);
break;
}
sdp_cid = channel;
mtu = little_endian_read_16(packet, 17);
// handle = little_endian_read_16(packet, 9);
log_debug("SDP Client Connected, cid %x, mtu %u.", sdp_cid, mtu);
sdp_client_state = W2_SEND;
l2cap_request_can_send_now_event(sdp_cid);
break;
case L2CAP_EVENT_CAN_SEND_NOW:
if(l2cap_event_can_send_now_get_local_cid(packet) == sdp_cid){
sdp_client_send_request(sdp_cid);
}
break;
case L2CAP_EVENT_CHANNEL_CLOSED: {
if (sdp_cid != little_endian_read_16(packet, 2)) {
// log_info("Received L2CAP_EVENT_CHANNEL_CLOSED for cid %x, current cid %x\n", little_endian_read_16(packet, 2),sdp_cid);
break;
}
log_info("SDP Client disconnected.");
uint8_t status = sdp_client_state == QUERY_COMPLETE ? 0 : SDP_QUERY_INCOMPLETE;
sdp_client_state = INIT;
sdp_parser_handle_done(status);
break;
}
default:
break;
}
}
/*************** PANU client routines *********************/
static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
{
uint8_t event;
bd_addr_t event_addr;
bd_addr_t src_addr;
bd_addr_t dst_addr;
uint16_t uuid_source;
uint16_t uuid_dest;
uint16_t mtu;
uint16_t network_type;
uint8_t protocol_type;
uint8_t icmp_type;
int ihl;
int payload_offset;
switch (packet_type) {
case HCI_EVENT_PACKET:
event = packet[0];
switch (event) {
case BTSTACK_EVENT_STATE:
/* BT Stack activated, get started */
if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){
printf("BNEP Test ready\n");
show_usage();
}
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_read_bd_addr)){
reverse_bd_addr(&packet[6], local_addr);
printf("BD-ADDR: %s\n", bd_addr_to_str(local_addr));
break;
}
break;
case HCI_EVENT_USER_CONFIRMATION_REQUEST:
// inform about user confirmation request
printf("SSP User Confirmation Request with numeric value '%06u'\n", little_endian_read_32(packet, 8));
printf("SSP User Confirmation Auto accept\n");
break;
case BNEP_EVENT_CHANNEL_OPENED:
if (bnep_event_channel_opened_get_status(packet)) {
printf("BNEP channel open failed, status %02x\n", bnep_event_channel_opened_get_status(packet));
} else {
// data: event(8), len(8), status (8), bnep source uuid (16), bnep destination uuid (16), remote_address (48)
bnep_cid = bnep_event_channel_opened_get_bnep_cid(packet);
uuid_source = bnep_event_channel_opened_get_source_uuid(packet);
uuid_dest = bnep_event_channel_opened_get_destination_uuid(packet);
mtu = bnep_event_channel_opened_get_mtu(packet);
//bt_flip_addr(event_addr, &packet[9]);
memcpy(&event_addr, &packet[11], sizeof(bd_addr_t));
printf("BNEP connection open succeeded to %s source UUID 0x%04x dest UUID: 0x%04x, max frame size %u\n", bd_addr_to_str(event_addr), uuid_source, uuid_dest, mtu);
}
break;
case BNEP_EVENT_CHANNEL_TIMEOUT:
printf("BNEP channel timeout! Channel will be closed\n");
break;
case BNEP_EVENT_CHANNEL_CLOSED:
printf("BNEP channel closed\n");
break;
case BNEP_EVENT_CAN_SEND_NOW:
/* Check for parked network packets and send it out now */
if (network_buffer_len > 0) {
bnep_send(bnep_cid, network_buffer, network_buffer_len);
network_buffer_len = 0;
}
break;
default:
break;
}
break;
case BNEP_DATA_PACKET:
// show received packet on console
// TODO: fix BNEP to return BD ADDR in little endian, to use these lines
// bt_flip_addr(dst_addr, &packet[0]);
// bt_flip_addr(src_addr, &packet[6]);
// instead of these
memcpy(dst_addr, &packet[0], 6);
memcpy(src_addr, &packet[6], 6);
// END TOOD
network_type = big_endian_read_16(packet, 12);
printf("BNEP packet received\n");
printf("Dst Addr: %s\n", bd_addr_to_str(dst_addr));
printf("Src Addr: %s\n", bd_addr_to_str(src_addr));
printf("Net Type: %04x\n", network_type);
// ignore the next 60 bytes
// hexdumpf(&packet[74], size - 74);
switch (network_type){
case NETWORK_TYPE_IPv4:
ihl = packet[14] & 0x0f;
payload_offset = 14 + (ihl << 2);
// protocol
protocol_type = packet[14 + 9]; // offset 9 into IPv4
switch (protocol_type){
case 0x01: // ICMP
icmp_type = packet[payload_offset];
hexdumpf(&packet[payload_offset], size - payload_offset);
printf("ICMP packet of type %x\n", icmp_type);
switch (icmp_type){
case ICMP_V4_TYPE_PING_REQUEST:
printf("IPv4 Ping Request received, sending pong\n");
send_ping_response_ipv4();
break;
break;
}
case 0x11: // UDP
printf("UDP IPv4 packet\n");
hexdumpf(&packet[payload_offset], size - payload_offset);
break;
default:
printf("Unknown IPv4 protocol type %x", protocol_type);
break;
}
break;
case NETWORK_TYPE_IPv6:
protocol_type = packet[6];
switch(protocol_type){
case 0x11: // UDP
printf("UDP IPv6 packet\n");
payload_offset = 40; // fixed
hexdumpf(&packet[payload_offset], size - payload_offset);
// send response
break;
default:
printf("IPv6 packet of protocol 0x%02x\n", protocol_type);
hexdumpf(&packet[14], size - 14);
break;
}
break;
default:
printf("Unknown network type %x", network_type);
break;
}
break;
default:
break;
}
}
