+++ /dev/null
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-#include <string.h>
-#include <errno.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <fcntl.h>
-#include <sys/poll.h>
-#include <sys/ioctl.h>
-#include "hcidriver.h"
-
-
-
-
-
-/************************** hci command functions**************************/
-int call_hci_inquiry_cmd(int dd, void *p_addressarray,__u16 *p_ocf_ogf){
- inquiry_cp cmdp,*p_cmdp=&cmdp;
- hci_request req,*p_req=&req;
-
- p_cmdp->lap[0]=0x33;
- p_cmdp->lap[1]=0x8b;
- p_cmdp->lap[2]=0x9e;
- p_cmdp->length=INQ_TIME_14s40;
- p_cmdp->num_rsp=NUM_RSP_20;
- memset(p_req,0,sizeof(req));
- p_req->p_OCF_OGF=p_ocf_ogf;
- p_req->p_retcmdp=p_addressarray;
- p_req->p_cmdp=p_cmdp;
- p_req->retcmdp_len=0;
- p_req->cmdp_len=INQUIRY_CP_SIZE;
- p_req->event=EVT_INQUIRY_COMPLETE;
-
- if(hci_send_request(dd,p_req,INQ_TIME_4s24*1280)<0)
- return -1;
-
-
- return p_req->retcmdp_len;
-}
-// fill up the input parameter address pointer by one address
-int call_hci_read_bd_addr_cmd(int dd,bt_address *p_address,__u16 *p_ocf_ogf){
- read_bd_addr_rp cmdp,*p_cmdp=&cmdp; //command parameters return/comand
- hci_request req,*p_req=&req;
-
- memset(p_req,0,sizeof(req));
- p_req->p_OCF_OGF=p_ocf_ogf;
- p_req->p_retcmdp=p_cmdp;
- p_req->retcmdp_len=READ_BD_ADDR_RP_SIZE;
- p_req->cmdp_len=0;
- p_req->event=EVT_CMD_COMPLETE;
-
- if(hci_send_request(dd,p_req,2000)<0)
- return -1;
- if(p_cmdp->status)
- return -1;
- bacpy(p_address, &cmdp.bdaddr);
-
- return 0;
-}
-
-int call_hci_read_local_name_cmd(int dd, void *p_name,__u16 *p_ocf_ogf){
- read_local_name_rp cmdp,*p_cmdp=&cmdp;
- hci_request req,*p_req=&req;
-
- memset(p_req,0,sizeof(req));
- p_req->p_OCF_OGF=p_ocf_ogf;
- p_req->p_retcmdp=p_cmdp;
- p_req->retcmdp_len=READ_LOCAL_NAME_RP_SIZE;
- p_req->cmdp_len=0;
- p_req->event=EVT_CMD_COMPLETE;
-
- if(hci_send_request(dd,p_req,1000)<0)
- return -1;
- if(p_cmdp->status)
- return -1;
- memcpy(p_name, &p_cmdp->name, 8); //copy name
- return 0;
-
-}
-
-int call_hci_create_connection_cmd(int dd, bt_address *p_address, __u16 *p_ocf_ogf){
- create_conn_cp cmdp,*p_cmdp=&cmdp;
- hci_request req,*p_req=&req;
- evt_conn_complete cc_evp,*p_cc_evp=&cc_evp;
-
- p_cmdp->bdaddr=*p_address;
- p_cmdp->pkt_type=0x0010;
- p_cmdp->pscan_rep_mode=0x01;
- p_cmdp->pscan_mode=0x00;
- p_cmdp->clock_offset=0xf000;
- p_cmdp->role_switch=0x00;
- memset(p_req,0,sizeof(req));
- p_req->p_OCF_OGF=p_ocf_ogf;
- p_req->p_cmdp=p_cmdp;
- p_req->cmdp_len=CREATE_CONN_CP_SIZE;
- p_req->p_retcmdp=p_cc_evp;
- p_req->retcmdp_len=EVT_CONN_COMPLETE_SIZE;
- p_req->event=EVT_CONN_COMPLETE;
-
- if(hci_send_request(dd,p_req,2000)<0)
- return -1;
-
- return p_cc_evp->handle;
-
-}
-
-int call_hci_accept_conn_req_cmd(int dd, bt_address *p_address, __u16 *p_ocf_ogf){
- accept_conn_req_cp cmdp, *p_cmdp=&cmdp;
- hci_request req,*p_req=&req;
-
- p_cmdp->bdaddr=*p_address;
- p_cmdp->role=0x01;
- memset(p_req,0,sizeof(req));
- p_req->p_OCF_OGF=p_ocf_ogf;
- p_req->p_cmdp=p_cmdp;
- p_req->cmdp_len=ACCEPT_CONN_REQ_CP_SIZE;
-
- if(hci_send_command(dd,p_req)<0){
- printf("problem with command sending\n");
- return -1;
- }
- return 0;
-}
-
-
-/*******************************HCI main functions ********************************/
-
-//create and bind the socket to one device, return -1 when error, device descriptor when OK
-int hci_open_device(int dev_id){
- struct 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.hci_dev=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_open_device_nonblock(int dev_id){
- struct sockaddr_hci address;
- int dd;
- int oldflag;
-
- if((dd=socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI))<0){
- perror("socket no created");
- return dd; //it return -1 when socket isn't created
- }
-
- oldflag=fcntl(dd, F_GETFL, 0);
- if(fcntl(dd, F_SETFL, oldflag | O_NONBLOCK) < 0){
- printf("problem with socket flag setting");
- return -1;
- }
-
- memset(&address,0,sizeof(address));
- address.hci_family=AF_BLUETOOTH;
- address.hci_dev=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];
- 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){
- perror("write interrupted");
- 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;
-
- 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(EVT_CMD_STATUS, &nf);
- hci_filter_set_event(EVT_INQUIRY_RESULT_WITH_RSSI, &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= 30;
-
- while(try_count--){
- int sign=0;
- printf("try %d \n",try_count);
- evt_cmd_complete *cc;
- evt_cmd_status *cs;
- evt_remote_name_req_complete *rnrc;
- remote_name_req_cp *cpar;
- inquiry_info_with_rssi *p_res_ev;
- bt_address *p_actual;
-
- 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 reading");
- 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 EVT_CMD_STATUS:
- cs = (void *) p_recbuf;
- if(cs->opcode != *(p_req->p_OCF_OGF))
- continue;
- if(p_req->event != EVT_CMD_STATUS){
- 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 EVT_CMD_COMPLETE:
- cc = (void *) p_recbuf;
- if(cc->opcode != *(p_req->p_OCF_OGF))
- continue;
- p_recbuf += EVT_CMD_COMPLETE_SIZE;
- len -= EVT_CMD_COMPLETE_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;
-
- case EVT_REMOTE_NAME_REQ_COMPLETE:
- if(p_hdr->evt != p_req->event)
- break;
- rnrc = (void *) p_recbuf;
- cpar = p_req->p_cmdp;
-
- if(bacmp(&rnrc->bdaddr, &cpar->bdaddr))
- 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 EVT_INQUIRY_RESULT_WITH_RSSI:
- printf("bingo inquiry result RSSI event \n");
- p_recbuf = recbuf + (1 + HCI_EVENT_HDR_SIZE + 1);// one byte set forward
- p_res_ev = (void *) p_recbuf;
- p_actual = &(p_res_ev->bdaddr);
- //todo:involve signal strength
- if(count==0){ //array is empty
- *(bt_address*)(p_req->p_retcmdp)= *p_actual;
- count++;
- }
- else{
- for(j=0;j<count;j++){
- if(compare_bda(p_actual,(bt_address *)(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++;
- }
- }
-
- //printf("type %2.2X \n",((hci_inquiry_result_RSSI_ev *)p_req->p_retcmdp)->RSSI_array);
- break;
- //goto succes;
-
- case EVT_INQUIRY_COMPLETE:
- if(p_hdr->evt != p_req->event)
- break;
- if(*p_recbuf != 0)
- goto fail;
- printf("bingo inquiry complete event \n");
- p_req->retcmdp_len=count; //count of diferent remote decices
-
- goto succes;
-
- case EVT_CONN_COMPLETE:
- if(p_hdr->evt != p_req->event)
- break;
- if(*p_recbuf != 0)
- goto fail;
- printf("bingo two devices were connected");
- p_req->retcmdp_len = min(len, p_req->retcmdp_len);
- memcpy(p_req->p_retcmdp, p_recbuf, p_req->retcmdp_len);
- 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;
-
-}
-
-
-/*****************************HCI support functions*****************************/
-
-
-void assemble_ocf_ogf(__u8 ocf,__u8 ogf,__u16 *p_ocf_ogf){
- __u16 var1;
- __u16 result;
-
- 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;
-}
-__u16 swap_2_bytes(__u16 twobytes){
- __u16 first,second,result=0;
- first=twobytes&255;
- second=twobytes&65280;
- result=result|(first<<8);
- result=result|(second>>8);
- return result;
-}
-
-__u8 swap8(__u8 byte1){
- __u8 i,mask=0,hvar1=0,resvar=0;
- for(i=0;i<8;i++){
- mask=1<<i; // 1,2,4,8,16,32,64,128
- hvar1=byte1 & mask;
- if(hvar1>0)
- resvar=resvar + (128>>i);
- }
- return resvar;
-}
-
-__u16 swap16(__u16 byte2){
- __u8 i;
- __u16 mask=0,hvar1=0,resvar=0;
- for(i=0;i<16;i++){
- mask=1<<i;
- hvar1=byte2 & mask;
- if(hvar1>0)
- resvar=resvar + (32768>>i);
- }
- return resvar;
-}
-
-void fill_add(bt_address *addr,__u8 first, __u8 sec, __u8 third, __u8 forth, __u8 fifth, __u8 sixth){
- addr->byte[0]=first;
- addr->byte[1]=sec;
- addr->byte[2]=third;
- addr->byte[3]=forth;
- addr->byte[4]=fifth;
- addr->byte[5]=sixth;
-}
-
projects / tiny-bt.git / blobdiff