linux/drivers/tty/mips_ejtag_fdc.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * TTY driver for MIPS EJTAG Fast Debug Channels.
   4 *
   5 * Copyright (C) 2007-2015 Imagination Technologies Ltd
   6 */
   7
   8#include <linux/atomic.h>
   9#include <linux/bitops.h>
  10#include <linux/completion.h>
  11#include <linux/console.h>
  12#include <linux/delay.h>
  13#include <linux/export.h>
  14#include <linux/init.h>
  15#include <linux/interrupt.h>
  16#include <linux/kernel.h>
  17#include <linux/kgdb.h>
  18#include <linux/kthread.h>
  19#include <linux/sched.h>
  20#include <linux/serial.h>
  21#include <linux/serial_core.h>
  22#include <linux/slab.h>
  23#include <linux/spinlock.h>
  24#include <linux/string.h>
  25#include <linux/timer.h>
  26#include <linux/tty.h>
  27#include <linux/tty_driver.h>
  28#include <linux/tty_flip.h>
  29#include <linux/uaccess.h>
  30
  31#include <asm/cdmm.h>
  32#include <asm/irq.h>
  33
  34/* Register offsets */
  35#define REG_FDACSR      0x00    /* FDC Access Control and Status Register */
  36#define REG_FDCFG       0x08    /* FDC Configuration Register */
  37#define REG_FDSTAT      0x10    /* FDC Status Register */
  38#define REG_FDRX        0x18    /* FDC Receive Register */
  39#define REG_FDTX(N)     (0x20+0x8*(N))  /* FDC Transmit Register n (0..15) */
  40
  41/* Register fields */
  42
  43#define REG_FDCFG_TXINTTHRES_SHIFT      18
  44#define REG_FDCFG_TXINTTHRES            (0x3 << REG_FDCFG_TXINTTHRES_SHIFT)
  45#define REG_FDCFG_TXINTTHRES_DISABLED   (0x0 << REG_FDCFG_TXINTTHRES_SHIFT)
  46#define REG_FDCFG_TXINTTHRES_EMPTY      (0x1 << REG_FDCFG_TXINTTHRES_SHIFT)
  47#define REG_FDCFG_TXINTTHRES_NOTFULL    (0x2 << REG_FDCFG_TXINTTHRES_SHIFT)
  48#define REG_FDCFG_TXINTTHRES_NEAREMPTY  (0x3 << REG_FDCFG_TXINTTHRES_SHIFT)
  49#define REG_FDCFG_RXINTTHRES_SHIFT      16
  50#define REG_FDCFG_RXINTTHRES            (0x3 << REG_FDCFG_RXINTTHRES_SHIFT)
  51#define REG_FDCFG_RXINTTHRES_DISABLED   (0x0 << REG_FDCFG_RXINTTHRES_SHIFT)
  52#define REG_FDCFG_RXINTTHRES_FULL       (0x1 << REG_FDCFG_RXINTTHRES_SHIFT)
  53#define REG_FDCFG_RXINTTHRES_NOTEMPTY   (0x2 << REG_FDCFG_RXINTTHRES_SHIFT)
  54#define REG_FDCFG_RXINTTHRES_NEARFULL   (0x3 << REG_FDCFG_RXINTTHRES_SHIFT)
  55#define REG_FDCFG_TXFIFOSIZE_SHIFT      8
  56#define REG_FDCFG_TXFIFOSIZE            (0xff << REG_FDCFG_TXFIFOSIZE_SHIFT)
  57#define REG_FDCFG_RXFIFOSIZE_SHIFT      0
  58#define REG_FDCFG_RXFIFOSIZE            (0xff << REG_FDCFG_RXFIFOSIZE_SHIFT)
  59
  60#define REG_FDSTAT_TXCOUNT_SHIFT        24
  61#define REG_FDSTAT_TXCOUNT              (0xff << REG_FDSTAT_TXCOUNT_SHIFT)
  62#define REG_FDSTAT_RXCOUNT_SHIFT        16
  63#define REG_FDSTAT_RXCOUNT              (0xff << REG_FDSTAT_RXCOUNT_SHIFT)
  64#define REG_FDSTAT_RXCHAN_SHIFT         4
  65#define REG_FDSTAT_RXCHAN               (0xf << REG_FDSTAT_RXCHAN_SHIFT)
  66#define REG_FDSTAT_RXE                  BIT(3)  /* Rx Empty */
  67#define REG_FDSTAT_RXF                  BIT(2)  /* Rx Full */
  68#define REG_FDSTAT_TXE                  BIT(1)  /* Tx Empty */
  69#define REG_FDSTAT_TXF                  BIT(0)  /* Tx Full */
  70
  71/* Default channel for the early console */
  72#define CONSOLE_CHANNEL      1
  73
  74#define NUM_TTY_CHANNELS     16
  75
  76#define RX_BUF_SIZE 1024
  77
  78/*
  79 * When the IRQ is unavailable, the FDC state must be polled for incoming data
  80 * and space becoming available in TX FIFO.
  81 */
  82#define FDC_TTY_POLL (HZ / 50)
  83
  84struct mips_ejtag_fdc_tty;
  85
  86/**
  87 * struct mips_ejtag_fdc_tty_port - Wrapper struct for FDC tty_port.
  88 * @port:               TTY port data
  89 * @driver:             TTY driver.
  90 * @rx_lock:            Lock for rx_buf.
  91 *                      This protects between the hard interrupt and user
  92 *                      context. It's also held during read SWITCH operations.
  93 * @rx_buf:             Read buffer.
  94 * @xmit_lock:          Lock for xmit_*, and port.xmit_buf.
  95 *                      This protects between user context and kernel thread.
  96 *                      It is used from chars_in_buffer()/write_room() TTY
  97 *                      callbacks which are used during wait operations, so a
  98 *                      mutex is unsuitable.
  99 * @xmit_cnt:           Size of xmit buffer contents.
 100 * @xmit_head:          Head of xmit buffer where data is written.
 101 * @xmit_tail:          Tail of xmit buffer where data is read.
 102 * @xmit_empty:         Completion for xmit buffer being empty.
 103 */
 104struct mips_ejtag_fdc_tty_port {
 105        struct tty_port                  port;
 106        struct mips_ejtag_fdc_tty       *driver;
 107        raw_spinlock_t                   rx_lock;
 108        void                            *rx_buf;
 109        spinlock_t                       xmit_lock;
 110        unsigned int                     xmit_cnt;
 111        unsigned int                     xmit_head;
 112        unsigned int                     xmit_tail;
 113        struct completion                xmit_empty;
 114};
 115
 116/**
 117 * struct mips_ejtag_fdc_tty - Driver data for FDC as a whole.
 118 * @dev:                FDC device (for dev_*() logging).
 119 * @driver:             TTY driver.
 120 * @cpu:                CPU number for this FDC.
 121 * @fdc_name:           FDC name (not for base of channel names).
 122 * @driver_name:        Base of driver name.
 123 * @ports:              Per-channel data.
 124 * @waitqueue:          Wait queue for waiting for TX data, or for space in TX
 125 *                      FIFO.
 126 * @lock:               Lock to protect FDCFG (interrupt enable).
 127 * @thread:             KThread for writing out data to FDC.
 128 * @reg:                FDC registers.
 129 * @tx_fifo:            TX FIFO size.
 130 * @xmit_size:          Size of each port's xmit buffer.
 131 * @xmit_total:         Total number of bytes (from all ports) to transmit.
 132 * @xmit_next:          Next port number to transmit from (round robin).
 133 * @xmit_full:          Indicates TX FIFO is full, we're waiting for space.
 134 * @irq:                IRQ number (negative if no IRQ).
 135 * @removing:           Indicates the device is being removed and @poll_timer
 136 *                      should not be restarted.
 137 * @poll_timer:         Timer for polling for interrupt events when @irq < 0.
 138 * @sysrq_pressed:      Whether the magic sysrq key combination has been
 139 *                      detected. See mips_ejtag_fdc_handle().
 140 */
 141struct mips_ejtag_fdc_tty {
 142        struct device                   *dev;
 143        struct tty_driver               *driver;
 144        unsigned int                     cpu;
 145        char                             fdc_name[16];
 146        char                             driver_name[16];
 147        struct mips_ejtag_fdc_tty_port   ports[NUM_TTY_CHANNELS];
 148        wait_queue_head_t                waitqueue;
 149        raw_spinlock_t                   lock;
 150        struct task_struct              *thread;
 151
 152        void __iomem                    *reg;
 153        u8                               tx_fifo;
 154
 155        unsigned int                     xmit_size;
 156        atomic_t                         xmit_total;
 157        unsigned int                     xmit_next;
 158        bool                             xmit_full;
 159
 160        int                              irq;
 161        bool                             removing;
 162        struct timer_list                poll_timer;
 163
 164#ifdef CONFIG_MAGIC_SYSRQ
 165        bool                             sysrq_pressed;
 166#endif
 167};
 168
 169/* Hardware access */
 170
 171static inline void mips_ejtag_fdc_write(struct mips_ejtag_fdc_tty *priv,
 172                                        unsigned int offs, unsigned int data)
 173{
 174        __raw_writel(data, priv->reg + offs);
 175}
 176
 177static inline unsigned int mips_ejtag_fdc_read(struct mips_ejtag_fdc_tty *priv,
 178                                               unsigned int offs)
 179{
 180        return __raw_readl(priv->reg + offs);
 181}
 182
 183/* Encoding of byte stream in FDC words */
 184
 185/**
 186 * struct fdc_word - FDC word encoding some number of bytes of data.
 187 * @word:               Raw FDC word.
 188 * @bytes:              Number of bytes encoded by @word.
 189 */
 190struct fdc_word {
 191        u32             word;
 192        unsigned int    bytes;
 193};
 194
 195/*
 196 * This is a compact encoding which allows every 1 byte, 2 byte, and 3 byte
 197 * sequence to be encoded in a single word, while allowing the majority of 4
 198 * byte sequences (including all ASCII and common binary data) to be encoded in
 199 * a single word too.
 200 *    _______________________ _____________
 201 *   |       FDC Word        |             |
 202 *   |31-24|23-16|15-8 | 7-0 |    Bytes    |
 203 *   |_____|_____|_____|_____|_____________|
 204 *   |     |     |     |     |             |
 205 *   |0x80 |0x80 |0x80 |  WW | WW          |
 206 *   |0x81 |0x81 |  XX |  WW | WW XX       |
 207 *   |0x82 |  YY |  XX |  WW | WW XX YY    |
 208 *   |  ZZ |  YY |  XX |  WW | WW XX YY ZZ |
 209 *   |_____|_____|_____|_____|_____________|
 210 *
 211 * Note that the 4-byte encoding can only be used where none of the other 3
 212 * encodings match, otherwise it must fall back to the 3 byte encoding.
 213 */
 214
 215/* ranges >= 1 && sizes[0] >= 1 */
 216static struct fdc_word mips_ejtag_fdc_encode(const char **ptrs,
 217                                             unsigned int *sizes,
 218                                             unsigned int ranges)
 219{
 220        struct fdc_word word = { 0, 0 };
 221        const char **ptrs_end = ptrs + ranges;
 222
 223        for (; ptrs < ptrs_end; ++ptrs) {
 224                const char *ptr = *(ptrs++);
 225                const char *end = ptr + *(sizes++);
 226
 227                for (; ptr < end; ++ptr) {
 228                        word.word |= (u8)*ptr << (8*word.bytes);
 229                        ++word.bytes;
 230                        if (word.bytes == 4)
 231                                goto done;
 232                }
 233        }
 234done:
 235        /* Choose the appropriate encoding */
 236        switch (word.bytes) {
 237        case 4:
 238                /* 4 byte encoding, but don't match the 1-3 byte encodings */
 239                if ((word.word >> 8) != 0x808080 &&
 240                    (word.word >> 16) != 0x8181 &&
 241                    (word.word >> 24) != 0x82)
 242                        break;
 243                /* Fall back to a 3 byte encoding */
 244                word.bytes = 3;
 245                word.word &= 0x00ffffff;
 246                fallthrough;
 247        case 3:
 248                /* 3 byte encoding */
 249                word.word |= 0x82000000;
 250                break;
 251        case 2:
 252                /* 2 byte encoding */
 253                word.word |= 0x81810000;
 254                break;
 255        case 1:
 256                /* 1 byte encoding */
 257                word.word |= 0x80808000;
 258                break;
 259        }
 260        return word;
 261}
 262
 263static unsigned int mips_ejtag_fdc_decode(u32 word, char *buf)
 264{
 265        buf[0] = (u8)word;
 266        word >>= 8;
 267        if (word == 0x808080)
 268                return 1;
 269        buf[1] = (u8)word;
 270        word >>= 8;
 271        if (word == 0x8181)
 272                return 2;
 273        buf[2] = (u8)word;
 274        word >>= 8;
 275        if (word == 0x82)
 276                return 3;
 277        buf[3] = (u8)word;
 278        return 4;
 279}
 280
 281/* Console operations */
 282
 283/**
 284 * struct mips_ejtag_fdc_console - Wrapper struct for FDC consoles.
 285 * @cons:               Console object.
 286 * @tty_drv:            TTY driver associated with this console.
 287 * @lock:               Lock to protect concurrent access to other fields.
 288 *                      This is raw because it may be used very early.
 289 * @initialised:        Whether the console is initialised.
 290 * @regs:               Registers base address for each CPU.
 291 */
 292struct mips_ejtag_fdc_console {
 293        struct console           cons;
 294        struct tty_driver       *tty_drv;
 295        raw_spinlock_t           lock;
 296        bool                     initialised;
 297        void __iomem            *regs[NR_CPUS];
 298};
 299
 300/* Low level console write shared by early console and normal console */
 301static void mips_ejtag_fdc_console_write(struct console *c, const char *s,
 302                                         unsigned int count)
 303{
 304        struct mips_ejtag_fdc_console *cons =
 305                container_of(c, struct mips_ejtag_fdc_console, cons);
 306        void __iomem *regs;
 307        struct fdc_word word;
 308        unsigned long flags;
 309        unsigned int i, buf_len, cpu;
 310        bool done_cr = false;
 311        char buf[4];
 312        const char *buf_ptr = buf;
 313        /* Number of bytes of input data encoded up to each byte in buf */
 314        u8 inc[4];
 315
 316        local_irq_save(flags);
 317        cpu = smp_processor_id();
 318        regs = cons->regs[cpu];
 319        /* First console output on this CPU? */
 320        if (!regs) {
 321                regs = mips_cdmm_early_probe(0xfd);
 322                cons->regs[cpu] = regs;
 323        }
 324        /* Already tried and failed to find FDC on this CPU? */
 325        if (IS_ERR(regs))
 326                goto out;
 327        while (count) {
 328                /*
 329                 * Copy the next few characters to a buffer so we can inject
 330                 * carriage returns before newlines.
 331                 */
 332                for (buf_len = 0, i = 0; buf_len < 4 && i < count; ++buf_len) {
 333                        if (s[i] == '\n' && !done_cr) {
 334                                buf[buf_len] = '\r';
 335                                done_cr = true;
 336                        } else {
 337                                buf[buf_len] = s[i];
 338                                done_cr = false;
 339                                ++i;
 340                        }
 341                        inc[buf_len] = i;
 342                }
 343                word = mips_ejtag_fdc_encode(&buf_ptr, &buf_len, 1);
 344                count -= inc[word.bytes - 1];
 345                s += inc[word.bytes - 1];
 346
 347                /* Busy wait until there's space in fifo */
 348                while (__raw_readl(regs + REG_FDSTAT) & REG_FDSTAT_TXF)
 349                        ;
 350                __raw_writel(word.word, regs + REG_FDTX(c->index));
 351        }
 352out:
 353        local_irq_restore(flags);
 354}
 355
 356static struct tty_driver *mips_ejtag_fdc_console_device(struct console *c,
 357                                                        int *index)
 358{
 359        struct mips_ejtag_fdc_console *cons =
 360                container_of(c, struct mips_ejtag_fdc_console, cons);
 361
 362        *index = c->index;
 363        return cons->tty_drv;
 364}
 365
 366/* Initialise an FDC console (early or normal */
 367static int __init mips_ejtag_fdc_console_init(struct mips_ejtag_fdc_console *c)
 368{
 369        void __iomem *regs;
 370        unsigned long flags;
 371        int ret = 0;
 372
 373        raw_spin_lock_irqsave(&c->lock, flags);
 374        /* Don't init twice */
 375        if (c->initialised)
 376                goto out;
 377        /* Look for the FDC device */
 378        regs = mips_cdmm_early_probe(0xfd);
 379        if (IS_ERR(regs)) {
 380                ret = PTR_ERR(regs);
 381                goto out;
 382        }
 383
 384        c->initialised = true;
 385        c->regs[smp_processor_id()] = regs;
 386        register_console(&c->cons);
 387out:
 388        raw_spin_unlock_irqrestore(&c->lock, flags);
 389        return ret;
 390}
 391
 392static struct mips_ejtag_fdc_console mips_ejtag_fdc_con = {
 393        .cons   = {
 394                .name   = "fdc",
 395                .write  = mips_ejtag_fdc_console_write,
 396                .device = mips_ejtag_fdc_console_device,
 397                .flags  = CON_PRINTBUFFER,
 398                .index  = -1,
 399        },
 400        .lock   = __RAW_SPIN_LOCK_UNLOCKED(mips_ejtag_fdc_con.lock),
 401};
 402
 403/* TTY RX/TX operations */
 404
 405/**
 406 * mips_ejtag_fdc_put_chan() - Write out a block of channel data.
 407 * @priv:       Pointer to driver private data.
 408 * @chan:       Channel number.
 409 *
 410 * Write a single block of data out to the debug adapter. If the circular buffer
 411 * is wrapped then only the first block is written.
 412 *
 413 * Returns:     The number of bytes that were written.
 414 */
 415static unsigned int mips_ejtag_fdc_put_chan(struct mips_ejtag_fdc_tty *priv,
 416                                            unsigned int chan)
 417{
 418        struct mips_ejtag_fdc_tty_port *dport;
 419        struct tty_struct *tty;
 420        const char *ptrs[2];
 421        unsigned int sizes[2] = { 0 };
 422        struct fdc_word word = { .bytes = 0 };
 423        unsigned long flags;
 424
 425        dport = &priv->ports[chan];
 426        spin_lock(&dport->xmit_lock);
 427        if (dport->xmit_cnt) {
 428                ptrs[0] = dport->port.xmit_buf + dport->xmit_tail;
 429                sizes[0] = min_t(unsigned int,
 430                                 priv->xmit_size - dport->xmit_tail,
 431                                 dport->xmit_cnt);
 432                ptrs[1] = dport->port.xmit_buf;
 433                sizes[1] = dport->xmit_cnt - sizes[0];
 434                word = mips_ejtag_fdc_encode(ptrs, sizes, 1 + !!sizes[1]);
 435
 436                dev_dbg(priv->dev, "%s%u: out %08x: \"%*pE%*pE\"\n",
 437                        priv->driver_name, chan, word.word,
 438                        min_t(int, word.bytes, sizes[0]), ptrs[0],
 439                        max_t(int, 0, word.bytes - sizes[0]), ptrs[1]);
 440
 441                local_irq_save(flags);
 442                /* Maybe we raced with the console and TX FIFO is full */
 443                if (mips_ejtag_fdc_read(priv, REG_FDSTAT) & REG_FDSTAT_TXF)
 444                        word.bytes = 0;
 445                else
 446                        mips_ejtag_fdc_write(priv, REG_FDTX(chan), word.word);
 447                local_irq_restore(flags);
 448
 449                dport->xmit_cnt -= word.bytes;
 450                if (!dport->xmit_cnt) {
 451                        /* Reset pointers to avoid wraps */
 452                        dport->xmit_head = 0;
 453                        dport->xmit_tail = 0;
 454                        complete(&dport->xmit_empty);
 455                } else {
 456                        dport->xmit_tail += word.bytes;
 457                        if (dport->xmit_tail >= priv->xmit_size)
 458                                dport->xmit_tail -= priv->xmit_size;
 459                }
 460                atomic_sub(word.bytes, &priv->xmit_total);
 461        }
 462        spin_unlock(&dport->xmit_lock);
 463
 464        /* If we've made more data available, wake up tty */
 465        if (sizes[0] && word.bytes) {
 466                tty = tty_port_tty_get(&dport->port);
 467                if (tty) {
 468                        tty_wakeup(tty);
 469                        tty_kref_put(tty);
 470                }
 471        }
 472
 473        return word.bytes;
 474}
 475
 476/**
 477 * mips_ejtag_fdc_put() - Kernel thread to write out channel data to FDC.
 478 * @arg:        Driver pointer.
 479 *
 480 * This kernel thread runs while @priv->xmit_total != 0, and round robins the
 481 * channels writing out blocks of buffered data to the FDC TX FIFO.
 482 */
 483static int mips_ejtag_fdc_put(void *arg)
 484{
 485        struct mips_ejtag_fdc_tty *priv = arg;
 486        struct mips_ejtag_fdc_tty_port *dport;
 487        unsigned int ret;
 488        u32 cfg;
 489
 490        __set_current_state(TASK_RUNNING);
 491        while (!kthread_should_stop()) {
 492                /* Wait for data to actually write */
 493                wait_event_interruptible(priv->waitqueue,
 494                                         atomic_read(&priv->xmit_total) ||
 495                                         kthread_should_stop());
 496                if (kthread_should_stop())
 497                        break;
 498
 499                /* Wait for TX FIFO space to write data */
 500                raw_spin_lock_irq(&priv->lock);
 501                if (mips_ejtag_fdc_read(priv, REG_FDSTAT) & REG_FDSTAT_TXF) {
 502                        priv->xmit_full = true;
 503                        if (priv->irq >= 0) {
 504                                /* Enable TX interrupt */
 505                                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
 506                                cfg &= ~REG_FDCFG_TXINTTHRES;
 507                                cfg |= REG_FDCFG_TXINTTHRES_NOTFULL;
 508                                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
 509                        }
 510                }
 511                raw_spin_unlock_irq(&priv->lock);
 512                wait_event_interruptible(priv->waitqueue,
 513                                         !(mips_ejtag_fdc_read(priv, REG_FDSTAT)
 514                                           & REG_FDSTAT_TXF) ||
 515                                         kthread_should_stop());
 516                if (kthread_should_stop())
 517                        break;
 518
 519                /* Find next channel with data to output */
 520                for (;;) {
 521                        dport = &priv->ports[priv->xmit_next];
 522                        spin_lock(&dport->xmit_lock);
 523                        ret = dport->xmit_cnt;
 524                        spin_unlock(&dport->xmit_lock);
 525                        if (ret)
 526                                break;
 527                        /* Round robin */
 528                        ++priv->xmit_next;
 529                        if (priv->xmit_next >= NUM_TTY_CHANNELS)
 530                                priv->xmit_next = 0;
 531                }
 532
 533                /* Try writing data to the chosen channel */
 534                ret = mips_ejtag_fdc_put_chan(priv, priv->xmit_next);
 535
 536                /*
 537                 * If anything was output, move on to the next channel so as not
 538                 * to starve other channels.
 539                 */
 540                if (ret) {
 541                        ++priv->xmit_next;
 542                        if (priv->xmit_next >= NUM_TTY_CHANNELS)
 543                                priv->xmit_next = 0;
 544                }
 545        }
 546
 547        return 0;
 548}
 549
 550/**
 551 * mips_ejtag_fdc_handle() - Handle FDC events.
 552 * @priv:       Pointer to driver private data.
 553 *
 554 * Handle FDC events, such as new incoming data which needs draining out of the
 555 * RX FIFO and feeding into the appropriate TTY ports, and space becoming
 556 * available in the TX FIFO which would allow more data to be written out.
 557 */
 558static void mips_ejtag_fdc_handle(struct mips_ejtag_fdc_tty *priv)
 559{
 560        struct mips_ejtag_fdc_tty_port *dport;
 561        unsigned int stat, channel, data, cfg, i, flipped;
 562        int len;
 563        char buf[4];
 564
 565        for (;;) {
 566                /* Find which channel the next FDC word is destined for */
 567                stat = mips_ejtag_fdc_read(priv, REG_FDSTAT);
 568                if (stat & REG_FDSTAT_RXE)
 569                        break;
 570                channel = (stat & REG_FDSTAT_RXCHAN) >> REG_FDSTAT_RXCHAN_SHIFT;
 571                dport = &priv->ports[channel];
 572
 573                /* Read out the FDC word, decode it, and pass to tty layer */
 574                raw_spin_lock(&dport->rx_lock);
 575                data = mips_ejtag_fdc_read(priv, REG_FDRX);
 576
 577                len = mips_ejtag_fdc_decode(data, buf);
 578                dev_dbg(priv->dev, "%s%u: in  %08x: \"%*pE\"\n",
 579                        priv->driver_name, channel, data, len, buf);
 580
 581                flipped = 0;
 582                for (i = 0; i < len; ++i) {
 583#ifdef CONFIG_MAGIC_SYSRQ
 584#ifdef CONFIG_MIPS_EJTAG_FDC_KGDB
 585                        /* Support just Ctrl+C with KGDB channel */
 586                        if (channel == CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN) {
 587                                if (buf[i] == '\x03') { /* ^C */
 588                                        handle_sysrq('g');
 589                                        continue;
 590                                }
 591                        }
 592#endif
 593                        /* Support Ctrl+O for console channel */
 594                        if (channel == mips_ejtag_fdc_con.cons.index) {
 595                                if (buf[i] == '\x0f') { /* ^O */
 596                                        priv->sysrq_pressed =
 597                                                !priv->sysrq_pressed;
 598                                        if (priv->sysrq_pressed)
 599                                                continue;
 600                                } else if (priv->sysrq_pressed) {
 601                                        handle_sysrq(buf[i]);
 602                                        priv->sysrq_pressed = false;
 603                                        continue;
 604                                }
 605                        }
 606#endif /* CONFIG_MAGIC_SYSRQ */
 607
 608                        /* Check the port isn't being shut down */
 609                        if (!dport->rx_buf)
 610                                continue;
 611
 612                        flipped += tty_insert_flip_char(&dport->port, buf[i],
 613                                                        TTY_NORMAL);
 614                }
 615                if (flipped)
 616                        tty_flip_buffer_push(&dport->port);
 617
 618                raw_spin_unlock(&dport->rx_lock);
 619        }
 620
 621        /* If TX FIFO no longer full we may be able to write more data */
 622        raw_spin_lock(&priv->lock);
 623        if (priv->xmit_full && !(stat & REG_FDSTAT_TXF)) {
 624                priv->xmit_full = false;
 625
 626                /* Disable TX interrupt */
 627                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
 628                cfg &= ~REG_FDCFG_TXINTTHRES;
 629                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
 630                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
 631
 632                /* Wait the kthread so it can try writing more data */
 633                wake_up_interruptible(&priv->waitqueue);
 634        }
 635        raw_spin_unlock(&priv->lock);
 636}
 637
 638/**
 639 * mips_ejtag_fdc_isr() - Interrupt handler.
 640 * @irq:        IRQ number.
 641 * @dev_id:     Pointer to driver private data.
 642 *
 643 * This is the interrupt handler, used when interrupts are enabled.
 644 *
 645 * It simply triggers the common FDC handler code.
 646 *
 647 * Returns:     IRQ_HANDLED if an FDC interrupt was pending.
 648 *              IRQ_NONE otherwise.
 649 */
 650static irqreturn_t mips_ejtag_fdc_isr(int irq, void *dev_id)
 651{
 652        struct mips_ejtag_fdc_tty *priv = dev_id;
 653
 654        /*
 655         * We're not using proper per-cpu IRQs, so we must be careful not to
 656         * handle IRQs on CPUs we're not interested in.
 657         *
 658         * Ideally proper per-cpu IRQ handlers could be used, but that doesn't
 659         * fit well with the whole sharing of the main CPU IRQ lines. When we
 660         * have something with a GIC that routes the FDC IRQs (i.e. no sharing
 661         * between handlers) then support could be added more easily.
 662         */
 663        if (smp_processor_id() != priv->cpu)
 664                return IRQ_NONE;
 665
 666        /* If no FDC interrupt pending, it wasn't for us */
 667        if (!(read_c0_cause() & CAUSEF_FDCI))
 668                return IRQ_NONE;
 669
 670        mips_ejtag_fdc_handle(priv);
 671        return IRQ_HANDLED;
 672}
 673
 674/**
 675 * mips_ejtag_fdc_tty_timer() - Poll FDC for incoming data.
 676 * @opaque:     Pointer to driver private data.
 677 *
 678 * This is the timer handler for when interrupts are disabled and polling the
 679 * FDC state is required.
 680 *
 681 * It simply triggers the common FDC handler code and arranges for further
 682 * polling.
 683 */
 684static void mips_ejtag_fdc_tty_timer(struct timer_list *t)
 685{
 686        struct mips_ejtag_fdc_tty *priv = from_timer(priv, t, poll_timer);
 687
 688        mips_ejtag_fdc_handle(priv);
 689        if (!priv->removing)
 690                mod_timer(&priv->poll_timer, jiffies + FDC_TTY_POLL);
 691}
 692
 693/* TTY Port operations */
 694
 695static int mips_ejtag_fdc_tty_port_activate(struct tty_port *port,
 696                                            struct tty_struct *tty)
 697{
 698        struct mips_ejtag_fdc_tty_port *dport =
 699                container_of(port, struct mips_ejtag_fdc_tty_port, port);
 700        void *rx_buf;
 701
 702        /* Allocate the buffer we use for writing data */
 703        if (tty_port_alloc_xmit_buf(port) < 0)
 704                goto err;
 705
 706        /* Allocate the buffer we use for reading data */
 707        rx_buf = kzalloc(RX_BUF_SIZE, GFP_KERNEL);
 708        if (!rx_buf)
 709                goto err_free_xmit;
 710
 711        raw_spin_lock_irq(&dport->rx_lock);
 712        dport->rx_buf = rx_buf;
 713        raw_spin_unlock_irq(&dport->rx_lock);
 714
 715        return 0;
 716err_free_xmit:
 717        tty_port_free_xmit_buf(port);
 718err:
 719        return -ENOMEM;
 720}
 721
 722static void mips_ejtag_fdc_tty_port_shutdown(struct tty_port *port)
 723{
 724        struct mips_ejtag_fdc_tty_port *dport =
 725                container_of(port, struct mips_ejtag_fdc_tty_port, port);
 726        struct mips_ejtag_fdc_tty *priv = dport->driver;
 727        void *rx_buf;
 728        unsigned int count;
 729
 730        spin_lock(&dport->xmit_lock);
 731        count = dport->xmit_cnt;
 732        spin_unlock(&dport->xmit_lock);
 733        if (count) {
 734                /*
 735                 * There's still data to write out, so wake and wait for the
 736                 * writer thread to drain the buffer.
 737                 */
 738                wake_up_interruptible(&priv->waitqueue);
 739                wait_for_completion(&dport->xmit_empty);
 740        }
 741
 742        /* Null the read buffer (timer could still be running!) */
 743        raw_spin_lock_irq(&dport->rx_lock);
 744        rx_buf = dport->rx_buf;
 745        dport->rx_buf = NULL;
 746        raw_spin_unlock_irq(&dport->rx_lock);
 747        /* Free the read buffer */
 748        kfree(rx_buf);
 749
 750        /* Free the write buffer */
 751        tty_port_free_xmit_buf(port);
 752}
 753
 754static const struct tty_port_operations mips_ejtag_fdc_tty_port_ops = {
 755        .activate       = mips_ejtag_fdc_tty_port_activate,
 756        .shutdown       = mips_ejtag_fdc_tty_port_shutdown,
 757};
 758
 759/* TTY operations */
 760
 761static int mips_ejtag_fdc_tty_install(struct tty_driver *driver,
 762                                      struct tty_struct *tty)
 763{
 764        struct mips_ejtag_fdc_tty *priv = driver->driver_state;
 765
 766        tty->driver_data = &priv->ports[tty->index];
 767        return tty_port_install(&priv->ports[tty->index].port, driver, tty);
 768}
 769
 770static int mips_ejtag_fdc_tty_open(struct tty_struct *tty, struct file *filp)
 771{
 772        return tty_port_open(tty->port, tty, filp);
 773}
 774
 775static void mips_ejtag_fdc_tty_close(struct tty_struct *tty, struct file *filp)
 776{
 777        return tty_port_close(tty->port, tty, filp);
 778}
 779
 780static void mips_ejtag_fdc_tty_hangup(struct tty_struct *tty)
 781{
 782        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
 783        struct mips_ejtag_fdc_tty *priv = dport->driver;
 784
 785        /* Drop any data in the xmit buffer */
 786        spin_lock(&dport->xmit_lock);
 787        if (dport->xmit_cnt) {
 788                atomic_sub(dport->xmit_cnt, &priv->xmit_total);
 789                dport->xmit_cnt = 0;
 790                dport->xmit_head = 0;
 791                dport->xmit_tail = 0;
 792                complete(&dport->xmit_empty);
 793        }
 794        spin_unlock(&dport->xmit_lock);
 795
 796        tty_port_hangup(tty->port);
 797}
 798
 799static int mips_ejtag_fdc_tty_write(struct tty_struct *tty,
 800                                    const unsigned char *buf, int total)
 801{
 802        int count, block;
 803        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
 804        struct mips_ejtag_fdc_tty *priv = dport->driver;
 805
 806        /*
 807         * Write to output buffer.
 808         *
 809         * The reason that we asynchronously write the buffer is because if we
 810         * were to write the buffer synchronously then because the channels are
 811         * per-CPU the buffer would be written to the channel of whatever CPU
 812         * we're running on.
 813         *
 814         * What we actually want to happen is have all input and output done on
 815         * one CPU.
 816         */
 817        spin_lock(&dport->xmit_lock);
 818        /* Work out how many bytes we can write to the xmit buffer */
 819        total = min(total, (int)(priv->xmit_size - dport->xmit_cnt));
 820        atomic_add(total, &priv->xmit_total);
 821        dport->xmit_cnt += total;
 822        /* Write the actual bytes (may need splitting if it wraps) */
 823        for (count = total; count; count -= block) {
 824                block = min(count, (int)(priv->xmit_size - dport->xmit_head));
 825                memcpy(dport->port.xmit_buf + dport->xmit_head, buf, block);
 826                dport->xmit_head += block;
 827                if (dport->xmit_head >= priv->xmit_size)
 828                        dport->xmit_head -= priv->xmit_size;
 829                buf += block;
 830        }
 831        count = dport->xmit_cnt;
 832        /* Xmit buffer no longer empty? */
 833        if (count)
 834                reinit_completion(&dport->xmit_empty);
 835        spin_unlock(&dport->xmit_lock);
 836
 837        /* Wake up the kthread */
 838        if (total)
 839                wake_up_interruptible(&priv->waitqueue);
 840        return total;
 841}
 842
 843static int mips_ejtag_fdc_tty_write_room(struct tty_struct *tty)
 844{
 845        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
 846        struct mips_ejtag_fdc_tty *priv = dport->driver;
 847        int room;
 848
 849        /* Report the space in the xmit buffer */
 850        spin_lock(&dport->xmit_lock);
 851        room = priv->xmit_size - dport->xmit_cnt;
 852        spin_unlock(&dport->xmit_lock);
 853
 854        return room;
 855}
 856
 857static int mips_ejtag_fdc_tty_chars_in_buffer(struct tty_struct *tty)
 858{
 859        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
 860        int chars;
 861
 862        /* Report the number of bytes in the xmit buffer */
 863        spin_lock(&dport->xmit_lock);
 864        chars = dport->xmit_cnt;
 865        spin_unlock(&dport->xmit_lock);
 866
 867        return chars;
 868}
 869
 870static const struct tty_operations mips_ejtag_fdc_tty_ops = {
 871        .install                = mips_ejtag_fdc_tty_install,
 872        .open                   = mips_ejtag_fdc_tty_open,
 873        .close                  = mips_ejtag_fdc_tty_close,
 874        .hangup                 = mips_ejtag_fdc_tty_hangup,
 875        .write                  = mips_ejtag_fdc_tty_write,
 876        .write_room             = mips_ejtag_fdc_tty_write_room,
 877        .chars_in_buffer        = mips_ejtag_fdc_tty_chars_in_buffer,
 878};
 879
 880int __weak get_c0_fdc_int(void)
 881{
 882        return -1;
 883}
 884
 885static int mips_ejtag_fdc_tty_probe(struct mips_cdmm_device *dev)
 886{
 887        int ret, nport;
 888        struct mips_ejtag_fdc_tty_port *dport;
 889        struct mips_ejtag_fdc_tty *priv;
 890        struct tty_driver *driver;
 891        unsigned int cfg, tx_fifo;
 892
 893        priv = devm_kzalloc(&dev->dev, sizeof(*priv), GFP_KERNEL);
 894        if (!priv)
 895                return -ENOMEM;
 896        priv->cpu = dev->cpu;
 897        priv->dev = &dev->dev;
 898        mips_cdmm_set_drvdata(dev, priv);
 899        atomic_set(&priv->xmit_total, 0);
 900        raw_spin_lock_init(&priv->lock);
 901
 902        priv->reg = devm_ioremap(priv->dev, dev->res.start,
 903                                         resource_size(&dev->res));
 904        if (!priv->reg) {
 905                dev_err(priv->dev, "ioremap failed for resource %pR\n",
 906                        &dev->res);
 907                return -ENOMEM;
 908        }
 909
 910        cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
 911        tx_fifo = (cfg & REG_FDCFG_TXFIFOSIZE) >> REG_FDCFG_TXFIFOSIZE_SHIFT;
 912        /* Disable interrupts */
 913        cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
 914        cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
 915        cfg |= REG_FDCFG_RXINTTHRES_DISABLED;
 916        mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
 917
 918        /* Make each port's xmit FIFO big enough to fill FDC TX FIFO */
 919        priv->xmit_size = min(tx_fifo * 4, (unsigned int)SERIAL_XMIT_SIZE);
 920
 921        driver = tty_alloc_driver(NUM_TTY_CHANNELS, TTY_DRIVER_REAL_RAW);
 922        if (IS_ERR(driver))
 923                return PTR_ERR(driver);
 924        priv->driver = driver;
 925
 926        driver->driver_name = "ejtag_fdc";
 927        snprintf(priv->fdc_name, sizeof(priv->fdc_name), "ttyFDC%u", dev->cpu);
 928        snprintf(priv->driver_name, sizeof(priv->driver_name), "%sc",
 929                 priv->fdc_name);
 930        driver->name = priv->driver_name;
 931        driver->major = 0; /* Auto-allocate */
 932        driver->minor_start = 0;
 933        driver->type = TTY_DRIVER_TYPE_SERIAL;
 934        driver->subtype = SERIAL_TYPE_NORMAL;
 935        driver->init_termios = tty_std_termios;
 936        driver->init_termios.c_cflag |= CLOCAL;
 937        driver->driver_state = priv;
 938
 939        tty_set_operations(driver, &mips_ejtag_fdc_tty_ops);
 940        for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) {
 941                dport = &priv->ports[nport];
 942                dport->driver = priv;
 943                tty_port_init(&dport->port);
 944                dport->port.ops = &mips_ejtag_fdc_tty_port_ops;
 945                raw_spin_lock_init(&dport->rx_lock);
 946                spin_lock_init(&dport->xmit_lock);
 947                /* The xmit buffer starts empty, i.e. completely written */
 948                init_completion(&dport->xmit_empty);
 949                complete(&dport->xmit_empty);
 950        }
 951
 952        /* Set up the console */
 953        mips_ejtag_fdc_con.regs[dev->cpu] = priv->reg;
 954        if (dev->cpu == 0)
 955                mips_ejtag_fdc_con.tty_drv = driver;
 956
 957        init_waitqueue_head(&priv->waitqueue);
 958        priv->thread = kthread_create(mips_ejtag_fdc_put, priv, priv->fdc_name);
 959        if (IS_ERR(priv->thread)) {
 960                ret = PTR_ERR(priv->thread);
 961                dev_err(priv->dev, "Couldn't create kthread (%d)\n", ret);
 962                goto err_destroy_ports;
 963        }
 964        /*
 965         * Bind the writer thread to the right CPU so it can't migrate.
 966         * The channels are per-CPU and we want all channel I/O to be on a
 967         * single predictable CPU.
 968         */
 969        kthread_bind(priv->thread, dev->cpu);
 970        wake_up_process(priv->thread);
 971
 972        /* Look for an FDC IRQ */
 973        priv->irq = get_c0_fdc_int();
 974
 975        /* Try requesting the IRQ */
 976        if (priv->irq >= 0) {
 977                /*
 978                 * IRQF_SHARED, IRQF_COND_SUSPEND: The FDC IRQ may be shared with
 979                 * other local interrupts such as the timer which sets
 980                 * IRQF_TIMER (including IRQF_NO_SUSPEND).
 981                 *
 982                 * IRQF_NO_THREAD: The FDC IRQ isn't individually maskable so it
 983                 * cannot be deferred and handled by a thread on RT kernels. For
 984                 * this reason any spinlocks used from the ISR are raw.
 985                 */
 986                ret = devm_request_irq(priv->dev, priv->irq, mips_ejtag_fdc_isr,
 987                                       IRQF_PERCPU | IRQF_SHARED |
 988                                       IRQF_NO_THREAD | IRQF_COND_SUSPEND,
 989                                       priv->fdc_name, priv);
 990                if (ret)
 991                        priv->irq = -1;
 992        }
 993        if (priv->irq >= 0) {
 994                /* IRQ is usable, enable RX interrupt */
 995                raw_spin_lock_irq(&priv->lock);
 996                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
 997                cfg &= ~REG_FDCFG_RXINTTHRES;
 998                cfg |= REG_FDCFG_RXINTTHRES_NOTEMPTY;
 999                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
1000                raw_spin_unlock_irq(&priv->lock);
1001        } else {
1002                /* If we didn't get an usable IRQ, poll instead */
1003                timer_setup(&priv->poll_timer, mips_ejtag_fdc_tty_timer,
1004                            TIMER_PINNED);
1005                priv->poll_timer.expires = jiffies + FDC_TTY_POLL;
1006                /*
1007                 * Always attach the timer to the right CPU. The channels are
1008                 * per-CPU so all polling should be from a single CPU.
1009                 */
1010                add_timer_on(&priv->poll_timer, dev->cpu);
1011
1012                dev_info(priv->dev, "No usable IRQ, polling enabled\n");
1013        }
1014
1015        ret = tty_register_driver(driver);
1016        if (ret < 0) {
1017                dev_err(priv->dev, "Couldn't install tty driver (%d)\n", ret);
1018                goto err_stop_irq;
1019        }
1020
1021        return 0;
1022
1023err_stop_irq:
1024        if (priv->irq >= 0) {
1025                raw_spin_lock_irq(&priv->lock);
1026                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
1027                /* Disable interrupts */
1028                cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
1029                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
1030                cfg |= REG_FDCFG_RXINTTHRES_DISABLED;
1031                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
1032                raw_spin_unlock_irq(&priv->lock);
1033        } else {
1034                priv->removing = true;
1035                del_timer_sync(&priv->poll_timer);
1036        }
1037        kthread_stop(priv->thread);
1038err_destroy_ports:
1039        if (dev->cpu == 0)
1040                mips_ejtag_fdc_con.tty_drv = NULL;
1041        for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) {
1042                dport = &priv->ports[nport];
1043                tty_port_destroy(&dport->port);
1044        }
1045        put_tty_driver(priv->driver);
1046        return ret;
1047}
1048
1049static int mips_ejtag_fdc_tty_cpu_down(struct mips_cdmm_device *dev)
1050{
1051        struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev);
1052        unsigned int cfg;
1053
1054        if (priv->irq >= 0) {
1055                raw_spin_lock_irq(&priv->lock);
1056                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
1057                /* Disable interrupts */
1058                cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
1059                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
1060                cfg |= REG_FDCFG_RXINTTHRES_DISABLED;
1061                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
1062                raw_spin_unlock_irq(&priv->lock);
1063        } else {
1064                priv->removing = true;
1065                del_timer_sync(&priv->poll_timer);
1066        }
1067        kthread_stop(priv->thread);
1068
1069        return 0;
1070}
1071
1072static int mips_ejtag_fdc_tty_cpu_up(struct mips_cdmm_device *dev)
1073{
1074        struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev);
1075        unsigned int cfg;
1076        int ret = 0;
1077
1078        if (priv->irq >= 0) {
1079                /*
1080                 * IRQ is usable, enable RX interrupt
1081                 * This must be before kthread is restarted, as kthread may
1082                 * enable TX interrupt.
1083                 */
1084                raw_spin_lock_irq(&priv->lock);
1085                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
1086                cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
1087                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
1088                cfg |= REG_FDCFG_RXINTTHRES_NOTEMPTY;
1089                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
1090                raw_spin_unlock_irq(&priv->lock);
1091        } else {
1092                /* Restart poll timer */
1093                priv->removing = false;
1094                add_timer_on(&priv->poll_timer, dev->cpu);
1095        }
1096
1097        /* Restart the kthread */
1098        priv->thread = kthread_create(mips_ejtag_fdc_put, priv, priv->fdc_name);
1099        if (IS_ERR(priv->thread)) {
1100                ret = PTR_ERR(priv->thread);
1101                dev_err(priv->dev, "Couldn't re-create kthread (%d)\n", ret);
1102                goto out;
1103        }
1104        /* Bind it back to the right CPU and set it off */
1105        kthread_bind(priv->thread, dev->cpu);
1106        wake_up_process(priv->thread);
1107out:
1108        return ret;
1109}
1110
1111static const struct mips_cdmm_device_id mips_ejtag_fdc_tty_ids[] = {
1112        { .type = 0xfd },
1113        { }
1114};
1115
1116static struct mips_cdmm_driver mips_ejtag_fdc_tty_driver = {
1117        .drv            = {
1118                .name   = "mips_ejtag_fdc",
1119        },
1120        .probe          = mips_ejtag_fdc_tty_probe,
1121        .cpu_down       = mips_ejtag_fdc_tty_cpu_down,
1122        .cpu_up         = mips_ejtag_fdc_tty_cpu_up,
1123        .id_table       = mips_ejtag_fdc_tty_ids,
1124};
1125builtin_mips_cdmm_driver(mips_ejtag_fdc_tty_driver);
1126
1127static int __init mips_ejtag_fdc_init_console(void)
1128{
1129        return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_con);
1130}
1131console_initcall(mips_ejtag_fdc_init_console);
1132
1133#ifdef CONFIG_MIPS_EJTAG_FDC_EARLYCON
1134static struct mips_ejtag_fdc_console mips_ejtag_fdc_earlycon = {
1135        .cons   = {
1136                .name   = "early_fdc",
1137                .write  = mips_ejtag_fdc_console_write,
1138                .flags  = CON_PRINTBUFFER | CON_BOOT,
1139                .index  = CONSOLE_CHANNEL,
1140        },
1141        .lock   = __RAW_SPIN_LOCK_UNLOCKED(mips_ejtag_fdc_earlycon.lock),
1142};
1143
1144int __init setup_early_fdc_console(void)
1145{
1146        return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_earlycon);
1147}
1148#endif
1149
1150#ifdef CONFIG_MIPS_EJTAG_FDC_KGDB
1151
1152/* read buffer to allow decompaction */
1153static unsigned int kgdbfdc_rbuflen;
1154static unsigned int kgdbfdc_rpos;
1155static char kgdbfdc_rbuf[4];
1156
1157/* write buffer to allow compaction */
1158static unsigned int kgdbfdc_wbuflen;
1159static char kgdbfdc_wbuf[4];
1160
1161static void __iomem *kgdbfdc_setup(void)
1162{
1163        void __iomem *regs;
1164        unsigned int cpu;
1165
1166        /* Find address, piggy backing off console percpu regs */
1167        cpu = smp_processor_id();
1168        regs = mips_ejtag_fdc_con.regs[cpu];
1169        /* First console output on this CPU? */
1170        if (!regs) {
1171                regs = mips_cdmm_early_probe(0xfd);
1172                mips_ejtag_fdc_con.regs[cpu] = regs;
1173        }
1174        /* Already tried and failed to find FDC on this CPU? */
1175        if (IS_ERR(regs))
1176                return regs;
1177
1178        return regs;
1179}
1180
1181/* read a character from the read buffer, filling from FDC RX FIFO */
1182static int kgdbfdc_read_char(void)
1183{
1184        unsigned int stat, channel, data;
1185        void __iomem *regs;
1186
1187        /* No more data, try and read another FDC word from RX FIFO */
1188        if (kgdbfdc_rpos >= kgdbfdc_rbuflen) {
1189                kgdbfdc_rpos = 0;
1190                kgdbfdc_rbuflen = 0;
1191
1192                regs = kgdbfdc_setup();
1193                if (IS_ERR(regs))
1194                        return NO_POLL_CHAR;
1195
1196                /* Read next word from KGDB channel */
1197                do {
1198                        stat = __raw_readl(regs + REG_FDSTAT);
1199
1200                        /* No data waiting? */
1201                        if (stat & REG_FDSTAT_RXE)
1202                                return NO_POLL_CHAR;
1203
1204                        /* Read next word */
1205                        channel = (stat & REG_FDSTAT_RXCHAN) >>
1206                                        REG_FDSTAT_RXCHAN_SHIFT;
1207                        data = __raw_readl(regs + REG_FDRX);
1208                } while (channel != CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN);
1209
1210                /* Decode into rbuf */
1211                kgdbfdc_rbuflen = mips_ejtag_fdc_decode(data, kgdbfdc_rbuf);
1212        }
1213        pr_devel("kgdbfdc r %c\n", kgdbfdc_rbuf[kgdbfdc_rpos]);
1214        return kgdbfdc_rbuf[kgdbfdc_rpos++];
1215}
1216
1217/* push an FDC word from write buffer to TX FIFO */
1218static void kgdbfdc_push_one(void)
1219{
1220        const char *bufs[1] = { kgdbfdc_wbuf };
1221        struct fdc_word word;
1222        void __iomem *regs;
1223        unsigned int i;
1224
1225        /* Construct a word from any data in buffer */
1226        word = mips_ejtag_fdc_encode(bufs, &kgdbfdc_wbuflen, 1);
1227        /* Relocate any remaining data to beginnning of buffer */
1228        kgdbfdc_wbuflen -= word.bytes;
1229        for (i = 0; i < kgdbfdc_wbuflen; ++i)
1230                kgdbfdc_wbuf[i] = kgdbfdc_wbuf[i + word.bytes];
1231
1232        regs = kgdbfdc_setup();
1233        if (IS_ERR(regs))
1234                return;
1235
1236        /* Busy wait until there's space in fifo */
1237        while (__raw_readl(regs + REG_FDSTAT) & REG_FDSTAT_TXF)
1238                ;
1239        __raw_writel(word.word,
1240                     regs + REG_FDTX(CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN));
1241}
1242
1243/* flush the whole write buffer to the TX FIFO */
1244static void kgdbfdc_flush(void)
1245{
1246        while (kgdbfdc_wbuflen)
1247                kgdbfdc_push_one();
1248}
1249
1250/* write a character into the write buffer, writing out if full */
1251static void kgdbfdc_write_char(u8 chr)
1252{
1253        pr_devel("kgdbfdc w %c\n", chr);
1254        kgdbfdc_wbuf[kgdbfdc_wbuflen++] = chr;
1255        if (kgdbfdc_wbuflen >= sizeof(kgdbfdc_wbuf))
1256                kgdbfdc_push_one();
1257}
1258
1259static struct kgdb_io kgdbfdc_io_ops = {
1260        .name           = "kgdbfdc",
1261        .read_char      = kgdbfdc_read_char,
1262        .write_char     = kgdbfdc_write_char,
1263        .flush          = kgdbfdc_flush,
1264};
1265
1266static int __init kgdbfdc_init(void)
1267{
1268        kgdb_register_io_module(&kgdbfdc_io_ops);
1269        return 0;
1270}
1271early_initcall(kgdbfdc_init);
1272#endif
1273