1 | // SPDX-License-Identifier: GPL-2.0 |
2 | #include <linux/types.h> |
3 | #include <linux/delay.h> |
4 | #include <linux/slab.h> |
5 | #include <linux/console.h> |
6 | #include <asm/hvsi.h> |
7 | |
8 | #include "hvc_console.h" |
9 | |
10 | static int hvsi_send_packet(struct hvsi_priv *pv, struct hvsi_header *packet) |
11 | { |
12 | packet->seqno = cpu_to_be16(atomic_inc_return(&pv->seqno)); |
13 | |
14 | /* Assumes that always succeeds, works in practice */ |
15 | return pv->put_chars(pv->termno, (u8 *)packet, packet->len); |
16 | } |
17 | |
18 | static void hvsi_start_handshake(struct hvsi_priv *pv) |
19 | { |
20 | struct hvsi_query q; |
21 | |
22 | /* Reset state */ |
23 | pv->established = 0; |
24 | atomic_set(v: &pv->seqno, i: 0); |
25 | |
26 | pr_devel("HVSI@%x: Handshaking started\n" , pv->termno); |
27 | |
28 | /* Send version query */ |
29 | q.hdr.type = VS_QUERY_PACKET_HEADER; |
30 | q.hdr.len = sizeof(struct hvsi_query); |
31 | q.verb = cpu_to_be16(VSV_SEND_VERSION_NUMBER); |
32 | hvsi_send_packet(pv, packet: &q.hdr); |
33 | } |
34 | |
35 | static int hvsi_send_close(struct hvsi_priv *pv) |
36 | { |
37 | struct hvsi_control ctrl; |
38 | |
39 | pv->established = 0; |
40 | |
41 | ctrl.hdr.type = VS_CONTROL_PACKET_HEADER; |
42 | ctrl.hdr.len = sizeof(struct hvsi_control); |
43 | ctrl.verb = cpu_to_be16(VSV_CLOSE_PROTOCOL); |
44 | return hvsi_send_packet(pv, packet: &ctrl.hdr); |
45 | } |
46 | |
47 | static void hvsi_cd_change(struct hvsi_priv *pv, int cd) |
48 | { |
49 | if (cd) |
50 | pv->mctrl |= TIOCM_CD; |
51 | else { |
52 | pv->mctrl &= ~TIOCM_CD; |
53 | |
54 | /* We copy the existing hvsi driver semantics |
55 | * here which are to trigger a hangup when |
56 | * we get a carrier loss. |
57 | * Closing our connection to the server will |
58 | * do just that. |
59 | */ |
60 | if (!pv->is_console && pv->opened) { |
61 | pr_devel("HVSI@%x Carrier lost, hanging up !\n" , |
62 | pv->termno); |
63 | hvsi_send_close(pv); |
64 | } |
65 | } |
66 | } |
67 | |
68 | static void hvsi_got_control(struct hvsi_priv *pv) |
69 | { |
70 | struct hvsi_control *pkt = (struct hvsi_control *)pv->inbuf; |
71 | |
72 | switch (be16_to_cpu(pkt->verb)) { |
73 | case VSV_CLOSE_PROTOCOL: |
74 | /* We restart the handshaking */ |
75 | hvsi_start_handshake(pv); |
76 | break; |
77 | case VSV_MODEM_CTL_UPDATE: |
78 | /* Transition of carrier detect */ |
79 | hvsi_cd_change(pv, be32_to_cpu(pkt->word) & HVSI_TSCD); |
80 | break; |
81 | } |
82 | } |
83 | |
84 | static void hvsi_got_query(struct hvsi_priv *pv) |
85 | { |
86 | struct hvsi_query *pkt = (struct hvsi_query *)pv->inbuf; |
87 | struct hvsi_query_response r; |
88 | |
89 | /* We only handle version queries */ |
90 | if (be16_to_cpu(pkt->verb) != VSV_SEND_VERSION_NUMBER) |
91 | return; |
92 | |
93 | pr_devel("HVSI@%x: Got version query, sending response...\n" , |
94 | pv->termno); |
95 | |
96 | /* Send version response */ |
97 | r.hdr.type = VS_QUERY_RESPONSE_PACKET_HEADER; |
98 | r.hdr.len = sizeof(struct hvsi_query_response); |
99 | r.verb = cpu_to_be16(VSV_SEND_VERSION_NUMBER); |
100 | r.u.version = HVSI_VERSION; |
101 | r.query_seqno = pkt->hdr.seqno; |
102 | hvsi_send_packet(pv, packet: &r.hdr); |
103 | |
104 | /* Assume protocol is open now */ |
105 | pv->established = 1; |
106 | } |
107 | |
108 | static void hvsi_got_response(struct hvsi_priv *pv) |
109 | { |
110 | struct hvsi_query_response *r = |
111 | (struct hvsi_query_response *)pv->inbuf; |
112 | |
113 | switch(r->verb) { |
114 | case VSV_SEND_MODEM_CTL_STATUS: |
115 | hvsi_cd_change(pv, be32_to_cpu(r->u.mctrl_word) & HVSI_TSCD); |
116 | pv->mctrl_update = 1; |
117 | break; |
118 | } |
119 | } |
120 | |
121 | static int hvsi_check_packet(struct hvsi_priv *pv) |
122 | { |
123 | u8 len, type; |
124 | |
125 | /* Check header validity. If it's invalid, we ditch |
126 | * the whole buffer and hope we eventually resync |
127 | */ |
128 | if (pv->inbuf[0] < 0xfc) { |
129 | pv->inbuf_len = pv->inbuf_pktlen = 0; |
130 | return 0; |
131 | } |
132 | type = pv->inbuf[0]; |
133 | len = pv->inbuf[1]; |
134 | |
135 | /* Packet incomplete ? */ |
136 | if (pv->inbuf_len < len) |
137 | return 0; |
138 | |
139 | pr_devel("HVSI@%x: Got packet type %x len %d bytes:\n" , |
140 | pv->termno, type, len); |
141 | |
142 | /* We have a packet, yay ! Handle it */ |
143 | switch(type) { |
144 | case VS_DATA_PACKET_HEADER: |
145 | pv->inbuf_pktlen = len - 4; |
146 | pv->inbuf_cur = 4; |
147 | return 1; |
148 | case VS_CONTROL_PACKET_HEADER: |
149 | hvsi_got_control(pv); |
150 | break; |
151 | case VS_QUERY_PACKET_HEADER: |
152 | hvsi_got_query(pv); |
153 | break; |
154 | case VS_QUERY_RESPONSE_PACKET_HEADER: |
155 | hvsi_got_response(pv); |
156 | break; |
157 | } |
158 | |
159 | /* Swallow packet and retry */ |
160 | pv->inbuf_len -= len; |
161 | memmove(pv->inbuf, &pv->inbuf[len], pv->inbuf_len); |
162 | return 1; |
163 | } |
164 | |
165 | static int hvsi_get_packet(struct hvsi_priv *pv) |
166 | { |
167 | /* If we have room in the buffer, ask HV for more */ |
168 | if (pv->inbuf_len < HVSI_INBUF_SIZE) |
169 | pv->inbuf_len += pv->get_chars(pv->termno, |
170 | &pv->inbuf[pv->inbuf_len], |
171 | HVSI_INBUF_SIZE - pv->inbuf_len); |
172 | /* |
173 | * If we have at least 4 bytes in the buffer, check for |
174 | * a full packet and retry |
175 | */ |
176 | if (pv->inbuf_len >= 4) |
177 | return hvsi_check_packet(pv); |
178 | return 0; |
179 | } |
180 | |
181 | ssize_t hvsilib_get_chars(struct hvsi_priv *pv, u8 *buf, size_t count) |
182 | { |
183 | unsigned int tries; |
184 | size_t read = 0; |
185 | |
186 | if (WARN_ON(!pv)) |
187 | return -ENXIO; |
188 | |
189 | /* If we aren't open, don't do anything in order to avoid races |
190 | * with connection establishment. The hvc core will call this |
191 | * before we have returned from notifier_add(), and we need to |
192 | * avoid multiple users playing with the receive buffer |
193 | */ |
194 | if (!pv->opened) |
195 | return 0; |
196 | |
197 | /* We try twice, once with what data we have and once more |
198 | * after we try to fetch some more from the hypervisor |
199 | */ |
200 | for (tries = 1; count && tries < 2; tries++) { |
201 | /* Consume existing data packet */ |
202 | if (pv->inbuf_pktlen) { |
203 | size_t l = min(count, pv->inbuf_pktlen); |
204 | memcpy(&buf[read], &pv->inbuf[pv->inbuf_cur], l); |
205 | pv->inbuf_cur += l; |
206 | pv->inbuf_pktlen -= l; |
207 | count -= l; |
208 | read += l; |
209 | } |
210 | if (count == 0) |
211 | break; |
212 | |
213 | /* Data packet fully consumed, move down remaning data */ |
214 | if (pv->inbuf_cur) { |
215 | pv->inbuf_len -= pv->inbuf_cur; |
216 | memmove(pv->inbuf, &pv->inbuf[pv->inbuf_cur], |
217 | pv->inbuf_len); |
218 | pv->inbuf_cur = 0; |
219 | } |
220 | |
221 | /* Try to get another packet */ |
222 | if (hvsi_get_packet(pv)) |
223 | tries--; |
224 | } |
225 | if (!pv->established) { |
226 | pr_devel("HVSI@%x: returning -EPIPE\n" , pv->termno); |
227 | return -EPIPE; |
228 | } |
229 | return read; |
230 | } |
231 | |
232 | ssize_t hvsilib_put_chars(struct hvsi_priv *pv, const u8 *buf, size_t count) |
233 | { |
234 | struct hvsi_data dp; |
235 | size_t adjcount = min_t(size_t, count, HVSI_MAX_OUTGOING_DATA); |
236 | int rc; |
237 | |
238 | if (WARN_ON(!pv)) |
239 | return -ENODEV; |
240 | |
241 | dp.hdr.type = VS_DATA_PACKET_HEADER; |
242 | dp.hdr.len = adjcount + sizeof(struct hvsi_header); |
243 | memcpy(dp.data, buf, adjcount); |
244 | rc = hvsi_send_packet(pv, packet: &dp.hdr); |
245 | if (rc <= 0) |
246 | return rc; |
247 | return adjcount; |
248 | } |
249 | |
250 | static void maybe_msleep(unsigned long ms) |
251 | { |
252 | /* During early boot, IRQs are disabled, use mdelay */ |
253 | if (irqs_disabled()) |
254 | mdelay(ms); |
255 | else |
256 | msleep(msecs: ms); |
257 | } |
258 | |
259 | int hvsilib_read_mctrl(struct hvsi_priv *pv) |
260 | { |
261 | struct hvsi_query q; |
262 | int rc, timeout; |
263 | |
264 | pr_devel("HVSI@%x: Querying modem control status...\n" , |
265 | pv->termno); |
266 | |
267 | pv->mctrl_update = 0; |
268 | q.hdr.type = VS_QUERY_PACKET_HEADER; |
269 | q.hdr.len = sizeof(struct hvsi_query); |
270 | q.verb = cpu_to_be16(VSV_SEND_MODEM_CTL_STATUS); |
271 | rc = hvsi_send_packet(pv, packet: &q.hdr); |
272 | if (rc <= 0) { |
273 | pr_devel("HVSI@%x: Error %d...\n" , pv->termno, rc); |
274 | return rc; |
275 | } |
276 | |
277 | /* Try for up to 200ms */ |
278 | for (timeout = 0; timeout < 20; timeout++) { |
279 | if (!pv->established) |
280 | return -ENXIO; |
281 | if (pv->mctrl_update) |
282 | return 0; |
283 | if (!hvsi_get_packet(pv)) |
284 | maybe_msleep(ms: 10); |
285 | } |
286 | return -EIO; |
287 | } |
288 | |
289 | int hvsilib_write_mctrl(struct hvsi_priv *pv, int dtr) |
290 | { |
291 | struct hvsi_control ctrl; |
292 | unsigned short mctrl; |
293 | |
294 | mctrl = pv->mctrl; |
295 | if (dtr) |
296 | mctrl |= TIOCM_DTR; |
297 | else |
298 | mctrl &= ~TIOCM_DTR; |
299 | if (mctrl == pv->mctrl) |
300 | return 0; |
301 | pv->mctrl = mctrl; |
302 | |
303 | pr_devel("HVSI@%x: %s DTR...\n" , pv->termno, |
304 | dtr ? "Setting" : "Clearing" ); |
305 | |
306 | ctrl.hdr.type = VS_CONTROL_PACKET_HEADER, |
307 | ctrl.hdr.len = sizeof(struct hvsi_control); |
308 | ctrl.verb = cpu_to_be16(VSV_SET_MODEM_CTL); |
309 | ctrl.mask = cpu_to_be32(HVSI_TSDTR); |
310 | ctrl.word = cpu_to_be32(dtr ? HVSI_TSDTR : 0); |
311 | return hvsi_send_packet(pv, packet: &ctrl.hdr); |
312 | } |
313 | |
314 | void hvsilib_establish(struct hvsi_priv *pv) |
315 | { |
316 | int timeout; |
317 | |
318 | pr_devel("HVSI@%x: Establishing...\n" , pv->termno); |
319 | |
320 | /* Try for up to 200ms, there can be a packet to |
321 | * start the process waiting for us... |
322 | */ |
323 | for (timeout = 0; timeout < 20; timeout++) { |
324 | if (pv->established) |
325 | goto established; |
326 | if (!hvsi_get_packet(pv)) |
327 | maybe_msleep(ms: 10); |
328 | } |
329 | |
330 | /* Failed, send a close connection packet just |
331 | * in case |
332 | */ |
333 | pr_devel("HVSI@%x: ... sending close\n" , pv->termno); |
334 | |
335 | hvsi_send_close(pv); |
336 | |
337 | /* Then restart handshake */ |
338 | |
339 | pr_devel("HVSI@%x: ... restarting handshake\n" , pv->termno); |
340 | |
341 | hvsi_start_handshake(pv); |
342 | |
343 | pr_devel("HVSI@%x: ... waiting handshake\n" , pv->termno); |
344 | |
345 | /* Try for up to 400ms */ |
346 | for (timeout = 0; timeout < 40; timeout++) { |
347 | if (pv->established) |
348 | goto established; |
349 | if (!hvsi_get_packet(pv)) |
350 | maybe_msleep(ms: 10); |
351 | } |
352 | |
353 | if (!pv->established) { |
354 | pr_devel("HVSI@%x: Timeout handshaking, giving up !\n" , |
355 | pv->termno); |
356 | return; |
357 | } |
358 | established: |
359 | /* Query modem control lines */ |
360 | |
361 | pr_devel("HVSI@%x: ... established, reading mctrl\n" , pv->termno); |
362 | |
363 | hvsilib_read_mctrl(pv); |
364 | |
365 | /* Set our own DTR */ |
366 | |
367 | pr_devel("HVSI@%x: ... setting mctrl\n" , pv->termno); |
368 | |
369 | hvsilib_write_mctrl(pv, dtr: 1); |
370 | |
371 | /* Set the opened flag so reads are allowed */ |
372 | wmb(); |
373 | pv->opened = 1; |
374 | } |
375 | |
376 | int hvsilib_open(struct hvsi_priv *pv, struct hvc_struct *hp) |
377 | { |
378 | pr_devel("HVSI@%x: open !\n" , pv->termno); |
379 | |
380 | /* Keep track of the tty data structure */ |
381 | pv->tty = tty_port_tty_get(port: &hp->port); |
382 | |
383 | hvsilib_establish(pv); |
384 | |
385 | return 0; |
386 | } |
387 | |
388 | void hvsilib_close(struct hvsi_priv *pv, struct hvc_struct *hp) |
389 | { |
390 | unsigned long flags; |
391 | |
392 | pr_devel("HVSI@%x: close !\n" , pv->termno); |
393 | |
394 | if (!pv->is_console) { |
395 | pr_devel("HVSI@%x: Not a console, tearing down\n" , |
396 | pv->termno); |
397 | |
398 | /* Clear opened, synchronize with khvcd */ |
399 | spin_lock_irqsave(&hp->lock, flags); |
400 | pv->opened = 0; |
401 | spin_unlock_irqrestore(lock: &hp->lock, flags); |
402 | |
403 | /* Clear our own DTR */ |
404 | if (!pv->tty || (pv->tty->termios.c_cflag & HUPCL)) |
405 | hvsilib_write_mctrl(pv, dtr: 0); |
406 | |
407 | /* Tear down the connection */ |
408 | hvsi_send_close(pv); |
409 | } |
410 | |
411 | tty_kref_put(tty: pv->tty); |
412 | pv->tty = NULL; |
413 | } |
414 | |
415 | void hvsilib_init(struct hvsi_priv *pv, |
416 | ssize_t (*get_chars)(uint32_t termno, u8 *buf, size_t count), |
417 | ssize_t (*put_chars)(uint32_t termno, const u8 *buf, |
418 | size_t count), |
419 | int termno, int is_console) |
420 | { |
421 | memset(pv, 0, sizeof(*pv)); |
422 | pv->get_chars = get_chars; |
423 | pv->put_chars = put_chars; |
424 | pv->termno = termno; |
425 | pv->is_console = is_console; |
426 | } |
427 | |