src2/slavetest.c

   1 /*
   2 *  C Implementation: slavetest
   3 *
   4 * Description:
   5 *
   6 *
   7 * Author: root <root@ubuntu>, (C) 2008
   8 *
   9 * Copyright: See COPYING file that comes with this distribution
  10 *
  11 */
  12
  13 #include <stdio.h>
  14 #include <stdlib.h>
  15 #include <math.h>
  16 #include <string.h>
  17 #include <errno.h>
  18 #include <unistd.h>
  19 #include <sys/poll.h>
  20 #include <sys/ioctl.h>
  21 #include "hcidriver.h"
  22 #include "hil2cap.h"
  23 #include "slavetest.h"
  24
  25 incoming_evt evt_array[MAX_CMD_STATE_REQUEST];
  26 int evt_id;
  27 int global_index;
  28 struct hci_filter nnf, oof; // the oldest and newest filters
  29 connection_hci connection,*p_connection=&connection;
  30
  31
  32 int factorial(int num){
  33         if(num==1) return 1;
  34         return num*factorial(num-1);
  35 }
  36
  37 int add_evt_toarray(incoming_evt *p_evt,struct hci_filter *p_pf,int *p_fchanged){ //fcn add the request at the end of array or replace the oldest request
  38         int i,sig=0,index=0;
  39         incoming_evt the_oldest=evt_array[0];
  40
  41
  42         if(!hci_filter_test_event(p_evt->evt_code,&nnf)){
  43                 *p_pf=nnf;
  44                 *p_fchanged=1;
  45                 hci_filter_set_event(p_evt->evt_code,&nnf);
  46         }
  47         for(i=0;i<MAX_EVT_COUNT;i++){
  48                 if((evt_array[i].actual_status)==0 || evt_array[i].actual_status==3 ){
  49                         evt_array[i]=*p_evt;
  50                         sig=1;
  51                         global_index=i;
  52                         break;
  53                 }
  54                 if((evt_array[i].id) < the_oldest.id){ //the smallest id (oldest request) will stay here
  55                         the_oldest = evt_array[i];
  56                         index=i;
  57
  58                 }
  59         }
  60         if(!sig){
  61
  62                 evt_array[index]=*p_evt;
  63                 global_index=index;
  64         }
  65         return 0;
  66 }
  67
  68 int look_up_inarray(__u8 evt_code){
  69         int i,index=-1,oldestid=65535;
  70
  71         if(evt_array[global_index].evt_code == evt_code && evt_array[global_index].actual_status != DONE){
  72                 return global_index;
  73         }
  74         else{
  75                 for(i=0;i<MAX_CMD_STATE_REQUEST;i++){
  76                         if(evt_array[i].evt_code == evt_code && evt_array[i].actual_status != DONE){
  77                                 if(evt_array[i].id<oldestid){
  78                                         oldestid=evt_array[i].id;
  79                                         index=i;
  80                                 }
  81                         }
  82                 }
  83                 return index;
  84         }
  85 }
  86
  87 int rutine_con_req(void *p_recbuf, int dd, struct hci_filter *p_pf, int *p_fchanged){
  88         __u16 sw_opcode;
  89         incoming_evt new_evt,*p_new_evt=&new_evt;
  90         evt_conn_request *p_evt;
  91
  92         p_evt = (void *) p_recbuf;
  93
  94         p_connection->master.bdaddr=p_evt->bdaddr;
  95
  96         p_new_evt->id=evt_id++;
  97         p_new_evt->actual_status=ESTABLISHED;
  98         p_new_evt->evt_code = EVT_CONN_COMPLETE;
  99         p_new_evt->p_serv_rutine=&rutine_connection_complete;
 100         add_evt_toarray(p_new_evt,p_pf,p_fchanged);
 101
 102         sw_opcode=swap_2_bytes(ACCEPT_CONNECTION_REQ_OP);
 103         hci_filter_set_opcode(sw_opcode, &nnf);
 104         if(*p_fchanged){
 105                         if(setsockopt(dd, SOL_HCI, HCI_FILTER, &nnf, sizeof(nnf))<0){
 106                         printf("some problem with setsockopt: %d",dd);
 107                         return -1;
 108                         }
 109                         *p_fchanged=0;
 110                 }
 111
 112         if(call_hci_accept_conn_req_cmd(dd, &p_evt->bdaddr, &sw_opcode)<0){
 113                 printf("error with accepting connection\n");
 114                 return -1;
 115         }
 116         printf("accept connection command was sended\n");
 117         return 0;
 118 }
 119
 120 int rutine_cmd_status(void *p_recbuf, int dd, struct hci_filter *p_pf, int *p_fchanged){
 121         evt_cmd_status *p_evt;
 122         p_evt = (void *) p_recbuf;
 123
 124         if(p_evt->status){
 125                 printf("command status error\n");
 126                 return -1;
 127         }
 128
 129         printf("command status with opcode: %d",p_evt->opcode);
 130         return 0;
 131
 132 }
 133
 134 int rutine_connection_complete(void *p_recbuf, int dd, struct hci_filter *p_pf, int *p_fchanged){
 135         __u16 sw_opcode;
 136         incoming_evt new_evt,*p_new_evt=&new_evt;
 137         evt_conn_complete *p_evt;
 138         p_evt = (void *) p_recbuf;
 139
 140         if(p_evt->status){
 141                 printf("connection was not setted\n");
 142                 return -1;
 143         }
 144         p_connection->con_state=CONNECTED;
 145         p_connection->handle=p_evt->handle;
 146         printf("connection was setted: %d\n",p_connection->handle);
 147
 148         p_new_evt->id=evt_id++;
 149         p_new_evt->actual_status=ESTABLISHED;
 150         p_new_evt->evt_code = EVT_DISCONN_COMPLETE;
 151         p_new_evt->p_serv_rutine=&rutine_disconnect;
 152         add_evt_toarray(p_new_evt,p_pf,p_fchanged);
 153
 154         sw_opcode=swap_2_bytes(ACCEPT_CONNECTION_REQ_OP);
 155         *p_pf=nnf;
 156         *p_fchanged=1;
 157         hci_filter_clear_event(EVT_CONN_REQUEST,&nnf);
 158         hci_filter_clear_opcode(&nnf);
 159
 160
 161
 162         return 0;
 163 }
 164
 165 int rutine_disconnect(void *p_recbuf, int dd, struct hci_filter *p_pf, int *p_fchanged){
 166         evt_disconn_complete *p_evt;
 167         incoming_evt new_evt,*p_new_evt=&new_evt;
 168
 169         p_evt = (void *) p_recbuf;
 170         if(p_evt->status){
 171                 printf("disconnection error: %d\n",p_evt->status);
 172                 return -1;
 173         }
 174         *p_pf=nnf;
 175         *p_fchanged=1;
 176         hci_filter_clear_event(EVT_CONN_COMPLETE,&nnf);
 177
 178         p_connection->con_state=DISCONNECTED;
 179         printf("connection was canceled: %d\n",p_connection->handle);
 180         printf("disconnection reason: %d\n",p_evt->reason);
 181
 182         p_new_evt->id=evt_id++;
 183         p_new_evt->actual_status=ESTABLISHED;
 184         p_new_evt->evt_code = EVT_CONN_REQUEST;
 185         p_new_evt->p_serv_rutine=&rutine_con_req;
 186         add_evt_toarray(p_new_evt,p_pf,p_fchanged);
 187
 188         return 0;
 189 }
 190
 191 int check_socket(int dd,struct hci_filter *p_pf, int *p_fchanged){
 192         int len,j,index;
 193         hci_event_hdr *p_hdr;
 194         __u8 recbuf[HCI_MAX_EVENT_SIZE],*p_recbuf;
 195
 196         while((len=read(dd, recbuf, sizeof(recbuf)))<0){
 197                         if(errno == EINTR)
 198                                 continue;
 199                         if(errno == EAGAIN)
 200                                 return 0;
 201                         perror("Problem with cmd reading \n");
 202                         return -1;
 203                 }
 204
 205         if(len){
 206                 printf("Count of received bytes %d \n ",len);
 207                 for(j=0;j<len;j++){
 208                         printf("%2.2X ",recbuf[j]);
 209                 }
 210                 printf("\n");
 211                 if(recbuf[0]==4){ // konstanty
 212                         p_hdr = (void *) (recbuf + 1);
 213                         p_recbuf = (void *) (recbuf + (1 + HCI_EVENT_HDR_SIZE));
 214                         if((index=look_up_inarray(p_hdr->evt))<0){
 215                                 printf("There is no awaiting evt like this\n");
 216                                 return -1;
 217                         }
 218                         else{
 219                                 if(evt_array[index].p_serv_rutine(p_recbuf, dd, p_pf, p_fchanged)<0){
 220                                         printf("error with service rutine");
 221                                         return -1;
 222                                 }
 223                                 if(evt_array[index].actual_status != PERMANENT){
 224                                         evt_array[index].actual_status = DONE;
 225                                 }
 226                         }
 227                         return 1;
 228                 }
 229                 else if(recbuf[0]==2){
 230
 231                 }
 232                 else return 0; // 0 or -1
 233
 234         }
 235         else{
 236                 printf("There is nothing on socket %d \n",dd);
 237                 return 0;
 238         }
 239
 240 return 0;
 241 }
 242
 243
 244
 245
 246 int main(void){
 247
 248         incoming_evt new_evt,*p_new_evt=&new_evt;
 249         int fchanged=0;
 250         socklen_t len;
 251         struct hci_filter pf;
 252         memset(evt_array,0,(sizeof(incoming_evt)*MAX_EVT_COUNT));
 253         evt_id=0;
 254         global_index=0;
 255         p_connection->con_id=0;
 256         p_connection->con_state=DISCONNECTED;
 257
 258         int dd=hci_open_device_nonblock(0); // only for testig 1
 259
 260         len = sizeof(oof); //the oldest filter
 261         if(getsockopt(dd, SOL_HCI, HCI_FILTER, &oof, &len)<0){
 262                 printf("some problem with getsockopt: %d",dd);
 263                 return -1;
 264         }
 265         hci_filter_clear(&nnf);
 266         hci_filter_set_ptype(HCI_EVENT_PKT, &nnf);
 267         //hci_filter_set_opcode(*(p_req->p_OCF_OGF), &nf);
 268
 269         p_new_evt->id=evt_id++;
 270         p_new_evt->actual_status=PERMANENT;
 271         p_new_evt->evt_code = EVT_CMD_STATUS;
 272         p_new_evt->p_serv_rutine=&rutine_cmd_status;
 273         add_evt_toarray(p_new_evt,&pf,&fchanged);
 274
 275         p_new_evt->id=evt_id++;
 276         p_new_evt->actual_status=ESTABLISHED;
 277         p_new_evt->evt_code = EVT_CONN_REQUEST;
 278         p_new_evt->p_serv_rutine=&rutine_con_req;
 279         add_evt_toarray(p_new_evt,&pf,&fchanged);
 280
 281         while(1){
 282
 283                 if(fchanged){
 284                         if(setsockopt(dd, SOL_HCI, HCI_FILTER, &nnf, sizeof(nnf))<0){
 285                         printf("some problem with setsockopt: %d",dd);
 286                         return -1;
 287                         }
 288                         fchanged=0;
 289                 }
 290                 if(check_socket(dd, &pf, &fchanged)<0){
 291                         printf("problem with sock checking \n");
 292                         setsockopt(dd, SOL_HCI, HCI_FILTER, &pf, sizeof(pf));
 293                         continue;
 294                 }
 295
 296
 297         }
 298         setsockopt(dd, SOL_HCI, HCI_FILTER, &oof, sizeof(oof));
 299         hci_close_dev(dd);
 300
 301 return 0;
 302 }
 303
 304