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 27KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730
  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 "fusb302b.h"
  26. #include "pd.h"
  27. thread_t *pdb_pe_thread;
  28. enum policy_engine_state {
  29. PESinkStartup,
  30. PESinkDiscovery,
  31. PESinkWaitCap,
  32. PESinkEvalCap,
  33. PESinkSelectCap,
  34. PESinkTransitionSink,
  35. PESinkReady,
  36. PESinkGetSourceCap,
  37. PESinkGiveSinkCap,
  38. PESinkHardReset,
  39. PESinkTransitionDefault,
  40. PESinkSoftReset,
  41. PESinkSendSoftReset,
  42. PESinkSendReject,
  43. PESinkSourceUnresponsive
  44. };
  45. /* The received message we're currently working with */
  46. static union pd_msg *policy_engine_message = NULL;
  47. /* The most recent Request from the DPM */
  48. static union pd_msg *last_dpm_request = NULL;
  49. /* Whether or not the source capabilities match our required power */
  50. static bool capability_match = false;
  51. /* Whether or not we have an explicit contract */
  52. static bool explicit_contract = false;
  53. /* Whether or not we're receiving minimum power*/
  54. static bool min_power = false;
  55. /* Keep track of how many hard resets we've sent */
  56. static int hard_reset_counter = 0;
  57. /* Policy Engine thread mailbox */
  58. static msg_t pdb_pe_mailbox_queue[PDB_MSG_POOL_SIZE];
  59. mailbox_t pdb_pe_mailbox;
  60. static enum policy_engine_state pe_sink_startup(void)
  61. {
  62. /* We don't have an explicit contract currently */
  63. explicit_contract = false;
  64. /* Tell the DPM that we've started negotiations */
  65. pdb_dpm_pd_start();
  66. /* No need to reset the protocol layer here. There are two ways into this
  67. * state: startup and exiting hard reset. On startup, the protocol layer
  68. * is reset by the startup procedure. When exiting hard reset, the
  69. * protocol layer is reset by the hard reset state machine. Since it's
  70. * already done somewhere else, there's no need to do it again here. */
  71. return PESinkDiscovery;
  72. }
  73. static enum policy_engine_state pe_sink_discovery(void)
  74. {
  75. /* Wait for VBUS. Since it's our only power source, we already know that
  76. * we have it, so just move on. */
  77. return PESinkWaitCap;
  78. }
  79. static enum policy_engine_state pe_sink_wait_cap(void)
  80. {
  81. /* Fetch a message from the protocol layer */
  82. eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX
  83. | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_RESET, PD_T_TYPEC_SINK_WAIT_CAP);
  84. /* If we timed out waiting for Source_Capabilities, send a hard reset */
  85. if (evt == 0) {
  86. return PESinkHardReset;
  87. }
  88. /* If we got reset signaling, transition to default */
  89. if (evt & PDB_EVT_PE_RESET) {
  90. return PESinkTransitionDefault;
  91. }
  92. /* If we're too hot, we shouldn't negotiate power yet */
  93. if (evt & PDB_EVT_PE_I_OVRTEMP) {
  94. return PESinkWaitCap;
  95. }
  96. /* If we got a message */
  97. if (evt & PDB_EVT_PE_MSG_RX) {
  98. /* Get the message */
  99. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  100. /* If we got a Source_Capabilities message, read it. */
  101. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOURCE_CAPABILITIES
  102. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  103. return PESinkEvalCap;
  104. /* If the message was a Soft_Reset, do the soft reset procedure */
  105. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  106. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  107. chPoolFree(&pdb_msg_pool, policy_engine_message);
  108. policy_engine_message = NULL;
  109. return PESinkSoftReset;
  110. /* If we got an unexpected message, reset */
  111. } else {
  112. /* Free the received message */
  113. chPoolFree(&pdb_msg_pool, policy_engine_message);
  114. return PESinkHardReset;
  115. }
  116. }
  117. }
  118. /* If we failed to get a message, send a hard reset */
  119. return PESinkHardReset;
  120. }
  121. static enum policy_engine_state pe_sink_eval_cap(void)
  122. {
  123. /* Get a message object for the request if we don't have one already */
  124. if (last_dpm_request == NULL) {
  125. last_dpm_request = chPoolAlloc(&pdb_msg_pool);
  126. }
  127. /* Ask the DPM what to request */
  128. capability_match = pdb_dpm_evaluate_capability(policy_engine_message,
  129. last_dpm_request);
  130. /* It's up to the DPM to free the Source_Capabilities message, which it can
  131. * do whenever it sees fit. Just remove our reference to it since we won't
  132. * know when it's no longer valid. */
  133. policy_engine_message = NULL;
  134. return PESinkSelectCap;
  135. }
  136. static enum policy_engine_state pe_sink_select_cap(void)
  137. {
  138. /* Transmit the request */
  139. chMBPost(&pdb_prltx_mailbox, (msg_t) last_dpm_request, TIME_IMMEDIATE);
  140. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  141. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  142. | PDB_EVT_PE_RESET);
  143. /* Don't free the request; we might need it again */
  144. /* If we got reset signaling, transition to default */
  145. if (evt & PDB_EVT_PE_RESET) {
  146. return PESinkTransitionDefault;
  147. }
  148. /* If the message transmission failed, send a hard reset */
  149. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  150. return PESinkHardReset;
  151. }
  152. /* Wait for a response */
  153. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
  154. PD_T_SENDER_RESPONSE);
  155. /* If we got reset signaling, transition to default */
  156. if (evt & PDB_EVT_PE_RESET) {
  157. return PESinkTransitionDefault;
  158. }
  159. /* If we didn't get a response before the timeout, send a hard reset */
  160. if (evt == 0) {
  161. return PESinkHardReset;
  162. }
  163. /* Get the response message */
  164. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  165. /* If the source accepted our request, wait for the new power */
  166. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_ACCEPT
  167. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  168. /* Transition to Sink Standby if necessary */
  169. pdb_dpm_sink_standby();
  170. min_power = false;
  171. chPoolFree(&pdb_msg_pool, policy_engine_message);
  172. policy_engine_message = NULL;
  173. return PESinkTransitionSink;
  174. /* If the message was a Soft_Reset, do the soft reset procedure */
  175. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  176. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  177. chPoolFree(&pdb_msg_pool, policy_engine_message);
  178. policy_engine_message = NULL;
  179. return PESinkSoftReset;
  180. /* If the message was Wait or Reject */
  181. } else if ((PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_REJECT
  182. || PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_WAIT)
  183. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  184. /* If we don't have an explicit contract, wait for capabilities */
  185. if (!explicit_contract) {
  186. chPoolFree(&pdb_msg_pool, policy_engine_message);
  187. policy_engine_message = NULL;
  188. return PESinkWaitCap;
  189. /* If we do have an explicit contract, go to the ready state */
  190. } else {
  191. /* If we got here from a Wait message, we Should run
  192. * SinkRequestTimer in the Ready state. */
  193. min_power = (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_WAIT);
  194. chPoolFree(&pdb_msg_pool, policy_engine_message);
  195. policy_engine_message = NULL;
  196. return PESinkReady;
  197. }
  198. } else {
  199. chPoolFree(&pdb_msg_pool, policy_engine_message);
  200. policy_engine_message = NULL;
  201. return PESinkSendSoftReset;
  202. }
  203. }
  204. return PESinkHardReset;
  205. }
  206. static enum policy_engine_state pe_sink_transition_sink(void)
  207. {
  208. /* Wait for the PS_RDY message */
  209. eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
  210. PD_T_PS_TRANSITION);
  211. /* If we got reset signaling, transition to default */
  212. if (evt & PDB_EVT_PE_RESET) {
  213. return PESinkTransitionDefault;
  214. }
  215. /* If no message was received, send a hard reset */
  216. if (evt == 0) {
  217. return PESinkHardReset;
  218. }
  219. /* If we received a message, read it */
  220. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  221. /* If we got a PS_RDY, handle it */
  222. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PS_RDY
  223. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  224. /* We just finished negotiating an explicit contract */
  225. explicit_contract = true;
  226. /* Set the output appropriately */
  227. if (!min_power) {
  228. pdb_dpm_output_set(capability_match);
  229. }
  230. chPoolFree(&pdb_msg_pool, policy_engine_message);
  231. policy_engine_message = NULL;
  232. return PESinkReady;
  233. /* If there was a protocol error, send a hard reset */
  234. } else {
  235. /* Turn off the power output before this hard reset to make sure we
  236. * don't supply an incorrect voltage to the device we're powering.
  237. */
  238. pdb_dpm_output_set(false);
  239. chPoolFree(&pdb_msg_pool, policy_engine_message);
  240. policy_engine_message = NULL;
  241. return PESinkHardReset;
  242. }
  243. }
  244. return PESinkHardReset;
  245. }
  246. static enum policy_engine_state pe_sink_ready(void)
  247. {
  248. eventmask_t evt;
  249. /* Wait for an event */
  250. if (min_power) {
  251. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET
  252. | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_GET_SOURCE_CAP
  253. | PDB_EVT_PE_NEW_POWER, PD_T_SINK_REQUEST);
  254. } else {
  255. evt = chEvtWaitAny(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET
  256. | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_GET_SOURCE_CAP
  257. | PDB_EVT_PE_NEW_POWER);
  258. }
  259. /* If we got reset signaling, transition to default */
  260. if (evt & PDB_EVT_PE_RESET) {
  261. return PESinkTransitionDefault;
  262. }
  263. /* If we overheated, send a hard reset */
  264. if (evt & PDB_EVT_PE_I_OVRTEMP) {
  265. return PESinkHardReset;
  266. }
  267. /* If the DPM wants us to, send a Get_Source_Cap message */
  268. if (evt & PDB_EVT_PE_GET_SOURCE_CAP) {
  269. return PESinkGetSourceCap;
  270. }
  271. /* If the DPM wants new power, let it figure out what power it wants
  272. * exactly. This isn't exactly the transition from the spec (that would be
  273. * SelectCap, not EvalCap), but this works better with the particular
  274. * design of this firmware. */
  275. if (evt & PDB_EVT_PE_NEW_POWER) {
  276. /* Make sure we're evaluating NULL capabilities to use the old ones */
  277. if (policy_engine_message != NULL) {
  278. chPoolFree(&pdb_msg_pool, policy_engine_message);
  279. policy_engine_message = NULL;
  280. }
  281. return PESinkEvalCap;
  282. }
  283. /* If no event was received, the timer ran out. */
  284. if (evt == 0) {
  285. /* Repeat our Request message */
  286. return PESinkSelectCap;
  287. }
  288. /* If we received a message */
  289. if (evt & PDB_EVT_PE_MSG_RX) {
  290. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  291. /* Ignore vendor-defined messages */
  292. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_VENDOR_DEFINED
  293. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  294. chPoolFree(&pdb_msg_pool, policy_engine_message);
  295. policy_engine_message = NULL;
  296. return PESinkReady;
  297. /* Ignore Ping messages */
  298. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PING
  299. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  300. chPoolFree(&pdb_msg_pool, policy_engine_message);
  301. policy_engine_message = NULL;
  302. return PESinkReady;
  303. /* Reject DR_Swap messages */
  304. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_DR_SWAP
  305. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  306. chPoolFree(&pdb_msg_pool, policy_engine_message);
  307. policy_engine_message = NULL;
  308. return PESinkSendReject;
  309. /* Reject Get_Source_Cap messages */
  310. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GET_SOURCE_CAP
  311. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  312. chPoolFree(&pdb_msg_pool, policy_engine_message);
  313. policy_engine_message = NULL;
  314. return PESinkSendReject;
  315. /* Reject PR_Swap messages */
  316. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_PR_SWAP
  317. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  318. chPoolFree(&pdb_msg_pool, policy_engine_message);
  319. policy_engine_message = NULL;
  320. return PESinkSendReject;
  321. /* Reject VCONN_Swap messages */
  322. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_VCONN_SWAP
  323. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  324. chPoolFree(&pdb_msg_pool, policy_engine_message);
  325. policy_engine_message = NULL;
  326. return PESinkSendReject;
  327. /* Reject Request messages */
  328. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_REQUEST
  329. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  330. chPoolFree(&pdb_msg_pool, policy_engine_message);
  331. policy_engine_message = NULL;
  332. return PESinkSendReject;
  333. /* Reject Sink_Capabilities messages */
  334. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SINK_CAPABILITIES
  335. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  336. chPoolFree(&pdb_msg_pool, policy_engine_message);
  337. policy_engine_message = NULL;
  338. return PESinkSendReject;
  339. /* Handle GotoMin messages */
  340. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GOTOMIN
  341. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  342. if (pdb_dpm_giveback_enabled()) {
  343. /* We support GiveBack, so go to minimum power */
  344. pdb_dpm_output_set(false);
  345. min_power = true;
  346. chPoolFree(&pdb_msg_pool, policy_engine_message);
  347. policy_engine_message = NULL;
  348. return PESinkTransitionSink;
  349. } else {
  350. /* We don't support GiveBack, so send a Reject */
  351. chPoolFree(&pdb_msg_pool, policy_engine_message);
  352. policy_engine_message = NULL;
  353. return PESinkSendReject;
  354. }
  355. /* Evaluate new Source_Capabilities */
  356. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOURCE_CAPABILITIES
  357. && PD_NUMOBJ_GET(policy_engine_message) > 0) {
  358. /* Don't free the message: we need to keep the
  359. * Source_Capabilities message so we can evaluate it. */
  360. return PESinkEvalCap;
  361. /* Give sink capabilities when asked */
  362. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_GET_SINK_CAP
  363. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  364. chPoolFree(&pdb_msg_pool, policy_engine_message);
  365. policy_engine_message = NULL;
  366. return PESinkGiveSinkCap;
  367. /* If the message was a Soft_Reset, do the soft reset procedure */
  368. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  369. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  370. chPoolFree(&pdb_msg_pool, policy_engine_message);
  371. policy_engine_message = NULL;
  372. return PESinkSoftReset;
  373. /* If we got an unknown message, send a soft reset */
  374. } else {
  375. chPoolFree(&pdb_msg_pool, policy_engine_message);
  376. policy_engine_message = NULL;
  377. return PESinkSendSoftReset;
  378. }
  379. }
  380. }
  381. return PESinkReady;
  382. }
  383. static enum policy_engine_state pe_sink_get_source_cap(void)
  384. {
  385. /* Get a message object */
  386. union pd_msg *get_source_cap = chPoolAlloc(&pdb_msg_pool);
  387. /* Make a Get_Source_Cap message */
  388. get_source_cap->hdr = PD_MSGTYPE_GET_SOURCE_CAP | PD_DATAROLE_UFP
  389. | PD_SPECREV_2_0 | PD_POWERROLE_SINK | PD_NUMOBJ(0);
  390. /* Transmit the Get_Source_Cap */
  391. chMBPost(&pdb_prltx_mailbox, (msg_t) get_source_cap, TIME_IMMEDIATE);
  392. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  393. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  394. | PDB_EVT_PE_RESET);
  395. /* Free the sent message */
  396. chPoolFree(&pdb_msg_pool, get_source_cap);
  397. get_source_cap = NULL;
  398. /* If we got reset signaling, transition to default */
  399. if (evt & PDB_EVT_PE_RESET) {
  400. return PESinkTransitionDefault;
  401. }
  402. /* If the message transmission failed, send a hard reset */
  403. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  404. return PESinkHardReset;
  405. }
  406. return PESinkReady;
  407. }
  408. static enum policy_engine_state pe_sink_give_sink_cap(void)
  409. {
  410. /* Get a message object */
  411. union pd_msg *snk_cap = chPoolAlloc(&pdb_msg_pool);
  412. /* Get our capabilities from the DPM */
  413. pdb_dpm_get_sink_capability(snk_cap);
  414. /* Transmit our capabilities */
  415. chMBPost(&pdb_prltx_mailbox, (msg_t) snk_cap, TIME_IMMEDIATE);
  416. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  417. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  418. | PDB_EVT_PE_RESET);
  419. /* Free the Sink_Capabilities message */
  420. chPoolFree(&pdb_msg_pool, snk_cap);
  421. snk_cap = NULL;
  422. /* If we got reset signaling, transition to default */
  423. if (evt & PDB_EVT_PE_RESET) {
  424. return PESinkTransitionDefault;
  425. }
  426. /* If the message transmission failed, send a hard reset */
  427. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  428. return PESinkHardReset;
  429. }
  430. return PESinkReady;
  431. }
  432. static enum policy_engine_state pe_sink_hard_reset(void)
  433. {
  434. /* If we've already sent the maximum number of hard resets, assume the
  435. * source is unresponsive. */
  436. if (hard_reset_counter > PD_N_HARD_RESET_COUNT) {
  437. return PESinkSourceUnresponsive;
  438. }
  439. /* Generate a hard reset signal */
  440. chEvtSignal(pdb_hardrst_thread, PDB_EVT_HARDRST_RESET);
  441. chEvtWaitAny(PDB_EVT_PE_HARD_SENT);
  442. /* Increment HardResetCounter */
  443. hard_reset_counter++;
  444. return PESinkTransitionDefault;
  445. }
  446. static enum policy_engine_state pe_sink_transition_default(void)
  447. {
  448. explicit_contract = false;
  449. /* Tell the DPM to transition to default power */
  450. pdb_dpm_output_default();
  451. /* There is no local hardware to reset. */
  452. /* Since we never change our data role from UFP, there is no reason to set
  453. * it here. */
  454. /* Tell the protocol layer we're done with the reset */
  455. chEvtSignal(pdb_hardrst_thread, PDB_EVT_HARDRST_DONE);
  456. return PESinkStartup;
  457. }
  458. static enum policy_engine_state pe_sink_soft_reset(void)
  459. {
  460. /* No need to explicitly reset the protocol layer here. It resets itself
  461. * when a Soft_Reset message is received. */
  462. /* Get a message object */
  463. union pd_msg *accept = chPoolAlloc(&pdb_msg_pool);
  464. /* Make an Accept message */
  465. accept->hdr = PD_MSGTYPE_ACCEPT | PD_DATAROLE_UFP | PD_SPECREV_2_0
  466. | PD_POWERROLE_SINK | PD_NUMOBJ(0);
  467. /* Transmit the Accept */
  468. chMBPost(&pdb_prltx_mailbox, (msg_t) accept, TIME_IMMEDIATE);
  469. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  470. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  471. | PDB_EVT_PE_RESET);
  472. /* Free the sent message */
  473. chPoolFree(&pdb_msg_pool, accept);
  474. accept = NULL;
  475. /* If we got reset signaling, transition to default */
  476. if (evt & PDB_EVT_PE_RESET) {
  477. return PESinkTransitionDefault;
  478. }
  479. /* If the message transmission failed, send a hard reset */
  480. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  481. return PESinkHardReset;
  482. }
  483. return PESinkWaitCap;
  484. }
  485. static enum policy_engine_state pe_sink_send_soft_reset(void)
  486. {
  487. /* No need to explicitly reset the protocol layer here. It resets itself
  488. * just before a Soft_Reset message is transmitted. */
  489. /* Get a message object */
  490. union pd_msg *softrst = chPoolAlloc(&pdb_msg_pool);
  491. /* Make a Soft_Reset message */
  492. softrst->hdr = PD_MSGTYPE_SOFT_RESET | PD_DATAROLE_UFP | PD_SPECREV_2_0
  493. | PD_POWERROLE_SINK | PD_NUMOBJ(0);
  494. /* Transmit the soft reset */
  495. chMBPost(&pdb_prltx_mailbox, (msg_t) softrst, TIME_IMMEDIATE);
  496. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  497. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  498. | PDB_EVT_PE_RESET);
  499. /* Free the sent message */
  500. chPoolFree(&pdb_msg_pool, softrst);
  501. softrst = NULL;
  502. /* If we got reset signaling, transition to default */
  503. if (evt & PDB_EVT_PE_RESET) {
  504. return PESinkTransitionDefault;
  505. }
  506. /* If the message transmission failed, send a hard reset */
  507. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  508. return PESinkHardReset;
  509. }
  510. /* Wait for a response */
  511. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
  512. PD_T_SENDER_RESPONSE);
  513. /* If we got reset signaling, transition to default */
  514. if (evt & PDB_EVT_PE_RESET) {
  515. return PESinkTransitionDefault;
  516. }
  517. /* If we didn't get a response before the timeout, send a hard reset */
  518. if (evt == 0) {
  519. return PESinkHardReset;
  520. }
  521. /* Get the response message */
  522. if (chMBFetch(&pdb_pe_mailbox, (msg_t *) &policy_engine_message, TIME_IMMEDIATE) == MSG_OK) {
  523. /* If the source accepted our soft reset, wait for capabilities. */
  524. if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_ACCEPT
  525. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  526. chPoolFree(&pdb_msg_pool, policy_engine_message);
  527. policy_engine_message = NULL;
  528. return PESinkWaitCap;
  529. /* If the message was a Soft_Reset, do the soft reset procedure */
  530. } else if (PD_MSGTYPE_GET(policy_engine_message) == PD_MSGTYPE_SOFT_RESET
  531. && PD_NUMOBJ_GET(policy_engine_message) == 0) {
  532. chPoolFree(&pdb_msg_pool, policy_engine_message);
  533. policy_engine_message = NULL;
  534. return PESinkSoftReset;
  535. /* Otherwise, send a hard reset */
  536. } else {
  537. chPoolFree(&pdb_msg_pool, policy_engine_message);
  538. policy_engine_message = NULL;
  539. return PESinkHardReset;
  540. }
  541. }
  542. return PESinkHardReset;
  543. }
  544. static enum policy_engine_state pe_sink_send_reject(void)
  545. {
  546. /* Get a message object */
  547. union pd_msg *reject = chPoolAlloc(&pdb_msg_pool);
  548. /* Make a Reject message */
  549. reject->hdr = PD_MSGTYPE_REJECT | PD_DATAROLE_UFP | PD_SPECREV_2_0
  550. | PD_POWERROLE_SINK | PD_NUMOBJ(0);
  551. /* Transmit the message */
  552. chMBPost(&pdb_prltx_mailbox, (msg_t) reject, TIME_IMMEDIATE);
  553. chEvtSignal(pdb_prltx_thread, PDB_EVT_PRLTX_MSG_TX);
  554. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  555. | PDB_EVT_PE_RESET);
  556. /* Free the Reject message */
  557. chPoolFree(&pdb_msg_pool, reject);
  558. reject = NULL;
  559. /* If we got reset signaling, transition to default */
  560. if (evt & PDB_EVT_PE_RESET) {
  561. return PESinkTransitionDefault;
  562. }
  563. /* If the message transmission failed, send a soft reset */
  564. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  565. return PESinkSendSoftReset;
  566. }
  567. return PESinkReady;
  568. }
  569. /*
  570. * When Power Delivery is unresponsive, fall back to Type-C Current
  571. */
  572. static enum policy_engine_state pe_sink_source_unresponsive(void)
  573. {
  574. static int old_tcc_match = -1;
  575. int tcc_match = pdb_dpm_evaluate_typec_current(fusb_get_typec_current());
  576. /* If the last two readings are the same, set the output */
  577. if (old_tcc_match == tcc_match) {
  578. pdb_dpm_output_set(tcc_match);
  579. }
  580. /* Remember whether or not the last measurement succeeded */
  581. old_tcc_match = tcc_match;
  582. /* Wait tPDDebounce between measurements */
  583. chThdSleep(PD_T_PD_DEBOUNCE);
  584. return PESinkSourceUnresponsive;
  585. }
  586. /*
  587. * Policy Engine state machine thread
  588. */
  589. static THD_WORKING_AREA(waPolicyEngine, 128);
  590. static THD_FUNCTION(PolicyEngine, arg) {
  591. (void) arg;
  592. enum policy_engine_state state = PESinkStartup;
  593. /* Initialize the mailbox */
  594. chMBObjectInit(&pdb_pe_mailbox, pdb_pe_mailbox_queue, PDB_MSG_POOL_SIZE);
  595. while (true) {
  596. switch (state) {
  597. case PESinkStartup:
  598. state = pe_sink_startup();
  599. break;
  600. case PESinkDiscovery:
  601. state = pe_sink_discovery();
  602. break;
  603. case PESinkWaitCap:
  604. state = pe_sink_wait_cap();
  605. break;
  606. case PESinkEvalCap:
  607. state = pe_sink_eval_cap();
  608. break;
  609. case PESinkSelectCap:
  610. state = pe_sink_select_cap();
  611. break;
  612. case PESinkTransitionSink:
  613. state = pe_sink_transition_sink();
  614. break;
  615. case PESinkReady:
  616. state = pe_sink_ready();
  617. break;
  618. case PESinkGetSourceCap:
  619. state = pe_sink_get_source_cap();
  620. break;
  621. case PESinkGiveSinkCap:
  622. state = pe_sink_give_sink_cap();
  623. break;
  624. case PESinkHardReset:
  625. state = pe_sink_hard_reset();
  626. break;
  627. case PESinkTransitionDefault:
  628. state = pe_sink_transition_default();
  629. break;
  630. case PESinkSoftReset:
  631. state = pe_sink_soft_reset();
  632. break;
  633. case PESinkSendSoftReset:
  634. state = pe_sink_send_soft_reset();
  635. break;
  636. case PESinkSendReject:
  637. state = pe_sink_send_reject();
  638. break;
  639. case PESinkSourceUnresponsive:
  640. state = pe_sink_source_unresponsive();
  641. break;
  642. default:
  643. /* This is an error. It really shouldn't happen. We might
  644. * want to handle it anyway, though. */
  645. state = PESinkStartup;
  646. break;
  647. }
  648. }
  649. }
  650. void pdb_pe_run(void)
  651. {
  652. pdb_pe_thread = chThdCreateStatic(waPolicyEngine, sizeof(waPolicyEngine),
  653. PDB_PRIO_PE, PolicyEngine, NULL);
  654. }