src/hcidriver.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <math.h>
   4 #include <string.h>
   5 #include <errno.h>
   6 #include <unistd.h>
   7 #include <sys/poll.h>
   8 #include <sys/ioctl.h>
   9 #include "hcidriver.h"
  10
  11
  12
  13
  14 /************************** hci command functions**************************/
  15 int call_hci_inquiry_cmd(int dd, bt_address *p_addressarray,int timeout){
  16         inquiry_cp cmdp,*p_cmdp=&cmdp;
  17         hci_request req,*p_req=&req;
  18
  19         //hci_inquiry_complete_ev com_ev,*p_com_ev=&com_ev;
  20         __u16 OCF_OGF,*p_OCF_OGF=&OCF_OGF;
  21
  22         p_cmdp->lap[0]=0x33;
  23         p_cmdp->lap[1]=0x8b;
  24         p_cmdp->lap[2]=0x9e;
  25         p_cmdp->length=5;
  26         p_cmdp->num_rsp=5;
  27         memset(p_req,0,sizeof(req));
  28         assemble_ocf_ogf(0x01,0x01,p_OCF_OGF);
  29         p_req->p_OCF_OGF=p_OCF_OGF;
  30         p_req->p_retcmdp=p_addressarray;
  31         p_req->p_cmdp=p_cmdp;
  32         p_req->retcmdp_len=INQUIRY_INFO_SIZE;
  33         p_req->cmdp_len=INQUIRY_CP_SIZE;
  34         p_req->event=EVT_INQUIRY_RESULT;
  35
  36         if(hci_send_request(dd,p_req,timeout)<0)
  37                 return -1;
  38 //      if(p_com_ev->status)
  39 //              return -1;
  40
  41
  42         return 0;
  43 }
  44 // fill up the input parameter address pointer by one address
  45 int call_hci_read_bd_addr_cmd(int dd,bt_address *p_address,__u16 *p_ocf_ogf){
  46         read_bd_addr_rp cmdp,*p_cmdp=&cmdp;   //command parameters return/comand
  47         hci_request req,*p_req=&req;
  48
  49         memset(p_req,0,sizeof(req));
  50         p_req->p_OCF_OGF=p_ocf_ogf;
  51         p_req->p_retcmdp=p_cmdp;
  52         p_req->retcmdp_len=READ_BD_ADDR_RP_SIZE;
  53         p_req->cmdp_len=0;
  54         p_req->event=EVT_CMD_COMPLETE;
  55
  56         if(hci_send_request(dd,p_req,1000)<0)
  57                 return -1;
  58         if(p_cmdp->status)
  59                 return -1;
  60         bacpy(p_address, &cmdp.bdaddr);
  61
  62         return 0;
  63 }
  64
  65 int call_hci_read_local_name_cmd(int dd, void *p_name,__u16 *p_ocf_ogf){
  66         read_local_name_rp cmdp,*p_cmdp=&cmdp;
  67         hci_request req,*p_req=&req;
  68
  69         memset(p_req,0,sizeof(req));
  70         p_req->p_OCF_OGF=p_ocf_ogf;
  71         p_req->p_retcmdp=p_cmdp;
  72         p_req->retcmdp_len=READ_LOCAL_NAME_RP_SIZE;
  73         p_req->cmdp_len=0;
  74         p_req->event=EVT_CMD_COMPLETE;
  75
  76         if(hci_send_request(dd,p_req,1000)<0)
  77                 return -1;
  78         if(p_cmdp->status)
  79                 return -1;
  80         memcpy(p_name, &p_cmdp->name, 8); //copy name
  81         return 0;
  82
  83 }
  84
  85 int call_hci_create_connection_cmd(int dd, bt_address *p_address, int timeout){
  86         create_conn_cp cmdp,*p_cmdp=&cmdp;
  87         hci_request req,*p_req=&req;
  88         __u16 OCF_OGF,*p_OCF_OGF=&OCF_OGF;
  89
  90         p_cmdp->bdaddr=*p_address;
  91         p_cmdp->pkt_type=0x0010;
  92         p_cmdp->pscan_rep_mode=0x01;
  93         p_cmdp->pscan_mode=0x00;
  94         p_cmdp->clock_offset=0xf000;
  95         p_cmdp->role_switch=0x00;
  96         memset(p_req,0,sizeof(req));
  97         assemble_ocf_ogf(0x05,0x01,p_OCF_OGF);
  98         p_req->p_OCF_OGF=p_OCF_OGF;
  99         p_req->p_cmdp=p_cmdp;
 100         p_req->cmdp_len=CREATE_CONN_CP_SIZE;
 101         p_req->event=EVT_CONN_COMPLETE;
 102
 103         if(hci_send_request(dd,p_req,timeout)<0)
 104                 return -1;
 105         //if(p_com_ev->status)
 106         //      return -1;
 107
 108         return 0;
 109 }
 110
 111
 112 /*******************************HCI main functions ********************************/
 113
 114 //create and bind the socket to one device, return -1 when error, device descriptor when OK
 115 int hci_open_device(int dev_id){
 116         struct sockaddr_hci address;
 117         int dd;
 118
 119         if((dd=socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI))<0){
 120                 perror("socket no created");
 121                 return dd; //it return -1 when socket isn't created
 122         }
 123
 124         memset(&address,0,sizeof(address));
 125         address.hci_family=AF_BLUETOOTH;
 126         address.hci_dev=dev_id;
 127         if(bind(dd,(struct sockaddr *) &address, sizeof(address))<0){
 128                 perror("Socket not binded to hci device");
 129                 close(dd);
 130                 return -1;
 131         }
 132
 133         return dd;
 134 }
 135
 136 int hci_close_dev(int dd){
 137         return close(dd);
 138 }
 139
 140 int hci_send_command(int dd, hci_request *p_req){
 141         __u8 array[p_req->cmdp_len+4]; //type + OCF+OGF+plen
 142         //__u16 correct_opcode,*p_cor_opcode=&correct_opcode;
 143         //correct_opcode=swap_2_bytes(*(p_req->p_OCF_OGF));
 144         int ii;
 145         array[0]=0x01;
 146         memcpy(&array[1],p_req->p_OCF_OGF,2);
 147         array[3]= p_req->cmdp_len;
 148         if(p_req->cmdp_len > 0){
 149                 memcpy(&array[4],p_req->p_cmdp,p_req->cmdp_len); // !!!!!!!!! segmentation fault
 150         }
 151
 152         for(ii=0;ii<sizeof(array);ii++){
 153                 printf(" %x",array[ii]);
 154         }
 155         printf("\n");
 156
 157         while(write(dd, &array, (p_req->cmdp_len+4))<0){
 158                 perror("write interrupted");
 159                 if(errno == EAGAIN || errno == EINTR)
 160                         continue;
 161                 return -1;
 162         }
 163         return 0;
 164 }
 165
 166
 167 int hci_send_request(int dd, hci_request *p_req,int timeout){
 168         __u8 recbuf[HCI_MAX_EVENT_SIZE],*p_recbuf;
 169         int j,count=0;
 170         socklen_t len;
 171         hci_event_hdr *p_hdr;
 172         struct hci_filter nf, of;
 173         int try_count, sign=0;;
 174
 175
 176
 177
 178         len = sizeof(of);
 179         if(getsockopt(dd, SOL_HCI, HCI_FILTER, &of, &len)<0){
 180                 printf("some problem with getsockopt: %d",dd);
 181                 return -1;
 182         }
 183
 184         hci_filter_clear(&nf);
 185         hci_filter_set_ptype(HCI_EVENT_PKT, &nf);
 186         hci_filter_set_event(EVT_CMD_STATUS, &nf);
 187         hci_filter_set_event(p_req->event, &nf);
 188         hci_filter_set_opcode(*(p_req->p_OCF_OGF), &nf);
 189
 190         if(setsockopt(dd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf))<0){
 191                 printf("some problem with setsockopt: %d",dd);
 192                 return -1;
 193         }
 194
 195         if(hci_send_command(dd, p_req)<0){
 196                 printf("some problems with sending command: %d",dd);
 197                 goto fail;
 198         }
 199
 200         try_count= 10;
 201
 202         while(try_count--){
 203                 printf("try %d \n",try_count);
 204                 evt_cmd_complete *cc;
 205                 evt_cmd_status *cs;
 206                 evt_remote_name_req_complete *rnrc;
 207                 remote_name_req_cp *cpar;
 208                 inquiry_info_with_rssi *p_res_ev;
 209
 210                 if(timeout){
 211                         struct pollfd p;
 212                         int n;
 213
 214                         p.fd = dd; p.events = POLLIN;
 215                         while((n = poll(&p, 1, timeout)) < 0){  //pool return 1 when there are some incoming data
 216                                 if(errno == EAGAIN || errno == EINTR)
 217                                         continue;
 218                                 perror("Problem with poll");
 219                                 goto fail;
 220                         }
 221
 222                         if (!n) {  //time out pool return 0
 223                                 errno = ETIMEDOUT;
 224                                 perror("Poll return 0 timeout");
 225                                 printf("exited pool timeout \n ");
 226                                 goto fail;
 227                         }
 228
 229                         timeout -= 10;
 230                         if(timeout < 0) timeout = 0;
 231
 232                 }
 233                 printf("Timeout after round is: %d \n",timeout);
 234
 235                 while((len=read(dd, recbuf, sizeof(recbuf)))<0){
 236                         if(errno == EAGAIN || errno == EINTR)
 237                                 continue;
 238                         perror("Problem with cmd sending");
 239                         goto fail;
 240                 }
 241                 printf("Count of received bytes %d \n ",len);
 242                 for(j=0;j<len;j++){
 243                         printf("%2.2X ",recbuf[j]);
 244                 }
 245                 printf("\n");
 246                 p_hdr = (void *) (recbuf + 1);
 247                 p_recbuf = recbuf + (1 + HCI_EVENT_HDR_SIZE);
 248                 len -= (1 + HCI_EVENT_HDR_SIZE);
 249
 250                 switch(p_hdr->evt){
 251                         case EVT_CMD_STATUS:
 252                                 cs = (void *) p_recbuf;
 253                                 if(cs->opcode != *(p_req->p_OCF_OGF))
 254                                         continue;
 255                                 if(p_req->event != EVT_CMD_STATUS){
 256                                         if(cs->status){         // if there is something except 0 it is error
 257                                                 errno = EIO;
 258                                                 perror("Some error state has occured on receive");
 259                                                 goto fail;
 260                                         }
 261                                         break;
 262                                 }
 263                                 p_req->retcmdp_len = min(len, p_req->retcmdp_len);
 264                                 memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
 265                                 goto succes;
 266
 267                         case EVT_CMD_COMPLETE:
 268                                 cc = (void *) p_recbuf;
 269                                 if(cc->opcode != *(p_req->p_OCF_OGF))
 270                                         continue;
 271                                 p_recbuf += EVT_CMD_COMPLETE_SIZE;
 272                                 len -= EVT_CMD_COMPLETE_SIZE;
 273
 274                                 p_req->retcmdp_len = min(len, p_req->retcmdp_len);
 275                                 memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
 276                                 printf("retcmdp lenght: %d \n",p_req->retcmdp_len);
 277                                 goto succes;
 278                         //      __u8 first,*p_first;
 279                         //      p_first=&first;
 280                         //      for(j=0;j<p_req->retcmdp_len;j++){
 281                 //                      memcpy(p_first,p_req->p_retcmdp+j,1);
 282                 //                      printf("%d. byte is: %X \n",j,first);
 283                                 //      printf(" %X",*((unsigned int *)p_req->p_retcmdp+j));
 284                                         //printf("address of p_req: %d \n",p_req);
 285                                         //printf("address of p_req: %d \n",p_req->p_retcmdp);
 286                 //              }
 287                 //              printf("Ok\n");
 288
 289
 290                         case EVT_REMOTE_NAME_REQ_COMPLETE:
 291                                 if(p_hdr->evt != p_req->event)
 292                                         break;
 293                                 rnrc = (void *) p_recbuf;
 294                                 cpar = p_req->p_cmdp;
 295
 296                                 if(bacmp(&rnrc->bdaddr, &cpar->bdaddr))
 297                                         continue;
 298
 299                                 p_req->retcmdp_len = min(len, p_req->retcmdp_len);
 300                                 memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
 301                                 goto succes;
 302
 303                         case EVT_INQUIRY_RESULT:
 304                                 printf("bingo inquiry result event \n");
 305                                 break;
 306                                 //goto succes;
 307
 308 /*                      case EVT_INQUIRY_RESULT_WITH_RSSI:
 309                                 printf("bingo inquiry result RSSI event \n");
 310                                 p_res_ev = (void *) p_recbuf;
 311                                 p_actual = &(p_res_ev->ba_array);
 312                                 for(j=0;j<count;j++){
 313                                         if(compare_bda(p_actual,(p_req->p_retcmdp+(j*sizeof(bt_address)))) == 1){
 314                                                 sign=1;
 315                                         } //im looking for all array members  an compare with actual address
 316                                 }
 317                                 if(!sign){ // if in array address doesnt exist i will add it to array
 318                                         *(bt_address*)(p_req->p_retcmdp+(count*sizeof(bt_address)))= *p_actual;
 319                                         count++;
 320                                 }
 321                                 if(count==0){
 322                                         *(bt_address*)(p_req->p_retcmdp)= *p_actual;
 323                                         count++;
 324                                 }
 325
 326                                 //printf("type %2.2X \n",((hci_inquiry_result_RSSI_ev *)p_req->p_retcmdp)->RSSI_array);
 327                                 break;
 328                                 //goto succes;
 329 */
 330                         case EVT_INQUIRY_COMPLETE:
 331                                 printf("bingo inquiry complete event \n");
 332                                 goto succes;
 333
 334
 335                         default: if(p_hdr->evt != p_req->event)
 336                                         break;
 337                                 p_req->retcmdp_len = min(len, p_req->retcmdp_len);
 338                                 memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
 339                                 goto succes;
 340
 341                 }
 342
 343
 344         }
 345         errno = ETIMEDOUT;
 346         return -1;
 347
 348
 349 fail:
 350         setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
 351         hci_close_dev(dd);
 352         return -1;
 353 succes:
 354         setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
 355         return 0;
 356
 357 }
 358
 359
 360 /*****************************HCI support functions*****************************/
 361
 362
 363 void assemble_ocf_ogf(__u8 ocf,__u8 ogf,__u16 *p_ocf_ogf){
 364         __u16 var1;
 365         __u16 result;
 366
 367         var1=(ogf<<10);
 368         result=ocf;
 369         *p_ocf_ogf=(result|var1);
 370 }
 371
 372 void printba(bt_address *ba){
 373         printf("address: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X: \n",ba->byte[0],ba->byte[1],ba->byte[2],ba->byte[3],ba->byte[4],ba->byte[5]);
 374 }
 375
 376 int compare_bda(bt_address *p_first, bt_address *p_second){
 377         __u8 i,k=0;
 378         for(i=0;i<sizeof(bt_address);i++){
 379                 if(p_first->byte[i]==p_second->byte[i])
 380                         k++;
 381         }
 382         if(k==sizeof(bt_address)) return 1; //all bytes are similar
 383
 384         return 0; //addreses are different in one byte at least
 385 }
 386
 387 void fill_zero(bt_address *p_addr){
 388         __u8 i;
 389         for(i=0;i<sizeof(bt_address);i++){
 390                 p_addr->byte[i]=0x00;
 391         }
 392 }
 393 void swap_addrbytes(bt_address *p_addr){
 394         bt_address help,*p_help=&help;
 395         __u8 i;
 396         for(i=0;i<sizeof(bt_address);i++){
 397                 p_help->byte[i]=p_addr->byte[5-i];
 398         }
 399         *p_addr=*p_help;
 400 }
 401 __u16 swap_2_bytes(__u16 twobytes){
 402         __u16 first,second,result=0;
 403         first=twobytes&255;
 404         second=twobytes&65280;
 405         result=result|(first<<8);
 406         result=result|(second>>8);
 407         return result;
 408 }
 409
 410 __u8 swap8(__u8 byte1){
 411         __u8 i,mask=0,hvar1=0,resvar=0;
 412         for(i=0;i<8;i++){
 413                 mask=1<<i; // 1,2,4,8,16,32,64,128
 414                 hvar1=byte1 & mask;
 415                 if(hvar1>0)
 416                 resvar=resvar + (128>>i);
 417         }
 418         return resvar;
 419 }
 420
 421 __u16 swap16(__u16 byte2){
 422         __u8 i;
 423         __u16 mask=0,hvar1=0,resvar=0;
 424         for(i=0;i<16;i++){
 425         mask=1<<i;
 426         hvar1=byte2 & mask;
 427         if(hvar1>0)
 428         resvar=resvar + (32768>>i);
 429         }
 430         return resvar;
 431 }
 432
 433 void fill_add(bt_address *addr,__u8 first, __u8 sec, __u8 third, __u8 forth, __u8 fifth, __u8 sixth){
 434         addr->byte[0]=first;
 435         addr->byte[1]=sec;
 436         addr->byte[2]=third;
 437         addr->byte[3]=forth;
 438         addr->byte[4]=fifth;
 439         addr->byte[5]=sixth;
 440 }
 441