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