drivers/tty/hvc/hvc_vio.c

   1 /*
   2  * vio driver interface to hvc_console.c
   3  *
   4  * This code was moved here to allow the remaining code to be reused as a
   5  * generic polling mode with semi-reliable transport driver core to the
   6  * console and tty subsystems.
   7  *
   8  *
   9  * Copyright (C) 2001 Anton Blanchard <anton@au.ibm.com>, IBM
  10  * Copyright (C) 2001 Paul Mackerras <paulus@au.ibm.com>, IBM
  11  * Copyright (C) 2004 Benjamin Herrenschmidt <benh@kernel.crashing.org>, IBM Corp.
  12  * Copyright (C) 2004 IBM Corporation
  13  *
  14  * Additional Author(s):
  15  *  Ryan S. Arnold <rsa@us.ibm.com>
  16  *
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 2 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful,
  23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  25  * GNU General Public License for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License
  28  * along with this program; if not, write to the Free Software
  29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  30  *
  31  * TODO:
  32  *
  33  *   - handle error in sending hvsi protocol packets
  34  *   - retry nego on subsequent sends ?
  35  */
  36
  37 #undef DEBUG
  38
  39 #include <linux/types.h>
  40 #include <linux/init.h>
  41 #include <linux/delay.h>
  42 #include <linux/slab.h>
  43 #include <linux/console.h>
  44
  45 #include <asm/hvconsole.h>
  46 #include <asm/vio.h>
  47 #include <asm/prom.h>
  48 #include <asm/firmware.h>
  49 #include <asm/hvsi.h>
  50 #include <asm/udbg.h>
  51
  52 #include "hvc_console.h"
  53
  54 static const char hvc_driver_name[] = "hvc_console";
  55
  56 static struct vio_device_id hvc_driver_table[] __devinitdata = {
  57         {"serial", "hvterm1"},
  58 #ifndef HVC_OLD_HVSI
  59         {"serial", "hvterm-protocol"},
  60 #endif
  61         { "", "" }
  62 };
  63 MODULE_DEVICE_TABLE(vio, hvc_driver_table);
  64
  65 typedef enum hv_protocol {
  66         HV_PROTOCOL_RAW,
  67         HV_PROTOCOL_HVSI
  68 } hv_protocol_t;
  69
  70 #define HV_INBUF_SIZE           255
  71
  72 struct hvterm_priv {
  73         u32             termno;         /* HV term number */
  74         hv_protocol_t   proto;          /* Raw data or HVSI packets */
  75         unsigned int    inbuf_len;      /* Data in input buffer */
  76         unsigned char   inbuf[HV_INBUF_SIZE];
  77         unsigned int    inbuf_cur;      /* Cursor in input buffer */
  78         unsigned int    inbuf_pktlen;   /* HVSI packet lenght from cursor */
  79         atomic_t        seqno;          /* HVSI packet sequence number */
  80         unsigned int    opened:1;       /* HVSI driver opened */
  81         unsigned int    established:1;  /* HVSI protocol established */
  82         unsigned int    is_console:1;   /* Used as a kernel console device */
  83         unsigned int    mctrl_update:1; /* HVSI modem control updated */
  84         unsigned short  mctrl;          /* HVSI modem control */
  85         struct tty_struct *tty;         /* TTY structure */
  86 };
  87 static struct hvterm_priv *hvterm_privs[MAX_NR_HVC_CONSOLES];
  88
  89 /* For early boot console */
  90 static struct hvterm_priv hvterm_priv0;
  91
  92 static int hvterm_raw_get_chars(uint32_t vtermno, char *buf, int count)
  93 {
  94         struct hvterm_priv *pv = hvterm_privs[vtermno];
  95         unsigned long got, i;
  96
  97         if (WARN_ON(!pv))
  98                 return 0;
  99
 100         /*
 101          * Vio firmware will read up to SIZE_VIO_GET_CHARS at its own discretion
 102          * so we play safe and avoid the situation where got > count which could
 103          * overload the flip buffer.
 104          */
 105         if (count < SIZE_VIO_GET_CHARS)
 106                 return -EAGAIN;
 107
 108         got = hvc_get_chars(pv->termno, buf, count);
 109
 110         /*
 111          * Work around a HV bug where it gives us a null
 112          * after every \r.  -- paulus
 113          */
 114         for (i = 1; i < got; ++i) {
 115                 if (buf[i] == 0 && buf[i-1] == '\r') {
 116                         --got;
 117                         if (i < got)
 118                                 memmove(&buf[i], &buf[i+1], got - i);
 119                 }
 120         }
 121         return got;
 122 }
 123
 124 static int hvterm_raw_put_chars(uint32_t vtermno, const char *buf, int count)
 125 {
 126         struct hvterm_priv *pv = hvterm_privs[vtermno];
 127
 128         if (WARN_ON(!pv))
 129                 return 0;
 130
 131         return hvc_put_chars(pv->termno, buf, count);
 132 }
 133
 134 static const struct hv_ops hvterm_raw_ops = {
 135         .get_chars = hvterm_raw_get_chars,
 136         .put_chars = hvterm_raw_put_chars,
 137         .notifier_add = notifier_add_irq,
 138         .notifier_del = notifier_del_irq,
 139         .notifier_hangup = notifier_hangup_irq,
 140 };
 141
 142 static int hvterm_hvsi_send_packet(struct hvterm_priv *pv, struct hvsi_header *packet)
 143 {
 144         packet->seqno = atomic_inc_return(&pv->seqno);
 145
 146         /* Assumes that always succeeds, works in practice */
 147         return hvc_put_chars(pv->termno, (char *)packet, packet->len);
 148 }
 149
 150 static void hvterm_hvsi_start_handshake(struct hvterm_priv *pv)
 151 {
 152         struct hvsi_query q;
 153
 154         /* Reset state */
 155         pv->established = 0;
 156         atomic_set(&pv->seqno, 0);
 157
 158         pr_devel("HVSI@%x: Handshaking started\n", pv->termno);
 159
 160         /* Send version query */
 161         q.hdr.type = VS_QUERY_PACKET_HEADER;
 162         q.hdr.len = sizeof(struct hvsi_query);
 163         q.verb = VSV_SEND_VERSION_NUMBER;
 164         hvterm_hvsi_send_packet(pv, &q.hdr);
 165 }
 166
 167 static int hvterm_hvsi_send_close(struct hvterm_priv *pv)
 168 {
 169         struct hvsi_control ctrl;
 170
 171         pv->established = 0;
 172
 173         ctrl.hdr.type = VS_CONTROL_PACKET_HEADER;
 174         ctrl.hdr.len = sizeof(struct hvsi_control);
 175         ctrl.verb = VSV_CLOSE_PROTOCOL;
 176         return hvterm_hvsi_send_packet(pv, &ctrl.hdr);
 177 }
 178
 179 static void hvterm_cd_change(struct hvterm_priv *pv, int cd)
 180 {
 181         if (cd)
 182                 pv->mctrl |= TIOCM_CD;
 183         else {
 184                 pv->mctrl &= ~TIOCM_CD;
 185
 186                 /* We copy the existing hvsi driver semantics
 187                  * here which are to trigger a hangup when
 188                  * we get a carrier loss.
 189                  * Closing our connection to the server will
 190                  * do just that.
 191                  */
 192                 if (!pv->is_console && pv->opened) {
 193                         pr_devel("HVSI@%x Carrier lost, hanging up !\n",
 194                                  pv->termno);
 195                         hvterm_hvsi_send_close(pv);
 196                 }
 197         }
 198 }
 199
 200 static void hvterm_hvsi_got_control(struct hvterm_priv *pv)
 201 {
 202         struct hvsi_control *pkt = (struct hvsi_control *)pv->inbuf;
 203
 204         switch (pkt->verb) {
 205         case VSV_CLOSE_PROTOCOL:
 206                 /* We restart the handshaking */
 207                 hvterm_hvsi_start_handshake(pv);
 208                 break;
 209         case VSV_MODEM_CTL_UPDATE:
 210                 /* Transition of carrier detect */
 211                 hvterm_cd_change(pv, pkt->word & HVSI_TSCD);
 212                 break;
 213         }
 214 }
 215
 216 static void hvterm_hvsi_got_query(struct hvterm_priv *pv)
 217 {
 218         struct hvsi_query *pkt = (struct hvsi_query *)pv->inbuf;
 219         struct hvsi_query_response r;
 220
 221         /* We only handle version queries */
 222         if (pkt->verb != VSV_SEND_VERSION_NUMBER)
 223                 return;
 224
 225         pr_devel("HVSI@%x: Got version query, sending response...\n",
 226                  pv->termno);
 227
 228         /* Send version response */
 229         r.hdr.type = VS_QUERY_RESPONSE_PACKET_HEADER;
 230         r.hdr.len = sizeof(struct hvsi_query_response);
 231         r.verb = VSV_SEND_VERSION_NUMBER;
 232         r.u.version = HVSI_VERSION;
 233         r.query_seqno = pkt->hdr.seqno;
 234         hvterm_hvsi_send_packet(pv, &r.hdr);
 235
 236         /* Assume protocol is open now */
 237         pv->established = 1;
 238 }
 239
 240 static void hvterm_hvsi_got_response(struct hvterm_priv *pv)
 241 {
 242         struct hvsi_query_response *r = (struct hvsi_query_response *)pv->inbuf;
 243
 244         switch(r->verb) {
 245         case VSV_SEND_MODEM_CTL_STATUS:
 246                 hvterm_cd_change(pv, r->u.mctrl_word & HVSI_TSCD);
 247                 pv->mctrl_update = 1;
 248                 break;
 249         }
 250 }
 251
 252 static int hvterm_hvsi_check_packet(struct hvterm_priv *pv)
 253 {
 254         u8 len, type;
 255
 256         /* Check header validity. If it's invalid, we ditch
 257          * the whole buffer and hope we eventually resync
 258          */
 259         if (pv->inbuf[0] < 0xfc) {
 260                 pv->inbuf_len = pv->inbuf_pktlen = 0;
 261                 return 0;
 262         }
 263         type = pv->inbuf[0];
 264         len = pv->inbuf[1];
 265
 266         /* Packet incomplete ? */
 267         if (pv->inbuf_len < len)
 268                 return 0;
 269
 270         pr_devel("HVSI@%x: Got packet type %x len %d bytes:\n",
 271                  pv->termno, type, len);
 272
 273         /* We have a packet, yay ! Handle it */
 274         switch(type) {
 275         case VS_DATA_PACKET_HEADER:
 276                 pv->inbuf_pktlen = len - 4;
 277                 pv->inbuf_cur = 4;
 278                 return 1;
 279         case VS_CONTROL_PACKET_HEADER:
 280                 hvterm_hvsi_got_control(pv);
 281                 break;
 282         case VS_QUERY_PACKET_HEADER:
 283                 hvterm_hvsi_got_query(pv);
 284                 break;
 285         case VS_QUERY_RESPONSE_PACKET_HEADER:
 286                 hvterm_hvsi_got_response(pv);
 287                 break;
 288         }
 289
 290         /* Swallow packet and retry */
 291         pv->inbuf_len -= len;
 292         memmove(pv->inbuf, &pv->inbuf[len], pv->inbuf_len);
 293         return 1;
 294 }
 295
 296 static int hvterm_hvsi_get_packet(struct hvterm_priv *pv)
 297 {
 298         /* If we have room in the buffer, ask HV for more */
 299         if (pv->inbuf_len < HV_INBUF_SIZE)
 300                 pv->inbuf_len += hvc_get_chars(pv->termno,
 301                                                &pv->inbuf[pv->inbuf_len],
 302                                                HV_INBUF_SIZE - pv->inbuf_len);
 303         /*
 304          * If we have at least 4 bytes in the buffer, check for
 305          * a full packet and retry
 306          */
 307         if (pv->inbuf_len >= 4)
 308                 return hvterm_hvsi_check_packet(pv);
 309         return 0;
 310 }
 311
 312 static int hvterm_hvsi_get_chars(uint32_t vtermno, char *buf, int count)
 313 {
 314         struct hvterm_priv *pv = hvterm_privs[vtermno];
 315         unsigned int tries, read = 0;
 316
 317         if (WARN_ON(!pv))
 318                 return 0;
 319
 320         /* If we aren't open, dont do anything in order to avoid races
 321          * with connection establishment. The hvc core will call this
 322          * before we have returned from notifier_add(), and we need to
 323          * avoid multiple users playing with the receive buffer
 324          */
 325         if (!pv->opened)
 326                 return 0;
 327
 328         /* We try twice, once with what data we have and once more
 329          * after we try to fetch some more from the hypervisor
 330          */
 331         for (tries = 1; count && tries < 2; tries++) {
 332                 /* Consume existing data packet */
 333                 if (pv->inbuf_pktlen) {
 334                         unsigned int l = min(count, (int)pv->inbuf_pktlen);
 335                         memcpy(&buf[read], &pv->inbuf[pv->inbuf_cur], l);
 336                         pv->inbuf_cur += l;
 337                         pv->inbuf_pktlen -= l;
 338                         count -= l;
 339                         read += l;
 340                 }
 341                 if (count == 0)
 342                         break;
 343
 344                 /* Data packet fully consumed, move down remaning data */
 345                 if (pv->inbuf_cur) {
 346                         pv->inbuf_len -= pv->inbuf_cur;
 347                         memmove(pv->inbuf, &pv->inbuf[pv->inbuf_cur], pv->inbuf_len);
 348                         pv->inbuf_cur = 0;
 349                 }
 350
 351                 /* Try to get another packet */
 352                 if (hvterm_hvsi_get_packet(pv))
 353                         tries--;
 354         }
 355         if (!pv->established) {
 356                 pr_devel("HVSI@%x: returning -EPIPE\n", pv->termno);
 357                 return -EPIPE;
 358         }
 359         return read;
 360 }
 361
 362 static int hvterm_hvsi_put_chars(uint32_t vtermno, const char *buf, int count)
 363 {
 364         struct hvterm_priv *pv = hvterm_privs[vtermno];
 365         struct hvsi_data dp;
 366         int rc, adjcount = min(count, HVSI_MAX_OUTGOING_DATA);
 367
 368         if (WARN_ON(!pv))
 369                 return 0;
 370
 371         dp.hdr.type = VS_DATA_PACKET_HEADER;
 372         dp.hdr.len = adjcount + sizeof(struct hvsi_header);
 373         memcpy(dp.data, buf, adjcount);
 374         rc = hvterm_hvsi_send_packet(pv, &dp.hdr);
 375         if (rc <= 0)
 376                 return rc;
 377         return adjcount;
 378 }
 379
 380 static void maybe_msleep(unsigned long ms)
 381 {
 382         /* During early boot, IRQs are disabled, use mdelay */
 383         if (irqs_disabled())
 384                 mdelay(ms);
 385         else
 386                 msleep(ms);
 387 }
 388
 389 static int hvterm_hvsi_read_mctrl(struct hvterm_priv *pv)
 390 {
 391         struct hvsi_query q;
 392         int rc, timeout;
 393
 394         pr_devel("HVSI@%x: Querying modem control status...\n",
 395                  pv->termno);
 396
 397         pv->mctrl_update = 0;
 398         q.hdr.type = VS_QUERY_PACKET_HEADER;
 399         q.hdr.len = sizeof(struct hvsi_query);
 400         q.hdr.seqno = atomic_inc_return(&pv->seqno);
 401         q.verb = VSV_SEND_MODEM_CTL_STATUS;
 402         rc = hvterm_hvsi_send_packet(pv, &q.hdr);
 403         if (rc <= 0) {
 404                 pr_devel("HVSI@%x: Error %d...\n", pv->termno, rc);
 405                 return rc;
 406         }
 407
 408         /* Try for up to 1s */
 409         for (timeout = 0; timeout < 1000; timeout++) {
 410                 if (!pv->established)
 411                         return -ENXIO;
 412                 if (pv->mctrl_update)
 413                         return 0;
 414                 if (!hvterm_hvsi_get_packet(pv))
 415                         maybe_msleep(1);
 416         }
 417         return -EIO;
 418 }
 419
 420 static int hvterm_hvsi_write_mctrl(struct hvterm_priv *pv, int dtr)
 421 {
 422         struct hvsi_control ctrl;
 423
 424         pr_devel("HVSI@%x: %s DTR...\n", pv->termno,
 425                  dtr ? "Setting" : "Clearing");
 426
 427         ctrl.hdr.type = VS_CONTROL_PACKET_HEADER,
 428         ctrl.hdr.len = sizeof(struct hvsi_control);
 429         ctrl.verb = VSV_SET_MODEM_CTL;
 430         ctrl.mask = HVSI_TSDTR;
 431         ctrl.word = dtr ? HVSI_TSDTR : 0;
 432         if (dtr)
 433                 pv->mctrl |= TIOCM_DTR;
 434         else
 435                 pv->mctrl &= ~TIOCM_DTR;
 436         return hvterm_hvsi_send_packet(pv, &ctrl.hdr);
 437 }
 438
 439 static void hvterm_hvsi_establish(struct hvterm_priv *pv)
 440 {
 441         int timeout;
 442
 443         /* Try for up to 10ms, there can be a packet to
 444          * start the process waiting for us...
 445          */
 446         for (timeout = 0; timeout < 10; timeout++) {
 447                 if (pv->established)
 448                         goto established;
 449                 if (!hvterm_hvsi_get_packet(pv))
 450                         maybe_msleep(1);
 451         }
 452
 453         /* Failed, send a close connection packet just
 454          * in case
 455          */
 456         hvterm_hvsi_send_close(pv);
 457
 458         /* Then restart handshake */
 459         hvterm_hvsi_start_handshake(pv);
 460
 461         /* Try for up to 100ms */
 462         for (timeout = 0; timeout < 100; timeout++) {
 463                 if (pv->established)
 464                         goto established;
 465                 if (!hvterm_hvsi_get_packet(pv))
 466                         maybe_msleep(1);
 467         }
 468
 469         if (!pv->established) {
 470                 pr_devel("HVSI@%x: Timeout handshaking, giving up !\n",
 471                          pv->termno);
 472                 return;
 473         }
 474  established:
 475         /* Query modem control lines */
 476         hvterm_hvsi_read_mctrl(pv);
 477
 478         /* Set our own DTR */
 479         hvterm_hvsi_write_mctrl(pv, 1);
 480
 481         /* Set the opened flag so reads are allowed */
 482         wmb();
 483         pv->opened = 1;
 484 }
 485
 486 static int hvterm_hvsi_open(struct hvc_struct *hp, int data)
 487 {
 488         struct hvterm_priv *pv = hvterm_privs[hp->vtermno];
 489         int rc;
 490
 491         pr_devel("HVSI@%x: open !\n", pv->termno);
 492
 493         rc = notifier_add_irq(hp, data);
 494         if (rc)
 495                 return rc;
 496
 497         /* Keep track of the tty data structure */
 498         pv->tty = tty_kref_get(hp->tty);
 499
 500         hvterm_hvsi_establish(pv);
 501         return 0;
 502 }
 503
 504 static void hvterm_hvsi_shutdown(struct hvc_struct *hp, struct hvterm_priv *pv)
 505 {
 506         unsigned long flags;
 507
 508         if (!pv->is_console) {
 509                 pr_devel("HVSI@%x: Not a console, tearing down\n",
 510                          pv->termno);
 511
 512                 /* Clear opened, synchronize with khvcd */
 513                 spin_lock_irqsave(&hp->lock, flags);
 514                 pv->opened = 0;
 515                 spin_unlock_irqrestore(&hp->lock, flags);
 516
 517                 /* Clear our own DTR */
 518                 if (!pv->tty || (pv->tty->termios->c_cflag & HUPCL))
 519                         hvterm_hvsi_write_mctrl(pv, 0);
 520
 521                 /* Tear down the connection */
 522                 hvterm_hvsi_send_close(pv);
 523         }
 524
 525         if (pv->tty)
 526                 tty_kref_put(pv->tty);
 527         pv->tty = NULL;
 528 }
 529
 530 static void hvterm_hvsi_close(struct hvc_struct *hp, int data)
 531 {
 532         struct hvterm_priv *pv = hvterm_privs[hp->vtermno];
 533
 534         pr_devel("HVSI@%x: close !\n", pv->termno);
 535
 536         hvterm_hvsi_shutdown(hp, pv);
 537
 538         notifier_del_irq(hp, data);
 539 }
 540
 541 void hvterm_hvsi_hangup(struct hvc_struct *hp, int data)
 542 {
 543         struct hvterm_priv *pv = hvterm_privs[hp->vtermno];
 544
 545         pr_devel("HVSI@%x: hangup !\n", pv->termno);
 546
 547         hvterm_hvsi_shutdown(hp, pv);
 548
 549         notifier_hangup_irq(hp, data);
 550 }
 551
 552 static int hvterm_hvsi_tiocmget(struct hvc_struct *hp)
 553 {
 554         struct hvterm_priv *pv = hvterm_privs[hp->vtermno];
 555
 556         if (!pv)
 557                 return -EINVAL;
 558         return pv->mctrl;
 559 }
 560
 561 static int hvterm_hvsi_tiocmset(struct hvc_struct *hp, unsigned int set,
 562                                 unsigned int clear)
 563 {
 564         struct hvterm_priv *pv = hvterm_privs[hp->vtermno];
 565
 566         pr_devel("HVSI@%x: Set modem control, set=%x,clr=%x\n",
 567                  pv->termno, set, clear);
 568
 569         if (set & TIOCM_DTR)
 570                 hvterm_hvsi_write_mctrl(pv, 1);
 571         else if (clear & TIOCM_DTR)
 572                 hvterm_hvsi_write_mctrl(pv, 0);
 573
 574         return 0;
 575 }
 576
 577 static const struct hv_ops hvterm_hvsi_ops = {
 578         .get_chars = hvterm_hvsi_get_chars,
 579         .put_chars = hvterm_hvsi_put_chars,
 580         .notifier_add = hvterm_hvsi_open,
 581         .notifier_del = hvterm_hvsi_close,
 582         .notifier_hangup = hvterm_hvsi_hangup,
 583         .tiocmget = hvterm_hvsi_tiocmget,
 584         .tiocmset = hvterm_hvsi_tiocmset,
 585 };
 586
 587 static int __devinit hvc_vio_probe(struct vio_dev *vdev,
 588                                    const struct vio_device_id *id)
 589 {
 590         const struct hv_ops *ops;
 591         struct hvc_struct *hp;
 592         struct hvterm_priv *pv;
 593         hv_protocol_t proto;
 594         int i, termno = -1;
 595
 596         /* probed with invalid parameters. */
 597         if (!vdev || !id)
 598                 return -EPERM;
 599
 600         if (of_device_is_compatible(vdev->dev.of_node, "hvterm1")) {
 601                 proto = HV_PROTOCOL_RAW;
 602                 ops = &hvterm_raw_ops;
 603         } else if (of_device_is_compatible(vdev->dev.of_node, "hvterm-protocol")) {
 604                 proto = HV_PROTOCOL_HVSI;
 605                 ops = &hvterm_hvsi_ops;
 606         } else {
 607                 pr_err("hvc_vio: Unkown protocol for %s\n", vdev->dev.of_node->full_name);
 608                 return -ENXIO;
 609         }
 610
 611         pr_devel("hvc_vio_probe() device %s, using %s protocol\n",
 612                  vdev->dev.of_node->full_name,
 613                  proto == HV_PROTOCOL_RAW ? "raw" : "hvsi");
 614
 615         /* Is it our boot one ? */
 616         if (hvterm_privs[0] == &hvterm_priv0 &&
 617             vdev->unit_address == hvterm_priv0.termno) {
 618                 pv = hvterm_privs[0];
 619                 termno = 0;
 620                 pr_devel("->boot console, using termno 0\n");
 621         }
 622         /* nope, allocate a new one */
 623         else {
 624                 for (i = 0; i < MAX_NR_HVC_CONSOLES && termno < 0; i++)
 625                         if (!hvterm_privs[i])
 626                                 termno = i;
 627                 pr_devel("->non-boot console, using termno %d\n", termno);
 628                 if (termno < 0)
 629                         return -ENODEV;
 630                 pv = kzalloc(sizeof(struct hvterm_priv), GFP_KERNEL);
 631                 if (!pv)
 632                         return -ENOMEM;
 633                 pv->termno = vdev->unit_address;
 634                 pv->proto = proto;
 635                 hvterm_privs[termno] = pv;
 636         }
 637
 638         hp = hvc_alloc(termno, vdev->irq, ops, MAX_VIO_PUT_CHARS);
 639         if (IS_ERR(hp))
 640                 return PTR_ERR(hp);
 641         dev_set_drvdata(&vdev->dev, hp);
 642
 643         return 0;
 644 }
 645
 646 static int __devexit hvc_vio_remove(struct vio_dev *vdev)
 647 {
 648         struct hvc_struct *hp = dev_get_drvdata(&vdev->dev);
 649         int rc, termno;
 650
 651         termno = hp->vtermno;
 652         rc = hvc_remove(hp);
 653         if (rc == 0) {
 654                 if (hvterm_privs[termno] != &hvterm_priv0)
 655                         kfree(hvterm_privs[termno]);
 656                 hvterm_privs[termno] = NULL;
 657         }
 658         return rc;
 659 }
 660
 661 static struct vio_driver hvc_vio_driver = {
 662         .id_table       = hvc_driver_table,
 663         .probe          = hvc_vio_probe,
 664         .remove         = __devexit_p(hvc_vio_remove),
 665         .driver         = {
 666                 .name   = hvc_driver_name,
 667                 .owner  = THIS_MODULE,
 668         }
 669 };
 670
 671 static int __init hvc_vio_init(void)
 672 {
 673         int rc;
 674
 675         if (firmware_has_feature(FW_FEATURE_ISERIES))
 676                 return -EIO;
 677
 678         /* Register as a vio device to receive callbacks */
 679         rc = vio_register_driver(&hvc_vio_driver);
 680
 681         return rc;
 682 }
 683 module_init(hvc_vio_init); /* after drivers/char/hvc_console.c */
 684
 685 static void __exit hvc_vio_exit(void)
 686 {
 687         vio_unregister_driver(&hvc_vio_driver);
 688 }
 689 module_exit(hvc_vio_exit);
 690
 691 static void udbg_hvc_putc(char c)
 692 {
 693         int count = -1;
 694
 695         if (c == '\n')
 696                 udbg_hvc_putc('\r');
 697
 698         do {
 699                 switch(hvterm_priv0.proto) {
 700                 case HV_PROTOCOL_RAW:
 701                         count = hvterm_raw_put_chars(0, &c, 1);
 702                         break;
 703                 case HV_PROTOCOL_HVSI:
 704                         count = hvterm_hvsi_put_chars(0, &c, 1);
 705                         break;
 706                 }
 707         } while(count == 0);
 708 }
 709
 710 static int udbg_hvc_getc_poll(void)
 711 {
 712         int rc = 0;
 713         char c;
 714
 715         switch(hvterm_priv0.proto) {
 716         case HV_PROTOCOL_RAW:
 717                 rc = hvterm_raw_get_chars(0, &c, 1);
 718                 break;
 719         case HV_PROTOCOL_HVSI:
 720                 rc = hvterm_hvsi_get_chars(0, &c, 1);
 721                 break;
 722         }
 723         if (!rc)
 724                 return -1;
 725         return c;
 726 }
 727
 728 static int udbg_hvc_getc(void)
 729 {
 730         int ch;
 731         for (;;) {
 732                 ch = udbg_hvc_getc_poll();
 733                 if (ch == -1) {
 734                         /* This shouldn't be needed...but... */
 735                         volatile unsigned long delay;
 736                         for (delay=0; delay < 2000000; delay++)
 737                                 ;
 738                 } else {
 739                         return ch;
 740                 }
 741         }
 742 }
 743
 744 void __init hvc_vio_init_early(void)
 745 {
 746         struct device_node *stdout_node;
 747         const u32 *termno;
 748         const char *name;
 749         const struct hv_ops *ops;
 750
 751         /* find the boot console from /chosen/stdout */
 752         if (!of_chosen)
 753                 return;
 754         name = of_get_property(of_chosen, "linux,stdout-path", NULL);
 755         if (name == NULL)
 756                 return;
 757         stdout_node = of_find_node_by_path(name);
 758         if (!stdout_node)
 759                 return;
 760         name = of_get_property(stdout_node, "name", NULL);
 761         if (!name) {
 762                 printk(KERN_WARNING "stdout node missing 'name' property!\n");
 763                 goto out;
 764         }
 765
 766         /* Check if it's a virtual terminal */
 767         if (strncmp(name, "vty", 3) != 0)
 768                 goto out;
 769         termno = of_get_property(stdout_node, "reg", NULL);
 770         if (termno == NULL)
 771                 goto out;
 772         hvterm_priv0.termno = *termno;
 773         hvterm_priv0.is_console = 1;
 774         hvterm_privs[0] = &hvterm_priv0;
 775
 776         /* Check the protocol */
 777         if (of_device_is_compatible(stdout_node, "hvterm1")) {
 778                 hvterm_priv0.proto = HV_PROTOCOL_RAW;
 779                 ops = &hvterm_raw_ops;
 780         }
 781         else if (of_device_is_compatible(stdout_node, "hvterm-protocol")) {
 782                 hvterm_priv0.proto = HV_PROTOCOL_HVSI;
 783                 ops = &hvterm_hvsi_ops;
 784                 /* HVSI, perform the handshake now */
 785                 hvterm_hvsi_establish(&hvterm_priv0);
 786         } else
 787                 goto out;
 788         udbg_putc = udbg_hvc_putc;
 789         udbg_getc = udbg_hvc_getc;
 790         udbg_getc_poll = udbg_hvc_getc_poll;
 791 #ifdef HVC_OLD_HVSI
 792         /* When using the old HVSI driver don't register the HVC
 793          * backend for HVSI, only do udbg
 794          */
 795         if (hvterm_priv0.proto == HV_PROTOCOL_HVSI)
 796                 goto out;
 797 #endif
 798         add_preferred_console("hvc", 0, NULL);
 799         hvc_instantiate(0, 0, ops);
 800 out:
 801         of_node_put(stdout_node);
 802 }
 803
 804 /* call this from early_init() for a working debug console on
 805  * vterm capable LPAR machines
 806  */
 807 #ifdef CONFIG_PPC_EARLY_DEBUG_LPAR
 808 void __init udbg_init_debug_lpar(void)
 809 {
 810         hvterm_privs[0] = &hvterm_priv0;
 811         hvterm_priv0.termno = 0;
 812         hvterm_priv0.proto = HV_PROTOCOL_RAW;
 813         hvterm_priv0.is_console = 1;
 814         udbg_putc = udbg_hvc_putc;
 815         udbg_getc = udbg_hvc_getc;
 816         udbg_getc_poll = udbg_hvc_getc_poll;
 817 }
 818 #endif /* CONFIG_PPC_EARLY_DEBUG_LPAR */
 819
 820 #ifdef CONFIG_PPC_EARLY_DEBUG_LPAR_HVSI
 821 void __init udbg_init_debug_lpar_hvsi(void)
 822 {
 823         hvterm_privs[0] = &hvterm_priv0;
 824         hvterm_priv0.termno = CONFIG_PPC_EARLY_DEBUG_HVSI_VTERMNO;
 825         hvterm_priv0.proto = HV_PROTOCOL_HVSI;
 826         hvterm_priv0.is_console = 1;
 827         udbg_putc = udbg_hvc_putc;
 828         udbg_getc = udbg_hvc_getc;
 829         udbg_getc_poll = udbg_hvc_getc_poll;
 830         hvterm_hvsi_establish(&hvterm_priv0);
 831 }
 832 #endif /* CONFIG_PPC_EARLY_DEBUG_LPAR_HVSI */