static uint16_t sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern, uint8_t * attributeIDList) { uint16_t total_response_size = 0; linked_item_t *it; 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; // for all service records that match total_response_size += 3 + spd_get_filtered_size(item->service_record, attributeIDList); } return total_response_size; }
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; }
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); }