/**
 * \file    sys_pd_hook.c
 * \brief   
 * 
 * \par     Copyright (c) 2018 UWE Technologies Corp., Ltd. All Rights Reserved.
 * \author  James
 * \date    2018-08-31
 * \version 
 * 
 * \history 
 *  Date        Note
 *  2018-08-31  Original Creation
 * 
 */

#include "libc.h"
#include "drivers.h"
#include "sys_pd_config.h"
#include "usbpd.h"
#include "sys_pd_hook.h"
#include "sys_low_power.h"
#include "sys_gpio.h"
#include "sys_pd_config.h"
#if SYS_QUICK_CHARGE_ENABLED
#include "sys_qc_hook.h"
#endif /* SYS_QUICK_CHARGE_ENABLED */


volatile SYS_PD_INFO_t g_sys_pd_info;

void sys_pd_sink_unattached_hook()
{
#if SYS_QUICK_CHARGE_ENABLED
    if (usbpd_cfg_port_quick_charge_en_get())
    {
        dpm_qc_enable_set(&g_usbc_qc_info, FALSE, FALSE);
        sys_gpio_uart0_enable_set(TRUE);
    }
    else {
        if (DRV_UART_ID_0 == drv_uart_default_get())
        {
            if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(TRUE);
        }
        else {
            sys_gpio_uart0_enable_set(TRUE);
        }
    }
#else
    if (DRV_UART_ID_0 == drv_uart_default_get())
    {
        if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(TRUE);
    }
    else {
        sys_gpio_uart0_enable_set(TRUE);
    }
#endif /* SYS_QUICK_CHARGE_ENABLED */

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nSink Unattached Hook");
    }
#if SYS_PWM_CTRL_VOLT_ENABLED
    if (usbpd_cfg_port_src_pwm_en_get())
    {
    	drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,FALSE);
    }
#endif /* #if SYS_PWM_CTRL_VOLT_ENABLED */

    sys_pd_cfg_snk_operating_power_profile_set(SNK_POWER_PROFILE_CUSTOM);
    sys_gpio_src_cap_led_off();
    drv_timer_stop(&g_sys_pd_info.snk_request_timer);
    g_sys_pd_info.snk_request_auto_is_need = FALSE;

    g_sys_pd_info.snk_request_pps_in_epr_mode = FALSE;
    g_sys_pd_info.snk_request_pps_index_in_epr_mode = 0;
    usbpd_cfg_port_epr_mode_en_set(TRUE);
}

void sys_pd_sink_attachwait_hook()
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nSink Attachwait Hook");
    }
}

void sys_pd_sink_attached_hook()
{
    sys_lpm_activate();
#if SYS_QUICK_CHARGE_ENABLED
    if (usbpd_cfg_port_quick_charge_en_get())
    {
        sys_gpio_uart0_enable_set(FALSE);
        if (!dpm_port_dead_battery_get())
        {
            // No need to enable QC sink
            dpm_qc_enable_set(&g_usbc_qc_info, FALSE, FALSE);
        }
    }
    else {
        if (DRV_UART_ID_0 == drv_uart_default_get())
        {
            if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(FALSE);
        }
        else {
            sys_gpio_uart0_enable_set(FALSE);
        }
    }
#else
    if (DRV_UART_ID_0 == drv_uart_default_get())
    {
        if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(FALSE);
    }
    else {
        sys_gpio_uart0_enable_set(FALSE);
    }
#endif /* SYS_QUICK_CHARGE_ENABLED */

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nSink Attached Hook");
    }
#if SYS_PWM_CTRL_VOLT_ENABLED
    if (usbpd_cfg_port_src_pwm_en_get())
    {
    	drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,FALSE);
    }
#endif /* #if SYS_PWM_CTRL_VOLT_ENABLED */

    sys_pd_cfg_snk_operating_power_profile_set(SNK_POWER_PROFILE_CUSTOM);
    //sys_gpio_src_cap_full_led_on();
    sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_1);
    drv_timer_stop(&g_sys_pd_info.snk_request_timer);
    g_sys_pd_info.snk_request_auto_is_need = FALSE;
}

status_t sys_pd_sink_transit_to_new_power_hook()
{
    if (dpm_current_pwr_mode_get())
        return S_ERROR;

    if (!dpm_src_obj_pos_get())
        return S_ERROR;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nSink Transit To New Power Hook");
    }
    int cap_pos = dpm_src_obj_pos_get();
    u32_t volt, current;
    UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(cap_pos-1);

    if (POWER_SUPPLY_TYPE_BATTERY == p_src_cap_fixed->supply_type)
    {
        UPD_DATA_MSG_SOURCE_CAP_BATTERY_SUPPLY_t *p_src_cap_battery = (UPD_DATA_MSG_SOURCE_CAP_BATTERY_SUPPLY_t *)dpm_rx_src_cap_object_get(cap_pos-1);
        volt = p_src_cap_battery->max_volt * VOLTAGE_DIV;
        current = (u32_t)(p_src_cap_battery->max_allow_pwr) * POWER_DIV * 1000 / volt;
    }
    else if (POWER_SUPPLY_TYPE_VARIABLE == p_src_cap_fixed->supply_type)
    {
        UPD_DATA_MSG_SOURCE_CAP_VARIABLE_SUPPLY_t *p_src_cap_vari = (UPD_DATA_MSG_SOURCE_CAP_VARIABLE_SUPPLY_t *)dpm_rx_src_cap_object_get(cap_pos-1);
        volt = p_src_cap_vari->max_volt * VOLTAGE_DIV;
        current = p_src_cap_vari->max_curr * CURRENT_DIV;
    }
    else if(POWER_SUPPLY_TYPE_PPS == p_src_cap_fixed->supply_type)
    {
        if (cap_pos >= 7)
        {
            volatile UPD_DATA_MSG_SOURCE_CAP_AVS_APDU_t *p_cap_avs;
            p_cap_avs = (volatile UPD_DATA_MSG_SOURCE_CAP_AVS_APDU_t *)dpm_rx_src_cap_object_get(cap_pos-1);
            volt = p_cap_avs->max_volt * VOLTAGE_PPS_DIV;
            current = p_cap_avs->pdp * 1000000 / volt;
        }
        else {
            UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *p_src_cap_pps = (UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *)dpm_rx_src_cap_object_get(cap_pos-1);
            volt = p_src_cap_pps->max_volt * VOLTAGE_PPS_DIV;
            current = p_src_cap_pps->max_curr * CURRENT_PPS_DIV;
        }
    }
    else {
        volt = p_src_cap_fixed->volt * VOLTAGE_DIV;
        current = p_src_cap_fixed->max_curr * CURRENT_DIV;
    }
    g_sys_pd_info.vbus_requested = volt;
    g_sys_pd_info.ibus_requested = current;
    if (dpm_port_epr_mode_get())
    {
        if(POWER_SUPPLY_TYPE_PPS == p_src_cap_fixed->supply_type)
        {
            sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_11);
        }
        else
        {
            if (volt == VOLTAGE_28V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_8);
            }
            else if (volt == VOLTAGE_36V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_9);
            }
            else if(volt == VOLTAGE_48V)
            {
                 sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_10);
            }
            else if (volt == VOLTAGE_20V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_5);
            }
            else if(volt == VOLTAGE_15V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_4);
            }
            else if(volt == VOLTAGE_12V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_3);
            }
            else if (volt == VOLTAGE_9V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_2);
            }
            else if(volt == VOLTAGE_5V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_1);
            }

        }
    }
    else
    {
        if (POWER_SUPPLY_TYPE_PPS == p_src_cap_fixed->supply_type)
        {

            if (!g_sys_pd_info.pps_set_pos)
            {
                g_sys_pd_info.pps_set_pos = cap_pos;
                g_sys_pd_info.pps_led_on_num = SYS_GPIO_SRC_CAP_LED_6;
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_6);
            }
            else
            {
                if (g_sys_pd_info.pps_set_pos != cap_pos)
                {
                    if (cap_pos > g_sys_pd_info.pps_set_pos )
                    {
                        sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_7);
                        g_sys_pd_info.pps_led_on_num = SYS_GPIO_SRC_CAP_LED_7;
                    }
                    else
                    {
                        sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_6);
                        g_sys_pd_info.pps_led_on_num = SYS_GPIO_SRC_CAP_LED_6;
                    }
                    g_sys_pd_info.pps_set_pos = cap_pos;
                }
                else
                {
                    if ( g_sys_pd_info.pps_led_on_num == SYS_GPIO_SRC_CAP_LED_6)
                    {
                        sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_6);
                    }
                    else if(g_sys_pd_info.pps_led_on_num == SYS_GPIO_SRC_CAP_LED_7)
                    {
                        sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_7);
                    }
                }
            }
        }    
        else 
        {
            g_sys_pd_info.pps_set_pos = 0;
            if (volt == VOLTAGE_20V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_5);
#if 0
                if (usbpd_cfg_port_epr_mode_en_get()&& (!dpm_port_epr_mode_get())&&
                    (volt == VOLTAGE_20V)&&(!dpm_rx_epr_fail_msg_get())&&
                    (!g_sys_pd_info.auto_epr_request_flg))
                {
                    g_sys_pd_info.auto_epr_request_flg = TRUE;
                    UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed_5v = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(0);
                    if ((p_src_cap_fixed_5v->epr == TRUE)&&(!dpm_snk_epr_mode_req_get()))
                    {
                        dpm_snk_epr_mode_req_set(TRUE);
                        sys_pd_cfg_snk_epr_operating_power_profile_set(SNK_POWER_REQUIRE_140W);
                    }
                }
#endif
            }
            else if(volt == VOLTAGE_15V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_4);
            }
            else if(volt == VOLTAGE_12V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_3);
            }
            else if (volt == VOLTAGE_9V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_2);
            }
            else if(volt == VOLTAGE_5V)
            {
                sys_gpio_src_cap_led_on(SYS_GPIO_SRC_CAP_LED_1);
            }
        }
    }
    
    //sys_gpio_src_cap_led_on((dpm_src_obj_pos_get() - 1));

    if (((POWER_SUPPLY_TYPE_PPS != dpm_rx_src_cap_supply_type_get((dpm_src_obj_pos_get()-1)))) || 
        ((POWER_SUPPLY_TYPE_PPS == dpm_rx_src_cap_supply_type_get((dpm_src_obj_pos_get()-1))) && (dpm_vbus_volt_check((usbpd_cfg_port_snk_pwr_volt_get() * VOLTAGE_DIV)))))
    {
        if ((usbpd_cfg_port_vbus_check_get()) && 
            (g_sys_pd_info.snk_request_auto_is_need) && 
            (dpm_src_obj_pos_get() < dpm_rx_src_cap_num_get()))
        {
            drv_timer_start(500, &g_sys_pd_info.snk_request_timer, NULL);
        }
    }

    if ((usbpd_cfg_port_epr_mode_en_get()) && 
        (!dpm_port_epr_mode_get()) &&
        (volt == VOLTAGE_5V) &&
        (!dpm_rx_epr_fail_msg_get()) &&
        (!g_sys_pd_info.snk_request_pps_in_epr_mode))
    {
        UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed_5v = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(0);
        if ((p_src_cap_fixed_5v->epr == TRUE)&&(!dpm_snk_epr_mode_req_get()))
        {
            dpm_snk_epr_mode_req_set(TRUE);
            sys_pd_cfg_snk_epr_operating_power_profile_set(SNK_POWER_REQUIRE_15W);
            if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_1) 
            {
                drv_uart_puts("\nReq EPR Mode");
            }
        }
    }

    return S_OK;
}

void sys_pd_source_unattached_hook()
{
#if SYS_QUICK_CHARGE_ENABLED
    if (usbpd_cfg_port_quick_charge_en_get())
    {
        dpm_qc_enable_set(&g_usbc_qc_info, FALSE, FALSE);
        sys_gpio_uart0_enable_set(TRUE);
    }
    else {
        if (DRV_UART_ID_0 == drv_uart_default_get())
        {
            if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(TRUE);
        }
        else {
            sys_gpio_uart0_enable_set(TRUE);
        }
    }
#else
    if (DRV_UART_ID_0 == drv_uart_default_get())
    {
        if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(TRUE);
    }
    else {
        sys_gpio_uart0_enable_set(TRUE);
    }
#endif /* SYS_QUICK_CHARGE_ENABLED */

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nSrc Unattached Hook");
    }
#if SYS_PWM_CTRL_VOLT_ENABLED
    if (usbpd_cfg_port_src_pwm_en_get())
    {
    	drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,FALSE);
    }
#endif /* #if SYS_PWM_CTRL_VOLT_ENABLED */
    usbpd_cfg_port_epr_mode_en_set(TRUE);
}

void sys_pd_source_attachwait_hook()
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nSrc Attachwait Hook");
    }
}

void sys_pd_source_attached_hook()
{
#if SYS_QUICK_CHARGE_ENABLED
    if (usbpd_cfg_port_quick_charge_en_get())
    {
        sys_gpio_uart0_enable_set(FALSE);
        if (!dpm_port_dead_battery_get())
        {
            dpm_qc_enable_set(&g_usbc_qc_info, TRUE, FALSE);
        }
    }
    else {
        if (DRV_UART_ID_0 == drv_uart_default_get())
        {
            if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(FALSE);
        }
        else {
            sys_gpio_uart0_enable_set(FALSE);
        }
    }
#else
    if (DRV_UART_ID_0 == drv_uart_default_get())
    {
        if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(FALSE);
    }
    else {
        sys_gpio_uart0_enable_set(FALSE);
    }
#endif /* SYS_QUICK_CHARGE_ENABLED */

    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nSrc Attached Hook");
    }
    sys_pd_led_state_set(SYS_PD_LED_STATE_ON);

#if SYS_PWM_CTRL_VOLT_ENABLED
    if (usbpd_cfg_port_src_pwm_en_get())
    {
        drv_pwm_init_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,FALSE,usbpd_cfg_port_src_pwm_per_get());
        drv_pwm_duty_set2(SYS_PWM_INDEX_FOR_VOLT_CTRL, usbpd_cfg_port_src_fixed_pwr_pwm_duty_get(CFG_VOLTAGE_5V_IDX));
        drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,usbpd_cfg_port_src_pwm_en_get());
    }
#endif /* #if SYS_PWM_CTRL_VOLT_ENABLED */

}

status_t sys_pd_source_transit_to_new_power_hook(u32_t pos)
{
#if SYS_PWM_CTRL_VOLT_ENABLED
	u32_t duty;
#endif

    if (!dpm_current_pwr_mode_get())
        return S_ERROR;

    if (pos >= CFG_MAX_PWR_CAP_NUM)
        return S_INVALID_PARAMS;

#if SYS_QUICK_CHARGE_ENABLED
    if (usbpd_cfg_port_quick_charge_en_get())
    {
        //dpm_qc_enable_set(&g_usbc_qc_info, FALSE, FALSE);
        dpm_qc_state_set(&g_usbc_qc_info, DPM_QC_DISABLED);
        if (DRV_UART_ID_0 == drv_uart_default_get())
        {
            if (SYS_DEBUG_LEVEL_OFF == sys_cfg_dbg_get()) sys_gpio_uart0_enable_set(FALSE);
        }
        else {
            sys_gpio_uart0_enable_set(FALSE);
        }
    }
#endif /* SYS_QUICK_CHARGE_ENABLED */

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        printf("\nSrc Transit To New Power Hook(%u)\n",pos);
    }

#if (SYS_PWM_CTRL_VOLT_ENABLED && SYS_PWM_STEP_CTRL_VOLT_ENABLED)
    if ((POWER_SUPPLY_TYPE_FIXED == usbpd_cfg_port_src_pwr_supply_type_get(pos)) && 
        (VOLTAGE_VAL(VOLTAGE_12V) < usbpd_cfg_port_src_fixed_pwr_volt_cap_get(pos)))
    {
        duty = drv_pwm_duty_get(SYS_PWM_INDEX_FOR_VOLT_CTRL);
        if (duty == usbpd_cfg_port_src_fixed_pwr_pwm_duty_get(CFG_VOLTAGE_5V_IDX))
        {
            drv_pwm_duty_set2(SYS_PWM_INDEX_FOR_VOLT_CTRL, usbpd_cfg_port_src_fixed_pwr_pwm_duty_get(sys_pd_cfg_src_fixed_power_profile_index_get(VOLTAGE_12V)));
            drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL, TRUE);
            rtc_delay_us(3000);
        }
    }
#endif /* (SYS_PWM_CTRL_VOLT_ENABLED && SYS_PWM_STEP_CTRL_VOLT_ENABLED) */

#if SYS_PWM_CTRL_VOLT_ENABLED
    if (POWER_SUPPLY_TYPE_PPS == usbpd_cfg_port_src_pwr_supply_type_get(pos))
    {
        duty = usbpd_cfg_port_src_pps_pwr_pwm_duty_get(pos, REQ_PPS_OUTPUT_VOLT_GET(dpm_current_rdo_get())*VOLTAGE_PPS_REQ_DIV);
#if USBPD_DEBUG_ENABLED
        if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) printf("\nVBUS-Set(%u):%umV,Duty:%x\n", pos, REQ_PPS_OUTPUT_VOLT_GET(dpm_current_rdo_get())*VOLTAGE_PPS_REQ_DIV,duty);
#endif /* USBPD_DEBUG_ENABLED */
    }
    else {
        duty = usbpd_cfg_port_src_fixed_pwr_pwm_duty_get(pos);
#if USBPD_DEBUG_ENABLED
        if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) printf("\nVBUS-Set(%u):Duty:%x\n", pos, duty);
#endif /* USBPD_DEBUG_ENABLED */
    }
    drv_pwm_duty_set2(SYS_PWM_INDEX_FOR_VOLT_CTRL, duty);
    drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL, TRUE);
#else

#endif /* #if SYS_PWM_CTRL_VOLT_ENABLED */
    return S_OK;
}

bool_t sys_pd_vbus_source_en_get(void)
{
    return sys_gpio_vbus_source_en_get();
}

void sys_pd_vbus_source_en_set(bool_t flg)
{
    g_sys_pd_info.vbus_source_en = flg;
#if SYS_PWM_CTRL_VOLT_ENABLED
    if (flg && usbpd_cfg_port_src_pwm_en_get())
    {
        //make sure the voltage is VSafe5V
        drv_pwm_duty_set2(SYS_PWM_INDEX_FOR_VOLT_CTRL, usbpd_cfg_port_src_fixed_pwr_pwm_duty_get(CFG_VOLTAGE_5V_IDX));
        drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,TRUE);
    }
#endif /* SYS_PWM_CTRL_VOLT_ENABLED */
    sys_gpio_vbus_source_en_set(flg);
}

bool_t sys_pd_vbus_discharge_en_get(void)
{
    return dpm_vbus_discharge_get();
}

void sys_pd_vbus_discharge_en_set(bool_t flg)
{
    dpm_vbus_discharge_set(flg);
}

bool_t sys_pd_cc1_vconn_en_get(void)
{
    return sys_gpio_vconn1_get();
}

void sys_pd_cc1_vconn_en_set(bool_t flg)
{
    sys_gpio_vconn1_set(flg);
}

bool_t sys_pd_cc2_vconn_en_get(void)
{
    return sys_gpio_vconn2_get();
}

void sys_pd_cc2_vconn_en_set(bool_t flg)
{
    sys_gpio_vconn2_set(flg);
}


void sys_pd_prs_started_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nPRS Started Hook");
    }

}

void sys_pd_prs_completed_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nPRS Completed Hook");
    }

}

void sys_pd_drs_started_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nDRS Started Hook");
    }

}

void sys_pd_drs_completed_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nDRS Completed Hook");
    }

}

void sys_pd_vcs_started_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nVCS Started Hook");
    }

}

void sys_pd_vcs_completed_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nVCS Completed Hook");
    }

}

void sys_pd_power_change_req_hook(u32_t arg)
{
    sys_lpm_activate();
    sys_gpio_src_cap_led_off();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nPower Change Req Hook");
    }
}

void sys_pd_prl_msg_received_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_3) 
    {
        drv_uart_puts("\nPRL Msg Recevied Hook");
    }
    g_sys_pd_info.rx_pkt_cnt++;
}

void sys_pd_prl_msg_sent_hook(u32_t arg)
{
    sys_lpm_activate();
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_3) 
    {
        drv_uart_puts("\nPRL Msg Sent Hook");
    }
    g_sys_pd_info.tx_pkt_cnt++;
}

void sys_pd_led_state_set(SYS_PD_LED_STATE_t state)
{
    g_sys_pd_info.led_state = state;
    if (state)
    {
        g_sys_pd_info.led_on_start_tick = g_sys_pd_info.continuous_timer_tick;
    }
    if (state == SYS_PD_LED_STATE_FLASH)
    {
        g_sys_pd_info.led_display_flash_interval = 10;
    }
    else if (state == SYS_PD_LED_STATE_FLASH_FAST)
    {
        g_sys_pd_info.led_display_flash_interval = 10;
    }
}


bool_t sys_new_power_level_req()
{
    UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_cap_fixed;
    UPD_DATA_MSG_SOURCE_CAP_VARIABLE_SUPPLY_t *p_cap_vari;
    UPD_DATA_MSG_SOURCE_CAP_BATTERY_SUPPLY_t *p_cap_battery;
    u32_t voltage, min_voltage, current_volt;
    u32_t current;
    int src_obj = dpm_src_obj_pos_get();

    if (usbpd_cfg_cable_plug_get() || (dpm_current_pwr_mode_get()) ||
        (!dpm_cable_detect_done_get()))
        return(FALSE);

    
    if (src_obj == dpm_rx_src_cap_num_get())
    {
        dpm_src_obj_pos_set(1);
        src_obj = 0;
    }
    else {
        dpm_src_obj_pos_set(src_obj+1);
    }
    
    
    usbpd_cfg_port_snk_pwr_prof_set(SNK_POWER_PROFILE_CUSTOM);
    p_cap_fixed = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(src_obj);
    while (p_cap_fixed->volt == 0)
    {
        src_obj = dpm_src_obj_pos_get();
        if (src_obj == dpm_rx_src_cap_num_get())
        {
            dpm_src_obj_pos_set(1);
            src_obj = 0;
        }
        else {
            dpm_src_obj_pos_set(src_obj+1);
        }
        p_cap_fixed = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(src_obj);
    }

    g_sys_pd_info.snk_request_pps_in_epr_mode = FALSE;
    g_sys_pd_info.snk_request_pps_index_in_epr_mode = 0;

    if (POWER_SUPPLY_TYPE_BATTERY == p_cap_fixed->supply_type)
    {
        p_cap_battery = (UPD_DATA_MSG_SOURCE_CAP_BATTERY_SUPPLY_t *)dpm_rx_src_cap_object_get(src_obj);
        voltage = (MAX_VOLT_GET(p_cap_battery) + MIN_VOLT_GET(p_cap_battery))/2;
        current = (u32_t)(MAX_ALLOW_PWR_GET(p_cap_battery)) * POWER_DIV / (voltage * VOLTAGE_DIV) * 1000 / CURRENT_DIV;
        if (dpm_port_epr_mode_get() == TRUE)
        {
            usbpd_cfg_port_snk_epr_pwr_volt_set(voltage);
            usbpd_cfg_port_snk_epr_pwr_curr_set(current);
            usbpd_cfg_port_snk_epr_pwr_max_curr_set(current);
        }
        else
        {
            usbpd_cfg_port_snk_pwr_volt_set(voltage);
            usbpd_cfg_port_snk_pwr_curr_set(current);
            usbpd_cfg_port_snk_pwr_max_curr_set(current);
        }
    }
    else if (POWER_SUPPLY_TYPE_VARIABLE == p_cap_fixed->supply_type)
    {
        p_cap_vari = (UPD_DATA_MSG_SOURCE_CAP_VARIABLE_SUPPLY_t *)dpm_rx_src_cap_object_get(src_obj);
        voltage = (MAX_VOLT_GET(p_cap_vari) + MIN_VOLT_GET(p_cap_vari))/2;
        if (dpm_port_epr_mode_get() == TRUE)
        {
            usbpd_cfg_port_snk_epr_pwr_volt_set(voltage);
            usbpd_cfg_port_snk_epr_pwr_curr_set(MAX_CURR_GET(p_cap_fixed));
            usbpd_cfg_port_snk_epr_pwr_max_curr_set(MAX_CURR_GET(p_cap_fixed));
        }
        else
        {
            usbpd_cfg_port_snk_pwr_volt_set(voltage);
            usbpd_cfg_port_snk_pwr_curr_set(MAX_CURR_GET(p_cap_vari));
            usbpd_cfg_port_snk_pwr_max_curr_set(MAX_CURR_GET(p_cap_vari));
        } 
    }
    else if (POWER_SUPPLY_TYPE_FIXED == p_cap_fixed->supply_type)
    {
        if (dpm_port_epr_mode_get() == TRUE)
        {
            if (VOLT_GET(p_cap_fixed)!= 0)
            {
                usbpd_cfg_port_snk_epr_pwr_volt_set(VOLT_GET(p_cap_fixed));
                usbpd_cfg_port_snk_epr_pwr_curr_set(MAX_CURR_GET(p_cap_fixed));
                usbpd_cfg_port_snk_epr_pwr_max_curr_set(MAX_CURR_GET(p_cap_fixed));
            }
        }
        else
        {
            usbpd_cfg_port_snk_pwr_volt_set(VOLT_GET(p_cap_fixed));
            usbpd_cfg_port_snk_pwr_curr_set(MAX_CURR_GET(p_cap_fixed));
            usbpd_cfg_port_snk_pwr_max_curr_set(MAX_CURR_GET(p_cap_fixed));
            if (src_obj >= 7)
            {
                if (usbpd_cfg_port_epr_mode_en_get()&& (!dpm_port_epr_mode_get())&&
                    (!dpm_rx_epr_fail_msg_get()))
                {
                    UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed_5v = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(0);
                    if ((p_src_cap_fixed_5v->epr == TRUE)&&(!dpm_snk_epr_mode_req_get()))
                    {
                        dpm_snk_epr_mode_req_set(TRUE);
                        sys_pd_cfg_snk_epr_operating_power_profile_set(SNK_POWER_REQUIRE_140W);
                        g_sys_pd_info.snk_request_is_need = FALSE;
#if USBPD_DEBUG_ENABLED
    		            if (usbpd_cfg_dbg_get()) drv_uart_puts("EPR req");
#endif /* CLI_ENABLED */
                    }
                }
            }
        } 
    }
    else if (POWER_SUPPLY_TYPE_PPS == p_cap_fixed->supply_type)
    {
        if (src_obj >= 7)
        {
            if (usbpd_cfg_port_snk_epr_pwr_pps_en_get())
            {
                UPD_DATA_MSG_SOURCE_CAP_AVS_APDU_t *p_cap_avs = (UPD_DATA_MSG_SOURCE_CAP_AVS_APDU_t *)dpm_rx_src_cap_object_get(src_obj);
                voltage = p_cap_avs->max_volt * VOLTAGE_PPS_DIV;
                min_voltage = p_cap_avs->min_volt * VOLTAGE_PPS_DIV;
                current = p_cap_avs->pdp * 1000000 / voltage;
#if USBPD_DEBUG_ENABLED
                if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2)
                {
                    printf("avs-volt=%u,avs-curr=%u\n",voltage,current);
                }
#endif
                usbpd_cfg_port_pps_volt_step_set(100);
                if (VOLTAGE_28V <= voltage)
                {
                    usbpd_cfg_port_pps_volt_step_set(1500);
                }
                if (min_voltage < VOLTAGE_15V)
                    min_voltage = VOLTAGE_15V;
                usbpd_cfg_port_snk_epr_pwr_volt_set( min_voltage / VOLTAGE_DIV );
                usbpd_cfg_port_snk_epr_pwr_curr_set( current / CURRENT_DIV );
                usbpd_cfg_port_snk_epr_pwr_max_curr_set( current / CURRENT_DIV );
                dpm_pps_output_voltage_set(min_voltage);
                current_volt = dpm_vbus_volt_get();
                if (current_volt >= min_voltage)
                {
                    if (current_volt >= voltage)
                    {
                        dpm_pps_output_voltage_set(voltage);
                    }
                    else {
                        dpm_pps_output_voltage_set(current_volt);
                    }
                }
                usbpd_cfg_port_snk_epr_pwr_prof_set(SNK_POWER_PROFILE_PPS);
                usbpd_cfg_port_epr_operational_pdp_set(voltage * current / 1000000);
                //usbpd_cfg_port_maximum_pdp_set(usbpd_cfg_port_operational_pdp_get());
#if USBPD_DEBUG_ENABLED
                if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2)
                {
                    printf("epr_pwr_volt=%u\n",usbpd_cfg_port_snk_epr_pwr_volt_get());
                    printf("epr_pwr_curr=%u\n",usbpd_cfg_port_snk_epr_pwr_curr_get());
                }
#endif
            }
            else {
                if (1 == dpm_src_obj_pos_get())
                {
                    dpm_src_obj_pos_set(dpm_rx_src_cap_num_get());
                }
                else {
                    dpm_src_obj_pos_set(src_obj);
                }
                return FALSE;
            }
        }
        else
        {
            if (usbpd_cfg_port_snk_pwr_pps_en_get())
            {
                if (dpm_port_epr_mode_get())
                {
                    g_sys_pd_info.snk_request_pps_in_epr_mode = TRUE;
                    g_sys_pd_info.snk_request_pps_index_in_epr_mode = dpm_src_obj_pos_get();
                    dpm_epr_mode_exit_req_set(TRUE);
                    g_sys_pd_info.snk_request_epr_after_pps = TRUE;
                    g_sys_pd_info.snk_request_epr_index_after_pps = 7;
                    return TRUE;
                }
                else 
                {
                    UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *p_cap_pps = (UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *)dpm_rx_src_cap_object_get(src_obj);
                    voltage = MAX_VOLT_GET(p_cap_pps) * VOLTAGE_PPS_DIV;
                    min_voltage = p_cap_pps->min_volt * VOLTAGE_PPS_DIV;
                    current = MAX_CURR_GET(p_cap_pps) * CURRENT_PPS_DIV;
                    usbpd_cfg_port_pps_volt_step_set(100);
                    if ((VOLTAGE_PPS_MAX_9V == voltage))
                    {
                        usbpd_cfg_port_pps_volt_step_set(400);
                    }
                    else if ((VOLTAGE_PPS_MAX_15V == voltage))
                    {
                        usbpd_cfg_port_pps_volt_step_set(1000);
                    }
                    else if ((VOLTAGE_PPS_MAX_20V == voltage))
                    {
                        usbpd_cfg_port_pps_volt_step_set(1500);
                    }
                    if (min_voltage < VOLTAGE_5V)
                        min_voltage = VOLTAGE_5V;
                    usbpd_cfg_port_snk_pwr_volt_set( min_voltage / VOLTAGE_DIV );
                    usbpd_cfg_port_snk_pwr_curr_set( current / CURRENT_DIV );
                    usbpd_cfg_port_snk_pwr_max_curr_set( current / CURRENT_DIV );
                    dpm_pps_output_voltage_set(min_voltage);
                    current_volt = dpm_vbus_volt_get();
                    if (current_volt >= min_voltage)
                    {
                        if (current_volt >= voltage)
                        {
                            dpm_pps_output_voltage_set(voltage);
                        }
                        else {
                            dpm_pps_output_voltage_set(current_volt);
                        }
                    }
                    usbpd_cfg_port_snk_pwr_prof_set(SNK_POWER_PROFILE_PPS);
                    usbpd_cfg_port_operational_pdp_set(voltage * current / 1000000);
                    usbpd_cfg_port_maximum_pdp_set(usbpd_cfg_port_operational_pdp_get());
                }
            }
            else {
                if (1 == dpm_src_obj_pos_get())
                {
                    dpm_src_obj_pos_set(dpm_rx_src_cap_num_get());
                }
                else {
                    dpm_src_obj_pos_set(src_obj);
                }
                return FALSE;
            }

        }
        
    }
    if (VOLT_GET(p_cap_fixed)!= 0)
    {
        dpm_new_pwrlevel_req_set(TRUE);
    }
    
    return TRUE;
}



void sys_pd_idle_hook(u32_t arg)
{
    g_sys_pd_info.current_time = now();

    sys_tick_t start_time = now();

    if ((drv_timer_stopped(&(g_sys_pd_info.continuous_timer))) && 
        (!(drv_timer_expired(&(g_sys_pd_info.continuous_timer)))))
    {
        drv_timer_start(SYS_CONTINUOUS_TIMER_TIMEOUT, &(g_sys_pd_info.continuous_timer), NULL);
    }

    g_sys_pd_info.snk_request_current_state = sys_gpio_snk_request_detected();
    if (g_sys_pd_info.snk_request_current_state)
    {
        if (g_sys_pd_info.snk_request_current_state != g_sys_pd_info.snk_request_last_state)
        {
            g_sys_pd_info.snk_request_last_state = g_sys_pd_info.snk_request_current_state;
            g_sys_pd_info.snk_request_on_start_time = g_sys_pd_info.current_time;
        }
#if SYS_RESET_TO_FACTORY_ENABLED
        // reset configuration to factory default process
        if ((g_sys_pd_info.snk_request_last_state) && 
            (g_sys_pd_info.current_time > g_sys_pd_info.snk_request_on_start_time) && 
            ((u32_t)(g_sys_pd_info.current_time - g_sys_pd_info.snk_request_on_start_time) > SYS_RESET_TO_FACTORY_PIN_HOLDING_TIME))
        {
            sys_cfg_erase();
#if USBPD_DEBUG_ENABLED
    		if (usbpd_cfg_dbg_get()) drv_uart_puts("Config erased");
#endif /* CLI_ENABLED */
            rtc_delay_us(20000);
            drv_chip_reset();
        }
#endif /* SYS_RESET_TO_FACTORY_ENABLED */
        // sink request process
        if (PE_SNK_READY == pe_state_get())
        {
            if ((g_sys_pd_info.current_time > g_sys_pd_info.snk_request_on_last_time) && 
                ((u32_t)(g_sys_pd_info.current_time - g_sys_pd_info.snk_request_on_last_time) > 500000UL) && 
                !g_sys_pd_info.snk_request_is_need)
            {
                g_sys_pd_info.snk_request_is_need = TRUE;
                int cap_pos = dpm_src_obj_pos_get();
#if 0
                u32_t volt;
                UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(cap_pos-1);
                if (POWER_SUPPLY_TYPE_BATTERY == p_src_cap_fixed->supply_type)
                {
                    UPD_DATA_MSG_SOURCE_CAP_BATTERY_SUPPLY_t *p_src_cap_battery = (UPD_DATA_MSG_SOURCE_CAP_BATTERY_SUPPLY_t *)dpm_rx_src_cap_object_get(cap_pos-1);
                    volt = p_src_cap_battery->max_volt * VOLTAGE_DIV;
                }
                else if (POWER_SUPPLY_TYPE_VARIABLE == p_src_cap_fixed->supply_type)
                {
                    UPD_DATA_MSG_SOURCE_CAP_VARIABLE_SUPPLY_t *p_src_cap_vari = (UPD_DATA_MSG_SOURCE_CAP_VARIABLE_SUPPLY_t *)dpm_rx_src_cap_object_get(cap_pos-1);
                    volt = p_src_cap_vari->max_volt * VOLTAGE_DIV;
                }
                else if (POWER_SUPPLY_TYPE_PPS == p_src_cap_fixed->supply_type)
                {
                    if (cap_pos >= 7)
                    {
                        volatile UPD_DATA_MSG_SOURCE_CAP_AVS_APDU_t *p_cap_avs;
                        p_cap_avs = (volatile UPD_DATA_MSG_SOURCE_CAP_AVS_APDU_t *)dpm_rx_src_cap_object_get(cap_pos-1);
                        volt = p_cap_avs->max_volt * VOLTAGE_PPS_DIV;
                    }
                    else {
                        UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *p_src_cap_pps = (UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *)dpm_rx_src_cap_object_get(cap_pos-1);
                        volt = p_src_cap_pps->max_volt * VOLTAGE_PPS_DIV;
                    }
                }
                else {
                    volt = p_src_cap_fixed->volt * VOLTAGE_DIV;
                }

                if(dpm_port_epr_mode_get()&&(dpm_src_obj_pos_get()==dpm_rx_src_cap_num_get()))
                {
                    dpm_epr_mode_exit_req_set(TRUE);
                    sys_pd_cfg_snk_operating_power_profile_set(SNK_POWER_REQUIRE_15W);
                }
#endif
                if (usbpd_cfg_port_epr_mode_en_get()&& (!dpm_port_epr_mode_get())&&
                    (cap_pos >= 7)&&(!dpm_rx_epr_fail_msg_get()))
                {
                    UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed_5v = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(0);
                    if ((p_src_cap_fixed_5v->epr == TRUE)&&(!dpm_snk_epr_mode_req_get()))
                    {
                        dpm_snk_epr_mode_req_set(TRUE);
                        g_sys_pd_info.snk_request_is_need = FALSE;
                        sys_pd_cfg_snk_epr_operating_power_profile_set(SNK_POWER_REQUIRE_140W);
                    }
                }
            }
            
        }
    }
    else {
        g_sys_pd_info.snk_request_last_state = 0;
        g_sys_pd_info.snk_request_on_start_time = 0;
    }
    if ((g_sys_pd_info.snk_request_is_need) || (drv_timer_expired(&g_sys_pd_info.snk_request_timer)))
    {
        g_sys_pd_info.snk_request_timer.timeout = FALSE;
        g_sys_pd_info.snk_request_is_need = FALSE;
        sys_new_power_level_req();
        g_sys_pd_info.snk_request_on_last_time = g_sys_pd_info.current_time;
        if ((g_sys_pd_info.snk_request_auto_is_need) && 
            (dpm_src_obj_pos_get() == dpm_rx_src_cap_num_get()))
        {
            g_sys_pd_info.snk_request_auto_is_need = FALSE;
        }
    }

    //handler QC state machine
#if SYS_QUICK_CHARGE_ENABLED
    if ((usbpd_cfg_port_quick_charge_en_get()) && 
        (!pe_pd_connected_get()) &&
        (DPM_QC_DISABLED != dpm_qc_state_get(&g_usbc_qc_info)))
    {
        dpm_qc_state_machine(&g_usbc_qc_info);
    }
#endif /* SYS_QUICK_CHARGE_ENABLED */

    // handle system process here per 100ms
    if (drv_timer_expired(&(g_sys_pd_info.continuous_timer)))
    {
        g_sys_pd_info.continuous_timer_tick++;

        g_sys_pd_info.vbus_current = dpm_vbus_volt_get();
        g_sys_pd_info.ibus_current = sys_pd_ibus_get();

        // C1 VBUS check
        if ((!usbpd_cfg_capture_mode_get()) && 
            (usbpd_cfg_port_vbus_check_get()) && 
            (!dpm_cable_detect_done_get()) && 
            (g_sys_pd_info.vbus_current > 6000U) && 
            ((u32_t)(now() - dpm_cc_connection_changed_time_get()) > 600000UL) && 
            (!pe_error_recovery_directed_get()))
        {
#if USBPD_DEBUG_ENABLED
    		if (usbpd_cfg_dbg_get()) drv_uart_puts("VBUS > 6V, No connection");
#endif /* CLI_ENABLED */
            pe_error_recovery_directed_set(TRUE);
        }

        if ((g_sys_pd_info.snk_request_pps_in_epr_mode) &&
            (!dpm_port_epr_mode_get()) &&
            (pe_state_is_snk_rdy()))
        {
            UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *p_cap_pps = (UPD_DATA_MSG_SOURCE_CAP_PPS_APDU_t *)dpm_rx_src_cap_object_get(g_sys_pd_info.snk_request_pps_index_in_epr_mode-1);
            u32_t voltage = MAX_VOLT_GET(p_cap_pps) * VOLTAGE_PPS_DIV;
            u32_t min_voltage = p_cap_pps->min_volt * VOLTAGE_PPS_DIV;
            u32_t current = MAX_CURR_GET(p_cap_pps) * CURRENT_PPS_DIV;
            if (voltage)
            {
                dpm_src_obj_pos_set(g_sys_pd_info.snk_request_pps_index_in_epr_mode);
                usbpd_cfg_port_pps_volt_step_set(100);
                if ((VOLTAGE_PPS_MAX_9V == voltage))
                {
                    usbpd_cfg_port_pps_volt_step_set(400);
                }
                else if ((VOLTAGE_PPS_MAX_15V == voltage))
                {
                    usbpd_cfg_port_pps_volt_step_set(1000);
                }
                else if ((VOLTAGE_PPS_MAX_20V == voltage))
                {
                    usbpd_cfg_port_pps_volt_step_set(1500);
                }
                u32_t cfg_volt = usbpd_cfg_port_snk_pwr_volt_get()*VOLTAGE_DIV;
                if (min_voltage < VOLTAGE_5V)
                    min_voltage = VOLTAGE_5V;
                if ((cfg_volt < min_voltage) && (cfg_volt > voltage))
                {
                    usbpd_cfg_port_snk_pwr_volt_set( min_voltage / VOLTAGE_DIV );
                }
                usbpd_cfg_port_snk_pwr_curr_set( current / CURRENT_DIV );
                usbpd_cfg_port_snk_pwr_max_curr_set( current / CURRENT_DIV );
                dpm_pps_output_voltage_set(min_voltage);
                if (g_sys_pd_info.vbus_current >= min_voltage)
                {
                    if (g_sys_pd_info.vbus_current >= voltage)
                    {
                        dpm_pps_output_voltage_set(voltage);
                    }
                    else {
                        dpm_pps_output_voltage_set(g_sys_pd_info.vbus_current);
                    }
                }
                usbpd_cfg_port_snk_pwr_prof_set(SNK_POWER_PROFILE_PPS);
                usbpd_cfg_port_operational_pdp_set(voltage * current / 1000000);
                usbpd_cfg_port_maximum_pdp_set(usbpd_cfg_port_operational_pdp_get());
                dpm_new_pwrlevel_req_set(TRUE);
            }
            g_sys_pd_info.snk_request_pps_in_epr_mode = FALSE;
            g_sys_pd_info.snk_request_pps_index_in_epr_mode = 0;
        }

        if ((g_sys_pd_info.snk_request_epr_after_pps) &&
            (dpm_port_epr_mode_get()) &&
            (pe_state_is_snk_rdy()))
        {
            UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_cap_fixed = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(g_sys_pd_info.snk_request_epr_index_after_pps);
            u32_t voltage = p_cap_fixed->volt * VOLTAGE_DIV;
            if (voltage)
            {
                dpm_src_obj_pos_set(g_sys_pd_info.snk_request_epr_index_after_pps+1);
                usbpd_cfg_port_snk_pwr_volt_set(VOLT_GET(p_cap_fixed));
                usbpd_cfg_port_snk_pwr_curr_set(MAX_CURR_GET(p_cap_fixed));
                usbpd_cfg_port_snk_pwr_max_curr_set(MAX_CURR_GET(p_cap_fixed));
                dpm_new_pwrlevel_req_set(TRUE);
            }
            g_sys_pd_info.snk_request_epr_after_pps = FALSE;
            g_sys_pd_info.snk_request_epr_index_after_pps = 0;
        }

        // restart timer
        sys_tick_t current_time = now();
        u32_t duration = (u32_t)(current_time - start_time) / 1000;
        if (duration >= SYS_CONTINUOUS_TIMER_TIMEOUT)
        {
            // no need to restart timer and keep the timeout flag for next run
        }
        else {
            drv_timer_start(SYS_CONTINUOUS_TIMER_TIMEOUT-duration, &(g_sys_pd_info.continuous_timer), NULL);
        }
    }
}

void sys_pd_vdm_disc_ident_ack_received_hook(u32_t arg)
{
    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        drv_uart_puts("\nVDM DISC IDENT ACK Hook");
    }

    // Change power policy here
    if ((MSG_SOP_TYPE_SOPP == dpm_msg_sop_type_get()) &&
        (VBUS_CURR_HANDLING_CAP_5A == dpm_rx_vdm_ack_cable_vbus_curr_cap_get()))
    {
        
    }
}


void sys_pd_prl_msg_vdm_attention_hook(UPD_DATA_MSG_VDM_ATTENTION_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("VDM Attention Hook:\n");
    }
}


void sys_pd_prl_msg_vdm_unstructured_hook(u8_t *p_msg, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_msg)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Unstructured VDM Hook:\n");
    }
    if (PRL_MSG_DIR_RX == dir)
    {
        pe_unsupported_msg_received_set(TRUE);
    }
}

void sys_pd_prl_msg_alert_hook(UPD_DATA_MSG_ALERT_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_puts("Alert Hook:");
        printf(" %-24s: %02X\n", "Type", p_info->type);
        if (p_info->type > 0)
        {
            if (p_info->type & UPD_ALERT_TYPE_RSVD1)
            {
                drv_uart_puts("  Bit0: Reserved");
            }
            if (p_info->type & UPD_ALERT_TYPE_BAT_STA_CHG)
            {
                drv_uart_puts("  Bit1: Battery Status Change");
            }
            if (p_info->type & UPD_ALERT_TYPE_OCP)
            {
                drv_uart_puts("  Bit2: Over-Current Protection");
            }
            if (p_info->type & UPD_ALERT_TYPE_OTP)
            {
                drv_uart_puts("  Bit3: Over-Temperature Protection");
            }
            if (p_info->type & UPD_ALERT_TYPE_OPR_COND_CHG)
            {
                drv_uart_puts("  Bit4: Operating Condition Change");
            }
            if (p_info->type & UPD_ALERT_TYPE_SRC_INPT_CHG)
            {
                drv_uart_puts("  Bit5: Source Input Change");
            }
            if (p_info->type & UPD_ALERT_TYPE_OVP)
            {
                drv_uart_puts("  Bit6: Over-Voltage Protection");
            }
            if (p_info->type & UPD_ALERT_TYPE_RSVD2)
            {
                drv_uart_puts("  Bit7: Reserved");
            }
        }

        printf(" %-24s: %02X\n %-24s: %02X\n", 
            "Fixed Batteries", p_info->fixed_batteries,
            "Hot Swappable Batteries", p_info->hot_swappable_batteries);
    }
}

void sys_pd_prl_msg_get_country_info_hook(UPD_DATA_MSG_GET_COUNTRY_INFO_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        printf("Get Country Info Hook:\n %-24s: %c%c\n", 
            "Country Code", p_info->alpha_2_code_1, p_info->alpha_2_code_2);
    }
}

void sys_pd_prl_msg_battery_status_hook(UPD_DATA_MSG_BATTERY_STATUS_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        printf("Battery Status Hook:\n %-24s: %02X\n %-24s: %u\n", 
            "Info", p_info->info,
            "Capacity", p_info->present_capacity);
    }
    if (dir == PRL_MSG_DIR_TX)
    {
        if (g_sys_pd_info.invalid_bat_sta_ref)
        {
            p_info->info = 1; // Invalid Battery reference
            p_info->present_capacity = 0xFFFF;
            p_info->resv = 0;
            g_sys_pd_info.invalid_bat_sta_ref = 0;
        }        
    }
    else {

    }
}

void sys_pd_prl_msg_ext_src_cap_hook(UPD_EXT_MSG_SRC_CAP_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        printf("Extended Source Capability Hook:\n %-24s: %02X\n %-24s: %u\n", 
            "Batteries", p_info->batteries,
            "PDP", p_info->src_pdp);
        printf("Configuration:\n %-24s: %02X\n %-24s: %u\n", 
            "Batteries", usbpd_cfg_port_battery_num_get(),
            "PDP", usbpd_cfg_port_src_pdp_get());
    }
}

void sys_pd_prl_msg_ext_status_hook(UPD_EXT_MSG_STATUS_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Status Hook:\n");
    }
}

void sys_pd_prl_msg_ext_get_bat_cap_hook(UPD_EXT_MSG_GET_BAT_CAP_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Get Battery Capability Hook:\n");
    }
    if (dir == PRL_MSG_DIR_TX)
    {
        
    }
    else {
        if (p_info->bat_cap_ref > 0)
        {
            g_sys_pd_info.invalid_bat_cap_ref = 1;
        }
    }
}

void sys_pd_prl_msg_ext_get_bat_status_hook(UPD_EXT_MSG_GET_BAT_STATUS_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Get Battery Status Hook:\n");
    }
    if (dir == PRL_MSG_DIR_TX)
    {
        
    }
    else {
        if (p_info->bat_status_ref > 0)
        {
            g_sys_pd_info.invalid_bat_sta_ref = 1;
        }
    }
}

void sys_pd_prl_msg_ext_bat_cap_hook(UPD_EXT_MSG_BAT_CAP_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Battery Capability Hook:\n");
    }
    if (dir == PRL_MSG_DIR_TX)
    {
        if (g_sys_pd_info.invalid_bat_cap_ref)
        {
            p_info->type = 1; // Invalid Battery reference
            p_info->design_capacity = 0xFFFF;
            g_sys_pd_info.invalid_bat_cap_ref = 0;
        }
    }
    else {
        
    }
}


void sys_pd_prl_msg_ext_get_mfr_info_hook(UPD_EXT_MSG_GET_MFR_INFO_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Get Manufacturer Info Hook:\n");
        if (1 == p_info->mfr_info_target)
        {
            printf(" Battery Reference: %u\n", p_info->mfr_info_ref);
        }
    }

    if (dir == PRL_MSG_DIR_RX)
    {
        g_sys_pd_info.rx_get_mfr_info.mfr_info_target = p_info->mfr_info_target;
        g_sys_pd_info.rx_get_mfr_info.mfr_info_ref = p_info->mfr_info_ref;
    }
}

void sys_pd_prl_msg_ext_mfr_info_hook(UPD_EXT_MSG_MFR_INFO_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Manufacturer Info Hook:\n");
        printf(" %-24s: %u\n", "VID", p_info->vid);
        printf(" %-24s: %u\n", "PID", p_info->pid);
        printf(" %-24s: %s\n", "String", p_info->mfr_str);
    }

    if (dir == PRL_MSG_DIR_TX)
    {
        if ((g_sys_pd_info.rx_get_mfr_info.mfr_info_target != 0) || 
            (g_sys_pd_info.rx_get_mfr_info.mfr_info_ref != 0))
        {
            memset((char *)p_info->mfr_str, 0, UPD_EXT_MSG_MFR_MAX_STRING_LEN);
            strcpy((char *)p_info->mfr_str, "Not Supported");
        }
    }
}


void sys_pd_prl_msg_ext_secur_req_hook(UPD_EXT_MSG_SECURITY_REQ_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    
}

void sys_pd_prl_msg_ext_secur_resp_hook(UPD_EXT_MSG_SECURITY_RESP_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    
}


void sys_pd_prl_msg_ext_fw_up_req_hook(void *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    
}

void sys_pd_prl_msg_ext_fw_up_resp_hook(void *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    
}


void sys_pd_prl_msg_ext_pps_status_hook(UPD_EXT_MSG_PPS_STATUS_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended PPS Status Hook:\n");
    }
}


void sys_pd_prl_msg_ext_country_info_hook(UPD_EXT_MSG_COUNTRY_INFO_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Country Info Hook:\n");
    }
}

void sys_pd_prl_msg_ext_country_codes_hook(UPD_EXT_MSG_COUNTRY_CODES_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Country Codes Hook:\n");
    }
}


void sys_pd_prl_msg_ext_snk_cap_hook(UPD_EXT_MSG_SNK_CAP_t *p_info, PRL_MSG_DIR_TYPE_t dir)
{
    if (!p_info)
        return;

    if (usbpd_cfg_dbg_get() >= USBPD_DEBUG_LEVEL_2) 
    {
        if (dir == PRL_MSG_DIR_TX)
        {
            drv_uart_putstr("\nTX ");
        }
        else {
            drv_uart_putstr("\nRX ");
        }
        drv_uart_putstr("Extended Sink Capability Hook:\n");
    }
}
bool_t sys_usbc_vbus_get()
{
    return ((drv_adc_read_multi_avg(SYS_USBC_VBUS_ADC_ID, 3) * drv_flash_trim_ref_volt_get() * SYS_USBC_VBUS_ADC_CONSTANT )>>12)/10;
}

void sys_pd_hook_init()
{
    memset((void *)&g_sys_pd_info, 0, sizeof(SYS_PD_INFO_t));

    g_sys_pd_info.sys_initializing = TRUE;

    sys_pd_led_state_set(SYS_PD_LED_STATE_ON);

#if SYS_PWM_CTRL_VOLT_ENABLED
    if (usbpd_cfg_port_src_pwm_en_get())
    {
        if (POWER_ROLE_SINK == usbpd_cfg_port_def_pr_get())
        {
            drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,FALSE);
        }
        else {
            drv_pwm_init_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,FALSE,usbpd_cfg_port_src_pwm_per_get());
            drv_pwm_duty_set2(SYS_PWM_INDEX_FOR_VOLT_CTRL, usbpd_cfg_port_src_fixed_pwr_pwm_duty_get(CFG_VOLTAGE_5V_IDX));
            drv_pwm_en_set(SYS_PWM_INDEX_FOR_VOLT_CTRL,usbpd_cfg_port_src_pwm_en_get());
        }
    }
#endif /* SYS_PWM_CTRL_VOLT_ENABLED */

#if SYS_QUICK_CHARGE_ENABLED
    sys_qc_init();
#endif

    usbpd_cc_hook_set(snk,unattached, sys_pd_sink_unattached_hook);
    usbpd_cc_hook_set(snk,attachwait, sys_pd_sink_attachwait_hook);
    usbpd_cc_hook_set(snk,attached, sys_pd_sink_attached_hook);
    usbpd_cc_hook_set(src,unattached, sys_pd_source_unattached_hook);
    usbpd_cc_hook_set(src,attachwait, sys_pd_source_attachwait_hook);
    usbpd_cc_hook_set(src,attached, sys_pd_source_attached_hook);

    usbpd_snk_transit_to_new_power_hook_set(sys_pd_sink_transit_to_new_power_hook);
    usbpd_src_transit_to_new_power_hook_set(sys_pd_source_transit_to_new_power_hook);

    usbpd_hook_set(src_vbus_en, sys_pd_vbus_source_en_set);
    usbpd_hook_set(discharge_en, sys_pd_vbus_discharge_en_set);
    usbpd_hook_set(cc1_vconn_en, sys_pd_cc1_vconn_en_set);
    usbpd_hook_set(cc2_vconn_en, sys_pd_cc2_vconn_en_set);

    usbpd_hook_with_return_set(src_vbus_en, sys_pd_vbus_source_en_get);
    usbpd_hook_with_return_set(discharge_en, sys_pd_vbus_discharge_en_get);
    usbpd_hook_with_return_set(cc1_vconn_en, sys_pd_cc1_vconn_en_get);
    usbpd_hook_with_return_set(cc2_vconn_en, sys_pd_cc2_vconn_en_get);
    usbpd_hook_with_return_set(c1_vbus_volt, sys_usbc_vbus_get);
    
    usbpd_event_hook_set(prs_started, sys_pd_prs_started_hook);
    usbpd_event_hook_set(prs_completed, sys_pd_prs_completed_hook);
    usbpd_event_hook_set(drs_started, sys_pd_drs_started_hook);
    usbpd_event_hook_set(drs_completed, sys_pd_drs_completed_hook);
    usbpd_event_hook_set(vcs_started, sys_pd_vcs_started_hook);
    usbpd_event_hook_set(vcs_completed, sys_pd_vcs_completed_hook);
    usbpd_event_hook_set(power_change_req, sys_pd_power_change_req_hook);
    usbpd_event_hook_set(idle, sys_pd_idle_hook);
    usbpd_event_hook_set(vdm_disc_ident_ack_received, sys_pd_vdm_disc_ident_ack_received_hook);
    usbpd_event_hook_set(prl_msg_received, sys_pd_prl_msg_received_hook);
    usbpd_event_hook_set(prl_msg_sent, sys_pd_prl_msg_sent_hook);

    usbpd_prl_msg_hook_set(vdm_attention, (v_func_args_ptr_t)sys_pd_prl_msg_vdm_attention_hook);
    usbpd_prl_msg_hook_set(vdm_unstructured, (v_func_args_ptr_t)sys_pd_prl_msg_vdm_unstructured_hook);

    usbpd_prl_msg_hook_set(alert, (v_func_args_ptr_t)sys_pd_prl_msg_alert_hook);
    usbpd_prl_msg_hook_set(get_country_info, (v_func_args_ptr_t)sys_pd_prl_msg_get_country_info_hook);
    usbpd_prl_msg_hook_set(battery_status, (v_func_args_ptr_t)sys_pd_prl_msg_battery_status_hook);
    usbpd_prl_msg_hook_set(ext_src_cap, (v_func_args_ptr_t)sys_pd_prl_msg_ext_src_cap_hook);
    usbpd_prl_msg_hook_set(ext_status, (v_func_args_ptr_t)sys_pd_prl_msg_ext_status_hook);
    usbpd_prl_msg_hook_set(ext_get_bat_cap, (v_func_args_ptr_t)sys_pd_prl_msg_ext_get_bat_cap_hook);
    usbpd_prl_msg_hook_set(ext_get_bat_status, (v_func_args_ptr_t)sys_pd_prl_msg_ext_get_bat_status_hook);
    usbpd_prl_msg_hook_set(ext_bat_cap, (v_func_args_ptr_t)sys_pd_prl_msg_ext_bat_cap_hook);
    usbpd_prl_msg_hook_set(ext_get_mfr_info, (v_func_args_ptr_t)sys_pd_prl_msg_ext_get_mfr_info_hook);
    usbpd_prl_msg_hook_set(ext_mfr_info, (v_func_args_ptr_t)sys_pd_prl_msg_ext_mfr_info_hook);
    usbpd_prl_msg_hook_set(ext_secur_req, (v_func_args_ptr_t)sys_pd_prl_msg_ext_secur_req_hook);
    usbpd_prl_msg_hook_set(ext_secur_resp, (v_func_args_ptr_t)sys_pd_prl_msg_ext_secur_resp_hook);
    usbpd_prl_msg_hook_set(ext_fw_up_req, (v_func_args_ptr_t)sys_pd_prl_msg_ext_fw_up_req_hook);
    usbpd_prl_msg_hook_set(ext_fw_up_resp, (v_func_args_ptr_t)sys_pd_prl_msg_ext_fw_up_resp_hook);
    usbpd_prl_msg_hook_set(ext_pps_status, (v_func_args_ptr_t)sys_pd_prl_msg_ext_pps_status_hook);
    usbpd_prl_msg_hook_set(ext_country_info, (v_func_args_ptr_t)sys_pd_prl_msg_ext_country_info_hook);
    usbpd_prl_msg_hook_set(ext_country_codes, (v_func_args_ptr_t)sys_pd_prl_msg_ext_country_codes_hook);
    usbpd_prl_msg_hook_set(ext_snk_cap, (v_func_args_ptr_t)sys_pd_prl_msg_ext_snk_cap_hook);
}


