btd.c

bluetooth 驱动

/* Create a thread that reads data from the pty and 
   passes it to the rfcomm layer */
int init_pty_thread(void)
{
  printf("Initiating pty thread\n");
  if (pthread_create(&pty_thread, NULL, 
                     (void*)pty_receive_thread, NULL)!=0)
    perror("pthread_create");    
  sleep(1); /* wait for thread to start */
  return 0;
}

void pty_receive_thread(void)
{
  fd_set rfd;
  char databuf[4096];

  while (1)
  {  
    FD_ZERO(&rfd); 
    FD_SET(pty_master_fd, &rfd);
    
    select(pty_master_fd+1, &rfd, NULL, NULL, NULL);

    if (FD_ISSET(pty_master_fd, &rfd))
    {
      int len = read(pty_master_fd, &databuf, 4096);

      if (len > 0)
      {
	bt_write_top(databuf,len,0);
      }
    }
  }
}

int bt_write_lower_driver(unsigned char *data, int len)
{
  int i;

  BT_DATA("<--|X|    %3d\n", len);

  PRINTPKT("bt_write_lower_driver ", data, len);
  i = write(phys_fd, data, len);
  return i;
}


/* Only works for the last connected rfcomm session */
int
bt_write_top(char *buf, int count, int line)
{
  int retval;
  int bytes_sent = 0;

  printf("bt_write_top : try sending data on last connected ch\n");

  BT_DATA("   |X|<-- %3d [%d]\n", count, line);
  
  while (bytes_sent!=count)
  {
    retval = rfcomm_send_data(CREATE_RFCOMM_ID(line,test_dlci), 
			      buf + bytes_sent, count-bytes_sent); 
    
   if (retval > 0)
      bytes_sent+=retval;
    else if (retval==0)
      usleep(100000); /* wait some ... */
   else
   {
     printf("error\n");
     return retval;
   }   
    usleep(1000); /* wait some time ... */
  };

  bt_stat.bytes_sent+=bytes_sent;
  
  return bytes_sent;
}

int bt_receive_top(unsigned int con_id, unsigned char *data, int len)
{
  int n;

  BT_DATA("   |X|--> %3d [%d]\n", len, GET_LINE(con_id));

  if (modem_emulation && !modem_connected)
  {
    modem_emulator(0xb055e, data, len);
    return len;
  }
  
  /* feed this to PTY connected to pppd or whatever application 
     that may be running there... */
  
  if ((n = write(pty_master_fd, data, len)) != len)
  {
    BT_DATA("bt_receive_top: tried to write %d bytes, wrote %d\n", len, n);
  }

  bt_stat.bytes_received+=n;

  return n;
}

void
bt_connect_ind(unsigned int con_id) 
{
  DSYS("Got connect indication on PSM %d\n", GET_PSM(con_id));
}

void
bt_connect_cfm(unsigned int con_id, int status)
{
  unsigned short psm;
  int line;

  psm = GET_PSM(con_id);

  if (status) {
    /* fixme -- only works for rfcomm now */
    line = GET_LINE(con_id);

    if ((line < 0) || (line > BT_NBR_DATAPORTS)) {
      printf("bt_connect_cfm on invalid line (%d)\n", line);
      return;
    }
    printf("bt_connect_cfm : failed, status %d [%s] line %d\n", status, psmname(psm), line);

    return;
  }

  switch (psm) {
  case RFCOMM_LAYER:
    printf("bt_connect_cfm [%s]\n", psmname(psm));
    break;

  case SDP_LAYER:
    printf("bt_connect_cfm [%s]\n", psmname(psm));
    break;

  case TCS_LAYER:
    printf("bt_connect_cfm [%s]\n", psmname(psm));
    break;

  default:
    printf("bt_connect_cfm : unknown layer %d\n", psm);
    break;
  }
}

void
bt_disconnect_ind(unsigned int con_id) 
{
  DSYS("Got disconnect indication on PSM %d\n", GET_PSM(con_id));
}


int
bt_register_rfcomm(struct rfcomm_con *rfcomm, u8 dlci)
{
  test_rfcomm = rfcomm;
  test_dlci = dlci;
  return 0;
}

int bt_register_sdp(u8 line)
{
  return 0;
}
 
int bt_unregister_sdp(u8 line)
{
  return 0;
}

int bt_initiated(void)
{
  return bt_initdone;
}

int bt_use_bcsp(int new_use_bcsp)
{
  return 0;
}

int bt_dfu_mode(int new_dfu_mode)
{
  return 0;
}

/* TCP socket */
static int open_tcpsocket(char *addrstr, int role)
{
  int server_sockfd;
  struct sockaddr_in server_address;
  int client_sockfd;
  struct sockaddr_in client_address;

  int server_len;
  int client_len;
  int port;
  char ipstr[16];
  char *pos;
  
  /* parse address string */

  if (!(pos = strchr(addrstr, ':')))
  {
    fprintf(stderr, "Port argument missing!\n");
    exit(1);
  }

  /* copy ip address */
  *pos = '\0';
  strncpy(ipstr, addrstr, sizeof ipstr);
  ipstr[sizeof ipstr - 1] = '\0';

  /* extract port number */
  port = atoi(pos+1);

  if (role == SERVER)
  {
    /* open socket */    
    server_sockfd = socket(AF_INET, SOCK_STREAM, 0);
    server_address.sin_family = AF_INET;

    server_address.sin_addr.s_addr = htonl(INADDR_ANY);
    server_address.sin_port = htons(port);

    server_len = sizeof(server_address);

    if (bind(server_sockfd, (struct sockaddr *)&server_address, server_len))
    {
      perror("bind");
      exit(1);
    }

    if (listen(server_sockfd, 5))
    {
      perror("listen");
      exit(1);
    }
    
    /* request for a new connection */
    client_sockfd = accept(server_sockfd,
			   (struct sockaddr *)&(client_address),
			   &client_len);
  }
  else
  {
    /* Client */

    printf("Connecting to TCP server socket [%s:%d]\n", ipstr, port);
    
    /* open socket */    
    client_sockfd = socket(AF_INET, SOCK_STREAM, 0);

    /* destination address */
    server_address.sin_family = AF_INET;
    server_address.sin_addr.s_addr = inet_addr(ipstr);
    server_address.sin_port = htons(port); /* for example... */
    server_len = sizeof(server_address);
 
    if (connect(client_sockfd, 
		(struct sockaddr *)&server_address, 
		server_len) < 0) {
      perror("open_tcpsocket");
      exit(1);
    }
  }

  return client_sockfd;
}

#endif /* BTD_USERSTACK */

/* Local UNIX socket stuff */
static int open_socket(char *name, int role)
{
  int server_sockfd;
  struct sockaddr_un server_address;
  int client_sockfd;
  struct sockaddr_un client_address;
  int server_len;
  int client_len;  

  syslog(LOG_INFO, "Opening socket %s ", name);
  if (role == SERVER)
  {
    /* remove any old socket */
    unlink(name);
    
    /* open socket */    
    server_sockfd = socket(AF_UNIX, SOCK_STREAM, 0);
    server_address.sun_family = AF_UNIX;
    strcpy(server_address.sun_path, name);
    server_len = sizeof(server_address);
    bind(server_sockfd, (struct sockaddr *)&server_address, server_len);
    listen(server_sockfd, 5);
    client_sockfd = accept(server_sockfd, 
                           (struct sockaddr *)&(client_address),
                           &client_len);
  }
  else
  {
    /* Client */
    client_sockfd = socket(AF_UNIX, SOCK_STREAM, 0);

    /* 'destination' socket */
    server_address.sun_family = AF_UNIX;
    strcpy(server_address.sun_path, name);
    server_len = sizeof(server_address);
      
    if (connect(client_sockfd, 
                (struct sockaddr *)&server_address, 
                server_len) < 0) {
      syslog(LOG_ERR, "open_socket %s failed", name);
      return -1;
    }

    syslog(LOG_INFO, "Socket connected to %s\n", server_address.sun_path);
  }

  return client_sockfd;
}

#ifdef USE_IPASSIGN

struct ipa_client* ipa_send(ipa_request *req)
{
  int len;
  struct ipa_client *rsp = NULL;
  
  syslog(LOG_INFO, "Opening socket to IP Assigner\n");
  
  if ((ipa_sock = open_socket(SRVSOCKET, CLIENT)) < 0)
  {  
    perror("open_socket");
    return NULL;
  }
  
  syslog(LOG_INFO, "ipa_send : sending ipa request\n");
  write(ipa_sock, req, sizeof(struct ipa_request));

  syslog(LOG_INFO, "ipa_send : listening for incoming responses");  

  len = read(ipa_sock, &ipa_buf, 128);  

  /* FIXME -- clean up */

  if(!strncasecmp((char *)&ipa_buf, "OK", 2))
  {
    syslog(LOG_INFO, "ipa_send : request succeeded");
    syslog(LOG_INFO, "ipa_send : closing ipa socket");
    close(ipa_sock);
    return (struct ipa_client *)ipa_buf; /* FIXME !*/ 
  }
  else if(!strncasecmp((char *)&ipa_buf, "ERROR", 5))
  {
    syslog(LOG_INFO, "ipa_send : request failed");
    syslog(LOG_INFO, "ipa_send : closing ipa socket");
    close(ipa_sock);
    return (struct ipa_client *)NULL;
  }
  else if (len == sizeof(struct ipa_client))
  {
    syslog(LOG_INFO, "ipa_send : request succeeded");
    rsp = (struct ipa_client*)ipa_buf;  
  }

  syslog(LOG_INFO, "ipa_send : closing ipa socket");
  close(ipa_sock);
  return rsp;
}


void ipa_getpars(void)
{
  unsigned char local_bd[6];
  unsigned char remote_bd[6];
  char *ms_dns = NULL;
  char *ms_dns2 = NULL;
  char *ms_wins = NULL;
  char *ms_wins2 = NULL;
  struct ipa_client *client;

  read_local_bd(bt_cfd, local_bd);

  /* fixme -- for now only use line 0 */
  read_remote_bd(bt_cfd, 0, remote_bd);

  /* 
   * Get options from IPAssigner 
   */
  ipa_req.type = GETCLIENTIP;
  memcpy(&ipa_req.remote_bd, remote_bd, 6);

  syslog(LOG_INFO, "ipa_getpars sending req");

  /* send request */
  client = ipa_send(&ipa_req);

  if (client == NULL)
  {
    syslog(LOG_INFO, "Failed to get client settings");
    sleep(3);

    /* Use noip if not set from btd option */
    if (!remote_address)
    {
      remote_address = strdup("");
    }   
    use_radius = 0;
    use_radius_ip_assign = 0;
    use_proxyarp = 1;
  } 
  else 
  {
    a_client = *client;
    
    /* Set local values */
    use_proxyarp = a_client.proxyarp;
    use_radius = a_client.useradius;
    use_radius_ip_assign = a_client.useradiusip;

    if (!remote_address)
    {     
      remote_address = strdup(inet_ntoa(a_client.ip));
    }
    else
    {
      syslog(LOG_INFO, "remote_address already set from btd option!");
    }

    ppp_netmask = strdup(inet_ntoa(a_client.netmask));
    syslog(LOG_INFO, "Client IP : %s", remote_address);

    /* Since most clients should be able to receive the DNS configuration the 
       MS style we send them with ms-dns (if we got any)! */
    
    if(a_client.nbr_of_dns > 0)
    {
      ms_dns = "ms-dns";
      prim_dns = strdup(inet_ntoa(a_client.dns[0]));
      if(a_client.nbr_of_dns > 1)
      {
        ms_dns2 = "ms-dns";
        sec_dns = strdup(inet_ntoa(a_client.dns[1]));
      }
      printf("DNS used : prim: %s sec: %s\n", prim_dns?prim_dns:"none", 
             sec_dns?sec_dns:"none");
      syslog(LOG_INFO, "DNS used : prim: %s sec: %s\n", prim_dns?prim_dns:"none", sec_dns?sec_dns:"none");
    }
    else
      syslog(LOG_INFO, "No DNS configured");

    if(a_client.nbr_of_wins > 0)
    {
      ms_wins = "ms-wins";
      prim_wins = strdup(inet_ntoa(a_client.wins[0]));
      if(a_client.nbr_of_wins > 1)
      {
        ms_wins2 = "ms-wins";
        sec_wins = strdup(inet_ntoa(a_client.wins[1]));
      }
      printf("MS WINS used : prim: %s sec: %s\n", prim_wins?prim_wins:"none", 
             sec_wins?sec_wins:"none");
      syslog(LOG_INFO, "MS WINS used : prim: %s sec: %s\n", prim_wins?prim_wins:"none", sec_wins?sec_wins:"none");
    }
    else
      syslog(LOG_INFO, "No WINS configured");
    

    sprintf(local_bd_string, "%02X:%02X:%02X:%02X:%02X:%02X",
            local_bd[0], local_bd[1], local_bd[2],
            local_bd[3], local_bd[4], local_bd[5]);

    sprintf(remote_bd_string, "%02X:%02X:%02X:%02X:%02X:%02X",
            remote_bd[0], remote_bd[1], remote_bd[2],
            remote_bd[3], remote_bd[4], remote_bd[5]);    

    syslog(LOG_INFO, "local bd %s, remote bd %s", local_bd_string, remote_bd_string);
  }
}

#endif /* USE_IPASSIGN */


void
set_bt_line_disc(int phys_fd, int bt_disc, char* physdev)
{
  if (ioctl(phys_fd, TIOCSETD, &bt_disc) < 0)
  {
    perror("Set bt line disc");
    printf("Forgot to insmod bt.o ? [bt_disc %d]\n", bt_disc);
    exit(1);
  }
  printf("Registered bluetooth line discipline on %s\n", physdev);
}

/* unique message status codes 32 bits : | layer 16 bits | code 16 bits | */

#define MSG_LAYER_HCI    0
#define MSG_LAYER_L2CAP  1
#define MSG_LAYER_RFCOMM 2
#define MSG_LAYER_SDP    4
#define MSG_LAYER_TCS    8

/* Result in responses in L2CAP */
#define RES_PSMNEG 0x02
#define RES_SECNEG 0x03
#define RES_NOSRC  0x04

const char *error_msg(int err)
{
  int layer = (err >> 16) & 0xffff;
  int code = err & 0xffff;

  switch (layer)
  {
  case MSG_LAYER_HCI:
    switch(code)
    {       
    case 4:        
      return "HCI - Page Timeout";
    default:
      return "HCI - Unknown reason";
      break;
    }
    break;
    
   case MSG_LAYER_L2CAP:
     switch(code)
     { 
     case RES_PSMNEG:
       return "L2CAP - PSM not valid";
     case RES_SECNEG:
       return "L2CAP - Security block";
     case RES_NOSRC:
       return "L2CAP - Remote side has no resources";
     default:
       return "L2CAP - unknown reason";
     }
     break;
    
  default:
    return "Unknown layer - unknown reason";
  }
}

const char* psmname(unsigned short psm)
{
  switch(psm)
  {
  case RFCOMM_LAYER:
    return "RCOMM";
  case SDP_LAYER:
    return "SDP";
  case  TCS_LAYER:
    return "TCS";
  default:
    return "UNKNOWN";
  }
}

void
bt_connect(int bt_fd, unsigned char *bd, unsigned i