Problem in GAP profile (when connecting ) under Windows 7 and newer.

#include "SS1BTPS.h"             /* Main SS1 Bluetooth Stack Header.          */
#include "BTPSKRNL.h"            /* BTPS Kernel Header.                       */
#include "cardicell_board.h"
#include "cardicell_app.h"
#include "spp.h"
#include "EHCILL.h"              /* eHCILL Implementation Header.             */
#include "utils.h"
#include "gap.h"


#define REQUEST_LINK_KEY_TABLE          1
#define SEND_LINK_KEY                   2

extern unsigned int BluetoothStackID;
static st_gap_context gap_context ;
static char debug_buffer[256];
static BD_ADDR_t gap_null_bd_addr;

typedef enum
{
  enGAP_INIT = 0,
  enGAP_TEMPO,
  enGAP_SENDPINCODE
}enState;

static unsigned int _gap_tempo;
static enState _gap_state;

// The following string table is used to map the API I/O Capabilities values to
// an easily displayable string.
static BTPSCONST char *IOCapabilitiesStrings[] =
{
   "Display Only",
   "Display Yes/No",
   "Keyboard Only",
   "No Input/Output"
} ;


void _gap_request_linkkey( unsigned char src_port,  unsigned char dest_port ,unsigned int size )
{
  unsigned int checksum = 0;
  unsigned char msg_type = REQUEST_LINK_KEY_TABLE;
  
  checksum += (size + sizeof(msg_type));
  checksum += src_port;
  checksum += dest_port;
  checksum += msg_type;
  

  msg_send_header( src_port, dest_port, size );

  drv_main_interface_transmit_buffer( (const char*)msg_type, sizeof(char) );  
  drv_main_interface_transmit_buffer( (const char*)&checksum, sizeof(unsigned int) );
}


void _gap_send_linkkey( char * ptr,  unsigned char src_port,  unsigned char dest_port ,unsigned int size )
{
  unsigned int checksum ;
  unsigned char msg_type = SEND_LINK_KEY;
  
  checksum = (size + sizeof(msg_type));
  checksum += src_port;
  checksum += dest_port;
  checksum += msg_type;
  
  checksum += calculate_checksum( ptr, size );
  
  msg_send_header( src_port, dest_port, size );

  drv_main_interface_transmit_buffer( (const char*)msg_type, sizeof(char) );  
  drv_main_interface_transmit_buffer( (const char*)ptr, sizeof(LinkKeyInfo_t) );
  drv_main_interface_transmit_buffer( (const char*)&checksum, sizeof(unsigned int) );
}


static void _set_tempo(  unsigned int delay )
{
  _gap_tempo = delay;
  _gap_state = enGAP_TEMPO;
}

// The following function is responsible for issuing a GAP
// Authentication Response with a PIN Code value specified via the
// input parameter.  This function returns zero on successful
// execution and a negative value on all errors.
static int _pin_code_response( unsigned int bt_stack_id )
{
  int                                  result;
  int                                  ret_val;
  PIN_Code_t                           pin_code;
  GAP_Authentication_Information_t     GAP_Authentication_Information;
  
  // First, check that valid Bluetooth Stack ID exists.
  if( !bt_stack_id )
    return INVALID_STACK_ID_ERROR;
  
  // First, check to see if there is an on-going Pairing operation active.
  if(!COMPARE_BD_ADDR(gap_context.currentRemoteAddr, gap_null_bd_addr))
  {
    if( BTPS_StringLength( BT_PIN_CODE ) <= sizeof(PIN_Code_t) )
    {
      // Initialize the PIN code.
      ASSIGN_PIN_CODE(pin_code, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
      
      BTPS_MemCopy(&pin_code, BT_PIN_CODE, BTPS_StringLength(BT_PIN_CODE));
      
      // Populate the response structure.
      GAP_Authentication_Information.GAP_Authentication_Type      = atPINCode;
      GAP_Authentication_Information.Authentication_Data_Length   = BTPS_StringLength(BT_PIN_CODE);
      GAP_Authentication_Information.Authentication_Data.PIN_Code = pin_code;
      
      // Submit the Authentication Response.
      result = GAP_Authentication_Response(bt_stack_id, gap_context.currentRemoteAddr, &GAP_Authentication_Information);
      
      // Check the return value for the submitted command for success.
      if(!result)
      {
        // Operation was successful, inform the user.
        drv_debug_log("GAP_Authentication_Response(), Pin Code Response Success.\r\n");
        
        // Flag success to the caller.
        ret_val = 0;
      }
      else
      {
        // Inform the user that the Authentication Response was
        // not successful.
        drv_debug_log("GAP_Authentication_Response() Failed\n\r");
        
        ret_val = FUNCTION_ERROR;
      }
      
      // Flag that there is no longer a current Authentication
      // procedure in progress.
      ASSIGN_BD_ADDR(gap_context.currentRemoteAddr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);
    }
    else
    {
      // One or more of the necessary parameters is/are invalid.
      drv_debug_log("PINCodeResponse [PIN Code]");
      
      ret_val = INVALID_PARAMETERS_ERROR;
    }
  }
  else
  {
    // There is not currently an on-going authentication operation, inform 
    // the user of this error condition.
    drv_debug_log("PIN Code Authentication Response: Authentication not in progress.\r\n");
    
    ret_val = FUNCTION_ERROR;
  }
  
  return ret_val;
}


static void _process_remote_name_request_event( GAP_Event_Data_t * GAP_Event_Data )
{
  GAP_Remote_Name_Event_Data_t     *GAP_Remote_Name_Event_Data;
  
  // Bluetooth Stack has responded to a previously issued Remote Name Request that was issued.
  GAP_Remote_Name_Event_Data = GAP_Event_Data->Event_Data.GAP_Remote_Name_Event_Data;
  
  if(GAP_Remote_Name_Event_Data)
  {
    if(GAP_Remote_Name_Event_Data->Remote_Name)
    {
      //TODO: Notify main interface
      strncpy( gap_context.remoteDeviceNameBuffer, GAP_Remote_Name_Event_Data->Remote_Name, MAX_NAME_LENGTH );
      gap_context.remoteDeviceNameBuffer[MAX_NAME_LENGTH-1]=0;
      
      SprintF(gap_context.unsolicited_buffer,"remote device: %s\n\r", gap_context.remoteDeviceNameBuffer);
      //cmd_line_put_unsolicited_response((const char *)gap_context.unsolicited_buffer);
      drv_debug_log((const char *)gap_context.unsolicited_buffer);
    }
  }
}

static void _process_authentication_event( GAP_Event_Data_t * GAP_Event_Data )
{
  int i, result;
  GAP_Authentication_Information_t  GAP_Authentication_Information;
  GAP_IO_Capability_t               remote_io_capability;

  Boolean_t                         oob_data;
  Boolean_t                         mitm;

  switch(GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->GAP_Authentication_Event_Type)
  {
  case atLinkKeyRequest:
    drv_debug_log("\r\natLinkKeyRequest\r\n");

    // Setup the authentication information response structure.
    GAP_Authentication_Information.GAP_Authentication_Type    = atLinkKey;
    GAP_Authentication_Information.Authentication_Data_Length = 0;
    
    // See if we have stored a Link Key for the specified device.
    for(i=0;i< MAX_SUPPORTED_LINK_KEYS;i++)
    {
      if(COMPARE_BD_ADDR(gap_context.LinkKeyInfo[i].BD_ADDR, 
                         GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device))
      {
        /* Link Key information stored, go ahead and    */
        /* respond with the stored Link Key.            */
        GAP_Authentication_Information.Authentication_Data_Length   = sizeof(Link_Key_t);
        GAP_Authentication_Information.Authentication_Data.Link_Key = gap_context.LinkKeyInfo[i].LinkKey;
        break;
      }
    }
    
    // Submit the authentication response.
    if(!GAP_Authentication_Response(BluetoothStackID, 
                                         GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device, 
                                         &GAP_Authentication_Information))
    {
      drv_debug_log("GAP_Authentication_Response succedded\n\r");
    }
    else
    {
      drv_debug_log("GAP_Authentication_Response error\n\r");
    }

    break;
    
  case atPINCodeRequest:
    // A pin code request event occurred, first display the BD_ADD of the 
    // remote device requesting the pin.
    drv_debug_log("\r\natPINCodeRequest\r\n");
   
   // Note the current Remote BD_ADDR that is requesting the PIN Code.
   gap_context.currentRemoteAddr = GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device;
   
#if 0
    //Send a PIN Code Response.
    _pin_code_response( BluetoothStackID );
#else
    _set_tempo( 3000 );
#endif
    break;
    
  case atAuthenticationStatus:
    // An authentication status event occurred, display all relevant information.
    SprintF(debug_buffer,"\r\natAuthenticationStatus: %d.\r\n",
            GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.Authentication_Status);
    drv_debug_log(debug_buffer);
    
    // Flag that there is no longer a current Authentication procedure in progress.
    ASSIGN_BD_ADDR(gap_context.currentRemoteAddr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);
    break;
  case atLinkKeyCreation:
      // A link key creation event occurred, first display the remote device that caused this event
      drv_debug_log("atLinkKeyCreation\r\n");
      
      for(i=0; i< MAX_SUPPORTED_LINK_KEYS ;i++)
      {
        if(COMPARE_BD_ADDR(gap_context.LinkKeyInfo[i].BD_ADDR, 
                           GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device))
        {
          drv_debug_log("Already in the memory\r\n");
          gap_context.current_linkkey_index = i;
          return;
        }
      }
      
      if(gap_context.nb_key_in_table < MAX_SUPPORTED_LINK_KEYS)
      {
        gap_context.LinkKeyInfo[gap_context.nb_key_in_table].BD_ADDR = GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device;
        gap_context.LinkKeyInfo[gap_context.nb_key_in_table].LinkKey = GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.Link_Key_Info.Link_Key;
        gap_context.current_linkkey_index = gap_context.nb_key_in_table;
        drv_debug_log("The link key table is full, remove last element to store last linkkey.\n\r");
        
        // Send to upper board the link key.
        _gap_send_linkkey((char *)&gap_context.LinkKeyInfo[gap_context.nb_key_in_table], 2, 14 , sizeof(LinkKeyInfo_t));
        
        gap_context.nb_key_in_table++;
      }
      else
      {
        gap_context.LinkKeyInfo[MAX_SUPPORTED_LINK_KEYS-1].BD_ADDR = GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device;
        gap_context.LinkKeyInfo[MAX_SUPPORTED_LINK_KEYS-1].LinkKey = GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.Link_Key_Info.Link_Key;
        gap_context.current_linkkey_index = gap_context.nb_key_in_table;
        drv_debug_log("The link key table is full, remove last element to store last linkkey.\n\r");
        
        // Send to upper board the link key.
        _gap_send_linkkey((char *)&gap_context.LinkKeyInfo[MAX_SUPPORTED_LINK_KEYS-1], 2, 14 , sizeof(LinkKeyInfo_t));
      }
      
      drv_debug_log("Link Key sent.\r\n");
      
    break;
    
  case atIOCapabilityRequest:
    drv_debug_log("\n\ratIOCapabilityRequest\r\n");
    
    // Setup the Authentication Information Response structure.
    GAP_Authentication_Information.GAP_Authentication_Type                                      = atIOCapabilities;
    GAP_Authentication_Information.Authentication_Data_Length                                   = sizeof(GAP_IO_Capabilities_t);
    GAP_Authentication_Information.Authentication_Data.IO_Capabilities.IO_Capability            = (GAP_IO_Capability_t)gap_context.io_capability;
    GAP_Authentication_Information.Authentication_Data.IO_Capabilities.MITM_Protection_Required = gap_context.mitm_protection;
    GAP_Authentication_Information.Authentication_Data.IO_Capabilities.OOB_Data_Present         = gap_context.oob_support;
    
    // Submit the Authentication Response.
    result = GAP_Authentication_Response(BluetoothStackID, 
                                         GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device, 
                                         &GAP_Authentication_Information);
    
    // Check the result of the submitted command.
    if(!result)
      drv_debug_log("Auth success");
    else
      drv_debug_log("Auth error");
    break;
    
  case atIOCapabilityResponse:
    drv_debug_log("\n\ratIOCapabilityResponse.\r\n");
    
    remote_io_capability = GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.IO_Capabilities.IO_Capability;
    mitm               = (Boolean_t)GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.IO_Capabilities.MITM_Protection_Required;
    oob_data           = (Boolean_t)GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.IO_Capabilities.OOB_Data_Present;
    
    SprintF(debug_buffer,"Capabilities: %s%s%s\r\n", IOCapabilitiesStrings[remote_io_capability], ((mitm)?", MITM":""), ((oob_data)?", OOB Data":""));
    drv_debug_log(debug_buffer);
    break;
    
  case atUserConfirmationRequest:
    drv_debug_log("atUserConfirmationRequest !!! I don't know what I need to responds\n\r");
    break;
    
  case atPasskeyRequest:
    drv_debug_log("atPasskeyRequest\r\n");
    
    // Note the current Remote BD_ADDR that is requesting the Passkey.
    gap_context.currentRemoteAddr = GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device;
    
    // Inform the user that they will need to respond with a Passkey Response.
    drv_debug_log("Respond with: PassKeyResponse\r\n");
    break;
    
  case atRemoteOutOfBandDataRequest:
    drv_debug_log("atRemoteOutOfBandDataRequest\r\n");
    
    // This application does not support OOB data so respond with a data length 
    // of Zero to force a negative reply.
    GAP_Authentication_Information.GAP_Authentication_Type    = atOutOfBandData;
    GAP_Authentication_Information.Authentication_Data_Length = 0;
    
    result = GAP_Authentication_Response(BluetoothStackID, GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Remote_Device, &GAP_Authentication_Information);
    
    if(!result)
      drv_debug_log("GAP_Authentication_Response succedded");
    else
      drv_debug_log("GAP_Authentication_Response failed\n\r");
    break;
    
  case atPasskeyNotification:
    drv_debug_log("atPasskeyNotification\r\n");
    SprintF(debug_buffer,"Passkey Value: %lu\r\n", GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.Numeric_Value);
    drv_debug_log(debug_buffer);
    break;
    
  case atKeypressNotification:
    drv_debug_log("atKeypressNotification\r\n");
    SprintF(debug_buffer,"Keypress: %d\r\n", (int)GAP_Event_Data->Event_Data.GAP_Authentication_Event_Data->Authentication_Event_Data.Keypress_Type);
    drv_debug_log(debug_buffer);
    break;
    
  default:
    drv_debug_log("Un-handled Auth. Event.\r\n");
    break;
  }
}
   /*********************************************************************/
   /*                         Event Callbacks                           */
   /*********************************************************************/

   /* The following function is for the GAP Event Receive Data Callback.*/
   /* This function will be called whenever a Callback has been         */
   /* registered for the specified GAP Action that is associated with   */
   /* the Bluetooth Stack.  This function passes to the caller the GAP  */
   /* Event Data of the specified Event and the GAP Event Callback      */
   /* Parameter that was specified when this Callback was installed.    */
   /* The caller is free to use the contents of the GAP Event Data ONLY */
   /* in the context of this callback.  If the caller requires the Data */
   /* for a longer period of time, then the callback function MUST copy */
   /* the data into another Data Buffer.  This function is guaranteed   */
   /* NOT to be invoked more than once simultaneously for the specified */
   /* installed callback (i.e.  this function DOES NOT have be          */
   /* reentrant).  It Needs to be noted however, that if the same       */
   /* Callback is installed more than once, then the callbacks will be  */
   /* called serially.  Because of this, the processing in this function*/
   /* should be as efficient as possible.  It should also be noted that */
   /* this function is called in the Thread Context of a Thread that the*/
   /* User does NOT own.  Therefore, processing in this function should */
   /* be as efficient as possible (this argument holds anyway because   */
   /* other GAP Events will not be processed while this function call is*/
   /* outstanding).                                                     */
   /* * NOTE * This function MUST NOT Block and wait for events that    */
   /*          can only be satisfied by Receiving other GAP Events.  A  */
   /*          Deadlock WILL occur because NO GAP Event Callbacks will  */
   /*          be issued while this function is currently outstanding.  */
void BTPSAPI gap_event_callback(unsigned int BluetoothStackID, GAP_Event_Data_t *GAP_Event_Data, unsigned long CallbackParameter)
{
   // First, check to see if the required parameters appear to be semi-valid.
   if((BluetoothStackID) && (GAP_Event_Data))
   {
      switch(GAP_Event_Data->Event_Data_Type)
      {
      case etInquiry_Result:
        // No Inquiry should be done in our application.
        drv_debug_log("\n\retInquiry_Result\n\r");
        break;
      case etInquiry_Entry_Result:
        drv_debug_log("\n\retInquiry_Entry_Result\n\r");
        break;
      case etAuthentication:
        // An authentication event occurred, determine which type of authentication event occurred.
        _process_authentication_event( GAP_Event_Data );
        break;
      case etRemote_Name_Result:
        _process_remote_name_request_event( GAP_Event_Data );
        break;
      case etEncryption_Change_Result:
        drv_debug_log("\n\retEncryption_Change_Result\n\r");
#if 0
        BD_ADDRToStr(GAP_Event_Data->Event_Data.GAP_Encryption_Mode_Event_Data->Remote_Device, Callback_BoardStr);
        Display(("\r\netEncryption_Change_Result for %s, Status: 0x%02X, Mode: %s.\r\n", Callback_BoardStr,
                     GAP_Event_Data->Event_Data.GAP_Encryption_Mode_Event_Data->Encryption_Change_Status,
                     ((GAP_Event_Data->Event_Data.GAP_Encryption_Mode_Event_Data->Encryption_Mode == emDisabled)?"Disabled": "Enabled")));
#endif
        break;
      default:
        //An unknown/unexpected GAP event was received.
        SprintF(debug_buffer,"\r\nUnknown Event: %d.\r\n", GAP_Event_Data->Event_Data_Type);
        drv_debug_log(debug_buffer);
        break;
      }
   }
}

// The following function is responsible for placing the Local
// Bluetooth Device into Connectable Mode.  Once in this mode the
// Device will respond to Page Scans from other Bluetooth Devices.
// This function requires that a valid Bluetooth Stack ID exists
// before running.  This function returns zero on success and a
// negative value if an error occurred.
static int _set_connectable( unsigned int bt_stack_id )
{
  // First, check that a valid Bluetooth Stack ID exists.
  if( !bt_stack_id )
    return INVALID_STACK_ID_ERROR;
  
  // Attempt to set the attached Device to be Connectable.
  if( GAP_Set_Connectability_Mode( bt_stack_id , cmConnectableMode) )
    return -1;
  else
    return 0;
}

// The following function is responsible for placing the Local
// Bluetooth Device into General Discoverablity Mode.  Once in this
// mode the Device will respond to Inquiry Scans from other Bluetooth
// Devices.  This function requires that a valid Bluetooth Stack ID
// exists before running.  This function returns zero on successful
// execution and a negative value if an error occurred.
static int _set_discoverable( unsigned int bt_stack_id )
{
  // First, check that a valid Bluetooth Stack ID exists.
  if( !bt_stack_id )
    return INVALID_STACK_ID_ERROR;
  
  if( GAP_Set_Discoverability_Mode(bt_stack_id, dmGeneralDiscoverableMode, 0) )
    return -1;
  else
    return 0;
}

// The following function is responsible for placing the local
// Bluetooth device into Pairable mode.  Once in this mode the device
// will response to pairing requests from other Bluetooth devices.
// This function returns zero on successful execution and a negative
// value on all errors.
static int _set_pairable( unsigned int bt_stack_id )
{
  // First, check that a valid Bluetooth Stack ID exists.
  if( !bt_stack_id )
    return INVALID_STACK_ID_ERROR;

  // Attempt to set the attached device to be pairable.
  if(GAP_Set_Pairability_Mode( bt_stack_id , pmPairableMode ))
    return -1;
  
  // The device has been set to pairable mode, now register an Authentication 
  // Callback to handle the Authentication events
  if(GAP_Register_Remote_Authentication(bt_stack_id, gap_event_callback, (unsigned long)0))
    return -1;
  
  return 0;
}



static void _init_gap_value( void )
{
  ASSIGN_BD_ADDR( gap_null_bd_addr , 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);
  
  // Initialize the default Secure Simple Pairing parameters.
  gap_context.io_capability     =       DEFAULT_IO_CAPABILITY;
  gap_context.oob_support       =       FALSE;
  gap_context.mitm_protection   =       DEFAULT_MITM_PROTECTION;
  
  _gap_request_linkkey(GAP_SRC_PORT,GAP_DEST_PORT,0);

  gap_context.nb_key_in_table = 0;
  gap_context.current_linkkey_index = 0;
    
  _gap_tempo = 0;
  _gap_state = enGAP_INIT;

}


int gap_init( unsigned int bt_stack_id )
{

  if( !bt_stack_id )
    return INVALID_STACK_ID_ERROR;

  _init_gap_value( );
  
  if(_set_connectable( BluetoothStackID ) < 0)
    return -1;
  
  if( _set_discoverable( BluetoothStackID ) < 0)
    return -1;
  
  if( _set_pairable( BluetoothStackID ) < 0)
    return -1;

  return 0;
}

// The following function is responsible for setting the name of the
// local Bluetooth Device to a specified name.  This function returns
// zero on successful execution and a negative value on all errors.
int gap_set_local_name( unsigned int bt_stack_id, char * arg )
{
   if( !bt_stack_id )
     return INVALID_STACK_ID_ERROR;

   if( !arg )
     return INVALID_PARAMETERS_ERROR;
   
   if(strlen( arg ) > MAX_NAME_LENGTH )
     arg[MAX_NAME_LENGTH-1] = 0;
   
   if(GAP_Set_Local_Device_Name( bt_stack_id , arg ))
     return FUNCTION_ERROR;

   return 0;
}

// The following function is responsible for querying the Bluetooth
// Device Name of the specified remote Bluetooth Device.  This
// function returns zero on successful execution and a negative value
// on all errors.

int gap_get_remote_name( unsigned int bt_stack_id , BD_ADDR_t BD_ADDR )
{
  // First, check that a valid Bluetooth Stack ID exists.
  if( !bt_stack_id )
    return INVALID_STACK_ID_ERROR;  
  
  // Attempt to submit the command.
  if(GAP_Query_Remote_Device_Name(bt_stack_id, BD_ADDR , gap_event_callback, (unsigned long)0))
    return -1;
  
  return 0;
}

void gap_base_time_1ms( void )
{
  if(_gap_tempo)
    _gap_tempo--;
}

void gap_task_handler( void * param )
{
  switch( _gap_state )
  {
  
  case enGAP_INIT:
    //nothing to do...
      break;
  
  case enGAP_TEMPO:
    if(_gap_tempo == 0)
      _gap_state = enGAP_SENDPINCODE;
      break;
  
  case enGAP_SENDPINCODE:
        //Send a PIN Code Response.
    _pin_code_response( BluetoothStackID );
    _gap_state = enGAP_INIT;
      break;
      
  }
}

void gap_push_bt_link_key( char * ptr )
{
  if(gap_context.nb_key_in_table >= MAX_SUPPORTED_LINK_KEYS)
    return;
  
  BTPS_MemCopy(&gap_context.LinkKeyInfo[gap_context.nb_key_in_table],ptr,sizeof(LinkKeyInfo_t));    
  
  gap_context.nb_key_in_table++;
}

#include "SS1BTPS.h"
#include "BTPSKRNL.h"
#include "cardicell_board.h"
#include "cardicell_app.h"
#include "spp.h"
#include "gap.h"
#include "utils.h"
#include "EHCILL.h"

#define MAX_SPP_PORT    16
   /* Internal Variables to this Module (Remember that all variables    */
   /* declared static are initialized to 0 automatically by the         */
   /* compiler as part of standard C/C++).                              */

unsigned int        BluetoothStackID;        /* Variable which holds the Handle */
                                                    /* of the opened Bluetooth Protocol*/
                                                    /* Stack.                          */

static int                 SerialPortID;            /* Variable which contains the     */
                                                    /* Handle of the most recent       */
                                                    /* SPP Port that was opened.       */

static int                 ServerPortID[MAX_SPP_PORT];            /* Variable which contains the     */
                                                    /* Handle of the SPP Server Port   */
                                                    /* that was opened.                */

static Word_t              Connection_Handle;       /* Holds the Connection Handle of  */
                                                    /* the most recent SPP Connection. */

Boolean_t           Connected;               /* Variable which flags whether or */
                                                    /* not there is currently an active*/
                                                    /* connection.                     */

static DWord_t             SPPServerSDPHandle[MAX_SPP_PORT];      /* Variable used to hold the Serial*/
                                                    /* Port Service Record of the      */
                                                    /* Serial Port Server SDP Service  */
                                                    /* Record.                         */

static LinkKeyInfo_t       LinkKeyInfo[MAX_SUPPORTED_LINK_KEYS]; /* Variable holds     */
                                                    /* BD_ADDR <-> Link Keys for       */
                                                    /* pairing.                        */

static BD_ADDR_t           SelectedBD_ADDR;         /* Holds address of selected Device*/

static BoardStr_t          Callback_BoardStr;       /* Holds a BD_ADDR string in the   */
                                                    /* Callbacks.                      */

static BD_ADDR_t        remoteConnectedDevice;

static char             _closed_port_indication;

static char             BluetoothAddress[15]; // Address of the chipset in string format.

static char             board_name[32]; // published SPP name.

static unsigned long    spp_tempo;


#ifndef DMA_APPROACH
static void _send_data( void );
#endif
static char debug_buffer[256];

static unsigned char _need_to_close_stack;

static enSPP_STATE _spp_state = SPP_UNINIT;

   /* The following string table is used to map HCI Version information */
   /* to an easily displayable version string.                          */
static BTPSCONST char *HCIVersionStrings[] =
{
   "1.0b",
   "1.1",
   "1.2",
   "2.0",
   "2.1",
   "3.0",
   "4.0",
   "Unknown (greater 4.0)"
} ;

#define NUM_SUPPORTED_HCI_VERSIONS              (sizeof(HCIVersionStrings)/sizeof(char *) - 1)

#define SPP_BUFFER_SIZE         300

// Internal function prototypes.


static int _delete_link_key(BD_ADDR_t BD_ADDR);

static int _set_config_params(Word_t MaximumFrameSize, 
                              unsigned int TransmitBufferSize , 
                              unsigned int ReceiveBufferSize );

static void _reset_spp_data( void );

static int _set_baud_rate( double baudrate );

static int _open_stack(HCI_DriverInformation_t *HCI_DriverInformation);

static int _close_stack( void );

static int _start_server( int comm_port );

static int _open_server( unsigned int port );


// BTPS Callback function prototypes.
static void BTPSAPI SPP_Event_Callback(unsigned int BluetoothStackID,
                                       SPP_Event_Data_t *SPP_Event_Data,
                                       unsigned long CallbackParameter);

static void BTPSAPI HCI_Event_Callback(unsigned int BluetoothStackID, 
                                       HCI_Event_Data_t *HCI_Event_Data, 
                                       unsigned long CallbackParameter);

/*----------------------------------------------------------------------------*/

// The following function is responsible for opening the SS1         
// Bluetooth Protocol Stack.  This function accepts a pre-populated  
// HCI Driver Information structure that contains the HCI Driver     
// Transport Information.  This function returns zero on successful  
// execution and a negative value on all errors.                     

static int _open_stack(HCI_DriverInformation_t *HCI_DriverInformation)
{
   int                        Result;
   int                        ret_val = 0;
   Byte_t                     Status;
   BD_ADDR_t                  BD_ADDR;
   HCI_Version_t              HCIVersion;
   L2CA_Link_Connect_Params_t L2CA_Link_Connect_Params;

   /* First check to see if the Stack has already been opened.          */
   if(!BluetoothStackID)
   {
      /* Next, makes sure that the Driver Information passed appears to */
      /* be semi-valid.                                                 */
      if(HCI_DriverInformation)
      {
         drv_debug_log("\r\n");

         drv_debug_log("_open_stack().\r\n");

         /* Initialize the Stack                                        */
         Result = BSC_Initialize(HCI_DriverInformation, 0);

         /* Next, check the return value of the initialization to see if*/
         /* it was successful.                                          */
         if(Result > 0)
         {
            /* The Stack was initialized successfully, inform the user  */
            /* and set the return value of the initialization function  */
            /* to the Bluetooth Stack ID.                               */
            BluetoothStackID = Result;
            
            SprintF(debug_buffer,"Bluetooth Stack ID: %d\r\n", BluetoothStackID);
            drv_debug_log(debug_buffer);
            
            if(!HCI_Version_Supported(BluetoothStackID, &HCIVersion))
            {
              SprintF(debug_buffer,"Device Chipset: %s\r\n", (HCIVersion <= NUM_SUPPORTED_HCI_VERSIONS)?HCIVersionStrings[HCIVersion]:HCIVersionStrings[NUM_SUPPORTED_HCI_VERSIONS]);
              drv_debug_log(debug_buffer);
            }
            
            /* Let's output the Bluetooth Device Address so that the    */
            /* user knows what the Device Address is.                   */
            if(!GAP_Query_Local_BD_ADDR(BluetoothStackID, &BD_ADDR))
            {
               BD_ADDRToStr(BD_ADDR, BluetoothAddress);
				SprintF(debug_buffer,"BD_ADDR: %s\r\n", BluetoothAddress);
				drv_debug_log(debug_buffer);
            }

            /* Go ahead and allow Master/Slave Role Switch.             */
            L2CA_Link_Connect_Params.L2CA_Link_Connect_Request_Config  = cqAllowRoleSwitch;
            L2CA_Link_Connect_Params.L2CA_Link_Connect_Response_Config = csMaintainCurrentRole;

            L2CA_Set_Link_Connection_Configuration(BluetoothStackID, &L2CA_Link_Connect_Params);

            if(HCI_Command_Supported(BluetoothStackID, HCI_SUPPORTED_COMMAND_WRITE_DEFAULT_LINK_POLICY_BIT_NUMBER) > 0)
               HCI_Write_Default_Link_Policy_Settings(BluetoothStackID, (HCI_LINK_POLICY_SETTINGS_ENABLE_MASTER_SLAVE_SWITCH|HCI_LINK_POLICY_SETTINGS_ENABLE_SNIFF_MODE), &Status);

            /* Delete all Stored Link Keys.                             */
            ASSIGN_BD_ADDR(BD_ADDR, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);

            _delete_link_key(BD_ADDR);
         }
         else
         {
            /* The Stack was NOT initialized successfully, inform the   */
            /* user and set the return value of the initialization      */
            /* function to an error.                                    */
            drv_debug_log("Stack Init Failed");

            BluetoothStackID = 0;

            ret_val          = UNABLE_TO_INITIALIZE_STACK;
         }
      }
      else
      {
         /* One or more of the necessary parameters are invalid.        */
         ret_val = INVALID_PARAMETERS_ERROR;
      }
   }

   return(ret_val);
}

// The following function is responsible for closing the SS1
// Bluetooth Protocol Stack.  This function requires that the
// Bluetooth Protocol stack previously have been initialized via the
// _open_stack() function.  This function returns zero on successful
// execution and a negative value on all errors.
static int _close_stack(void)
{
  // First check to see if the Stack has been opened.
  if(BluetoothStackID == 0)
  {
    drv_debug_log(("error, BluetoothStackID is null.\r\n"));
    
    // A valid Stack ID does not exist, inform to user.
    return UNABLE_TO_INITIALIZE_STACK;
  }
  
  // Simply close the Stack
  BSC_Shutdown(BluetoothStackID);
  
  // Free BTPSKRNL allocated memory.
  // We can keep BTPS initialized.
  //BTPS_DeInit();
  
  drv_debug_log(("Stack Shutdown.\r\n"));
  
  // Flag that the Stack is no longer initialized.
  BluetoothStackID = 0;
  
  return 0;
}




// The following function is a utility function that exists to delete
// the specified Link Key from the Local Bluetooth Device.  If a NULL
// Bluetooth Device Address is specified, then all Link Keys will be 
// deleted.                                                          
static int _delete_link_key( BD_ADDR_t BD_ADDR )
{
   int       Result;
   Byte_t    Status_Result;
   Word_t    Num_Keys_Deleted = 0;
   BD_ADDR_t NULL_BD_ADDR;

   Result = HCI_Delete_Stored_Link_Key(BluetoothStackID, BD_ADDR, TRUE, &Status_Result, &Num_Keys_Deleted);

   /* Any stored link keys for the specified address (or all) have been */
   /* deleted from the chip.  Now, let's make sure that our stored Link */
   /* Key Array is in sync with these changes.                          */

   /* First check to see all Link Keys were deleted.                    */
   ASSIGN_BD_ADDR(NULL_BD_ADDR, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);

   if(COMPARE_BD_ADDR(BD_ADDR, NULL_BD_ADDR))
      BTPS_MemInitialize(LinkKeyInfo, 0, sizeof(LinkKeyInfo));
   else
   {
      /* Individual Link Key.  Go ahead and see if know about the entry */
      /* in the list.                                                   */
      for(Result=0;(Result<sizeof(LinkKeyInfo)/sizeof(LinkKeyInfo_t));Result++)
      {
         if(COMPARE_BD_ADDR(BD_ADDR, LinkKeyInfo[Result].BD_ADDR))
         {
            LinkKeyInfo[Result].BD_ADDR = NULL_BD_ADDR;

            break;
         }
      }
   }

   return(Result);
}

// The following function is responsible for opening a Serial Port
// Server on the Local Device.  This function opens the Serial Port
// Server on the specified RFCOMM Channel.  This function returns
// zero if successful, or a negative return value if an error
// occurred.
static int _open_server( unsigned int port ) 
{
   int  ret_val;
   char *ServiceName;

   if(port > MAX_SPP_PORT-1 )
     return FUNCTION_ERROR;
   
   // First check to see if a valid Bluetooth Stack ID exists.
   if(BluetoothStackID)
   {
      // Make sure that there is not already a Serial Port Server open.
      if(!ServerPortID[port])
      {
         // Next, check to see if the parameters specified are valid.
         if( port > 0 )
         {
            // Simply attempt to open an Serial Server, on RFCOMM Server Port 1.
            ret_val = SPP_Open_Server_Port(BluetoothStackID, port, SPP_Event_Callback, (unsigned long)0);

            /* If the Open was successful, then note the Serial Port    */
            /* Server ID.                                               */
            if(ret_val > 0)
            {
               /* Note the Serial Port Server ID of the opened Serial   */
               /* Port Server.                                          */
               ServerPortID[port] = ret_val;

               // Create a Buffer to hold the Service Name.
               if((ServiceName = BTPS_AllocateMemory(64)) != NULL)
               {
                  /* The Server was opened successfully, now register a */
                  /* SDP Record indicating that an Serial Port Server   */
                  /* exists. Do this by first creating a Service Name.  */
                  BTPS_SprintF(ServiceName, "SPP port %d", port);

                  /* Now that a Service Name has been created try to    */
                  /* Register the SDP Record.                           */
                  ret_val = SPP_Register_SDP_Record(BluetoothStackID, 
                                                    ServerPortID[port], 
                                                    NULL, 
                                                    ServiceName, 
                                                    &SPPServerSDPHandle[port]);

                  /* If there was an error creating the Serial Port     */
                  /* Server's SDP Service Record then go ahead an close */
                  /* down the server an flag an error.                  */
                  if(ret_val < 0)
                  {
                    SprintF(debug_buffer,"Unable to Register Server SDP Record, Error = %d.\r\n", ret_val);
                    drv_debug_log(debug_buffer);
                    SPP_Close_Server_Port(BluetoothStackID, ServerPortID[port]);

                     /* Flag that there is no longer an Serial Port     */
                     /* Server Open.                                    */
                     ServerPortID[port] = 0;

                     /* Flag that we are no longer connected.           */
                     Connected    = FALSE;

                     ret_val      = UNABLE_TO_REGISTER_SERVER;
                  }
                  else
                  {
                    // Simply flag to the user that everything initialized correctly.
                    SprintF(debug_buffer,"Server Opened: %d.\r\n", port);
                    drv_debug_log(debug_buffer);
                    
                    // Flag success to the caller.
                    ret_val = 0;
                  }

                  // Free the Service Name buffer.
                  BTPS_FreeMemory(ServiceName);
               }
               else
               {
                  drv_debug_log("Failed to allocate buffer to hold Service Name in SDP Record.\r\n");
               }
               
#ifdef __FIX_PACKET_LOSS__
               if( SPP_Change_Buffer_Size(BluetoothStackID,ServerPortID[port], 512 , 512) >= 0)
               {
                 drv_debug_log("Change buffer succedded.\n\r");
               }
               else
               {
                 drv_debug_log("Change buffer failed.\n\r");
               }
#endif
            }
            else
            {
				SprintF(debug_buffer,"Unable to Open Server on: %d, Error = %d.\r\n", port, ret_val);
               drv_debug_log(debug_buffer);
               ret_val = UNABLE_TO_REGISTER_SERVER;
            }
         }
         else
         {
            ret_val = INVALID_PARAMETERS_ERROR;
         }
      }
      else
      {
         // A Server is already open, this program only supports one Server or Client at a time.                                 */
         drv_debug_log("Server already open.\r\n");

         ret_val = FUNCTION_ERROR;
      }
   }
   else
   {
      // No valid Bluetooth Stack ID exists.
      ret_val = INVALID_STACK_ID_ERROR;
   }

   return(ret_val);
}

// The following function is responsible for closing a Serial Port
// Server that was previously opened via a successful call to the
// OpenServer() function.  This function returns zero if successful
// or a negative return error code if there was an error.
int spp_close_server( unsigned char port )
{
  int ret_val = 0;
  
  if(port > MAX_SPP_PORT -1)
    return FUNCTION_ERROR;
  
  // First check to see if a valid Bluetooth Stack ID exists.
  if(BluetoothStackID <= 0)
    return INVALID_STACK_ID_ERROR;
  
  // If a Serial Port Server is already opened, then simply close it
  if(ServerPortID[port])
  {
    /* If there is an SDP Service Record associated with the Serial*/
    /* Port Server then we need to remove it from the SDP Database.*/
    if(SPPServerSDPHandle[port])
    {
      SPP_Un_Register_SDP_Record(BluetoothStackID, SerialPortID, SPPServerSDPHandle[port]);
      
      /* Flag that there is no longer an SDP Serial Port Server   */
      /* Record.                                                  */
      SPPServerSDPHandle[port] = 0;
    }
    
    /* Finally close the Serial Port Server.                       */
    ret_val = SPP_Close_Server_Port(BluetoothStackID, ServerPortID[port]);
    
    if(ret_val < 0)
    {
      drv_debug_log("SPP_Close_Server_Port failed\n");
      
      ret_val = FUNCTION_ERROR;
    }
    else
      ret_val = 0;
    
    /* Flag that there is no Serial Port Server currently open.    */
    SerialPortID         = 0;
    
    /* Flag that we are no longer connected.                       */
    Connected    = FALSE;
    
    drv_gpio_clear_connected();

    drv_debug_log( "spp disconnected.\n\r" );
    
    drv_debug_log("Server Closed.\r\n");
  }
  else
  {
    drv_debug_log("NO Server open.\r\n");
    
    ret_val = INVALID_PARAMETERS_ERROR;
  }
  return(ret_val);
}

// The following function is responsible for setting the current     
// configuration parameters that are used by SPP.  This function will
// return zero on successful execution and a negative value on       
// errors.                                                           
static int _set_config_params(Word_t MaximumFrameSize, 
                           unsigned int TransmitBufferSize , 
                           unsigned int ReceiveBufferSize )
{
   int                        ret_val;
   SPP_Configuration_Params_t SPPConfigurationParams;

   /* First check to see if the parameters required for the execution of*/
   /* this function appear to be semi-valid.                            */
   if(BluetoothStackID)
   {
         /* Parameters have been specified, go ahead and write them to  */
         /* the stack.                                                  */
         SPPConfigurationParams.MaximumFrameSize   = (unsigned int) MaximumFrameSize;
         SPPConfigurationParams.TransmitBufferSize = (unsigned int) TransmitBufferSize;
         SPPConfigurationParams.ReceiveBufferSize  = (unsigned int) ReceiveBufferSize;

         ret_val = SPP_Set_Configuration_Parameters(BluetoothStackID, &SPPConfigurationParams);

         if(ret_val >= 0)
         {
#if 0
            Display(("SPP_Set_Configuration_Parameters(): Success\r\n", ret_val));
            Display(("   MaximumFrameSize   : %d (0x%X)\r\n", SPPConfigurationParams.MaximumFrameSize, SPPConfigurationParams.MaximumFrameSize));
            Display(("   TransmitBufferSize : %d (0x%X)\r\n", SPPConfigurationParams.TransmitBufferSize, SPPConfigurationParams.TransmitBufferSize));
            Display(("   ReceiveBufferSize  : %d (0x%X)\r\n", SPPConfigurationParams.ReceiveBufferSize, SPPConfigurationParams.ReceiveBufferSize));
#endif
            /* Flag success.                                            */
            ret_val = 0;
         }
         else
         {
#if 0
            /* Error setting the current parameters.                    */
            Display(("SPP_Set_Configuration_Parameters(): Error %d.\r\n", ret_val));
#endif
            ret_val = FUNCTION_ERROR;
         }
   }
   else
   {
      /* One or more of the necessary parameters are invalid.           */
      ret_val = INVALID_PARAMETERS_ERROR;
   }

   return(ret_val);
}


//    The following thread is responsible for checking changing the     
//    current Baud Rate used to talk to the Radio.                      
//    * NOTE * This function ONLY configures the Baud Rate for a TI     
//             Bluetooth chipset.                                       
static int _set_baud_rate( double new_baudrate )
{
   int                              ret_val;
   Byte_t                           Length;
   Byte_t                           Status;
   NonAlignedDWord_t                _BaudRate;

   union
   {
      Byte_t                        Buffer[16];
      HCI_Driver_Reconfigure_Data_t DriverReconfigureData;
   } Data;

   /* First check to see if the parameters required for the execution of*/
   /* this function appear to be semi-valid.                            */
   if(BluetoothStackID)
   {
      /* Next check to see if the parameters required for the execution */
      /* of this function appear to be semi-valid.                      */
      if( new_baudrate > 0 )
      {
            /* Write the Baud Rate.                                     */
            ASSIGN_HOST_DWORD_TO_LITTLE_ENDIAN_UNALIGNED_DWORD(&_BaudRate, new_baudrate);

            /* Next, write the command to the device.                   */
            Length  = sizeof(Data.Buffer);
            ret_val = HCI_Send_Raw_Command(BluetoothStackID, 0x3F, 0x0336, sizeof(NonAlignedDWord_t), (Byte_t *)&_BaudRate, &Status, &Length, Data.Buffer, TRUE);

            if((!ret_val) && (!Status))
            {
               /* We were successful, now we need to change the baud    */
               /* rate of the driver.                                   */
               BTPS_MemInitialize(&(Data.DriverReconfigureData), 0, sizeof(HCI_Driver_Reconfigure_Data_t));

               Data.DriverReconfigureData.ReconfigureCommand = HCI_COMM_DRIVER_RECONFIGURE_DATA_COMMAND_CHANGE_PARAMETERS;
               Data.DriverReconfigureData.ReconfigureData    = (void *)&_BaudRate;

               ret_val = HCI_Reconfigure_Driver(BluetoothStackID, FALSE, &(Data.DriverReconfigureData));

               if(ret_val >= 0)
               {
				   SprintF(debug_buffer,"HCI_Reconfigure_Driver(%lu): Success.\r\n", _BaudRate);
				   drv_debug_log(debug_buffer);

                  /* Flag success.                                      */
                  ret_val = 0;
               }
               else
               {
				   SprintF(debug_buffer,"HCI_Reconfigure_Driver(%lu): Failure %d.\r\n", _BaudRate, ret_val);
				   drv_debug_log(debug_buffer);
				   ret_val = FUNCTION_ERROR;
               }
            }
            else
            {
				/* Unable to write vendor specific command to chipset.   */
				SprintF(debug_buffer,"HCI_Send_Raw_Command(%lu): Failure %d, %d.\r\n", _BaudRate, ret_val, Status);
				drv_debug_log(debug_buffer);
				ret_val = FUNCTION_ERROR;
            }
      }
      else
      {
         drv_debug_log("SetBaudRate [BaudRate]");

         ret_val = INVALID_PARAMETERS_ERROR;
      }
   }
   else
   {
      /* One or more of the necessary parameters are invalid.           */
      ret_val = INVALID_PARAMETERS_ERROR;
   }

   return(ret_val);
}

   /*********************************************************************/
   /*                         Event Callbacks                           */
   /*********************************************************************/


   /* The following function is for an SPP Event Callback.  This        */
   /* function will be called whenever a SPP Event occurs that is       */
   /* associated with the Bluetooth Stack.  This function passes to the */
   /* caller the SPP Event Data that occurred and the SPP Event Callback*/
   /* Parameter that was specified when this Callback was installed.    */
   /* The caller is free to use the contents of the SPP SPP Event Data  */
   /* ONLY in the context of this callback.  If the caller requires the */
   /* Data for a longer period of time, then the callback function MUST */
   /* copy the data into another Data Buffer.  This function is         */
   /* guaranteed NOT to be invoked more than once simultaneously for the*/
   /* specified installed callback (i.e.  this function DOES NOT have be*/
   /* reentrant).  It Needs to be noted however, that if the same       */
   /* Callback is installed more than once, then the callbacks will be  */
   /* called serially.  Because of this, the processing in this function*/
   /* should be as efficient as possible.  It should also be noted that */
   /* this function is called in the Thread Context of a Thread that the*/
   /* User does NOT own.  Therefore, processing in this function should */
   /* be as efficient as possible (this argument holds anyway because   */
   /* another SPP Event will not be processed while this function call  */
   /* is outstanding).                                                  */
   /* * NOTE * This function MUST NOT Block and wait for Events that    */
   /*          can only be satisfied by Receiving SPP Event Packets.  A */
   /*          Deadlock WILL occur because NO SPP Event Callbacks will  */
   /*          be issued while this function is currently outstanding.  */
static void BTPSAPI SPP_Event_Callback(unsigned int BluetoothStackID, SPP_Event_Data_t *SPP_Event_Data, unsigned long CallbackParameter)
{
   int       ret_val = 0;

   static char  mybuffer[SPP_BUFFER_SIZE];
   unsigned int   lenghtRxSPPdata;
   unsigned int   currentLenghtRxSPP;

   /* **** SEE SPPAPI.H for a list of all possible event types.  This   */
   /* program only services its required events.                   **** */

   /* First, check to see if the required parameters appear to be       */
   /* semi-valid.                                                       */
   if((SPP_Event_Data) && (BluetoothStackID))
   {
      /* The parameters appear to be semi-valid, now check to see what  */
      /* type the incoming event is.                                    */
      switch(SPP_Event_Data->Event_Data_Type)
      {
         case etPort_Data_Indication:
           lenghtRxSPPdata = SPP_Event_Data->Event_Data.SPP_Data_Indication_Data->DataLength;
           
           while(lenghtRxSPPdata > 0)
           {
             if(lenghtRxSPPdata > SPP_BUFFER_SIZE)
               currentLenghtRxSPP = SPP_Data_Read(BluetoothStackID, SerialPortID, SPP_BUFFER_SIZE, (Byte_t*)&mybuffer);
             else
               currentLenghtRxSPP = SPP_Data_Read(BluetoothStackID, SerialPortID, lenghtRxSPPdata, (Byte_t*)&mybuffer);
             
             if(currentLenghtRxSPP > 0)
             {
                 drv_main_interface_transmit_buffer( (const char*)mybuffer, currentLenghtRxSPP );
                 if(lenghtRxSPPdata > currentLenghtRxSPP)
                   lenghtRxSPPdata-= currentLenghtRxSPP;
                 else
                   lenghtRxSPPdata = 0;
             }
             else
             {
               lenghtRxSPPdata = 0;
             }
           }
           break;
           
           
         case etPort_Open_Indication:
            // A remote port is requesting a connection.
            BD_ADDRToStr(SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR, Callback_BoardStr);
            drv_debug_log("\r\n");
            SprintF(debug_buffer,"SPP Open Indication, ID: 0x%04X, Board: %s.\r\n", SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->SerialPortID, Callback_BoardStr);
            drv_debug_log(debug_buffer);

            // Flag that we are now connected.
            Connected    = TRUE;
            
            remoteConnectedDevice.BD_ADDR0 = SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR.BD_ADDR0;
            remoteConnectedDevice.BD_ADDR1 = SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR.BD_ADDR1;
            remoteConnectedDevice.BD_ADDR2 = SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR.BD_ADDR2;
            remoteConnectedDevice.BD_ADDR3 = SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR.BD_ADDR3;
            remoteConnectedDevice.BD_ADDR4 = SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR.BD_ADDR4;
            remoteConnectedDevice.BD_ADDR5 = SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR.BD_ADDR5;
            
            /* Save the Serial Port ID for later use.                   */
            SerialPortID = SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->SerialPortID;


            /* Query the connection handle.                             */
            ret_val = GAP_Query_Connection_Handle(BluetoothStackID, SPP_Event_Data->Event_Data.SPP_Open_Port_Indication_Data->BD_ADDR, &Connection_Handle);

            if(ret_val)
            {
               /* Failed to Query the Connection Handle.                */
               drv_debug_log("GAP_Query_Connection_Handle()Failed\n\r");

               ret_val           = 0;
               Connection_Handle = 0;
            }
            
            drv_debug_log("SPP Connected\n\r" );            
            
            spp_get_remote_name( );
            
            break;
            
            
         case etPort_Open_Confirmation:
            /* A Client Port was opened.  The Status indicates the      */
            /* Status of the Open.                                      */
            drv_debug_log(("\r\n"));
            SprintF(debug_buffer,"SPP Open Confirmation, ID: 0x%04X, Status 0x%04X.\r\n", SPP_Event_Data->Event_Data.SPP_Open_Port_Confirmation_Data->SerialPortID,
                                                                              SPP_Event_Data->Event_Data.SPP_Open_Port_Confirmation_Data->PortOpenStatus);
			drv_debug_log(debug_buffer);

            /* Check the Status to make sure that an error did not      */
            /* occur.                                                   */
            if(SPP_Event_Data->Event_Data.SPP_Open_Port_Confirmation_Data->PortOpenStatus)
            {
               /* An error occurred while opening the Serial Port so    */
               /* invalidate the Serial Port ID.                        */
               SerialPortID      = 0;
               Connection_Handle = 0;

               /* Flag that we are no longer connected.                 */
               Connected    = FALSE;
            }
            else
            {
              
               /* Flag that we are now connected.                       */
               Connected = TRUE;
               
               _reset_spp_data();

               /* Query the connection Handle.                          */
               ret_val = GAP_Query_Connection_Handle(BluetoothStackID, SelectedBD_ADDR, &Connection_Handle);
               if(ret_val)
               {
                  /* Failed to Query the Connection Handle.             */
                  drv_debug_log("GAP_Query_Connection_Handle()failed \n\r");

                  ret_val           = 0;
                  Connection_Handle = 0;
               }
            }
            break;
            
            
         case etPort_Close_Port_Indication:
            /* The Remote Port was Disconnected.                        */
			drv_debug_log(("\r\n"));
            SprintF(debug_buffer,"SPP Close Port, ID: 0x%04X\r\n", SPP_Event_Data->Event_Data.SPP_Close_Port_Indication_Data->SerialPortID);
			drv_debug_log(debug_buffer);

            drv_debug_log("spp client disconnected\n\r");
               
            //SerialPortID = 0; TODO: tester
            Connection_Handle    = 0;

            /* Flag that we are no longer connected.                    */
            Connected = FALSE;
            
            _closed_port_indication = TRUE;
            
            break;
            
            
         case etPort_Status_Indication:
            /* Display Information about the new Port Status.           */
			drv_debug_log(("\r\n"));
            SprintF(debug_buffer,"SPP Port Status Indication: 0x%04X, Status: 0x%04X, Break Status: 0x%04X, Length: 0x%04X.\r\n", SPP_Event_Data->Event_Data.SPP_Port_Status_Indication_Data->SerialPortID,
                                                                                                                    SPP_Event_Data->Event_Data.SPP_Port_Status_Indication_Data->PortStatus,
                                                                                                                    SPP_Event_Data->Event_Data.SPP_Port_Status_Indication_Data->BreakStatus,
                                                                                                                    SPP_Event_Data->Event_Data.SPP_Port_Status_Indication_Data->BreakTimeout);
			drv_debug_log(debug_buffer);
            break;

            
         case etPort_Send_Port_Information_Indication:
           /* Simply Respond with the information that was sent to us. */
           drv_debug_log("\r\netPort_Send_Port_Information_Indication\n\r");
           ret_val = SPP_Respond_Port_Information(BluetoothStackID, SPP_Event_Data->Event_Data.SPP_Send_Port_Information_Indication_Data->SerialPortID, &SPP_Event_Data->Event_Data.SPP_Send_Port_Information_Indication_Data->SPPPortInformation);
           break;
      
      case etPort_Send_Port_Information_Confirmation:
        drv_debug_log("\r\netPort_Send_Port_Information_Confirmation , don't know what I need to do\n\r");
        break;
        
        
      case etPort_Query_Port_Information_Indication:
        drv_debug_log("\r\netPort_Query_Port_Information_Indication , don't know what I need to do\n\r");
        break;
        
        
      case etPort_Query_Port_Information_Confirmation:
        drv_debug_log("\r\netPort_Query_Port_Information_Confirmation , don't know what I need to do\n\r");
        break;
        
        
      case etPort_Open_Request_Indication:
        {
#if 0
          if( BTPSAPI SPP_Open_Port_Request_Response(BluetoothStackID, 
                                                     SerialPortID,
                                                     TRUE))
          {
            drv_debug_log("etPort_Open_Request_Indication , don't know what I need to do\n\r");
          }
#else
          drv_debug_log("etPort_Open_Request_Indication , don't know what I need to do\n\r");
#endif
        }
        break;
        
        
      case etPort_Transmit_Buffer_Empty_Indication:
        break;
        
        
         default:
           // An unknown/unexpected SPP event was received.
           drv_debug_log("\r\n");
           drv_debug_log("Unknown Event.\r\n");
           break;
      }

      /* Check the return value of any function that might have been    */
      /* executed in the callback.                                      */
      if(ret_val)
      {
         /* An error occurred, so output an error message.              */
         drv_debug_log("\r\n");
         drv_debug_log("Error \r\n");
      }
   }
   else
   {
      /* There was an error with one or more of the input parameters.   */
      drv_debug_log("Null Event\r\n");
   }
}

   /* The following function is responsible for processing HCI Mode     */
   /* change events.                                                    */
static void BTPSAPI HCI_Event_Callback(unsigned int BluetoothStackID, HCI_Event_Data_t *HCI_Event_Data, unsigned long CallbackParameter)
{
   char *Mode;

   /* Make sure that the input parameters that were passed to us are    */
   /* semi-valid.                                                       */
   if((BluetoothStackID) && (HCI_Event_Data))
   {
      /* Process the Event Data.                                        */
      switch(HCI_Event_Data->Event_Data_Type)
      {
         case etMode_Change_Event:
            if(HCI_Event_Data->Event_Data.HCI_Mode_Change_Event_Data)
            {
               switch(HCI_Event_Data->Event_Data.HCI_Mode_Change_Event_Data->Current_Mode)
               {
                  case HCI_CURRENT_MODE_HOLD_MODE:
                     Mode = "Hold";
                     break;
                  case HCI_CURRENT_MODE_SNIFF_MODE:
                     Mode = "Sniff";
                     break;
                  case HCI_CURRENT_MODE_PARK_MODE:
                     Mode = "Park";
                     break;
                  case HCI_CURRENT_MODE_ACTIVE_MODE:
                  default:
                     Mode = "Active";
                     break;
               }

               drv_debug_log("\r\n");
		SprintF(debug_buffer,"HCI Mode Change Event, Status: 0x%02X, Connection Handle: %d, Mode: %s, Interval: %d\r\n", HCI_Event_Data->Event_Data.HCI_Mode_Change_Event_Data->Status,
                                                                                                                    HCI_Event_Data->Event_Data.HCI_Mode_Change_Event_Data->Connection_Handle,
                                                                                                                    Mode,
                                                                                                                    HCI_Event_Data->Event_Data.HCI_Mode_Change_Event_Data->Interval);
		drv_debug_log(debug_buffer);
            }
            break;
      }
   }
}

// The following function is used to initialize the application
// instance.  This function should open the stack and prepare to
// execute commands based on user input.  The first parameter passed
// to this function is the HCI Driver Information that will be used
// when opening the stack and the second parameter is used to pass
// parameters to BTPS_Init.  This function returns the
// BluetoothStackID returned from BSC_Initialize on success or a
// negative error code (of the form APPLICATION_ERROR_XXX).
static int _init_bluetooth(HCI_DriverInformation_t *HCI_DriverInformation)
{
  int ret_val = APPLICATION_ERROR_UNABLE_TO_OPEN_STACK;
  
  memset((void *)&SerialPortID,0,sizeof(int)*MAX_SPP_PORT);
  
  // makes sure that the Driver Information passed appears to be
  // semi-valid.                                                      
  if((HCI_DriverInformation == NULL))
    return APPLICATION_ERROR_INVALID_PARAMETERS;
  
  // Try to Open the stack and check if it was successful.
  if(!_open_stack(HCI_DriverInformation))
  {
    ret_val = gap_init( BluetoothStackID );

    if(!ret_val)
    {
          // Attempt to register a HCI Event Callback.
          ret_val = HCI_Register_Event_Callback(BluetoothStackID, HCI_Event_Callback, (unsigned long)NULL);
          
          if(ret_val > 0)
          {
            // Return success to the caller.
            ret_val = (int)BluetoothStackID;
          }
          else
            drv_debug_log("HCI_Register_Event_Callback() failed\n\r");
    }
    else
    {
      drv_debug_log("\n\rError GAP init\n\r");
    }

    /* In some error occurred then close the stack.                */
    if(ret_val < 0)
    {
      /* Close the Bluetooth Stack.                               */
      _close_stack();
    }
  }
  else
  {
    /* There was an error while attempting to open the Stack.      */
    drv_debug_log("Unable to open the stack.\r\n");
  }
  
  return(ret_val);
}


// This function lauch Serial port profile as server mode.
// It will open a port and will wait for connection
int spp_open_stack( void )
{
  int Result;
  
  HCI_DriverInformation_t HCI_DriverInformation;
  
  if( BluetoothStackID == 0)
  {
    // Configure the UART Parameters.
    HCI_DRIVER_SET_COMM_INFORMATION(&HCI_DriverInformation, 1, 115200, cpUART);
    HCI_DriverInformation.DriverInformation.COMMDriverInformation.InitializationDelay = 300;

    // bt configure
    drv_configure_bt_enabled();

    // bt power on
    drv_bt_power_off();
    
    __delay_cycles(25000000);
    
    drv_bt_power_on();
    
    //delay (1 secondes)
    __delay_cycles(25000000);
    
    // Initialize the application.
    if((Result = _init_bluetooth(&HCI_DriverInformation)) <= 0)
      return -1;
    
    // Save the Bluetooth Stack ID.
    BluetoothStackID = (unsigned int)Result;
    
    // Go ahead an enable HCILL Mode.
    HCILL_Init();
    HCILL_Configure(BluetoothStackID, HCILL_MODE_INACTIVITY_TIMEOUT, HCILL_MODE_RETRANSMIT_TIMEOUT, TRUE);

#if 1    
    SprintF(board_name,"enova_%s", BluetoothAddress);
    spp_set_name( board_name );
#else
    spp_set_name("enova");
#endif
    _set_baud_rate( 921600 );
    //_set_baud_rate( 460800 );
    //_set_baud_rate( 230400 );
    //_set_baud_rate( 115200 );
    return BluetoothStackID;
  }
  
  return -1;
}


int _start_server( int comm_port )
{
  int ret = 0;
  
  drv_gpio_clear_connected();
  
  _closed_port_indication = FALSE;
  
  main_interface_reset_queue();

#if 1
  ret = _set_config_params( 50, 300 , 300 );
  
  if(ret >= 0 )  
    drv_debug_log("spp set config succedded...\n\r");
#endif
  
  ret = _open_server(comm_port);

  return ret;
}

int spp_set_name( char * arg )
{
  unsigned int len = strlen(arg);
  
  while(len > 0)
  {
    if(arg[len-1] == '\n')
      arg[len-1] = 0;
    len --;
  }
  
  return gap_set_local_name( BluetoothStackID, arg );
}


int spp_get_remote_name( void )
{
  if(Connected == TRUE)
  {
    return gap_get_remote_name( BluetoothStackID, remoteConnectedDevice );
  }
  
  return -1;
}


// Base time to delay the machine state
void spp_base_time_1ms( void )
{
  if(spp_tempo != 0)
    spp_tempo--;
}

// Lauch the machine state
void spp_init( void )
{
  if(_spp_state == SPP_UNINIT)
  {
    _spp_state = SPP_OPEN_STACK;
    _need_to_close_stack = 0;
    Connected = 0;
  }
}

void spp_stop( void )
{
  if(_spp_state == SPP_UNINIT)
    return;
  
  _need_to_close_stack = 1;
  
  drv_debug_log("need to close the stack\n\r");
}


void spp_task_handler( void * param )
{
   gap_task_handler( NULL );
   
  switch(_spp_state)
  {
  case SPP_UNINIT:
    break;

  case SPP_OPEN_STACK:
    if( spp_open_stack() <= 0 )
    {
      _spp_state = SPP_UNINIT;
      main_interface_communication_off();
      drv_debug_log("Open stack failed, return to UNINIT.\n\r");
    }
    else
      _spp_state = SPP_OPEN_PORT;
    
    if(_need_to_close_stack == 1)
    {
      //Close only the stack.
      _spp_state = SPP_CLOSE_STACK;      
    }
    break;


  case SPP_OPEN_PORT:
    if( _start_server( 1 ) >= 0 )
    {
      _spp_state = SPP_IDLE;
      drv_debug_log("Succedded to open spp port\n\r");
      main_interface_communication_on();
    }
    else
    {
      _spp_state = SPP_CLOSE_STACK;
      drv_debug_log("\n\rFailed to open spp port\n\r");
    }
    break;


  case SPP_IDLE:    
    if(Connected == TRUE)
    {
      _spp_state = SPP_CONNECTED;
      drv_gpio_set_connected();
    }

    if(_need_to_close_stack == 1)
    {
      // Here the stack and the server are opened
      // so we need to close them.
      _spp_state = SPP_TEMPO;
    }
    break;
        
  case SPP_CONNECTED:
    if( _closed_port_indication == TRUE )
    {
      drv_gpio_clear_connected();
      
      _closed_port_indication = FALSE;
      // Here there is a bug. Sometimes the server is blocked
      // so we need to close the port and restart the server.
      //_spp_state = SPP_TEMPO;
      //spp_tempo = 2000;
      _spp_state = SPP_IDLE;      
    }
    
    if(_need_to_close_stack == 1)
    {
      // Here the stack and the server are opened
      // so we need to close them.
      _spp_state = SPP_TEMPO;
    }
    break;
    
  case SPP_TEMPO:
    if(spp_tempo == 0)
      _spp_state = SPP_CLOSE_SERVER;
    break;
    
  case SPP_CLOSE_SERVER:
    drv_debug_log("Close port\n\r");
    if(spp_close_server( 1 ) == 0)
    {
      ServerPortID[1] = 0;
      drv_debug_log("Close port succedded\n\r");      
    }
    else
    {
      drv_debug_log("Close port failed\n\r");      
    }
    _spp_state = SPP_CLOSE_STACK;
#if 0
    if( spp_close_server( 1 ) == 0 )
    {
      ServerPortID = 0;
      _spp_state = SPP_OPEN_PORT;
    }else
    {
      _spp_state = SPP_CLOSE_STACK;
    }
#endif
    break;
    
  case SPP_CLOSE_STACK:
    drv_debug_log("\n\rShutdown\n\r");
    _close_stack( );
    drv_clock_disable_slow_clock();
    // power down
    drv_bt_power_off();
    // Uninit UART
    // put here
    _spp_state = SPP_UNINIT;
    main_interface_communication_off();
    _need_to_close_stack = 0;
    break;
  }
}


static void _reset_spp_data( void )
{
  if(BluetoothStackID != 0 && SerialPortID != 0)
  {
    SPP_Purge_Buffer( BluetoothStackID, SerialPortID, SPP_PURGE_MASK_TRANSMIT_ABORT_BIT | 
                   SPP_PURGE_MASK_RECEIVE_ABORT_BIT);
    SPP_Purge_Buffer( BluetoothStackID, SerialPortID, SPP_PURGE_MASK_TRANSMIT_FLUSH_BIT );
  }
  
  main_interface_reset_queue();  
  
}

#ifndef DMA_APPROACH
static void _send_data( void )
{
  int delta ;
  unsigned int lenght ;
  
  __disable_interrupt();
  
  // if no data in the buffer
  if( backupTxBuffer.push_index == backupTxBuffer.pop_index )
  {
    __enable_interrupt();  
    return ;
  }
  
  // if there is data in the buffer
  if( backupTxBuffer.push_index > backupTxBuffer.pop_index )
    delta = backupTxBuffer.push_index - backupTxBuffer.pop_index;
  else
    delta = MAX_BUFFER_SIZE - backupTxBuffer.pop_index;
  
  if(delta > 0 )
  {
    lenght = SPP_Data_Write(BluetoothStackID, SerialPortID, delta, &backupTxBuffer.TxSPPBuffer[backupTxBuffer.pop_index]);
    backupTxBuffer.pop_index+= lenght;
    backupTxBuffer.pop_index%=MAX_BUFFER_SIZE;
  }
  else
  {
    while(1);
  }
  
  __enable_interrupt();

}
#endif

// return the machine state current state
enSPP_STATE spp_get_state( void )
{
  return _spp_state;
}


unsigned int spp_send_data( char * ptr_data, unsigned int bytetosend )
{
  if(BluetoothStackID != 0)
    return SPP_Data_Write(BluetoothStackID, SerialPortID, bytetosend, (Byte_t *)ptr_data);
  else
    return 0;
}