PD Buddy Sink Firmware
Nevar pievienot vairāk kā 25 tēmas Tēmai ir jāsākas ar burtu vai ciparu, tā var saturēt domu zīmes ('-') un var būt līdz 35 simboliem gara.

policy_engine.c 33KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864
  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 <pd.h>
  21. #include "priorities.h"
  22. #include "protocol_tx.h"
  23. #include "hard_reset.h"
  24. #include "fusb302b.h"
  25. static void pe_sink_pps_periodic_timer_cb(void *cfg)
  26. {
  27. /* Signal the PE thread to make a new PPS request */
  28. chSysLockFromISR();
  29. chEvtSignalI(((struct pdb_config *) cfg)->pe.thread, PDB_EVT_PE_PPS_REQUEST);
  30. chSysUnlockFromISR();
  31. }
  32. enum policy_engine_state {
  33. PESinkStartup,
  34. PESinkDiscovery,
  35. PESinkWaitCap,
  36. PESinkEvalCap,
  37. PESinkSelectCap,
  38. PESinkTransitionSink,
  39. PESinkReady,
  40. PESinkGetSourceCap,
  41. PESinkGiveSinkCap,
  42. PESinkHardReset,
  43. PESinkTransitionDefault,
  44. PESinkSoftReset,
  45. PESinkSendSoftReset,
  46. PESinkSendNotSupported,
  47. PESinkChunkReceived,
  48. PESinkNotSupportedReceived,
  49. PESinkSourceUnresponsive
  50. };
  51. static enum policy_engine_state pe_sink_startup(struct pdb_config *cfg)
  52. {
  53. /* We don't have an explicit contract currently */
  54. cfg->pe._explicit_contract = false;
  55. /* Tell the DPM that we've started negotiations, if it cares */
  56. if (cfg->dpm.pd_start != NULL) {
  57. cfg->dpm.pd_start(cfg);
  58. }
  59. /* No need to reset the protocol layer here. There are two ways into this
  60. * state: startup and exiting hard reset. On startup, the protocol layer
  61. * is reset by the startup procedure. When exiting hard reset, the
  62. * protocol layer is reset by the hard reset state machine. Since it's
  63. * already done somewhere else, there's no need to do it again here. */
  64. return PESinkDiscovery;
  65. }
  66. static enum policy_engine_state pe_sink_discovery(struct pdb_config *cfg)
  67. {
  68. (void) cfg;
  69. /* Wait for VBUS. Since it's our only power source, we already know that
  70. * we have it, so just move on. */
  71. return PESinkWaitCap;
  72. }
  73. static enum policy_engine_state pe_sink_wait_cap(struct pdb_config *cfg)
  74. {
  75. /* Fetch a message from the protocol layer */
  76. eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX
  77. | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_RESET, PD_T_TYPEC_SINK_WAIT_CAP);
  78. /* If we timed out waiting for Source_Capabilities, send a hard reset */
  79. if (evt == 0) {
  80. return PESinkHardReset;
  81. }
  82. /* If we got reset signaling, transition to default */
  83. if (evt & PDB_EVT_PE_RESET) {
  84. return PESinkTransitionDefault;
  85. }
  86. /* If we're too hot, we shouldn't negotiate power yet */
  87. if (evt & PDB_EVT_PE_I_OVRTEMP) {
  88. return PESinkWaitCap;
  89. }
  90. /* If we got a message */
  91. if (evt & PDB_EVT_PE_MSG_RX) {
  92. /* Get the message */
  93. if (chMBFetch(&cfg->pe.mailbox, (msg_t *) &cfg->pe._message, TIME_IMMEDIATE) == MSG_OK) {
  94. /* If we got a Source_Capabilities message, read it. */
  95. if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_SOURCE_CAPABILITIES
  96. && PD_NUMOBJ_GET(cfg->pe._message) > 0) {
  97. /* First, determine what PD revision we're using */
  98. if ((cfg->pe.hdr_template & PD_HDR_SPECREV) == PD_SPECREV_1_0) {
  99. /* If the other end is using at least version 3.0, we'll
  100. * use version 3.0. */
  101. if ((cfg->pe._message->hdr & PD_HDR_SPECREV) >= PD_SPECREV_3_0) {
  102. cfg->pe.hdr_template |= PD_SPECREV_3_0;
  103. /* Otherwise, use 2.0. Don't worry about the 1.0 case
  104. * because we don't have hardware for PD 1.0 signaling. */
  105. } else {
  106. cfg->pe.hdr_template |= PD_SPECREV_2_0;
  107. }
  108. }
  109. return PESinkEvalCap;
  110. /* If the message was a Soft_Reset, do the soft reset procedure */
  111. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_SOFT_RESET
  112. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  113. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  114. cfg->pe._message = NULL;
  115. return PESinkSoftReset;
  116. /* If we got an unexpected message, reset */
  117. } else {
  118. /* Free the received message */
  119. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  120. return PESinkHardReset;
  121. }
  122. }
  123. }
  124. /* If we failed to get a message, send a hard reset */
  125. return PESinkHardReset;
  126. }
  127. static enum policy_engine_state pe_sink_eval_cap(struct pdb_config *cfg)
  128. {
  129. /* If we have a Source_Capabilities message, remember the index of the
  130. * first PPS APDO so we can check if the request is for a PPS APDO in
  131. * PE_SNK_Select_Cap. */
  132. if (cfg->pe._message != NULL) {
  133. /* Start by assuming we won't find a PPS APDO (set the index greater
  134. * than the maximum possible) */
  135. cfg->pe._pps_index = 8;
  136. /* Search for the first PPS APDO */
  137. for (int8_t i = 0; i < PD_NUMOBJ_GET(cfg->pe._message); i++) {
  138. if ((cfg->pe._message->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_AUGMENTED
  139. && (cfg->pe._message->obj[i] & PD_APDO_TYPE) == PD_APDO_TYPE_PPS) {
  140. cfg->pe._pps_index = i + 1;
  141. break;
  142. }
  143. }
  144. /* New capabilities also means we can't be making a request from the
  145. * same PPS APDO */
  146. cfg->pe._last_pps = 8;
  147. }
  148. /* Get a message object for the request if we don't have one already */
  149. if (cfg->pe._last_dpm_request == NULL) {
  150. cfg->pe._last_dpm_request = chPoolAlloc(&pdb_msg_pool);
  151. } else {
  152. /* Remember the last PDO we requested if it was a PPS APDO */
  153. if (PD_RDO_OBJPOS_GET(cfg->pe._last_dpm_request) >= cfg->pe._pps_index) {
  154. cfg->pe._last_pps = PD_RDO_OBJPOS_GET(cfg->pe._last_dpm_request);
  155. /* Otherwise, forget any PPS APDO we had requested */
  156. } else {
  157. cfg->pe._last_pps = 8;
  158. }
  159. }
  160. /* Ask the DPM what to request */
  161. cfg->dpm.evaluate_capability(cfg, cfg->pe._message,
  162. cfg->pe._last_dpm_request);
  163. /* It's up to the DPM to free the Source_Capabilities message, which it can
  164. * do whenever it sees fit. Just remove our reference to it since we won't
  165. * know when it's no longer valid. */
  166. cfg->pe._message = NULL;
  167. return PESinkSelectCap;
  168. }
  169. static enum policy_engine_state pe_sink_select_cap(struct pdb_config *cfg)
  170. {
  171. /* Transmit the request */
  172. chMBPost(&cfg->prl.tx_mailbox, (msg_t) cfg->pe._last_dpm_request, TIME_IMMEDIATE);
  173. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_MSG_TX);
  174. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  175. | PDB_EVT_PE_RESET);
  176. /* Don't free the request; we might need it again */
  177. /* If we got reset signaling, transition to default */
  178. if (evt & PDB_EVT_PE_RESET) {
  179. return PESinkTransitionDefault;
  180. }
  181. /* If the message transmission failed, send a hard reset */
  182. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  183. return PESinkHardReset;
  184. }
  185. /* If we're using PD 3.0 */
  186. if ((cfg->pe.hdr_template & PD_HDR_SPECREV) == PD_SPECREV_3_0) {
  187. /* If the request was for a PPS APDO, start SinkPPSPeriodicTimer */
  188. if (PD_RDO_OBJPOS_GET(cfg->pe._last_dpm_request) >= cfg->pe._pps_index) {
  189. chVTSet(&cfg->pe._sink_pps_periodic_timer, PD_T_PPS_REQUEST,
  190. pe_sink_pps_periodic_timer_cb, cfg);
  191. /* Otherwise, stop SinkPPSPeriodicTimer */
  192. } else {
  193. chVTReset(&cfg->pe._sink_pps_periodic_timer);
  194. }
  195. }
  196. /* This will use a virtual timer to send an event flag to this thread after
  197. * PD_T_PPS_REQUEST */
  198. /* Wait for a response */
  199. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
  200. PD_T_SENDER_RESPONSE);
  201. /* If we got reset signaling, transition to default */
  202. if (evt & PDB_EVT_PE_RESET) {
  203. return PESinkTransitionDefault;
  204. }
  205. /* If we didn't get a response before the timeout, send a hard reset */
  206. if (evt == 0) {
  207. return PESinkHardReset;
  208. }
  209. /* Get the response message */
  210. if (chMBFetch(&cfg->pe.mailbox, (msg_t *) &cfg->pe._message, TIME_IMMEDIATE) == MSG_OK) {
  211. /* If the source accepted our request, wait for the new power */
  212. if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_ACCEPT
  213. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  214. /* Transition to Sink Standby if necessary */
  215. if (PD_RDO_OBJPOS_GET(cfg->pe._last_dpm_request) != cfg->pe._last_pps) {
  216. cfg->dpm.transition_standby(cfg);
  217. }
  218. cfg->pe._min_power = false;
  219. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  220. cfg->pe._message = NULL;
  221. return PESinkTransitionSink;
  222. /* If the message was a Soft_Reset, do the soft reset procedure */
  223. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_SOFT_RESET
  224. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  225. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  226. cfg->pe._message = NULL;
  227. return PESinkSoftReset;
  228. /* If the message was Wait or Reject */
  229. } else if ((PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_REJECT
  230. || PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_WAIT)
  231. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  232. /* If we don't have an explicit contract, wait for capabilities */
  233. if (!cfg->pe._explicit_contract) {
  234. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  235. cfg->pe._message = NULL;
  236. return PESinkWaitCap;
  237. /* If we do have an explicit contract, go to the ready state */
  238. } else {
  239. /* If we got here from a Wait message, we Should run
  240. * SinkRequestTimer in the Ready state. */
  241. cfg->pe._min_power = (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_WAIT);
  242. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  243. cfg->pe._message = NULL;
  244. return PESinkReady;
  245. }
  246. } else {
  247. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  248. cfg->pe._message = NULL;
  249. return PESinkSendSoftReset;
  250. }
  251. }
  252. return PESinkHardReset;
  253. }
  254. static enum policy_engine_state pe_sink_transition_sink(struct pdb_config *cfg)
  255. {
  256. /* Wait for the PS_RDY message */
  257. eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
  258. PD_T_PS_TRANSITION);
  259. /* If we got reset signaling, transition to default */
  260. if (evt & PDB_EVT_PE_RESET) {
  261. return PESinkTransitionDefault;
  262. }
  263. /* If no message was received, send a hard reset */
  264. if (evt == 0) {
  265. return PESinkHardReset;
  266. }
  267. /* If we received a message, read it */
  268. if (chMBFetch(&cfg->pe.mailbox, (msg_t *) &cfg->pe._message, TIME_IMMEDIATE) == MSG_OK) {
  269. /* If we got a PS_RDY, handle it */
  270. if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_PS_RDY
  271. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  272. /* We just finished negotiating an explicit contract */
  273. cfg->pe._explicit_contract = true;
  274. /* Set the output appropriately */
  275. if (!cfg->pe._min_power) {
  276. cfg->dpm.transition_requested(cfg);
  277. }
  278. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  279. cfg->pe._message = NULL;
  280. return PESinkReady;
  281. /* If there was a protocol error, send a hard reset */
  282. } else {
  283. /* Turn off the power output before this hard reset to make sure we
  284. * don't supply an incorrect voltage to the device we're powering.
  285. */
  286. cfg->dpm.transition_default(cfg);
  287. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  288. cfg->pe._message = NULL;
  289. return PESinkHardReset;
  290. }
  291. }
  292. return PESinkHardReset;
  293. }
  294. static enum policy_engine_state pe_sink_ready(struct pdb_config *cfg)
  295. {
  296. eventmask_t evt;
  297. /* Wait for an event */
  298. if (cfg->pe._min_power) {
  299. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET
  300. | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_GET_SOURCE_CAP
  301. | PDB_EVT_PE_NEW_POWER | PDB_EVT_PE_PPS_REQUEST,
  302. PD_T_SINK_REQUEST);
  303. } else {
  304. evt = chEvtWaitAny(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET
  305. | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_GET_SOURCE_CAP
  306. | PDB_EVT_PE_NEW_POWER | PDB_EVT_PE_PPS_REQUEST);
  307. }
  308. /* If we got reset signaling, transition to default */
  309. if (evt & PDB_EVT_PE_RESET) {
  310. return PESinkTransitionDefault;
  311. }
  312. /* If we overheated, send a hard reset */
  313. if (evt & PDB_EVT_PE_I_OVRTEMP) {
  314. return PESinkHardReset;
  315. }
  316. /* If the DPM wants us to, send a Get_Source_Cap message */
  317. if (evt & PDB_EVT_PE_GET_SOURCE_CAP) {
  318. /* Tell the protocol layer we're starting an AMS */
  319. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_START_AMS);
  320. return PESinkGetSourceCap;
  321. }
  322. /* If the DPM wants new power, let it figure out what power it wants
  323. * exactly. This isn't exactly the transition from the spec (that would be
  324. * SelectCap, not EvalCap), but this works better with the particular
  325. * design of this firmware. */
  326. if (evt & PDB_EVT_PE_NEW_POWER) {
  327. /* Make sure we're evaluating NULL capabilities to use the old ones */
  328. if (cfg->pe._message != NULL) {
  329. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  330. cfg->pe._message = NULL;
  331. }
  332. /* Tell the protocol layer we're starting an AMS */
  333. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_START_AMS);
  334. return PESinkEvalCap;
  335. }
  336. /* If SinkPPSPeriodicTimer ran out, send a new request */
  337. if (evt & PDB_EVT_PE_PPS_REQUEST) {
  338. /* Tell the protocol layer we're starting an AMS */
  339. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_START_AMS);
  340. return PESinkSelectCap;
  341. }
  342. /* If no event was received, the timer ran out. */
  343. if (evt == 0) {
  344. /* Repeat our Request message */
  345. return PESinkSelectCap;
  346. }
  347. /* If we received a message */
  348. if (evt & PDB_EVT_PE_MSG_RX) {
  349. if (chMBFetch(&cfg->pe.mailbox, (msg_t *) &cfg->pe._message, TIME_IMMEDIATE) == MSG_OK) {
  350. /* Ignore vendor-defined messages */
  351. if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_VENDOR_DEFINED
  352. && PD_NUMOBJ_GET(cfg->pe._message) > 0) {
  353. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  354. cfg->pe._message = NULL;
  355. return PESinkReady;
  356. /* Ignore Ping messages */
  357. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_PING
  358. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  359. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  360. cfg->pe._message = NULL;
  361. return PESinkReady;
  362. /* DR_Swap messages are not supported */
  363. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_DR_SWAP
  364. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  365. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  366. cfg->pe._message = NULL;
  367. return PESinkSendNotSupported;
  368. /* Get_Source_Cap messages are not supported */
  369. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_GET_SOURCE_CAP
  370. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  371. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  372. cfg->pe._message = NULL;
  373. return PESinkSendNotSupported;
  374. /* PR_Swap messages are not supported */
  375. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_PR_SWAP
  376. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  377. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  378. cfg->pe._message = NULL;
  379. return PESinkSendNotSupported;
  380. /* VCONN_Swap messages are not supported */
  381. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_VCONN_SWAP
  382. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  383. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  384. cfg->pe._message = NULL;
  385. return PESinkSendNotSupported;
  386. /* Request messages are not supported */
  387. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_REQUEST
  388. && PD_NUMOBJ_GET(cfg->pe._message) > 0) {
  389. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  390. cfg->pe._message = NULL;
  391. return PESinkSendNotSupported;
  392. /* Sink_Capabilities messages are not supported */
  393. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_SINK_CAPABILITIES
  394. && PD_NUMOBJ_GET(cfg->pe._message) > 0) {
  395. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  396. cfg->pe._message = NULL;
  397. return PESinkSendNotSupported;
  398. /* Handle GotoMin messages */
  399. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_GOTOMIN
  400. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  401. if (cfg->dpm.giveback_enabled != NULL
  402. && cfg->dpm.giveback_enabled(cfg)) {
  403. /* Transition to the minimum current level */
  404. cfg->dpm.transition_min(cfg);
  405. cfg->pe._min_power = true;
  406. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  407. cfg->pe._message = NULL;
  408. return PESinkTransitionSink;
  409. } else {
  410. /* GiveBack is not supported */
  411. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  412. cfg->pe._message = NULL;
  413. return PESinkSendNotSupported;
  414. }
  415. /* Evaluate new Source_Capabilities */
  416. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_SOURCE_CAPABILITIES
  417. && PD_NUMOBJ_GET(cfg->pe._message) > 0) {
  418. /* Don't free the message: we need to keep the
  419. * Source_Capabilities message so we can evaluate it. */
  420. return PESinkEvalCap;
  421. /* Give sink capabilities when asked */
  422. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_GET_SINK_CAP
  423. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  424. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  425. cfg->pe._message = NULL;
  426. return PESinkGiveSinkCap;
  427. /* If the message was a Soft_Reset, do the soft reset procedure */
  428. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_SOFT_RESET
  429. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  430. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  431. cfg->pe._message = NULL;
  432. return PESinkSoftReset;
  433. /* PD 3.0 messges */
  434. } else if ((cfg->pe.hdr_template & PD_HDR_SPECREV) == PD_SPECREV_3_0) {
  435. /* If the message is a multi-chunk extended message, let it
  436. * time out. */
  437. if ((cfg->pe._message->hdr & PD_HDR_EXT)
  438. && (PD_DATA_SIZE_GET(cfg->pe._message) > PD_MAX_EXT_MSG_LEGACY_LEN)) {
  439. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  440. cfg->pe._message = NULL;
  441. return PESinkChunkReceived;
  442. /* Tell the DPM a message we sent got a response of
  443. * Not_Supported. */
  444. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_NOT_SUPPORTED
  445. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  446. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  447. cfg->pe._message = NULL;
  448. return PESinkNotSupportedReceived;
  449. /* If we got an unknown message, send a soft reset */
  450. } else {
  451. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  452. cfg->pe._message = NULL;
  453. return PESinkSendSoftReset;
  454. }
  455. /* If we got an unknown message, send a soft reset
  456. *
  457. * XXX I don't like that this is duplicated. */
  458. } else {
  459. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  460. cfg->pe._message = NULL;
  461. return PESinkSendSoftReset;
  462. }
  463. }
  464. }
  465. return PESinkReady;
  466. }
  467. static enum policy_engine_state pe_sink_get_source_cap(struct pdb_config *cfg)
  468. {
  469. /* Get a message object */
  470. union pd_msg *get_source_cap = chPoolAlloc(&pdb_msg_pool);
  471. /* Make a Get_Source_Cap message */
  472. get_source_cap->hdr = cfg->pe.hdr_template | PD_MSGTYPE_GET_SOURCE_CAP
  473. | PD_NUMOBJ(0);
  474. /* Transmit the Get_Source_Cap */
  475. chMBPost(&cfg->prl.tx_mailbox, (msg_t) get_source_cap, TIME_IMMEDIATE);
  476. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_MSG_TX);
  477. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  478. | PDB_EVT_PE_RESET);
  479. /* Free the sent message */
  480. chPoolFree(&pdb_msg_pool, get_source_cap);
  481. get_source_cap = NULL;
  482. /* If we got reset signaling, transition to default */
  483. if (evt & PDB_EVT_PE_RESET) {
  484. return PESinkTransitionDefault;
  485. }
  486. /* If the message transmission failed, send a hard reset */
  487. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  488. return PESinkHardReset;
  489. }
  490. return PESinkReady;
  491. }
  492. static enum policy_engine_state pe_sink_give_sink_cap(struct pdb_config *cfg)
  493. {
  494. /* Get a message object */
  495. union pd_msg *snk_cap = chPoolAlloc(&pdb_msg_pool);
  496. /* Get our capabilities from the DPM */
  497. cfg->dpm.get_sink_capability(cfg, snk_cap);
  498. /* Transmit our capabilities */
  499. chMBPost(&cfg->prl.tx_mailbox, (msg_t) snk_cap, TIME_IMMEDIATE);
  500. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_MSG_TX);
  501. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  502. | PDB_EVT_PE_RESET);
  503. /* Free the Sink_Capabilities message */
  504. chPoolFree(&pdb_msg_pool, snk_cap);
  505. snk_cap = NULL;
  506. /* If we got reset signaling, transition to default */
  507. if (evt & PDB_EVT_PE_RESET) {
  508. return PESinkTransitionDefault;
  509. }
  510. /* If the message transmission failed, send a hard reset */
  511. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  512. return PESinkHardReset;
  513. }
  514. return PESinkReady;
  515. }
  516. static enum policy_engine_state pe_sink_hard_reset(struct pdb_config *cfg)
  517. {
  518. /* If we've already sent the maximum number of hard resets, assume the
  519. * source is unresponsive. */
  520. if (cfg->pe._hard_reset_counter > PD_N_HARD_RESET_COUNT) {
  521. return PESinkSourceUnresponsive;
  522. }
  523. /* Generate a hard reset signal */
  524. chEvtSignal(cfg->prl.hardrst_thread, PDB_EVT_HARDRST_RESET);
  525. chEvtWaitAny(PDB_EVT_PE_HARD_SENT);
  526. /* Increment HardResetCounter */
  527. cfg->pe._hard_reset_counter++;
  528. return PESinkTransitionDefault;
  529. }
  530. static enum policy_engine_state pe_sink_transition_default(struct pdb_config *cfg)
  531. {
  532. cfg->pe._explicit_contract = false;
  533. /* Tell the DPM to transition to default power */
  534. cfg->dpm.transition_default(cfg);
  535. /* There is no local hardware to reset. */
  536. /* Since we never change our data role from UFP, there is no reason to set
  537. * it here. */
  538. /* Tell the protocol layer we're done with the reset */
  539. chEvtSignal(cfg->prl.hardrst_thread, PDB_EVT_HARDRST_DONE);
  540. return PESinkStartup;
  541. }
  542. static enum policy_engine_state pe_sink_soft_reset(struct pdb_config *cfg)
  543. {
  544. /* No need to explicitly reset the protocol layer here. It resets itself
  545. * when a Soft_Reset message is received. */
  546. /* Get a message object */
  547. union pd_msg *accept = chPoolAlloc(&pdb_msg_pool);
  548. /* Make an Accept message */
  549. accept->hdr = cfg->pe.hdr_template | PD_MSGTYPE_ACCEPT | PD_NUMOBJ(0);
  550. /* Transmit the Accept */
  551. chMBPost(&cfg->prl.tx_mailbox, (msg_t) accept, TIME_IMMEDIATE);
  552. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_MSG_TX);
  553. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  554. | PDB_EVT_PE_RESET);
  555. /* Free the sent message */
  556. chPoolFree(&pdb_msg_pool, accept);
  557. accept = NULL;
  558. /* If we got reset signaling, transition to default */
  559. if (evt & PDB_EVT_PE_RESET) {
  560. return PESinkTransitionDefault;
  561. }
  562. /* If the message transmission failed, send a hard reset */
  563. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  564. return PESinkHardReset;
  565. }
  566. return PESinkWaitCap;
  567. }
  568. static enum policy_engine_state pe_sink_send_soft_reset(struct pdb_config *cfg)
  569. {
  570. /* No need to explicitly reset the protocol layer here. It resets itself
  571. * just before a Soft_Reset message is transmitted. */
  572. /* Get a message object */
  573. union pd_msg *softrst = chPoolAlloc(&pdb_msg_pool);
  574. /* Make a Soft_Reset message */
  575. softrst->hdr = cfg->pe.hdr_template | PD_MSGTYPE_SOFT_RESET | PD_NUMOBJ(0);
  576. /* Transmit the soft reset */
  577. chMBPost(&cfg->prl.tx_mailbox, (msg_t) softrst, TIME_IMMEDIATE);
  578. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_MSG_TX);
  579. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  580. | PDB_EVT_PE_RESET);
  581. /* Free the sent message */
  582. chPoolFree(&pdb_msg_pool, softrst);
  583. softrst = NULL;
  584. /* If we got reset signaling, transition to default */
  585. if (evt & PDB_EVT_PE_RESET) {
  586. return PESinkTransitionDefault;
  587. }
  588. /* If the message transmission failed, send a hard reset */
  589. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  590. return PESinkHardReset;
  591. }
  592. /* Wait for a response */
  593. evt = chEvtWaitAnyTimeout(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
  594. PD_T_SENDER_RESPONSE);
  595. /* If we got reset signaling, transition to default */
  596. if (evt & PDB_EVT_PE_RESET) {
  597. return PESinkTransitionDefault;
  598. }
  599. /* If we didn't get a response before the timeout, send a hard reset */
  600. if (evt == 0) {
  601. return PESinkHardReset;
  602. }
  603. /* Get the response message */
  604. if (chMBFetch(&cfg->pe.mailbox, (msg_t *) &cfg->pe._message, TIME_IMMEDIATE) == MSG_OK) {
  605. /* If the source accepted our soft reset, wait for capabilities. */
  606. if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_ACCEPT
  607. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  608. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  609. cfg->pe._message = NULL;
  610. return PESinkWaitCap;
  611. /* If the message was a Soft_Reset, do the soft reset procedure */
  612. } else if (PD_MSGTYPE_GET(cfg->pe._message) == PD_MSGTYPE_SOFT_RESET
  613. && PD_NUMOBJ_GET(cfg->pe._message) == 0) {
  614. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  615. cfg->pe._message = NULL;
  616. return PESinkSoftReset;
  617. /* Otherwise, send a hard reset */
  618. } else {
  619. chPoolFree(&pdb_msg_pool, cfg->pe._message);
  620. cfg->pe._message = NULL;
  621. return PESinkHardReset;
  622. }
  623. }
  624. return PESinkHardReset;
  625. }
  626. static enum policy_engine_state pe_sink_send_not_supported(struct pdb_config *cfg)
  627. {
  628. /* Get a message object */
  629. union pd_msg *not_supported = chPoolAlloc(&pdb_msg_pool);
  630. if ((cfg->pe.hdr_template & PD_HDR_SPECREV) == PD_SPECREV_2_0) {
  631. /* Make a Reject message */
  632. not_supported->hdr = cfg->pe.hdr_template | PD_MSGTYPE_REJECT | PD_NUMOBJ(0);
  633. } else if ((cfg->pe.hdr_template & PD_HDR_SPECREV) == PD_SPECREV_3_0) {
  634. /* Make a Not_Supported message */
  635. not_supported->hdr = cfg->pe.hdr_template | PD_MSGTYPE_NOT_SUPPORTED | PD_NUMOBJ(0);
  636. }
  637. /* Transmit the message */
  638. chMBPost(&cfg->prl.tx_mailbox, (msg_t) not_supported, TIME_IMMEDIATE);
  639. chEvtSignal(cfg->prl.tx_thread, PDB_EVT_PRLTX_MSG_TX);
  640. eventmask_t evt = chEvtWaitAny(PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR
  641. | PDB_EVT_PE_RESET);
  642. /* Free the message */
  643. chPoolFree(&pdb_msg_pool, not_supported);
  644. not_supported = NULL;
  645. /* If we got reset signaling, transition to default */
  646. if (evt & PDB_EVT_PE_RESET) {
  647. return PESinkTransitionDefault;
  648. }
  649. /* If the message transmission failed, send a soft reset */
  650. if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
  651. return PESinkSendSoftReset;
  652. }
  653. return PESinkReady;
  654. }
  655. static enum policy_engine_state pe_sink_chunk_received(struct pdb_config *cfg)
  656. {
  657. (void) cfg;
  658. /* Wait for tChunkingNotSupported */
  659. eventmask_t evt = chEvtWaitAnyTimeout(PDB_EVT_PE_RESET,
  660. PD_T_CHUNKING_NOT_SUPPORTED);
  661. /* If we got reset signaling, transition to default */
  662. if (evt & PDB_EVT_PE_RESET) {
  663. return PESinkTransitionDefault;
  664. }
  665. return PESinkSendNotSupported;
  666. }
  667. static enum policy_engine_state pe_sink_not_supported_received(struct pdb_config *cfg)
  668. {
  669. /* Inform the Device Policy Manager that we received a Not_Supported
  670. * message. */
  671. if (cfg->dpm.not_supported_received != NULL) {
  672. cfg->dpm.not_supported_received(cfg);
  673. }
  674. return PESinkReady;
  675. }
  676. /*
  677. * When Power Delivery is unresponsive, fall back to Type-C Current
  678. */
  679. static enum policy_engine_state pe_sink_source_unresponsive(struct pdb_config *cfg)
  680. {
  681. /* If the DPM can evaluate the Type-C Current advertisement */
  682. if (cfg->dpm.evaluate_typec_current != NULL) {
  683. /* Make the DPM evaluate the Type-C Current advertisement */
  684. int tcc_match = cfg->dpm.evaluate_typec_current(cfg,
  685. fusb_get_typec_current(&cfg->fusb));
  686. /* If the last two readings are the same, set the output */
  687. if (cfg->pe._old_tcc_match == tcc_match) {
  688. cfg->dpm.transition_typec(cfg);
  689. }
  690. /* Remember whether or not the last measurement succeeded */
  691. cfg->pe._old_tcc_match = tcc_match;
  692. }
  693. /* Wait tPDDebounce between measurements */
  694. chThdSleep(PD_T_PD_DEBOUNCE);
  695. return PESinkSourceUnresponsive;
  696. }
  697. /*
  698. * Policy Engine state machine thread
  699. */
  700. static THD_FUNCTION(PolicyEngine, vcfg) {
  701. struct pdb_config *cfg = vcfg;
  702. enum policy_engine_state state = PESinkStartup;
  703. /* Initialize the mailbox */
  704. chMBObjectInit(&cfg->pe.mailbox, cfg->pe._mailbox_queue, PDB_MSG_POOL_SIZE);
  705. /* Initialize the VT for SinkPPSPeriodicTimer */
  706. chVTObjectInit(&cfg->pe._sink_pps_periodic_timer);
  707. /* Initialize the old_tcc_match */
  708. cfg->pe._old_tcc_match = -1;
  709. /* Initialize the pps_index */
  710. cfg->pe._pps_index = 8;
  711. /* Initialize the last_pps */
  712. cfg->pe._last_pps = 8;
  713. /* Initialize the PD message header template */
  714. cfg->pe.hdr_template = PD_DATAROLE_UFP | PD_POWERROLE_SINK;
  715. while (true) {
  716. switch (state) {
  717. case PESinkStartup:
  718. state = pe_sink_startup(cfg);
  719. break;
  720. case PESinkDiscovery:
  721. state = pe_sink_discovery(cfg);
  722. break;
  723. case PESinkWaitCap:
  724. state = pe_sink_wait_cap(cfg);
  725. break;
  726. case PESinkEvalCap:
  727. state = pe_sink_eval_cap(cfg);
  728. break;
  729. case PESinkSelectCap:
  730. state = pe_sink_select_cap(cfg);
  731. break;
  732. case PESinkTransitionSink:
  733. state = pe_sink_transition_sink(cfg);
  734. break;
  735. case PESinkReady:
  736. state = pe_sink_ready(cfg);
  737. break;
  738. case PESinkGetSourceCap:
  739. state = pe_sink_get_source_cap(cfg);
  740. break;
  741. case PESinkGiveSinkCap:
  742. state = pe_sink_give_sink_cap(cfg);
  743. break;
  744. case PESinkHardReset:
  745. state = pe_sink_hard_reset(cfg);
  746. break;
  747. case PESinkTransitionDefault:
  748. state = pe_sink_transition_default(cfg);
  749. break;
  750. case PESinkSoftReset:
  751. state = pe_sink_soft_reset(cfg);
  752. break;
  753. case PESinkSendSoftReset:
  754. state = pe_sink_send_soft_reset(cfg);
  755. break;
  756. case PESinkSendNotSupported:
  757. state = pe_sink_send_not_supported(cfg);
  758. break;
  759. case PESinkChunkReceived:
  760. state = pe_sink_chunk_received(cfg);
  761. break;
  762. case PESinkSourceUnresponsive:
  763. state = pe_sink_source_unresponsive(cfg);
  764. break;
  765. case PESinkNotSupportedReceived:
  766. state = pe_sink_not_supported_received(cfg);
  767. break;
  768. default:
  769. /* This is an error. It really shouldn't happen. We might
  770. * want to handle it anyway, though. */
  771. state = PESinkStartup;
  772. break;
  773. }
  774. }
  775. }
  776. void pdb_pe_run(struct pdb_config *cfg)
  777. {
  778. cfg->pe.thread = chThdCreateStatic(cfg->pe._wa, sizeof(cfg->pe._wa),
  779. PDB_PRIO_PE, PolicyEngine, cfg);
  780. }