1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
|
/* Copyright 2022 The Chromium OS Authors. All rights reserved.
* Use of this source code is governed by a BSD-style license that can be
* found in the LICENSE file.
*/
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(tcpci_faulty_snk_emul, CONFIG_TCPCI_EMUL_LOG_LEVEL);
#include <zephyr/sys/byteorder.h>
#include <zephyr/zephyr.h>
#include "common.h"
#include "emul/tcpc/emul_tcpci.h"
#include "emul/tcpc/emul_tcpci_partner_common.h"
#include "emul/tcpc/emul_tcpci_partner_faulty_snk.h"
#include "emul/tcpc/emul_tcpci_partner_snk.h"
#include "usb_pd.h"
/**
* @brief Reduce number of times to repeat action. If count reaches zero, action
* is removed from queue.
*
* @param data Pointer to USB-C malfunctioning sink device extension data
*/
static void tcpci_faulty_snk_emul_reduce_action_count(
struct tcpci_faulty_snk_emul_data *data)
{
struct tcpci_faulty_snk_action *action;
action = k_fifo_peek_head(&data->action_list);
if (action->count == TCPCI_FAULTY_SNK_INFINITE_ACTION) {
return;
}
action->count--;
if (action->count != 0) {
return;
}
/* Remove action from queue */
k_fifo_get(&data->action_list, K_FOREVER);
}
void tcpci_faulty_snk_emul_append_action(
struct tcpci_faulty_snk_emul_data *data,
struct tcpci_faulty_snk_action *action)
{
k_fifo_put(&data->action_list, action);
}
void tcpci_faulty_snk_emul_clear_actions_list(
struct tcpci_faulty_snk_emul_data *data)
{
while (!k_fifo_is_empty(&data->action_list)) {
k_fifo_get(&data->action_list, K_FOREVER);
}
}
/**
* @brief Handle SOP messages as TCPCI malfunctioning device
*
* @param ext Pointer to USB-C malfunctioning sink device emulator extension
* @param common_data Pointer to USB-C device emulator common data
* @param msg Pointer to received message
*
* @return TCPCI_PARTNER_COMMON_MSG_HANDLED Message was handled
* @return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED Message wasn't handled
*/
static enum tcpci_partner_handler_res tcpci_faulty_snk_emul_handle_sop_msg(
struct tcpci_partner_extension *ext,
struct tcpci_partner_data *common_data,
const struct tcpci_emul_msg *msg)
{
struct tcpci_faulty_snk_emul_data *data =
CONTAINER_OF(ext, struct tcpci_faulty_snk_emul_data, ext);
struct tcpci_faulty_snk_action *action;
uint16_t header;
action = k_fifo_peek_head(&data->action_list);
header = sys_get_le16(msg->buf);
if (action == NULL) {
/* No faulty action, so send GoodCRC */
tcpci_emul_partner_msg_status(common_data->tcpci_emul,
TCPCI_EMUL_TX_SUCCESS);
return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
}
if (PD_HEADER_CNT(header)) {
/* Handle data message */
switch (PD_HEADER_TYPE(header)) {
case PD_DATA_SOURCE_CAP:
if (action->action_mask &
TCPCI_FAULTY_SNK_FAIL_SRC_CAP) {
/* Fail is not sending GoodCRC from partner */
tcpci_partner_received_msg_status(
common_data, TCPCI_EMUL_TX_FAILED);
tcpci_faulty_snk_emul_reduce_action_count(data);
return TCPCI_PARTNER_COMMON_MSG_HANDLED;
}
if (action->action_mask &
TCPCI_FAULTY_SNK_DISCARD_SRC_CAP) {
/* Discard because partner is sending message */
tcpci_partner_received_msg_status(
common_data, TCPCI_EMUL_TX_DISCARDED);
tcpci_partner_send_control_msg(
common_data,
PD_CTRL_ACCEPT, 0);
tcpci_faulty_snk_emul_reduce_action_count(data);
return TCPCI_PARTNER_COMMON_MSG_HANDLED;
}
if (action->action_mask &
TCPCI_FAULTY_SNK_IGNORE_SRC_CAP) {
/* Send only GoodCRC */
tcpci_partner_received_msg_status(
common_data, TCPCI_EMUL_TX_SUCCESS);
tcpci_faulty_snk_emul_reduce_action_count(data);
return TCPCI_PARTNER_COMMON_MSG_HANDLED;
}
}
}
/*
* Send GoodCRC for all unhandled messages, since we disabled it in
* common handler
*/
tcpci_partner_received_msg_status(common_data, TCPCI_EMUL_TX_SUCCESS);
return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
}
/** USB-C malfunctioning sink device extension callbacks */
struct tcpci_partner_extension_ops tcpci_faulty_snk_emul_ops = {
.sop_msg_handler = tcpci_faulty_snk_emul_handle_sop_msg,
.hard_reset = NULL,
.soft_reset = NULL,
.disconnect = NULL,
.connect = NULL,
};
struct tcpci_partner_extension *tcpci_faulty_snk_emul_init(
struct tcpci_faulty_snk_emul_data *data,
struct tcpci_partner_data *common_data,
struct tcpci_partner_extension *ext)
{
struct tcpci_partner_extension *snk_ext = &data->ext;
k_fifo_init(&data->action_list);
common_data->send_goodcrc = false;
snk_ext->next = ext;
snk_ext->ops = &tcpci_faulty_snk_emul_ops;
return snk_ext;
}
|
