
/**
 * PD_UFP_Protocol.cpp
 *
 *      Author: Ryan Ma
 *      Edited: Kai Liebich
 *
 * Minimalist USB PD implement with only UFP(device) sink only functionality
 * Requires PD PHY to do automatic GoodCRC response on valid SOP messages.
 * Requires only stdint.h, stdbool.h and string.h
 * No use of bit-field for better cross-platform compatibility
 *
 * Support PD3.0 PPS
 * Do not support extended message. Not necessary for PD trigger and PPS.
 * 
 * Reference: USB_PD_R2_0 V1.3 - 20170112
 *            USB_PD_R3_0 V2.0 20190829 + ECNs 2020-12-10
 *            - Chapter 6. Protocol Layer
 *
 */
 
#include <string.h>
#include "PD_UFP_Protocol.h"

#define PD_SPECIFICATION_REVISION           0x2

#define PD_CONTROL_MSG_TYPE_ACCEPT          0x3
#define PD_CONTROL_MSG_TYPE_REJECT          0x4
#define PD_CONTROL_MSG_TYPE_GET_SRC_CAP     0x7
#define PD_CONTROL_MSG_TYPE_NOT_SUPPORT     0x10
#define PD_CONTROL_MSG_TYPE_GET_PPS_STATUS  0x14

#define PD_DATA_MSG_TYPE_REQUEST            0x2
#define PD_DATA_MSG_TYPE_SINK_CAP           0x4
#define PD_DATA_MSG_TYPE_VENDOR_DEFINED     0xF

#define PD_EXT_MSG_TYPE_SINK_CAP_EXT        0xF

typedef struct {
    uint8_t type;
    uint8_t spec_rev;
    uint8_t id;
    uint8_t num_of_obj;
} PD_msg_header_info_t;

typedef struct {
    uint16_t limit;
    uint8_t use_voltage;
    uint8_t use_current;
} PD_power_option_setting_t;

struct PD_msg_state_t {
    const char * name;
    void (*handler)(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
    bool (*responder)(PD_protocol_t * p, uint16_t * header, uint32_t * obj);
};

/* Optimize RAM usage on AVR MCU by allocate const in PROGMEM */
#if defined(__AVR__)
#include <avr/pgmspace.h>
#define SET_MSG_STAGE(d, s) do { static struct PD_msg_state_t m; memcpy_P(&m, s, sizeof(struct PD_msg_state_t)); d = &m; } while (0)
#define SET_MSG_NAME(d, s)  do { static char n[16]; strncpy_P(n, s, 15); d = n; } while (0)
#define COPY_PDO(d, s)      do { memcpy_P(&d, &s, 4); } while (0)
#else
#define PROGMEM
//#define SET_MSG_STAGE(d, s) do { d = s; } while (0)
#define SET_MSG_STAGE(d, s) do { static struct PD_msg_state_t m; m=*(s); d = &m; } while (0)
//#define SET_MSG_NAME(d, s)  do { d = s; } while (0)
#define SET_MSG_NAME(d, s)  do { static char n[16]; strncpy(n, s, 15); d = n; } while (0)
#define COPY_PDO(d, s)      do { d = s; } while (0)
#endif

#define T(name) static const char str_ ## name [] PROGMEM = #name

static void handler_good_crc   (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_goto_min   (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_accept     (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_reject     (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_ps_rdy     (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_source_cap (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_BIST       (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_alert      (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_vender_def (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);
static void handler_PPS_Status (PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events);

static bool responder_get_sink_cap  (PD_protocol_t * p, uint16_t * header, uint32_t * obj);
static bool responder_reject        (PD_protocol_t * p, uint16_t * header, uint32_t * obj);
static bool responder_soft_reset    (PD_protocol_t * p, uint16_t * header, uint32_t * obj);
static bool responder_source_cap    (PD_protocol_t * p, uint16_t * header, uint32_t * obj);
static bool responder_vender_def    (PD_protocol_t * p, uint16_t * header, uint32_t * obj);
static bool responder_sink_cap_ext  (PD_protocol_t * p, uint16_t * header, uint32_t * obj);
static bool responder_not_support   (PD_protocol_t * p, uint16_t * header, uint32_t * obj);

T(C0); T(GoodCRC); T(GotoMin); T(Accept); T(Reject); T(Ping); T(PS_RDY); T(Get_Src_Cap);
T(Get_Sink_Cap); T(DR_Swap); T(PR_Swap); T(VCONN_Swap); T(Wait); T(Soft_Rst); T(Dat_Rst); T(Dat_Rst_Cpt);
T(NS); T(Get_Src_Ext); T(Get_Stat); T(FR_Swap); T(Get_PPS_Stat); T(Get_CC); T(Get_Sink_Ext); T(C_R);

static const struct PD_msg_state_t ctrl_msg_list[] PROGMEM = {
    {.name = str_C0,            .handler = 0,                   .responder = 0},
    {.name = str_GoodCRC,       .handler = handler_good_crc,    .responder = 0},
    {.name = str_GotoMin,       .handler = handler_goto_min,    .responder = 0},
    {.name = str_Accept,        .handler = handler_accept,      .responder = 0},
    {.name = str_Reject,        .handler = handler_reject,      .responder = 0},
    {.name = str_Ping,          .handler = 0,                   .responder = 0},
    {.name = str_PS_RDY,        .handler = handler_ps_rdy,      .responder = 0},
    {.name = str_Get_Src_Cap,   .handler = 0,                   .responder = responder_not_support},
    {.name = str_Get_Sink_Cap,  .handler = 0,                   .responder = responder_get_sink_cap},
    {.name = str_DR_Swap,       .handler = 0,                   .responder = responder_reject},
    {.name = str_PR_Swap,       .handler = 0,                   .responder = responder_not_support},
    {.name = str_VCONN_Swap,    .handler = 0,                   .responder = responder_reject},
    {.name = str_Wait,          .handler = 0,                   .responder = 0},
    {.name = str_Soft_Rst,      .handler = 0,                   .responder = responder_soft_reset},
    {.name = str_Dat_Rst,       .handler = 0,                   .responder = 0},
    {.name = str_Dat_Rst_Cpt,   .handler = 0,                   .responder = 0},
    
    {.name = str_NS,            .handler = 0,                   .responder = 0},
    {.name = str_Get_Src_Ext,   .handler = 0,                   .responder = responder_not_support},
    {.name = str_Get_Stat,      .handler = 0,                   .responder = responder_not_support},
    {.name = str_FR_Swap,       .handler = 0,                   .responder = responder_not_support},
    {.name = str_Get_PPS_Stat,  .handler = 0,                   .responder = responder_not_support},
    {.name = str_Get_CC,        .handler = 0,                   .responder = responder_not_support},
    {.name = str_Get_Sink_Ext,  .handler = 0,                   .responder = responder_sink_cap_ext},
    {.name = str_C_R,           .handler = 0,                   .responder = responder_not_support},
};

T(D0); T(Src_Cap); T(Request); T(BIST); T(Sink_Cap); T(Bat_Stat); T(Alert); T(Get_CI);
T(Enter_USB); T(D9); T(D10); T(D11); T(D12); T(D13); T(D14); T(VDM);
T(D_R); 

static const struct PD_msg_state_t data_msg_list[] PROGMEM = {
    {.name = str_D0,            .handler = 0,                   .responder = 0},
    {.name = str_Src_Cap,       .handler = handler_source_cap,  .responder = responder_source_cap},
    {.name = str_Request,       .handler = 0,                   .responder = responder_not_support},
    {.name = str_BIST,          .handler = handler_BIST,        .responder = 0},
    {.name = str_Sink_Cap,      .handler = 0,                   .responder = responder_not_support},
    {.name = str_Bat_Stat,      .handler = 0,                   .responder = responder_not_support},
    {.name = str_Alert,         .handler = handler_alert,       .responder = 0},
    {.name = str_Get_CI,        .handler = 0,                   .responder = responder_not_support},
    {.name = str_Enter_USB,     .handler = 0,                   .responder = 0},
    {.name = str_D9,            .handler = 0,                   .responder = 0},
    {.name = str_D10,           .handler = 0,                   .responder = 0},
    {.name = str_D11,           .handler = 0,                   .responder = 0},
    {.name = str_D12,           .handler = 0,                   .responder = 0},
    {.name = str_D13,           .handler = 0,                   .responder = 0},
    {.name = str_D14,           .handler = 0,                   .responder = 0},
    {.name = str_VDM,           .handler = handler_vender_def,  .responder = responder_vender_def},

    {.name = str_D_R,           .handler = 0,                   .responder = responder_not_support},
};

T(E0); T(Src_Cap_Ext); T(Status); T(Get_Bat_cap); T(Get_Bat_Stat); T(Bat_Cap); T(Get_Mfg_Info); T(Mfg_Info);
T(Sec_Request); T(Sec_Response); T(FU_request); T(FU_Response); T(PPS_Stat); T(Country_Info); T(Country_Code); T(Sink_Cap_Ext);
T(E_R);

static const struct PD_msg_state_t ext_msg_list[] PROGMEM = {
    {.name = str_E0,            .handler = 0,                   .responder = responder_not_support},
    {.name = str_Src_Cap_Ext,   .handler = 0,                   .responder = 0},
    {.name = str_Status,        .handler = 0,                   .responder = 0},
    {.name = str_Get_Bat_cap,   .handler = 0,                   .responder = responder_not_support},
    {.name = str_Get_Bat_Stat,  .handler = 0,                   .responder = responder_not_support},
    {.name = str_Bat_Cap,       .handler = 0,                   .responder = 0},
    {.name = str_Get_Mfg_Info,  .handler = 0,                   .responder = responder_not_support},
    {.name = str_Mfg_Info,      .handler = 0,                   .responder = 0},
    {.name = str_Sec_Request,   .handler = 0,                   .responder = responder_not_support},
    {.name = str_Sec_Response,  .handler = 0,                   .responder = 0},
    {.name = str_FU_request,    .handler = 0,                   .responder = responder_not_support},
    {.name = str_FU_Response,   .handler = 0,                   .responder = 0},
    {.name = str_PPS_Stat,      .handler = handler_PPS_Status,  .responder = 0},
    {.name = str_Country_Info,  .handler = 0,                   .responder = 0},
    {.name = str_Country_Code,  .handler = 0,                   .responder = 0},
    {.name = str_Sink_Cap_Ext,  .handler = 0,                   .responder = responder_not_support},

    {.name = str_E_R,           .handler = 0,                   .responder = responder_not_support},
};

static const PD_power_option_setting_t power_option_setting[8] = {
    {.limit = 25,   .use_voltage = 1, .use_current = 0},    /* PD_POWER_OPTION_MAX_5V */
    {.limit = 45,   .use_voltage = 1, .use_current = 0},    /* PD_POWER_OPTION_MAX_9V */
    {.limit = 60,   .use_voltage = 1, .use_current = 0},    /* PD_POWER_OPTION_MAX_12V */
    {.limit = 75,   .use_voltage = 1, .use_current = 0},    /* PD_POWER_OPTION_MAX_15V */
    {.limit = 100,  .use_voltage = 1, .use_current = 0},    /* PD_POWER_OPTION_MAX_20V */
    {.limit = 100,  .use_voltage = 1, .use_current = 0},    /* PD_POWER_OPTION_MAX_VOLTAGE */
    {.limit = 125,  .use_voltage = 0, .use_current = 1},    /* PD_POWER_OPTION_MAX_CURRENT */
    {.limit = 12500,.use_voltage = 1, .use_current = 1},    /* PD_POWER_OPTION_MAX_POWER */  
};

static uint8_t evaluate_src_cap(PD_protocol_t * p, uint16_t PPS_voltage, uint8_t PPS_current)
{
    const PD_power_option_setting_t * setting;
    PD_power_info_t info;
    uint8_t option = p->power_option;
    uint8_t selected = 0;

    /* If selected option is not available, use first PDO. Reference: 6.4.1 Capabilities Message
       The vSafe5V Fixed Supply Object Shall always be the first object. */
    if (option >= sizeof(power_option_setting) / sizeof(power_option_setting[0])) {
        return 0;
    }

    setting = &power_option_setting[option];
    for (uint8_t n = 0; PD_protocol_get_power_info(p, n, &info); n++) {
        if (info.type == PD_PDO_TYPE_AUGMENTED_PDO) {
            uint16_t pps_v = PPS_voltage * 2;    /* Voltage in 20mV units */
            uint16_t pps_i = PPS_current * 5;    /* Current in 50mA units */
            /* PD_power_info_t: Voltage in 50mV units, Current in 10mA units */
            if (info.min_v * 5 <= pps_v && pps_v <= info.max_v * 5 && pps_i <= info.max_i) {
                return n;
            }
        } else {
            uint8_t v = setting->use_voltage ? info.max_v >> 2 : 1;
            uint8_t i = setting->use_current ? info.max_i >> 2 : 1;
            uint16_t power = (uint16_t)v * i;  /* reduce 10-bit power info to 8-bit and use 8-bit x 8-bit multiplication */
            if (power <= setting->limit) {
                selected = n;
            }
        }
    }
    return selected;
}

static void parse_header(PD_msg_header_info_t * info, uint16_t header)
{
    /* Reference: 6.2.1.1 Message Header */ 
    info->type = (header >> 0) & 0x1F;                  /*   4...0  Message Type */
    info->spec_rev = (header >> 6) & 0x3;               /*   7...6  Specification Revision */
    info->id = (header >> 9) & 0x7;                     /*  11...9  MessageID */
    info->num_of_obj = (header >> 12) & 0x7;            /* 14...12  Number of Data Objects */
}

static uint16_t generate_header(PD_protocol_t * p, uint8_t type, uint8_t obj_count)
{
    /* Reference: 6.2.1.1 Message Header */ 
    uint16_t h = ((uint16_t)type << 0) |                      /*   4...0  Message Type */
                 ((uint16_t)PD_SPECIFICATION_REVISION << 6) | /*   7...6  Specification Revision */
                 ((uint16_t)p->message_id << 9) |             /*  11...9  MessageID */
                 ((uint16_t)obj_count << 12);                 /* 14...12  Number of Data Objects */
    p->tx_msg_header = h;
    return h;
}

static uint16_t generate_header_ext(PD_protocol_t * p, uint8_t type, uint8_t data_size, uint32_t * obj)
{
    uint16_t h = generate_header(p, type, (data_size + 5) >> 2); /* set obj_count to fit ext header and data */
    h |= (uint16_t)1 << 15;     /* Set extended field */
    /* Reference: 6.2.1.2 Extended Message Headerr */ 
    obj[0] |= ((uint16_t)data_size << 0) |  /*   8...0  ata Size */
              /* Assume short message, set Chunk Number and Request Chunk to 0 */
              ((uint16_t)1 << 15);          /*      15  Chunked */
    p->tx_msg_header = h;
    return h;
}

static void handler_good_crc(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    /* Reference: 6.2.1.3 Message ID 
       MessageIDCounter Shall be initialized to zero at power-on / reset, increment when receive GoodCRC Message */
    uint8_t message_id = p->message_id;
    if (++message_id > 7) {
        message_id = 0;
    }
    p->message_id = message_id;
}

static void handler_goto_min(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    // Not implemented
}

static void handler_accept(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    if (events) {
        *events |= PD_PROTOCOL_EVENT_ACCEPT;
    }
}

static void handler_reject(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    if (events) {
        *events |= PD_PROTOCOL_EVENT_PS_RDY;
    }
}

static void handler_ps_rdy(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    if (events) {
        *events |= PD_PROTOCOL_EVENT_PS_RDY;
    }
}

static void handler_source_cap(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    PD_msg_header_info_t h;
    parse_header(&h, header);
    p->power_data_obj_count = h.num_of_obj;
    for (uint8_t i = 0; i < h.num_of_obj; i++) {
        p->power_data_obj[i] = obj[i];
    }
    p->power_data_obj_selected = evaluate_src_cap(p, p->PPS_voltage, p->PPS_current);
    if (events) {
        *events |= PD_PROTOCOL_EVENT_SRC_CAP;
    }
}

static void handler_BIST(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    // TODO: implement BIST
}

static void handler_alert(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    // TODO: implement alert
}

static void handler_vender_def(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    // TODO: implement VDM parsing
}

static void handler_PPS_Status(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    /* Handle chunked Extended message,  Offset 2 byte for Extended Message Header */
    p->PPSSDB[0] = (obj[0] >> 16) & 0xFF;
    p->PPSSDB[1] = (obj[0] >> 24) & 0xFF;
    p->PPSSDB[2] = (obj[1] >>  0) & 0xFF;
    p->PPSSDB[3] = (obj[1] >>  8) & 0xFF;
    if (events) {
        *events |= PD_PROTOCOL_EVENT_PPS_STATUS;
    }
}

static bool responder_get_sink_cap(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    /* Reference: 6.4.1.2.3 Sink Fixed Supply Power Data Object */
    uint32_t data = ((uint32_t)100 << 0) |                        /* B9...0     Operational Current in 10mA units */
                    ((uint32_t)100 << 10) |                       /* B19...10   Voltage in 50mV units */
                    ((uint32_t)1 << 26) |                         /* B26        USB Communications Capable */
                    ((uint32_t)1 << 28) |                         /* B28        Higher Capability */
                    ((uint32_t)PD_PDO_TYPE_FIXED_SUPPLY << 30);   /* B31...30   Fixed supply */
    *obj = data; /* Only implement 5V 1A Fix supply PDO. Source rarely request sink cap */
    *header = generate_header(p, PD_DATA_MSG_TYPE_SINK_CAP, 1);
    return true;
}

static bool responder_sink_cap_ext(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    /* Reference: 6.5.13 Sink_Capabilities_Extended Message 
                  6.12.3 Applicability of Extended Messages  (Normative; Shall be supported) */
    #define SINK_CAP_VID                0
    #define SINK_CAP_PID                0
    #define SINK_CAP_XID                0       /* If the vendor does not have an XID, then it Shall return zero */
    #define SINK_CAP_FW_Version         1
    #define SINK_CAP_HW_Version         1
    #define SINK_CAP_SKEDB_Version      1
    #define SINK_CAP_SINK_MODE          0x3     /* Bit 0: PPS charging supported, Bit 1: VBUS powered */
    #define SINK_CAP_SINK_MIN_PDP       5       /* Minimum     PD Power in Watt */
    #define SINK_CAP_SINK_OP_PDP        5       /* Operational PD Power in Watt */
    #define SINK_CAP_SINK_MAX_PDP       100     /* Maximum     PD Power in Watt */
    static const uint32_t SKEDB[6] PROGMEM = { /* 2-byte header + 21-byte data, chunked to 6 PDO */
    /* PDO[0], data byte  0...1  */
        /* 16-bit LSB is reserved for Extended Message Header */
        ((uint32_t)SINK_CAP_VID << 16),             /* Byte  0...1  VID */
    /* PDO[1], data byte  2...5  */
        ((uint32_t)SINK_CAP_PID << 0) |             /* Byte  2...3  PID */
        (((uint32_t)SINK_CAP_XID & 0xFF) << 16),    /* Byte  4...5  XID */
    /* PDO[2], data byte  6...9  */
        (((uint32_t)SINK_CAP_XID >> 16) << 0) |     /* Byte  6...7  XID */
        ((uint32_t)SINK_CAP_FW_Version << 16) |     /* Byte      8  FW Version */
        ((uint32_t)SINK_CAP_HW_Version << 24),      /* Byte      9  HW Version */
    /* PDO[3], data byte 10...13 */
        ((uint32_t)SINK_CAP_SKEDB_Version << 0),    /* Byte     10  SKEDB Version */
        /* Not set Byte 11 Load Step, Byte 13..12 Sink Load Characteristics */
    /* PDO[4], data byte 14...17 */
        /* Not set Byte 14 Compliance, Byte 15 Touch Temp, Byte 16 Battery Info */
        ((uint32_t)SINK_CAP_SINK_MODE << 24),       /* Byte     17  Sink Modes */
    /* PDO[5], data byte 18...20 */
        ((uint32_t)SINK_CAP_SINK_MIN_PDP << 0) |    /* Byte     18  Minimum PDP */
        ((uint32_t)SINK_CAP_SINK_OP_PDP << 8) |     /* Byte     19  Operational PDP */
        ((uint32_t)SINK_CAP_SINK_MAX_PDP << 16)     /* Byte     20  Maximum PDP */
    };
    uint8_t i;
    for (i = 0; i < 6; i++) {
        COPY_PDO(obj[i], SKEDB[i]);
    }
    *header = generate_header_ext(p, PD_EXT_MSG_TYPE_SINK_CAP_EXT, 21, obj);
    return false;
}

static bool responder_reject(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    *header = generate_header(p, PD_CONTROL_MSG_TYPE_REJECT, 0);
    return true;
}

static bool responder_not_support(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    *header = generate_header(p, PD_CONTROL_MSG_TYPE_NOT_SUPPORT, 0);
    return true;
}

static bool responder_soft_reset(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    *header = generate_header(p, PD_CONTROL_MSG_TYPE_ACCEPT, 0);
    return true;
}

static bool responder_source_cap(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    PD_power_info_t info;
    uint32_t data, pos = p->power_data_obj_selected + 1;
    PD_protocol_get_power_info(p, p->power_data_obj_selected, &info);
    /* Reference: 6.4.2 Request Message */
    if (info.type == PD_PDO_TYPE_AUGMENTED_PDO) {
        /* NOTE: To compatible PD2.0 PHY, do not set Unchunked Extended Messages Supported */
        data = ((uint32_t)p->PPS_current << 0) |    /* B6 ...0    Operating Current 50mA units */
               ((uint32_t)p->PPS_voltage << 9) |    /* B19...9    Output Voltage in 20mV units */
               ((uint32_t)1 << 25) |                /* B25        USB Communication Capable */
               ((uint32_t)pos << 28);               /* B30...28   Object position (000b is Reserved and Shall Not be used) */
    } else {
        uint32_t req = info.max_i ? info.max_i : info.max_p;
        data = ((uint32_t)req << 0) |    /* B9 ...0    Max Operating Current 10mA units / Max Operating Power in 250mW units */
               ((uint32_t)req << 10) |   /* B19...10   Operating Current 10mA units / Operating Power in 250mW units */
               ((uint32_t)1 << 25) |     /* B25        USB Communication Capable */
               ((uint32_t)pos << 28);    /* B30...28   Object position (000b is Reserved and Shall Not be used) */
    }
    *obj = data;
    *header = generate_header(p, PD_DATA_MSG_TYPE_REQUEST, 1);
    return true;
}

static bool responder_vender_def(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    // TODO: implement VDM respond
    return false;
}

void PD_protocol_handle_msg(PD_protocol_t * p, uint16_t header, uint32_t * obj, PD_protocol_event_t * events)
{
    #define EXT_MSG_LIMIT   (sizeof(ext_msg_list) / sizeof(ext_msg_list[0]) - 1)
    #define DATA_MSG_LIMIT  (sizeof(data_msg_list) / sizeof(data_msg_list[0]) - 1)
    #define CTRL_MSG_LIMIT  (sizeof(ctrl_msg_list) / sizeof(ctrl_msg_list[0]) - 1)

    const struct PD_msg_state_t * state;
    PD_msg_header_info_t h;
    parse_header(&h, header);
    p->rx_msg_header = header;
    if ((header >> 15) & 0x1) {
        state = &ext_msg_list[h.type > EXT_MSG_LIMIT ? EXT_MSG_LIMIT : h.type];
    } else if (h.num_of_obj) {
        state = &data_msg_list[h.type > DATA_MSG_LIMIT ? DATA_MSG_LIMIT : h.type];
    } else {
        state =&ctrl_msg_list[h.type > CTRL_MSG_LIMIT ? CTRL_MSG_LIMIT : h.type];
    }
    SET_MSG_STAGE(p->msg_state, state);
    if (p->msg_state->handler) {
        p->msg_state->handler(p, header, obj, events);
    }
}

bool PD_protocol_respond(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    if (p && p->msg_state && p->msg_state->responder && header && obj) {
        return p->msg_state->responder(p, (uint16_t *)header, obj);
    }
    return false;
}

void PD_protocol_create_get_src_cap(PD_protocol_t * p, uint16_t * header)
{
    *header = generate_header(p, PD_CONTROL_MSG_TYPE_GET_SRC_CAP, 0);
}

void PD_protocol_create_get_PPS_status(PD_protocol_t *p, uint16_t *header)
{
    *header = generate_header(p, PD_CONTROL_MSG_TYPE_GET_PPS_STATUS, 0);
}

void PD_protocol_create_request(PD_protocol_t * p, uint16_t * header, uint32_t * obj)
{
    responder_source_cap(p, header, obj);
}

bool PD_protocol_get_power_info(PD_protocol_t * p, uint8_t index, PD_power_info_t * power_info)
{
    if (p && index < p->power_data_obj_count && power_info) {
        uint32_t obj = p->power_data_obj[index];
        power_info->type = (PD_power_data_obj_type_t)(obj >> 30);
        switch (power_info->type) {
        case PD_PDO_TYPE_FIXED_SUPPLY:
            /* Reference: 6.4.1.2.3 Source Fixed Supply Power Data Object */
            power_info->min_v = 0;
            power_info->max_v = (obj >> 10) & 0x3FF;    /*  B19...10  Voltage in 50mV units */
            power_info->max_i = (obj >>  0) & 0x3FF;    /*  B9 ...0   Max Current in 10mA units */
            power_info->max_p = 0;
            break;
        case PD_PDO_TYPE_BATTERY:
            /* Reference: 6.4.1.2.5 Battery Supply Power Data Object */
            power_info->min_v = (obj >> 10) & 0x3FF;    /*  B19...10  Min Voltage in 50mV units */
            power_info->max_v = (obj >> 20) & 0x3FF;    /*  B29...20  Max Voltage in 50mV units */
            power_info->max_i = 0;
            power_info->max_p = (obj >>  0) & 0x3FF;    /*  B9 ...0   Max Allowable Power in 250mW units */
            break;
        case PD_PDO_TYPE_VARIABLE_SUPPLY:
            /* Reference: 6.4.1.2.4 Variable Supply (non-Battery) Power Data Object */
            power_info->min_v = (obj >> 10) & 0x3FF;    /*  B19...10  Min Voltage in 50mV units */
            power_info->max_v = (obj >> 20) & 0x3FF;    /*  B29...20  Max Voltage in 50mV units */
            power_info->max_i = (obj >>  0) & 0x3FF;    /*  B9 ...0   Max Current in 10mA units */
            power_info->max_p = 0;
            break;
        case PD_PDO_TYPE_AUGMENTED_PDO:
            /* Reference: 6.4.1.3.4 Programmable Power Supply Augmented Power Data Object */
            power_info->max_v = ((obj >> 17) & 0xFF) * 2;   /*  B24...17  Max Voltage in 100mV units */
            power_info->min_v = ((obj >>  8) & 0xFF) * 2;   /*  B15...8   Min Voltage in 100mV units */
            power_info->max_i = ((obj >>  0) & 0x7F) * 5;   /*  B6 ...0   Max Current in 50mA units */
            power_info->max_p = 0;
            break;
        }
        return true;
    }
    return false;
}

bool PD_protocol_get_msg_info(uint16_t header, PD_msg_info_t * msg_info)
{
    PD_msg_header_info_t h;
    parse_header(&h, header);
    if (msg_info) {
        const char * name;
        const struct PD_msg_state_t * state;
        uint8_t type = h.type;
        SET_MSG_STAGE(state, header & 0x8000 ? &ext_msg_list[type] :
                        h.num_of_obj ? &data_msg_list[type] : &ctrl_msg_list[type]);
        SET_MSG_NAME(name, state->name);
        msg_info->name = name;
        msg_info->id = h.id;
        msg_info->spec_rev = h.spec_rev;
        msg_info->num_of_obj = h.num_of_obj;
        msg_info->extended = header >> 15;
        return true;
    }
    return false;
}

bool PD_protocol_get_PPS_status(PD_protocol_t *p, PPS_status_t * PPS_status)
{
    if (p && PPS_status) {
        /* Reference: 6.5.10 PPS_Status Message */
        PPS_status->output_voltage = ((uint16_t)p->PPSSDB[1] << 8) | p->PPSSDB[0];
        PPS_status->output_current = p->PPSSDB[2];
        PPS_status->flag_PTF = (PPS_PTF_t)((p->PPSSDB[3] >> 1) & 0x3);   /* Bit 1 ... 2 */
        PPS_status->flag_OMF = (PPS_OMF_t)((p->PPSSDB[3] >> 3) & 0x1);   /* Bit 3 */
        return true;
    }
    return false;
}

bool PD_protocol_set_power_option(PD_protocol_t * p, enum PD_power_option_t option)
{
    p->power_option = option;
    p->PPS_voltage = 0;
    p->PPS_current = 0;
    if (p->power_data_obj_count > 0) {
        p->power_data_obj_selected = evaluate_src_cap(p, p->PPS_voltage, p->PPS_current);
        return true;    /* need to re-send request */
    }
    return false;
}

bool PD_protocol_select_power(PD_protocol_t * p, uint8_t index)
{
    if (index < p->power_data_obj_count) {
        p->power_data_obj_selected = index;
        return true;    /* need to re-send request */
    }
    return false;
}

bool PD_protocol_set_PPS(PD_protocol_t * p, uint16_t PPS_voltage, uint8_t PPS_current, bool strict)
{
    if (p->PPS_voltage != PPS_voltage || p->PPS_current != PPS_current) {
        uint8_t selected = evaluate_src_cap(p, PPS_voltage, PPS_current);
        if (selected || !strict) {
            p->PPS_voltage = PPS_voltage;
            p->PPS_current = PPS_current;
            p->power_data_obj_selected = selected;
            return true;    /* need to re-send request */            
        }
    }
    return false;
}

void PD_protocol_reset(PD_protocol_t * p)
{
    p->msg_state = &ctrl_msg_list[0];
    p->message_id = 0;
}

void PD_protocol_init(PD_protocol_t * p)
{
    memset(p, 0, sizeof(PD_protocol_t));
    p->msg_state = &ctrl_msg_list[0];
}
