+++ /dev/null
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-#include <string.h>
-#include <errno.h>
-#include <unistd.h>
-#include <sys/poll.h>
-#include <sys/ioctl.h>
-#include "hcidriver.h"
-
-int call_hci_read_bd_addr_cmd(int dd,bt_address *p_address, int timeout){
- read_bd_addr_cmd cmdp,*p_cmdp; //command parameters return/comand
- hci_request req,*p_req;
- __u16 OCF_OGF,*p_OCF_OGF;
- p_OCF_OGF=&OCF_OGF;
- p_cmdp=&cmdp;
- p_req=&req;
-
-
- memset(p_req,0,sizeof(req));
- assemble_ocf_ogf(0x09,0x04,p_OCF_OGF);
- p_req->p_OCF_OGF=p_OCF_OGF;
- p_req->p_retcmdp=p_cmdp;
- p_req->retcmdp_len=READ_BD_ADDR_CMD_PARL;
- p_req->cmdp_len=0;
- p_req->event=CMD_COMPLETE_EV;
-
- if(hci_send_request(dd,p_req,timeout)<0)
- return -1;
- if(p_cmdp->status)
- return -1;
-
- bacpy(p_address, &cmdp.address);
- return 0;
-}
-
-int call_hci_inquiry_cmd(int dd, bt_address *p_addressarray,int timeout){
- hci_inquiry_cmd cmdp,*p_cmdp=&cmdp;
- hci_request req,*p_req=&req;
-
- hci_inquiry_complete_ev com_ev,*p_com_ev=&com_ev;
- __u16 OCF_OGF,*p_OCF_OGF=&OCF_OGF;
-
- p_cmdp->lap[0]=0x33;
- p_cmdp->lap[1]=0x8b;
- p_cmdp->lap[2]=0x9e;
- p_cmdp->inquiry_length=5;
- p_cmdp->num_responces=5;
- memset(p_req,0,sizeof(req));
- assemble_ocf_ogf(0x01,0x01,p_OCF_OGF);
- p_req->p_OCF_OGF=p_OCF_OGF;
- p_req->p_retcmdp=p_addressarray;
- p_req->p_cmdp=p_cmdp;
- p_req->retcmdp_len=INQUIRY_RESULT_EV_PARL;
- p_req->cmdp_len=HCI_INQUIRY_CMD_PARL;
- p_req->event=INQUIRY_RESULT_EV;
-
- if(hci_send_request(dd,p_req,timeout)<0)
- return -1;
- if(p_com_ev->status)
- return -1;
-
-
- return 0;
-}
-
-int call_hci_create_connection_cmd(int dd, bt_address *p_address, int timeout){
- hci_create_connection_cmd cmdp,*p_cmdp=&cmdp;
- hci_request req,*p_req=&req;
- __u16 OCF_OGF,*p_OCF_OGF=&OCF_OGF;
-
- p_cmdp->address=*p_address;
- p_cmdp->packet_type=0x0010;
- p_cmdp->rep_mode=0x01;
- p_cmdp->reserved=0x00;
- p_cmdp->clock_offset=0xf000;
- p_cmdp->role_switch=0x00;
- memset(p_req,0,sizeof(req));
- assemble_ocf_ogf(0x05,0x01,p_OCF_OGF);
- p_req->p_OCF_OGF=p_OCF_OGF;
- p_req->p_cmdp=p_cmdp;
- p_req->cmdp_len=HCI_CREATE_CONNECTION_CMD_PARL;
- p_req->event=CONNECTION_COMPLETE_EV;
-
- if(hci_send_request(dd,p_req,timeout)<0)
- return -1;
- //if(p_com_ev->status)
- // return -1;
-
-
- return 0;
-}
-
-
-int hci_open_device(int dev_id){
- sockaddr_hci address;
- int dd;
-
- if((dd=socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI))<0){
- perror("socket no created");
- return dd; //it return -1 when socket isn't created
- }
-
- memset(&address,0,sizeof(address));
- address.hci_family=AF_BLUETOOTH;
- address.dev_id=dev_id;
- if(bind(dd,(struct sockaddr *) &address, sizeof(address))<0){
- perror("Socket not binded to hci device");
- close(dd);
- return -1;
- }
-
- return dd;
-}
-
-int hci_close_dev(int dd){
- return close(dd);
-}
-
-int hci_send_command(int dd, hci_request *p_req){
- __u8 array[p_req->cmdp_len+4]; //type + OCF+OGF+plen
- int ii;
- array[0]=0x01;
- memcpy(&array[1],p_req->p_OCF_OGF,2);
- array[3]= p_req->cmdp_len;
- if(p_req->cmdp_len > 0){
- memcpy(&array[4],p_req->p_cmdp,p_req->cmdp_len); // !!!!!!!!! segmentation fault
- }
-
- for(ii=0;ii<sizeof(array);ii++){
- printf(" %x",array[ii]);
- }
- printf("\n");
-
- while(write(dd, &array, (p_req->cmdp_len+4))<0){
- printf("write was interupted: %d",dd);
- if(errno == EAGAIN || errno == EINTR)
- continue;
- return -1;
- }
- return 0;
-}
-
-int hci_send_request(int dd, hci_request *p_req,int timeout){
- __u8 recbuf[HCI_MAX_EVENT_SIZE],*p_recbuf;
- int j,count=0;
- socklen_t len;
- hci_event_hdr *p_hdr;
- struct hci_filter nf, of;
- int try_count, sign=0;;
- bt_address *p_actual;
-
-
-
- len = sizeof(of);
- if(getsockopt(dd, SOL_HCI, HCI_FILTER, &of, &len)<0){
- printf("some problem with getsockopt: %d",dd);
- return -1;
- }
-
- hci_filter_clear(&nf);
- hci_filter_set_ptype(HCI_EVENT_PKT, &nf);
- hci_filter_set_event(CMD_STATUS_EV, &nf);
- hci_filter_set_event(INQUIRY_COMPLETE_EV, &nf);
- hci_filter_set_event(INQUIRY_RESULT_RSSI_EV, &nf);
- hci_filter_set_event(p_req->event, &nf);
- hci_filter_set_opcode(*(p_req->p_OCF_OGF), &nf);
-
- if(setsockopt(dd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf))<0){
- printf("some problem with setsockopt: %d",dd);
- return -1;
- }
-
- if(hci_send_command(dd, p_req)<0){
- printf("some problems with sending command: %d",dd);
- goto fail;
- }
-
- try_count= 10;
-
- while(try_count--){
- printf("try %d \n",try_count);
- hci_cmd_complete_ev *cc;
- hci_cmd_status_ev *cs;
- hci_remote_name_request_complete_ev *rnrc;
- hci_remote_name_req_cmd *cpar;
- hci_inquiry_result_RSSI_ev *p_res_ev;
-
-
-
- if(timeout){
- struct pollfd p;
- int n;
-
- p.fd = dd; p.events = POLLIN;
- while((n = poll(&p, 1, timeout)) < 0){ //pool return 1 when there are some incoming data
- if(errno == EAGAIN || errno == EINTR)
- continue;
- perror("Problem with poll");
- goto fail;
- }
-
- if (!n) { //time out pool return 0
- errno = ETIMEDOUT;
- perror("Poll return 0 timeout");
- printf("exited pool timeout \n ");
- goto fail;
- }
-
- timeout -= 10;
- if(timeout < 0) timeout = 0;
-
- }
- printf("Timeout after round is: %d \n",timeout);
-
- while((len=read(dd, recbuf, sizeof(recbuf)))<0){
- if(errno == EAGAIN || errno == EINTR)
- continue;
- perror("Problem with cmd sending");
- goto fail;
- }
- printf("Count of received bytes %d \n ",len);
- for(j=0;j<len;j++){
- printf("%2.2X ",recbuf[j]);
- }
- printf("\n");
- p_hdr = (void *) (recbuf + 1);
- p_recbuf = recbuf + (1 + HCI_EVENT_HDR_SIZE);
- len -= (1 + HCI_EVENT_HDR_SIZE);
-
- switch(p_hdr->evt){
- case CMD_STATUS_EV:
- cs = (void *) p_recbuf;
- if(cs->cmd_opcode != *(p_req->p_OCF_OGF))
- continue;
- if(p_req->event != CMD_STATUS_EV){
- if(cs->status){ // if there is something except 0 it is error
- errno = EIO;
- perror("Some error state has occured on receive");
- goto fail;
- }
- break;
- }
- p_req->retcmdp_len = min(len, p_req->retcmdp_len);
- memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
- goto succes;
-
- case CMD_COMPLETE_EV:
- cc = (void *) p_recbuf;
- if(cc->cmd_opcode != *(p_req->p_OCF_OGF))
- continue;
- p_recbuf += CMD_COMPLETE_EV_SIZE;
- len -= CMD_COMPLETE_EV_SIZE;
-
- p_req->retcmdp_len = min(len, p_req->retcmdp_len);
- memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
- printf("retcmdp lenght: %d \n",p_req->retcmdp_len);
- goto succes;
- // __u8 first,*p_first;
- // p_first=&first;
- // for(j=0;j<p_req->retcmdp_len;j++){
- // memcpy(p_first,p_req->p_retcmdp+j,1);
- // printf("%d. byte is: %X \n",j,first);
- // printf(" %X",*((unsigned int *)p_req->p_retcmdp+j));
- //printf("address of p_req: %d \n",p_req);
- //printf("address of p_req: %d \n",p_req->p_retcmdp);
- // }
- // printf("Ok\n");
-
-
- case REMOTE_NAME_REQUEST_EV:
- if(p_hdr->evt != p_req->event)
- break;
- rnrc = (void *) p_recbuf;
- cpar = p_req->p_cmdp;
-
- if(bacmp(&rnrc->address, &cpar->address))
- continue;
-
- p_req->retcmdp_len = min(len, p_req->retcmdp_len);
- memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
- goto succes;
-
- case INQUIRY_RESULT_EV:
- printf("bingo inquiry result event \n");
- break;
- //goto succes;
-
- case INQUIRY_RESULT_RSSI_EV:
- printf("bingo inquiry result RSSI event \n");
- p_res_ev = (void *) p_recbuf;
- p_actual = &(p_res_ev->ba_array);
- for(j=0;j<count;j++){
- if(compare_bda(p_actual,(p_req->p_retcmdp+(j*sizeof(bt_address)))) == 1){
- sign=1;
- } //im looking for all array members an compare with actual address
- }
- if(!sign){ // if in array address doesnt exist i will add it to array
- *(bt_address*)(p_req->p_retcmdp+(count*sizeof(bt_address)))= *p_actual;
- count++;
- }
- if(count==0){
- *(bt_address*)(p_req->p_retcmdp)= *p_actual;
- count++;
- }
-
- //printf("type %2.2X \n",((hci_inquiry_result_RSSI_ev *)p_req->p_retcmdp)->RSSI_array);
- break;
- //goto succes;
-
- case INQUIRY_COMPLETE_EV:
- printf("bingo inquiry complete event \n");
- goto succes;
-
-
- default: if(p_hdr->evt != p_req->event)
- break;
- p_req->retcmdp_len = min(len, p_req->retcmdp_len);
- memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
- goto succes;
-
- }
-
-
- }
- errno = ETIMEDOUT;
- return -1;
-
-
-fail:
- setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
- hci_close_dev(dd);
- return -1;
-succes:
- setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
- return 0;
-
-}
-
-void assemble_ocf_ogf(__u8 ocf,__u8 ogf,__u16 *p_ocf_ogf){
- __u16 var1;
- __u16 result;
- //result=ocf;
- //result=(result<<8);
- //var1=(ogf<<2);
- //*p_ocf_ogf=(result|var1);
- var1=(ogf<<10);
- result=ocf;
- *p_ocf_ogf=(result|var1);
-}
-
-void printba(bt_address *ba){
- 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]);
-}
-
-int compare_bda(bt_address *p_first, bt_address *p_second){
- __u8 i,k=0;
- for(i=0;i<sizeof(bt_address);i++){
- if(p_first->byte[i]==p_second->byte[i])
- k++;
- }
- if(k==sizeof(bt_address)) return 1; //all bytes are similar
-
- return 0; //addreses are different in one byte at least
-}
-
-void fill_zero(bt_address *p_addr){
- __u8 i;
- for(i=0;i<sizeof(bt_address);i++){
- p_addr->byte[i]=0x00;
- }
-}
-void swap_addrbytes(bt_address *p_addr){
- bt_address help,*p_help=&help;
- __u8 i;
- for(i=0;i<sizeof(bt_address);i++){
- p_help->byte[i]=p_addr->byte[5-i];
- }
- *p_addr=*p_help;
-}
-
-
-
-int main(void){
- bt_address addressarray[HCI_MAX_DEV];
- bt_address address,*p_address;
- p_address=&address;
- int dd,i;
-
- memset(addressarray,0,sizeof(bt_address)*HCI_MAX_DEV);
-
- if((dd=hci_open_device(0))<0){
- printf("some problem with socket creating or binding: %d \n",dd);
- return -1;
- }
- if(call_hci_read_bd_addr_cmd(dd,p_address,1000)<0){
- printf("some problem with call_hci_read_bd_addr: %d \n",dd);
- return -1;
-
- }
- printba(p_address);
-
- if(call_hci_inquiry_cmd(dd,addressarray,3000)<0){
- printf("some problem with call_hci_inquiry_cmd: %d \n",dd);
- return -1;
- }
- for(i=0;i<HCI_MAX_DEV;i++){
- //swap_addrbytes(&addressarray[i]);
- printba(&addressarray[i]);
- }
- if(call_hci_create_connection_cmd(dd,&addressarray[0],3000)<0){
- printf("some problem with call_create_connection %d \n",dd);
- return -1;
- }
-
- return 0;
-}
-
-
projects / tiny-bt.git / blobdiff