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; } }