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
|
/* Copyright 2022 The ChromiumOS Authors
* 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_ext, CONFIG_TCPCI_EMUL_LOG_LEVEL);
#include <zephyr/sys/byteorder.h>
#include <zephyr/kernel.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_ext.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 device extension data
*/
static void
tcpci_faulty_ext_reduce_action_count(struct tcpci_faulty_ext_data *data)
{
struct tcpci_faulty_ext_action *action;
action = k_fifo_peek_head(&data->action_list);
if (action->count == TCPCI_FAULTY_EXT_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_ext_append_action(struct tcpci_faulty_ext_data *data,
struct tcpci_faulty_ext_action *action)
{
k_fifo_put(&data->action_list, action);
}
void tcpci_faulty_ext_clear_actions_list(struct tcpci_faulty_ext_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 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_ext_handle_sop_msg(struct tcpci_partner_extension *ext,
struct tcpci_partner_data *common_data,
const struct tcpci_emul_msg *msg)
{
struct tcpci_faulty_ext_data *data =
CONTAINER_OF(ext, struct tcpci_faulty_ext_data, ext);
struct tcpci_faulty_ext_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_EXT_FAIL_SRC_CAP) {
/* Fail is not sending GoodCRC from partner */
tcpci_partner_received_msg_status(
common_data, TCPCI_EMUL_TX_FAILED);
tcpci_faulty_ext_reduce_action_count(data);
return TCPCI_PARTNER_COMMON_MSG_HANDLED;
}
if (action->action_mask &
TCPCI_FAULTY_EXT_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_ext_reduce_action_count(data);
return TCPCI_PARTNER_COMMON_MSG_HANDLED;
}
if (action->action_mask &
TCPCI_FAULTY_EXT_IGNORE_SRC_CAP) {
/* Send only GoodCRC */
tcpci_partner_received_msg_status(
common_data, TCPCI_EMUL_TX_SUCCESS);
tcpci_faulty_ext_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 device extension callbacks */
struct tcpci_partner_extension_ops tcpci_faulty_ext_ops = {
.sop_msg_handler = tcpci_faulty_ext_handle_sop_msg,
.hard_reset = NULL,
.soft_reset = NULL,
.disconnect = NULL,
.connect = NULL,
};
struct tcpci_partner_extension *
tcpci_faulty_ext_init(struct tcpci_faulty_ext_data *data,
struct tcpci_partner_data *common_data,
struct tcpci_partner_extension *ext)
{
struct tcpci_partner_extension *faulty_ext = &data->ext;
k_fifo_init(&data->action_list);
common_data->send_goodcrc = false;
faulty_ext->next = ext;
faulty_ext->ops = &tcpci_faulty_ext_ops;
return faulty_ext;
}
|
