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