btstack/src/sdp_client.c

/*
 * Copyright (C) 2009-2013 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.
 * 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 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.
 *
 * Please inquire about commercial licensing options at btstack@ringwald.ch
 *
 */

/*
 *  sdp_client.c
 */

#include "btstack-config.h"
#include "sdp_client.h"

#include <btstack/hci_cmds.h>

#include "l2cap.h"
#include "sdp_parser.h"
#include "sdp.h"
#include "debug.h"

typedef enum {
    INIT, W4_CONNECT, W2_SEND, W4_RESPONSE
} sdp_client_state_t;


void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);

static uint16_t setup_service_search_attribute_request(uint8_t * data);

#ifdef HAVE_SDP_EXTRA_QUERIES
static uint16_t setup_service_search_request(uint8_t * data);
static uint16_t setup_service_attribute_request(uint8_t * data);
static void     parse_service_search_response(uint8_t* packet);
static void     parse_service_attribute_response(uint8_t* packet);
static uint32_t serviceRecordHandle;
#endif

// SDP Client Query
static uint16_t  mtu;
static uint16_t  sdp_cid = 0x40;
static uint8_t * serviceSearchPattern;
static uint8_t * attributeIDList;
static uint16_t  transactionID = 0;
static uint8_t   continuationState[16];
static uint8_t   continuationStateLen;

static sdp_client_state_t sdp_client_state;
static SDP_PDU_ID_t PDU_ID = SDP_Invalid;



void sdp_client_handle_done(uint8_t status){
    if (status == 0){
        l2cap_disconnect_internal(sdp_cid, 0);
    }
    sdp_parser_handle_done(status);
}

// TODO: inline if not needed (des(des))
void parse_attribute_lists(uint8_t* packet, uint16_t length){
    sdp_parser_handle_chunk(packet, length);
}

/* Queries the SDP service of the remote device given a service search pattern 
and a list of attribute IDs. The remote data is handled by the SDP parser. The 
SDP parser delivers attribute values and done event via a registered callback. */

void sdp_client_query(bd_addr_t remote, uint8_t * des_serviceSearchPattern, uint8_t * des_attributeIDList){
    serviceSearchPattern = des_serviceSearchPattern;
    attributeIDList = des_attributeIDList;
    continuationStateLen = 0;
    PDU_ID = SDP_ServiceSearchAttributeResponse;

    sdp_client_state = W4_CONNECT;
    l2cap_create_channel_internal(NULL, sdp_packet_handler, remote, PSM_SDP, l2cap_max_mtu());
}


static void try_to_send(uint16_t channel){
    if (sdp_client_state != W2_SEND) return;

    if (!l2cap_can_send_packet_now(channel)) return;

    l2cap_reserve_packet_buffer();
    uint8_t * data = l2cap_get_outgoing_buffer();
    uint16_t request_len = 0;

    switch (PDU_ID){
#ifdef HAVE_SDP_EXTRA_QUERIES
        case SDP_ServiceSearchResponse:
            request_len = setup_service_search_request(data);
            break;
        case SDP_ServiceAttributeResponse:
            request_len = setup_service_attribute_request(data);
            break;
#endif
        case SDP_ServiceSearchAttributeResponse:
            request_len = setup_service_search_attribute_request(data);
            break;
        default:
            log_info("SDP Client try_to_send :: PDU ID invalid. %u", PDU_ID);
            return;
    }

    int err = l2cap_send_prepared(channel, request_len);
    switch (err){
        case 0:
            // packet is sent prepare next one
            // printf("l2cap_send_internal() -> OK\n\r");
            PDU_ID = SDP_Invalid;
            sdp_client_state = W4_RESPONSE;
            break;
        case BTSTACK_ACL_BUFFERS_FULL:
            log_info("l2cap_send_internal() ->BTSTACK_ACL_BUFFERS_FULL\n\r");
            break;
        default:
            log_error("l2cap_send_internal() -> err %d\n\r", err);
            break;
    }
}


static void parse_service_search_attribute_response(uint8_t* packet){
    uint16_t offset = 3;
    uint16_t parameterLength = READ_NET_16(packet,offset);
    offset+=2;
    // AttributeListByteCount <= mtu
    uint16_t attributeListByteCount = READ_NET_16(packet,offset);
    offset+=2;

    if (attributeListByteCount > mtu){
        log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in found attribute list is larger then the MaximumAttributeByteCount.\n");
        return;
    }

    // AttributeLists
    parse_attribute_lists(packet+offset, attributeListByteCount);
    offset+=attributeListByteCount;

    continuationStateLen = packet[offset];
    offset++;

    if (continuationStateLen > 16){
        log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in continuation state exceedes 16.\n");
        return;
    }
    memcpy(continuationState, packet+offset, continuationStateLen);
    offset+=continuationStateLen;

    if (parameterLength != offset - 5){
        log_error("Error parsing ServiceSearchAttributeResponse: wrong size of parameters, number of expected bytes%u, actual number %u.\n", parameterLength, offset);
    }
}

void sdp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
    // uint16_t handle;

    if (packet_type == L2CAP_DATA_PACKET){

        uint16_t responseTransactionID = READ_NET_16(packet,1);
        if ( responseTransactionID != transactionID){
            log_error("Missmatching transaction ID, expected %u, found %u.\n", transactionID, responseTransactionID);
            return;
        } 
        
        if (packet[0] != SDP_ServiceSearchAttributeResponse 
            && packet[0] != SDP_ServiceSearchResponse
            && packet[0] != SDP_ServiceAttributeResponse){
            log_error("Not a valid PDU ID, expected %u, %u or %u, found %u.\n", SDP_ServiceSearchResponse, 
                                    SDP_ServiceAttributeResponse, SDP_ServiceSearchAttributeResponse, packet[0]);
            return;
        }

        PDU_ID = packet[0];
        log_info("SDP Client :: PDU ID. %u ,%u", PDU_ID, packet[0]);
        switch (PDU_ID){
#ifdef HAVE_SDP_EXTRA_QUERIES
            case SDP_ServiceSearchResponse:
                parse_service_search_response(packet);
                break;
            case SDP_ServiceAttributeResponse:
                parse_service_attribute_response(packet);
                break;
#endif
            case SDP_ServiceSearchAttributeResponse:
                parse_service_search_attribute_response(packet);
                break;
            default:
                log_error("SDP Client :: PDU ID invalid. %u ,%u", PDU_ID, packet[0]);
                return;
        }

        // continuation set or DONE?
        if (continuationStateLen == 0){
            // printf("DONE! All clients already notified.\n");
            sdp_client_handle_done(0);
            sdp_client_state = INIT;
            return;
        }
        // prepare next request and send
        sdp_client_state = W2_SEND;
        try_to_send(sdp_cid);
    
        return;
    }
    
    if (packet_type != HCI_EVENT_PACKET) return;
    
    switch(packet[0]){

        case L2CAP_EVENT_CHANNEL_OPENED: 
            if (sdp_client_state != W4_CONNECT) break;

            // data: event (8), len(8), status (8), address(48), handle (16), psm (16), local_cid(16), remote_cid (16), local_mtu(16), remote_mtu(16) 
            if (packet[2]) {
                log_error("Connection failed.\n\r");
                sdp_client_handle_done(packet[2]);
                break;
            }
            sdp_cid = channel;
            mtu = READ_BT_16(packet, 17);
            // handle = READ_BT_16(packet, 9);
            log_info("Connected, cid %x, mtu %u.\n\r", sdp_cid, mtu);

            sdp_client_state = W2_SEND;
            try_to_send(sdp_cid);
            break;
        case L2CAP_EVENT_CREDITS:
        case DAEMON_EVENT_HCI_PACKET_SENT:
            try_to_send(sdp_cid);
            break;
        case L2CAP_EVENT_CHANNEL_CLOSED:
            log_info("Channel closed.\n\r");
            if (sdp_client_state == INIT) break;
            sdp_client_handle_done(SDP_QUERY_INCOMPLETE);
            break;
        default:
            break;
    }
}


static uint16_t setup_service_search_attribute_request(uint8_t * data){

    uint16_t offset = 0;
    transactionID++;
    // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest;
    data[offset++] = SDP_ServiceSearchAttributeRequest;
    // uint16_t transactionID
    net_store_16(data, offset, transactionID);
    offset += 2;

    // param legnth
    offset += 2;

    // parameters: 
    //     ServiceSearchPattern - DES (min 1 UUID, max 12)
    uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
    memcpy(data + offset, serviceSearchPattern, serviceSearchPatternLen);
    offset += serviceSearchPatternLen;

    //     MaximumAttributeByteCount - uint16_t  0x0007 - 0xffff -> mtu
    net_store_16(data, offset, mtu);
    offset += 2;

    //     AttibuteIDList  
    uint16_t attributeIDListLen = de_get_len(attributeIDList);
    memcpy(data + offset, attributeIDList, attributeIDListLen);
    offset += attributeIDListLen;

    //     ContinuationState - uint8_t number of cont. bytes N<=16 
    data[offset++] = continuationStateLen;
    //                       - N-bytes previous response from server
    memcpy(data + offset, continuationState, continuationStateLen);
    offset += continuationStateLen;

    // uint16_t paramLength 
    net_store_16(data, 3, offset - 5);

    return offset;
}

#ifdef HAVE_SDP_EXTRA_QUERIES
void parse_service_record_handle_list(uint8_t* packet, uint16_t total_count, uint16_t current_count){
    sdp_parser_handle_service_search(packet, total_count, current_count);
}

static uint16_t setup_service_search_request(uint8_t * data){
    uint16_t offset = 0;
    transactionID++;
    // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest;
    data[offset++] = SDP_ServiceSearchRequest;
    // uint16_t transactionID
    net_store_16(data, offset, transactionID);
    offset += 2;

    // param legnth
    offset += 2;

    // parameters: 
    //     ServiceSearchPattern - DES (min 1 UUID, max 12)
    uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
    memcpy(data + offset, serviceSearchPattern, serviceSearchPatternLen);
    offset += serviceSearchPatternLen;

    //     MaximumAttributeByteCount - uint16_t  0x0007 - 0xffff -> mtu
    net_store_16(data, offset, mtu);
    offset += 2;

    //     ContinuationState - uint8_t number of cont. bytes N<=16 
    data[offset++] = continuationStateLen;
    //                       - N-bytes previous response from server
    memcpy(data + offset, continuationState, continuationStateLen);
    offset += continuationStateLen;

    // uint16_t paramLength 
    net_store_16(data, 3, offset - 5);

    return offset;
}


static uint16_t setup_service_attribute_request(uint8_t * data){

    uint16_t offset = 0;
    transactionID++;
    // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest;
    data[offset++] = SDP_ServiceAttributeRequest;
    // uint16_t transactionID
    net_store_16(data, offset, transactionID);
    offset += 2;

    // param legnth
    offset += 2;

    // parameters: 
    //     ServiceRecordHandle
    net_store_32(data, offset, serviceRecordHandle);
    offset += 4;

    //     MaximumAttributeByteCount - uint16_t  0x0007 - 0xffff -> mtu
    net_store_16(data, offset, mtu);
    offset += 2;

    //     AttibuteIDList  
    uint16_t attributeIDListLen = de_get_len(attributeIDList);
    memcpy(data + offset, attributeIDList, attributeIDListLen);
    offset += attributeIDListLen;

    //     ContinuationState - uint8_t number of cont. bytes N<=16 
    data[offset++] = continuationStateLen;
    //                       - N-bytes previous response from server
    memcpy(data + offset, continuationState, continuationStateLen);
    offset += continuationStateLen;

    // uint16_t paramLength 
    net_store_16(data, 3, offset - 5);

    return offset;
}

static void parse_service_search_response(uint8_t* packet){
    uint16_t offset = 3;
    uint16_t parameterLength = READ_NET_16(packet,offset);
    offset+=2;

    uint16_t totalServiceRecordCount = READ_NET_16(packet,offset);
    offset+=2;

    uint16_t currentServiceRecordCount = READ_NET_16(packet,offset);
    offset+=2;

    if (currentServiceRecordCount > totalServiceRecordCount){
        log_error("CurrentServiceRecordCount is larger then TotalServiceRecordCount.\n");
        return;
    }
    
    parse_service_record_handle_list(packet+offset, totalServiceRecordCount, currentServiceRecordCount);
    offset+=(currentServiceRecordCount * 4);


    continuationStateLen = packet[offset];
    offset++;

    if (continuationStateLen > 16){
        log_error("Error parsing ServiceSearchResponse: Number of bytes in continuation state exceedes 16.\n");
        return;
    }
    memcpy(continuationState, packet+offset, continuationStateLen);
    offset+=continuationStateLen;

    if (parameterLength != offset - 5){
        log_error("Error parsing ServiceSearchResponse: wrong size of parameters, number of expected bytes%u, actual number %u.\n", parameterLength, offset);
    }
}

static void parse_service_attribute_response(uint8_t* packet){
    uint16_t offset = 3;
    uint16_t parameterLength = READ_NET_16(packet,offset);
    offset+=2;

    // AttributeListByteCount <= mtu
    uint16_t attributeListByteCount = READ_NET_16(packet,offset);
    offset+=2;

    if (attributeListByteCount > mtu){
        log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in found attribute list is larger then the MaximumAttributeByteCount.\n");
        return;
    }

    // AttributeLists
    parse_attribute_lists(packet+offset, attributeListByteCount);
    offset+=attributeListByteCount;

    continuationStateLen = packet[offset];
    offset++;

    if (continuationStateLen > 16){
        log_error("Error parsing ServiceAttributeResponse: Number of bytes in continuation state exceedes 16.\n");
        return;
    }
    memcpy(continuationState, packet+offset, continuationStateLen);
    offset+=continuationStateLen;

    if (parameterLength != offset - 5){
        log_error("Error parsing ServiceAttributeResponse: wrong size of parameters, number of expected bytes%u, actual number %u.\n", parameterLength, offset);
    }
}

void sdp_client_service_attribute_search(bd_addr_t remote, uint32_t search_serviceRecordHandle, uint8_t * des_attributeIDList){
    serviceRecordHandle = search_serviceRecordHandle;
    attributeIDList = des_attributeIDList;
    continuationStateLen = 0;
    PDU_ID = SDP_ServiceAttributeResponse;

    sdp_client_state = W4_CONNECT;
    l2cap_create_channel_internal(NULL, sdp_packet_handler, remote, PSM_SDP, l2cap_max_mtu());
}

void sdp_client_service_search(bd_addr_t remote, uint8_t * des_serviceSearchPattern){
    serviceSearchPattern = des_serviceSearchPattern;
    continuationStateLen = 0;
    PDU_ID = SDP_ServiceSearchResponse;

    sdp_client_state = W4_CONNECT;
    l2cap_create_channel_internal(NULL, sdp_packet_handler, remote, PSM_SDP, l2cap_max_mtu());
}
#endif