sdp_parser.c

bluetooth 驱动

/*
 * sdp_parser.c -- This file describes a parser that parses incomming
 *                 bluetooth sdp data and sends it to the sdp_server
 *
 * Copyright (C) 2000, 2001  Axis Communications AB
 *
 * Author: Mats Friden <[e-mail]>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 * Exceptionally, Axis Communications AB grants discretionary and
 * conditional permissions for additional use of the text contained
 * in the company's release of the AXIS OpenBT Stack under the
 * provisions set forth hereunder.
 *
 * Provided that, if you use the AXIS OpenBT Stack with other files,
 * that do not implement functionality as specified in the Bluetooth
 * System specification, to produce an executable, this does not by
 * itself cause the resulting executable to be covered by the GNU
 * General Public License. Your use of that executable is in no way
 * restricted on account of using the AXIS OpenBT Stack code with it.
 *
 * This exception does not however invalidate any other reasons why
 * the executable file might be covered by the provisions of the GNU
 * General Public License.
 *
 * $Id: sdp_parser.c,v 1.21 2001/08/27 15:08:58 mattiasagren Exp $
 *
 */

/****************** INCLUDE FILES SECTION ***********************************/

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <sys/time.h>
#include <syslog.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <sys/ioctl.h>
#include <sys/uio.h>

#include "sdp_server.h"
#include "sdp_parser.h"

/****************** CONSTANT AND MACRO SECTION ******************************/

#define SDP_DEBUG_XMIT       0
#define SDP_DEBUG_REC        0
#define SDP_DEBUG_MISC       0
#define SDP_DEBUG_MEM        0
#define SDP_DEBUG_PROC       0
#define SDP_DEBUG_PRINT_DATA 0

#ifdef BTD_USERSTACK
#define syslog(x, fmt...) do { fprintf(stderr, __FILE__ "::"); fprintf(stderr, fmt); fprintf(stderr, "\n"); } while (0)
#endif

#if SDP_DEBUG_XMIT
#define D_XMIT(fmt...) syslog(LOG_INFO, __FUNCTION__ ": " fmt)
#else
#define D_XMIT(fmt...)
#endif

#if SDP_DEBUG_REC
#define D_REC(fmt...) syslog(LOG_INFO, __FUNCTION__ ": " fmt)
#else
#define D_REC(fmt...)
#endif

#if SDP_DEBUG_MISC
#define D_MISC(fmt...) syslog(LOG_INFO, __FUNCTION__ ": " fmt)
#else
#define D_MISC(fmt...)
#endif

#if SDP_DEBUG_MEM
#define D_MEM(fmt...) syslog(LOG_INFO, __FUNCTION__ ": " fmt)
#else
#define D_MEM(fmt...)
#endif

#if SDP_DEBUG_PROC
#define D_PROC(fmt...) syslog(LOG_INFO, __FUNCTION__ ": " fmt)
#else
#define D_PROC(fmt...)
#endif

#if SDP_DEBUG_PRINT_DATA
#define PRINT_DATA(str, data, len) print_data(str, data, len)
#else
#define PRINT_DATA(str, data, len)
#endif

#define D_ERR(fmt...) do { fprintf(stderr, __FUNCTION__ ": ERROR: " fmt); fprintf(stderr, "\n"); } while (0)

#define GET_TYPE(ch) (((ch) >> 3) & 0x1f)
#define GET_SIZE(ch) ((ch) & 0x7)
#define SET_DE_HDR(type, size) ((((type) << 3) & 0xf8) + ((size) & 0x7))

#define SDP_HDR_SIZE 5
/* The size of the SDP packet header */

#define DES_TYPE 0x6
#define DES_HDR_LEN 2

#define UUID16_HDR 0x19
#define UUID32_HDR 0x1a
#define UUID128_HDR 0x1c

/****************** TYPE DEFINITION SECTION *********************************/

/****************** LOCAL FUNCTION DECLARATION SECTION **********************/

void sdp_parse_data(unsigned char* data, unsigned int len);
void process_service_search_req(database_query_struct *db, unsigned char *data,
                                unsigned short len);
void process_service_attr_req(database_query_struct *db, unsigned char *data,
                              unsigned short len);
void process_service_search_attr_req(database_query_struct *db,
				     unsigned char *data, unsigned short len);

void process_service_search_rsp(int sdp_con_id, unsigned char *data);
void process_service_attr_rsp(int sdp_con_id, unsigned char *data);
void process_service_search_attr_rsp(int sdp_con_id, unsigned char *data);

int get_size(unsigned char *data, unsigned int *new_pos);

/****************** GLOBAL VARIABLE DECLARATION SECTION *********************/

/****************** LOCAL VARIABLE DECLARATION SECTION **********************/

static int stack_if_fd;

int malloc_dbg = 0;

/* pointers to contionuationstate data */
cont_state_struct *cont_state_buf = NULL;

/****************** FUNCTION DEFINITION SECTION *****************************/

void 
sdp_parse_data(unsigned char* data, unsigned int len)
{
  database_query_struct db;
  unsigned char id;
  unsigned short par_len;
  unsigned int pkt_len;
  data_struct *db_hdl;

  D_REC("%d bytes",len);
  PRINT_DATA(__FUNCTION__, data, len);

  db_hdl = (data_struct*)data;
  data += sizeof(*db_hdl);
  len -= sizeof(*db_hdl);

  db.sdp_con_id = db_hdl->sdp_con_id;
  db.l2cap_mtu = db_hdl->l2cap_mtu;
  pkt_len = db_hdl->len;

  id = data[SDP_HDR_TYPE];
  db.trans_id = CHAR2INT16(data[SDP_HDR_TRANS_ID_MS], data[SDP_HDR_TRANS_ID_LS]);
  par_len = CHAR2INT16(data[SDP_HDR_LENGTH_MS], data[SDP_HDR_LENGTH_LS]);
  data += SDP_HDR_SIZE;

  if (par_len > (len - SDP_HDR_SIZE))
  {
    D_XMIT("Packet length (%d) does not match received data length (%d)",
           len - SDP_HDR_SIZE, par_len);
    send_error_rsp(&db, SDP_INVALID_PDU_SIZE);
    return;
  }

  D_REC("id: 0x%x, trans_id: 0x%x, par_len: %d", id, db.trans_id, par_len);

  switch (id)
  {
  case SDP_ERROR_RSP:
    switch (CHAR2INT16(data[0], data[1]))
    {
    case SDP_INVALID_SDP_VERSION:
      D_ERR("Got Invalid/unsupported SDP version");
      break;
    case SDP_INVALID_SERVICE_RECORD_HANDLE:
      D_ERR("Got Invalid Service Record Handle");
      break;
    case SDP_INVALID_REQUEST_SYNTAX:
      D_ERR("Got Invalid Request Syntax");
      break;
    case SDP_INVALID_PDU_SIZE:
      D_ERR("Got Invalid PDU Size");
      break;
    case SDP_INVALID_CONTINUATION_STATE:
      D_ERR("Got Invalid Continuation State");
      break;
    case SDP_INSUFFICIENT_RESOURCES:
      D_ERR("Got Insufficient Resources to satisfy Request");
      break;
    default:
      D_ERR("Error code 0x%04x is not specified",
            CHAR2INT16(data[0], data[1]));
      break;
    }
    break;

  case SDP_SERVICESEARCH_REQ:
    D_REC("SDP_SERVICESEARCH_REQ %d bytes", par_len);
    process_service_search_req(&db, data, par_len);
    break;
    
  case SDP_SERVICESEARCH_RSP:
    D_REC("SDP_SERVICESEARCH_RSP");
    process_service_search_rsp(db.sdp_con_id, data);
    break;
    
  case SDP_SERVICEATTR_REQ:
    D_REC("SDP_SERVICEATTR_REQ");
    process_service_attr_req(&db, data, par_len);
    break;
    
  case SDP_SERVICEATTR_RSP:
    D_REC("SDP_SERVICEATTR_RSP");
    process_service_attr_rsp(db.sdp_con_id, data);
    break;
    
  case SDP_SERVICESEARCHATTR_REQ:
    D_REC("SDP_SERVICESEARCHATTR_REQ");
    process_service_search_attr_req(&db, data, par_len);
    break;
    
  case SDP_SERVICESEARCHATTR_RSP:
    D_REC("SDP_SERVICESEARCHATTR_RSP");
    process_service_search_attr_rsp(db.sdp_con_id, data);
    break;
    
  default:
    D_REC("ERROR Invalid pdu type");
    /* Send an error response with error code Invalid request syntax */
    send_error_rsp(&db, SDP_INVALID_REQUEST_SYNTAX);
    break;
  }
}

void
process_service_search_req(database_query_struct *db, unsigned char *data, unsigned short len)
{
  unsigned int service_search_uuid[12];
  int service_search_uuid_cnt;
  unsigned char des_len;
  int tmp_pos, i, cur_pos = 0;
  int max_rec_cnt;
  int new_pos[1];
  int cont_state_len;

  D_REC("Got %d bytes", len);
  if (GET_TYPE(data[cur_pos]) != DES_TYPE)
  {
    D_REC("Incorrect packet: Data Element Sequence expected");
    /* Send an error msg with error code Invalid request syntax */
    send_error_rsp(db, SDP_INVALID_REQUEST_SYNTAX);
    return;
  }

  des_len = get_size(data + cur_pos, new_pos);
  cur_pos += *new_pos;
  D_MISC("des_len: %d, new_pos: %d", des_len, *new_pos);

  if (des_len > (len - DES_HDR_LEN))
  {
    D_REC("Incorrect packet: Incorrect length field or whole packet was not received");
    /* Send an error msg with error code Invalid request syntax */
    send_error_rsp(db, SDP_INVALID_REQUEST_SYNTAX);
    return;
  }

  tmp_pos = 0;
  i = 0;
  while (tmp_pos < des_len)
  {
    if (i >= 12)
    {
      D_REC("More than 12 UUID in one request");
      /* Send an error msg with error code Invalid request syntax */
      send_error_rsp(db, SDP_INVALID_REQUEST_SYNTAX);
      break;
    }

    if (data[cur_pos] == UUID16_HDR)
    {
      service_search_uuid[i] = CHAR2INT16(data[cur_pos + 1],
                                          data[cur_pos + 2]);
      D_MISC("Found UUID16 0x%04x", service_search_uuid[i]); 
      i++;
      tmp_pos += 3;
      cur_pos += 3;
    }
    else if (data[cur_pos] == UUID32_HDR)
    {
      cur_pos += 2;
      service_search_uuid[i] = CHAR2INT16(data[cur_pos + 1],
                                          data[cur_pos + 2]);
      D_MISC("Found UUID32 0x%08x", service_search_uuid[i]); 
      i++;
      tmp_pos += 5;
      cur_pos += 3;
    }
    else if (data[cur_pos] == UUID128_HDR)
    {
      cur_pos += 2;
      service_search_uuid[i] = CHAR2INT16(data[cur_pos + 1],
                                          data[cur_pos + 2]);
      D_MISC("Found UUID128 0x%08x", service_search_uuid[i]);
      i++;
      tmp_pos += 17;
      cur_pos += 15;
    }
    else
    {
      D_REC("Unknown UUID size 0x%02x", data[cur_pos]);
      /* Send an error msg with error code Invalid request syntax */
      send_error_rsp(db, SDP_INVALID_REQUEST_SYNTAX);
      return;
    }
  }
  service_search_uuid_cnt = i;

  max_rec_cnt = CHAR2INT16(data[cur_pos], data[cur_pos + 1]);

  D_MISC("max_rec_cnt: %d", max_rec_cnt);

  cur_pos += 2;

  cont_state_len = data[cur_pos++];

  if (len < cur_pos)
  {
    D_ERR("Packet length (%d) shorter than actual packet length (%d)", len, cur_pos);

    send_error_rsp(db, SDP_INVALID_PDU_SIZE);
    return;
  }
  else if (len > cur_pos)
  {
    D_ERR("Packet length (%d) longer than actual packet length (%d)", len, cur_pos);
    send_error_rsp(db, SDP_INVALID_PDU_SIZE);
    return;
  }
  
  if (cont_state_len)
  {
    cur_pos += data[cur_pos];
    
    D_MISC("Sending continuationstate packet");
    send_cont_state_search_rsp(cont_state_len, data + cur_pos, max_rec_cnt,db);
  }
  else
  {
    service_search_struct db_hdl;

    db->pkt_type = SDP_SERVICESEARCH_REQ;
    memcpy(&db_hdl.db, db, sizeof(database_query_struct));

    db_hdl.max_rec_cnt = max_rec_cnt;
    db_hdl.service_class_cnt = service_search_uuid_cnt;
    memcpy(db_hdl.service_class_list, service_search_uuid,
           service_search_uuid_cnt * sizeof(*service_search_uuid));

    /* Here we ask the database for the requested attributes */
    handle_query(&db_hdl.db);
  }
}

void 
process_service_attr_req(database_query_struct *db, unsigned char *data, unsigned short len)
{
  service_attr_struct *db_hdl;

  int tmp_len, cur_pos = 0;
  unsigned char des_len;

  unsigned int rec_hdl;
  unsigned int max_attr_cnt;
  unsigned int attr_list[256];
  int attr_list_pos = 0;

  int cont_state_len;
  
  int new_pos[1];

  D_REC("Got %d bytes", len);

  rec_hdl = CHAR2INT32(data[cur_pos], data[cur_pos + 1],
                       data[cur_pos  + 2], data[cur_pos + 3]);

  D_MISC("rec_hdl: 0x%08x", rec_hdl);
  cur_pos += 4;

  max_attr_cnt = CHAR2INT16(data[cur_pos], data[cur_pos + 1]);
  D_MISC("max_attr_cnt: %d", max_attr_cnt);
  cur_pos += 2;

  if (GET_TYPE(data[cur_pos]) != DES_TYPE)
  {
    D_REC("Incorrect packet: Data Element Sequence expected");
    /* Send an error msg with error code Invalid request syntax */
    send_error_rsp(db, SDP_INVALID_REQUEST_SYNTAX);
    return;
  }

  des_len = get_size(data + cur_pos, new_pos);
  cur_pos += *new_pos;
  D_MISC("des_len: %d, new_pos: %d", des_len, *new_pos);

  if (des_len > (len - DES_HDR_LEN))
  {
    D_REC("Incorrect packet: Incorrect length field or whole packet was not received");
    /* Send an error msg with error code Invalid request syntax */
    send_error_rsp(db, SDP_INVALID_REQUEST_SYNTAX);
    return;
  }

  /* Now we parses the third parameter which is a data element list. i.e
     we have to go through the whole list and pick out one element at
     time */
  while (des_len > 0)
  {
    /* Get the length of the data element */
    tmp_len = get_size(data + cur_pos, new_pos);
    D_MISC("tmp_len: %d, new_pos: %d", tmp_len, *new_pos);
    /* Move the data pointer past the length field to the start of
       the data element */
    cur_pos += *new_pos;
    if (tmp_len == 4)
    {
      /* Now we got a range of attributes */
      attr_list[attr_list_pos] = CHAR2INT32(data[cur_pos], data[cur_pos + 1], data[cur_pos  + 2], data[cur_pos + 3]);
      D_MISC("Found range of attributes: 0x%08x", attr_list[attr_list_pos]); 
      cur_pos += 4;
      attr_list_pos++;
    }
    else
    {
      /* Just a single attributes */
      attr_list[attr_list_pos] = CHAR2INT16(data[cur_pos], data[cur_pos + 1]);
      /* We stor all attributes as ranges */
      attr_list[attr_list_pos] |= attr_list[attr_list_pos] << 16;
      D_MISC("Found single attributes: 0x%04x", attr_list[attr_list_pos]); 
      cur_pos += 2;
      attr_list_pos++;
    }
    des_len -= (*new_pos + tmp_len);
  }

  cont_state_len = data[cur_pos++];
  cur_pos += cont_state_len;

  if (len < cur_pos)