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

device_policy_manager.c 16KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447
  1. /*
  2. * PD Buddy - USB Power Delivery for everyone
  3. * Copyright (C) 2017-2018 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 "device_policy_manager.h"
  19. #include <stdint.h>
  20. #include <hal.h>
  21. #include <pd.h>
  22. #include "led.h"
  23. #include "config.h"
  24. /* The current draw when the output is disabled */
  25. #define DPM_MIN_CURRENT PD_MA2PDI(100)
  26. /*
  27. * Return the current specified by the given PDBS configuration object at the
  28. * given voltage (in millivolts), in centiamperes.
  29. */
  30. static uint16_t dpm_get_current(struct pdbs_config *scfg, uint16_t mv)
  31. {
  32. switch (scfg->flags & PDBS_CONFIG_FLAGS_CURRENT_DEFN) {
  33. case PDBS_CONFIG_FLAGS_CURRENT_DEFN_I:
  34. return scfg->i;
  35. case PDBS_CONFIG_FLAGS_CURRENT_DEFN_P:
  36. return (scfg->p * 1000 + mv - 1) / mv;
  37. case PDBS_CONFIG_FLAGS_CURRENT_DEFN_R:
  38. return (mv * 10 + scfg->r - 1) / scfg->r;
  39. default:
  40. /* In the case of an error, return a current that can't be
  41. * requested. */
  42. return PD_CA_MAX + 1;
  43. }
  44. }
  45. /*
  46. * Find the index of the first PDO from capabilities in the voltage range,
  47. * using the desired order.
  48. *
  49. * If there is no such PDO, returns -1 instead.
  50. */
  51. static int8_t dpm_get_range_fixed_pdo_index(const union pd_msg *caps,
  52. struct pdbs_config *scfg)
  53. {
  54. /* Get the number of PDOs */
  55. uint8_t numobj = PD_NUMOBJ_GET(caps);
  56. /* Get ready to iterate over the PDOs */
  57. int8_t i;
  58. int8_t step;
  59. if (scfg->flags & PDBS_CONFIG_FLAGS_HV_PREFERRED) {
  60. i = numobj - 1;
  61. step = -1;
  62. } else {
  63. i = 0;
  64. step = 1;
  65. }
  66. /* Look at the PDOs to see if one falls in our voltage range. */
  67. while (0 <= i && i < numobj) {
  68. /* If we have a fixed PDO, its V is within our range, and its I is at
  69. * least our desired I */
  70. uint16_t v = PD_PDO_SRC_FIXED_VOLTAGE_GET(caps->obj[i]);
  71. if ((caps->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_FIXED
  72. && PD_PDO_SRC_FIXED_CURRENT_GET(caps->obj[i]) >= dpm_get_current(scfg, PD_PDV2MV(v))
  73. && v >= PD_MV2PDV(scfg->vmin)
  74. && v <= PD_MV2PDV(scfg->vmax)) {
  75. return i;
  76. }
  77. i += step;
  78. }
  79. return -1;
  80. }
  81. bool pdbs_dpm_evaluate_capability(struct pdb_config *cfg,
  82. const union pd_msg *caps, union pd_msg *request)
  83. {
  84. /* Cast the dpm_data to the right type */
  85. struct pdbs_dpm_data *dpm_data = cfg->dpm_data;
  86. /* Update the stored Source_Capabilities */
  87. if (caps != NULL) {
  88. if (dpm_data->capabilities != NULL) {
  89. chPoolFree(&pdb_msg_pool, (union pd_msg *) dpm_data->capabilities);
  90. }
  91. dpm_data->capabilities = caps;
  92. } else {
  93. /* No new capabilities; use a shorter name for the stored ones. */
  94. caps = dpm_data->capabilities;
  95. }
  96. /* Get the current configuration */
  97. struct pdbs_config *scfg = pdbs_config_flash_read();
  98. /* Get the number of PDOs */
  99. uint8_t numobj = PD_NUMOBJ_GET(caps);
  100. /* Make the LED blink to indicate ongoing power negotiations */
  101. if (dpm_data->led_pd_status) {
  102. chEvtSignal(pdbs_led_thread, PDBS_EVT_LED_NEGOTIATING);
  103. }
  104. /* Get whether or not the power supply is constrained */
  105. dpm_data->_unconstrained_power = caps->obj[0] & PD_PDO_SRC_FIXED_UNCONSTRAINED;
  106. /* Get the current we want */
  107. uint16_t current = dpm_get_current(scfg, scfg->v);
  108. /* Make sure we have configuration */
  109. if (scfg != NULL && dpm_data->output_enabled) {
  110. /* Look at the PDOs to see if one matches our desires */
  111. for (uint8_t i = 0; i < numobj; i++) {
  112. /* If we have a fixed PDO, its V equals our desired V, and its I is
  113. * at least our desired I */
  114. if ((caps->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_FIXED
  115. && PD_PDO_SRC_FIXED_VOLTAGE_GET(caps->obj[i]) == PD_MV2PDV(scfg->v)
  116. && PD_PDO_SRC_FIXED_CURRENT_GET(caps->obj[i]) >= current) {
  117. /* We got what we wanted, so build a request for that */
  118. request->hdr = cfg->pe.hdr_template | PD_MSGTYPE_REQUEST
  119. | PD_NUMOBJ(1);
  120. if (scfg->flags & PDBS_CONFIG_FLAGS_GIVEBACK) {
  121. /* GiveBack enabled */
  122. request->obj[0] = PD_RDO_FV_MIN_CURRENT_SET(DPM_MIN_CURRENT)
  123. | PD_RDO_FV_CURRENT_SET(current)
  124. | PD_RDO_NO_USB_SUSPEND | PD_RDO_GIVEBACK
  125. | PD_RDO_OBJPOS_SET(i + 1);
  126. } else {
  127. /* GiveBack disabled */
  128. request->obj[0] = PD_RDO_FV_MAX_CURRENT_SET(current)
  129. | PD_RDO_FV_CURRENT_SET(current)
  130. | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(i + 1);
  131. }
  132. if (dpm_data->usb_comms) {
  133. request->obj[0] |= PD_RDO_USB_COMMS;
  134. }
  135. /* Update requested voltage */
  136. dpm_data->_requested_voltage = PD_PDV2MV(PD_MV2PDV(scfg->v));
  137. dpm_data->_capability_match = true;
  138. return true;
  139. }
  140. /* If we have a PPS APDO, our desired V lies within its range, and
  141. * its I is at least our desired I */
  142. if ((caps->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_AUGMENTED
  143. && (caps->obj[i] & PD_APDO_TYPE) == PD_APDO_TYPE_PPS
  144. && PD_APDO_PPS_MAX_VOLTAGE_GET(caps->obj[i]) >= PD_MV2PAV(scfg->v)
  145. && PD_APDO_PPS_MIN_VOLTAGE_GET(caps->obj[i]) <= PD_MV2PAV(scfg->v)
  146. && PD_APDO_PPS_CURRENT_GET(caps->obj[i]) >= PD_CA2PAI(current)) {
  147. /* We got what we wanted, so build a request for that */
  148. request->hdr = cfg->pe.hdr_template | PD_MSGTYPE_REQUEST
  149. | PD_NUMOBJ(1);
  150. /* Build a request */
  151. request->obj[0] = PD_RDO_PROG_CURRENT_SET(PD_CA2PAI(current))
  152. | PD_RDO_PROG_VOLTAGE_SET(PD_MV2PRV(scfg->v))
  153. | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(i + 1);
  154. if (dpm_data->usb_comms) {
  155. request->obj[0] |= PD_RDO_USB_COMMS;
  156. }
  157. /* Update requested voltage */
  158. dpm_data->_requested_voltage = PD_PRV2MV(PD_MV2PRV(scfg->v));
  159. dpm_data->_capability_match = true;
  160. return true;
  161. }
  162. }
  163. /* If there's a PDO in the voltage range, use it */
  164. int8_t i = dpm_get_range_fixed_pdo_index(caps, scfg);
  165. if (i >= 0) {
  166. /* We got what we wanted, so build a request for that */
  167. request->hdr = cfg->pe.hdr_template | PD_MSGTYPE_REQUEST
  168. | PD_NUMOBJ(1);
  169. /* Get the current we need at this voltage */
  170. current = dpm_get_current(scfg, PD_PDV2MV(PD_PDO_SRC_FIXED_VOLTAGE_GET(caps->obj[i])));
  171. if (scfg->flags & PDBS_CONFIG_FLAGS_GIVEBACK) {
  172. /* GiveBack enabled */
  173. request->obj[0] = PD_RDO_FV_MIN_CURRENT_SET(DPM_MIN_CURRENT)
  174. | PD_RDO_FV_CURRENT_SET(current)
  175. | PD_RDO_NO_USB_SUSPEND | PD_RDO_GIVEBACK
  176. | PD_RDO_OBJPOS_SET(i + 1);
  177. } else {
  178. /* GiveBack disabled */
  179. request->obj[0] = PD_RDO_FV_MAX_CURRENT_SET(current)
  180. | PD_RDO_FV_CURRENT_SET(current)
  181. | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(i + 1);
  182. }
  183. if (dpm_data->usb_comms) {
  184. request->obj[0] |= PD_RDO_USB_COMMS;
  185. }
  186. /* Update requested voltage */
  187. dpm_data->_requested_voltage = PD_PDV2MV(PD_PDO_SRC_FIXED_VOLTAGE_GET(caps->obj[i]));
  188. dpm_data->_capability_match = true;
  189. return true;
  190. }
  191. }
  192. /* Nothing matched (or no configuration), so get 5 V at low current */
  193. request->hdr = cfg->pe.hdr_template | PD_MSGTYPE_REQUEST | PD_NUMOBJ(1);
  194. request->obj[0] = PD_RDO_FV_MAX_CURRENT_SET(DPM_MIN_CURRENT)
  195. | PD_RDO_FV_CURRENT_SET(DPM_MIN_CURRENT)
  196. | PD_RDO_NO_USB_SUSPEND
  197. | PD_RDO_OBJPOS_SET(1);
  198. /* If the output is enabled and we got here, it must be a capability
  199. * mismatch. */
  200. if (dpm_data->output_enabled) {
  201. request->obj[0] |= PD_RDO_CAP_MISMATCH;
  202. }
  203. /* If we can do USB communications, tell the power supply */
  204. if (dpm_data->usb_comms) {
  205. request->obj[0] |= PD_RDO_USB_COMMS;
  206. }
  207. /* Update requested voltage */
  208. dpm_data->_requested_voltage = 5000;
  209. /* At this point, we have a capability match iff the output is disabled */
  210. dpm_data->_capability_match = !dpm_data->output_enabled;
  211. return !dpm_data->output_enabled;
  212. }
  213. void pdbs_dpm_get_sink_capability(struct pdb_config *cfg, union pd_msg *cap)
  214. {
  215. /* Keep track of how many PDOs we've added */
  216. int numobj = 0;
  217. /* Get the current configuration */
  218. struct pdbs_config *scfg = pdbs_config_flash_read();
  219. /* Cast the dpm_data to the right type */
  220. struct pdbs_dpm_data *dpm_data = cfg->dpm_data;
  221. /* If we have no configuration or want something other than 5 V, add a PDO
  222. * for vSafe5V */
  223. if (scfg == NULL || PD_MV2PDV(scfg->v) != PD_MV2PDV(5000)) {
  224. /* Minimum current, 5 V, and higher capability. */
  225. cap->obj[numobj++] = PD_PDO_TYPE_FIXED
  226. | PD_PDO_SNK_FIXED_VOLTAGE_SET(PD_MV2PDV(5000))
  227. | PD_PDO_SNK_FIXED_CURRENT_SET(DPM_MIN_CURRENT);
  228. }
  229. if (scfg != NULL) {
  230. /* Get the current we want */
  231. uint16_t current = dpm_get_current(scfg, scfg->v);
  232. /* Add a PDO for the desired power. */
  233. cap->obj[numobj++] = PD_PDO_TYPE_FIXED
  234. | PD_PDO_SNK_FIXED_VOLTAGE_SET(PD_MV2PDV(scfg->v))
  235. | PD_PDO_SNK_FIXED_CURRENT_SET(current);
  236. /* Get the PDO from the voltage range */
  237. int8_t i = dpm_get_range_fixed_pdo_index(dpm_data->capabilities, scfg);
  238. /* If it's vSafe5V, set our vSafe5V's current to what we want */
  239. if (i == 0) {
  240. cap->obj[0] &= ~PD_PDO_SNK_FIXED_CURRENT;
  241. cap->obj[0] |= PD_PDO_SNK_FIXED_CURRENT_SET(current);
  242. } else {
  243. /* If we want more than 5 V, set the Higher Capability flag */
  244. if (PD_MV2PDV(scfg->v) != PD_MV2PDV(5000)) {
  245. cap->obj[0] |= PD_PDO_SNK_FIXED_HIGHER_CAP;
  246. }
  247. /* If the range PDO is a different voltage than the preferred
  248. * voltage, add it to the array. */
  249. if (i > 0 && PD_PDO_SRC_FIXED_VOLTAGE_GET(dpm_data->capabilities->obj[i]) != PD_MV2PDV(scfg->v)) {
  250. cap->obj[numobj++] = PD_PDO_TYPE_FIXED
  251. | PD_PDO_SNK_FIXED_VOLTAGE_SET(PD_PDO_SRC_FIXED_VOLTAGE_GET(dpm_data->capabilities->obj[i]))
  252. | PD_PDO_SNK_FIXED_CURRENT_SET(PD_PDO_SRC_FIXED_CURRENT_GET(dpm_data->capabilities->obj[i]));
  253. }
  254. /* If we have three PDOs at this point, make sure the last two are
  255. * sorted by voltage. */
  256. if (numobj == 3
  257. && (cap->obj[1] & PD_PDO_SNK_FIXED_VOLTAGE)
  258. > (cap->obj[2] & PD_PDO_SNK_FIXED_VOLTAGE)) {
  259. cap->obj[1] ^= cap->obj[2];
  260. cap->obj[2] ^= cap->obj[1];
  261. cap->obj[1] ^= cap->obj[2];
  262. }
  263. }
  264. /* If we're using PD 3.0, add a PPS APDO for our desired voltage */
  265. if ((cfg->pe._message->hdr & PD_HDR_SPECREV) >= PD_SPECREV_3_0) {
  266. cap->obj[numobj++] = PD_PDO_TYPE_AUGMENTED | PD_APDO_TYPE_PPS
  267. | PD_APDO_PPS_MAX_VOLTAGE_SET(PD_MV2PAV(scfg->v))
  268. | PD_APDO_PPS_MIN_VOLTAGE_SET(PD_MV2PAV(scfg->v))
  269. | PD_APDO_PPS_CURRENT_SET(PD_CA2PAI(current));
  270. }
  271. }
  272. /* Set the unconstrained power flag. */
  273. if (dpm_data->_unconstrained_power) {
  274. cap->obj[0] |= PD_PDO_SNK_FIXED_UNCONSTRAINED;
  275. }
  276. /* Set the USB communications capable flag. */
  277. if (dpm_data->usb_comms) {
  278. cap->obj[0] |= PD_PDO_SNK_FIXED_USB_COMMS;
  279. }
  280. /* Set the Sink_Capabilities message header */
  281. cap->hdr = cfg->pe.hdr_template | PD_MSGTYPE_SINK_CAPABILITIES
  282. | PD_NUMOBJ(numobj);
  283. }
  284. bool pdbs_dpm_giveback_enabled(struct pdb_config *cfg)
  285. {
  286. (void) cfg;
  287. struct pdbs_config *scfg = pdbs_config_flash_read();
  288. return scfg->flags & PDBS_CONFIG_FLAGS_GIVEBACK;
  289. }
  290. bool pdbs_dpm_evaluate_typec_current(struct pdb_config *cfg,
  291. enum fusb_typec_current tcc)
  292. {
  293. struct pdbs_config *scfg = pdbs_config_flash_read();
  294. /* Cast the dpm_data to the right type */
  295. struct pdbs_dpm_data *dpm_data = cfg->dpm_data;
  296. /* We don't control the voltage anymore; it will always be 5 V. */
  297. dpm_data->_requested_voltage = 5000;
  298. /* Make the present Type-C Current advertisement available to the rest of
  299. * the DPM */
  300. dpm_data->typec_current = tcc;
  301. /* If we have no configuration or don't want 5 V, Type-C Current can't
  302. * possibly satisfy our needs */
  303. if (scfg == NULL || (scfg->v != 5000 && (scfg->vmin > 5000
  304. || scfg->vmax < 5000))) {
  305. dpm_data->_capability_match = false;
  306. return false;
  307. }
  308. /* Get the current we want */
  309. uint16_t current = dpm_get_current(scfg, 5000);
  310. /* If 1.5 A is available and we want no more than that, great. */
  311. if (tcc == fusb_tcc_1_5 && current <= 150) {
  312. dpm_data->_capability_match = true;
  313. return true;
  314. }
  315. /* If 3 A is available and we want no more than that, that's great too. */
  316. if (tcc == fusb_tcc_3_0 && current <= 300) {
  317. dpm_data->_capability_match = true;
  318. return true;
  319. }
  320. /* We're overly cautious if USB default current is available, since that
  321. * could mean different things depending on the port we're connected to,
  322. * and since we're really supposed to enumerate in order to request more
  323. * than 100 mA. This could be changed in the future. */
  324. dpm_data->_capability_match = false;
  325. return false;
  326. }
  327. void pdbs_dpm_pd_start(struct pdb_config *cfg)
  328. {
  329. /* Cast the dpm_data to the right type */
  330. struct pdbs_dpm_data *dpm_data = cfg->dpm_data;
  331. if (dpm_data->led_pd_status) {
  332. chEvtSignal(pdbs_led_thread, PDBS_EVT_LED_NEGOTIATING);
  333. }
  334. }
  335. /*
  336. * Set the output state, with LED indication.
  337. */
  338. static void dpm_output_set(struct pdbs_dpm_data *dpm_data, bool state)
  339. {
  340. /* Update the present voltage */
  341. dpm_data->_present_voltage = dpm_data->_requested_voltage;
  342. /* Set the power output */
  343. if (state && dpm_data->output_enabled) {
  344. /* Turn the output on */
  345. if (dpm_data->led_pd_status) {
  346. chEvtSignal(pdbs_led_thread, PDBS_EVT_LED_OUTPUT_ON);
  347. }
  348. palSetLine(LINE_OUT_CTRL);
  349. } else {
  350. /* Turn the output off */
  351. if (dpm_data->led_pd_status) {
  352. chEvtSignal(pdbs_led_thread, PDBS_EVT_LED_OUTPUT_OFF);
  353. }
  354. palClearLine(LINE_OUT_CTRL);
  355. }
  356. }
  357. void pdbs_dpm_transition_default(struct pdb_config *cfg)
  358. {
  359. /* Cast the dpm_data to the right type */
  360. struct pdbs_dpm_data *dpm_data = cfg->dpm_data;
  361. /* Pretend we requested 5 V */
  362. dpm_data->_requested_voltage = 5000;
  363. /* Turn the output off */
  364. dpm_output_set(cfg->dpm_data, false);
  365. }
  366. void pdbs_dpm_transition_min(struct pdb_config *cfg)
  367. {
  368. dpm_output_set(cfg->dpm_data, false);
  369. }
  370. void pdbs_dpm_transition_standby(struct pdb_config *cfg)
  371. {
  372. /* Cast the dpm_data to the right type */
  373. struct pdbs_dpm_data *dpm_data = cfg->dpm_data;
  374. /* If the voltage is changing, enter Sink Standby */
  375. if (dpm_data->_requested_voltage != dpm_data->_present_voltage) {
  376. /* For the PD Buddy Sink, entering Sink Standby is equivalent to
  377. * turning the output off. However, we don't want to change the LED
  378. * state for standby mode. */
  379. palClearLine(LINE_OUT_CTRL);
  380. }
  381. }
  382. void pdbs_dpm_transition_requested(struct pdb_config *cfg)
  383. {
  384. /* Cast the dpm_data to the right type */
  385. struct pdbs_dpm_data *dpm_data = cfg->dpm_data;
  386. dpm_output_set(cfg->dpm_data, dpm_data->_capability_match);
  387. }