sdp.c

蓝牙协议源代码 bluetooth stack for lwip

/*
 * Copyright (c) 2003 EISLAB, Lulea University of Technology.
 * All rights reserved. 
 * 
 * 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. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission. 
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 THE AUTHOR 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.
 *
 * This file is part of the lwBT Bluetooth stack.
 * 
 * Author: Conny Ohult <[e-mail]>
 *
 */

/*-----------------------------------------------------------------------------------*/
/* sdp.c
 *
 * Implementation of the service discovery protocol (SDP)
 */
/*-----------------------------------------------------------------------------------*/

#include "netif/lwbt/sdp.h"
#include "netif/lwbt/lwbt_memp.h"
#include "lwbtopts.h"
#include "lwip/debug.h"

#include "lwip/inet.h"

/* Next service record handle to be used */
u32_t rhdl_next;

/* Next transaction id to be used */
u16_t tid_next;

/* The SDP PCB lists */
struct sdp_pcb *sdp_pcbs;
struct sdp_pcb *sdp_tmp_pcb;

/* List of all active service records in the SDP server */
struct sdp_record *sdp_server_records;
struct sdp_record *sdp_tmp_record; /* Only used for temp storage */

/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_init():
 * 
 * Initializes the SDP layer.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_init(void)
{
  /* Clear globals */
  sdp_server_records = NULL;
  sdp_tmp_record = NULL;
  
  /* Inialize service record handles */
  rhdl_next = 0x0000FFFF;
  
  /* Initialize transaction ids */
  tid_next = 0x0000;
}
/*-----------------------------------------------------------------------------------*/
/* Server API
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_next_rhdl():
 * 
 * Issues a service record handler.
 */
/*-----------------------------------------------------------------------------------*/
u32_t
sdp_next_rhdl(void)
{
  ++rhdl_next;
  if(rhdl_next == 0) {
    rhdl_next = 0x0000FFFF;
  }
  return rhdl_next;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_record_new():
 * 
 * Creates a new service record.
 */
/*-----------------------------------------------------------------------------------*/
struct sdp_record *
sdp_record_new(u8_t *record_de_list, u8_t rlen)
{
  struct sdp_record *record;

  record = lwbt_memp_malloc(MEMP_SDP_RECORD);
  if(record != NULL) {
    record->hdl = sdp_next_rhdl();
    record->record_de_list = record_de_list;
    record->len = rlen;
    return record;
  }
  return NULL;
}
/*-----------------------------------------------------------------------------------*/
void
sdp_record_free(struct sdp_record *record)
{
  lwbt_memp_free(MEMP_SDP_RECORD, record);
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_register_service():
 * 
 * Add a record to the list of records in the service record database, making it 
 * available to clients.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_register_service(struct sdp_record *record)
{
  if(record == NULL) {
    return ERR_ARG;
  }
  SDP_RECORD_REG(&sdp_server_records, record);
  return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_unregister_service():
 * 
 * Remove a record from the list of records in the service record database, making it 
 * unavailable to clients.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_unregister_service(struct sdp_record *record)
{
  SDP_RECORD_RMV(&sdp_server_records, record);
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_next_transid():
 * 
 * Issues a transaction identifier that helps matching a request with the reply.
 */
/*-----------------------------------------------------------------------------------*/
u16_t
sdp_next_transid(void)
{
  ++tid_next;
  return tid_next;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_pattern_search():
 * 
 * Check if the given service search pattern matches the record.
 */
 /*-----------------------------------------------------------------------------------*/
u8_t
sdp_pattern_search(struct sdp_record *record, u8_t size, struct pbuf *p) 
{
  u8_t i, j;
  u8_t *payload = (u8_t *)p->payload;

  for(i = 0; i < size; ++i) {
    if(SDP_DE_TYPE(payload[i]) == SDP_DE_TYPE_UUID)  {
      switch(SDP_DE_SIZE(payload[i])) {
      case SDP_DE_SIZE_16:
	for(j = 0; j < record->len; ++j) {
	  if(SDP_DE_TYPE(record->record_de_list[j]) == SDP_DE_TYPE_UUID) {
	    if(*((u16_t *)(payload + i + 1)) == *((u16_t *)(record->record_de_list + j + 1))) {
	      return 1; /* Found a matching UUID in record */
	    }
	    ++j;
	  }
	}
	i += 2;
	break;
      case SDP_DE_SIZE_32:
	i += 4;
	break;
      case SDP_DE_SIZE_128:
	i+= 16;
	break;
      default:
	break;
      }
    }
  }
  return 0;
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_attribute_search():
 * 
 * Searches a record for attributes and add them to a given packet buffer.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
struct pbuf *
sdp_attribute_search(u16_t max_attribl_bc, struct pbuf *p, struct sdp_record *record) 
{
  struct pbuf *q = NULL;
  struct pbuf *r; 
  struct pbuf *s = NULL;
  u8_t *payload = (u8_t *)p->payload;
  u8_t size;
  u8_t i = 0, j;
  u16_t attr_id = 0, attr_id2 = 0;

  u16_t attribl_bc = 0; /* Byte count of the sevice attributes */
  u32_t hdl = htonl(record->hdl);
  
  if(SDP_DE_TYPE(payload[0]) == SDP_DE_TYPE_DES  && 
     SDP_DE_SIZE(payload[0]) == SDP_DE_SIZE_N1) {
    /* Get size of attribute ID list */
    size = payload[1]; //TODO: correct to assume only one size byte in remote request? probably  
    
    while(i < size) {
      /* Check if this is an attribute ID or a range of attribute IDs */
      if(payload[2+i] == (SDP_DE_TYPE_UINT  | SDP_DE_SIZE_16)) {
	attr_id = *((u16_t *)(payload+3+i));
	attr_id2 = attr_id; /* For the range to cover this attribute ID only */
	i += 3;
      } else if(payload[2+i] == (SDP_DE_TYPE_UINT | SDP_DE_SIZE_32)) {
	attr_id = *((u16_t *)(payload+3+i));
	attr_id2 = *((u16_t *)(payload+5+i));
	i += 5;
      } else {
	/* ERROR: Invalid req syntax */
	//TODO
      }
  
      for(j = 0; j < record->len; ++j) {
	if(SDP_DE_TYPE(record->record_de_list[j]) == SDP_DE_TYPE_DES) {
	  if(record->record_de_list[j + 2] == (SDP_DE_TYPE_UINT | SDP_DE_SIZE_16)) {
	    if(*((u16_t *)(record->record_de_list + j + 3)) >= attr_id && 
	       *((u16_t *)(record->record_de_list + j + 3)) <= attr_id2) {
	      if(attribl_bc +  record->record_de_list[j + 1] + 2 > max_attribl_bc) {
		/* Abort attribute search since attribute list byte count must not 
		   exceed max attribute byte count in req */
		break;
	      }
	      /* Allocate a pbuf for the service attribute */
	      r = pbuf_alloc(PBUF_RAW, record->record_de_list[j + 1], PBUF_RAM);
	      memcpy((u8_t *)r->payload, record->record_de_list + j + 2, r->len);
	      attribl_bc += r->len;

	      /* If request included a service record handle attribute id, add the correct id to the 
		 response */
	      if(*((u16_t *)(record->record_de_list + j + 3)) == 0) {
		memcpy(((u8_t *)r->payload) + 4, &hdl, 4);
	      }

	      /* Add the attribute to the service attribute list */
	      if(s == NULL) {
		s = r;
	      } else {
		pbuf_chain(s, r);
		pbuf_free(r);
	      }
	    }
	  }
	}
      } /* for */
    } /* while */
  } else {
    /* ERROR: Invalid req syntax */
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_attribute_search: Invalid req syntax"));
  }
  /* Return service attribute list */
  if(s != NULL) {
    q = pbuf_alloc(PBUF_RAW, 2, PBUF_RAM);
    ((u8_t *)q->payload)[0] = SDP_DE_TYPE_DES | SDP_DE_SIZE_N1;
    ((u8_t *)q->payload)[1] = s->tot_len;
    pbuf_chain(q, s);
    pbuf_free(s);
  }

  return q;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/*
 * SDP CLIENT API.
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_new():
 * 
 * Creates a new SDP protocol control block but doesn't place it on
 * any of the SDP PCB lists.
 */
/*-----------------------------------------------------------------------------------*/
struct sdp_pcb *
sdp_new(struct l2cap_pcb *l2cappcb)
{
  struct sdp_pcb *pcb;

  pcb = lwbt_memp_malloc(MEMP_SDP_PCB);
  if(pcb != NULL) {
    memset(pcb, 0, sizeof(struct sdp_pcb));
    pcb->l2cappcb = l2cappcb;
    return pcb;
  }
  return NULL;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_free():
 * 
 * Free the SDP protocol control block.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_free(struct sdp_pcb *pcb) 
{
  lwbt_memp_free(MEMP_SDP_PCB, pcb);
  pcb = NULL;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_reset_all():
 * 
 * Free all SDP protocol control blocks and registered records.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_reset_all(void) 
{
  struct sdp_pcb *pcb, *tpcb;
  struct sdp_record *record, *trecord;

  for(pcb = sdp_pcbs; pcb != NULL;) {
    tpcb = pcb->next;
    SDP_RMV(&sdp_pcbs, pcb);
    sdp_free(pcb);
    pcb = tpcb;
  }

  for(record = sdp_server_records; record != NULL;) {
    trecord = record->next;
    sdp_unregister_service(record);
    sdp_record_free(record);
    record = trecord;
  }

  sdp_init();
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_arg():
 *
 * Used to specify the argument that should be passed callback functions.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_arg(struct sdp_pcb *pcb, void *arg)
{
  pcb->callback_arg = arg;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_lp_disconnected():
 *
 * Called by the application to indicate that the lower protocol disconnected.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_lp_disconnected(struct l2cap_pcb *l2cappcb)
{
  struct sdp_pcb *pcb, *tpcb;

  pcb = sdp_pcbs;
  while(pcb != NULL) {
    tpcb = pcb->next;
    if(bd_addr_cmp(&(l2cappcb->remote_bdaddr), &(pcb->l2cappcb->remote_bdaddr))) {
      /* We do not need to notify upper layer, free PCB */
      sdp_free(pcb);
    }
    pcb = tpcb;
  }
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_req():
 * 
 * Sends a request to a SDP server to locate service records that match the service 
 * search pattern.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_service_search_req(struct sdp_pcb *pcb, u8_t *ssp, u8_t ssplen, u16_t max_src,
		   void (* service_searched)(void *arg, struct sdp_pcb *pcb, u16_t tot_src, 
					     u16_t curr_src, u32_t *rhdls)) 
{
  struct pbuf *p;
  struct sdp_hdr *sdphdr;

  /* Update PCB */
  pcb->tid = sdp_next_transid(); /* Set transaction id */

  /* Allocate packet for PDU hdr + service search pattern + max service record count +
     continuation state */
  p = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+ssplen+2+1, PBUF_RAM);
  sdphdr = p->payload;
  /* Add PDU header to packet */
  sdphdr->pdu = SDP_SS_PDU;
  sdphdr->id = htons(pcb->tid);
  sdphdr->len = htons(ssplen + 3); /* Seq descr + ServiceSearchPattern + MaxServiceRecCount + ContState */

  /* Add service search pattern to packet */
  memcpy(((u8_t *)p->payload) + SDP_PDUHDR_LEN, ssp, ssplen);

  /* Add maximum service record count to packet */
  *((u16_t *)(((u8_t *)p->payload) + ssplen + SDP_PDUHDR_LEN)) = htons(max_src);

  ((u8_t *)p->payload)[SDP_PDUHDR_LEN+ssplen+2] = 0; /* No continuation */
  
  /* Update PCB */
  pcb->service_searched = service_searched; /* Set callback */
  SDP_REG(&sdp_pcbs, pcb); /* Register request */
  
  return l2ca_datawrite(pcb->l2cappcb, p);
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_attrib_req():
 * 
 * Retrieves specified attribute values from a specific service record.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_service_attrib_req(struct sdp_pcb *pcb, u32_t srhdl, u16_t max_abc, u8_t *attrids, u8_t attrlen,
		       void (* attributes_recv)(void *arg, struct sdp_pcb *pcb, u16_t attribl_bc, struct pbuf *p))
{
  struct sdp_hdr *sdphdr;
  u8_t *payload;
  struct pbuf *p;
  
  /* Allocate packet for PDU hdr + service rec hdl + max attribute byte count +
     attribute id data element sequense lenght  + continuation state */
  p = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN + attrlen + 7, PBUF_RAM);

  /* Update PCB */
  pcb->tid = sdp_next_transid(); /* Set transaction id */

  /* Add PDU header to packet */
  sdphdr = p->payload;
  sdphdr->pdu = SDP_SA_PDU;
  sdphdr->id = htons(pcb->tid);
  sdphdr->len = htons((attrlen + 7)); /* Service rec hdl + Max attrib B count + Seq descr + Attribute sequence + ContState */

  payload = p->payload;

  /* Add service record handle to packet */
  *((u32_t *)(payload + SDP_PDUHDR_LEN)) = htonl(srhdl);

  /* Add maximum attribute count to packet */
  *((u16_t *)(payload + SDP_PDUHDR_LEN + 4)) = htons(max_abc);

  /* Add attribute id data element sequence to packet */
  memcpy(payload + SDP_PDUHDR_LEN + 6, attrids, attrlen);
  
  payload[SDP_PDUHDR_LEN + 6 + attrlen] = 0x00; /* No continuation */