sdp_parser.c

bluetooth 驱动

  {
    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)
  {
    D_MISC("Sending continuationstate packet");
    send_cont_state_attr_rsp(cont_state_len, data + cur_pos, max_attr_cnt,
                             db);
  }
  else
  {
    len = sizeof(service_attr_struct) + attr_list_pos * sizeof *attr_list;

    if (!(db_hdl = malloc(len))) {
            D_ERR("malloc failed to allocate %d bytes!", len);
            send_error_rsp(db, SDP_INSUFFICIENT_RESOURCES);
            return;
    }

    D_MEM("---> malloc%d %d bytes at 0x%08x", malloc_dbg++, len, (int)db_hdl);

    
    db->pkt_type = SDP_SERVICEATTR_REQ;
    memcpy(&db_hdl->db, db, sizeof(database_query_struct));    
    
    db_hdl->max_attr_byte_cnt = max_attr_cnt;
    db_hdl->rec_hdl = rec_hdl;
    db_hdl->attr_cnt = attr_list_pos;
    memcpy(db_hdl->attr_list, attr_list, attr_list_pos * sizeof *attr_list);
    
    /* Here we ask the database for the requested attributes */

    handle_query(&db_hdl->db);
    
    D_MEM("<--- free%d 0x%08x", --malloc_dbg, (int) db_hdl);
    free(db_hdl);
  }
}

void 
process_service_search_attr_req(database_query_struct *db, unsigned char *data, unsigned short len)
{
  service_search_attr_struct *db_hdl;
  unsigned int service_search_uuid[12];
  int service_search_uuid_cnt;
  unsigned char des_len;
  int tmp_pos, tmp_len, i, cur_pos = 0;
  int max_attr_cnt;
  int new_pos[1];
  unsigned int attr_list[256];
  int attr_list_pos = 0;
  unsigned char 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");
      break;
    }

    if (data[cur_pos] == UUID16_HDR)
    {
      service_search_uuid[i] = CHAR2INT16(data[cur_pos + 1],
                                          data[cur_pos + 2]);
      D_MISC("Got Service class 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("Got Service class 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("Got Service class 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_error_rsp(db, SDP_INVALID_REQUEST_SYNTAX);
      return;
    }
  }
  service_search_uuid_cnt = i;

  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);

  /* 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]) & 0xffff); 
      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)
  {
    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 8%d)", len, cur_pos);
    send_error_rsp(db, SDP_INVALID_PDU_SIZE);
    return;
  }
  
  if (cont_state_len)
  {
    D_MISC("Sending continuationstate packet");
    send_cont_state_attr_rsp(cont_state_len, data + cur_pos, max_attr_cnt,
                             db);
  }
  else
  {
    tmp_len = sizeof(service_search_attr_struct) + attr_list_pos * sizeof *attr_list;

    if (!(db_hdl = malloc(tmp_len))) {
            D_ERR("malloc failed to allocate %d bytes!", tmp_len);
            send_error_rsp(db, SDP_INSUFFICIENT_RESOURCES);
            return;
    }

    D_MEM("---> malloc%d %d bytes at 0x%08x", malloc_dbg++, tmp_len, (int) db_hdl);
    
    db->pkt_type = SDP_SERVICESEARCHATTR_REQ;
    memcpy(&db_hdl->db, db, sizeof(database_query_struct));
    
    db_hdl->max_attr_byte_cnt = max_attr_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);
    db_hdl->attr_cnt = attr_list_pos;
    memcpy(db_hdl->attr_list, attr_list, attr_list_pos * sizeof *attr_list);
    
    /* Here we ask the database for the requested attributes */
    handle_query(&db_hdl->db);

    D_MEM("<--- free%d 0x%08x", --malloc_dbg, (int) db_hdl);
    free(db_hdl);
  }
}

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)
{
}

void
send_error_rsp(database_query_struct *db, unsigned short err_code)
{
  unsigned char sdp_data[7];
  unsigned short pdu_len;

  /* Since we do not send any error information, the pdu length is just the
     size of the error code length, which is two bytes */

  pdu_len = 2;
  
  sdp_data[0] = SDP_ERROR_RSP;
  sdp_data[1] = (db->trans_id >> 8) & 0xff;
  sdp_data[2] = db->trans_id & 0xff;
  sdp_data[3] = (pdu_len >> 8) & 0xff;
  sdp_data[4] = pdu_len & 0xff;

  sdp_data[5] = (err_code >> 8) & 0xff;
  sdp_data[6] = err_code & 0xff;

  write2stack(db->sdp_con_id, sdp_data, 7);
}

/* "Support" functions used by the other functions in this file */

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

  size_index = GET_SIZE(*data);
  switch (size_index)
  {
  case 0: 
    *new_pos = 1;
    return 1;
  case 1:
    *new_pos = 1;
    return 2;
  case 2:
    *new_pos = 1;
    return 4;
  case 3:
    *new_pos = 1;
    return 8;
  case 4:
    *new_pos = 1;
    return 16;
  case 5:
    *new_pos = 2;
    return data[1];
  case 6:
    *new_pos = 3;
    return CHAR2INT16(data[1], data[2]);
  case 7:
    *new_pos = 5;
    return CHAR2INT32(data[1], data[2], data[3], data[4]);
  }
  return 0;
}

void
write2stack(int sdp_con_id, char *data, int len)
{
  data_struct db_hdl;
  struct iovec vec[2];

  D_XMIT("Writing %d bytes to sdp_con_id %d", len, sdp_con_id);
  
  db_hdl.sdp_con_id = sdp_con_id;
  db_hdl.len = len;

  vec[0].iov_base = &db_hdl;
  vec[0].iov_len = sizeof db_hdl;
  vec[1].iov_base = data;
  vec[1].iov_len = len;
  
  writev(stack_if_fd, vec, 2);
}

#ifndef BTD_USERSTACK
void
start_srvsock(char *name)
{
  stack_if_fd = open(name, O_RDWR);
  if (stack_if_fd < 0)
  {
    perror("Could not open proc file");
    exit(1);
  }

  while (1)
  {
    char buf[256];
    int len = read(stack_if_fd, buf, 256);

    if (len < 0)
    {
      perror("Read error");
    }
    else if (len == 0)
    {
      syslog(LOG_INFO, "No data was read");
    }
    else
    {
      sdp_parse_data(buf, len);
    }
  }
}

#else

void
listen_srvsock(int clnt_fd)
{
  fd_set rfd;
  char databuf[256];

  while (1)
  {  
    FD_ZERO(&rfd); 
    FD_SET(clnt_fd, &rfd);
    
    select(FD_SETSIZE, &rfd, NULL, NULL, NULL);

    printf(__FUNCTION__ "\n");
    
    if (FD_ISSET(clnt_fd, &rfd))
    {
      int len = read(clnt_fd, databuf, 256);
      
      if (len > 0)
      {
        sdp_parse_data(databuf, len);
      }
      else
      {
        /* Client closed... lets restart server */
        return;
      } 
    }
  }
}

/* Local UNIX socket stuff */
void
start_srvsock(char *name)
{
  struct sockaddr_un server_address;
  struct sockaddr_un client_address;
  int server_len;
  int client_len;  
  int sdp_srv_sock;
  int sdp_clnt_sock;

  syslog(LOG_INFO, "Opening server socket %s", name);

  /* remove any old socket */
  unlink(name);

  /* open socket */    
  if ((sdp_srv_sock = socket(AF_UNIX, SOCK_STREAM, 0)) < 0)
  {  
    perror("Open socket");
    exit(1);
  }

  server_address.sun_family = AF_UNIX;
  strcpy(server_address.sun_path, name);
  server_len = sizeof(server_address);
  bind(sdp_srv_sock, (struct sockaddr *)&server_address, server_len);

  while (1)
  {
    syslog(LOG_INFO, "SDP Server listens...");
    listen(sdp_srv_sock, 5);

    client_len = sizeof(client_address);
    sdp_clnt_sock =
      accept(sdp_srv_sock, (struct sockaddr*)&client_address, &client_len);

    stack_if_fd = sdp_clnt_sock;

    listen_srvsock(sdp_clnt_sock);
    close(sdp_clnt_sock);
  }
}
#endif /* BTD_USERSTACK */

int
main(int argc, char **argv)
{
  char *xml_file;
  char *proc_file;
  int xml_fd;

  if (argc > 3)
  {
    fprintf(stderr, "Syntax: sdp_server [<xml file> [<proc file>]]\n");
    exit(1);
  }

  syslog(LOG_INFO, "SDP Server starting");  

  xml_file = (argc >= 2 ? argv[1] : SDP_XML_FILE);
  proc_file = (argc >= 3 ? argv[2] : SDP_PROC_FILE);
  
  xml_fd = open(xml_file, O_RDONLY);
  if (xml_fd < 0)
  {
    perror("Could not open xml file");
    exit(1);
  }

  init_sdp_server(xml_fd);
  
  start_srvsock(proc_file);

  exit(0);
}

/****************** END OF FILE sdp_parser.c ********************************/