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