1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright (c) 2021, MediaTek Inc.
4 * Copyright (c) 2021-2022, Intel Corporation.
5 *
6 * Authors:
7 * Haijun Liu <haijun.liu@mediatek.com>
8 * Ricardo Martinez <ricardo.martinez@linux.intel.com>
9 * Moises Veleta <moises.veleta@intel.com>
10 *
11 * Contributors:
12 * Amir Hanania <amir.hanania@intel.com>
13 * Chiranjeevi Rapolu <chiranjeevi.rapolu@intel.com>
14 * Eliot Lee <eliot.lee@intel.com>
15 * Sreehari Kancharla <sreehari.kancharla@intel.com>
16 */
17
18 #include <linux/bitfield.h>
19 #include <linux/device.h>
20 #include <linux/err.h>
21 #include <linux/kthread.h>
22 #include <linux/netdevice.h>
23 #include <linux/skbuff.h>
24 #include <linux/spinlock.h>
25
26 #include "t7xx_port.h"
27 #include "t7xx_port_proxy.h"
28 #include "t7xx_state_monitor.h"
29
30 #define PORT_MSG_VERSION GENMASK(31, 16)
31 #define PORT_MSG_PRT_CNT GENMASK(15, 0)
32
33 struct port_msg {
34 __le32 head_pattern;
35 __le32 info;
36 __le32 tail_pattern;
37 __le32 data[];
38 };
39
port_ctl_send_msg_to_md(struct t7xx_port * port,unsigned int msg,unsigned int ex_msg)40 static int port_ctl_send_msg_to_md(struct t7xx_port *port, unsigned int msg, unsigned int ex_msg)
41 {
42 struct sk_buff *skb;
43 int ret;
44
45 skb = t7xx_ctrl_alloc_skb(0);
46 if (!skb)
47 return -ENOMEM;
48
49 ret = t7xx_port_send_ctl_skb(port, skb, msg, ex_msg);
50 if (ret)
51 dev_kfree_skb_any(skb);
52
53 return ret;
54 }
55
fsm_ee_message_handler(struct t7xx_port * port,struct t7xx_fsm_ctl * ctl,struct sk_buff * skb)56 static int fsm_ee_message_handler(struct t7xx_port *port, struct t7xx_fsm_ctl *ctl,
57 struct sk_buff *skb)
58 {
59 struct ctrl_msg_header *ctrl_msg_h = (struct ctrl_msg_header *)skb->data;
60 struct device *dev = &ctl->md->t7xx_dev->pdev->dev;
61 enum md_state md_state;
62 int ret = -EINVAL;
63
64 md_state = t7xx_fsm_get_md_state(ctl);
65 if (md_state != MD_STATE_EXCEPTION) {
66 dev_err(dev, "Receive invalid MD_EX %x when MD state is %d\n",
67 ctrl_msg_h->ex_msg, md_state);
68 return -EINVAL;
69 }
70
71 switch (le32_to_cpu(ctrl_msg_h->ctrl_msg_id)) {
72 case CTL_ID_MD_EX:
73 if (le32_to_cpu(ctrl_msg_h->ex_msg) != MD_EX_CHK_ID) {
74 dev_err(dev, "Receive invalid MD_EX %x\n", ctrl_msg_h->ex_msg);
75 break;
76 }
77
78 ret = port_ctl_send_msg_to_md(port, CTL_ID_MD_EX, MD_EX_CHK_ID);
79 if (ret) {
80 dev_err(dev, "Failed to send exception message to modem\n");
81 break;
82 }
83
84 ret = t7xx_fsm_append_event(ctl, FSM_EVENT_MD_EX, NULL, 0);
85 if (ret)
86 dev_err(dev, "Failed to append Modem Exception event");
87
88 break;
89
90 case CTL_ID_MD_EX_ACK:
91 if (le32_to_cpu(ctrl_msg_h->ex_msg) != MD_EX_CHK_ACK_ID) {
92 dev_err(dev, "Receive invalid MD_EX_ACK %x\n", ctrl_msg_h->ex_msg);
93 break;
94 }
95
96 ret = t7xx_fsm_append_event(ctl, FSM_EVENT_MD_EX_REC_OK, NULL, 0);
97 if (ret)
98 dev_err(dev, "Failed to append Modem Exception Received event");
99
100 break;
101
102 case CTL_ID_MD_EX_PASS:
103 ret = t7xx_fsm_append_event(ctl, FSM_EVENT_MD_EX_PASS, NULL, 0);
104 if (ret)
105 dev_err(dev, "Failed to append Modem Exception Passed event");
106
107 break;
108
109 case CTL_ID_DRV_VER_ERROR:
110 dev_err(dev, "AP/MD driver version mismatch\n");
111 }
112
113 return ret;
114 }
115
116 /**
117 * t7xx_port_enum_msg_handler() - Parse the port enumeration message to create/remove nodes.
118 * @md: Modem context.
119 * @msg: Message.
120 * @msg_len: Length of @msg in bytes.
121 *
122 * Used to control create/remove device node.
123 *
124 * Return:
125 * * 0 - Success.
126 * * -EFAULT - Message check failure.
127 */
t7xx_port_enum_msg_handler(struct t7xx_modem * md,void * msg,size_t msg_len)128 int t7xx_port_enum_msg_handler(struct t7xx_modem *md, void *msg, size_t msg_len)
129 {
130 struct device *dev = &md->t7xx_dev->pdev->dev;
131 unsigned int version, port_count, i;
132 struct port_msg *port_msg = msg;
133
134 if (msg_len < sizeof(*port_msg)) {
135 dev_err(dev, "Port enum msg too short for header: need %zu, have %zu\n",
136 sizeof(*port_msg), msg_len);
137 return -EINVAL;
138 }
139
140 version = FIELD_GET(PORT_MSG_VERSION, le32_to_cpu(port_msg->info));
141 if (version != PORT_ENUM_VER ||
142 le32_to_cpu(port_msg->head_pattern) != PORT_ENUM_HEAD_PATTERN ||
143 le32_to_cpu(port_msg->tail_pattern) != PORT_ENUM_TAIL_PATTERN) {
144 dev_err(dev, "Invalid port control message %x:%x:%x\n",
145 version, le32_to_cpu(port_msg->head_pattern),
146 le32_to_cpu(port_msg->tail_pattern));
147 return -EFAULT;
148 }
149
150 port_count = FIELD_GET(PORT_MSG_PRT_CNT, le32_to_cpu(port_msg->info));
151
152 if (msg_len < struct_size(port_msg, data, port_count)) {
153 dev_err(dev, "Port enum msg too short: need %zu, have %zu\n",
154 struct_size(port_msg, data, port_count), msg_len);
155 return -EINVAL;
156 }
157
158 for (i = 0; i < port_count; i++) {
159 u32 port_info = le32_to_cpu(port_msg->data[i]);
160 unsigned int ch_id;
161 bool en_flag;
162
163 ch_id = FIELD_GET(PORT_INFO_CH_ID, port_info);
164 en_flag = port_info & PORT_INFO_ENFLG;
165 if (t7xx_port_proxy_chl_enable_disable(md->port_prox, ch_id, en_flag))
166 dev_dbg(dev, "Port:%x not found\n", ch_id);
167 }
168
169 return 0;
170 }
171
control_msg_handler(struct t7xx_port * port,struct sk_buff * skb)172 static int control_msg_handler(struct t7xx_port *port, struct sk_buff *skb)
173 {
174 const struct t7xx_port_conf *port_conf = port->port_conf;
175 struct t7xx_fsm_ctl *ctl = port->t7xx_dev->md->fsm_ctl;
176 struct ctrl_msg_header *ctrl_msg_h;
177 int ret = 0;
178
179 ctrl_msg_h = (struct ctrl_msg_header *)skb->data;
180 switch (le32_to_cpu(ctrl_msg_h->ctrl_msg_id)) {
181 case CTL_ID_HS2_MSG:
182 skb_pull(skb, sizeof(*ctrl_msg_h));
183
184 if (port_conf->rx_ch == PORT_CH_CONTROL_RX ||
185 port_conf->rx_ch == PORT_CH_AP_CONTROL_RX) {
186 int event = port_conf->rx_ch == PORT_CH_CONTROL_RX ?
187 FSM_EVENT_MD_HS2 : FSM_EVENT_AP_HS2;
188
189 ret = t7xx_fsm_append_event(ctl, event, skb->data,
190 le32_to_cpu(ctrl_msg_h->data_length));
191 if (ret)
192 dev_err(port->dev, "Failed to append Handshake 2 event");
193 }
194
195 dev_kfree_skb_any(skb);
196 break;
197
198 case CTL_ID_MD_EX:
199 case CTL_ID_MD_EX_ACK:
200 case CTL_ID_MD_EX_PASS:
201 case CTL_ID_DRV_VER_ERROR:
202 ret = fsm_ee_message_handler(port, ctl, skb);
203 dev_kfree_skb_any(skb);
204 break;
205
206 case CTL_ID_PORT_ENUM:
207 skb_pull(skb, sizeof(*ctrl_msg_h));
208 ret = t7xx_port_enum_msg_handler(ctl->md, (struct port_msg *)skb->data, skb->len);
209 if (!ret)
210 ret = port_ctl_send_msg_to_md(port, CTL_ID_PORT_ENUM, 0);
211 else
212 ret = port_ctl_send_msg_to_md(port, CTL_ID_PORT_ENUM,
213 PORT_ENUM_VER_MISMATCH);
214
215 break;
216
217 default:
218 ret = -EINVAL;
219 dev_err(port->dev, "Unknown control message ID to FSM %x\n",
220 le32_to_cpu(ctrl_msg_h->ctrl_msg_id));
221 break;
222 }
223
224 if (ret)
225 dev_err(port->dev, "%s control message handle error: %d\n", port_conf->name, ret);
226
227 return ret;
228 }
229
port_ctl_rx_thread(void * arg)230 static int port_ctl_rx_thread(void *arg)
231 {
232 while (!kthread_should_stop()) {
233 struct t7xx_port *port = arg;
234 struct sk_buff *skb;
235 unsigned long flags;
236
237 spin_lock_irqsave(&port->rx_wq.lock, flags);
238 if (skb_queue_empty(&port->rx_skb_list) &&
239 wait_event_interruptible_locked_irq(port->rx_wq,
240 !skb_queue_empty(&port->rx_skb_list) ||
241 kthread_should_stop())) {
242 spin_unlock_irqrestore(&port->rx_wq.lock, flags);
243 continue;
244 }
245 if (kthread_should_stop()) {
246 spin_unlock_irqrestore(&port->rx_wq.lock, flags);
247 break;
248 }
249 skb = __skb_dequeue(&port->rx_skb_list);
250 spin_unlock_irqrestore(&port->rx_wq.lock, flags);
251
252 control_msg_handler(port, skb);
253 }
254
255 return 0;
256 }
257
port_ctl_init(struct t7xx_port * port)258 static int port_ctl_init(struct t7xx_port *port)
259 {
260 const struct t7xx_port_conf *port_conf = port->port_conf;
261
262 port->thread = kthread_run(port_ctl_rx_thread, port, "%s", port_conf->name);
263 if (IS_ERR(port->thread)) {
264 dev_err(port->dev, "Failed to start port control thread\n");
265 return PTR_ERR(port->thread);
266 }
267
268 port->rx_length_th = CTRL_QUEUE_MAXLEN;
269 return 0;
270 }
271
port_ctl_uninit(struct t7xx_port * port)272 static void port_ctl_uninit(struct t7xx_port *port)
273 {
274 unsigned long flags;
275 struct sk_buff *skb;
276
277 if (port->thread)
278 kthread_stop(port->thread);
279
280 spin_lock_irqsave(&port->rx_wq.lock, flags);
281 port->rx_length_th = 0;
282 while ((skb = __skb_dequeue(&port->rx_skb_list)) != NULL)
283 dev_kfree_skb_any(skb);
284 spin_unlock_irqrestore(&port->rx_wq.lock, flags);
285 }
286
287 struct port_ops ctl_port_ops = {
288 .init = port_ctl_init,
289 .recv_skb = t7xx_port_enqueue_skb,
290 .uninit = port_ctl_uninit,
291 };
292