PD Buddy Sink Firmware
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

policy_engine.c 23KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612
  1. /*
  2. * PD Buddy - USB Power Delivery for everyone
  3. * Copyright (C) 2017 Clayton G. Hobbs <clay@lakeserv.net>
  4. *
  5. * This program is free software: you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License as published by
  7. * the Free Software Foundation, either version 3 of the License, or
  8. * (at your option) any later version.
  9. *
  10. * This program is distributed in the hope that it will be useful,
  11. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13. * GNU General Public License for more details.
  14. *
  15. * You should have received a copy of the GNU General Public License
  16. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  17. */
  18. #include "policy_engine.h"
  19. #include <stdbool.h>
  20. #include "messages.h"
  21. #include "priorities.h"
  22. #include "device_policy_manager.h"
  23. #include "protocol_tx.h"
  24. #include "hard_reset.h"
  25. #include "pd.h"
  26. thread_t *pdb_pe_thread;
  27. enum policy_engine_state {
  28. PESinkStartup,
  29. PESinkDiscovery,
  30. PESinkWaitCap,
  31. PESinkEvalCap,
  32. PESinkSelectCap,
  33. PESinkTransitionSink,
  34. PESinkReady,
  35. PESinkGetSourceCap,
  36. PESinkGiveSinkCap,
  37. PESinkHardReset,
  38. PESinkTransitionDefault,
  39. PESinkSoftReset,
  40. PESinkSendSoftReset,
  41. PESinkSendReject,
  42. PESinkSourceUnresponsive
  43. };
  44. /* The received message we're currently working with */
  45. static union pd_msg *policy_engine_message = NULL;
  46. /* Whether or not the source capabilities match our required power */
  47. static bool capability_match = false;
  48. /* Whether or not we have an explicit contract */
  49. static bool explicit_contract = false;
  50. /* Keep track of how many hard resets we've sent */
  51. static int hard_reset_counter = 0;
  52. /* Policy Engine thread mailbox */
  53. static msg_t pdb_pe_mailbox_queue[PDB_MSG_POOL_SIZE];
  54. mailbox_t pdb_pe_mailbox;
  55. static enum policy_engine_state pe_sink_startup(void)
  56. {
  57. /* We don't have an explicit contract currently */
  58. explicit_contract = false;
  59. /* Tell the DPM that we've started negotiations */
  60. pdb_dpm_pd_start();
  61. /* No need to reset the protocol layer here. There are two ways into this
  62. * state: startup and exiting hard reset. On startup, the protocol layer
  63. * is reset by the startup procedure. When exiting hard reset, the
  64. * protocol layer is reset by the hard reset state machine. Since it's
  65. * already done somewhere else, there's no need to do it again here. */
  66. return PESinkDiscovery;
  67. }
  68. static enum policy_engine_state pe_sink_discovery(void)
  69. {
  70. /* Wait for VBUS. Since it's our only power source, we already know that
  71. * we have it, so just move on. */
  72. return PESinkWaitCap;
  73. }
  74. static enum policy_engine_state pe_sink_wait_cap(void)
  75. {
  76. /* Fetch a message from the protocol layer */
  77. eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX
  78. | PDB_EVT_PE_I_OVRTEMP, PD_T_TYPEC_SINK_WAIT_CAP);
  79. /* If we timed out waiting for Source_Capabilities, send a hard reset */
  80. if (evt == 0) {
  81. return PESinkHardReset;
  82. }
  83. /* If we're too hot, we shouldn't negotiate power yet */
  84. if (evt & PDB_EVT_PE_I_OVRTEMP) {
  85. return PESinkWaitCap;
  86. }
  87. /* If we got a message */
  88. if (evt & PDB_EVT_PE_MSG_RX) {
  89. /* Get the message */
  90. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  91. /* If we got a Source_Capabilities message, read it. */
  92. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOURCE_CAPABILITIES
  93. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  94. return PESinkEvalCap;
  95. /* If the message was a Soft_Reset, do the soft reset procedure */
  96. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  97. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  98. chPoolFree(&pdb_msg_pool, policy_engine_message);
  99. policy_engine_message = NULL;
  100. return PESinkSoftReset;
  101. /* If we got an unexpected message, reset */
  102. } else {
  103. /* Free the received message */
  104. chPoolFree(&pdb_msg_pool, policy_engine_message);
  105. return PESinkHardReset;
  106. }
  107. }
  108. }
  109. /* If we failed to get a message, send a hard reset */
  110. return PESinkHardReset;
  111. }
  112. static enum policy_engine_state pe_sink_eval_cap(void)
  113. {
  114. /* Get a message object */
  115. union pd_msg *request = chPoolAlloc(&pdb_msg_pool);
  116. /* Ask the DPM what to request */
  117. capability_match = pdb_dpm_evaluate_capability(policy_engine_message, request);
  118. /* Free the Source_Capabilities message */
  119. chPoolFree(&pdb_msg_pool, policy_engine_message);
  120. /* Put the request into policy_engine_message */
  121. policy_engine_message = request;
  122. return PESinkSelectCap;
  123. }
  124. static enum policy_engine_state pe_sink_select_cap(void)
  125. {
  126. /* Transmit the request */
  127. chMBPost(&pdb_prltx_mailbox, (msg_t) policy_engine_message, TIME_IMMEDIATE);
  128. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  129. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR);
  130. /* Free the sent message */
  131. chPoolFree(&pdb_msg_pool, policy_engine_message);
  132. policy_engine_message = NULL;
  133. /* If the message transmission failed, send a hard reset */
  134. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  135. return PESinkHardReset;
  136. }
  137. /* Wait for a response */
  138. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX, PD_T_SENDER_RESPONSE);
  139. /* If we didn't get a response before the timeout, send a hard reset */
  140. if (evt == 0) {
  141. return PESinkHardReset;
  142. }
  143. /* Get the response message */
  144. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  145. /* If the source accepted our request, wait for the new power */
  146. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_ACCEPT
  147. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  148. /* Transition to Sink Standby if necessary */
  149. pdb_dpm_sink_standby();
  150. chPoolFree(&pdb_msg_pool, policy_engine_message);
  151. policy_engine_message = NULL;
  152. return PESinkTransitionSink;
  153. /* If the message was a Soft_Reset, do the soft reset procedure */
  154. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  155. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  156. chPoolFree(&pdb_msg_pool, policy_engine_message);
  157. policy_engine_message = NULL;
  158. return PESinkSoftReset;
  159. /* If the message was Wait or Reject */
  160. } else if ((PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_REJECT
  161. || PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_WAIT)
  162. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  163. /* If we don't have an explicit contract, wait for capabilities */
  164. if (!explicit_contract) {
  165. chPoolFree(&pdb_msg_pool, policy_engine_message);
  166. policy_engine_message = NULL;
  167. return PESinkWaitCap;
  168. /* If we do have an explicit contract, go to the ready state */
  169. } else {
  170. /* TODO: we should take note if we got here from a Wait
  171. * message, because we Should run the SinkRequestTimer in the
  172. * Ready state if that's the case. */
  173. chPoolFree(&pdb_msg_pool, policy_engine_message);
  174. policy_engine_message = NULL;
  175. return PESinkReady;
  176. }
  177. } else {
  178. chPoolFree(&pdb_msg_pool, policy_engine_message);
  179. policy_engine_message = NULL;
  180. return PESinkSendSoftReset;
  181. }
  182. }
  183. return PESinkHardReset;
  184. }
  185. static enum policy_engine_state pe_sink_transition_sink(void)
  186. {
  187. /* Wait for the PS_RDY message */
  188. eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX, PD_T_PS_TRANSITION);
  189. /* If no message was received, send a hard reset */
  190. if (evt == 0) {
  191. return PESinkHardReset;
  192. }
  193. /* If we received a message, read it */
  194. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  195. /* If we got a PS_RDY, handle it */
  196. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PS_RDY
  197. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  198. /* We just finished negotiating an explicit contract */
  199. explicit_contract = true;
  200. /* Set the output appropriately */
  201. pdb_dpm_output_set(capability_match);
  202. chPoolFree(&pdb_msg_pool, policy_engine_message);
  203. policy_engine_message = NULL;
  204. return PESinkReady;
  205. /* If there was a protocol error, send a hard reset */
  206. } else {
  207. /* Turn off the power output before this hard reset to make sure we
  208. * don't supply an incorrect voltage to the device we're powering.
  209. */
  210. pdb_dpm_output_set(false);
  211. chPoolFree(&pdb_msg_pool, policy_engine_message);
  212. policy_engine_message = NULL;
  213. return PESinkHardReset;
  214. }
  215. }
  216. return PESinkHardReset;
  217. }
  218. static enum policy_engine_state pe_sink_ready(void)
  219. {
  220. /* Wait for an event */
  221. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET
  222. | PDB_EVT_PE_I_OVRTEMP);
  223. /* If we got reset signaling, transition to default */
  224. if (evt & PDB_EVT_PE_RESET) {
  225. return PESinkTransitionDefault;
  226. }
  227. /* If we overheated, send a hard reset */
  228. if (evt & PDB_EVT_PE_I_OVRTEMP) {
  229. return PESinkHardReset;
  230. }
  231. /* If we received a message */
  232. if (evt & PDB_EVT_PE_MSG_RX) {
  233. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  234. /* Ignore vendor-defined messages */
  235. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_VENDOR_DEFINED
  236. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  237. chPoolFree(&pdb_msg_pool, policy_engine_message);
  238. policy_engine_message = NULL;
  239. return PESinkReady;
  240. /* Ignore Ping messages */
  241. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PING
  242. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  243. chPoolFree(&pdb_msg_pool, policy_engine_message);
  244. policy_engine_message = NULL;
  245. return PESinkReady;
  246. /* Reject DR_Swap messages */
  247. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_DR_SWAP
  248. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  249. chPoolFree(&pdb_msg_pool, policy_engine_message);
  250. policy_engine_message = NULL;
  251. return PESinkSendReject;
  252. /* Reject Get_Source_Cap messages */
  253. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GET_SOURCE_CAP
  254. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  255. chPoolFree(&pdb_msg_pool, policy_engine_message);
  256. policy_engine_message = NULL;
  257. return PESinkSendReject;
  258. /* Reject PR_Swap messages */
  259. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PR_SWAP
  260. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  261. chPoolFree(&pdb_msg_pool, policy_engine_message);
  262. policy_engine_message = NULL;
  263. return PESinkSendReject;
  264. /* Reject VCONN_Swap messages */
  265. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_VCONN_SWAP
  266. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  267. chPoolFree(&pdb_msg_pool, policy_engine_message);
  268. policy_engine_message = NULL;
  269. return PESinkSendReject;
  270. /* Reject Request messages */
  271. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_REQUEST
  272. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  273. chPoolFree(&pdb_msg_pool, policy_engine_message);
  274. policy_engine_message = NULL;
  275. return PESinkSendReject;
  276. /* Reject Sink_Capabilities messages */
  277. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SINK_CAPABILITIES
  278. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  279. chPoolFree(&pdb_msg_pool, policy_engine_message);
  280. policy_engine_message = NULL;
  281. return PESinkSendReject;
  282. /* Reject GotoMin messages
  283. * Until we actually support GiveBack, this is the correct
  284. * behavior according to S. 6.3.4 */
  285. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GOTOMIN
  286. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  287. chPoolFree(&pdb_msg_pool, policy_engine_message);
  288. policy_engine_message = NULL;
  289. return PESinkSendReject;
  290. /* Evaluate new Source_Capabilities */
  291. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOURCE_CAPABILITIES
  292. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  293. /* Don't free the message: we need to keep the
  294. * Source_Capabilities message so we can evaluate it. */
  295. return PESinkEvalCap;
  296. /* Give sink capabilities when asked */
  297. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GET_SINK_CAP
  298. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  299. chPoolFree(&pdb_msg_pool, policy_engine_message);
  300. policy_engine_message = NULL;
  301. return PESinkGiveSinkCap;
  302. /* If the message was a Soft_Reset, do the soft reset procedure */
  303. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  304. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  305. chPoolFree(&pdb_msg_pool, policy_engine_message);
  306. policy_engine_message = NULL;
  307. return PESinkSoftReset;
  308. /* If we got an unknown message, send a soft reset */
  309. } else {
  310. chPoolFree(&pdb_msg_pool, policy_engine_message);
  311. policy_engine_message = NULL;
  312. return PESinkSendSoftReset;
  313. }
  314. }
  315. }
  316. return PESinkReady;
  317. }
  318. static enum policy_engine_state pe_sink_get_source_cap(void)
  319. {
  320. /* Stubbed until we actually have a need for this */
  321. return PESinkReady;
  322. }
  323. static enum policy_engine_state pe_sink_give_sink_cap(void)
  324. {
  325. /* Get a message object */
  326. union pd_msg *snk_cap = chPoolAlloc(&pdb_msg_pool);
  327. /* Get our capabilities from the DPM */
  328. pdb_dpm_get_sink_capability(snk_cap);
  329. /* Transmit our capabilities */
  330. chMBPost(&pdb_prltx_mailbox, (msg_t) snk_cap, TIME_IMMEDIATE);
  331. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  332. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR);
  333. /* Free the Sink_Capabilities message */
  334. chPoolFree(&pdb_msg_pool, snk_cap);
  335. snk_cap = NULL;
  336. /* If the message transmission failed, send a hard reset */
  337. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  338. return PESinkHardReset;
  339. }
  340. return PESinkReady;
  341. }
  342. static enum policy_engine_state pe_sink_hard_reset(void)
  343. {
  344. /* If we've already sent the maximum number of hard resets, assume the
  345. * source is unresponsive. */
  346. if (hard_reset_counter > PD_N_HARD_RESET_COUNT) {
  347. return PESinkSourceUnresponsive;
  348. }
  349. /* Generate a hard reset signal */
  350. chEvtSignal(pdb_hardrst_thread, PDB_EVT_HARDRST_RESET);
  351. chEvtWaitAny(PDB_EVT_PE_HARD_SENT);
  352. /* Increment HardResetCounter */
  353. hard_reset_counter++;
  354. return PESinkTransitionDefault;
  355. }
  356. static enum policy_engine_state pe_sink_transition_default(void)
  357. {
  358. explicit_contract = false;
  359. /* Tell the DPM to transition to default power */
  360. pdb_dpm_output_default();
  361. /* There is no local hardware to reset. */
  362. /* Since we never change our data role from UFP, there is no reason to set
  363. * it here. */
  364. /* Tell the protocol layer we're done with the reset */
  365. chEvtSignal(pdb_hardrst_thread, PDB_EVT_HARDRST_DONE);
  366. return PESinkStartup;
  367. }
  368. static enum policy_engine_state pe_sink_soft_reset(void)
  369. {
  370. /* No need to explicitly reset the protocol layer here. It resets itself
  371. * when a Soft_Reset message is received. */
  372. /* Get a message object */
  373. union pd_msg *accept = chPoolAlloc(&pdb_msg_pool);
  374. /* Make an Accept message */
  375. accept->hdr = PD_MSGTYPE_ACCEPT | PD_DATAROLE_UFP | PD_SPECREV_2_0
  376. | PD_POWERROLE_SINK | PD_NUMOBJ(0);
  377. /* Transmit the Accept */
  378. chMBPost(&pdb_prltx_mailbox, (msg_t) accept, TIME_IMMEDIATE);
  379. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  380. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR);
  381. /* Free the sent message */
  382. chPoolFree(&pdb_msg_pool, accept);
  383. accept = NULL;
  384. /* If the message transmission failed, send a hard reset */
  385. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  386. return PESinkHardReset;
  387. }
  388. return PESinkWaitCap;
  389. }
  390. static enum policy_engine_state pe_sink_send_soft_reset(void)
  391. {
  392. /* No need to explicitly reset the protocol layer here. It resets itself
  393. * just before a Soft_Reset message is transmitted. */
  394. /* Get a message object */
  395. union pd_msg *softrst = chPoolAlloc(&pdb_msg_pool);
  396. /* Make a Soft_Reset message */
  397. softrst->hdr = PD_MSGTYPE_SOFT_RESET | PD_DATAROLE_UFP | PD_SPECREV_2_0
  398. | PD_POWERROLE_SINK | PD_NUMOBJ(0);
  399. /* Transmit the soft reset */
  400. chMBPost(&pdb_prltx_mailbox, (msg_t) softrst, TIME_IMMEDIATE);
  401. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  402. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR);
  403. /* Free the sent message */
  404. chPoolFree(&pdb_msg_pool, softrst);
  405. softrst = NULL;
  406. /* If the message transmission failed, send a hard reset */
  407. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  408. return PESinkHardReset;
  409. }
  410. /* Wait for a response */
  411. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX, PD_T_SENDER_RESPONSE);
  412. /* If we didn't get a response before the timeout, send a hard reset */
  413. if (evt == 0) {
  414. return PESinkHardReset;
  415. }
  416. /* Get the response message */
  417. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  418. /* If the source accepted our soft reset, wait for capabilities. */
  419. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_ACCEPT
  420. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  421. chPoolFree(&pdb_msg_pool, policy_engine_message);
  422. policy_engine_message = NULL;
  423. return PESinkWaitCap;
  424. /* If the message was a Soft_Reset, do the soft reset procedure */
  425. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  426. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  427. chPoolFree(&pdb_msg_pool, policy_engine_message);
  428. policy_engine_message = NULL;
  429. return PESinkSoftReset;
  430. /* Otherwise, send a hard reset */
  431. } else {
  432. chPoolFree(&pdb_msg_pool, policy_engine_message);
  433. policy_engine_message = NULL;
  434. return PESinkHardReset;
  435. }
  436. }
  437. return PESinkHardReset;
  438. }
  439. static enum policy_engine_state pe_sink_send_reject(void)
  440. {
  441. /* Get a message object */
  442. union pd_msg *reject = chPoolAlloc(&pdb_msg_pool);
  443. /* Make a Reject message */
  444. reject->hdr = PD_MSGTYPE_REJECT | PD_DATAROLE_UFP | PD_SPECREV_2_0
  445. | PD_POWERROLE_SINK | PD_NUMOBJ(0);
  446. /* Transmit the message */
  447. chMBPost(&pdb_prltx_mailbox, (msg_t) reject, TIME_IMMEDIATE);
  448. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  449. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR);
  450. /* Free the Reject message */
  451. chPoolFree(&pdb_msg_pool, reject);
  452. reject = NULL;
  453. /* If the message transmission failed, send a soft reset */
  454. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  455. return PESinkSendSoftReset;
  456. }
  457. return PESinkReady;
  458. }
  459. /*
  460. * When Power Delivery is unresponsive, fall back to Type-C Current
  461. */
  462. static enum policy_engine_state pe_sink_source_unresponsive(void)
  463. {
  464. static int old_tcc_match = -1;
  465. int tcc_match = pdb_dpm_evaluate_typec_current();
  466. /* If the last two readings are the same, set the output */
  467. if (old_tcc_match == tcc_match) {
  468. pdb_dpm_output_set(tcc_match);
  469. }
  470. /* Remember whether or not the last measurement succeeded */
  471. old_tcc_match = tcc_match;
  472. /* Wait tPDDebounce between measurements */
  473. chThdSleep(PD_T_PD_DEBOUNCE);
  474. return PESinkSourceUnresponsive;
  475. }
  476. /*
  477. * Policy Engine state machine thread
  478. */
  479. static THD_WORKING_AREA(waPolicyEngine, 128);
  480. static THD_FUNCTION(PolicyEngine, arg) {
  481. (void) arg;
  482. enum policy_engine_state state = PESinkStartup;
  483. /* Initialize the mailbox */
  484. chMBObjectInit(&pdb_pe_mailbox, pdb_pe_mailbox_queue, PDB_MSG_POOL_SIZE);
  485. while (true) {
  486. switch (state) {
  487. case PESinkStartup:
  488. state = pe_sink_startup();
  489. break;
  490. case PESinkDiscovery:
  491. state = pe_sink_discovery();
  492. break;
  493. case PESinkWaitCap:
  494. state = pe_sink_wait_cap();
  495. break;
  496. case PESinkEvalCap:
  497. state = pe_sink_eval_cap();
  498. break;
  499. case PESinkSelectCap:
  500. state = pe_sink_select_cap();
  501. break;
  502. case PESinkTransitionSink:
  503. state = pe_sink_transition_sink();
  504. break;
  505. case PESinkReady:
  506. state = pe_sink_ready();
  507. break;
  508. case PESinkGetSourceCap:
  509. state = pe_sink_get_source_cap();
  510. break;
  511. case PESinkGiveSinkCap:
  512. state = pe_sink_give_sink_cap();
  513. break;
  514. case PESinkHardReset:
  515. state = pe_sink_hard_reset();
  516. break;
  517. case PESinkTransitionDefault:
  518. state = pe_sink_transition_default();
  519. break;
  520. case PESinkSoftReset:
  521. state = pe_sink_soft_reset();
  522. break;
  523. case PESinkSendSoftReset:
  524. state = pe_sink_send_soft_reset();
  525. break;
  526. case PESinkSendReject:
  527. state = pe_sink_send_reject();
  528. break;
  529. case PESinkSourceUnresponsive:
  530. state = pe_sink_source_unresponsive();
  531. break;
  532. default:
  533. /* This is an error. It really shouldn't happen. We might
  534. * want to handle it anyway, though. */
  535. state = PESinkStartup;
  536. break;
  537. }
  538. }
  539. }
  540. void pdb_pe_run(void)
  541. {
  542. pdb_pe_thread = chThdCreateStatic(waPolicyEngine, sizeof(waPolicyEngine),
  543. PDB_PRIO_PE, PolicyEngine, NULL);
  544. }