int de_state_size(uint8_t eventByte, de_state_t *de_state) {
if (de_state->in_state_GET_DE_HEADER_LENGTH) {
de_state->addon_header_bytes = de_get_header_size(&eventByte) - 1;
de_state->de_size = 0;
de_state->de_offset = 0;
if (de_state->addon_header_bytes == 0) {
de_state->de_size = de_get_data_size(&eventByte);
if (de_state->de_size == 0) {
log_error(" ERROR: ID size is zero");
}
// log_info("Data element payload is %d bytes.", de_state->de_size);
return 1;
}
de_state->in_state_GET_DE_HEADER_LENGTH = 0;
return 0;
}
if (de_state->addon_header_bytes > 0) {
de_state->de_size = (de_state->de_size << 8) | eventByte;
de_state->addon_header_bytes--;
}
if (de_state->addon_header_bytes > 0) return 0;
// log_info("Data element payload is %d bytes.", de_state->de_size);
de_state->in_state_GET_DE_HEADER_LENGTH = 1;
return 1;
}
// register service record internally - the normal version creates a copy of the record
// pre: AttributeIDs are in ascending order => ServiceRecordHandle is first attribute if present
// @returns ServiceRecordHandle or 0 if registration failed
uint32_t sdp_register_service_internal(void *connection, uint8_t * record) {
// dump for now
// log_info("Register service record");
// de_dump_data_element(record);
// get user record handle
uint32_t record_handle = sdp_get_service_record_handle(record);
// validate service record handle is not in reserved range
if (record_handle <= maxReservedServiceRecordHandle) record_handle = 0;
// check if already in use
if (record_handle) {
if (sdp_get_record_for_handle(record_handle)) {
record_handle = 0;
}
}
// create new handle if needed
if (!record_handle) {
record_handle = sdp_create_service_record_handle();
}
// calculate size of new service record: DES (2 byte len)
// + ServiceRecordHandle attribute (UINT16 UINT32) + size of existing attributes
uint16_t recordSize = 3 + (3 + 5) + de_get_data_size(record);
// alloc memory for new service_record_item
service_record_item_t * newRecordItem = (service_record_item_t *) malloc(recordSize + sizeof(service_record_item_t));
if (!newRecordItem) {
sdp_emit_service_registered(connection, 0, BTSTACK_MEMORY_ALLOC_FAILED);
return 0;
}
// link new service item to client connection
newRecordItem->connection = connection;
// set new handle
newRecordItem->service_record_handle = record_handle;
// create updated service record
uint8_t * newRecord = (uint8_t *) &(newRecordItem->service_record);
// create DES for new record
de_create_sequence(newRecord);
// set service record handle
de_add_number(newRecord, DE_UINT, DE_SIZE_16, 0);
de_add_number(newRecord, DE_UINT, DE_SIZE_32, record_handle);
// add other attributes
sdp_append_attributes_in_attributeIDList(record, (uint8_t *) removeServiceRecordHandleAttributeIDList, 0, recordSize, newRecord);
// dump for now
// de_dump_data_element(newRecord);
// log_info("reserved size %u, actual size %u", recordSize, de_get_len(newRecord));
// add to linked list
linked_list_add(&sdp_service_records, (linked_item_t *) newRecordItem);
sdp_emit_service_registered(connection, 0, newRecordItem->service_record_handle);
return record_handle;
}
static void handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
UNUSED(packet_type);
UNUSED(channel);
UNUSED(size);
des_iterator_t attribute_list_it;
des_iterator_t additional_des_it;
des_iterator_t prot_it;
uint8_t *des_element;
uint8_t *element;
uint32_t uuid;
uint8_t status;
switch (hci_event_packet_get_type(packet)){
case SDP_EVENT_QUERY_ATTRIBUTE_VALUE:
if (sdp_event_query_attribute_byte_get_attribute_length(packet) <= attribute_value_buffer_size) {
attribute_value[sdp_event_query_attribute_byte_get_data_offset(packet)] = sdp_event_query_attribute_byte_get_data(packet);
if ((uint16_t)(sdp_event_query_attribute_byte_get_data_offset(packet)+1) == sdp_event_query_attribute_byte_get_attribute_length(packet)) {
switch(sdp_event_query_attribute_byte_get_attribute_id(packet)) {
case BLUETOOTH_ATTRIBUTE_PROTOCOL_DESCRIPTOR_LIST:
for (des_iterator_init(&attribute_list_it, attribute_value); des_iterator_has_more(&attribute_list_it); des_iterator_next(&attribute_list_it)) {
if (des_iterator_get_type(&attribute_list_it) != DE_DES) continue;
des_element = des_iterator_get_element(&attribute_list_it);
des_iterator_init(&prot_it, des_element);
element = des_iterator_get_element(&prot_it);
if (de_get_element_type(element) != DE_UUID) continue;
uuid = de_get_uuid32(element);
switch (uuid){
case BLUETOOTH_PROTOCOL_L2CAP:
if (!des_iterator_has_more(&prot_it)) continue;
des_iterator_next(&prot_it);
de_element_get_uint16(des_iterator_get_element(&prot_it), &hid_control_psm);
printf("HID Control PSM: 0x%04x\n", (int) hid_control_psm);
break;
default:
break;
}
}
break;
case BLUETOOTH_ATTRIBUTE_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS:
for (des_iterator_init(&attribute_list_it, attribute_value); des_iterator_has_more(&attribute_list_it); des_iterator_next(&attribute_list_it)) {
if (des_iterator_get_type(&attribute_list_it) != DE_DES) continue;
des_element = des_iterator_get_element(&attribute_list_it);
for (des_iterator_init(&additional_des_it, des_element); des_iterator_has_more(&additional_des_it); des_iterator_next(&additional_des_it)) {
if (des_iterator_get_type(&additional_des_it) != DE_DES) continue;
des_element = des_iterator_get_element(&additional_des_it);
des_iterator_init(&prot_it, des_element);
element = des_iterator_get_element(&prot_it);
if (de_get_element_type(element) != DE_UUID) continue;
uuid = de_get_uuid32(element);
switch (uuid){
case BLUETOOTH_PROTOCOL_L2CAP:
if (!des_iterator_has_more(&prot_it)) continue;
des_iterator_next(&prot_it);
de_element_get_uint16(des_iterator_get_element(&prot_it), &hid_interrupt_psm);
printf("HID Interrupt PSM: 0x%04x\n", (int) hid_interrupt_psm);
break;
default:
break;
}
}
}
break;
case BLUETOOTH_ATTRIBUTE_HID_DESCRIPTOR_LIST:
for (des_iterator_init(&attribute_list_it, attribute_value); des_iterator_has_more(&attribute_list_it); des_iterator_next(&attribute_list_it)) {
if (des_iterator_get_type(&attribute_list_it) != DE_DES) continue;
des_element = des_iterator_get_element(&attribute_list_it);
for (des_iterator_init(&additional_des_it, des_element); des_iterator_has_more(&additional_des_it); des_iterator_next(&additional_des_it)) {
if (des_iterator_get_type(&additional_des_it) != DE_STRING) continue;
element = des_iterator_get_element(&additional_des_it);
const uint8_t * descriptor = de_get_string(element);
hid_descriptor_len = de_get_data_size(element);
memcpy(hid_descriptor, descriptor, hid_descriptor_len);
printf("HID Descriptor:\n");
printf_hexdump(hid_descriptor, hid_descriptor_len);
}
}
break;
default:
break;
}
}
} else {
fprintf(stderr, "SDP attribute value buffer size exceeded: available %d, required %d\n", attribute_value_buffer_size, sdp_event_query_attribute_byte_get_attribute_length(packet));
}
break;
case SDP_EVENT_QUERY_COMPLETE:
if (!hid_control_psm) {
printf("HID Control PSM missing\n");
break;
}
if (!hid_interrupt_psm) {
printf("HID Interrupt PSM missing\n");
break;
}
printf("Setup HID\n");
status = l2cap_create_channel(packet_handler, remote_addr, hid_control_psm, 48, &l2cap_hid_control_cid);
if (status){
printf("Connecting to HID Control failed: 0x%02x\n", status);
}
break;
}
}
