/* * Copyright (C) 2010 by Matthias Ringwald * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * 3. Neither the name of the copyright holders nor the names of * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * */ /* * Implementation of the Service Discovery Protocol Server */ #include "sdp.h" #include #include #include #include "l2cap.h" // max reserved ServiceRecordHandle #define maxReservedServiceRecordHandle 0xffff static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); // registered service records linked_list_t sdp_service_records; // our handles start after the reserved range static uint32_t sdp_next_service_record_handle = maxReservedServiceRecordHandle + 2; // AttributeIDList used to remove ServiceRecordHandle const uint8_t removeServiceRecordHandleAttributeIDList[] = { 0x36, 0x00, 0x05, 0x0A, 0x00, 0x01, 0xFF, 0xFF }; #define SDP_RESPONSE_BUFFER_SIZE HCI_ACL_3DH5_SIZE static uint8_t sdp_response_buffer[SDP_RESPONSE_BUFFER_SIZE]; void sdp_init(){ // register with l2cap psm sevices l2cap_register_service_internal(NULL, sdp_packet_handler, PSM_SDP, 1000); } uint32_t sdp_get_service_record_handle(uint8_t * record){ uint8_t * serviceRecordHandleAttribute = sdp_get_attribute_value_for_attribute_id(record, SDP_ServiceRecordHandle); if (!serviceRecordHandleAttribute) return 0; if (de_get_element_type(serviceRecordHandleAttribute) != DE_UINT) return 0; if (de_get_size_type(serviceRecordHandleAttribute) != DE_SIZE_32) return 0; return READ_NET_32(serviceRecordHandleAttribute, 1); } service_record_item_t * sdp_get_record_for_handle(uint32_t handle){ 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 (item->service_record_handle == handle){ return item; } } return NULL; } // get next free, unregistered service record handle uint32_t sdp_create_service_record_handle(){ uint32_t handle = 0; do { handle = sdp_next_service_record_handle++; if (sdp_get_record_for_handle(handle)) handle = 0; } while (handle == 0); return handle; } #ifdef EMBEDDED // register service record internally - this special version doesn't copy the record, it cannot be freeed // pre: AttributeIDs are in ascending order // pre: ServiceRecordHandle is first attribute and valid // pre: record // @returns ServiceRecordHandle or 0 if registration failed uint32_t sdp_register_service_internal(void *connection, service_record_item_t * record_item){ // get user record handle uint32_t record_handle = record_itme->service_record_handle; // validate service record handle is not in reserved range if (record_handle <= maxReservedServiceRecordHandle) record_handle = 0; // check if already registered if (sdp_get_record_for_handle(record_handle)) { return 0; } // add to linked list linked_list_add(&sdp_service_records, (linked_item_t *) record_item); return record_handle; } #else // 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 // printf("Register service record\n"); // 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) 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); // printf("reserved size %u, actual size %u\n", recordSize, de_get_len(newRecord)); // add to linked list linked_list_add(&sdp_service_records, (linked_item_t *) newRecordItem); return record_handle; } #endif // unregister service record internally // // makes sure one client cannot remove service records of other clients // void sdp_unregister_service_internal(void *connection, uint32_t service_record_handle){ service_record_item_t * record_item = sdp_get_record_for_handle(service_record_handle); if (record_item && record_item->connection == connection) { linked_list_remove(&sdp_service_records, (linked_item_t *) record_item); } } // remove all service record for a client connection void sdp_unregister_services_for_connection(void *connection){ linked_item_t *it = (linked_item_t *) &sdp_service_records; while (it->next){ service_record_item_t *record_item = (service_record_item_t *) it->next; if (record_item->connection == connection){ it->next = it->next->next; free(record_item); } else { it = it->next; } } } // PDU // PDU ID (1), Transaction ID (2), Param Length (2), Param 1, Param 2, .. int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code){ sdp_response_buffer[0] = SDP_ErrorResponse; net_store_16(sdp_response_buffer, 1, transaction_id); net_store_16(sdp_response_buffer, 3, 2); net_store_16(sdp_response_buffer, 5, error_code); // invalid syntax return 7; } 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 maximumServiceRecordCount2 = (remote_mtu - (9+3))/4; if (maximumServiceRecordCount2 < maximumServiceRecordCount) { maximumServiceRecordCount = maximumServiceRecordCount2; } // 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); } // header sdp_response_buffer[0] = SDP_ServiceSearchResponse; net_store_16(sdp_response_buffer, 1, transaction_id); // ServiceRecordHandleList at 9 uint16_t pos = 9; uint16_t total_service_count = 0; uint16_t current_service_count = 0; uint16_t current_service_index = 0; // for all service records that match linked_item_t *it; 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)){ // get total count total_service_count++; // add to list if index higher than last continuation index and space left if (current_service_index >= continuation_index && !continuation) { if ( current_service_count < maximumServiceRecordCount) { net_store_32(sdp_response_buffer, pos, item->service_record_handle); current_service_count++; pos += 4; } else { // next time start with this one continuation = 1; continuation_index = current_service_index; } } } } // 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; } // update header info 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; } 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 } // header sdp_response_buffer[0] = SDP_ServiceAttributeResponse; net_store_16(sdp_response_buffer, 1, transaction_id); // AttributeList - starts at offset 7 uint16_t pos = 7; uint8_t *attributeList = &sdp_response_buffer[pos]; de_create_sequence(attributeList); // copy ALL specified attributes sdp_append_attributes_in_attributeIDList(item->service_record, attributeIDList, 0, SDP_RESPONSE_BUFFER_SIZE-7-3, attributeList); pos += de_get_len(attributeList); // handle continuation if (continuation_offset){ memmove(&sdp_response_buffer[7], &sdp_response_buffer[7+continuation_offset], maximumAttributeByteCount); pos -= continuation_offset; } uint16_t attributeListByteCount = pos - 7; // Continuation State if (pos <= 7 + maximumAttributeByteCount) { // complete sdp_response_buffer[pos++] = 0; } else { pos = 7 + maximumAttributeByteCount; attributeListByteCount = maximumAttributeByteCount; continuation_offset += maximumAttributeByteCount; sdp_response_buffer[pos++] = 2; net_store_16(sdp_response_buffer, pos, (uint16_t) continuation_offset); pos += 2; } // update header 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_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); 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 uint16_t maximumAttributeByteCount2 = remote_mtu - (7+5); if (maximumAttributeByteCount2 < maximumAttributeByteCount) { maximumAttributeByteCount = maximumAttributeByteCount2; } // continuation state contains index of next service record to examine // continuation state contains the offset into this particular response uint16_t continuation_service_index = 0; uint16_t continuation_offset = 0; int continuation = 0; if (continuationState[0] == 4){ continuation_service_index = READ_NET_16(continuationState, 1); continuation_offset = READ_NET_16(continuationState, 3); } // AttributeLists - starts at offset 7 uint16_t pos = 7; int complete_response = 0; // get sum of all service attribute responses on first response and create DES header for it // note: this fills the response buffer several times uint16_t total_response_size = 0; linked_item_t *it; if (!continuation_service_index && !continuation_offset){ // for all service records that match 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)){ uint8_t *attributeList = &sdp_response_buffer[0]; de_create_sequence(attributeList); // copy ALL specified attributes sdp_append_attributes_in_attributeIDList(item->service_record, attributeIDList, 0, SDP_RESPONSE_BUFFER_SIZE-7-3, attributeList); total_response_size += de_get_len(attributeList); } } de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size); pos += de_get_header_size(&sdp_response_buffer[pos]); complete_response = total_response_size + 3 <= maximumAttributeByteCount; // all in one? } // also get highest index of matching record int highest_matching_record = -1; uint16_t current_service_index = 0; // for all service records that match 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)){ highest_matching_record = current_service_index; } } // header sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse; net_store_16(sdp_response_buffer, 1, transaction_id); // for all service records that match current_service_index = 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 (current_service_index >= continuation_service_index ) { if (sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)){ // create sequence and copy specified attributes uint8_t *attributeList = &sdp_response_buffer[pos]; de_create_sequence(attributeList); sdp_append_attributes_in_attributeIDList(item->service_record, attributeIDList, 0,SDP_RESPONSE_BUFFER_SIZE-7-3, attributeList); uint16_t listLen = de_get_len(attributeList); // handle continuation if (continuation_offset){ memmove(&sdp_response_buffer[pos], &sdp_response_buffer[pos+continuation_offset], maximumAttributeByteCount); listLen -= continuation_offset; } pos += listLen; // complete response from this record? if (pos - 7 <= maximumAttributeByteCount){ continuation_offset = 0; current_service_index++; if (current_service_index > highest_matching_record) { // done continuation = 0; break; } else { if (!complete_response){ continuation = 1; break; } } } else { continuation_offset += maximumAttributeByteCount; if (!continuation_service_index && continuation_offset == maximumAttributeByteCount){ continuation_offset -= 3; // without initial header } complete_response = 0; continuation = 1; break; } } } } // Continuation State uint16_t attributeListByteCount = pos - 7; if (continuation){ if (pos > 7 + maximumAttributeByteCount) { pos = 7 + maximumAttributeByteCount; attributeListByteCount = maximumAttributeByteCount; } 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; } // update header net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload net_store_16(sdp_response_buffer, 5, attributeListByteCount); // AttributeListsByteCount return pos; } 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 param_len; uint16_t remote_mtu; int pos = 5; switch (packet_type) { case L2CAP_DATA_PACKET: pdu_id = 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; } // printf("SDP Request: type %u, transaction id %u, len %u, mtu %u\n", pdu_id, transaction_id, param_len, remote_mtu); switch (pdu_id){ case SDP_ServiceSearchRequest: pos = sdp_handle_service_search_request(packet, remote_mtu); break; case SDP_ServiceAttributeRequest: pos = sdp_handle_service_attribute_request(packet, remote_mtu); break; case SDP_ServiceSearchAttributeRequest: pos = sdp_handle_service_search_attribute_request(packet, remote_mtu); break; default: pos = sdp_create_error_response(transaction_id, 0x0003); // invalid syntax break; } l2cap_send_internal(channel, sdp_response_buffer, pos); break; case HCI_EVENT_PACKET: switch (packet[0]) { case L2CAP_EVENT_INCOMING_CONNECTION: // accept l2cap_accept_connection_internal(channel); break; default: // other event break; } break; default: // other packet type break; } } #if 0 static uint8_t record[100]; static uint8_t request[100]; static uint8_t response[100]; static void dump_service_search_response(){ 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