src2/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 /************************** hci command functions**************************/
  13 int send_hci_inquiry_cmd(void)
  14 {
  15         inquiry_cp cmdp, *p_cmdp = &cmdp;
  16         hci_cmd_request creq, *p_creq = &creq;
  17         inq_time time = T14s40;
  18         responce_num num = NUM_RSP_20;
  19
  20         memset(p_cmdp, 0, sizeof(cmdp));
  21         p_cmdp->lap[0] = 0x33;
  22         p_cmdp->lap[1] = 0x8b;
  23         p_cmdp->lap[2] = 0x9e;
  24         p_cmdp->length = time;
  25         p_cmdp->num_rsp = num;
  26         memset(p_creq, 0, sizeof(creq));
  27         p_creq->OCF_OGF = INQUIRY_CMD_OP;
  28         p_creq->p_cmdp = p_cmdp;
  29         p_creq->cmdp_len = INQUIRY_CP_SIZE;
  30
  31         if (send_cmd(p_creq) < 0) {
  32                 perror("hci_inquiry_cmd wasn't sent\n");
  33                 return -1;
  34         }
  35         return 0;
  36 }
  37 /* fill up the input parameter address pointer by one address*/
  38 int send_hci_read_bd_addr_cmd(void)
  39 {
  40         hci_cmd_request creq, *p_creq = &creq;
  41
  42         memset(p_creq, 0, sizeof(creq));
  43         p_creq->OCF_OGF = READ_BD_ADDR_CMD_OP;
  44         p_creq->cmdp_len = 0;
  45
  46         if (send_cmd(p_creq) < 0) {
  47                 perror("hci_read_bd_addr wasn't sent\n");
  48                 return -1;
  49         }
  50         return 0;
  51 }
  52
  53 int send_hci_read_local_name_cmd(void)
  54 {
  55         hci_cmd_request creq, *p_creq = &creq;
  56
  57         memset(p_creq, 0, sizeof(creq));
  58         p_creq->OCF_OGF = READ_LOCAL_NAME_CMD_OP;
  59         p_creq->cmdp_len = 0;
  60
  61         if (send_cmd(p_creq) < 0) {
  62                 perror("hci_read_local_name_cmd wasn't sent\n");
  63                 return -1;
  64         }
  65         return 0;
  66 }
  67
  68 int send_hci_create_connection_cmd(bt_address *p_dest_addr)
  69 {
  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
  81         memset(p_creq, 0, sizeof(creq));
  82         p_creq->OCF_OGF = CREATE_CONNECTION_CMD_OP;
  83         p_creq->p_cmdp = p_cmdp;
  84         p_creq->cmdp_len = CREATE_CONN_CP_SIZE;
  85
  86         if (send_cmd(p_creq) < 0) {
  87                 perror("hci_create_connection_cmd wasn't sent\n");
  88                 return -1;
  89         }
  90         return 0;
  91 }
  92
  93 int send_hci_accept_conn_req_cmd(bt_address *p_address)
  94 {
  95         accept_conn_req_cp cmdp, *p_cmdp = &cmdp;
  96         hci_cmd_request creq, *p_creq = &creq;
  97
  98         memset(p_cmdp, 0, sizeof(cmdp));
  99         p_cmdp->bdaddr = *p_address;
 100         p_cmdp->role = 0x01;
 101         memset(p_creq, 0, sizeof(creq));
 102         p_creq->OCF_OGF = ACCEPT_CONNECTION_REQ_OP;
 103         p_creq->p_cmdp = p_cmdp;
 104         p_creq->cmdp_len = ACCEPT_CONN_REQ_CP_SIZE;
 105
 106         if (send_cmd(p_creq) < 0) {
 107                 perror("hci_accept_conn_req_cmd wasn't sent\n");
 108                 return -1;
 109         }
 110         return 0;
 111 }
 112
 113 int send_hci_disconnect_cmd(__u16 handle)
 114 {
 115         disconnect_cp cmdp, *p_cmdp = &cmdp;
 116         hci_cmd_request creq, *p_creq = &creq;
 117
 118         memset(p_cmdp, 0, sizeof(cmdp));
 119         p_cmdp->handle = handle;
 120         p_cmdp->reason = 0x13;
 121
 122         memset(p_creq, 0, sizeof(creq));
 123         p_creq->OCF_OGF = DISCONNECT_CMD_OP;
 124         p_creq->p_cmdp = p_cmdp;
 125         p_creq->cmdp_len = DISCONNECT_CP_SIZE;
 126
 127         if (send_cmd(p_creq) < 0) {
 128                 perror("hci_disconnect_cmd wasn't sent\n");
 129                 return -1;
 130         }
 131         return 0;
 132 }
 133
 134 int send_hci_data(char *p_data, __u16 lenght, __u16 *p_chandle)
 135 {
 136         hci_data_request dreq, *p_dreq = &dreq;
 137
 138         p_dreq->p_data = p_data;
 139         p_dreq->lenght = lenght;
 140         p_dreq->p_chandle = p_chandle;
 141
 142         if (send_data(p_dreq) < 0) {
 143                 perror("hci_accept_conn_req_cmd wasn't sent\n");
 144                 return -1;
 145         }
 146         return 0;
 147 }
 148
 149 /*********************************main functions****************************/
 150 int send_cmd(hci_cmd_request *p_creq)
 151 {
 152         __u8 array[p_creq->cmdp_len + 4];
 153         __u16 sw_opcode;
 154         int ii;
 155
 156         sw_opcode = swap_2_bytes(p_creq->OCF_OGF);
 157         array[0]= HCI_COMMAND_PKT;
 158         memcpy(&array[1], &sw_opcode,2);
 159         array[3] = p_creq->cmdp_len;
 160         if (p_creq->cmdp_len > 0) {
 161                 memcpy(&array[4], p_creq->p_cmdp, p_creq->cmdp_len); // !!!!!!!!! segmentation fault
 162         }
 163
 164         for (ii = 0; ii < sizeof(array); ii++) {
 165                 printf(" %x",array[ii]);
 166         }
 167         printf("\n");
 168
 169         if (hw_bt_write(array, sizeof(array)) < 0) {
 170                 perror("hw_bt_write problem\n");
 171                 return -1;
 172         }
 173         return 0;
 174 }
 175
 176 int send_data(hci_data_request *p_dreq)
 177 {
 178         int ii;
 179         __u8 array[p_dreq->lenght + 4];
 180
 181         array[0] = HCI_ACLDATA_PKT;
 182         memcpy(&array[1], p_dreq->p_chandle, 2);
 183         memcpy(&array[3], &p_dreq->lenght, 2);
 184         memcpy(&array[5], p_dreq->p_data, p_dreq->lenght);
 185
 186         for (ii = 0; ii < sizeof(array); ii++) {
 187                 printf(" %x",array[ii]);
 188         }
 189         printf("\n");
 190
 191         if (hw_bt_write(array,sizeof(array))<0) {
 192                 perror("hw_bt_write problem\n");
 193                 return -1;
 194         }
 195         return 0;
 196 }
 197
 198 /*****************************HCI support functions*****************************/
 199
 200 void assemble_ocf_ogf(__u8 ocf,__u8 ogf,__u16 *p_ocf_ogf)
 201 {
 202         __u16 var1;
 203         __u16 result;
 204
 205         var1 = (ogf << 10);
 206         result = ocf;
 207         *p_ocf_ogf = (result | var1);
 208 }
 209
 210 void printba(bt_address *ba)
 211 {
 212         printf("address: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X: \n", ba->byte[0],
 213                         ba->byte[1], ba->byte[2], ba->byte[3], ba->byte[4], ba->byte[5]);
 214 }
 215
 216 int compare_bda(bt_address *p_first, bt_address *p_second)
 217 {
 218         __u8 i, k = 0;
 219
 220         for (i = 0; i < sizeof(bt_address); i++) {
 221                 if (p_first->byte[i] == p_second->byte[i])
 222                         k++;
 223         }
 224
 225         if (k == sizeof(bt_address))
 226                 return 1; //all bytes are similar
 227
 228         return 0; //addreses are different in one byte at least
 229 }
 230
 231 void fill_zero(bt_address *p_addr)
 232 {
 233         __u8 i;
 234
 235         for (i = 0; i < sizeof(bt_address); i++) {
 236                 p_addr->byte[i] = 0x00;
 237         }
 238 }
 239
 240 void swap_addrbytes(bt_address *p_addr)
 241 {
 242         bt_address help, *p_help = &help;
 243         __u8 i;
 244
 245         for (i = 0; i < sizeof(bt_address); i++) {
 246                 p_help->byte[i] = p_addr->byte[5-i];
 247         }
 248         *p_addr = *p_help;
 249 }
 250
 251 __u16 swap_2_bytes(__u16 twobytes)
 252 {
 253         __u16 first, second, result=0;
 254
 255         first = twobytes & 255;
 256         second = twobytes & 65280;
 257         result = result | (first << 8);
 258         result = result | (second >> 8);
 259         return result;
 260 }
 261
 262 __u8 swap8(__u8 byte1)
 263 {
 264         __u8 i, mask = 0, hvar1 = 0, resvar = 0;
 265
 266         for (i = 0; i < 8; i++) {
 267                 mask = 1 << i; // 1,2,4,8,16,32,64,128
 268                 hvar1 = byte1 & mask;
 269                 if (hvar1 > 0)
 270                         resvar = resvar + (128 >> i);
 271         }
 272         return resvar;
 273 }
 274
 275 __u16 swap16(__u16 byte2)
 276 {
 277         __u8 i;
 278         __u16 mask = 0, hvar1 = 0, resvar = 0;
 279
 280         for (i = 0; i < 16; i++) {
 281                 mask = 1 << i;
 282                 hvar1 = byte2 & mask;
 283                 if (hvar1 > 0)
 284                         resvar = resvar + (32768 >> i);
 285         }
 286         return resvar;
 287 }
 288
 289 void fill_add(bt_address *addr,__u8 first, __u8 sec,
 290                          __u8 third, __u8 forth, __u8 fifth, __u8 sixth)
 291 {
 292         addr->byte[0] = first;
 293         addr->byte[1] = sec;
 294         addr->byte[2] = third;
 295         addr->byte[3] = forth;
 296         addr->byte[4] = fifth;
 297         addr->byte[5] = sixth;
 298 }
 299