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, (char *)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 | int hvsilib_get_chars(struct hvsi_priv *pv, char *buf, int count) |
182 | { |
183 | unsigned int tries, read = 0; |
184 | |
185 | if (WARN_ON(!pv)) |
186 | return -ENXIO; |
187 | |
188 | /* If we aren't open, don't do anything in order to avoid races |
189 | * with connection establishment. The hvc core will call this |
190 | * before we have returned from notifier_add(), and we need to |
191 | * avoid multiple users playing with the receive buffer |
192 | */ |
193 | if (!pv->opened) |
194 | return 0; |
195 | |
196 | /* We try twice, once with what data we have and once more |
197 | * after we try to fetch some more from the hypervisor |
198 | */ |
199 | for (tries = 1; count && tries < 2; tries++) { |
200 | /* Consume existing data packet */ |
201 | if (pv->inbuf_pktlen) { |
202 | unsigned int l = min(count, (int)pv->inbuf_pktlen); |
203 | memcpy(&buf[read], &pv->inbuf[pv->inbuf_cur], l); |
204 | pv->inbuf_cur += l; |
205 | pv->inbuf_pktlen -= l; |
206 | count -= l; |
207 | read += l; |
208 | } |
209 | if (count == 0) |
210 | break; |
211 | |
212 | /* Data packet fully consumed, move down remaning data */ |
213 | if (pv->inbuf_cur) { |
214 | pv->inbuf_len -= pv->inbuf_cur; |
215 | memmove(pv->inbuf, &pv->inbuf[pv->inbuf_cur], |
216 | pv->inbuf_len); |
217 | pv->inbuf_cur = 0; |
218 | } |
219 | |
220 | /* Try to get another packet */ |
221 | if (hvsi_get_packet(pv)) |
222 | tries--; |
223 | } |
224 | if (!pv->established) { |
225 | pr_devel("HVSI@%x: returning -EPIPE\n" , pv->termno); |
226 | return -EPIPE; |
227 | } |
228 | return read; |
229 | } |
230 | |
231 | int hvsilib_put_chars(struct hvsi_priv *pv, const char *buf, int count) |
232 | { |
233 | struct hvsi_data dp; |
234 | int rc, adjcount = min(count, HVSI_MAX_OUTGOING_DATA); |
235 | |
236 | if (WARN_ON(!pv)) |
237 | return -ENODEV; |
238 | |
239 | dp.hdr.type = VS_DATA_PACKET_HEADER; |
240 | dp.hdr.len = adjcount + sizeof(struct hvsi_header); |
241 | memcpy(dp.data, buf, adjcount); |
242 | rc = hvsi_send_packet(pv, packet: &dp.hdr); |
243 | if (rc <= 0) |
244 | return rc; |
245 | return adjcount; |
246 | } |
247 | |
248 | static void maybe_msleep(unsigned long ms) |
249 | { |
250 | /* During early boot, IRQs are disabled, use mdelay */ |
251 | if (irqs_disabled()) |
252 | mdelay(ms); |
253 | else |
254 | msleep(msecs: ms); |
255 | } |
256 | |
257 | int hvsilib_read_mctrl(struct hvsi_priv *pv) |
258 | { |
259 | struct hvsi_query q; |
260 | int rc, timeout; |
261 | |
262 | pr_devel("HVSI@%x: Querying modem control status...\n" , |
263 | pv->termno); |
264 | |
265 | pv->mctrl_update = 0; |
266 | q.hdr.type = VS_QUERY_PACKET_HEADER; |
267 | q.hdr.len = sizeof(struct hvsi_query); |
268 | q.verb = cpu_to_be16(VSV_SEND_MODEM_CTL_STATUS); |
269 | rc = hvsi_send_packet(pv, packet: &q.hdr); |
270 | if (rc <= 0) { |
271 | pr_devel("HVSI@%x: Error %d...\n" , pv->termno, rc); |
272 | return rc; |
273 | } |
274 | |
275 | /* Try for up to 200ms */ |
276 | for (timeout = 0; timeout < 20; timeout++) { |
277 | if (!pv->established) |
278 | return -ENXIO; |
279 | if (pv->mctrl_update) |
280 | return 0; |
281 | if (!hvsi_get_packet(pv)) |
282 | maybe_msleep(ms: 10); |
283 | } |
284 | return -EIO; |
285 | } |
286 | |
287 | int hvsilib_write_mctrl(struct hvsi_priv *pv, int dtr) |
288 | { |
289 | struct hvsi_control ctrl; |
290 | unsigned short mctrl; |
291 | |
292 | mctrl = pv->mctrl; |
293 | if (dtr) |
294 | mctrl |= TIOCM_DTR; |
295 | else |
296 | mctrl &= ~TIOCM_DTR; |
297 | if (mctrl == pv->mctrl) |
298 | return 0; |
299 | pv->mctrl = mctrl; |
300 | |
301 | pr_devel("HVSI@%x: %s DTR...\n" , pv->termno, |
302 | dtr ? "Setting" : "Clearing" ); |
303 | |
304 | ctrl.hdr.type = VS_CONTROL_PACKET_HEADER, |
305 | ctrl.hdr.len = sizeof(struct hvsi_control); |
306 | ctrl.verb = cpu_to_be16(VSV_SET_MODEM_CTL); |
307 | ctrl.mask = cpu_to_be32(HVSI_TSDTR); |
308 | ctrl.word = cpu_to_be32(dtr ? HVSI_TSDTR : 0); |
309 | return hvsi_send_packet(pv, packet: &ctrl.hdr); |
310 | } |
311 | |
312 | void hvsilib_establish(struct hvsi_priv *pv) |
313 | { |
314 | int timeout; |
315 | |
316 | pr_devel("HVSI@%x: Establishing...\n" , pv->termno); |
317 | |
318 | /* Try for up to 200ms, there can be a packet to |
319 | * start the process waiting for us... |
320 | */ |
321 | for (timeout = 0; timeout < 20; timeout++) { |
322 | if (pv->established) |
323 | goto established; |
324 | if (!hvsi_get_packet(pv)) |
325 | maybe_msleep(ms: 10); |
326 | } |
327 | |
328 | /* Failed, send a close connection packet just |
329 | * in case |
330 | */ |
331 | pr_devel("HVSI@%x: ... sending close\n" , pv->termno); |
332 | |
333 | hvsi_send_close(pv); |
334 | |
335 | /* Then restart handshake */ |
336 | |
337 | pr_devel("HVSI@%x: ... restarting handshake\n" , pv->termno); |
338 | |
339 | hvsi_start_handshake(pv); |
340 | |
341 | pr_devel("HVSI@%x: ... waiting handshake\n" , pv->termno); |
342 | |
343 | /* Try for up to 400ms */ |
344 | for (timeout = 0; timeout < 40; timeout++) { |
345 | if (pv->established) |
346 | goto established; |
347 | if (!hvsi_get_packet(pv)) |
348 | maybe_msleep(ms: 10); |
349 | } |
350 | |
351 | if (!pv->established) { |
352 | pr_devel("HVSI@%x: Timeout handshaking, giving up !\n" , |
353 | pv->termno); |
354 | return; |
355 | } |
356 | established: |
357 | /* Query modem control lines */ |
358 | |
359 | pr_devel("HVSI@%x: ... established, reading mctrl\n" , pv->termno); |
360 | |
361 | hvsilib_read_mctrl(pv); |
362 | |
363 | /* Set our own DTR */ |
364 | |
365 | pr_devel("HVSI@%x: ... setting mctrl\n" , pv->termno); |
366 | |
367 | hvsilib_write_mctrl(pv, dtr: 1); |
368 | |
369 | /* Set the opened flag so reads are allowed */ |
370 | wmb(); |
371 | pv->opened = 1; |
372 | } |
373 | |
374 | int hvsilib_open(struct hvsi_priv *pv, struct hvc_struct *hp) |
375 | { |
376 | pr_devel("HVSI@%x: open !\n" , pv->termno); |
377 | |
378 | /* Keep track of the tty data structure */ |
379 | pv->tty = tty_port_tty_get(port: &hp->port); |
380 | |
381 | hvsilib_establish(pv); |
382 | |
383 | return 0; |
384 | } |
385 | |
386 | void hvsilib_close(struct hvsi_priv *pv, struct hvc_struct *hp) |
387 | { |
388 | unsigned long flags; |
389 | |
390 | pr_devel("HVSI@%x: close !\n" , pv->termno); |
391 | |
392 | if (!pv->is_console) { |
393 | pr_devel("HVSI@%x: Not a console, tearing down\n" , |
394 | pv->termno); |
395 | |
396 | /* Clear opened, synchronize with khvcd */ |
397 | spin_lock_irqsave(&hp->lock, flags); |
398 | pv->opened = 0; |
399 | spin_unlock_irqrestore(lock: &hp->lock, flags); |
400 | |
401 | /* Clear our own DTR */ |
402 | if (!pv->tty || (pv->tty->termios.c_cflag & HUPCL)) |
403 | hvsilib_write_mctrl(pv, dtr: 0); |
404 | |
405 | /* Tear down the connection */ |
406 | hvsi_send_close(pv); |
407 | } |
408 | |
409 | tty_kref_put(tty: pv->tty); |
410 | pv->tty = NULL; |
411 | } |
412 | |
413 | void hvsilib_init(struct hvsi_priv *pv, |
414 | int (*get_chars)(uint32_t termno, char *buf, int count), |
415 | int (*put_chars)(uint32_t termno, const char *buf, |
416 | int count), |
417 | int termno, int is_console) |
418 | { |
419 | memset(pv, 0, sizeof(*pv)); |
420 | pv->get_chars = get_chars; |
421 | pv->put_chars = put_chars; |
422 | pv->termno = termno; |
423 | pv->is_console = is_console; |
424 | } |
425 | |