mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-04-16 17:42:51 +00:00
Code Issues Packages Projects Releases Wiki Activity
btstack/src/classic/sdp_server.c
Matthias Ringwald a145b39262 sdp_server: use request_can_send_now strategy
2016-04-08 12:07:30 +02:00

537 lines
21 KiB
C
Raw Blame History

/*
* Copyright (C) 2014 BlueKitchen GmbH
*
* 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.
* 4. Any redistribution, use, or modification is done solely for
* personal benefit and not for any commercial purpose or for
* monetary gain.
*
* THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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.
*
* Please inquire about commercial licensing options at
* contact@bluekitchen-gmbh.com
*
*/
/*
* Implementation of the Service Discovery Protocol Server
*/
#include <stdio.h>
#include <string.h>
#include "btstack_debug.h"
#include "btstack_event.h"
#include "btstack_memory.h"
#include "classic/core.h"
#include "classic/sdp_server.h"
#include "classic/sdp_util.h"
#include "hci_dump.h"
#include "l2cap.h"
// max reserved ServiceRecordHandle
#define maxReservedServiceRecordHandle 0xffff
// max SDP response matches L2CAP PDU -- allow to use smaller buffer
#ifndef SDP_RESPONSE_BUFFER_SIZE
#define SDP_RESPONSE_BUFFER_SIZE (HCI_ACL_BUFFER_SIZE-HCI_ACL_HEADER_SIZE)
#endif
static void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
// registered service records
static btstack_linked_list_t sdp_service_records = NULL;
// our handles start after the reserved range
static uint32_t sdp_next_service_record_handle = ((uint32_t) maxReservedServiceRecordHandle) + 2;
static uint8_t sdp_response_buffer[SDP_RESPONSE_BUFFER_SIZE];
static uint16_t l2cap_cid = 0;
static uint16_t sdp_response_size = 0;
void sdp_init(void){
// register with l2cap psm sevices - max MTU
l2cap_register_service(sdp_packet_handler, PSM_SDP, 0xffff, LEVEL_0);
}
uint32_t sdp_get_service_record_handle(const uint8_t * record){
// TODO: make sdp_get_attribute_value_for_attribute_id accept const data to remove cast
uint8_t * serviceRecordHandleAttribute = sdp_get_attribute_value_for_attribute_id((uint8_t *)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 big_endian_read_32(serviceRecordHandleAttribute, 1);
}
static service_record_item_t * sdp_get_record_item_for_handle(uint32_t handle){
btstack_linked_item_t *it;
for (it = (btstack_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;
}
uint8_t * sdp_get_record_for_handle(uint32_t handle){
service_record_item_t * record_item = sdp_get_record_item_for_handle(handle);
if (!record_item) return 0;
return record_item->service_record;
}
// get next free, unregistered service record handle
uint32_t sdp_create_service_record_handle(void){
uint32_t handle = 0;
do {
handle = sdp_next_service_record_handle++;
if (sdp_get_record_item_for_handle(handle)) handle = 0;
} while (handle == 0);
return handle;
}
/**
* @brief Register Service Record with database using ServiceRecordHandle stored in record
* @pre AttributeIDs are in ascending order
* @pre ServiceRecordHandle is first attribute and valid
* @param record is not copied!
* @result status
*/
uint8_t sdp_register_service(const uint8_t * record){
// validate service record handle. it must: exist, be in valid range, not have been already used
uint32_t record_handle = sdp_get_service_record_handle(record);
if (!record_handle) return SDP_HANDLE_INVALID;
if (record_handle <= maxReservedServiceRecordHandle) return SDP_HANDLE_INVALID;
if (sdp_get_record_item_for_handle(record_handle)) return SDP_HANDLE_ALREADY_REGISTERED;
// alloc memory for new service_record_item
service_record_item_t * newRecordItem = btstack_memory_service_record_item_get();
if (!newRecordItem) return BTSTACK_MEMORY_ALLOC_FAILED;
// set handle and record
newRecordItem->service_record_handle = record_handle;
newRecordItem->service_record = (uint8_t*) record;
// add to linked list
btstack_linked_list_add(&sdp_service_records, (btstack_linked_item_t *) newRecordItem);
return 0;
}
//
// unregister service record
//
void sdp_unregister_service(uint32_t service_record_handle){
service_record_item_t * record_item = sdp_get_record_item_for_handle(service_record_handle);
if (!record_item) return;
btstack_linked_list_remove(&sdp_service_records, (btstack_linked_item_t *) record_item);
}
// PDU
// PDU ID (1), Transaction ID (2), Param Length (2), Param 1, Param 2, ..
static int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code){
sdp_response_buffer[0] = SDP_ErrorResponse;
big_endian_store_16(sdp_response_buffer, 1, transaction_id);
big_endian_store_16(sdp_response_buffer, 3, 2);
big_endian_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 = big_endian_read_16(packet, 1);
// not used yet - uint16_t param_len = big_endian_read_16(packet, 3);
uint8_t * serviceSearchPattern = &packet[5];
uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
uint16_t maximumServiceRecordCount = big_endian_read_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 = big_endian_read_16(continuationState, 1);
}
// get and limit total count
btstack_linked_item_t *it;
uint16_t total_service_count = 0;
for (it = (btstack_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 = (btstack_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;
big_endian_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;
big_endian_store_16(sdp_response_buffer, pos, continuation_index);
pos += 2;
} else {
sdp_response_buffer[pos++] = 0;
}
// header
sdp_response_buffer[0] = SDP_ServiceSearchResponse;
big_endian_store_16(sdp_response_buffer, 1, transaction_id);
big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
big_endian_store_16(sdp_response_buffer, 5, total_service_count);
big_endian_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 = big_endian_read_16(packet, 1);
// not used yet - uint16_t param_len = big_endian_read_16(packet, 3);
uint32_t serviceRecordHandle = big_endian_read_32(packet, 5);
uint16_t maximumAttributeByteCount = big_endian_read_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 = big_endian_read_16(continuationState, 1);
}
// get service record
service_record_item_t * item = sdp_get_record_item_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;
big_endian_store_16(sdp_response_buffer, pos, continuation_offset);
pos += 2;
}
// header
sdp_response_buffer[0] = SDP_ServiceAttributeResponse;
big_endian_store_16(sdp_response_buffer, 1, transaction_id);
big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
big_endian_store_16(sdp_response_buffer, 5, attributeListByteCount);
return pos;
}
static uint16_t sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern, uint8_t * attributeIDList){
uint16_t total_response_size = 0;
btstack_linked_item_t *it;
for (it = (btstack_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 = big_endian_read_16(packet, 1);
// not used yet - uint16_t param_len = big_endian_read_16(packet, 3);
uint8_t * serviceSearchPattern = &packet[5];
uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
uint16_t maximumAttributeByteCount = big_endian_read_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 = big_endian_read_16(continuationState, 1);
continuation_offset = big_endian_read_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;
btstack_linked_item_t *it = (btstack_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;
big_endian_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index);
pos += 2;
big_endian_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;
big_endian_store_16(sdp_response_buffer, 1, transaction_id);
big_endian_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
big_endian_store_16(sdp_response_buffer, 5, attributeListsByteCount);
return pos;
}
static void sdp_respond(void){
if (!sdp_response_size ) return;
if (!l2cap_cid) return;
// update state before sending packet (avoid getting called when new l2cap credit gets emitted)
uint16_t size = sdp_response_size;
sdp_response_size = 0;
l2cap_send(l2cap_cid, sdp_response_buffer, size);
}
// 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 = big_endian_read_16(packet, 1);
// param_len = big_endian_read_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;
}
if (!sdp_response_size) break;
l2cap_request_can_send_now_event(l2cap_cid);
break;
case HCI_EVENT_PACKET:
switch (hci_event_packet_get_type(packet)) {
case L2CAP_EVENT_INCOMING_CONNECTION:
if (l2cap_cid) {
// CONNECTION REJECTED DUE TO LIMITED RESOURCES
l2cap_decline_connection(channel, 0x04);
break;
}
// accept
l2cap_cid = channel;
sdp_response_size = 0;
l2cap_accept_connection(channel);
break;
case L2CAP_EVENT_CHANNEL_OPENED:
if (packet[2]) {
// open failed -> reset
l2cap_cid = 0;
}
break;
case L2CAP_EVENT_CAN_SEND_NOW:
sdp_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;
}
}
Reference in New Issue View Git Blame Copy Permalink