/**
 * \file    sys_reg.c
 * \brief   System control register process engine
 *
 * \par     Copyright (c) 2020 UWE Technologies Corp., Ltd. All Rights Reserved.
 * \author  James
 * \date    2020-03-27
 * \version 
 * 
 * \history 
 *  Date        Note
 *  2020-03-27  Original Creation
 * 
 */

#include "libc.h"
#include "drivers.h"
#include "usbpd.h"
#include "sys_pd_config.h"
#include "sys_pd_hook.h"
#include "sys_qc_hook.h"
#include "sys_reg.h"
#if CLI_ENABLED
#include "cli_cmd_usbpd.h"
#endif

bool_t sys_reg_dummy_get(unsigned char *p_val)
{
#if CLI_ENABLED
    if (sys_cfg_dbg_get() > SYS_DEBUG_LEVEL_1) puts("sys_reg_dummy_get");
#endif /* CLI_ENABLED */
    return TRUE;
}

bool_t sys_reg_dummy_set(unsigned char *p_val)
{
#if CLI_ENABLED
    if (sys_cfg_dbg_get() > SYS_DEBUG_LEVEL_1) puts("sys_reg_dummy_set");
#endif /* CLI_ENABLED */
    return TRUE;
}

bool_t sys_pdo_info_reg_get(unsigned char *p_val)
{
    SYS_REG_PDO_INFO_t *p_info = (SYS_REG_PDO_INFO_t *)p_val;
    p_info->pdo_index = g_sys_pd_info.pdo_info.pdo_index;
    p_info->pdo_num = g_sys_pd_info.pdo_info.pdo_num;

    return TRUE;
}

bool_t sys_pdo_info_reg_set(unsigned char *p_val)
{
    SYS_REG_PDO_INFO_t *p_info = (SYS_REG_PDO_INFO_t *)p_val;

    if (p_info->pdo_index)
    {
        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(p_info->pdo_index-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(p_info->pdo_index-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(p_info->pdo_index-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 (p_info->pdo_index >= 8)
            {
                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(p_info->pdo_index-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(p_info->pdo_index-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.pdo_info.pdo_num = dpm_rx_src_cap_num_get();
        g_sys_pd_info.pdo_vbus = volt;
        g_sys_pd_info.pdo_ibus = current;
        g_sys_pd_info.pdo_info.pdo_index = p_info->pdo_index;
    }
    
    return TRUE;
}


// vbus
bool_t sys_pdo_vbus_hi_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_MSB_BYTE(g_sys_pd_info.pdo_vbus);
    return TRUE;
}

bool_t sys_pdo_vbus_lo_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_LSB_BYTE(g_sys_pd_info.pdo_vbus);
    return TRUE;
}

// ibus
bool_t sys_pdo_ibus_hi_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_MSB_BYTE(g_sys_pd_info.pdo_ibus);
    return TRUE;
}

bool_t sys_pdo_ibus_lo_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_LSB_BYTE(g_sys_pd_info.pdo_ibus);
    return TRUE;
}

bool_t sys_mcu_ctrl_reg_set(unsigned char *p_val)
{
    SYS_REG_MCU_CTRL_t *p_ctrl = (SYS_REG_MCU_CTRL_t *)p_val;
#if CLI_ENABLED
    if (sys_cfg_dbg_get() > SYS_DEBUG_LEVEL_1) printf("ctrl: %x, %x\n", *p_val, *(u8_t *)&(g_sys_pd_info.mcu_ctrl));
#endif /* CLI_ENABLED */
    if (p_ctrl->hard_reset)
    {
        dpm_source_ocp_flag_set(FALSE);
        if (pe_pd_connected_get())
        {
            dpm_hard_reset_req_set(TRUE);
        }
        else {
            DRV_REG_FIELD_SET(DPM_PRL_REG,HRST_REQ_FRM_PLCENG,TRUE);
        }
        sys_pd_cfg_snk_operating_power_profile_set(SNK_POWER_REQUIRE_SRC_MAX);
    }

    if (g_sys_pd_info.mcu_ctrl.disconnect != p_ctrl->disconnect)
    {
        if (!p_ctrl->disconnect)
        {
#if CLI_ENABLED
            if (sys_cfg_dbg_get()) printf("ctrl: connect\n");
#endif /* CLI_ENABLED */
            dpm_port_disabled_set(FALSE);
        }
        else 
        {
#if CLI_ENABLED
    	    if (sys_cfg_dbg_get()) printf("ctrl: disconnect\n");
#endif /* CLI_ENABLED */
            dpm_rp_rd_clear();
            dpm_cc_state_release();
            dpm_cc_state_init();
            pe_state_release();
            pe_state_init();
            dpm_info_reset();
            pe_local_hw_reset();
            prl_msg_parser_init();
            dpm_port_disabled_set(TRUE);
            g_sys_pd_info.vbus_requested = 0;
            g_sys_pd_info.ibus_requested = 0;
            sys_pd_cfg_snk_operating_power_profile_set(SNK_POWER_REQUIRE_SRC_MAX);
        }
    }

    if (g_sys_pd_info.mcu_ctrl.pd_off != p_ctrl->pd_off)
    {
        if (p_ctrl->pd_off)
        {
#if CLI_ENABLED
            if (sys_cfg_dbg_get()) printf("ctrl: PD off\n");
#endif /* CLI_ENABLED */
            usbpd_cfg_port_no_pd_mode_set(TRUE);
            dpm_port_reset();
        }
        else 
        {
#if CLI_ENABLED
            if (sys_cfg_dbg_get()) printf("ctrl: PD on\n");
#endif /* CLI_ENABLED */
            usbpd_cfg_port_no_pd_mode_set(FALSE);
            dpm_port_reset();
        }
    }

    if (p_ctrl->req_pdo_index)
    {
#if CLI_ENABLED
        if (sys_cfg_dbg_get()) printf("ctrl: request %d\n", p_ctrl->req_pdo_index);
#endif /* CLI_ENABLED */
        dpm_src_obj_pos_set(p_ctrl->req_pdo_index - 1);
        sys_new_power_level_req();
    }

    g_sys_pd_info.mcu_ctrl = *p_ctrl;
    return TRUE;
}

bool_t sys_mcu_ctrl_reg_get(unsigned char *p_val)
{
    *(SYS_REG_MCU_CTRL_t *)p_val = g_sys_pd_info.mcu_ctrl;
    return TRUE;
}

bool_t sys_pps_info_reg_get(unsigned char *p_val)
{
	UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed;
	*p_val = 0;
	for (int i = 0; i < dpm_rx_src_cap_num_get(); i++)
	{
	    p_src_cap_fixed = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(i);

	    if(POWER_SUPPLY_TYPE_PPS == p_src_cap_fixed->supply_type)
	    {
	    	*p_val |= (1 << i);
	    }
	}
    return TRUE;
}

u16_t pps_volt = 0;
u16_t pps_current = 0;
// vbus
bool_t sys_pps_vbus_hi_reg_set(unsigned char *p_val)
{
    pps_volt = *p_val;
    pps_volt <<= 8;
#if CLI_ENABLED
    if (sys_cfg_dbg_get() > SYS_DEBUG_LEVEL_1) printf("PPS: VBUSH %u\n", *p_val);
#endif /* CLI_ENABLED */
    return TRUE;
}

bool_t sys_pps_vbus_lo_reg_set(unsigned char *p_val)
{
    pps_volt += *p_val;
#if CLI_ENABLED
    if (sys_cfg_dbg_get() > SYS_DEBUG_LEVEL_1) printf("PPS: VBUSL %u\n", *p_val);
#endif /* CLI_ENABLED */
    usbpd_cfg_port_snk_pwr_volt_set( pps_volt / VOLTAGE_DIV );
    usbpd_cfg_port_snk_epr_pwr_volt_set( pps_volt / VOLTAGE_DIV );
    dpm_pps_output_voltage_set(pps_volt);
    usbpd_cfg_port_pps_volt_step_set(500);
    if ((g_sys_pd_info.pps_request_start_tick) &&
        (g_sys_pd_info.continuous_timer_tick-g_sys_pd_info.pps_request_start_tick > SYS_REG_PPS_REQUEST_TIMEOUT))
    {
        g_sys_pd_info.pps_request_start_tick = g_sys_pd_info.continuous_timer_tick;
    }
    return TRUE;
}

// ibus
bool_t sys_pps_ibus_hi_reg_set(unsigned char *p_val)
{
    pps_current = *p_val;
    pps_current <<= 8;
    return TRUE;
}

bool_t sys_pps_ibus_lo_reg_set(unsigned char *p_val)
{
    pps_current += *p_val;
    usbpd_cfg_port_snk_pwr_curr_set( pps_current / CURRENT_DIV );
    usbpd_cfg_port_snk_pwr_max_curr_set( pps_current / CURRENT_DIV );
    usbpd_cfg_port_snk_epr_pwr_curr_set( pps_current / CURRENT_DIV );
    usbpd_cfg_port_snk_epr_pwr_max_curr_set( pps_current / CURRENT_DIV );
    return TRUE;
}

// vbus
bool_t sys_pps_vbus_hi_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_MSB_BYTE(pps_volt);
    return TRUE;
}

bool_t sys_pps_vbus_lo_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_LSB_BYTE(pps_volt);
    return TRUE;
}

// ibus
bool_t sys_pps_ibus_hi_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_MSB_BYTE(pps_current);
    return TRUE;
}

bool_t sys_pps_ibus_lo_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_LSB_BYTE(pps_current);
    return TRUE;
}

bool_t sys_avs_info_reg_get(unsigned char *p_val)
{
    UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *p_src_cap_fixed;
    *p_val = 0;
    for (int i = 7; i < dpm_rx_src_cap_num_get(); i++)
    {
        p_src_cap_fixed = (UPD_DATA_MSG_SOURCE_CAP_FIXED_SUPPLY_t *)dpm_rx_src_cap_object_get(i);

        if(POWER_SUPPLY_TYPE_PPS == p_src_cap_fixed->supply_type)
        {
            *p_val |= (1 << (i-7));
        }
    }
    return TRUE;
}

bool_t sys_status_get(unsigned char *p_val)
{
    *(SYS_REG_STATUS_t *)p_val = g_sys_pd_info.status;
    return TRUE;
}

// vbus
bool_t sys_vbus_hi_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_MSB_BYTE(g_sys_pd_info.vbus_current);
    return TRUE;
}

bool_t sys_vbus_lo_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_LSB_BYTE(g_sys_pd_info.vbus_current);
    return TRUE;
}

// ibus
bool_t sys_ibus_hi_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_MSB_BYTE(g_sys_pd_info.ibus_current);
    return TRUE;
}

bool_t sys_ibus_lo_reg_get(unsigned char *p_val)
{
    *p_val = SAL_U16_LSB_BYTE(g_sys_pd_info.ibus_current);
    return TRUE;
}

bool_t sys_all_pdo_reg(unsigned char *p_val)
{
    int i;
    for (i = 0; i < dpm_rx_src_cap_num_get(); i++)
    {
        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(i);
        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(i);
            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(i);
            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 (i >= 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(i);
                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(i);
                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;
        }
        *p_val++ = SAL_U16_MSB_BYTE(volt);
        *p_val++ = SAL_U16_LSB_BYTE(volt);
        *p_val++ = SAL_U16_MSB_BYTE(current);
        *p_val++ = SAL_U16_LSB_BYTE(current);
    }
    if (dpm_rx_src_cap_num_get())
    {
        return i * 4;
    }
    else {
        *p_val++ = SAL_U16_MSB_BYTE(VOLTAGE_5V);
        *p_val++ = SAL_U16_LSB_BYTE(VOLTAGE_5V);
        *p_val++ = SAL_U16_MSB_BYTE(CURRENT_1000mA);
        *p_val++ = SAL_U16_LSB_BYTE(CURRENT_1000mA);
        return 4;
    }
}


typedef bool_t (*sys_reg_opr_fptr)(unsigned char *p_val);

static const sys_reg_opr_fptr sys_reg_read_entries[SYS_REG_MAX_NUM] =
{
    sys_pdo_info_reg_get,
    sys_pdo_vbus_hi_reg_get,
    sys_pdo_vbus_lo_reg_get,
    sys_pdo_ibus_hi_reg_get,
    sys_pdo_ibus_lo_reg_get,
    sys_mcu_ctrl_reg_get,
    sys_pps_info_reg_get,
    sys_pps_vbus_hi_reg_get,
    sys_pps_vbus_lo_reg_get,
    sys_pps_ibus_hi_reg_get,
    sys_pps_ibus_lo_reg_get,
    sys_avs_info_reg_get,
    sys_status_get,
    sys_vbus_hi_reg_get,
    sys_vbus_lo_reg_get,
    sys_ibus_hi_reg_get,
    sys_ibus_lo_reg_get,
    sys_all_pdo_reg
};

static const sys_reg_opr_fptr sys_reg_write_entries[SYS_REG_MAX_NUM] =
{
    sys_pdo_info_reg_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_mcu_ctrl_reg_set,
    sys_reg_dummy_set,
    sys_pps_vbus_hi_reg_set,
    sys_pps_vbus_lo_reg_set,
    sys_pps_ibus_hi_reg_set,
    sys_pps_ibus_lo_reg_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
    sys_reg_dummy_set,
};


bool_t sys_reg_read(unsigned char reg, unsigned char *p_val)
{
    bool_t ret = FALSE;

    if (reg < SYS_REG_MAX_NUM)
    {
        ret = sys_reg_read_entries[reg](p_val);
    }

    return ret;
}


bool_t sys_reg_write(unsigned char reg, unsigned char *p_val)
{
    bool_t ret = FALSE;

    if (reg < SYS_REG_MAX_NUM)
    {
        ret = sys_reg_write_entries[reg](p_val);
    }

    return ret;
}
