sdp.c

蓝牙协议源代码 bluetooth stack for lwip

  /* Update PCB */
  pcb->attributes_recv = attributes_recv; /* Set callback */
  SDP_REG(&sdp_pcbs, pcb); /* Register request */

  return l2ca_datawrite(pcb->l2cappcb, p);
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_attrib_req():
 * 
 * Combines the capabilities of the SDP_ServiceSearchRequest and the 
 * SDP_ServiceAttributeRequest into a single request. Contains both a service search 
 * pattern and a list of attributes to be retrieved from service records that match 
 * the service search pattern.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_service_search_attrib_req(struct sdp_pcb *pcb, u16_t max_abc, u8_t *ssp, u8_t ssplen, u8_t *attrids, 
			      u8_t attrlen, void (* attributes_searched)(void *arg, struct sdp_pcb *pcb, 
									 u16_t attribl_bc, 
									 struct pbuf *p))
{
  struct sdp_hdr *sdphdr;

  struct pbuf *p;
  u8_t *payload;
  u16_t pbuf_bc;

  /* Allocate packet for PDU hdr + service search pattern + max attribute byte count +
     attribute id list + continuation state */
  p = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+ssplen+2+attrlen+1, PBUF_RAM);

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

  /* Add PDU header to packet */
  sdphdr = p->payload;
  sdphdr->pdu = SDP_SSA_PDU;
  sdphdr->id = htons(pcb->tid);
  sdphdr->len = htons(ssplen + 2 + attrlen + 1);
  
  pbuf_bc = SDP_PDUHDR_LEN;
  payload = (u8_t *)p->payload;
  
  /* Add service search pattern to packet */
  memcpy(((u8_t *)p->payload) + SDP_PDUHDR_LEN, ssp, ssplen);
  
  /* Add maximum attribute count to packet */
  *((u16_t *)(payload + SDP_PDUHDR_LEN + ssplen)) = htons(max_abc);

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

  pcb->attributes_searched = attributes_searched; /* Set callback */
  SDP_REG(&sdp_pcbs, pcb); /* Register request */

  return l2ca_datawrite(pcb->l2cappcb, p);
}
/*-----------------------------------------------------------------------------------*/
/*
 * SDP SERVER API.
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_rsp():
 * 
 * The SDP server sends a list of service record handles for service records that 
 * match the service search pattern given in the request.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
sdp_service_search_rsp(struct l2cap_pcb *pcb, struct pbuf *p, struct sdp_hdr *reqhdr)
{
  struct sdp_record *record;
  struct sdp_hdr *rsphdr;
  
  struct pbuf *q; /* response packet */
  struct pbuf *r; /* tmp buffer */
  
  u16_t max_src = 0;
  u16_t curr_src = 0;
  u16_t tot_src = 0;

  u8_t size = 0;

  err_t ret;
  
  if(SDP_DE_TYPE(((u8_t *)p->payload)[0]) == SDP_DE_TYPE_DES && 
     SDP_DE_SIZE(((u8_t *)p->payload)[0]) ==  SDP_DE_SIZE_N1) {
    /* Size of the search pattern must be in the next byte since only 
       12 UUIDs are allowed in one pattern */
    size = ((u8_t *)p->payload)[1];
    
    /* Get maximum service record count that follows the service search pattern */
    max_src = ntohs(*((u16_t *)(((u8_t *)p->payload)+(2+size))));

    pbuf_header(p, -2);
  } else {
    //TODO: INVALID SYNTAX ERROR
  }

  /* Allocate header + Total service rec count + Current service rec count  */
  q  = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+4, PBUF_RAM);

  rsphdr = q->payload;
  rsphdr->pdu = SDP_SSR_PDU;
  rsphdr->id = reqhdr->id;

  for(record = sdp_server_records; record != NULL; record = record->next) {
    /* Check if service search pattern matches record */
    if(sdp_pattern_search(record, size, p)) {
      if(max_src > 0) {
	/* Add service record handle to packet */
	r = pbuf_alloc(PBUF_RAW, 4, PBUF_RAM);
	*((u32_t *)r->payload) = htonl(record->hdl);
	pbuf_chain(q, r);
	pbuf_free(r);
	--max_src;
	++curr_src;
      }
      ++tot_src;
    }
  }

  /* Add continuation state to packet */
  r = pbuf_alloc(PBUF_RAW, 1, PBUF_RAM);
  ((u8_t *)r->payload)[0] = 0x00;
  pbuf_chain(q, r);
  pbuf_free(r);
 
  /* Add paramenter length to header */
  rsphdr->len = htons(q->tot_len - SDP_PDUHDR_LEN);
  
  /* Add total service record count to packet */
  *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = htons(tot_src);

  /* Add current service record count to packet */
  *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN + 2)) = htons(curr_src);


  {
      u16_t i;
    for(r = q; r != NULL; r = r->next) {
      for(i = 0; i < r->len; ++i) {
	LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_rsp: 0x%x\n", ((u8_t *)r->payload)[i]));
      }
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_rsp: STOP\n"));
    }
    }

  ret = l2ca_datawrite(pcb, q);
  pbuf_free(q);
  return ret;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_attrib_rsp():
 * 
 * Sends a response that contains a list of attributes (both attribute ID and 
 * attribute value) from the requested service record.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
sdp_service_attrib_rsp(struct l2cap_pcb *pcb, struct pbuf *p, struct sdp_hdr *reqhdr)
{
  struct sdp_record *record;
  struct sdp_hdr *rsphdr;

  struct pbuf *q;
  struct pbuf *r;

  u16_t max_attribl_bc = 0; /* Maximum attribute list byte count */

  err_t ret;

  /* Find record */
  for(record = sdp_server_records; record != NULL; record = record->next) {
    if(record->hdl == ntohl(*((u32_t *)p->payload))) {
      break;
    }
  } 
  if(record != NULL) { 
    /* Get maximum attribute byte count */
    max_attribl_bc = ntohs(((u16_t *)p->payload)[2]); 
    
    /* Allocate rsp packet header + Attribute list count */
    q  = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+2, PBUF_RAM);
    rsphdr = q->payload;
    rsphdr->pdu = SDP_SAR_PDU;
    rsphdr->id = reqhdr->id;

    /* Search for attributes and add them to a pbuf */
    pbuf_header(p, -6);
    r = sdp_attribute_search(max_attribl_bc, p, record);
    if(r != NULL) {
      /* Add attribute list byte count length to header */
      *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = htons(r->tot_len);
      pbuf_chain(q, r); /* Chain attribute id list for service to response packet */
      pbuf_free(r);
    } else {
      *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = 0;
    }
    
    /* Add continuation state to packet */
    r = pbuf_alloc(PBUF_RAW, 1, PBUF_RAM);
    ((u8_t *)r->payload)[0] = 0x00; //TODO: Is this correct?
    pbuf_chain(q, r);
    pbuf_free(r);

    /* Add paramenter length to header */
    rsphdr->len = htons(q->tot_len - SDP_PDUHDR_LEN);

    {
      u16_t i;
    for(r = q; r != NULL; r = r->next) {
      for(i = 0; i < r->len; ++i) {
	LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_attrib_rsp: 0x%x\n", ((u8_t *)r->payload)[i]));
      }
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_attrib_rsp: STOP\n"));
    }
    }

    ret = l2ca_datawrite(pcb, q);
    pbuf_free(q);

    return ret;
  }
  //TODO: ERROR NO SERVICE RECORD MATCHING HANDLE FOUND
  return ERR_OK;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_attrib_rsp():
 * 
 * Sends a response that contains a list of attributes (both attribute ID and 
 * attribute value) from the service records that match the requested service search 
 * pattern.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
sdp_service_search_attrib_rsp(struct l2cap_pcb *pcb, struct pbuf *p, struct sdp_hdr *reqhdr)
{
  struct sdp_record *record;
  struct sdp_hdr *rsphdr;
  
  struct pbuf *q; /* response packet */
  struct pbuf *r = NULL; /* tmp buffer */
  struct pbuf *s = NULL; /* tmp buffer */

  u16_t max_attribl_bc = 0;
  u8_t size = 0;

  /* Get size of service search pattern */
  if(SDP_DE_TYPE(((u8_t *)p->payload)[0]) == SDP_DE_TYPE_DES && 
     SDP_DE_SIZE(((u8_t *)p->payload)[0]) ==  SDP_DE_SIZE_N1) {
    /* Size of the search pattern must be in the next byte since only 
       12 UUIDs are allowed in one pattern */
    size = ((u8_t *)p->payload)[1];

    /* Get maximum attribute byte count that follows the service search pattern */
    max_attribl_bc = ntohs(*((u16_t *)(((u8_t *)p->payload)+(2+size))));

    pbuf_header(p, -2);
  } else {
    //TODO: INVALID SYNTAX ERROR
  }

  /* Allocate header + attribute list count */
  q  = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN + 2, PBUF_RAM);

  rsphdr = q->payload;
  rsphdr->pdu = SDP_SSAR_PDU;
  rsphdr->id = reqhdr->id;

  for(record = sdp_server_records; record != NULL; record = record->next) {
    /* Check if service search pattern matches record */
    if(sdp_pattern_search(record, size, p)) {
      /* Search for attributes and add them to a pbuf */
      pbuf_header(p, -(size + 2));
      r = sdp_attribute_search(max_attribl_bc, p, record);
      if(r != NULL) {
	if(q->next == NULL) {
	  s = pbuf_alloc(PBUF_RAW, 2, PBUF_RAM);
	  pbuf_chain(q, s); /* Chain attribute id list for service to response packet */
	  pbuf_free(s);
	}
	max_attribl_bc -= r->tot_len; /* Calculate remaining number of bytes of attribute 
						       data the server is to return in response to the 
						       request */
	pbuf_chain(q, r); /* Chain attribute id list for service to response packet */
	pbuf_free(r);
      }
      pbuf_header(p, size + 2);
    }
  }
  
  /* Add attribute list byte count length and length of all attribute lists in this PDU to packet */
  if(q->next != NULL ) {
    *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = htons(q->tot_len - SDP_PDUHDR_LEN - 2);

    ((u8_t *)q->next->payload)[0] = 0x35;
    ((u8_t *)q->next->payload)[1] = q->tot_len - SDP_PDUHDR_LEN - 4;
  } else {
    *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = 0;
  }
  
  /* Add continuation state to packet */
  if((r = pbuf_alloc(PBUF_RAW, 1, PBUF_RAM)) == NULL) {
    //TODO: ERROR
  } else {
    ((u8_t *)r->payload)[0] = 0x00; //TODO: Is this correct?
    pbuf_chain(q, r);
    pbuf_free(r);
  }

  /* Add paramenter length to header */
  rsphdr->len = htons(q->tot_len - SDP_PDUHDR_LEN);

  for(r = q; r != NULL; r = r->next) {
    u8_t i;
    for(i = 0; i < r->len; ++i) {
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_attrib_rsp: 0x%x\n", ((u8_t *)r->payload)[i]));
    }
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_attrib_rsp: STOP\n"));
  }

  return l2ca_datawrite(pcb, q);
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_recv():
 * 
 * Called by the lower layer. Parses the header and handle the SDP message.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_recv(void *arg, struct l2cap_pcb *pcb, struct pbuf *s, err_t err)
{
  struct sdp_hdr *sdphdr;
  struct sdp_pcb *sdppcb;
  err_t ret = ERR_OK;
  u16_t i;
  struct pbuf *p, *q, *r;

  if(s->len != s->tot_len) {
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Fragmented packet received. Reassemble into one buffer\n"));
    if((p = pbuf_alloc(PBUF_RAW, s->tot_len, PBUF_RAM)) != NULL) {
      i = 0;
      for(r = s; r != NULL; r = r->next) {
	  memcpy(((u8_t *)p->payload) + i, r->payload, r->len);
	  i += r->len;
      }
      pbuf_free(s);
    } else {
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Could not allocate buffer for fragmented packet\n"));
      pbuf_free(s);
      return ERR_MEM; 
    }
  } else {
    p = s;
  }

  for(r = p; r != NULL; r = r->next) {
    for(i = 0; i < r->len; ++i) {
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: 0x%x\n", ((u8_t *)r->payload)[i]));
    }
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: STOP\n"));
  }

  sdphdr = p->payload;
  pbuf_header(p, -SDP_PDUHDR_LEN);

  switch(sdphdr->pdu) {
  case SDP_ERR_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Error response 0x%x\n", ntohs(*((u16_t *)p->payload))));
    pbuf_free(p);
    break;
#if LWBT_LAP
  case SDP_SS_PDU: /* Client request */
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search request\n"));
    ret = sdp_service_search_rsp(pcb, p, sdphdr);
    pbuf_free(p);
    break;
#endif /* LWBT_LAP */
  case SDP_SSR_PDU: /* Server response */
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search response\n"));
    /* Find the original request */
    for(sdppcb = sdp_pcbs; sdppcb != NULL; sdppcb = sdppcb->next) {
      if(sdppcb->tid == ntohs(sdphdr->id)) {
	break; /* Found */
      } /* if */
    } /* for */
    if(sdppcb != NULL) {
      /* Unregister the request */
      SDP_RMV(&sdp_pcbs, sdppcb);
      /* Callback function for a service search response */
      SDP_ACTION_SERVICE_SEARCHED(sdppcb, ntohs(((u16_t *)p->payload)[0]), ntohs(((u16_t *)p->payload)[1]), ((u32_t *)p->payload) + 1);
    }
    pbuf_free(p);
    break;
#if LWBT_LAP
  case SDP_SA_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service attribute request\n"));
    ret = sdp_service_attrib_rsp(pcb, p, sdphdr);
    pbuf_free(p);
    break;
#endif /* LWBT_LAP */
  case SDP_SAR_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service attribute response\n"));
    /* Find the original request */
    for(sdppcb = sdp_pcbs; sdppcb != NULL; sdppcb = sdppcb->next) {
      if(sdppcb->tid == ntohs(sdphdr->id)) {
	/* Unregister the request */
	SDP_RMV(&sdp_pcbs, sdppcb);
	/* If packet is divided into several pbufs we need to merge them */
	if(p->next != NULL) {
	  r = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
	  i = 0;
	  for(q = p; q != NULL; q = q->next) {
	    memcpy(((u8_t *)r->payload)+i, q->payload, q->len);
	    i += q->len;
	  }
	  pbuf_free(p);
	  p = r;
	}
	i = *((u16_t *)p->payload);
	pbuf_header(p, -2);	
	/* Callback function for a service attribute response */
	SDP_ACTION_ATTRIB_RECV(sdppcb, i, p);
      } /* if */
    } /* for */
    pbuf_free(p);
    break;
#if LWBT_LAP
  case SDP_SSA_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search attribute request\n"));
    ret = sdp_service_search_attrib_rsp(pcb, p, sdphdr);
    pbuf_free(p);
    break;
#endif /* LWBT_LAP */
  case SDP_SSAR_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search attribute response\n"));
    /* Find the original request */
    for(sdppcb = sdp_pcbs; sdppcb != NULL; sdppcb = sdppcb->next) {
      if(sdppcb->tid == ntohs(sdphdr->id)) {
	/* Unregister the request */
	SDP_RMV(&sdp_pcbs, sdppcb);
	/* If packet is divided into several pbufs we need to merge them */
	if(p->next != NULL) {
	  r = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
	  i = 0;
	  for(q = p; q != NULL; q = q->next) {
	    memcpy(((u8_t *)r->payload)+i, q->payload, q->len);
	    i += q->len;
	  }
	  pbuf_free(p);
	  p = r;
	}
	i = *((u16_t *)p->payload);
	pbuf_header(p, -2);
	/* Callback function for a service search attribute response */
	SDP_ACTION_ATTRIB_SEARCHED(sdppcb, i, p);
	break; /* Abort request search */
      } /* if */
    } /* for */
    pbuf_free(p);
    break;
  default:
    //TODO: INVALID SYNTAX ERROR
    break;
  }
  return ret;
}
/*-----------------------------------------------------------------------------------*/