427 lines · c
1// SPDX-License-Identifier: GPL-2.02#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 10static 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 18static 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(&pv->seqno, 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, &q.hdr);33}34 35static 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, &ctrl.hdr);45}46 47static 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 semantics55 * here which are to trigger a hangup when56 * we get a carrier loss.57 * Closing our connection to the server will58 * 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 68static 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 84static 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, &r.hdr);103 104 /* Assume protocol is open now */105 pv->established = 1;106}107 108static 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 121static int hvsi_check_packet(struct hvsi_priv *pv)122{123 u8 len, type;124 125 /* Check header validity. If it's invalid, we ditch126 * the whole buffer and hope we eventually resync127 */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 165static 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 for174 * a full packet and retry175 */176 if (pv->inbuf_len >= 4)177 return hvsi_check_packet(pv);178 return 0;179}180 181ssize_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 races190 * with connection establishment. The hvc core will call this191 * before we have returned from notifier_add(), and we need to192 * avoid multiple users playing with the receive buffer193 */194 if (!pv->opened)195 return 0;196 197 /* We try twice, once with what data we have and once more198 * after we try to fetch some more from the hypervisor199 */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 232ssize_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, &dp.hdr);245 if (rc <= 0)246 return rc;247 return adjcount;248}249 250static void maybe_msleep(unsigned long ms)251{252 /* During early boot, IRQs are disabled, use mdelay */253 if (irqs_disabled())254 mdelay(ms);255 else256 msleep(ms);257}258 259int 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, &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(10);285 }286 return -EIO;287}288 289int 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 else298 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, &ctrl.hdr);312}313 314void 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 to321 * 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(10);328 }329 330 /* Failed, send a close connection packet just331 * in case332 */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(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, 1);370 371 /* Set the opened flag so reads are allowed */372 wmb();373 pv->opened = 1;374}375 376int 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(&hp->port);382 383 hvsilib_establish(pv);384 385 return 0;386}387 388void 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(&hp->lock, flags);402 403 /* Clear our own DTR */404 if (!pv->tty || (pv->tty->termios.c_cflag & HUPCL))405 hvsilib_write_mctrl(pv, 0);406 407 /* Tear down the connection */408 hvsi_send_close(pv);409 }410 411 tty_kref_put(pv->tty);412 pv->tty = NULL;413}414 415void 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