#include "drivers/keycontrol/keycontrol_ll.h"
#include "drivers/keycontrol/keycontrol_ll_encoding.h"
#include "core/gpio.h"
#include "core/csect.h"
#include "core/config_pins.h"
#include "core/main.h" // HAL_Wait_us 
#include <stdint.h>
#include <stddef.h>

 #define SUPPORT_SERIAL_REGISTER_1               0
 #define SUPPORT_SERIAL_REGISTER_2               0

 // LowLevel KeyControl version ACM2543.1
 //
 //                 [3]    [4]    VDD    VSS                |  Logic Port Desc
 //    HEADER #1    (o)    (o)    (o)    (o)                |  PCB HOLE
 //                 *p1    *p2    *p3    *p4                |  Legend index
 //    -----------------------------------------------------+
 //                 VSS    VDD    [1]    [2]                |  Logic Port Desc
 //    HEADER #2    (o)    (o)    (o)    (o)                |  PCB HOLE
 //                 *p5    *p6    *p7    *p8                |  Legend index
 // Legend:
 //  p1 - ST_VCTRL1_ADRF3_1, #12, PA2, [Line3]               
 //  p2 - ST_VCTRL2_ADRF3_1, #11, PA1, [Line4]               
 //  p3 - VDD_ADRF3_1
 //  p4 - VSS_ADRF4_1
 //  p5 - VSS_ADRF3_1
 //  p6 - VDD_ADRF4_1
 //  p7 - ST_VCTRL1_ADRF4_1, #39, PB3, [Line1]
 //  p8 - ST_VCTRL2_ADRF4_1, #40, PB4, [Line2]

 static bool KeySwitch_Init( eKeySwitchControlMode_t mode );
 static bool KeySwitch_SetKeyCode( tKeySwitchCode_t code );
 static bool AmpSwitch_SetAmpCode( tKeySwitchCode_t code );
 static bool FiltSwitch_SetFiltCode( tKeySwitchCode_t code );
 static bool KeySwitch_SetKeyCodeParallel( tKeySwitchCode_t code );
 static bool KeySwitch_SetKeyCodeSerial( tKeySwitchCode_t code );
 static bool BandSwitch_SetBandCodeParallel( tKeySwitchCode_t code );
 static bool GainSwitch_SetGainCodeParallel( tKeySwitchCode_t code );
 #if SUPPORT_SERIAL_REGISTER_1
 static bool KeySwitch_SetKeyCodeSerial_Register1( tKeySwitchCode_t code );
 #endif
 #if SUPPORT_SERIAL_REGISTER_2
 static bool KeySwitch_SetKeyCodeSerial_Register2( tKeySwitchCode_t code );
 #endif
 static bool KeySwitch_GetKeyCode( tKeySwitchCode_t * pcode );
 static bool AmpSwitch_GetAmpCode( tKeySwitchCode_t * pcode );
 static bool FiltSwitch_GetFiltCode( tKeySwitchCode_t * pcode );
 static void KeySwitch_DeInit(); 
 static tKeySwitchCode_t  keyCode = 0;
 static bool bInitialized = false;
 static eKeySwitchControlMode_t xMode = eSwCtlMode_default;
 typedef enum
 {
    eLLKeySw_IOLine_1,
    eLLKeySw_IOLine_2,
    eLLKeySw_IOLine_3,
    eLLKeySw_IOLine_4,
    eLLKeySw_IOLine_5,
    eLLKeySw_IOLine_6,
 }
 e_LLKeySwitch_IOLines_t;

 #if SUPPORT_SERIAL_REGISTER_1
 #define Register1_IOLine_Shift  (eLLKeySw_IOLine_3)
 #define Register1_IOLine_Store  (eLLKeySw_IOLine_2)
 #define Register1_IOLine_Data   (eLLKeySw_IOLine_1)
 #endif
 #if SUPPORT_SERIAL_REGISTER_2
 #define Register2_IOLine_Shift  (eLLKeySw_IOLine_3)
 #define Register2_IOLine_Store  (eLLKeySw_IOLine_2)
 #define Register2_IOLine_Data   (eLLKeySw_IOLine_1)
 #endif

 static void LL_KeySwitch_SetIOLine( e_LLKeySwitch_IOLines_t port, bool state );

 const sKeySwitchControlLL_t KeyControlLLHandle = 
 {
     .Init = KeySwitch_Init, 
     .SetKeyCode = KeySwitch_SetKeyCode,
     .SetAmpCode = AmpSwitch_SetAmpCode,
     .SetFiltCode = FiltSwitch_SetFiltCode,
     .GetKeyCode = KeySwitch_GetKeyCode,
     .GetAmpCode = AmpSwitch_GetAmpCode,
     .GetFiltCode = FiltSwitch_GetFiltCode,
     .DeInit = KeySwitch_DeInit
 };
 
 static bool KeySwitch_Init( eKeySwitchControlMode_t mode )
 {    
    DI();

    xMode = mode;

    // Configure GPIO pins for KeySwitch control
    #if CONFIG_PORT__SINGLEPORTINIT
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    switch(xMode)
    {
       case eSwCtlMode_6line:
       GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L1|CONFIG_PIN__SWKEYPP_L2|CONFIG_PIN__SWKEYPP_L3|CONFIG_PIN__SWKEYPP_L4|CONFIG_PIN__SWKEYPP_L5|CONFIG_PIN__SWKEYPP_L6;
       break;
       case eSwCtlMode_4line:
       default:
       GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L1|CONFIG_PIN__SWKEYPP_L2|CONFIG_PIN__SWKEYPP_L3|CONFIG_PIN__SWKEYPP_L4;
       break;
    }
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L1, &GPIO_InitStruct);
    }
    #else
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L1;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L1, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L2;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L2, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L3;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L3, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L4;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L4, &GPIO_InitStruct);
    }
    if( xMode == eSwCtlMode_5line )
    {
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L5;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L5, &GPIO_InitStruct);
    }
    }
    if( xMode == eSwCtlMode_6line )
    {
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L5;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L5, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L6;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L6, &GPIO_InitStruct);
    }
    }
    #endif
    bInitialized = true;

    EI();

    return true;
 }

 static bool KeySwitch_SetKeyCode( tKeySwitchCode_t code )
 {
    // Check the control type:
    if( CHECK_CONTROL_TYPE_REGISTER( code ) )    
    {
        // Serial control (registers)
        return KeySwitch_SetKeyCodeSerial( code );
    }
    
    // Parallel control
    return KeySwitch_SetKeyCodeParallel( code );
    
 }
 
  static bool AmpSwitch_SetAmpCode( tKeySwitchCode_t code )
 {
    // Check the control type:
    if( CHECK_CONTROL_TYPE_REGISTER( code ) )    
    {
        // Serial control (registers)
        return KeySwitch_SetKeyCodeSerial( code );
    }
    
    // Parallel control
    return GainSwitch_SetGainCodeParallel( code );
    
 }
 
  static bool FiltSwitch_SetFiltCode( tKeySwitchCode_t code )
 {
    // Check the control type:
    if( CHECK_CONTROL_TYPE_REGISTER( code ) )
    {
        // Serial control (registers)
        return KeySwitch_SetKeyCodeSerial( code );
    }
    
    // Parallel control
    return BandSwitch_SetBandCodeParallel( code );
    
 }


 static bool KeySwitch_SetKeyCodeParallel( tKeySwitchCode_t code )
 {
    bool set = true;

    DI();

    if( bInitialized )
    {  
       // IOLine#1 = code.bit[6]
       // IOLine#2 = code.bit[7]
       // IOLine#3 = code.bit[8]
       // IOLine#4 = code.bit[9]
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_1, (code & (1ul<<6)) );
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_2, (code & (1ul<<7)) );
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_3, (code & (1ul<<8)) );
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_4, (code & (1ul<<9)) );
       if(xMode == eSwCtlMode_5line)
       {
         LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_5, (code & (1ul<<10)));
       }
       if(xMode == eSwCtlMode_6line )
       {
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_5, (code & (1ul<<10)));
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_6, (code & (1ul<<11)));       
       }
       keyCode = code;   
    }
    else
    {
       set = false;
    }

    EI();

    return set;
 }
 
   static bool BandSwitch_SetBandCodeParallel( tKeySwitchCode_t code )
 {
    bool set = true;

    DI();

    if( bInitialized )
    {
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_1, (code & (1ul<<6)) );  // CTRL-ADRFV1_ON
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_2, (code & (1ul<<7)) );  // CTRL-ADRFV2_ON
       keyCode = code;   
    }
    else
    {
       set = false;
    }

    EI();

    return set;
 }
 
 static bool GainSwitch_SetGainCodeParallel( tKeySwitchCode_t code )
 {
    bool set = true;

    DI();

    if( bInitialized )
    {
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_3, (code & (1ul<<8)) );   // CTRL-HMCV1_ON
       LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_4, (code & (1ul<<9)) );   // CTRL-HMCV2_ON
       if(xMode == eSwCtlMode_5line)
       {
         LL_KeySwitch_SetIOLine( eLLKeySw_IOLine_5, (code & (1ul<<10))); // CTRL-HMCV3_ON
       }
       keyCode = code;   
    }
    else
    {
       set = false;
    }

    EI();

    return set;
 }

 #if SUPPORT_SERIAL_REGISTER_1
 static bool KeySwitch_SetKeyCodeSerial_Register1( tKeySwitchCode_t code )
 {
    // Idle
    __DI__
    LL_KeySwitch_SetIOLine( Register1_IOLine_Shift, false );
    LL_KeySwitch_SetIOLine( Register1_IOLine_Store, false );
    LL_KeySwitch_SetIOLine( Register1_IOLine_Data,  false );
    __EI__
    
    // DataOut
    uint8_t value = GET_CONTROL_REGISTER_1( code ); 
    for( int32_t i = 7; i>=0; i-- )
    {
    LL_KeySwitch_SetIOLine( Register1_IOLine_Shift, false );
    LL_KeySwitch_SetIOLine( Register1_IOLine_Data,  (value & (1ul<<i)) );
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register1_IOLine_Shift, true );
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register1_IOLine_Shift, false );
    }

    // Store
    __DI__
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register1_IOLine_Store, true );
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register1_IOLine_Store, false );
    __EI__
    
    // Idle
    __DI__
    LL_KeySwitch_SetIOLine( Register1_IOLine_Shift, false );
    LL_KeySwitch_SetIOLine( Register1_IOLine_Store, false );
    LL_KeySwitch_SetIOLine( Register1_IOLine_Data,  false );
    __EI__

    return true;
 }
 #endif

 #if SUPPORT_SERIAL_REGISTER_2
 static bool KeySwitch_SetKeyCodeSerial_Register2( tKeySwitchCode_t code )
 {
    // Idle
    __DI__
    LL_KeySwitch_SetIOLine( Register2_IOLine_Shift, false );
    LL_KeySwitch_SetIOLine( Register2_IOLine_Store, false );
    LL_KeySwitch_SetIOLine( Register2_IOLine_Data,  false );
    __EI__
    
    // DataOut
    uint8_t value = GET_CONTROL_REGISTER_2( code ); 
    for( int32_t i = 7; i>=0; i-- )
    {
    LL_KeySwitch_SetIOLine( Register2_IOLine_Shift, false );
    LL_KeySwitch_SetIOLine( Register2_IOLine_Data,  (value & (1ul<<i)) );
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register2_IOLine_Shift, true );
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register2_IOLine_Shift, false );
    }

    // Store
    __DI__
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register2_IOLine_Store, true );
    HAL_Wait_us( SystemCoreClock / 1000000 );  // 1MHz
    LL_KeySwitch_SetIOLine( Register2_IOLine_Store, false );
    __EI__
    
    // Idle
    __DI__
    LL_KeySwitch_SetIOLine( Register2_IOLine_Shift, false );
    LL_KeySwitch_SetIOLine( Register2_IOLine_Store, false );
    LL_KeySwitch_SetIOLine( Register2_IOLine_Data,  false );
    __EI__

    return true;
 }
 #endif

 static bool KeySwitch_SetKeyCodeSerial( tKeySwitchCode_t code )
 {
    bool set = true;

    DI();

    if( bInitialized )
    {  
       EI();
       
       #if SUPPORT_SERIAL_REGISTER_1
       if( CHECK_CONTROL_REGISTER_1(code) )
        set = set && KeySwitch_SetKeyCodeSerial_Register1(code);
       #endif

       #if SUPPORT_SERIAL_REGISTER_2
       if( CHECK_CONTROL_REGISTER_2(code) )
        set = set && KeySwitch_SetKeyCodeSerial_Register2(code);
       #endif

       if( set )
          keyCode = code;   
    }
    else
    {
       EI();  

       set = false;
    }

    return set;
 }

 static void LL_KeySwitch_SetIOLine( e_LLKeySwitch_IOLines_t port, bool state )
 {   
     switch( port )
     {
        // LogicPort #1
        case eLLKeySw_IOLine_1: HAL_GPIO_WritePin( CONFIG_PORT__SWKEYPP_L1, CONFIG_PIN__SWKEYPP_L1, ((state)?GPIO_PIN_SET:GPIO_PIN_RESET) );
        break;
        // LogicPort #2
        case eLLKeySw_IOLine_2: HAL_GPIO_WritePin( CONFIG_PORT__SWKEYPP_L2, CONFIG_PIN__SWKEYPP_L2, ((state)?GPIO_PIN_SET:GPIO_PIN_RESET) );
        break;
        // LogicPort #3
        case eLLKeySw_IOLine_3: HAL_GPIO_WritePin( CONFIG_PORT__SWKEYPP_L3, CONFIG_PIN__SWKEYPP_L3, ((state)?GPIO_PIN_SET:GPIO_PIN_RESET) );
        break;
        // LogicPort #4
        case eLLKeySw_IOLine_4: HAL_GPIO_WritePin( CONFIG_PORT__SWKEYPP_L4, CONFIG_PIN__SWKEYPP_L4, ((state)?GPIO_PIN_SET:GPIO_PIN_RESET) );
        break;
        // LogicPort #5
        case eLLKeySw_IOLine_5: HAL_GPIO_WritePin( CONFIG_PORT__SWKEYPP_L5, CONFIG_PIN__SWKEYPP_L5, ((state)?GPIO_PIN_SET:GPIO_PIN_RESET) );
        break;
        // LogicPort #6
        case eLLKeySw_IOLine_6: HAL_GPIO_WritePin( CONFIG_PORT__SWKEYPP_L6, CONFIG_PIN__SWKEYPP_L6, ((state)?GPIO_PIN_SET:GPIO_PIN_RESET) );
        break;
     }
     
 }
 
 static bool KeySwitch_GetKeyCode( tKeySwitchCode_t * pcode )
 {
    if( pcode )
    {
       __DI__ *pcode = keyCode; __EI__
       return true; 
    }

    return false; 
 }
 
  static bool AmpSwitch_GetAmpCode( tKeySwitchCode_t * pcode )
 {
    if( pcode )
    {
       __DI__ *pcode = keyCode; __EI__
       return true; 
    }

    return false; 
 }
 
  static bool FiltSwitch_GetFiltCode( tKeySwitchCode_t * pcode )
 {
    if( pcode )
    {
       __DI__ *pcode = keyCode; __EI__
       return true; 
    }

    return false; 
 }

 static void KeySwitch_DeInit()
 {
    DI();

    bInitialized = false;
    #if CONFIG_PORT__SINGLEPORTINIT
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // Configure GPIO pins for KeySwitch control
    switch(xMode)
    {
    case eSwCtlMode_6line:
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L1|CONFIG_PIN__SWKEYPP_L2|CONFIG_PIN__SWKEYPP_L3|CONFIG_PIN__SWKEYPP_L4|CONFIG_PIN__SWKEYPP_L5|CONFIG_PIN__SWKEYPP_L6;
    break;
    case eSwCtlMode_4line:
    default:
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L1|CONFIG_PIN__SWKEYPP_L2|CONFIG_PIN__SWKEYPP_L3|CONFIG_PIN__SWKEYPP_L4;
    break;
    }
    }
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;    
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L1, &GPIO_InitStruct);
    }
    #else
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // Configure GPIO pins for KeySwitch control
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L1;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;    
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L1, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // Configure GPIO pins for KeySwitch control
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L2;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;    
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L2, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // Configure GPIO pins for KeySwitch control
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L3;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;    
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L3, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // Configure GPIO pins for KeySwitch control
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L4;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;    
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L4, &GPIO_InitStruct);
    }
    if( xMode == eSwCtlMode_6line )
    {
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // Configure GPIO pins for KeySwitch control
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L5;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;    
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L5, &GPIO_InitStruct);
    }
    {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // Configure GPIO pins for KeySwitch control
    GPIO_InitStruct.Pin = CONFIG_PIN__SWKEYPP_L6;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStruct.Pull = GPIO_NOPULL;    
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(CONFIG_PORT__SWKEYPP_L6, &GPIO_InitStruct);
    }
    }
    #endif
    xMode = eSwCtlMode_default;
    EI();
 }