PD Buddy Sink Firmware
Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.

policy_engine.c 25KB

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