drivers/tty/mips_ejtag_fdc.c

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