drivers/tty/mips_ejtag_fdc.c

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