diff --git a/drivers/usb/pd/policy_engine.c b/drivers/usb/pd/policy_engine.c index af64c85027b9..e6abc71493f3 100644 --- a/drivers/usb/pd/policy_engine.c +++ b/drivers/usb/pd/policy_engine.c @@ -10,30 +10,225 @@ * GNU General Public License for more details. */ +#include #include #include #include #include #include #include +#include #include +#include #include #include "usbpd.h" +enum usbpd_state { + PE_UNKNOWN, + PE_ERROR_RECOVERY, + PE_SRC_DISABLED, + PE_SRC_STARTUP, + PE_SRC_SEND_CAPABILITIES, + PE_SRC_SEND_CAPABILITIES_WAIT, /* substate to wait for Request */ + PE_SRC_NEGOTIATE_CAPABILITY, + PE_SRC_TRANSITION_SUPPLY, + PE_SRC_READY, + PE_SRC_HARD_RESET, + PE_SRC_SOFT_RESET, + PE_SRC_SEND_SOFT_RESET, + PE_SRC_DISCOVERY, + PE_SRC_TRANSITION_TO_DEFAULT, + PE_SNK_STARTUP, + PE_SNK_DISCOVERY, + PE_SNK_WAIT_FOR_CAPABILITIES, + PE_SNK_EVALUATE_CAPABILITY, + PE_SNK_SELECT_CAPABILITY, + PE_SNK_TRANSITION_SINK, + PE_SNK_READY, + PE_SNK_HARD_RESET, + PE_SNK_SOFT_RESET, + PE_SNK_SEND_SOFT_RESET, + PE_SNK_TRANSITION_TO_DEFAULT, + PE_DRS_SEND_DR_SWAP, + PE_PRS_SNK_SRC_SEND_SWAP, + PE_PRS_SNK_SRC_TRANSITION_TO_OFF, + PE_PRS_SNK_SRC_SOURCE_ON, + PE_PRS_SRC_SNK_SEND_SWAP, + PE_PRS_SRC_SNK_TRANSITION_TO_OFF, + PE_PRS_SRC_SNK_WAIT_SOURCE_ON, +}; + +static const char * const usbpd_state_strings[] = { + "UNKNOWN", + "ERROR_RECOVERY", + "SRC_Disabled", + "SRC_Startup", + "SRC_Send_Capabilities", + "SRC_Send_Capabilities (Wait for Request)", + "SRC_Negotiate_Capability", + "SRC_Transition_Supply", + "SRC_Ready", + "SRC_Hard_Reset", + "SRC_Soft_Reset", + "SRC_Send_Soft_Reset", + "SRC_Discovery", + "SRC_Transition_to_default", + "SNK_Startup", + "SNK_Discovery", + "SNK_Wait_for_Capabilities", + "SNK_Evaluate_Capability", + "SNK_Select_Capability", + "SNK_Transition_Sink", + "SNK_Ready", + "SNK_Hard_Reset", + "SNK_Soft_Reset", + "SNK_Send_Soft_Reset", + "SNK_Transition_to_default", + "DRS_Send_DR_Swap", + "PRS_SNK_SRC_Send_Swap", + "PRS_SNK_SRC_Transition_to_off", + "PRS_SNK_SRC_Source_on", + "PRS_SRC_SNK_Send_Swap", + "PRS_SRC_SNK_Transition_to_off", + "PRS_SRC_SNK_Wait_Source_on", +}; + +enum usbpd_control_msg_type { + MSG_RESERVED = 0, + MSG_GOODCRC, + MSG_GOTOMIN, + MSG_ACCEPT, + MSG_REJECT, + MSG_PING, + MSG_PS_RDY, + MSG_GET_SOURCE_CAP, + MSG_GET_SINK_CAP, + MSG_DR_SWAP, + MSG_PR_SWAP, + MSG_VCONN_SWAP, + MSG_WAIT, + MSG_SOFT_RESET, +}; + +enum usbpd_data_msg_type { + MSG_SOURCE_CAPABILITIES = 1, + MSG_REQUEST, + MSG_BIST, + MSG_SINK_CAPABILITIES, + MSG_VDM = 0xF, +}; + +/* Timeouts (in ms) */ +#define ERROR_RECOVERY_TIME 25 +#define SENDER_RESPONSE_TIME 30 +#define SINK_WAIT_CAP_TIME 620 +#define PS_TRANSITION_TIME 550 +#define SRC_CAP_TIME 120 +#define SRC_TRANSITION_TIME 25 +#define PS_HARD_RESET_TIME 35 +#define PS_SOURCE_ON 400 +#define PS_SOURCE_OFF 900 + +#define PD_CAPS_COUNT 50 + +#define PD_MAX_MSG_ID 7 + +#define PD_MSG_HDR(type, dr, pr, id, cnt) \ + (((type) & 0xF) | ((dr) << 5) | (1 << 6) | \ + ((pr) << 8) | ((id) << 9) | ((cnt) << 12)) +#define PD_MSG_HDR_COUNT(hdr) (((hdr) >> 12) & 7) +#define PD_MSG_HDR_TYPE(hdr) ((hdr) & 0xF) +#define PD_MSG_HDR_ID(hdr) (((hdr) >> 9) & 7) + +#define PD_RDO_FIXED(obj, gb, mismatch, usb_comm, no_usb_susp, curr1, curr2) \ + (((obj) << 28) | ((gb) << 27) | ((mismatch) << 26) | \ + ((usb_comm) << 25) | ((no_usb_susp) << 24) | \ + ((curr1) << 10) | (curr2)) + +#define PD_RDO_OBJ_POS(rdo) ((rdo) >> 28 & 7) +#define PD_RDO_GIVEBACK(rdo) ((rdo) >> 27 & 1) +#define PD_RDO_MISMATCH(rdo) ((rdo) >> 26 & 1) +#define PD_RDO_USB_COMM(rdo) ((rdo) >> 25 & 1) +#define PD_RDO_NO_USB_SUSP(rdo) ((rdo) >> 24 & 1) +#define PD_RDO_FIXED_CURR(rdo) ((rdo) >> 19 & 0x3FF) +#define PD_RDO_FIXED_CURR_MINMAX(rdo) ((rdo) & 0x3FF) + +#define PD_SRC_PDO_TYPE(pdo) (((pdo) >> 30) & 3) +#define PD_SRC_PDO_TYPE_FIXED 0 +#define PD_SRC_PDO_TYPE_BATTERY 1 +#define PD_SRC_PDO_TYPE_VARIABLE 2 + +#define PD_SRC_PDO_FIXED_PR_SWAP(pdo) (((pdo) >> 29) & 1) +#define PD_SRC_PDO_FIXED_USB_SUSP(pdo) (((pdo) >> 28) & 1) +#define PD_SRC_PDO_FIXED_EXT_POWERED(pdo) (((pdo) >> 27) & 1) +#define PD_SRC_PDO_FIXED_USB_COMM(pdo) (((pdo) >> 26) & 1) +#define PD_SRC_PDO_FIXED_DR_SWAP(pdo) (((pdo) >> 25) & 1) +#define PD_SRC_PDO_FIXED_PEAK_CURR(pdo) (((pdo) >> 20) & 3) +#define PD_SRC_PDO_FIXED_VOLTAGE(pdo) (((pdo) >> 10) & 0x3FF) +#define PD_SRC_PDO_FIXED_MAX_CURR(pdo) ((pdo) & 0x3FF) + +#define PD_SRC_PDO_VAR_BATT_MAX_VOLT(pdo) (((pdo) >> 20) & 0x3FF) +#define PD_SRC_PDO_VAR_BATT_MIN_VOLT(pdo) (((pdo) >> 10) & 0x3FF) +#define PD_SRC_PDO_VAR_BATT_MAX(pdo) ((pdo) & 0x3FF) + +static int min_sink_current = 900; +module_param(min_sink_current, int, S_IRUSR | S_IWUSR); + +static int max_sink_current = 3000; +module_param(max_sink_current, int, S_IRUSR | S_IWUSR); + +static const u32 default_src_caps[] = { 0x36019096 }; /* VSafe5V @ 1.5A */ + +static const u32 default_snk_caps[] = { 0x2601905A, /* 5V @ 900mA */ + 0x0002D096, /* 9V @ 1.5A */ + 0x0003C064 }; /* 12V @ 1A */ + struct usbpd { struct device dev; struct workqueue_struct *wq; - struct work_struct sm_work; + struct delayed_work sm_work; struct extcon_dev *extcon; + + enum usbpd_state current_state; + bool hard_reset; + u8 rx_msg_type; + u8 rx_msg_len; + u32 rx_payload[7]; + + u32 received_pdos[7]; + u8 selected_pdo; + u8 requested_pdo; + u32 rdo; /* can be either source or sink */ + int current_voltage; /* uV */ + int requested_voltage; /* uV */ + int requested_current; /* mA */ + bool pd_connected; + bool in_explicit_contract; + bool peer_usb_comm; + bool peer_pr_swap; + bool peer_dr_swap; + struct power_supply *usb_psy; struct notifier_block psy_nb; enum power_supply_typec_mode typec_mode; enum power_supply_type psy_type; + bool vbus_present; enum data_role current_dr; enum power_role current_pr; + bool in_pr_swap; + bool pd_phy_opened; + + struct regulator *vbus; + struct regulator *vconn; + bool vconn_enabled; + + u8 tx_msgid; + u8 rx_msgid; + int caps_count; + int hard_reset_count; struct list_head instance; }; @@ -48,27 +243,991 @@ static const unsigned int usbpd_extcon_cable[] = { static const u32 usbpd_extcon_exclusive[] = {0xffffffff, 0}; +static int set_power_role(struct usbpd *pd, enum power_role pr) +{ + union power_supply_propval val = {0}; + + switch (pr) { + case PR_NONE: + val.intval = POWER_SUPPLY_TYPEC_PR_NONE; + break; + case PR_SINK: + val.intval = POWER_SUPPLY_TYPEC_PR_SINK; + break; + case PR_SRC: + val.intval = POWER_SUPPLY_TYPEC_PR_SOURCE; + break; + } + + return power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_TYPEC_POWER_ROLE, &val); +} + +static int pd_send_msg(struct usbpd *pd, u8 hdr_type, const u32 *data, + size_t num_data, enum pd_msg_type type) +{ + int ret; + u16 hdr; + + hdr = PD_MSG_HDR(hdr_type, pd->current_dr, pd->current_pr, + pd->tx_msgid, num_data); + ret = pd_phy_write(hdr, (u8 *)data, num_data * sizeof(u32), type, 15); + /* TODO figure out timeout. based on tReceive=1.1ms x nRetryCount? */ + + /* MessageID incremented regardless of Tx error */ + pd->tx_msgid = (pd->tx_msgid + 1) & PD_MAX_MSG_ID; + + if (ret != num_data * sizeof(u32)) + return -EIO; + return 0; +} + +static int pd_select_pdo(struct usbpd *pd, int pdo_pos) +{ + int curr = min_sink_current; + int max_current = max_sink_current; + bool mismatch = false; + u32 pdo = pd->received_pdos[pdo_pos - 1]; + + /* TODO: handle variable/battery types */ + if (PD_SRC_PDO_TYPE(pdo) != PD_SRC_PDO_TYPE_FIXED) { + dev_err(&pd->dev, "Non-fixed PDOs currently unsupported\n"); + return -ENOTSUPP; + } + + /* + * Check if the PDO has enough current, otherwise set the + * Capability Mismatch flag + */ + if ((PD_SRC_PDO_FIXED_MAX_CURR(pdo) * 10) < curr) { + mismatch = true; + max_current = curr; + curr = PD_SRC_PDO_FIXED_MAX_CURR(pdo) * 10; + } + + pd->requested_voltage = PD_SRC_PDO_FIXED_VOLTAGE(pdo) * 50 * 1000; + pd->requested_current = max_current; + pd->requested_pdo = pdo_pos; + pd->rdo = PD_RDO_FIXED(pdo_pos, 0, mismatch, 1, 1, curr / 10, + max_current / 10); + + return 0; +} + +static int pd_eval_src_caps(struct usbpd *pd, const u32 *src_caps) +{ + u32 first_pdo = src_caps[0]; + + /* save the PDOs so userspace can further evaluate */ + memcpy(&pd->received_pdos, src_caps, sizeof(pd->received_pdos)); + + if (PD_SRC_PDO_TYPE(first_pdo) != PD_SRC_PDO_TYPE_FIXED) { + dev_err(&pd->dev, "First src_cap invalid! %08x\n", first_pdo); + return -EINVAL; + } + + pd->peer_usb_comm = PD_SRC_PDO_FIXED_USB_COMM(first_pdo); + pd->peer_pr_swap = PD_SRC_PDO_FIXED_PR_SWAP(first_pdo); + pd->peer_dr_swap = PD_SRC_PDO_FIXED_DR_SWAP(first_pdo); + + /* Select the first PDO (vSafe5V) immediately. */ + pd_select_pdo(pd, 1); + + return 0; +} + +static void pd_send_hard_reset(struct usbpd *pd) +{ + int ret; + + dev_dbg(&pd->dev, "%s\n", __func__); + + /* Force CC logic to source/sink to keep Rp/Rd unchanged */ + set_power_role(pd, pd->current_pr); + pd->hard_reset_count++; + ret = pd_phy_signal(HARD_RESET_SIG, 5); /* tHardResetComplete */ + if (!ret) + pd->hard_reset = true; +} + +static void phy_sig_received(struct usbpd *pd, enum pd_sig_type type) +{ + if (type != HARD_RESET_SIG) { + dev_err(&pd->dev, "invalid signal (%d) received\n", type); + return; + } + + dev_dbg(&pd->dev, "hard reset received\n"); + + /* Force CC logic to source/sink to keep Rp/Rd unchanged */ + set_power_role(pd, pd->current_pr); + pd->hard_reset = true; + mod_delayed_work(pd->wq, &pd->sm_work, 0); +} + +static void phy_msg_received(struct usbpd *pd, enum pd_msg_type type, + u8 *buf, size_t len) +{ + u16 header; + + if (type != SOP_MSG) { + dev_err(&pd->dev, "invalid msg type (%d) received; only SOP supported\n", + type); + return; + } + + if (len < 2) { + dev_err(&pd->dev, "invalid message received, len=%ld\n", len); + return; + } + + header = *((u16 *)buf); + buf += sizeof(u16); + len -= sizeof(u16); + + if (len % 4 != 0) { + dev_err(&pd->dev, "payload not multiple of 4, len=%ld\n", len); + return; + } + + /* if MSGID already seen, discard */ + if (PD_MSG_HDR_ID(header) == pd->rx_msgid && + PD_MSG_HDR_TYPE(header) != MSG_SOFT_RESET) { + dev_dbg(&pd->dev, "MessageID already seen, discarding\n"); + return; + } + + pd->rx_msgid = PD_MSG_HDR_ID(header); + + /* check header's count field to see if it matches len */ + if (PD_MSG_HDR_COUNT(header) != (len / 4)) { + dev_err(&pd->dev, "header count (%d) mismatch, len=%ld\n", + PD_MSG_HDR_COUNT(header), len); + return; + } + + pd->rx_msg_type = PD_MSG_HDR_TYPE(header); + pd->rx_msg_len = PD_MSG_HDR_COUNT(header); + memcpy(&pd->rx_payload, buf, len); + + mod_delayed_work(pd->wq, &pd->sm_work, 0); +} + +static void phy_shutdown(struct usbpd *pd) +{ + dev_dbg(&pd->dev, "%s\n", __func__); +} + +/* Enters new state and executes actions on entry */ +static void usbpd_set_state(struct usbpd *pd, enum usbpd_state next_state) +{ + struct pd_phy_params phy_params = { + .signal_cb = phy_sig_received, + .msg_rx_cb = phy_msg_received, + .shutdown_cb = phy_shutdown, + .frame_filter_val = FRAME_FILTER_EN_SOP | + FRAME_FILTER_EN_HARD_RESET + }; + union power_supply_propval val = {0}; + int ret; + + dev_dbg(&pd->dev, "%s: %s -> %s\n", __func__, + usbpd_state_strings[pd->current_state], + usbpd_state_strings[next_state]); + + pd->current_state = next_state; + + switch (next_state) { + case PE_ERROR_RECOVERY: /* perform hard disconnect/reconnect */ + pd->in_pr_swap = false; + set_power_role(pd, PR_NONE); + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(ERROR_RECOVERY_TIME)); + break; + + /* Source states */ + case PE_SRC_STARTUP: + if (pd->current_dr == DR_NONE) { + pd->current_dr = DR_DFP; + /* Defer starting USB host mode until after PD */ + } + + pd->rx_msg_len = 0; + pd->rx_msg_type = 0; + pd->rx_msgid = -1; + + if (!pd->in_pr_swap) { + if (pd->pd_phy_opened) { + pd_phy_close(); + pd->pd_phy_opened = false; + } + + phy_params.data_role = pd->current_dr; + phy_params.power_role = pd->current_pr; + + ret = pd_phy_open(&phy_params); + if (ret) { + WARN_ON_ONCE(1); + dev_err(&pd->dev, "error opening PD PHY %d\n", + ret); + pd->current_state = PE_UNKNOWN; + return; + } + + pd->pd_phy_opened = true; + } + + pd->in_pr_swap = false; + pd->current_state = PE_SRC_SEND_CAPABILITIES; + dev_dbg(&pd->dev, "Enter %s\n", + usbpd_state_strings[pd->current_state]); + /* fall-through */ + + case PE_SRC_SEND_CAPABILITIES: + queue_delayed_work(pd->wq, &pd->sm_work, 0); + break; + + /* reset counters */ + pd->hard_reset_count = 0; + pd->caps_count = 0; + pd->pd_connected = true; /* we know peer is PD capable */ + + /* wait for REQUEST */ + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(SENDER_RESPONSE_TIME * 3)); + break; + + case PE_SRC_NEGOTIATE_CAPABILITY: + if (PD_RDO_OBJ_POS(pd->rdo) != 1) { + /* send Reject */ + ret = pd_send_msg(pd, MSG_REJECT, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Reject\n"); + usbpd_set_state(pd, PE_SRC_SEND_SOFT_RESET); + break; + } + + dev_err(&pd->dev, "Invalid request: %08x\n", pd->rdo); + + if (pd->in_explicit_contract) + usbpd_set_state(pd, PE_SRC_READY); + else + /* + * bypass PE_SRC_Capability_Response and + * PE_SRC_Wait_New_Capabilities in this + * implementation for simplicity. + */ + usbpd_set_state(pd, PE_SRC_SEND_CAPABILITIES); + break; + } + + /* + * we only support VSafe5V so Aceept right away as there is + * nothing more to prepare from the power supply + */ + pd->current_state = PE_SRC_TRANSITION_SUPPLY; + dev_dbg(&pd->dev, "Enter %s\n", + usbpd_state_strings[pd->current_state]); + /* fall-through */ + + case PE_SRC_TRANSITION_SUPPLY: + ret = pd_send_msg(pd, MSG_ACCEPT, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Accept\n"); + usbpd_set_state(pd, PE_SRC_SEND_SOFT_RESET); + break; + } + + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(SRC_TRANSITION_TIME)); + break; + + case PE_SRC_READY: + if (pd->current_dr == DR_DFP) + extcon_set_cable_state_(pd->extcon, EXTCON_USB_HOST, 1); + pd->in_explicit_contract = true; + break; + + case PE_SRC_TRANSITION_TO_DEFAULT: + if (pd->vconn_enabled) + regulator_disable(pd->vconn); + regulator_disable(pd->vbus); + + if (pd->current_dr != DR_DFP) { + extcon_set_cable_state_(pd->extcon, EXTCON_USB, 0); + pd->current_dr = DR_DFP; + pd_phy_update_roles(pd->current_dr, pd->current_pr); + } + + msleep(1000); /* tSrcRecover */ + + ret = regulator_enable(pd->vbus); + if (ret) + dev_err(&pd->dev, "Unable to enable vbus\n"); + + if (pd->vconn_enabled) { + ret = regulator_enable(pd->vconn); + if (ret) { + dev_err(&pd->dev, "Unable to enable vconn\n"); + pd->vconn_enabled = false; + } + } + + usbpd_set_state(pd, PE_SRC_STARTUP); + break; + + case PE_SRC_HARD_RESET: + case PE_SNK_HARD_RESET: + /* hard reset may sleep; handle it in the workqueue */ + queue_delayed_work(pd->wq, &pd->sm_work, 0); + break; + + case PE_SRC_SEND_SOFT_RESET: + case PE_SNK_SEND_SOFT_RESET: + /* Reset protocol layer */ + pd->tx_msgid = 0; + pd->rx_msgid = -1; + + ret = pd_send_msg(pd, MSG_SOFT_RESET, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Soft Reset, do Hard Reset\n"); + usbpd_set_state(pd, pd->current_pr == PR_SRC ? + PE_SRC_HARD_RESET : PE_SNK_HARD_RESET); + break; + } + + /* wait for ACCEPT */ + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(SENDER_RESPONSE_TIME * 3)); + break; + + /* Sink states */ + case PE_SNK_STARTUP: + if (pd->current_dr == DR_NONE || pd->current_dr == DR_UFP) { + pd->current_dr = DR_UFP; + + if (pd->psy_type == POWER_SUPPLY_TYPE_USB || + pd->psy_type == POWER_SUPPLY_TYPE_USB_CDP) + extcon_set_cable_state_(pd->extcon, + EXTCON_USB, 1); + } + + pd->rx_msg_len = 0; + pd->rx_msg_type = 0; + pd->rx_msgid = -1; + + if (!pd->in_pr_swap) { + if (pd->pd_phy_opened) { + pd_phy_close(); + pd->pd_phy_opened = false; + } + + phy_params.data_role = pd->current_dr; + phy_params.power_role = pd->current_pr; + + ret = pd_phy_open(&phy_params); + if (ret) { + WARN_ON_ONCE(1); + dev_err(&pd->dev, "error opening PD PHY %d\n", + ret); + pd->current_state = PE_UNKNOWN; + return; + } + + pd->pd_phy_opened = true; + } + + pd->in_pr_swap = false; + pd->current_voltage = 5000000; + + if (!pd->vbus_present) { + /* can get here during a hard reset and we lost vbus */ + pd->current_state = PE_SNK_DISCOVERY; + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(2000)); + break; + } + + /* + * If VBUS is already present go and skip ahead to + * PE_SNK_WAIT_FOR_CAPABILITIES. + */ + pd->current_state = PE_SNK_WAIT_FOR_CAPABILITIES; + /* fall-through */ + + case PE_SNK_WAIT_FOR_CAPABILITIES: + if (pd->rx_msg_len && pd->rx_msg_type) + queue_delayed_work(pd->wq, &pd->sm_work, 0); + else + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(SINK_WAIT_CAP_TIME * 3)); + break; + + case PE_SNK_EVALUATE_CAPABILITY: + pd->pd_connected = true; /* we know peer is PD capable */ + pd->hard_reset_count = 0; + + /* evaluate PDOs and select one */ + ret = pd_eval_src_caps(pd, pd->rx_payload); + if (ret < 0) { + dev_err(&pd->dev, "Invalid src_caps received. Skipping request\n"); + break; + } + pd->current_state = PE_SNK_SELECT_CAPABILITY; + /* fall-through */ + + case PE_SNK_SELECT_CAPABILITY: + ret = pd_send_msg(pd, MSG_REQUEST, &pd->rdo, 1, SOP_MSG); + if (ret) + dev_err(&pd->dev, "Error sending Request\n"); + + /* wait for ACCEPT */ + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(SENDER_RESPONSE_TIME * 3)); + break; + + case PE_SNK_TRANSITION_SINK: + /* wait for PS_RDY */ + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(PS_TRANSITION_TIME)); + break; + + case PE_SNK_READY: + pd->in_explicit_contract = true; + break; + + case PE_SNK_TRANSITION_TO_DEFAULT: + if (pd->current_dr != DR_UFP) { + extcon_set_cable_state_(pd->extcon, EXTCON_USB_HOST, 0); + + pd->current_dr = DR_UFP; + extcon_set_cable_state_(pd->extcon, EXTCON_USB, 1); + pd_phy_update_roles(pd->current_dr, pd->current_pr); + } + if (pd->vconn_enabled) { + regulator_disable(pd->vconn); + pd->vconn_enabled = false; + } + + pd->tx_msgid = 0; + + val.intval = pd->requested_voltage; /* set range back to 5V */ + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_VOLTAGE_MAX, &val); + pd->current_voltage = pd->requested_voltage; + + val.intval = pd->requested_current * 1000; /* mA->uA */ + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_CURRENT_MAX, &val); + + /* recursive call; go back to beginning state */ + usbpd_set_state(pd, PE_SNK_STARTUP); + break; + + default: + dev_dbg(&pd->dev, "%s: No action for state %s\n", __func__, + usbpd_state_strings[pd->current_state]); + break; + } +} + +static void dr_swap(struct usbpd *pd) +{ + if (pd->current_dr == DR_DFP) { + extcon_set_cable_state_(pd->extcon, EXTCON_USB_HOST, 0); + extcon_set_cable_state_(pd->extcon, EXTCON_USB, 1); + pd->current_dr = DR_UFP; + } else if (pd->current_dr == DR_UFP) { + extcon_set_cable_state_(pd->extcon, EXTCON_USB, 0); + extcon_set_cable_state_(pd->extcon, EXTCON_USB_HOST, 1); + pd->current_dr = DR_DFP; + } + + pd_phy_update_roles(pd->current_dr, pd->current_pr); +} + +/* Handles current state and determines transitions */ static void usbpd_sm(struct work_struct *w) { - struct usbpd *pd = container_of(w, struct usbpd, sm_work); + struct usbpd *pd = container_of(w, struct usbpd, sm_work.work); + union power_supply_propval val = {0}; + int ret; + enum usbpd_control_msg_type ctrl_recvd = 0; + enum usbpd_data_msg_type data_recvd = 0; + + dev_dbg(&pd->dev, "%s: state %s\n", __func__, + usbpd_state_strings[pd->current_state]); + + if (pd->rx_msg_len) + data_recvd = pd->rx_msg_type; + else + ctrl_recvd = pd->rx_msg_type; + + /* Disconnect? */ + if (pd->typec_mode == POWER_SUPPLY_TYPEC_NONE) { + if (pd->current_state == PE_UNKNOWN) + return; + + dev_info(&pd->dev, "USB PD disconnect\n"); + + if (pd->pd_phy_opened) { + pd_phy_close(); + pd->pd_phy_opened = false; + } + + pd->in_pr_swap = false; + pd->pd_connected = false; + pd->in_explicit_contract = false; + pd->hard_reset = false; + pd->caps_count = 0; + pd->hard_reset_count = 0; + pd->src_cap_id = 0; + memset(&pd->received_pdos, 0, sizeof(pd->received_pdos)); + + val.intval = 0; + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_PD_ACTIVE, &val); + + if (pd->current_pr == PR_SRC) { + regulator_disable(pd->vconn); + regulator_disable(pd->vbus); + } - if (pd->current_pr == PR_NONE) { if (pd->current_dr == DR_UFP) extcon_set_cable_state_(pd->extcon, EXTCON_USB, 0); else if (pd->current_dr == DR_DFP) extcon_set_cable_state_(pd->extcon, EXTCON_USB_HOST, 0); + pd->current_pr = PR_NONE; pd->current_dr = DR_NONE; - } else if (pd->current_pr == PR_SINK && pd->current_dr == DR_NONE) { - pd->current_dr = DR_UFP; - if (pd->psy_type == POWER_SUPPLY_TYPE_USB || - pd->psy_type == POWER_SUPPLY_TYPE_USB_CDP) - extcon_set_cable_state_(pd->extcon, EXTCON_USB, 1); - } else if (pd->current_pr == PR_SRC && pd->current_dr == DR_NONE) { - pd->current_dr = DR_DFP; - extcon_set_cable_state_(pd->extcon, EXTCON_USB_HOST, 1); + /* Set CC back to DRP toggle */ + val.intval = POWER_SUPPLY_TYPEC_PR_DUAL; + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_TYPEC_POWER_ROLE, &val); + + pd->current_state = PE_UNKNOWN; + return; } + + /* Hard reset? */ + if (pd->hard_reset) { + if (pd->current_pr == PR_SINK) + usbpd_set_state(pd, PE_SNK_TRANSITION_TO_DEFAULT); + else + usbpd_set_state(pd, PE_SRC_TRANSITION_TO_DEFAULT); + pd->hard_reset = false; + } + + /* Soft reset? */ + if (ctrl_recvd == MSG_SOFT_RESET) { + dev_dbg(&pd->dev, "Handle soft reset\n"); + + if (pd->current_pr == PR_SRC) + pd->current_state = PE_SRC_SOFT_RESET; + else if (pd->current_pr == PR_SINK) + pd->current_state = PE_SNK_SOFT_RESET; + } + + switch (pd->current_state) { + case PE_UNKNOWN: + if (pd->current_pr == PR_SINK) { + usbpd_set_state(pd, PE_SNK_STARTUP); + } else if (pd->current_pr == PR_SRC) { + ret = regulator_enable(pd->vbus); + if (ret) + dev_err(&pd->dev, "Unable to enable vbus\n"); + + if (!pd->vconn_enabled && + pd->typec_mode == + POWER_SUPPLY_TYPEC_SINK_POWERED_CABLE) { + ret = regulator_enable(pd->vconn); + if (ret) + dev_err(&pd->dev, "Unable to enable vconn\n"); + else + pd->vconn_enabled = true; + } + + usbpd_set_state(pd, PE_SRC_STARTUP); + } + break; + + case PE_SRC_SEND_CAPABILITIES: + ret = pd_send_msg(pd, MSG_SOURCE_CAPABILITIES, default_src_caps, + ARRAY_SIZE(default_src_caps), SOP_MSG); + if (ret) { + pd->caps_count++; + + if (pd->caps_count == 5 && pd->current_dr == DR_DFP) { + /* Likely not PD-capable, start host now */ + extcon_set_cable_state_(pd->extcon, + EXTCON_USB_HOST, 1); + } else if (pd->caps_count >= PD_CAPS_COUNT) { + dev_dbg(&pd->dev, "Src CapsCounter exceeded, disabling PD\n"); + usbpd_set_state(pd, PE_SRC_DISABLED); + break; + } + + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(SRC_CAP_TIME)); + break; + } + + /* transmit was successful if GoodCRC was received */ + pd->caps_count = 0; + pd->hard_reset_count = 0; + + /* wait for REQUEST */ + pd->current_state = PE_SRC_SEND_CAPABILITIES_WAIT; + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(SENDER_RESPONSE_TIME * 3)); + break; + + case PE_SRC_SEND_CAPABILITIES_WAIT: + if (data_recvd == MSG_REQUEST) { + pd->rdo = pd->rx_payload[0]; + usbpd_set_state(pd, PE_SRC_NEGOTIATE_CAPABILITY); + } else { + usbpd_set_state(pd, PE_SRC_HARD_RESET); + } + break; + + case PE_SRC_TRANSITION_SUPPLY: + ret = pd_send_msg(pd, MSG_PS_RDY, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending PS_RDY\n"); + usbpd_set_state(pd, PE_SRC_SEND_SOFT_RESET); + break; + } + + usbpd_set_state(pd, PE_SRC_READY); + break; + + case PE_SRC_READY: + if (ctrl_recvd == MSG_GET_SOURCE_CAP) { + ret = pd_send_msg(pd, MSG_SOURCE_CAPABILITIES, + default_src_caps, + ARRAY_SIZE(default_src_caps), SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending SRC CAPs\n"); + usbpd_set_state(pd, PE_SRC_SEND_SOFT_RESET); + break; + } + } else if (data_recvd == MSG_REQUEST) { + pd->rdo = pd->rx_payload[0]; + usbpd_set_state(pd, PE_SRC_NEGOTIATE_CAPABILITY); + } else if (ctrl_recvd == MSG_DR_SWAP) { + ret = pd_send_msg(pd, MSG_ACCEPT, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Accept\n"); + usbpd_set_state(pd, PE_SRC_SEND_SOFT_RESET); + break; + } + + dr_swap(pd); + } else if (ctrl_recvd == MSG_PR_SWAP) { + /* we'll happily accept Src->Sink requests anytime */ + ret = pd_send_msg(pd, MSG_ACCEPT, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Accept\n"); + usbpd_set_state(pd, PE_SRC_SEND_SOFT_RESET); + break; + } + + pd->current_state = PE_PRS_SRC_SNK_TRANSITION_TO_OFF; + queue_delayed_work(pd->wq, &pd->sm_work, 0); + break; + } + break; + + case PE_SRC_HARD_RESET: + pd_send_hard_reset(pd); + pd->in_explicit_contract = false; + + msleep(PS_HARD_RESET_TIME); + usbpd_set_state(pd, PE_SRC_TRANSITION_TO_DEFAULT); + break; + + case PE_SNK_DISCOVERY: + if (!pd->vbus_present) { + /* Hard reset and VBUS didn't come back? */ + power_supply_get_property(pd->usb_psy, + POWER_SUPPLY_PROP_TYPE, &val); + if (val.intval == POWER_SUPPLY_TYPEC_NONE) { + pd->typec_mode = POWER_SUPPLY_TYPEC_NONE; + queue_delayed_work(pd->wq, &pd->sm_work, 0); + } + break; + } + + usbpd_set_state(pd, PE_SNK_WAIT_FOR_CAPABILITIES); + break; + + case PE_SNK_WAIT_FOR_CAPABILITIES: + if (data_recvd == MSG_SOURCE_CAPABILITIES) { + val.intval = 1; + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_PD_ACTIVE, &val); + usbpd_set_state(pd, PE_SNK_EVALUATE_CAPABILITY); + } else if (pd->hard_reset_count < 3) { + usbpd_set_state(pd, PE_SNK_HARD_RESET); + } else if (pd->pd_connected) { + dev_info(&pd->dev, "Sink hard reset count exceeded, forcing reconnect\n"); + usbpd_set_state(pd, PE_ERROR_RECOVERY); + } else { + dev_dbg(&pd->dev, "Sink hard reset count exceeded, disabling PD\n"); + val.intval = 0; + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_PD_ACTIVE, &val); + } + break; + + case PE_SNK_SELECT_CAPABILITY: + if (ctrl_recvd == MSG_ACCEPT) { + /* prepare for voltage increase/decrease */ + val.intval = pd->requested_voltage; + power_supply_set_property(pd->usb_psy, + pd->requested_voltage >= pd->current_voltage ? + POWER_SUPPLY_PROP_VOLTAGE_MAX : + POWER_SUPPLY_PROP_VOLTAGE_MIN, + &val); + + val.intval = 0; /* suspend charging */ + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_CURRENT_MAX, &val); + + pd->selected_pdo = pd->requested_pdo; + usbpd_set_state(pd, PE_SNK_TRANSITION_SINK); + } else if (ctrl_recvd == MSG_REJECT || ctrl_recvd == MSG_WAIT) { + if (pd->in_explicit_contract) + usbpd_set_state(pd, PE_SNK_READY); + else + usbpd_set_state(pd, + PE_SNK_WAIT_FOR_CAPABILITIES); + } else { + /* timed out; go to hard reset */ + usbpd_set_state(pd, PE_SNK_HARD_RESET); + } + break; + + case PE_SNK_TRANSITION_SINK: + if (ctrl_recvd == MSG_PS_RDY) { + val.intval = pd->requested_voltage; + power_supply_set_property(pd->usb_psy, + pd->requested_voltage >= pd->current_voltage ? + POWER_SUPPLY_PROP_VOLTAGE_MIN : + POWER_SUPPLY_PROP_VOLTAGE_MAX, &val); + pd->current_voltage = pd->requested_voltage; + + /* resume charging */ + val.intval = pd->requested_current * 1000; /* mA->uA */ + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_CURRENT_MAX, &val); + + usbpd_set_state(pd, PE_SNK_READY); + } else { + /* timed out; go to hard reset */ + usbpd_set_state(pd, PE_SNK_HARD_RESET); + } + break; + + case PE_SNK_READY: + if (data_recvd == MSG_SOURCE_CAPABILITIES) + usbpd_set_state(pd, PE_SNK_EVALUATE_CAPABILITY); + else if (ctrl_recvd == MSG_GET_SINK_CAP) { + ret = pd_send_msg(pd, MSG_SINK_CAPABILITIES, + default_snk_caps, + ARRAY_SIZE(default_snk_caps), SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Sink Caps\n"); + usbpd_set_state(pd, PE_SNK_SEND_SOFT_RESET); + } + } else if (ctrl_recvd == MSG_DR_SWAP) { + ret = pd_send_msg(pd, MSG_ACCEPT, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Accept\n"); + usbpd_set_state(pd, PE_SRC_SEND_SOFT_RESET); + break; + } + + dr_swap(pd); + } else if (ctrl_recvd == MSG_PR_SWAP) { + /* TODO: should we Reject in certain circumstances? */ + ret = pd_send_msg(pd, MSG_ACCEPT, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending Accept\n"); + usbpd_set_state(pd, PE_SNK_SEND_SOFT_RESET); + break; + } + + pd->in_pr_swap = true; + pd->current_state = PE_PRS_SNK_SRC_TRANSITION_TO_OFF; + /* turn off sink */ + pd->in_explicit_contract = false; + + /* + * need to update PR bit in message header so that + * proper GoodCRC is sent when receiving next PS_RDY + */ + pd->current_pr = PR_SRC; + pd_phy_update_roles(pd->current_dr, pd->current_pr); + + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(PS_SOURCE_OFF)); + break; + } + break; + + case PE_SRC_SOFT_RESET: + case PE_SNK_SOFT_RESET: + /* Reset protocol layer */ + pd->tx_msgid = 0; + pd->rx_msgid = -1; + + ret = pd_send_msg(pd, MSG_ACCEPT, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "%s: Error sending Accept, do Hard Reset\n", + usbpd_state_strings[pd->current_state]); + usbpd_set_state(pd, pd->current_pr == PR_SRC ? + PE_SRC_HARD_RESET : PE_SNK_HARD_RESET); + break; + } + + usbpd_set_state(pd, pd->current_pr == PR_SRC ? + PE_SRC_SEND_CAPABILITIES : + PE_SNK_WAIT_FOR_CAPABILITIES); + break; + + case PE_SRC_SEND_SOFT_RESET: + case PE_SNK_SEND_SOFT_RESET: + if (ctrl_recvd == MSG_ACCEPT) { + usbpd_set_state(pd, pd->current_pr == PR_SRC ? + PE_SRC_SEND_CAPABILITIES : + PE_SNK_WAIT_FOR_CAPABILITIES); + } else { + dev_err(&pd->dev, "%s: Did not see Accept, do Hard Reset\n", + usbpd_state_strings[pd->current_state]); + usbpd_set_state(pd, pd->current_pr == PR_SRC ? + PE_SRC_HARD_RESET : PE_SNK_HARD_RESET); + } + break; + + case PE_SNK_HARD_RESET: + /* prepare charger for VBUS change */ + val.intval = 1; + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_PD_ACTIVE, &val); + + pd->requested_voltage = 5000000; + pd->requested_current = max_sink_current; + + val.intval = pd->requested_voltage; + power_supply_set_property(pd->usb_psy, + POWER_SUPPLY_PROP_VOLTAGE_MIN, &val); + + pd_send_hard_reset(pd); + pd->in_explicit_contract = false; + usbpd_set_state(pd, PE_SNK_TRANSITION_TO_DEFAULT); + break; + + case PE_DRS_SEND_DR_SWAP: + if (ctrl_recvd == MSG_ACCEPT) + dr_swap(pd); + + usbpd_set_state(pd, pd->current_pr == PR_SRC ? + PE_SRC_READY : PE_SNK_READY); + break; + + case PE_PRS_SRC_SNK_SEND_SWAP: + if (ctrl_recvd != MSG_ACCEPT) { + pd->current_state = PE_SRC_READY; + break; + } + + pd->current_state = PE_PRS_SRC_SNK_TRANSITION_TO_OFF; + /* fall-through */ + case PE_PRS_SRC_SNK_TRANSITION_TO_OFF: + pd->in_pr_swap = true; + pd->in_explicit_contract = false; + + regulator_disable(pd->vbus); + set_power_role(pd, PR_SINK); /* switch Rp->Rd */ + pd->current_pr = PR_SINK; + pd_phy_update_roles(pd->current_dr, pd->current_pr); + + ret = pd_send_msg(pd, MSG_PS_RDY, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending PS_RDY\n"); + usbpd_set_state(pd, PE_ERROR_RECOVERY); + break; + } + + pd->current_state = PE_PRS_SRC_SNK_WAIT_SOURCE_ON; + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(PS_SOURCE_ON)); + break; + + case PE_PRS_SRC_SNK_WAIT_SOURCE_ON: + if (ctrl_recvd == MSG_PS_RDY) + usbpd_set_state(pd, PE_SNK_STARTUP); + else + usbpd_set_state(pd, PE_ERROR_RECOVERY); + break; + + case PE_PRS_SNK_SRC_SEND_SWAP: + if (ctrl_recvd != MSG_ACCEPT) { + pd->current_state = PE_SNK_READY; + break; + } + + pd->in_pr_swap = true; + pd->current_state = PE_PRS_SNK_SRC_TRANSITION_TO_OFF; + /* turn off sink */ + pd->in_explicit_contract = false; + + /* + * need to update PR bit in message header so that + * proper GoodCRC is sent when receiving next PS_RDY + */ + pd->current_pr = PR_SRC; + pd_phy_update_roles(pd->current_dr, pd->current_pr); + + queue_delayed_work(pd->wq, &pd->sm_work, + msecs_to_jiffies(PS_SOURCE_OFF)); + break; + + case PE_PRS_SNK_SRC_TRANSITION_TO_OFF: + if (ctrl_recvd != MSG_PS_RDY) { + usbpd_set_state(pd, PE_ERROR_RECOVERY); + break; + } + + pd->current_state = PE_PRS_SNK_SRC_SOURCE_ON; + /* fall-through */ + case PE_PRS_SNK_SRC_SOURCE_ON: + set_power_role(pd, PR_SRC); + ret = regulator_enable(pd->vbus); + if (ret) + dev_err(&pd->dev, "Unable to enable vbus\n"); + + ret = pd_send_msg(pd, MSG_PS_RDY, NULL, 0, SOP_MSG); + if (ret) { + dev_err(&pd->dev, "Error sending PS_RDY\n"); + usbpd_set_state(pd, PE_ERROR_RECOVERY); + break; + } + + usbpd_set_state(pd, PE_SRC_STARTUP); + break; + + default: + dev_err(&pd->dev, "%s: Unhandled state %s\n", __func__, + usbpd_state_strings[pd->current_state]); + break; + } + + /* Rx message should have been consumed now */ + pd->rx_msg_type = pd->rx_msg_len = 0; } static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr) @@ -93,6 +1252,15 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr) pd_allowed = val.intval; + ret = power_supply_get_property(pd->usb_psy, + POWER_SUPPLY_PROP_PRESENT, &val); + if (ret) { + dev_err(&pd->dev, "Unable to read USB PRESENT: %d\n", ret); + return ret; + } + + pd->vbus_present = val.intval; + ret = power_supply_get_property(pd->usb_psy, POWER_SUPPLY_PROP_TYPEC_MODE, &val); if (ret) { @@ -100,11 +1268,22 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr) return ret; } - /* don't proceed if PD_ALLOWED is false */ - if (!pd_allowed && val.intval != POWER_SUPPLY_TYPEC_NONE) + typec_mode = val.intval; + + /* + * Don't proceed if cable is connected but PD_ALLOWED is false. + * It means the PMIC may still be in the middle of performing + * charger type detection. + */ + if (!pd_allowed && typec_mode != POWER_SUPPLY_TYPEC_NONE) return 0; - typec_mode = val.intval; + if ((pd->hard_reset || pd->in_pr_swap) && + typec_mode == POWER_SUPPLY_TYPEC_NONE) { + dev_dbg(&pd->dev, "Ignoring disconnect due to %s\n", + pd->hard_reset ? "hard reset" : "PR swap"); + return 0; + } ret = power_supply_get_property(pd->usb_psy, POWER_SUPPLY_PROP_TYPE, &val); @@ -128,8 +1307,7 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr) switch (typec_mode) { /* Disconnect */ case POWER_SUPPLY_TYPEC_NONE: - pd->current_pr = PR_NONE; - queue_work(pd->wq, &pd->sm_work); + queue_delayed_work(pd->wq, &pd->sm_work, 0); break; /* Sink states */ @@ -137,16 +1315,20 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr) case POWER_SUPPLY_TYPEC_SOURCE_MEDIUM: case POWER_SUPPLY_TYPEC_SOURCE_HIGH: dev_info(&pd->dev, "Type-C Source connected\n"); - pd->current_pr = PR_SINK; - queue_work(pd->wq, &pd->sm_work); + if (pd->current_pr != PR_SINK) { + pd->current_pr = PR_SINK; + queue_delayed_work(pd->wq, &pd->sm_work, 0); + } break; /* Source states */ case POWER_SUPPLY_TYPEC_SINK_POWERED_CABLE: case POWER_SUPPLY_TYPEC_SINK: dev_info(&pd->dev, "Type-C Sink connected\n"); - pd->current_pr = PR_SRC; - queue_work(pd->wq, &pd->sm_work); + if (pd->current_pr != PR_SRC) { + pd->current_pr = PR_SRC; + queue_delayed_work(pd->wq, &pd->sm_work, 0); + } break; case POWER_SUPPLY_TYPEC_SINK_DEBUG_ACCESSORY: @@ -207,7 +1389,7 @@ struct usbpd *usbpd_create(struct device *parent) ret = -ENOMEM; goto del_pd; } - INIT_WORK(&pd->sm_work, usbpd_sm); + INIT_DELAYED_WORK(&pd->sm_work, usbpd_sm); pd->usb_psy = power_supply_get_by_name("usb"); if (!pd->usb_psy) { @@ -239,6 +1421,18 @@ struct usbpd *usbpd_create(struct device *parent) goto unreg_psy; } + pd->vbus = devm_regulator_get(parent, "vbus"); + if (IS_ERR(pd->vbus)) { + ret = PTR_ERR(pd->vbus); + goto unreg_psy; + } + + pd->vconn = devm_regulator_get(parent, "vconn"); + if (IS_ERR(pd->vconn)) { + ret = PTR_ERR(pd->vconn); + goto unreg_psy; + } + pd->current_pr = PR_NONE; pd->current_dr = DR_NONE; list_add_tail(&pd->instance, &_usbpd);