static void parse_service_attribute_response(uint8_t* packet){
uint16_t offset = 3;
uint16_t parameterLength = READ_NET_16(packet,offset);
offset+=2;
// AttributeListByteCount <= mtu
uint16_t attributeListByteCount = READ_NET_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
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 parse_service_search_response(uint8_t* packet){
uint16_t offset = 3;
uint16_t parameterLength = READ_NET_16(packet,offset);
offset+=2;
uint16_t totalServiceRecordCount = READ_NET_16(packet,offset);
offset+=2;
uint16_t currentServiceRecordCount = READ_NET_16(packet,offset);
offset+=2;
if (currentServiceRecordCount > totalServiceRecordCount){
log_error("CurrentServiceRecordCount is larger then TotalServiceRecordCount.");
return;
}
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 dump_service_search_response(void){
uint16_t nr_services = READ_NET_16(sdp_response_buffer, 7);
int i;
printf("Nr service handles: %u\n", nr_services);
for (i=0; i<nr_services;i++){
printf(" ServiceHandle %x\n", READ_NET_32(sdp_response_buffer, 9+i*4));
}
if (sdp_response_buffer[9 + nr_services * 4]){
printf(" Continuation index %u\n", READ_NET_16(sdp_response_buffer, 9+nr_services*4+1));
} else {
printf(" Continuation: NO\n");
}
}
static void handle_gps_data(stlv_packet pack, element_handle handle)
{
char type_buf[MAX_ELEMENT_TYPE_BUFSIZE];
uint16_t gps_spd = 0xffff;
uint16_t gps_alt = 0xffff;
uint32_t gps_dis = 0;
element_handle element = get_first_sub_element(pack, handle);
while (IS_VALID_STLV_HANDLE(element))
{
int type_len = get_element_type(pack, element, type_buf, sizeof(type_buf));
switch (type_buf[0])
{
case SUB_TYPE_ACTIVITY_ALT:
{
uint8_t* data = get_element_data_buffer(pack, element, type_buf, type_len);
uint8_t data_size = get_element_data_size(pack, element, type_buf, type_len);
log_info("gps.alt=%d:%02x %02x\n", data_size, data[0], data[1]);
//gps_alt = *((uint16_t*)data);
//gps_alt = htons(gps_alt);
gps_alt = READ_NET_16(data, 0);
}
break;
case SUB_TYPE_ACTIVITY_SPD:
{
uint8_t* data = get_element_data_buffer(pack, element, type_buf, type_len);
uint8_t data_size = get_element_data_size(pack, element, type_buf, type_len);
log_info("gps.spd=%d:%02x %02x\n", data_size, data[0], data[1]);
//gps_spd = *((uint16_t*)data);
//gps_spd = htons(gps_spd);
gps_spd = READ_NET_16(data, 0);
}
break;
case SUB_TYPE_ACTIVITY_DIS:
{
uint8_t* data = get_element_data_buffer(pack, element, type_buf, type_len);
uint8_t data_size = get_element_data_size(pack, element, type_buf, type_len);
log_info("gps.dis=%d:%02x %02x %02x %02x\n", data_size, data[0], data[1], data[2], data[3]);
gps_dis = READ_NET_32(data, 0);
}
break;
}
element = get_next_sub_element(pack, handle, element);
}
handle_gps_info(gps_spd, gps_alt, gps_dis);
}
void handle_set_watch_config(ui_config* new_config)
{
//TODO: help check this
ui_config* config = window_readconfig();
if (config != NULL)
{
memcpy(config, new_config, sizeof(ui_config));
//adjust values: big endian to little endian
uint8_t* p = (uint8_t*)new_config;
config->signature = READ_NET_32(p, 0);
config->goal_steps = READ_NET_16(p, 4);
config->goal_distance = READ_NET_16(p, 6);
config->goal_calories = READ_NET_16(p, 8);
config->lap_length = READ_NET_16(p, 10);
if (config->weight < 20) config->weight = 20;
if (config->height < 60) config->height = 60;
log_info("set_watch_config:\n");
log_info(" signature = %x\n", config->signature);
log_info(" default_clock = %d\n", config->default_clock); // 0 - analog, 1 - digit
log_info(" analog_clock = %d\n", config->analog_clock); // num : which lock face
log_info(" digit_clock = %d\n", config->digit_clock); // num : which clock face
log_info(" sports_grid = %d\n", config->sports_grid); // 0 - 3 grid, 1 - 4 grid, 2 - 5 grid
log_info(" sports_grids = %d, %d, %d, %d, %d\n",
config->sports_grid_data[0],
config->sports_grid_data[1],
config->sports_grid_data[2],
config->sports_grid_data[3],
config->sports_grid_data[4]);
log_info(" is_ukuint = %02x\n", config->is_ukuint);
log_info(" goal_steps = %d\n", config->goal_steps);
log_info(" goal_distance = %d\n", config->goal_distance);
log_info(" goal_calories = %d\n", config->goal_calories);
log_info(" weight = %d\n", config->weight); // in kg
log_info(" height = %d\n", config->height); // in cm
log_info(" circumference = %d\n", config->circumference);
log_info(" lap_len = %d\n", config->lap_length);
window_writeconfig();
window_loadconfig();
}
}
/*************** PANU client routines *********************/
static void packet_handler (void * connection, 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 (packet[2] == HCI_STATE_WORKING) {
printf("BNEP Test ready\n");
show_usage();
}
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)){
bt_flip_addr(local_addr, &packet[6]);
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", READ_BT_32(packet, 8));
printf("SSP User Confirmation Auto accept\n");
break;
case BNEP_EVENT_OPEN_CHANNEL_COMPLETE:
if (packet[2]) {
printf("BNEP channel open failed, status %02x\n", packet[2]);
} else {
// data: event(8), len(8), status (8), bnep source uuid (16), bnep destination uuid (16), remote_address (48)
uuid_source = READ_BT_16(packet, 3);
uuid_dest = READ_BT_16(packet, 5);
mtu = READ_BT_16(packet, 7);
bnep_cid = channel;
//bt_flip_addr(event_addr, &packet[9]);
memcpy(&event_addr, &packet[9], 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_READY_TO_SEND:
/* 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 = READ_NET_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;
}
}
// we assume that we don't get two requests in a row
static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
uint16_t transaction_id;
SDP_PDU_ID_t pdu_id;
uint16_t remote_mtu;
// uint16_t param_len;
switch (packet_type) {
case L2CAP_DATA_PACKET:
pdu_id = (SDP_PDU_ID_t) packet[0];
transaction_id = READ_NET_16(packet, 1);
// param_len = READ_NET_16(packet, 3);
remote_mtu = l2cap_get_remote_mtu_for_local_cid(channel);
// account for our buffer
if (remote_mtu > SDP_RESPONSE_BUFFER_SIZE) {
remote_mtu = SDP_RESPONSE_BUFFER_SIZE;
}
// log_info("SDP Request: type %u, transaction id %u, len %u, mtu %u", pdu_id, transaction_id, param_len, remote_mtu);
switch (pdu_id) {
case SDP_ServiceSearchRequest:
sdp_response_size = sdp_handle_service_search_request(packet, remote_mtu);
break;
case SDP_ServiceAttributeRequest:
sdp_response_size = sdp_handle_service_attribute_request(packet, remote_mtu);
break;
case SDP_ServiceSearchAttributeRequest:
sdp_response_size = sdp_handle_service_search_attribute_request(packet, remote_mtu);
break;
default:
sdp_response_size = sdp_create_error_response(transaction_id, 0x0003); // invalid syntax
break;
}
sdp_try_respond();
break;
case HCI_EVENT_PACKET:
switch (packet[0]) {
case L2CAP_EVENT_INCOMING_CONNECTION:
if (l2cap_cid) {
// CONNECTION REJECTED DUE TO LIMITED RESOURCES
l2cap_decline_connection_internal(channel, 0x04);
break;
}
// accept
l2cap_cid = channel;
sdp_response_size = 0;
l2cap_accept_connection_internal(channel);
break;
case L2CAP_EVENT_CHANNEL_OPENED:
if (packet[2]) {
// open failed -> reset
l2cap_cid = 0;
}
break;
case L2CAP_EVENT_CREDITS:
case DAEMON_EVENT_HCI_PACKET_SENT:
sdp_try_respond();
break;
case L2CAP_EVENT_CHANNEL_CLOSED:
if (channel == l2cap_cid) {
// reset
l2cap_cid = 0;
}
break;
default:
// other event
break;
}
break;
default:
// other packet type
break;
}
}
int sdp_handle_service_search_attribute_request(uint8_t * packet, uint16_t remote_mtu) {
// SDP header before attribute sevice list: 7
// Continuation, worst case: 5
// get request details
uint16_t transaction_id = READ_NET_16(packet, 1);
// not used yet - uint16_t param_len = READ_NET_16(packet, 3);
uint8_t * serviceSearchPattern = &packet[5];
uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
uint16_t maximumAttributeByteCount = READ_NET_16(packet, 5 + serviceSearchPatternLen);
uint8_t * attributeIDList = &packet[5+serviceSearchPatternLen+2];
uint16_t attributeIDListLen = de_get_len(attributeIDList);
uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2+attributeIDListLen];
// calc maximumAttributeByteCount based on remote MTU, SDP header and reserved Continuation block
uint16_t maximumAttributeByteCount2 = remote_mtu - 12;
if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
maximumAttributeByteCount = maximumAttributeByteCount2;
}
// continuation state contains: index of next service record to examine
// continuation state contains: byte offset into this service record
uint16_t continuation_service_index = 0;
uint16_t continuation_offset = 0;
if (continuationState[0] == 4) {
continuation_service_index = READ_NET_16(continuationState, 1);
continuation_offset = READ_NET_16(continuationState, 3);
}
// log_info("--> sdp_handle_service_search_attribute_request, cont %u/%u, max %u", continuation_service_index, continuation_offset, maximumAttributeByteCount);
// AttributeLists - starts at offset 7
uint16_t pos = 7;
// add DES with total size for first request
if (continuation_service_index == 0 && continuation_offset == 0) {
uint16_t total_response_size = sdp_get_size_for_service_search_attribute_response(serviceSearchPattern, attributeIDList);
de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size);
// log_info("total response size %u", total_response_size);
pos += 3;
maximumAttributeByteCount -= 3;
}
// create attribute list
int first_answer = 1;
int continuation = 0;
uint16_t current_service_index = 0;
linked_item_t *it = (linked_item_t *) sdp_service_records;
for ( ; it ; it = it->next, ++current_service_index) {
service_record_item_t * item = (service_record_item_t *) it;
if (current_service_index < continuation_service_index ) continue;
if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
if (continuation_offset == 0) {
// get size of this record
uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList);
// stop if complete record doesn't fits into response but we already have a partial response
if ((filtered_attributes_size + 3 > maximumAttributeByteCount) && !first_answer) {
continuation = 1;
break;
}
// store DES
de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
pos += 3;
maximumAttributeByteCount -= 3;
}
first_answer = 0;
// copy maximumAttributeByteCount from record
uint16_t bytes_used;
int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
pos += bytes_used;
maximumAttributeByteCount -= bytes_used;
if (complete) {
continuation_offset = 0;
continue;
}
continuation = 1;
continuation_offset += bytes_used;
break;
}
uint16_t attributeListsByteCount = pos - 7;
// Continuation State
if (continuation) {
sdp_response_buffer[pos++] = 4;
net_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index);
pos += 2;
net_store_16(sdp_response_buffer, pos, continuation_offset);
pos += 2;
} else {
// complete
sdp_response_buffer[pos++] = 0;
}
// create SDP header
sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse;
net_store_16(sdp_response_buffer, 1, transaction_id);
net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
net_store_16(sdp_response_buffer, 5, attributeListsByteCount);
return pos;
}
int sdp_handle_service_attribute_request(uint8_t * packet, uint16_t remote_mtu) {
// get request details
uint16_t transaction_id = READ_NET_16(packet, 1);
// not used yet - uint16_t param_len = READ_NET_16(packet, 3);
uint32_t serviceRecordHandle = READ_NET_32(packet, 5);
uint16_t maximumAttributeByteCount = READ_NET_16(packet, 9);
uint8_t * attributeIDList = &packet[11];
uint16_t attributeIDListLen = de_get_len(attributeIDList);
uint8_t * continuationState = &packet[11+attributeIDListLen];
// calc maximumAttributeByteCount based on remote MTU
uint16_t maximumAttributeByteCount2 = remote_mtu - (7+3);
if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
maximumAttributeByteCount = maximumAttributeByteCount2;
}
// continuation state contains the offset into the complete response
uint16_t continuation_offset = 0;
if (continuationState[0] == 2) {
continuation_offset = READ_NET_16(continuationState, 1);
}
// get service record
service_record_item_t * item = sdp_get_record_for_handle(serviceRecordHandle);
if (!item) {
// service record handle doesn't exist
return sdp_create_error_response(transaction_id, 0x0002); /// invalid Service Record Handle
}
// AttributeList - starts at offset 7
uint16_t pos = 7;
if (continuation_offset == 0) {
// get size of this record
uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList);
// store DES
de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
maximumAttributeByteCount -= 3;
pos += 3;
}
// copy maximumAttributeByteCount from record
uint16_t bytes_used;
int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
pos += bytes_used;
uint16_t attributeListByteCount = pos - 7;
if (complete) {
sdp_response_buffer[pos++] = 0;
} else {
continuation_offset += bytes_used;
sdp_response_buffer[pos++] = 2;
net_store_16(sdp_response_buffer, pos, continuation_offset);
pos += 2;
}
// header
sdp_response_buffer[0] = SDP_ServiceAttributeResponse;
net_store_16(sdp_response_buffer, 1, transaction_id);
net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
net_store_16(sdp_response_buffer, 5, attributeListByteCount);
return pos;
}
int sdp_handle_service_search_request(uint8_t * packet, uint16_t remote_mtu) {
// get request details
uint16_t transaction_id = READ_NET_16(packet, 1);
// not used yet - uint16_t param_len = READ_NET_16(packet, 3);
uint8_t * serviceSearchPattern = &packet[5];
uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
uint16_t maximumServiceRecordCount = READ_NET_16(packet, 5 + serviceSearchPatternLen);
uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2];
// calc maxumumServiceRecordCount based on remote MTU
uint16_t maxNrServiceRecordsPerResponse = (remote_mtu - (9+3))/4;
// continuation state contains index of next service record to examine
int continuation = 0;
uint16_t continuation_index = 0;
if (continuationState[0] == 2) {
continuation_index = READ_NET_16(continuationState, 1);
}
// get and limit total count
linked_item_t *it;
uint16_t total_service_count = 0;
for (it = (linked_item_t *) sdp_service_records; it ; it = it->next) {
service_record_item_t * item = (service_record_item_t *) it;
if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
total_service_count++;
}
if (total_service_count > maximumServiceRecordCount) {
total_service_count = maximumServiceRecordCount;
}
// ServiceRecordHandleList at 9
uint16_t pos = 9;
uint16_t current_service_count = 0;
uint16_t current_service_index = 0;
uint16_t matching_service_count = 0;
for (it = (linked_item_t *) sdp_service_records; it ; it = it->next, ++current_service_index) {
service_record_item_t * item = (service_record_item_t *) it;
if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
matching_service_count++;
if (current_service_index < continuation_index) continue;
net_store_32(sdp_response_buffer, pos, item->service_record_handle);
pos += 4;
current_service_count++;
if (matching_service_count >= total_service_count) break;
if (current_service_count >= maxNrServiceRecordsPerResponse) {
continuation = 1;
continuation_index = current_service_index + 1;
break;
}
}
// Store continuation state
if (continuation) {
sdp_response_buffer[pos++] = 2;
net_store_16(sdp_response_buffer, pos, continuation_index);
pos += 2;
} else {
sdp_response_buffer[pos++] = 0;
}
// header
sdp_response_buffer[0] = SDP_ServiceSearchResponse;
net_store_16(sdp_response_buffer, 1, transaction_id);
net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
net_store_16(sdp_response_buffer, 5, total_service_count);
net_store_16(sdp_response_buffer, 7, current_service_count);
return pos;
}
/*
parse the sdp record:
type DES (6), element len 26
type DES (6), element len 24
type UINT (1), element len 3 , value: 0x00000004
type DES (6), element len 19
type DES (6), element len 5
type UUID (3), element len 3 , value: 0x00000100
type DES (6), element len 7
type UUID (3), element len 3 , value: 0x00000003
type UINT (1), element len 2 , value: 0x00000002
type DES (6), element len 5
type UUID (3), element len 3 , value: 0x00000008
*/
static void sdpc_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
{
if (packet_type == HCI_EVENT_PACKET)
{
switch(packet[0]){
case L2CAP_EVENT_CHANNEL_OPENED:
{
if (packet[2]) {
log_info("Connection failed\n");
return;
}
log_info("Connected\n");
state = SENDING;
current_server = 0;
l2cap_cid = READ_BT_16(packet, 13);
sdpc_trysend();
break;
}
case DAEMON_EVENT_HCI_PACKET_SENT:
case L2CAP_EVENT_CREDITS:
{
sdpc_trysend();
break;
}
case L2CAP_EVENT_CHANNEL_CLOSED:
if (channel == l2cap_cid){
// reset
l2cap_cid = 0;
}
break;
}
}
if (packet_type == L2CAP_DATA_PACKET){
log_info("SDP Respone %d \n", READ_NET_16(packet, 5));
//de_dump_data_element(&packet[7]);
//check if valid answer returns
if (READ_NET_16(packet, 5) > 2)
{
switch(current_server)
{
case 0:
{
hfp_port = sdp_get_parameters_for_uuid(&packet[7], 0x0003);;
log_info("hfp port: %d\n", hfp_port);
break;
}
case 1:
break;
case 2:
{
mas_port = sdp_get_parameters_for_uuid(&packet[7], 0x0003);;
log_info("MAP port: %d\n", mas_port );
break;
}
}
}
current_server++;
if (current_server == 3)
{
state = DONE;
l2cap_close_connection(¤t_server);
if (hfp_port != 0)
hfp_open(&addr, hfp_port);
// if (mas_port != 0)
// mas_open(&addr, mas_port);
}
else
{
state = SENDING;
sdpc_trysend();
}
}
}
void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
// uint16_t handle;
if (packet_type == L2CAP_DATA_PACKET){
uint16_t responseTransactionID = READ_NET_16(packet,1);
if ( responseTransactionID != transactionID){
log_error("Missmatching transaction ID, expected %u, found %u.", transactionID, responseTransactionID);
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 = packet[0];
log_info("SDP Client :: PDU ID. %u ,%u", PDU_ID, packet[0]);
switch (PDU_ID){
#ifdef HAVE_SDP_EXTRA_QUERIES
case SDP_ServiceSearchResponse:
parse_service_search_response(packet);
break;
case SDP_ServiceAttributeResponse:
parse_service_attribute_response(packet);
break;
#endif
case SDP_ServiceSearchAttributeResponse:
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_info("SDP Client Query DONE! ");
sdp_client_state = QUERY_COMPLETE;
l2cap_disconnect_internal(sdp_cid, 0);
// sdp_parser_handle_done(0);
return;
}
// prepare next request and send
sdp_client_state = W2_SEND;
if (can_send_now(sdp_cid)) send_request(sdp_cid);
return;
}
if (packet_type != HCI_EVENT_PACKET) return;
switch(packet[0]){
case L2CAP_EVENT_TIMEOUT_CHECK:
log_info("sdp client: L2CAP_EVENT_TIMEOUT_CHECK");
break;
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_error("SDP Client Connection failed.");
sdp_parser_handle_done(packet[2]);
break;
}
sdp_cid = channel;
mtu = READ_BT_16(packet, 17);
// handle = READ_BT_16(packet, 9);
log_info("SDP Client Connected, cid %x, mtu %u.", sdp_cid, mtu);
sdp_client_state = W2_SEND;
if (can_send_now(sdp_cid)) send_request(sdp_cid);
break;
case L2CAP_EVENT_CREDITS:
case DAEMON_EVENT_HCI_PACKET_SENT:
if (can_send_now(sdp_cid)) send_request(sdp_cid);
break;
case L2CAP_EVENT_CHANNEL_CLOSED: {
if (sdp_cid != READ_BT_16(packet, 2)) {
// log_info("Received L2CAP_EVENT_CHANNEL_CLOSED for cid %x, current cid %x\n", READ_BT_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;
}
}
void sdp_test(){
const uint16_t remote_mtu = 48;
uint8_t allAttributeIDs[20]; //
// create an attribute list
de_create_sequence(allAttributeIDs);
de_add_number(allAttributeIDs, DE_UINT, DE_SIZE_32, 0x0000ffff);
// create two records with 2 attributes each
de_create_sequence(record);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle);
de_add_number(record, DE_UINT, DE_SIZE_32, 0x10001);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
uint32_t handle_1 = sdp_register_service_internal(NULL, record);
de_dump_data_element(record);
de_create_sequence(record);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle);
de_add_number(record, DE_UINT, DE_SIZE_32, 0x10002);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0002);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
sdp_register_service_internal(NULL, record);
de_dump_data_element(record);
uint16_t size = spd_get_filtered_size(record, allAttributeIDs);
printf("Attribute size %u\n", size);
uint16_t transactionID = 1;
uint8_t * attributeIDList;
uint16_t attributeIDListLen;
uint16_t response_pos;
uint8_t * serviceSearchPattern;
uint16_t serviceSearchPatternLen;
#if 1
// sdp_handle_service_search_request
uint16_t nr_services = 1;
request[0] = SDP_ServiceSearchRequest;
net_store_16(request, 1, transactionID++); // transaction ID
serviceSearchPattern = &request[5];
de_create_sequence(serviceSearchPattern);
{
de_add_number(serviceSearchPattern, DE_UUID, DE_SIZE_16, 0x0001);
}
serviceSearchPatternLen = de_get_len(serviceSearchPattern);
net_store_16(request, 5 + serviceSearchPatternLen, 2); // max
request[5 + serviceSearchPatternLen + 2] = 0; // cont
sdp_handle_service_search_request(request, 16);
dump_service_search_response();
memcpy(request + 5 + serviceSearchPatternLen + 2, sdp_response_buffer + 9 + nr_services*4, 3);
sdp_handle_service_search_request(request, remote_mtu);
dump_service_search_response();
#endif
#if 1
// sdp_handle_service_attribute_request
request[0] = SDP_ServiceAttributeRequest;
net_store_16(request, 1, transactionID++); // transaction ID
net_store_32(request, 5, handle_1); // record handle
net_store_16(request, 9, 11); // max bytes
attributeIDList = request + 11;
de_create_sequence(attributeIDList);
de_add_number(attributeIDList, DE_UINT, DE_SIZE_32, 0x0000ffff);
attributeIDListLen = de_get_len(attributeIDList);
request[11+attributeIDListLen] = 0;
response_pos = 0;
while(1) {
sdp_handle_service_attribute_request(request, remote_mtu);
uint16_t attributeListByteCount = READ_NET_16(sdp_response_buffer, 5);
memcpy( &response[response_pos], &sdp_response_buffer[7], attributeListByteCount);
response_pos += attributeListByteCount;
printf("attributeListByteCount %u\n", attributeListByteCount);
printf("Continuation %u\n", sdp_response_buffer[7+attributeListByteCount]);
if (sdp_response_buffer[7+attributeListByteCount] == 0) break;
printf("Continuation {%u}\n", READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+1));
memcpy(request+11+attributeIDListLen, sdp_response_buffer+7+attributeListByteCount, 3);
}
de_dump_data_element(response);
#endif
#if 1
// sdp_handle_service_search_attribute_request
request[0] = SDP_ServiceSearchAttributeRequest;
net_store_16(request, 1, transactionID++); // transaction ID
serviceSearchPattern = &request[5];
de_create_sequence(serviceSearchPattern);
{
de_add_number(serviceSearchPattern, DE_UUID, DE_SIZE_16, 0x0001);
}
serviceSearchPatternLen = de_get_len(serviceSearchPattern);
net_store_16(request, 5 + serviceSearchPatternLen, 11); // MaximumAttributeByteCount:
attributeIDList = request + 5 + serviceSearchPatternLen + 2;
de_create_sequence(attributeIDList);
de_add_number(attributeIDList, DE_UINT, DE_SIZE_32, 0x0000ffff);
attributeIDListLen = de_get_len(attributeIDList);
request[5 + serviceSearchPatternLen + 2 + attributeIDListLen] = 0;
response_pos = 0;
while (1) {
sdp_handle_service_search_attribute_request(request, remote_mtu);
uint16_t attributeListByteCount = READ_NET_16(sdp_response_buffer, 5);
memcpy( &response[response_pos], &sdp_response_buffer[7], attributeListByteCount);
response_pos += attributeListByteCount;
printf("attributeListByteCount %u\n", attributeListByteCount);
printf("Continuation %u\n", sdp_response_buffer[7+attributeListByteCount]);
if (sdp_response_buffer[7+attributeListByteCount] == 0) break;
printf("Continuation {%u,%u}\n", READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+1),
READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+3));
memcpy(request+5 + serviceSearchPatternLen + 2 + attributeIDListLen, sdp_response_buffer+7+attributeListByteCount, 5);
}
de_dump_data_element(response);
#endif
exit(0);
}
