git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/sdk_demo/Sources/UI/gamepad.c

   1 /**
   2  *  \brief    gamepad handling implementation
   3  *  \author   Sylvain Gaeremynck <sylvain.gaeremynck@parrot.fr>
   4  *  \version  1.0
   5  *  \date     04/06/2007
   6  *  \warning  Subject to completion
   7  */
   8
   9 #include <errno.h>
  10 #include <fcntl.h>
  11 #include <string.h>
  12 #include <stddef.h>
  13 #include <stdio.h>
  14 #include <unistd.h>
  15
  16 #include <linux/joystick.h>
  17
  18 #include <ardrone_api.h>
  19 #include <VP_Os/vp_os_print.h>
  20 #include "gamepad.h"
  21
  22 typedef struct {
  23   int32_t bus;
  24   int32_t vendor;
  25   int32_t product;
  26   int32_t version;
  27   char    name[MAX_NAME_LENGTH];
  28   char    handlers[MAX_NAME_LENGTH];
  29 } device_t;
  30
  31 extern int32_t MiscVar[];
  32
  33 static C_RESULT add_device(device_t* device, const int32_t id);
  34
  35 static C_RESULT parse_proc_input_devices(FILE* f, const int32_t id);
  36
  37 input_device_t gamepad = {
  38   "Gamepad",
  39   open_gamepad,
  40   update_gamepad,
  41   close_gamepad
  42 };
  43
  44 static int32_t joy_dev = 0;
  45
  46 input_device_t radioGP = {
  47   "GreatPlanes",
  48   open_radioGP,
  49   update_radioGP,
  50   close_radioGP
  51 };
  52
  53
  54 input_device_t ps3pad = {
  55   "PS3Gamepad",
  56   open_ps3pad,
  57   update_ps3pad,
  58   close_ps3pad
  59 };
  60
  61 ///////////////////////////////
  62 //  RadioGP input functions  //
  63 ///////////////////////////////
  64 C_RESULT open_radioGP(void)
  65 {
  66   C_RESULT res = C_FAIL;
  67
  68   FILE* f = fopen("/proc/bus/input/devices", "r");
  69
  70   if( f != NULL )
  71   {
  72     res = parse_proc_input_devices( f,  RADIO_GP_ID);
  73
  74     fclose( f );
  75
  76     if( SUCCEED( res ) && strcmp(radioGP.name, "GreatPlanes")!=0)
  77     {
  78                         char dev_path[20]="/dev/input/";
  79                         strcat(dev_path, radioGP.name);
  80       joy_dev = open(dev_path, O_NONBLOCK | O_RDONLY);
  81
  82                         return res;
  83     }
  84                 else
  85                 {
  86                         return C_FAIL;
  87                 }
  88   }
  89
  90   return res;
  91 }
  92
  93 C_RESULT update_radioGP(void)
  94 {
  95   static float32_t roll = 0, pitch = 0, gaz=0, yaw=0;
  96   static bool_t refresh_values = FALSE;
  97   ssize_t res;
  98   static struct js_event js_e_buffer[64];
  99
 100   res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
 101
 102   if( !res || (res < 0 && errno == EAGAIN) )
 103     return C_OK;
 104
 105   if( res < 0 )
 106     return C_FAIL;
 107
 108   if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
 109     return C_OK;
 110
 111   // Buffer decomposition in blocs (if the last is incomplete, it's ignored)
 112   int32_t idx = 0;
 113   refresh_values = FALSE;
 114   for (idx = 0; idx < res / sizeof(struct js_event); idx++)
 115   {
 116     if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
 117     {
 118       break;
 119     }
 120     else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
 121     {
 122       switch( js_e_buffer[idx].number )
 123       {
 124         case GP_BOARD_LEFT :
 125                                         ardrone_tool_set_ui_pad_start(js_e_buffer[idx].value);
 126                                         break;
 127         case GP_SIDE_RIGHT :
 128                                         ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
 129                                         break;
 130         case GP_IMPULSE :
 131                                         ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
 132                                         break;
 133         case GP_SIDE_LEFT_DOWN :
 134                                         ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 135                                         break;
 136         case GP_SIDE_LEFT_UP :
 137                                         ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 138                                         break;
 139         default: break;
 140       }
 141     }
 142     else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
 143     {
 144       refresh_values = TRUE;
 145       switch( js_e_buffer[idx].number )
 146       {
 147         case GP_PITCH:
 148           pitch = js_e_buffer[idx].value;
 149           break;
 150         case GP_GAZ:
 151           gaz = js_e_buffer[idx].value;
 152           break;
 153         case GP_ROLL:
 154           roll = js_e_buffer[idx].value;
 155           break;
 156         case GP_PID:
 157           break;
 158         case GP_YAW:
 159           yaw = js_e_buffer[idx].value;
 160           break;
 161         default:
 162           break;
 163       }
 164     }
 165     else
 166     {// TODO: default: ERROR (non-supported)
 167     }
 168   }
 169
 170   if(refresh_values)// Axis values to refresh
 171     {
 172        ardrone_at_set_progress_cmd( 1,
 173                                                roll/25000.0,
 174                                                pitch/25000.0,
 175                                                -gaz/25000.0,
 176                                                yaw/25000.0);
 177     }
 178
 179   return C_OK;
 180 }
 181
 182 C_RESULT close_radioGP(void)
 183 {
 184   close( joy_dev );
 185
 186   return C_OK;
 187 }
 188
 189
 190 ///////////////////////////////
 191 //  GamePad input functions  //
 192 ///////////////////////////////
 193
 194 C_RESULT open_gamepad(void)
 195 {
 196   C_RESULT res = C_FAIL;
 197
 198   FILE* f = fopen("/proc/bus/input/devices", "r");
 199
 200   if( f != NULL )
 201   {
 202     res = parse_proc_input_devices( f, GAMEPAD_LOGICTECH_ID);
 203
 204     fclose( f );
 205
 206     if( SUCCEED( res ) && strcmp(gamepad.name, "Gamepad")!=0)
 207     {
 208                         char dev_path[20]="/dev/input/";
 209                         strcat(dev_path, gamepad.name);
 210             joy_dev = open(dev_path, O_NONBLOCK | O_RDONLY);
 211     }
 212                 else
 213                 {
 214                         return C_FAIL;
 215                 }
 216   }
 217
 218   return res;
 219 }
 220
 221 C_RESULT update_gamepad(void)
 222 {
 223   static int32_t x = 0, y = 0;
 224   static bool_t refresh_values = FALSE;
 225   ssize_t res;
 226   static struct js_event js_e_buffer[64];
 227   static int32_t start = 0;
 228   input_state_t* input_state;
 229
 230   static int32_t theta_trim = 0;
 231   static int32_t phi_trim   = 0;
 232   static int32_t yaw_trim   = 0;
 233
 234
 235   res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
 236
 237   if( !res || (res < 0 && errno == EAGAIN) )
 238     return C_OK;
 239
 240   if( res < 0 )
 241     return C_FAIL;
 242
 243   if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
 244     return C_OK;
 245
 246   // Buffer decomposition in blocs (if the last is incomplete, it's ignored)
 247   int32_t idx = 0;
 248   refresh_values = FALSE;
 249   input_state = ardrone_tool_get_input_state();
 250   for (idx = 0; idx < res / sizeof(struct js_event); idx++)
 251   {
 252     if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
 253     {
 254       break;
 255     }
 256     else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
 257     {
 258       switch( js_e_buffer[idx].number )
 259       {
 260         case PAD_AG :
 261                 ardrone_tool_set_ui_pad_ag(js_e_buffer[idx].value);
 262                 break;
 263         case PAD_AB :
 264                 ardrone_tool_set_ui_pad_ab(js_e_buffer[idx].value);
 265                 break;
 266         case PAD_AD :
 267                 ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 268                 break;
 269         case PAD_AH :
 270                 ardrone_tool_set_ui_pad_ah(js_e_buffer[idx].value);
 271                 break;
 272         case PAD_L1 :
 273                 if( js_e_buffer[idx].value )
 274                 {
 275                         yaw_trim = 0;
 276
 277                         if( input_state->r2 )
 278                         {
 279                                 yaw_trim = -1;
 280                         }
 281                         else
 282                         {
 283                                 ardrone_tool_set_ui_pad_l1(1);
 284                         }
 285         // ui_pad_yaw_trim( yaw_trim );
 286                 }
 287       else
 288       {
 289          yaw_trim = 0;
 290          ardrone_tool_set_ui_pad_l1(0);
 291        //  ui_pad_yaw_trim( yaw_trim );
 292       }
 293                 break;
 294         case PAD_R1 :
 295                 if( js_e_buffer[idx].value )
 296                 {
 297                         yaw_trim = 0;
 298
 299                         if( input_state->r2 )
 300                         {
 301                                 yaw_trim = 1;
 302                         }
 303                         else
 304                         {
 305                                 ardrone_tool_set_ui_pad_r1(1);
 306                         }
 307         // ui_pad_yaw_trim( yaw_trim );
 308                 }
 309       else
 310       {
 311          yaw_trim = 0;
 312          ardrone_tool_set_ui_pad_r1(0);
 313         // ui_pad_yaw_trim( yaw_trim );
 314       }
 315                 break;
 316         case PAD_L2 :
 317                 ardrone_tool_set_ui_pad_l2(js_e_buffer[idx].value);
 318                 if( !js_e_buffer[idx].value )
 319                 {
 320                         ardrone_at_set_pmode( MiscVar[0] );
 321                         ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
 322                 }
 323                 break;
 324         case PAD_R2 :
 325                 ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
 326       if( !js_e_buffer[idx].value )
 327          ardrone_at_set_flat_trim();
 328                 break;
 329         case PAD_SELECT :
 330                 ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
 331                 break;
 332         case PAD_START :
 333       if( js_e_buffer[idx].value )
 334       {
 335          start ^= 1;
 336                    ardrone_tool_set_ui_pad_start( start );
 337                 }
 338       break;
 339         default:
 340                 break;
 341       }
 342
 343     }
 344     else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
 345     {
 346       refresh_values = TRUE;
 347       switch( js_e_buffer[idx].number )
 348       {
 349         case PAD_X:
 350           x = ( js_e_buffer[idx].value + 1 ) >> 15;
 351           break;
 352         case PAD_Y:
 353           y = ( js_e_buffer[idx].value + 1 ) >> 15;
 354           break;
 355         default:
 356           break;
 357       }
 358     }
 359     else
 360     {// TODO: default: ERROR (non-supported)
 361     }
 362   }
 363
 364   if(refresh_values)// Axis values to refresh
 365     {
 366
 367                  phi_trim    = 0;
 368                  theta_trim  = 0;
 369
 370                  if( input_state->r2 )
 371                  {
 372                          switch(x + 1)
 373                          {
 374                                  case 0:
 375                                          phi_trim = -1;
 376                                          break;
 377
 378                                  case 2:
 379                                          phi_trim = 1;
 380                                          break;
 381                          }
 382
 383                          switch(y + 1)
 384                          {
 385                                  case 0:
 386                                          theta_trim = -1;
 387                                          break;
 388
 389                                  case 2:
 390                                          theta_trim = 1;
 391                                          break;
 392                          }
 393                          // We are triming so we don't want to update ardrone position
 394                          x = 0;
 395                          y = 0;
 396                  }
 397                  ardrone_tool_set_ui_pad_xy( x, y );
 398        //ui_pad_phi_trim( phi_trim );
 399        //ui_pad_theta_trim( theta_trim );
 400     }
 401   return C_OK;
 402 }
 403
 404 C_RESULT close_gamepad(void)
 405 {
 406   close( joy_dev );
 407
 408   return C_OK;
 409 }
 410
 411
 412
 413
 414
 415
 416 ///////////////////////////////
 417 //  Playstation 3 Gamepad input functions  //
 418 ///////////////////////////////
 419
 420 C_RESULT open_ps3pad(void)
 421 {
 422     PRINT("Searching PS3 Pad device ...\n");
 423
 424   C_RESULT res = C_FAIL;
 425
 426   FILE* f = fopen("/proc/bus/input/devices", "r");
 427
 428   if( f != NULL )
 429   {
 430     res = parse_proc_input_devices( f, GAMEPAD_PLAYSTATION3_ID );
 431
 432     fclose( f );
 433
 434     if( SUCCEED( res ) && strcmp(ps3pad.name, "PS3Gamepad")!=0)
 435     {
 436                         char dev_path[20]="/dev/input/";
 437                         strcat(dev_path, ps3pad.name);
 438             joy_dev = open(dev_path, O_NONBLOCK | O_RDONLY);
 439
 440             if (joy_dev)
 441             {
 442                 printf("Joydev %s ouvert\n",dev_path);
 443             }
 444             else{
 445                 printf("Joydev %s pas ouvert\n",dev_path);
 446             }
 447     }
 448                 else
 449                 {
 450                     PRINT("PS3 Pad device not found.\n");
 451                         return C_FAIL;
 452
 453                 }
 454   }
 455
 456   return res;
 457 }
 458
 459 C_RESULT update_ps3pad(void)
 460 {
 461
 462   static int32_t stick1LR = 0, stick1UD = 0 , stick2LR = 0 , stick2UD = 0;
 463   static bool_t refresh_values = FALSE;
 464   ssize_t res;
 465   static struct js_event js_e_buffer[64];
 466   static int32_t start = 0;
 467   input_state_t* input_state;
 468
 469   static int center_x1=0;
 470   static int center_y1=0;
 471   static int center_x2=0;
 472   static int center_y2=0;
 473
 474   res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
 475
 476
 477   if( !res || (res < 0 && errno == EAGAIN) )
 478     return C_OK;
 479
 480
 481   if( res < 0 )
 482     return C_FAIL;
 483
 484
 485   if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
 486     return C_OK;
 487
 488
 489   int32_t idx = 0;
 490   refresh_values = FALSE;
 491   input_state = ardrone_tool_get_input_state();
 492   for (idx = 0; idx < res / sizeof(struct js_event); idx++)
 493   {
 494     if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
 495     {
 496       break;
 497     }
 498     else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
 499     {
 500       switch( js_e_buffer[idx].number )
 501       {
 502         case PS3BTN_LEFTARROW :
 503                 ardrone_tool_set_ui_pad_ag(js_e_buffer[idx].value);
 504                 break;
 505         case PS3BTN_DOWNARROW :
 506                 ardrone_tool_set_ui_pad_ab(js_e_buffer[idx].value);
 507                 break;
 508         case PS3BTN_RIGHTARROW :
 509                 ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 510                 break;
 511         case PS3BTN_UPARROW :
 512                 ardrone_tool_set_ui_pad_ah(js_e_buffer[idx].value);
 513                 break;
 514         case PS3BTN_L1 :
 515                 ardrone_tool_set_ui_pad_l1(js_e_buffer[idx].value);
 516                 break;
 517         case PS3BTN_R1 :
 518                 ardrone_tool_set_ui_pad_r1(js_e_buffer[idx].value);
 519                 break;
 520         case PS3BTN_L2 :
 521                 ardrone_tool_set_ui_pad_l2(js_e_buffer[idx].value);
 522                 break;
 523         case PS3BTN_R2 :
 524                 ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
 525                 break;
 526         case PS3BTN_SELECT :
 527                 ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
 528                 break;
 529         case PS3BTN_START :
 530             if( js_e_buffer[idx].value )   { start ^= 1;  ardrone_tool_set_ui_pad_start( start );   }
 531                 break;
 532         case PS3BTN_PS3:
 533             /* Calibrate joystick */
 534            /* center_x1 = stick1LR;
 535             center_y1 = stick1UD;
 536             center_x2 = stick2UD;
 537             center_y2 = stick2LR;*/
 538         break;
 539         default:
 540                 break;
 541       }
 542     }
 543     else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
 544     {
 545       refresh_values = TRUE;
 546       switch( js_e_buffer[idx].number )
 547       {
 548         case PS3AXIS_STICK1_LR:
 549           stick1LR = ( js_e_buffer[idx].value ) ;
 550           break;
 551         case PS3AXIS_STICK1_UD:
 552           stick1UD = ( js_e_buffer[idx].value ) ;
 553           break;
 554
 555         case PS3AXIS_STICK2_LR:
 556           stick2LR = ( js_e_buffer[idx].value ) ;
 557           break;
 558         case PS3AXIS_STICK2_UD:
 559           stick2UD = ( js_e_buffer[idx].value ) ;
 560           break;
 561
 562         default:
 563           break;
 564       }
 565     }
 566     else
 567     {// TODO: default: ERROR (non-supported)
 568     }
 569   }
 570
 571   if(refresh_values)// Axis values to refresh
 572     {
 573       ardrone_at_set_progress_cmd( 1,
 574                                     /*roll*/(float)(stick1LR-center_x1)/32767.0f,
 575                                     /*pitch*/(float)(stick1UD-center_y1)/32767.0f,
 576                                     /*gaz*/-(float)(stick2UD-center_x2)/32767.0f,
 577                                     /*yaw*/(float)(stick2LR-center_y2)/32767.0f );
 578     }
 579   return C_OK;
 580 }
 581
 582
 583
 584
 585
 586 C_RESULT close_ps3pad(void)
 587 {
 588   close( joy_dev );
 589
 590   return C_OK;
 591 }
 592
 593
 594
 595
 596
 597 static int32_t make_id(device_t* device)
 598 {
 599   return ( (device->vendor << 16) & 0xffff0000) | (device->product & 0xffff);
 600 }
 601
 602 static C_RESULT add_device(device_t* device, const int32_t id_wanted)
 603 {
 604   int32_t id = make_id(device);
 605
 606   if( id_wanted == GAMEPAD_LOGICTECH_ID && id == id_wanted)
 607   {
 608                 PRINT("Input device %s found\n", device->name);
 609     strncpy(gamepad.name, device->handlers, MAX_NAME_LENGTH);
 610     return C_OK;
 611         }
 612
 613         if(id_wanted == RADIO_GP_ID && id==id_wanted)
 614         {
 615                 PRINT("Input device %s found\n", device->name);
 616     strncpy(radioGP.name, device->handlers, MAX_NAME_LENGTH);
 617     return C_OK;
 618   }
 619
 620     if(id_wanted == GAMEPAD_PLAYSTATION3_ID && id==id_wanted)
 621         {
 622                 PRINT("PS3 : Input device %s found\n", device->name);
 623         strncpy(ps3pad.name, device->handlers, MAX_NAME_LENGTH);
 624         return C_OK;
 625   }
 626
 627   return C_FAIL;
 628 }
 629
 630
 631
 632
 633
 634 /** simple /proc/bus/input/devices generic LL(1) parser **/
 635
 636 #define KW_MAX_LEN 64
 637
 638 typedef enum {
 639   KW_BUS,
 640   KW_VENDOR,
 641   KW_PRODUCT,
 642   KW_VERSION,
 643   KW_NAME,
 644   KW_HANDLERS,
 645   KW_MAX
 646 } keyword_t;
 647
 648 typedef enum {
 649   INT,
 650   STRING,
 651 } value_type_t;
 652
 653 typedef struct {
 654   const char*   name;
 655   value_type_t  value_type;
 656   int32_t       value_offset;
 657 } kw_tab_entry_t;
 658
 659 static int current_c;
 660 static int next_c; // look ahead buffer
 661
 662 static device_t current_device;
 663
 664 static const int separators[] = { ' ',  ':', '=', '\"', '\n' };
 665 static const int quote = '\"';
 666 static const int eol = '\n';
 667
 668 static kw_tab_entry_t kw_tab[] = {
 669   [KW_BUS]      = {  "Bus",      INT,    offsetof(device_t, bus)       },
 670   [KW_VENDOR]   = {  "Vendor",   INT,    offsetof(device_t, vendor)    },
 671   [KW_PRODUCT]  = {  "Product",  INT,    offsetof(device_t, product)   },
 672   [KW_VERSION]  = {  "Version",  INT,    offsetof(device_t, version)   },
 673   [KW_NAME]     = {  "Name",     STRING, offsetof(device_t, name)      },
 674   [KW_HANDLERS] = {  "Handlers", STRING, offsetof(device_t, handlers)  }
 675 };
 676
 677 static const char* handler_names[] = {
 678   "js0",
 679   "js1",
 680   "js2",
 681   "js3",
 682   0
 683 };
 684
 685 static bool_t is_separator(int c)
 686 {
 687   int32_t i;
 688   bool_t found = FALSE;
 689
 690   for( i = 0; i < sizeof separators && !found; i++ )
 691   {
 692     found = ( c == separators[i] );
 693   }
 694
 695   return found;
 696 }
 697
 698 static bool_t is_quote(int c)
 699 {
 700   return c == quote;
 701 }
 702
 703 static bool_t is_eol(int c)
 704 {
 705   return c == eol;
 706 }
 707
 708 static C_RESULT fetch_char(FILE* f)
 709 {
 710   C_RESULT res = C_FAIL;
 711
 712   current_c = next_c;
 713
 714   if( !feof(f) )
 715   {
 716     next_c = fgetc(f);
 717     res = C_OK;
 718   }
 719
 720   // PRINT("current_c = %c, next_c = %c\n", current_c, next_c );
 721
 722   return res;
 723 }
 724
 725 static C_RESULT parse_string(FILE* f, char* str, int32_t maxlen)
 726 {
 727   int32_t i = 0;
 728   bool_t is_quoted = is_quote(current_c);
 729
 730   if( is_quoted )
 731   {
 732     while( SUCCEED(fetch_char(f)) && ! ( is_separator(current_c) && is_quote(current_c) ) )  {
 733       str[i] = current_c;
 734       i++;
 735     }
 736   }
 737   else
 738   {
 739     while( SUCCEED(fetch_char(f)) && !is_separator(current_c) )  {
 740       str[i] = current_c;
 741       i++;
 742     }
 743   }
 744
 745   str[i] = '\0';
 746   // PRINT("parse_string: %s\n", str);
 747
 748   return is_eol( current_c ) ? C_FAIL : C_OK;
 749 }
 750
 751 static C_RESULT parse_int(FILE* f, int32_t* i)
 752 {
 753   C_RESULT res = C_OK;
 754   int value;
 755
 756   *i = 0;
 757
 758   while( !is_separator(next_c) && SUCCEED(fetch_char(f)) && res == C_OK )  {
 759     value = current_c - '0';
 760
 761     if (value > 9 || value < 0)
 762     {
 763       value = current_c - 'a' + 10;
 764       res = (value > 0xF || value < 0xa) ? C_FAIL : C_OK;
 765     }
 766
 767     *i *= 16;
 768     *i += value;
 769   }
 770
 771   return res;
 772 }
 773
 774 static C_RESULT skip_line(FILE* f)
 775 {
 776   while( !is_eol(next_c) && SUCCEED(fetch_char(f)) );
 777
 778   return C_OK;
 779 }
 780
 781 static C_RESULT match_keyword( const char* keyword, keyword_t* kw )
 782 {
 783   int32_t i;
 784   C_RESULT res = C_FAIL;
 785
 786   for( i = 0; i < KW_MAX && res != C_OK; i++ )
 787   {
 788     res = ( strcmp( keyword, kw_tab[i].name ) == 0 ) ? C_OK : C_FAIL;
 789   }
 790
 791   *kw = i-1;
 792
 793   return res;
 794 }
 795
 796 static C_RESULT match_handler( void )
 797 {
 798   int32_t i = 0;
 799   bool_t found = FALSE;
 800
 801   while( !found && handler_names[i] != 0 )
 802   {
 803     found = strcmp( (char*)((char*)&current_device + kw_tab[KW_HANDLERS].value_offset), handler_names[i] ) == 0;
 804
 805     i ++;
 806   }
 807
 808         if(found)
 809         {
 810                 strcpy(current_device.handlers, handler_names[i-1]);
 811         }
 812
 813   return found ? C_OK : C_FAIL;
 814 }
 815
 816 static C_RESULT parse_keyword( FILE* f, keyword_t kw )
 817 {
 818   C_RESULT res = C_OK;
 819
 820   while( is_separator(next_c) && SUCCEED(fetch_char(f)) );
 821
 822   switch( kw_tab[kw].value_type ) {
 823     case INT:
 824       parse_int( f, (int32_t*)((char*)&current_device + kw_tab[kw].value_offset) );
 825       //PRINT("%s = %x\n", kw_tab[kw].name, *(int32_t*)((char*)&current_device + kw_tab[kw].value_offset) );
 826       break;
 827
 828     case STRING:
 829       parse_string( f, (char*)((char*)&current_device + kw_tab[kw].value_offset), KW_MAX_LEN );
 830       //PRINT("%s = %s\n", kw_tab[kw].name, (char*)((char*)&current_device + kw_tab[kw].value_offset) );
 831       break;
 832
 833     default:
 834       res = C_FAIL;
 835       break;
 836   }
 837
 838   return res;
 839 }
 840
 841 static C_RESULT parse_I(FILE* f)
 842 {
 843   char keyword[KW_MAX_LEN];
 844
 845   while( SUCCEED(fetch_char(f)) && is_separator(next_c) );
 846
 847   while( !is_eol(next_c) ) {
 848     keyword_t kw;
 849
 850     parse_string( f, keyword, KW_MAX_LEN );
 851     if( SUCCEED( match_keyword( keyword, &kw ) ) )
 852     {
 853       parse_keyword( f, kw );
 854     }
 855   }
 856
 857   return C_OK;
 858 }
 859
 860 static C_RESULT parse_N(FILE* f)
 861 {
 862   char keyword[KW_MAX_LEN];
 863
 864   while( SUCCEED(fetch_char(f)) && is_separator(next_c) );
 865
 866   while( !is_eol(next_c) ) {
 867     keyword_t kw;
 868
 869     parse_string( f, keyword, KW_MAX_LEN );
 870     if( SUCCEED( match_keyword( keyword, &kw ) ) )
 871     {
 872       parse_keyword( f, kw );
 873     }
 874   }
 875
 876
 877   return C_OK;
 878 }
 879
 880 static C_RESULT parse_H(FILE* f)
 881 {
 882   C_RESULT res = C_FAIL;
 883   char keyword[KW_MAX_LEN];
 884
 885   while( SUCCEED(fetch_char(f)) && is_separator(next_c) );
 886
 887   while( !is_eol(next_c) ) {
 888     parse_string( f, keyword, KW_MAX_LEN );
 889     if( strcmp( keyword, kw_tab[KW_HANDLERS].name ) == 0 )
 890     {
 891       while( FAILED(res) && SUCCEED( parse_string(f,
 892                                                   (char*)((char*)&current_device + kw_tab[KW_HANDLERS].value_offset),
 893                                                   KW_MAX_LEN ) ) )
 894       {
 895         res = match_handler();
 896       }
 897     }
 898   }
 899
 900   return res;
 901 }
 902
 903 static C_RESULT end_device(const int32_t id)
 904 {
 905   C_RESULT res = C_FAIL;
 906   res=add_device(&current_device, id);
 907   vp_os_memset( &current_device, 0, sizeof(device_t) );
 908
 909   return res;
 910 }
 911
 912 static C_RESULT parse_proc_input_devices(FILE* f, const int32_t id)
 913 {
 914   C_RESULT res = C_FAIL;
 915
 916   next_c = '\0';
 917   vp_os_memset( &current_device, 0, sizeof(device_t) );
 918
 919   while( res != C_OK && SUCCEED( fetch_char(f) ) )
 920   {
 921     switch( next_c )
 922     {
 923       case 'I': parse_I(f); break;
 924       case 'N': parse_N(f); break;
 925       case 'H': if( SUCCEED( parse_H(f) ) ) res = end_device(id); break;
 926       case 'P':
 927       case 'S':
 928       case 'B':
 929       default: skip_line(f); break;
 930     }
 931   }
 932
 933   return res;
 934 }