testf/t4/tiny_bt_hci_cmd.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <errno.h>
   5 #include <unistd.h>
   6 #include <stdint.h>
   7 #include <fcntl.h>
   8 #include <sys/poll.h>
   9 #include "tiny_bt_hci_cmd.h"
  10 #include "bt_hw.h"
  11
  12
  13
  14
  15
  16 /************************** hci command functions**************************/
  17 int send_hci_inquiry_cmd(void){
  18         inquiry_cp cmdp,*p_cmdp=&cmdp;
  19         hci_cmd_request creq,*p_creq=&creq;
  20         inq_time time=T14s40;
  21         responce_num num=NUM_RSP_20;
  22
  23         memset(p_cmdp,0,sizeof(cmdp));
  24         p_cmdp->lap[0]=0x33;
  25         p_cmdp->lap[1]=0x8b;
  26         p_cmdp->lap[2]=0x9e;
  27         p_cmdp->length=time;
  28         p_cmdp->num_rsp=num;
  29         memset(p_creq,0,sizeof(creq));
  30         p_creq->OCF_OGF=INQUIRY_CMD_OP;
  31         p_creq->p_cmdp=p_cmdp;
  32         p_creq->cmdp_len=INQUIRY_CP_SIZE;
  33
  34         if(send_cmd(p_creq)<0){
  35                 perror("hci_inquiry_cmd wasn't sent");
  36                 return -1;
  37         }
  38         return 0;
  39 }
  40 // fill up the input parameter address pointer by one address
  41 int send_hci_read_bd_addr_cmd(void){
  42         hci_cmd_request creq,*p_creq=&creq;
  43
  44         memset(p_creq,0,sizeof(creq));
  45         p_creq->OCF_OGF=READ_BD_ADDR_CMD_OP;
  46         p_creq->cmdp_len=0;
  47
  48         if(send_cmd(p_creq)<0){
  49                 perror("hci_read_bd_addr wasn't sent");
  50                 return -1;
  51         }
  52         return 0;
  53 }
  54
  55 int send_hci_read_local_name_cmd(void){
  56         hci_cmd_request creq,*p_creq=&creq;
  57
  58         memset(p_creq,0,sizeof(creq));
  59         p_creq->OCF_OGF=READ_LOCAL_NAME_CMD_OP;
  60         p_creq->cmdp_len=0;
  61
  62         if(send_cmd(p_creq)<0){
  63                 perror("hci_read_local_name_cmd wasn't sent");
  64                 return -1;
  65         }
  66         return 0;
  67 }
  68
  69 int send_hci_create_connection_cmd(bt_address *p_dest_addr){
  70         create_conn_cp cmdp,*p_cmdp=&cmdp;
  71         hci_cmd_request creq,*p_creq=&creq;
  72
  73         memset(p_cmdp,0,sizeof(cmdp));
  74         p_cmdp->bdaddr=*p_dest_addr;
  75         p_cmdp->pkt_type=0x0010;
  76         p_cmdp->pscan_rep_mode=0x01;
  77         p_cmdp->pscan_mode=0x00;
  78         p_cmdp->clock_offset=0xf000;
  79         p_cmdp->role_switch=0x00;
  80         memset(p_creq,0,sizeof(creq));
  81         p_creq->OCF_OGF=CREATE_CONNECTION_CMD_OP;
  82         p_creq->p_cmdp=p_cmdp;
  83         p_creq->cmdp_len=CREATE_CONN_CP_SIZE;
  84
  85         if(send_cmd(p_creq)<0){
  86                 perror("hci_create_connection_cmd wasn't sent");
  87                 return -1;
  88         }
  89         return 0;
  90 }
  91
  92 int send_hci_accept_conn_req_cmd(bt_address *p_address){
  93         accept_conn_req_cp cmdp, *p_cmdp=&cmdp;
  94         hci_cmd_request creq,*p_creq=&creq;
  95
  96         memset(p_cmdp,0,sizeof(cmdp));
  97         p_cmdp->bdaddr=*p_address;
  98         p_cmdp->role=0x01;
  99         memset(p_creq,0,sizeof(creq));
 100         p_creq->OCF_OGF=ACCEPT_CONNECTION_REQ_OP;
 101         p_creq->p_cmdp=p_cmdp;
 102         p_creq->cmdp_len=ACCEPT_CONN_REQ_CP_SIZE;
 103
 104         if(send_cmd(p_creq)<0){
 105                 perror("hci_accept_conn_req_cmd wasn't sent");
 106                 return -1;
 107         }
 108         return 0;
 109 }
 110
 111 /*********************************main functions****************************/
 112 int send_cmd(hci_cmd_request *p_creq){
 113         __u8 array[p_creq->cmdp_len+4];
 114         __u16 sw_opcode;
 115         int ii;
 116         sw_opcode=swap_2_bytes(p_creq->OCF_OGF);
 117         array[0]=0x01;
 118         memcpy(&array[1],&sw_opcode,2);
 119         array[3]= p_creq->cmdp_len;
 120         if(p_creq->cmdp_len > 0){
 121                 memcpy(&array[4],p_creq->p_cmdp,p_creq->cmdp_len); // !!!!!!!!! segmentation fault
 122         }
 123
 124         for(ii=0;ii<sizeof(array);ii++){
 125                 printf(" %x",array[ii]);
 126         }
 127         printf("\n");
 128
 129         if(hw_bt_write(array,sizeof(array))<0){
 130                 perror("hw_bt_write problem");
 131                 return -1;
 132         }
 133         return 0;
 134 }
 135
 136 /*****************************HCI support functions*****************************/
 137
 138
 139 void assemble_ocf_ogf(__u8 ocf,__u8 ogf,__u16 *p_ocf_ogf){
 140         __u16 var1;
 141         __u16 result;
 142
 143         var1=(ogf<<10);
 144         result=ocf;
 145         *p_ocf_ogf=(result|var1);
 146 }
 147
 148 void printba(bt_address *ba){
 149         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]);
 150 }
 151
 152 int compare_bda(bt_address *p_first, bt_address *p_second){
 153         __u8 i,k=0;
 154         for(i=0;i<sizeof(bt_address);i++){
 155                 if(p_first->byte[i]==p_second->byte[i])
 156                         k++;
 157         }
 158         if(k==sizeof(bt_address)) return 1; //all bytes are similar
 159
 160         return 0; //addreses are different in one byte at least
 161 }
 162
 163 void fill_zero(bt_address *p_addr){
 164         __u8 i;
 165         for(i=0;i<sizeof(bt_address);i++){
 166                 p_addr->byte[i]=0x00;
 167         }
 168 }
 169 void swap_addrbytes(bt_address *p_addr){
 170         bt_address help,*p_help=&help;
 171         __u8 i;
 172         for(i=0;i<sizeof(bt_address);i++){
 173                 p_help->byte[i]=p_addr->byte[5-i];
 174         }
 175         *p_addr=*p_help;
 176 }
 177 __u16 swap_2_bytes(__u16 twobytes){
 178         __u16 first,second,result=0;
 179         first=twobytes&255;
 180         second=twobytes&65280;
 181         result=result|(first<<8);
 182         result=result|(second>>8);
 183         return result;
 184 }
 185
 186 __u8 swap8(__u8 byte1){
 187         __u8 i,mask=0,hvar1=0,resvar=0;
 188         for(i=0;i<8;i++){
 189                 mask=1<<i; // 1,2,4,8,16,32,64,128
 190                 hvar1=byte1 & mask;
 191                 if(hvar1>0)
 192                 resvar=resvar + (128>>i);
 193         }
 194         return resvar;
 195 }
 196
 197 __u16 swap16(__u16 byte2){
 198         __u8 i;
 199         __u16 mask=0,hvar1=0,resvar=0;
 200         for(i=0;i<16;i++){
 201         mask=1<<i;
 202         hvar1=byte2 & mask;
 203         if(hvar1>0)
 204         resvar=resvar + (32768>>i);
 205         }
 206         return resvar;
 207 }
 208
 209 void fill_add(bt_address *addr,__u8 first, __u8 sec, __u8 third, __u8 forth, __u8 fifth, __u8 sixth){
 210         addr->byte[0]=first;
 211         addr->byte[1]=sec;
 212         addr->byte[2]=third;
 213         addr->byte[3]=forth;
 214         addr->byte[4]=fifth;
 215         addr->byte[5]=sixth;
 216 }
 217