PD Buddy Sink Firmware
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

policy_engine.c 26KB

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