sdp_server.c

bluetooth 驱动

    rec_hdl_cnt = max_rec_cnt;
  }
  
  rsp_pkt_len = SDP_HDR_SIZE;  
  
  /* Set the total service record count */
  rsp_pkt[rsp_pkt_len++] = SHORT2CHAR_MS(rec_hdl_cnt);
  rsp_pkt[rsp_pkt_len++] = SHORT2CHAR_LS(rec_hdl_cnt);
  
  /* fixme -- total/current should probably differ sometimes... */
  
  /* Set the current service record count */
  rsp_pkt[rsp_pkt_len++] = SHORT2CHAR_MS(rec_hdl_cnt);
  rsp_pkt[rsp_pkt_len++] = SHORT2CHAR_LS(rec_hdl_cnt);
  
  if (rec_hdl_list)
  {
    for (i = 0; i < rec_hdl_cnt; i++)
    {
      rsp_pkt[rsp_pkt_len++] = (rec_hdl_list[i] >> 24) & 0xff;
      rsp_pkt[rsp_pkt_len++] = (rec_hdl_list[i] >> 16) & 0xff;
      rsp_pkt[rsp_pkt_len++] = (rec_hdl_list[i] >> 8) & 0xff;
      rsp_pkt[rsp_pkt_len++] = rec_hdl_list[i] & 0xff;
    }
  }
  
  set_sdp_hdr(rsp_pkt, SDP_SERVICESEARCH_RSP, db_hdl->db.trans_id,
              rsp_pkt_len - SDP_HDR_SIZE);

  D_REC("l2cap_mtu:%d, mrc:%d, db->mrc:%d\n", db_hdl->db.l2cap_mtu, max_rec_cnt, db_hdl->max_rec_cnt);

  /* If Max Service Record Count is set to a value less than the number of
     available records in database we should not set cont state */

  /* set_cont_state_search searches rsp_pkt for currently set rec cnt 
     and we compare that with max_rec_cnt. If current is larger than
     max rec cnt, we set continuation state and change headers in rsp
     packet. If not we simply put a 0 in the end to indicate no cont 
     state */
  
  /* fixme -- as of today we can only handle continuation state for 
     1 client at a time */

  rsp_pkt_len = set_cont_state_search(rsp_pkt, rsp_pkt_len, max_rec_cnt);

  if (is_err())
  {
    send_error_rsp(&db_hdl->db, get_err());
    return;
  }
  
  write2stack(db_hdl->db.sdp_con_id, rsp_pkt, rsp_pkt_len); 
}

void 
handle_service_attr_req(service_attr_struct *db_hdl)
{
  unsigned char rsp_pkt[256];
  int rsp_pkt_len, i, des_len_pos;
  unsigned char *tmp_ptr;
  unsigned int max_attr_byte_cnt;
    
  /* Skip the sdp header and the attribute byte count field */
  rsp_pkt_len = SDP_HDR_SIZE + 2;

  rsp_pkt[rsp_pkt_len++] = DES_HDR;
  des_len_pos = rsp_pkt_len;
  rsp_pkt_len++;

  for (i = 0; i < db_hdl->attr_cnt; i++)
  {
    if ((db_hdl->attr_list[i] & 0xffff) ^ (db_hdl->attr_list[i] >> 16))
    {
      tmp_ptr = get_attribute_range(xml_fd, db_hdl->rec_hdl, db_hdl->attr_list[i]);
    }
    else
    {
      tmp_ptr = get_attribute_list(xml_fd, db_hdl->rec_hdl, db_hdl->attr_list[i]);
    }

    if (tmp_ptr)
    {
      memcpy(rsp_pkt + rsp_pkt_len, tmp_ptr + 2, tmp_ptr[1]);
      rsp_pkt_len += tmp_ptr[1];

      D_MEM("<--- free%d 0x%8p", --malloc_dbg, tmp_ptr);
      free(tmp_ptr);
    }
    else if (is_err())
    {
      send_error_rsp(&db_hdl->db, get_err());
      return;
    }
  }

  /* Set the attribute byte count to packet length minus sdp pdu header size
     minus attribute byte count field length, minus continuation field length*/
  rsp_pkt[SDP_HDR_SIZE]   = SHORT2CHAR_MS(rsp_pkt_len - SDP_HDR_SIZE - 2);
  rsp_pkt[SDP_HDR_SIZE+1] = SHORT2CHAR_LS(rsp_pkt_len - SDP_HDR_SIZE - 2);

  /* The length of the attribute list is the length of the attribute byte
     count minus the size of the data element sequence header */
  rsp_pkt[des_len_pos] = rsp_pkt_len - SDP_HDR_SIZE - 2 - 2;
  
  set_sdp_hdr(rsp_pkt, SDP_SERVICEATTR_RSP, db_hdl->db.trans_id,
              rsp_pkt_len - SDP_HDR_SIZE);

  max_attr_byte_cnt = MIN(db_hdl->max_attr_byte_cnt, db_hdl->db.l2cap_mtu - 9);
  /* 9 comes from SDP_HDR_SIZE + attr byte nt field + 2 bytes for cont state */

  D_REC("l2cap_mtu:%d, mabc:%d, db->mabc:%d\n", db_hdl->db.l2cap_mtu, max_attr_byte_cnt, db_hdl->max_attr_byte_cnt);
   
  rsp_pkt_len = set_cont_state_attr(rsp_pkt, rsp_pkt_len, max_attr_byte_cnt);

  if (is_err())
  {
    send_error_rsp(&db_hdl->db, get_err());
    return;
  }
  
  write2stack(db_hdl->db.sdp_con_id, rsp_pkt, rsp_pkt_len); 
}

void
handle_service_search_attr_req(service_search_attr_struct *db_hdl)
{
  unsigned char *tmp_ptr;
  unsigned int *rec_hdl_list;
  unsigned int rec_hdl_cnt;
  unsigned char rsp_pkt[1024];
  unsigned int max_attr_byte_cnt;
  int rsp_pkt_len = 0, tmp_len, des_len_pos, i, j = 0;

  rec_hdl_list = get_all_rec_hdl(db_hdl->service_class_list, db_hdl->service_class_cnt);

   D_REC("Got a record handle list");
  
  if (is_err())
  {
    D_REC("Error occured, sending error response");
    send_error_rsp(&db_hdl->db, get_err());
    return;
  }

  if (rec_hdl_list)
  {
    rec_hdl_cnt = 0;
    while (rec_hdl_list[rec_hdl_cnt] != NO_REC_HDL)
    {
      rec_hdl_cnt++;
    }
    D_REC("Found %d record handles", rec_hdl_cnt);
  }
  
  /* Skip the sdp header and the attribute byte count field */
  rsp_pkt_len = SDP_HDR_SIZE + 2;

  rsp_pkt[rsp_pkt_len++] = DES_HDR_2B;
  des_len_pos = rsp_pkt_len;
  rsp_pkt_len += 2;

  for (j = 0; j < rec_hdl_cnt; j++)
  {
    tmp_len = 2;
    for (i = 0; i < db_hdl->attr_cnt; i++)
    {
      if ((db_hdl->attr_list[i] & 0xffff) ^ (db_hdl->attr_list[i] >> 16))
      {
        tmp_ptr = get_attribute_range(xml_fd, rec_hdl_list[j], db_hdl->attr_list[i]);
      }
      else
      {
        tmp_ptr = get_attribute_list(xml_fd, rec_hdl_list[j], db_hdl->attr_list[i]);
      }
      if (tmp_ptr)
      {
        printf(__FUNCTION__ ": Copying %d bytes to rsp_pkt\n", tmp_ptr[1]);
        memcpy(rsp_pkt + rsp_pkt_len + tmp_len, tmp_ptr + 2, tmp_ptr[1]);
        tmp_len += tmp_ptr[1];

        printf(__FUNCTION__ ": list_len: %d\n", tmp_len);

        D_MEM("<--- free%d tmp_ptr 0x%8p", --malloc_dbg, tmp_ptr);
        free(tmp_ptr);
      }
      else if (is_err())
      {
        send_error_rsp(&db_hdl->db, get_err());
        return;
      }
    }
    if (tmp_len > 2) 
    {
      rsp_pkt[rsp_pkt_len] = DES_HDR;
      rsp_pkt[rsp_pkt_len + 1] = tmp_len - 2;
      rsp_pkt_len += tmp_len;
    }
  }

  /* Set the attribute byte count to packet length minus sdp pdu header size
     minus attribute byte count field length */
  rsp_pkt[SDP_HDR_SIZE]   = SHORT2CHAR_MS(rsp_pkt_len - SDP_HDR_SIZE - 2);
  rsp_pkt[SDP_HDR_SIZE+1] = SHORT2CHAR_LS(rsp_pkt_len - SDP_HDR_SIZE - 2);

  /* The length of the attribute list is the same as the packet length minus
     the packet header length minus the attibute byte count filed minus the
     data element sequence header*/
  rsp_pkt[des_len_pos]     = SHORT2CHAR_MS(rsp_pkt_len - SDP_HDR_SIZE - 2 - 3);
  rsp_pkt[des_len_pos + 1] = SHORT2CHAR_LS(rsp_pkt_len - SDP_HDR_SIZE - 2 - 3);

  set_sdp_hdr(rsp_pkt, SDP_SERVICESEARCHATTR_RSP, db_hdl->db.trans_id,
              rsp_pkt_len - SDP_HDR_SIZE);

  max_attr_byte_cnt = MIN(db_hdl->max_attr_byte_cnt, db_hdl->db.l2cap_mtu - 9);
  /* 9 comes from SDP_HDR_SIZE + attr byte nt field + 2 bytes for cont state */

  D_REC("l2cap_mtu:%d, mabc:%d, db->mabc:%d\n", db_hdl->db.l2cap_mtu, max_attr_byte_cnt, db_hdl->max_attr_byte_cnt);
  
  rsp_pkt_len = set_cont_state_attr(rsp_pkt, rsp_pkt_len, max_attr_byte_cnt);
  
  if (is_err())
  {
    send_error_rsp(&db_hdl->db, get_err());
    return;
  }
  
  /* FIXME: Add features to handle continuation state packets */
  write2stack(db_hdl->db.sdp_con_id, rsp_pkt, rsp_pkt_len); 
}

int
set_cont_state_search(unsigned char *pkt, unsigned int len, unsigned int max_rec_cnt)
{
  int cur_rec_cnt;
  int cnt_len;

  cur_rec_cnt = CHAR2INT16(pkt[SDP_HDR_SIZE+2], pkt[SDP_HDR_SIZE+3]);

  if (cur_rec_cnt > max_rec_cnt)
  {
    S_FNC("Setting cont state, cur_rec_cnt:%d, max_rec_cnt:%d",
          cur_rec_cnt, max_rec_cnt);

    pkt[SDP_HDR_SIZE]   = SHORT2CHAR_MS(max_rec_cnt);
    pkt[SDP_HDR_SIZE+1] = SHORT2CHAR_LS(max_rec_cnt);

    pkt[SDP_HDR_SIZE+2] = SHORT2CHAR_MS(max_rec_cnt);
    pkt[SDP_HDR_SIZE+3] = SHORT2CHAR_LS(max_rec_cnt);
    
     /* There are cnt_len byte too much to fit the data in one packet */
    cnt_len = (cur_rec_cnt - max_rec_cnt) * 4;
    
    /* The packet can't be longer than this, excluding the continuation state
       bytes */
    len -= cnt_len;
    
    cont_state_buf = malloc(sizeof(cont_state_struct) + cnt_len);
    D_MEM("---> malloc%d %ld bytes at 0x%8p", malloc_dbg++, sizeof(cont_state_struct) + cnt_len, cont_state_buf);

    if (!cont_state_buf)
    {
      set_err(SDP_INSUFFICIENT_RESOURCES);
      return 0;
    }
    
    cont_state_buf->pdu = pkt[SDP_HDR_TYPE];
    D_MISC("PDU: 0x%02x", cont_state_buf->pdu);
    
    cont_state_buf->len = cnt_len;
    memcpy(cont_state_buf->data, pkt + len, cnt_len);
    
    len = SDP_HDR_SIZE + 2 + 2 + (max_rec_cnt * 4);

    pkt[len++] = 1;
    pkt[len++] = 0;
  }
  else
  {
    D_MISC("No continuation state set\n");
    pkt[len++] = 0;
  }
  
  
  /* Change the length field */
  pkt[SDP_HDR_LENGTH_MS] = SHORT2CHAR_MS(len - SDP_HDR_SIZE);
  pkt[SDP_HDR_LENGTH_LS] = SHORT2CHAR_LS(len - SDP_HDR_SIZE);

  return len;
}

/* FIXME: This function only handles continuation state from one query yet */

int
set_cont_state_attr(unsigned char *pkt, int len, int max_attr_len)
{
  int cur_attr_cnt;
  int cont_len;

  cur_attr_cnt = CHAR2INT16(pkt[SDP_HDR_SIZE], pkt[SDP_HDR_SIZE+1]);

  S_FNC("len %d, max_attr_len %d, cur_attr_cnt %d", len, max_attr_len, cur_attr_cnt);

  PRINT_DATA(__FUNCTION__, pkt, len);

  if (max_attr_len < cur_attr_cnt)
  {
    S_FNC("max_attr_len:%d, cur_attr_cnt:%d and packet length:%d", max_attr_len, cur_attr_cnt, len);

    /* There are cont_len byte to much to fit the data in one packet */
    cont_len = cur_attr_cnt - max_attr_len;

    /* The packet can't be longer than this, excluding the continuation state
       bytes */
    len -= cont_len;

    cont_state_buf = malloc(sizeof(cont_state_struct) + cont_len);
    D_MEM("---> malloc%d %ld bytes at 0x%8p", malloc_dbg++, sizeof(cont_state_struct) + cont_len , cont_state_buf);

    if (!cont_state_buf)
    {
      set_err(SDP_INSUFFICIENT_RESOURCES);
      return 0;
    }
    
    cont_state_buf->pdu = pkt[SDP_HDR_TYPE];
    D_MISC("PDU: 0x%02x", cont_state_buf->pdu);

    cont_state_buf->len = cont_len;
    memcpy(cont_state_buf->data, pkt + len, cont_len);

    pkt[SDP_HDR_SIZE]   = SHORT2CHAR_MS(max_attr_len);
    pkt[SDP_HDR_SIZE+1] = SHORT2CHAR_LS(max_attr_len);
    
    pkt[len++] = 1;
    pkt[len++] = 0;
  }
  else
  {
    S_FNC("No continuation State set");
    pkt[len++] = 0;
  }
  
  /* Change the length field */

  S_FNC("Changing length field");
  pkt[SDP_HDR_LENGTH_MS] = SHORT2CHAR_MS(len - SDP_HDR_SIZE);
  pkt[SDP_HDR_LENGTH_LS] = SHORT2CHAR_LS(len - SDP_HDR_SIZE);

  return len;
}

void
send_cont_state_search_rsp(int len, unsigned char *info, int max_rec_cnt,
			   database_query_struct *db)
{
  unsigned char *send_buf;
  int send_len;
  
  if (!cont_state_buf) {
    send_error_rsp(db, SDP_INVALID_CONTINUATION_STATE);
  }
  else if ((max_rec_cnt * 4) >= cont_state_buf->len)
  {
    /* Allocate space for the SDP header, the attribute byte count field,
       the attributes and the continuation state field */
    send_len = SDP_HDR_SIZE + 2 + 2 + cont_state_buf->len + 1;

    send_buf = malloc(send_len);
    D_MEM("---> malloc%d %d bytes at 0x%8p", malloc_dbg++, send_len, send_buf);
    if (!send_buf)
    {
      send_error_rsp(db, SDP_INSUFFICIENT_RESOURCES);
      return;
    }

    set_sdp_hdr(send_buf, cont_state_buf->pdu, db->trans_id, send_len - SDP_HDR_SIZE);

    /* Set the attribute byte count field */
    send_buf[SDP_HDR_SIZE]   = SHORT2CHAR_MS(cont_state_buf->len / 4);
    send_buf[SDP_HDR_SIZE+1] = SHORT2CHAR_LS(cont_state_buf->len / 4);

    /* Set the attribute byte count field */
    send_buf[SDP_HDR_SIZE+2] = SHORT2CHAR_MS(cont_state_buf->len / 4);
    send_buf[SDP_HDR_SIZE+3] = SHORT2CHAR_LS(cont_state_buf->len / 4);

    memcpy(send_buf + SDP_HDR_SIZE + 2 + 2, cont_state_buf->data, cont_state_buf->len);

    send_buf[send_len - 1] = 0;

    /* Send the whole buffer */
    write2stack(db->sdp_con_id, send_buf, send_len);

    D_MEM("<--- free%d 0x%8p", --malloc_dbg, send_buf);
    free(send_buf);

    D_MEM("<--- free%d 0x%8p", --malloc_dbg, cont_state_buf);
    free(cont_state_buf);
    cont_state_buf = NULL;
  }
  else
  {
    /* Write as much as possible */

    /* FIXME: Have to implement this too... */

    send_error_rsp(db, SDP_INVALID_SDP_VERSION);
  }
}

void
send_cont_state_attr_rsp(int len, unsigned char *info, int max_attr_cnt,
                         database_query_struct *db)
{
  unsigned char *send_buf;
  int send_len;
  
  if (!cont_state_buf) {
    send_error_rsp(db, SDP_INVALID_CONTINUATION_STATE);
  }
  else if (max_attr_cnt >= cont_state_buf->len)
  {
    /* Allocate space for the SDP header, the attribute byte count field,
       the attributes and the continuation state field */
    send_len = SDP_HDR_SIZE + 2 + cont_state_buf->len + 1;

    send_buf = malloc(send_len);
    D_MEM("---> malloc%d %d bytes at 0x%8p", malloc_dbg++, send_len, send_buf);
    if (!send_buf)
    {
      send_error_rsp(db, SDP_INSUFFICIENT_RESOURCES);
      return;
    }

    set_sdp_hdr(send_buf, cont_state_buf->pdu, db->trans_id, send_len - SDP_HDR_SIZE);

    /* Set the attribute byte count field */
    send_buf[SDP_HDR_SIZE]   = SHORT2CHAR_MS(cont_state_buf->len);
    send_buf[SDP_HDR_SIZE+1] = SHORT2CHAR_LS(cont_state_buf->len);

    memcpy(send_buf + SDP_HDR_SIZE + 2, cont_state_buf->data, cont_state_buf->len);

    send_buf[send_len - 1] = 0;

    /* Send the whole buffer */
    write2stack(db->sdp_con_id, send_buf, send_len);

    D_MEM("<--- free%d 0x%8p", --malloc_dbg, send_buf);
    free(send_buf);

    D_MEM("<--- free%d 0x%8p", --malloc_dbg, cont_state_buf);
    free(cont_state_buf);
    cont_state_buf = NULL;
  }
  else
  {
    /* Write as much as possible */

    /* FIXME: Have to implement this too... */

    send_error_rsp(db, SDP_INVALID_SDP_VERSION);
  }
}