123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684 |
- /*
- * PD Buddy - USB Power Delivery for everyone
- * Copyright (C) 2017 Clayton G. Hobbs <clay@lakeserv.net>
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
- #include "policy_engine.h"
-
- #include <stdbool.h>
-
- #include "messages.h"
- #include "priorities.h"
- #include "device_policy_manager.h"
- #include "protocol_tx.h"
- #include "hard_reset.h"
- #include "pd.h"
-
-
- thread_t *pdb_pe_thread;
-
- enum policy_engine_state {
- PESinkStartup,
- PESinkDiscovery,
- PESinkWaitCap,
- PESinkEvalCap,
- PESinkSelectCap,
- PESinkTransitionSink,
- PESinkReady,
- PESinkGetSourceCap,
- PESinkGiveSinkCap,
- PESinkHardReset,
- PESinkTransitionDefault,
- PESinkSoftReset,
- PESinkSendSoftReset,
- PESinkSendReject,
- PESinkSourceUnresponsive
- };
-
- /* The received message we're currently working with */
- static union pd_msg *policy_engine_message = NULL;
- /* The most recent Request from the DPM */
- static union pd_msg *last_dpm_request = NULL;
- /* Whether or not the source capabilities match our required power */
- static bool capability_match = false;
- /* Whether or not we have an explicit contract */
- static bool explicit_contract = false;
- /* Whether or not we're receiving minimum power*/
- static bool min_power = false;
- /* Keep track of how many hard resets we've sent */
- static int hard_reset_counter = 0;
- /* Policy Engine thread mailbox */
- static msg_t pdb_pe_mailbox_queue[PDB_MSG_POOL_SIZE];
- mailbox_t pdb_pe_mailbox;
-
- static enum policy_engine_state pe_sink_startup(void)
- {
- /* We don't have an explicit contract currently */
- explicit_contract = false;
- /* Tell the DPM that we've started negotiations */
- pdb_dpm_pd_start();
-
- /* No need to reset the protocol layer here. There are two ways into this
- * state: startup and exiting hard reset. On startup, the protocol layer
- * is reset by the startup procedure. When exiting hard reset, the
- * protocol layer is reset by the hard reset state machine. Since it's
- * already done somewhere else, there's no need to do it again here. */
-
- return PESinkDiscovery;
- }
-
- static enum policy_engine_state pe_sink_discovery(void)
- {
- /* Wait for VBUS. Since it's our only power source, we already know that
- * we have it, so just move on. */
-
- return PESinkWaitCap;
- }
-
- static enum policy_engine_state pe_sink_wait_cap(void)
- {
- /* Fetch a message from the protocol layer */
- eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX
- | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_RESET, PD_T_TYPEC_SINK_WAIT_CAP);
- /* If we timed out waiting for Source_Capabilities, send a hard reset */
- if (evt == 0) {
- return PESinkHardReset;
- }
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If we're too hot, we shouldn't negotiate power yet */
- if (evt & PDB_EVT_PE_I_OVRTEMP) {
- return PESinkWaitCap;
- }
-
- /* If we got a message */
- if (evt & PDB_EVT_PE_MSG_RX) {
- /* Get the message */
- if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
- /* If we got a Source_Capabilities message, read it. */
- if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOURCE_CAPABILITIES
- && PD_NUMOBJ_GET(policy_engine_message) > 0) {
- return PESinkEvalCap;
- /* If the message was a Soft_Reset, do the soft reset procedure */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSoftReset;
- /* If we got an unexpected message, reset */
- } else {
- /* Free the received message */
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- return PESinkHardReset;
- }
- }
- }
-
- /* If we failed to get a message, send a hard reset */
- return PESinkHardReset;
- }
-
- static enum policy_engine_state pe_sink_eval_cap(void)
- {
- /* Get a message object for the request if we don't have one already */
- if (last_dpm_request == NULL) {
- last_dpm_request = chPoolAlloc(&pdb_msg_pool);
- }
- /* Ask the DPM what to request */
- capability_match = pdb_dpm_evaluate_capability(policy_engine_message,
- last_dpm_request);
- /* Free the Source_Capabilities message */
- chPoolFree(&pdb_msg_pool, policy_engine_message);
-
- return PESinkSelectCap;
- }
-
- static enum policy_engine_state pe_sink_select_cap(void)
- {
- /* Transmit the request */
- chMBPost(&pdb_prltx_mailbox, (msg_t) last_dpm_request, TIME_IMMEDIATE);
- chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
- eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
- | PDB_EVT_PE_RESET);
- /* Don't free the request; we might need it again */
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If the message transmission failed, send a hard reset */
- if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
- return PESinkHardReset;
- }
-
- /* Wait for a response */
- evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
- PD_T_SENDER_RESPONSE);
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If we didn't get a response before the timeout, send a hard reset */
- if (evt == 0) {
- return PESinkHardReset;
- }
-
- /* Get the response message */
- if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
- /* If the source accepted our request, wait for the new power */
- if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_ACCEPT
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- /* Transition to Sink Standby if necessary */
- pdb_dpm_sink_standby();
-
- min_power = false;
-
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkTransitionSink;
- /* If the message was a Soft_Reset, do the soft reset procedure */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSoftReset;
- /* If the message was Wait or Reject */
- } else if ((PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_REJECT
- || PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_WAIT)
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- /* If we don't have an explicit contract, wait for capabilities */
- if (!explicit_contract) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkWaitCap;
- /* If we do have an explicit contract, go to the ready state */
- } else {
- /* If we got here from a Wait message, we Should run
- * SinkRequestTimer in the Ready state. */
- min_power = (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_WAIT);
-
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkReady;
- }
- } else {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendSoftReset;
- }
- }
- return PESinkHardReset;
- }
-
- static enum policy_engine_state pe_sink_transition_sink(void)
- {
- /* Wait for the PS_RDY message */
- eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
- PD_T_PS_TRANSITION);
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If no message was received, send a hard reset */
- if (evt == 0) {
- return PESinkHardReset;
- }
-
- /* If we received a message, read it */
- if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
- /* If we got a PS_RDY, handle it */
- if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PS_RDY
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- /* We just finished negotiating an explicit contract */
- explicit_contract = true;
-
- /* Set the output appropriately */
- pdb_dpm_output_set(capability_match && !min_power);
-
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkReady;
- /* If there was a protocol error, send a hard reset */
- } else {
- /* Turn off the power output before this hard reset to make sure we
- * don't supply an incorrect voltage to the device we're powering.
- */
- pdb_dpm_output_set(false);
-
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkHardReset;
- }
- }
-
- return PESinkHardReset;
- }
-
- static enum policy_engine_state pe_sink_ready(void)
- {
- eventmask_t evt;
-
- /* Wait for an event */
- if (min_power) {
- evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET
- | PDB_EVT_PE_I_OVRTEMP, PD_T_SINK_REQUEST);
- } else {
- evt = chEvtWaitAny(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET
- | PDB_EVT_PE_I_OVRTEMP);
- }
-
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
-
- /* If we overheated, send a hard reset */
- if (evt & PDB_EVT_PE_I_OVRTEMP) {
- return PESinkHardReset;
- }
-
- /* If no event was received, the timer ran out. */
- if (evt == 0) {
- /* Repeat our Request message */
- return PESinkSelectCap;
- }
-
- /* If we received a message */
- if (evt & PDB_EVT_PE_MSG_RX) {
- if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
- /* Ignore vendor-defined messages */
- if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_VENDOR_DEFINED
- && PD_NUMOBJ_GET(policy_engine_message) > 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkReady;
- /* Ignore Ping messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PING
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkReady;
- /* Reject DR_Swap messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_DR_SWAP
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendReject;
- /* Reject Get_Source_Cap messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GET_SOURCE_CAP
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendReject;
- /* Reject PR_Swap messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PR_SWAP
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendReject;
- /* Reject VCONN_Swap messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_VCONN_SWAP
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendReject;
- /* Reject Request messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_REQUEST
- && PD_NUMOBJ_GET(policy_engine_message) > 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendReject;
- /* Reject Sink_Capabilities messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SINK_CAPABILITIES
- && PD_NUMOBJ_GET(policy_engine_message) > 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendReject;
- /* Handle GotoMin messages */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GOTOMIN
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- if (pdb_dpm_giveback_enabled()) {
- /* We support GiveBack, so go to minimum power */
- pdb_dpm_output_set(false);
- min_power = true;
-
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkTransitionSink;
- } else {
- /* We don't support GiveBack, so send a Reject */
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendReject;
- }
- /* Evaluate new Source_Capabilities */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOURCE_CAPABILITIES
- && PD_NUMOBJ_GET(policy_engine_message) > 0) {
- /* Don't free the message: we need to keep the
- * Source_Capabilities message so we can evaluate it. */
- return PESinkEvalCap;
- /* Give sink capabilities when asked */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GET_SINK_CAP
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkGiveSinkCap;
- /* If the message was a Soft_Reset, do the soft reset procedure */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSoftReset;
- /* If we got an unknown message, send a soft reset */
- } else {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSendSoftReset;
- }
- }
- }
-
- return PESinkReady;
- }
-
- static enum policy_engine_state pe_sink_get_source_cap(void)
- {
- /* Stubbed until we actually have a need for this */
- return PESinkReady;
- }
-
- static enum policy_engine_state pe_sink_give_sink_cap(void)
- {
- /* Get a message object */
- union pd_msg *snk_cap = chPoolAlloc(&pdb_msg_pool);
- /* Get our capabilities from the DPM */
- pdb_dpm_get_sink_capability(snk_cap);
-
- /* Transmit our capabilities */
- chMBPost(&pdb_prltx_mailbox, (msg_t) snk_cap, TIME_IMMEDIATE);
- chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
- eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
- | PDB_EVT_PE_RESET);
-
- /* Free the Sink_Capabilities message */
- chPoolFree(&pdb_msg_pool, snk_cap);
- snk_cap = NULL;
-
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If the message transmission failed, send a hard reset */
- if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
- return PESinkHardReset;
- }
-
- return PESinkReady;
- }
-
- static enum policy_engine_state pe_sink_hard_reset(void)
- {
- /* If we've already sent the maximum number of hard resets, assume the
- * source is unresponsive. */
- if (hard_reset_counter > PD_N_HARD_RESET_COUNT) {
- return PESinkSourceUnresponsive;
- }
-
- /* Generate a hard reset signal */
- chEvtSignal(pdb_hardrst_thread, PDB_EVT_HARDRST_RESET);
- chEvtWaitAny(PDB_EVT_PE_HARD_SENT);
-
- /* Increment HardResetCounter */
- hard_reset_counter++;
-
- return PESinkTransitionDefault;
- }
-
- static enum policy_engine_state pe_sink_transition_default(void)
- {
- explicit_contract = false;
-
- /* Tell the DPM to transition to default power */
- pdb_dpm_output_default();
-
- /* There is no local hardware to reset. */
- /* Since we never change our data role from UFP, there is no reason to set
- * it here. */
-
- /* Tell the protocol layer we're done with the reset */
- chEvtSignal(pdb_hardrst_thread, PDB_EVT_HARDRST_DONE);
-
- return PESinkStartup;
- }
-
- static enum policy_engine_state pe_sink_soft_reset(void)
- {
- /* No need to explicitly reset the protocol layer here. It resets itself
- * when a Soft_Reset message is received. */
-
- /* Get a message object */
- union pd_msg *accept = chPoolAlloc(&pdb_msg_pool);
- /* Make an Accept message */
- accept->hdr = PD_MSGTYPE_ACCEPT | PD_DATAROLE_UFP | PD_SPECREV_2_0
- | PD_POWERROLE_SINK | PD_NUMOBJ(0);
- /* Transmit the Accept */
- chMBPost(&pdb_prltx_mailbox, (msg_t) accept, TIME_IMMEDIATE);
- chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
- eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
- | PDB_EVT_PE_RESET);
- /* Free the sent message */
- chPoolFree(&pdb_msg_pool, accept);
- accept = NULL;
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If the message transmission failed, send a hard reset */
- if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
- return PESinkHardReset;
- }
-
- return PESinkWaitCap;
- }
-
- static enum policy_engine_state pe_sink_send_soft_reset(void)
- {
- /* No need to explicitly reset the protocol layer here. It resets itself
- * just before a Soft_Reset message is transmitted. */
-
- /* Get a message object */
- union pd_msg *softrst = chPoolAlloc(&pdb_msg_pool);
- /* Make a Soft_Reset message */
- softrst->hdr = PD_MSGTYPE_SOFT_RESET | PD_DATAROLE_UFP | PD_SPECREV_2_0
- | PD_POWERROLE_SINK | PD_NUMOBJ(0);
- /* Transmit the soft reset */
- chMBPost(&pdb_prltx_mailbox, (msg_t) softrst, TIME_IMMEDIATE);
- chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
- eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
- | PDB_EVT_PE_RESET);
- /* Free the sent message */
- chPoolFree(&pdb_msg_pool, softrst);
- softrst = NULL;
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If the message transmission failed, send a hard reset */
- if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
- return PESinkHardReset;
- }
-
- /* Wait for a response */
- evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
- PD_T_SENDER_RESPONSE);
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If we didn't get a response before the timeout, send a hard reset */
- if (evt == 0) {
- return PESinkHardReset;
- }
-
- /* Get the response message */
- if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
- /* If the source accepted our soft reset, wait for capabilities. */
- if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_ACCEPT
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkWaitCap;
- /* If the message was a Soft_Reset, do the soft reset procedure */
- } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
- && PD_NUMOBJ_GET(policy_engine_message) == 0) {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkSoftReset;
- /* Otherwise, send a hard reset */
- } else {
- chPoolFree(&pdb_msg_pool, policy_engine_message);
- policy_engine_message = NULL;
- return PESinkHardReset;
- }
- }
- return PESinkHardReset;
- }
-
- static enum policy_engine_state pe_sink_send_reject(void)
- {
- /* Get a message object */
- union pd_msg *reject = chPoolAlloc(&pdb_msg_pool);
- /* Make a Reject message */
- reject->hdr = PD_MSGTYPE_REJECT | PD_DATAROLE_UFP | PD_SPECREV_2_0
- | PD_POWERROLE_SINK | PD_NUMOBJ(0);
-
- /* Transmit the message */
- chMBPost(&pdb_prltx_mailbox, (msg_t) reject, TIME_IMMEDIATE);
- chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
- eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
- | PDB_EVT_PE_RESET);
-
- /* Free the Reject message */
- chPoolFree(&pdb_msg_pool, reject);
- reject = NULL;
-
- /* If we got reset signaling, transition to default */
- if (evt & PDB_EVT_PE_RESET) {
- return PESinkTransitionDefault;
- }
- /* If the message transmission failed, send a soft reset */
- if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
- return PESinkSendSoftReset;
- }
-
- return PESinkReady;
- }
-
- /*
- * When Power Delivery is unresponsive, fall back to Type-C Current
- */
- static enum policy_engine_state pe_sink_source_unresponsive(void)
- {
- static int old_tcc_match = -1;
- int tcc_match = pdb_dpm_evaluate_typec_current();
-
- /* If the last two readings are the same, set the output */
- if (old_tcc_match == tcc_match) {
- pdb_dpm_output_set(tcc_match);
- }
-
- /* Remember whether or not the last measurement succeeded */
- old_tcc_match = tcc_match;
-
- /* Wait tPDDebounce between measurements */
- chThdSleep(PD_T_PD_DEBOUNCE);
-
- return PESinkSourceUnresponsive;
- }
-
- /*
- * Policy Engine state machine thread
- */
- static THD_WORKING_AREA(waPolicyEngine, 128);
- static THD_FUNCTION(PolicyEngine, arg) {
- (void) arg;
- enum policy_engine_state state = PESinkStartup;
-
- /* Initialize the mailbox */
- chMBObjectInit(&pdb_pe_mailbox, pdb_pe_mailbox_queue, PDB_MSG_POOL_SIZE);
-
- while (true) {
- switch (state) {
- case PESinkStartup:
- state = pe_sink_startup();
- break;
- case PESinkDiscovery:
- state = pe_sink_discovery();
- break;
- case PESinkWaitCap:
- state = pe_sink_wait_cap();
- break;
- case PESinkEvalCap:
- state = pe_sink_eval_cap();
- break;
- case PESinkSelectCap:
- state = pe_sink_select_cap();
- break;
- case PESinkTransitionSink:
- state = pe_sink_transition_sink();
- break;
- case PESinkReady:
- state = pe_sink_ready();
- break;
- case PESinkGetSourceCap:
- state = pe_sink_get_source_cap();
- break;
- case PESinkGiveSinkCap:
- state = pe_sink_give_sink_cap();
- break;
- case PESinkHardReset:
- state = pe_sink_hard_reset();
- break;
- case PESinkTransitionDefault:
- state = pe_sink_transition_default();
- break;
- case PESinkSoftReset:
- state = pe_sink_soft_reset();
- break;
- case PESinkSendSoftReset:
- state = pe_sink_send_soft_reset();
- break;
- case PESinkSendReject:
- state = pe_sink_send_reject();
- break;
- case PESinkSourceUnresponsive:
- state = pe_sink_source_unresponsive();
- break;
- default:
- /* This is an error. It really shouldn't happen. We might
- * want to handle it anyway, though. */
- state = PESinkStartup;
- break;
- }
- }
- }
-
- void pdb_pe_run(void)
- {
- pdb_pe_thread = chThdCreateStatic(waPolicyEngine, sizeof(waPolicyEngine),
- PDB_PRIO_PE, PolicyEngine, NULL);
- }
|