1 /*
2 * Copyright (c) 2022 The Chromium OS Authors
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/sys/byteorder.h>
9 #include <zephyr/smf.h>
10 #include <zephyr/usb_c/usbc.h>
11 #include <zephyr/drivers/usb_c/usbc_pd.h>
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_DECLARE(usbc_stack, CONFIG_USBC_STACK_LOG_LEVEL);
14
15 #include "usbc_pe_common_internal.h"
16 #include "usbc_stack.h"
17
18 /**
19 * @brief Initialize the Source Policy Engine layer
20 */
pe_snk_init(const struct device * dev)21 void pe_snk_init(const struct device *dev)
22 {
23 struct usbc_port_data *data = dev->data;
24 struct policy_engine *pe = data->pe;
25
26 /* Initial role of sink is UFP */
27 pe_set_data_role(dev, TC_ROLE_UFP);
28
29 /* Initialize timers */
30 usbc_timer_init(&pe->pd_t_typec_sink_wait_cap, PD_T_TYPEC_SINK_WAIT_CAP_MAX_MS);
31 usbc_timer_init(&pe->pd_t_ps_transition, PD_T_SPR_PS_TRANSITION_NOM_MS);
32 usbc_timer_init(&pe->pd_t_wait_to_resend, PD_T_SINK_REQUEST_MIN_MS);
33
34 /* Goto startup state */
35 pe_set_state(dev, PE_SNK_STARTUP);
36 }
37
38 /**
39 * @brief Handle sink-specific DPM requests
40 */
sink_dpm_requests(const struct device * dev)41 void sink_dpm_requests(const struct device *dev)
42 {
43 struct usbc_port_data *data = dev->data;
44 struct policy_engine *pe = data->pe;
45
46 /*
47 * Handle any common DPM Requests
48 */
49 if (common_dpm_requests(dev)) {
50 return;
51 }
52
53 /*
54 * Handle Sink DPM Requests
55 */
56 if (pe->dpm_request > REQUEST_TC_END) {
57 atomic_set_bit(pe->flags, PE_FLAGS_DPM_INITIATED_AMS);
58
59 if (pe->dpm_request == REQUEST_PE_GET_SRC_CAPS) {
60 pe_set_state(dev, PE_SNK_GET_SOURCE_CAP);
61 }
62 }
63 }
64
65 /**
66 * @brief PE_SNK_Startup Entry State
67 */
pe_snk_startup_entry(void * obj)68 void pe_snk_startup_entry(void *obj)
69 {
70 struct policy_engine *pe = (struct policy_engine *)obj;
71 const struct device *dev = pe->dev;
72
73 LOG_INF("PE_SNK_Startup");
74
75 /* Reset the protocol layer */
76 prl_reset(dev);
77
78 /* Set power role to Sink */
79 pe->power_role = TC_ROLE_SINK;
80
81 /* Invalidate explicit contract */
82 atomic_clear_bit(pe->flags, PE_FLAGS_EXPLICIT_CONTRACT);
83
84 policy_notify(dev, NOT_PD_CONNECTED);
85 }
86
87 /**
88 * @brief PE_SNK_Startup Run State
89 */
pe_snk_startup_run(void * obj)90 void pe_snk_startup_run(void *obj)
91 {
92 struct policy_engine *pe = (struct policy_engine *)obj;
93 const struct device *dev = pe->dev;
94
95 /*
96 * Once the reset process completes, the Policy Engine Shall
97 * transition to the PE_SNK_Discovery state
98 */
99 if (prl_is_running(dev)) {
100 pe_set_state(dev, PE_SNK_DISCOVERY);
101 }
102 }
103
104 /**
105 * @brief PE_SNK_Discovery Entry State
106 */
pe_snk_discovery_entry(void * obj)107 void pe_snk_discovery_entry(void *obj)
108 {
109 LOG_INF("PE_SNK_Discovery");
110 }
111
112 /**
113 * @brief PE_SNK_Discovery Run State
114 */
pe_snk_discovery_run(void * obj)115 void pe_snk_discovery_run(void *obj)
116 {
117 struct policy_engine *pe = (struct policy_engine *)obj;
118 const struct device *dev = pe->dev;
119 struct usbc_port_data *data = dev->data;
120 const struct device *vbus = data->vbus;
121
122 /*
123 * Transition to the PE_SNK_Wait_for_Capabilities state when
124 * VBUS has been detected
125 */
126 if (usbc_vbus_check_level(vbus, TC_VBUS_PRESENT)) {
127 pe_set_state(dev, PE_SNK_WAIT_FOR_CAPABILITIES);
128 }
129 }
130
131 /**
132 * @brief PE_SNK_Wait_For_Capabilities Entry State
133 */
pe_snk_wait_for_capabilities_entry(void * obj)134 void pe_snk_wait_for_capabilities_entry(void *obj)
135 {
136 struct policy_engine *pe = (struct policy_engine *)obj;
137
138 LOG_INF("PE_SNK_Wait_For_Capabilities");
139
140 /* Initialize and start the SinkWaitCapTimer */
141 usbc_timer_start(&pe->pd_t_typec_sink_wait_cap);
142 }
143
144 /**
145 * @brief PE_SNK_Wait_For_Capabilities Run State
146 */
pe_snk_wait_for_capabilities_run(void * obj)147 void pe_snk_wait_for_capabilities_run(void *obj)
148 {
149 struct policy_engine *pe = (struct policy_engine *)obj;
150 const struct device *dev = pe->dev;
151 struct usbc_port_data *data = dev->data;
152 struct protocol_layer_rx_t *prl_rx = data->prl_rx;
153 union pd_header header;
154
155 /*
156 * Transition to the PE_SNK_Evaluate_Capability state when:
157 * 1) A Source_Capabilities Message is received.
158 */
159 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_MSG_RECEIVED)) {
160 header = prl_rx->emsg.header;
161 if (received_data_message(dev, header, PD_DATA_SOURCE_CAP)) {
162 pe_set_state(dev, PE_SNK_EVALUATE_CAPABILITY);
163 return;
164 }
165 }
166
167 /* When the SinkWaitCapTimer times out, perform a Hard Reset. */
168 if (usbc_timer_expired(&pe->pd_t_typec_sink_wait_cap)) {
169 atomic_set_bit(pe->flags, PE_FLAGS_SNK_WAIT_CAP_TIMEOUT);
170 pe_set_state(dev, PE_SNK_HARD_RESET);
171 }
172 }
173
174 /**
175 * @brief PE_SNK_Wait_For_Capabilities Exit State
176 */
pe_snk_wait_for_capabilities_exit(void * obj)177 void pe_snk_wait_for_capabilities_exit(void *obj)
178 {
179 struct policy_engine *pe = (struct policy_engine *)obj;
180
181 /* Stop SinkWaitCapTimer */
182 usbc_timer_stop(&pe->pd_t_typec_sink_wait_cap);
183 }
184
185 /**
186 * @brief PE_SNK_Evaluate_Capability Entry State
187 */
pe_snk_evaluate_capability_entry(void * obj)188 void pe_snk_evaluate_capability_entry(void *obj)
189 {
190 struct policy_engine *pe = (struct policy_engine *)obj;
191 const struct device *dev = pe->dev;
192 struct usbc_port_data *data = dev->data;
193 struct protocol_layer_rx_t *prl_rx = data->prl_rx;
194 union pd_header header;
195 uint32_t *pdos = (uint32_t *)prl_rx->emsg.data;
196 uint32_t num_pdo_objs = PD_CONVERT_BYTES_TO_PD_HEADER_COUNT(prl_rx->emsg.len);
197
198 LOG_INF("PE_SNK_Evaluate_Capability");
199
200 /* Inform the DPM of the reception of the source capabilities */
201 policy_notify(dev, SOURCE_CAPABILITIES_RECEIVED);
202
203 header = prl_rx->emsg.header;
204
205 /* Reset Hard Reset counter to zero */
206 pe->hard_reset_counter = 0;
207
208 /* Set to highest revision supported by both ports */
209 prl_set_rev(dev, PD_PACKET_SOP, MIN(PD_REV30, header.specification_revision));
210
211 /* Send source caps to Device Policy Manager for saving */
212 policy_set_src_cap(dev, pdos, num_pdo_objs);
213
214 /* Transition to PE_Snk_Select_Capability */
215 pe_set_state(dev, PE_SNK_SELECT_CAPABILITY);
216 }
217
218 /**
219 * @brief PE_SNK_Select_Capability Entry State
220 */
pe_snk_select_capability_entry(void * obj)221 void pe_snk_select_capability_entry(void *obj)
222 {
223 struct policy_engine *pe = (struct policy_engine *)obj;
224 const struct device *dev = pe->dev;
225 uint32_t rdo;
226
227 LOG_INF("PE_SNK_Select_Capability");
228
229 /* Get selected source cap from Device Policy Manager */
230 rdo = policy_get_request_data_object(dev);
231
232 /* Send Request */
233 pe_send_request_msg(dev, rdo);
234 /* Inform Device Policy Manager that we are PD Connected */
235 policy_notify(dev, PD_CONNECTED);
236 }
237
238 /**
239 * @brief PE_SNK_Select_Capability Run State
240 * NOTE: Sender Response Timer is handled in super state.
241 */
pe_snk_select_capability_run(void * obj)242 void pe_snk_select_capability_run(void *obj)
243 {
244 struct policy_engine *pe = (struct policy_engine *)obj;
245 const struct device *dev = pe->dev;
246 struct usbc_port_data *data = dev->data;
247 struct protocol_layer_rx_t *prl_rx = data->prl_rx;
248 union pd_header header;
249
250 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_MSG_DISCARDED)) {
251 /*
252 * The sent REQUEST message was discarded. This can be at
253 * the start of an AMS or in the middle. Handle what to
254 * do based on where we came from.
255 * 1) SE_SNK_EVALUATE_CAPABILITY: sends SoftReset
256 * 2) SE_SNK_READY: goes back to SNK Ready
257 */
258 if (pe_get_last_state(dev) == PE_SNK_EVALUATE_CAPABILITY) {
259 pe_send_soft_reset(dev, PD_PACKET_SOP);
260 } else {
261 pe_set_state(dev, PE_SNK_READY);
262 }
263 }
264
265 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_MSG_RECEIVED)) {
266 header = prl_rx->emsg.header;
267
268 /*
269 * Transition to the PE_SNK_Transition_Sink state when:
270 * 1) An Accept Message is received from the Source.
271 *
272 * Transition to the PE_SNK_Wait_for_Capabilities state when:
273 * 1) There is no Explicit Contract in place and
274 * 2) A Reject Message is received from the Source or
275 * 3) A Wait Message is received from the Source.
276 *
277 * Transition to the PE_SNK_Ready state when:
278 * 1) There is an Explicit Contract in place and
279 * 2) A Reject Message is received from the Source or
280 * 3) A Wait Message is received from the Source.
281 *
282 * Transition to the PE_SNK_Hard_Reset state when:
283 * 1) A SenderResponseTimer timeout occurs.
284 */
285 /* Only look at control messages */
286 if (received_control_message(dev, header, PD_CTRL_ACCEPT)) {
287 /* explicit contract is now in place */
288 atomic_set_bit(pe->flags, PE_FLAGS_EXPLICIT_CONTRACT);
289 pe_set_state(dev, PE_SNK_TRANSITION_SINK);
290 } else if (received_control_message(dev, header, PD_CTRL_REJECT) ||
291 received_control_message(dev, header, PD_CTRL_WAIT)) {
292 /*
293 * We had a previous explicit contract, so transition to
294 * PE_SNK_Ready
295 */
296 if (atomic_test_bit(pe->flags, PE_FLAGS_EXPLICIT_CONTRACT)) {
297 if (received_control_message(dev, header, PD_CTRL_WAIT)) {
298 /*
299 * Inform Device Policy Manager that Sink
300 * Request needs to Wait
301 */
302 if (policy_wait_notify(dev, WAIT_SINK_REQUEST)) {
303 atomic_set_bit(pe->flags,
304 PE_FLAGS_WAIT_SINK_REQUEST);
305 usbc_timer_start(&pe->pd_t_wait_to_resend);
306 }
307 }
308
309 pe_set_state(dev, PE_SNK_READY);
310 } else {
311 /*
312 * No previous explicit contract, so transition
313 * to PE_SNK_Wait_For_Capabilities
314 */
315 pe_set_state(dev, PE_SNK_WAIT_FOR_CAPABILITIES);
316 }
317 } else {
318 pe_send_soft_reset(dev, prl_rx->emsg.type);
319 }
320 return;
321 }
322 }
323
324 /**
325 * @brief PE_SNK_Transition_Sink Entry State
326 */
pe_snk_transition_sink_entry(void * obj)327 void pe_snk_transition_sink_entry(void *obj)
328 {
329 struct policy_engine *pe = (struct policy_engine *)obj;
330
331 LOG_INF("PE_SNK_Transition_Sink");
332
333 /* Initialize and run PSTransitionTimer */
334 usbc_timer_start(&pe->pd_t_ps_transition);
335 }
336
337 /**
338 * @brief PE_SNK_Transition_Sink Run State
339 */
pe_snk_transition_sink_run(void * obj)340 void pe_snk_transition_sink_run(void *obj)
341 {
342 struct policy_engine *pe = (struct policy_engine *)obj;
343 const struct device *dev = pe->dev;
344 struct usbc_port_data *data = dev->data;
345 struct protocol_layer_rx_t *prl_rx = data->prl_rx;
346 union pd_header header;
347
348 /*
349 * Transition to the PE_SNK_Ready state when:
350 * 1) A PS_RDY Message is received from the Source.
351 *
352 * Transition to the PE_SNK_Hard_Reset state when:
353 * 1) A Protocol Error occurs.
354 */
355 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_MSG_RECEIVED)) {
356 header = prl_rx->emsg.header;
357
358 /*
359 * PS_RDY message received
360 */
361 if (received_control_message(dev, header, PD_CTRL_PS_RDY)) {
362 /*
363 * Inform the Device Policy Manager to Transition
364 * the Power Supply
365 */
366 policy_notify(dev, TRANSITION_PS);
367 pe_set_state(dev, PE_SNK_READY);
368 } else {
369 /* Protocol Error */
370 pe_set_state(dev, PE_SNK_HARD_RESET);
371 }
372 return;
373 }
374
375 /*
376 * Timeout will lead to a Hard Reset
377 */
378 if (usbc_timer_expired(&pe->pd_t_ps_transition)) {
379 pe_set_state(dev, PE_SNK_HARD_RESET);
380 }
381 }
382
383 /**
384 * @brief PE_SNK_Transition_Sink Exit State
385 */
pe_snk_transition_sink_exit(void * obj)386 void pe_snk_transition_sink_exit(void *obj)
387 {
388 struct policy_engine *pe = (struct policy_engine *)obj;
389
390 /* Initialize and run PSTransitionTimer */
391 usbc_timer_stop(&pe->pd_t_ps_transition);
392 }
393
394 /**
395 * @brief PE_SNK_Ready Entry State
396 */
pe_snk_ready_entry(void * obj)397 void pe_snk_ready_entry(void *obj)
398 {
399 struct policy_engine *pe = (struct policy_engine *)obj;
400
401 LOG_INF("PE_SNK_Ready");
402
403 /* Clear AMS Flags */
404 atomic_clear_bit(pe->flags, PE_FLAGS_INTERRUPTIBLE_AMS);
405 atomic_clear_bit(pe->flags, PE_FLAGS_DPM_INITIATED_AMS);
406 }
407
408 /**
409 * @brief PE_SNK_Ready Run State
410 */
pe_snk_ready_run(void * obj)411 void pe_snk_ready_run(void *obj)
412 {
413 struct policy_engine *pe = (struct policy_engine *)obj;
414 const struct device *dev = pe->dev;
415 struct usbc_port_data *data = dev->data;
416 struct protocol_layer_rx_t *prl_rx = data->prl_rx;
417
418 /*
419 * Handle incoming messages before discovery and DPMs other than hard
420 * reset
421 */
422 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_MSG_RECEIVED)) {
423 union pd_header header = prl_rx->emsg.header;
424
425 /* Extended Message Request */
426 if (header.extended) {
427 extended_message_not_supported(dev);
428 return;
429 } else if (header.number_of_data_objects > 0) {
430 /* Data Messages */
431 switch (header.message_type) {
432 case PD_DATA_SOURCE_CAP:
433 pe_set_state(dev, PE_SNK_EVALUATE_CAPABILITY);
434 break;
435 default:
436 pe_set_state(dev, PE_SEND_NOT_SUPPORTED);
437 }
438 return;
439 } else {
440 /* Control Messages */
441 switch (header.message_type) {
442 case PD_CTRL_GOOD_CRC:
443 /* Do nothing */
444 break;
445 case PD_CTRL_PING:
446 /* Do nothing */
447 break;
448 case PD_CTRL_GET_SINK_CAP:
449 pe_set_state(dev, PE_SNK_GIVE_SINK_CAP);
450 return;
451 case PD_CTRL_DR_SWAP:
452 pe_set_state(dev, PE_DRS_EVALUATE_SWAP);
453 return;
454 case PD_CTRL_NOT_SUPPORTED:
455 /* Do nothing */
456 break;
457 /*
458 * USB PD 3.0 6.8.1:
459 * Receiving an unexpected message shall be responded
460 * to with a soft reset message.
461 */
462 case PD_CTRL_ACCEPT:
463 case PD_CTRL_REJECT:
464 case PD_CTRL_WAIT:
465 case PD_CTRL_PS_RDY:
466 pe_send_soft_reset(dev, prl_rx->emsg.type);
467 return;
468 /*
469 * Receiving an unknown or unsupported message
470 * shall be responded to with a not supported message.
471 */
472 default:
473 pe_set_state(dev, PE_SEND_NOT_SUPPORTED);
474 return;
475 }
476 }
477 }
478
479 /*
480 * Check if we are waiting to resend any messages
481 */
482 if (usbc_timer_expired(&pe->pd_t_wait_to_resend)) {
483 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_WAIT_SINK_REQUEST)) {
484 pe_set_state(dev, PE_SNK_SELECT_CAPABILITY);
485 return;
486 } else if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_WAIT_DATA_ROLE_SWAP)) {
487 pe_set_state(dev, PE_DRS_SEND_SWAP);
488 return;
489 }
490 }
491
492 /*
493 * Handle Device Policy Manager Requests
494 */
495 sink_dpm_requests(dev);
496 }
497
pe_snk_ready_exit(void * obj)498 void pe_snk_ready_exit(void *obj)
499 {
500 struct policy_engine *pe = (struct policy_engine *)obj;
501 const struct device *dev = pe->dev;
502
503 /*
504 * If the Source is initiating an AMS, then notify the
505 * PRL that the first message in an AMS will follow.
506 */
507 if (pe_dpm_initiated_ams(dev)) {
508 prl_first_msg_notificaiton(dev);
509 }
510 }
511
512 /**
513 * @brief PE_SNK_Hard_Reset Entry State
514 */
pe_snk_hard_reset_entry(void * obj)515 void pe_snk_hard_reset_entry(void *obj)
516 {
517 struct policy_engine *pe = (struct policy_engine *)obj;
518 const struct device *dev = pe->dev;
519 struct usbc_port_data *data = dev->data;
520
521 LOG_INF("PE_SNK_Hard_Reset");
522
523 /*
524 * Note: If the SinkWaitCapTimer times out and the HardResetCounter is
525 * greater than nHardResetCount the Sink Shall assume that the
526 * Source is non-responsive.
527 */
528 if (atomic_test_bit(pe->flags, PE_FLAGS_SNK_WAIT_CAP_TIMEOUT) &&
529 pe->hard_reset_counter > PD_N_HARD_RESET_COUNT) {
530 /* Inform the DPM that the port partner is not responsive */
531 policy_notify(dev, PORT_PARTNER_NOT_RESPONSIVE);
532
533 /* Pause the Policy Engine */
534 data->pe_enabled = false;
535 return;
536 }
537
538 /* Set Hard Reset Pending Flag */
539 atomic_set_bit(pe->flags, PE_FLAGS_HARD_RESET_PENDING);
540
541 atomic_clear_bit(pe->flags, PE_FLAGS_SNK_WAIT_CAP_TIMEOUT);
542
543 /* Request the generation of Hard Reset Signaling by the PHY Layer */
544 prl_execute_hard_reset(dev);
545 /* Increment the HardResetCounter */
546 pe->hard_reset_counter++;
547 }
548
549 /**
550 * @brief PE_SNK_Hard_Reset Run State
551 */
pe_snk_hard_reset_run(void * obj)552 void pe_snk_hard_reset_run(void *obj)
553 {
554 struct policy_engine *pe = (struct policy_engine *)obj;
555 const struct device *dev = pe->dev;
556
557 /*
558 * Transition to the PE_SNK_Transition_to_default state when:
559 * 1) The Hard Reset is complete.
560 */
561 if (atomic_test_bit(pe->flags, PE_FLAGS_HARD_RESET_PENDING)) {
562 return;
563 }
564
565 pe_set_state(dev, PE_SNK_TRANSITION_TO_DEFAULT);
566 }
567
568 /**
569 * @brief PE_SNK_Transition_to_default Entry State
570 */
pe_snk_transition_to_default_entry(void * obj)571 void pe_snk_transition_to_default_entry(void *obj)
572 {
573 struct policy_engine *pe = (struct policy_engine *)obj;
574 const struct device *dev = pe->dev;
575
576 LOG_INF("PE_SNK_Transition_to_default");
577
578 /* Reset flags */
579 atomic_clear(pe->flags);
580 pe->data_role = TC_ROLE_UFP;
581
582 /*
583 * Indicate to the Device Policy Manager that the Sink Shall
584 * transition to default
585 */
586 policy_notify(dev, SNK_TRANSITION_TO_DEFAULT);
587 /*
588 * Request the Device Policy Manger that the Port Data Role is
589 * set to UFP
590 */
591 policy_notify(dev, DATA_ROLE_IS_UFP);
592 }
593
594 /**
595 * @brief PE_SNK_Transition_to_default Run State
596 */
pe_snk_transition_to_default_run(void * obj)597 void pe_snk_transition_to_default_run(void *obj)
598 {
599 struct policy_engine *pe = (struct policy_engine *)obj;
600 const struct device *dev = pe->dev;
601
602 /*
603 * Wait until Device Policy Manager has transitioned the sink to
604 * default level
605 */
606 if (policy_is_snk_at_default(dev)) {
607 /* Inform the Protocol Layer that the Hard Reset is complete */
608 prl_hard_reset_complete(dev);
609 pe_set_state(dev, PE_SNK_STARTUP);
610 }
611 }
612
613 /**
614 * @brief PE_SNK_Get_Source_Cap Entry State
615 *
616 */
pe_snk_get_source_cap_entry(void * obj)617 void pe_snk_get_source_cap_entry(void *obj)
618 {
619 struct policy_engine *pe = (struct policy_engine *)obj;
620 const struct device *dev = pe->dev;
621
622 LOG_INF("PE_SNK_Get_Source_Cap");
623
624 /*
625 * On entry to the PE_SNK_Get_Source_Cap state the Policy Engine
626 * Shall request the Protocol Layer to send a get Source
627 * Capabilities message in order to retrieve the Source’s
628 * capabilities.
629 */
630 pe_send_ctrl_msg(dev, PD_PACKET_SOP, PD_CTRL_GET_SOURCE_CAP);
631 }
632
633 /**
634 * @brief PE_SNK_Get_Source_Cap Run State
635 * NOTE: Sender Response Timer is handled in super state.
636 */
pe_snk_get_source_cap_run(void * obj)637 void pe_snk_get_source_cap_run(void *obj)
638 {
639 struct policy_engine *pe = (struct policy_engine *)obj;
640 const struct device *dev = pe->dev;
641 struct usbc_port_data *data = dev->data;
642 struct protocol_layer_rx_t *prl_rx = data->prl_rx;
643 union pd_header header;
644
645 /* Wait until message is sent or dropped */
646 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_MSG_RECEIVED)) {
647 /*
648 * The Policy Engine Shall transition to the PE_SNK_Evaluate_Capability
649 * State when:
650 * 1: In SPR Mode and SPR Source Capabilities were requested and
651 * a Source_Capabilities Message is received
652 */
653 header = prl_rx->emsg.header;
654
655 if (received_control_message(dev, header, PD_DATA_SOURCE_CAP)) {
656 pe_set_state(dev, PE_SNK_EVALUATE_CAPABILITY);
657 }
658 }
659 }
660
661 /**
662 * @brief PE_SNK_Give_Sink_Cap Entry state
663 */
pe_snk_give_sink_cap_entry(void * obj)664 void pe_snk_give_sink_cap_entry(void *obj)
665 {
666 struct policy_engine *pe = (struct policy_engine *)obj;
667 const struct device *dev = pe->dev;
668 struct usbc_port_data *data = dev->data;
669 struct protocol_layer_tx_t *prl_tx = data->prl_tx;
670 struct pd_msg *msg = &prl_tx->emsg;
671 uint32_t *pdos;
672 uint32_t num_pdos;
673
674 /* Get present sink capabilities from Device Policy Manager */
675 policy_get_snk_cap(dev, &pdos, &num_pdos);
676
677 msg->len = PD_CONVERT_PD_HEADER_COUNT_TO_BYTES(num_pdos);
678 memcpy(msg->data, (uint8_t *)pdos, msg->len);
679 pe_send_data_msg(dev, PD_PACKET_SOP, PD_DATA_SINK_CAP);
680 }
681
682 /**
683 * @brief PE_SNK_Give_Sink_Cap Run state
684 */
pe_snk_give_sink_cap_run(void * obj)685 void pe_snk_give_sink_cap_run(void *obj)
686 {
687 struct policy_engine *pe = (struct policy_engine *)obj;
688 const struct device *dev = pe->dev;
689 struct usbc_port_data *data = dev->data;
690 struct protocol_layer_rx_t *prl_rx = data->prl_rx;
691
692 /* Wait until message is sent or dropped */
693 if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_TX_COMPLETE)) {
694 pe_set_state(dev, PE_SNK_READY);
695 } else if (atomic_test_and_clear_bit(pe->flags, PE_FLAGS_MSG_DISCARDED)) {
696 pe_send_soft_reset(dev, prl_rx->emsg.type);
697 }
698 }
699
