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