/*
 * TTY driver for MIPS EJTAG Fast Debug Channels.
 *
 * Copyright (C) 2007-2015 Imagination Technologies Ltd
 *
 * This file is subject to the terms and conditions of the GNU General Public
 * License. See the file COPYING in the main directory of this archive for more
 * details.
 */

#include <linux/atomic.h>
#include <linux/bitops.h>
#include <linux/completion.h>
#include <linux/console.h>
#include <linux/delay.h>
#include <linux/export.h>
#include <linux/init.h>
#include <linux/interrupt.h>
#include <linux/kernel.h>
#include <linux/kgdb.h>
#include <linux/kthread.h>
#include <linux/sched.h>
#include <linux/serial.h>
#include <linux/serial_core.h>
#include <linux/slab.h>
#include <linux/spinlock.h>
#include <linux/string.h>
#include <linux/timer.h>
#include <linux/tty.h>
#include <linux/tty_driver.h>
#include <linux/tty_flip.h>
#include <linux/uaccess.h>

#include <asm/cdmm.h>
#include <asm/irq.h>

/* Register offsets */
#define REG_FDACSR      0x00    /* FDC Access Control and Status Register */
#define REG_FDCFG       0x08    /* FDC Configuration Register */
#define REG_FDSTAT      0x10    /* FDC Status Register */
#define REG_FDRX        0x18    /* FDC Receive Register */
#define REG_FDTX(N)     (0x20+0x8*(N))  /* FDC Transmit Register n (0..15) */

/* Register fields */

#define REG_FDCFG_TXINTTHRES_SHIFT      18
#define REG_FDCFG_TXINTTHRES            (0x3 << REG_FDCFG_TXINTTHRES_SHIFT)
#define REG_FDCFG_TXINTTHRES_DISABLED   (0x0 << REG_FDCFG_TXINTTHRES_SHIFT)
#define REG_FDCFG_TXINTTHRES_EMPTY      (0x1 << REG_FDCFG_TXINTTHRES_SHIFT)
#define REG_FDCFG_TXINTTHRES_NOTFULL    (0x2 << REG_FDCFG_TXINTTHRES_SHIFT)
#define REG_FDCFG_TXINTTHRES_NEAREMPTY  (0x3 << REG_FDCFG_TXINTTHRES_SHIFT)
#define REG_FDCFG_RXINTTHRES_SHIFT      16
#define REG_FDCFG_RXINTTHRES            (0x3 << REG_FDCFG_RXINTTHRES_SHIFT)
#define REG_FDCFG_RXINTTHRES_DISABLED   (0x0 << REG_FDCFG_RXINTTHRES_SHIFT)
#define REG_FDCFG_RXINTTHRES_FULL       (0x1 << REG_FDCFG_RXINTTHRES_SHIFT)
#define REG_FDCFG_RXINTTHRES_NOTEMPTY   (0x2 << REG_FDCFG_RXINTTHRES_SHIFT)
#define REG_FDCFG_RXINTTHRES_NEARFULL   (0x3 << REG_FDCFG_RXINTTHRES_SHIFT)
#define REG_FDCFG_TXFIFOSIZE_SHIFT      8
#define REG_FDCFG_TXFIFOSIZE            (0xff << REG_FDCFG_TXFIFOSIZE_SHIFT)
#define REG_FDCFG_RXFIFOSIZE_SHIFT      0
#define REG_FDCFG_RXFIFOSIZE            (0xff << REG_FDCFG_RXFIFOSIZE_SHIFT)

#define REG_FDSTAT_TXCOUNT_SHIFT        24
#define REG_FDSTAT_TXCOUNT              (0xff << REG_FDSTAT_TXCOUNT_SHIFT)
#define REG_FDSTAT_RXCOUNT_SHIFT        16
#define REG_FDSTAT_RXCOUNT              (0xff << REG_FDSTAT_RXCOUNT_SHIFT)
#define REG_FDSTAT_RXCHAN_SHIFT         4
#define REG_FDSTAT_RXCHAN               (0xf << REG_FDSTAT_RXCHAN_SHIFT)
#define REG_FDSTAT_RXE                  BIT(3)  /* Rx Empty */
#define REG_FDSTAT_RXF                  BIT(2)  /* Rx Full */
#define REG_FDSTAT_TXE                  BIT(1)  /* Tx Empty */
#define REG_FDSTAT_TXF                  BIT(0)  /* Tx Full */

/* Default channel for the early console */
#define CONSOLE_CHANNEL      1

#define NUM_TTY_CHANNELS     16

#define RX_BUF_SIZE 1024

/*
 * When the IRQ is unavailable, the FDC state must be polled for incoming data
 * and space becoming available in TX FIFO.
 */
#define FDC_TTY_POLL (HZ / 50)

struct mips_ejtag_fdc_tty;

/**
 * struct mips_ejtag_fdc_tty_port - Wrapper struct for FDC tty_port.
 * @port:               TTY port data
 * @driver:             TTY driver.
 * @rx_lock:            Lock for rx_buf.
 *                      This protects between the hard interrupt and user
 *                      context. It's also held during read SWITCH operations.
 * @rx_buf:             Read buffer.
 * @xmit_lock:          Lock for xmit_*, and port.xmit_buf.
 *                      This protects between user context and kernel thread.
 *                      It is used from chars_in_buffer()/write_room() TTY
 *                      callbacks which are used during wait operations, so a
 *                      mutex is unsuitable.
 * @xmit_cnt:           Size of xmit buffer contents.
 * @xmit_head:          Head of xmit buffer where data is written.
 * @xmit_tail:          Tail of xmit buffer where data is read.
 * @xmit_empty:         Completion for xmit buffer being empty.
 */
struct mips_ejtag_fdc_tty_port {
        struct tty_port                  port;
        struct mips_ejtag_fdc_tty       *driver;
        raw_spinlock_t                   rx_lock;
        void                            *rx_buf;
        spinlock_t                       xmit_lock;
        unsigned int                     xmit_cnt;
        unsigned int                     xmit_head;
        unsigned int                     xmit_tail;
        struct completion                xmit_empty;
};

/**
 * struct mips_ejtag_fdc_tty - Driver data for FDC as a whole.
 * @dev:                FDC device (for dev_*() logging).
 * @driver:             TTY driver.
 * @cpu:                CPU number for this FDC.
 * @fdc_name:           FDC name (not for base of channel names).
 * @driver_name:        Base of driver name.
 * @ports:              Per-channel data.
 * @waitqueue:          Wait queue for waiting for TX data, or for space in TX
 *                      FIFO.
 * @lock:               Lock to protect FDCFG (interrupt enable).
 * @thread:             KThread for writing out data to FDC.
 * @reg:                FDC registers.
 * @tx_fifo:            TX FIFO size.
 * @xmit_size:          Size of each port's xmit buffer.
 * @xmit_total:         Total number of bytes (from all ports) to transmit.
 * @xmit_next:          Next port number to transmit from (round robin).
 * @xmit_full:          Indicates TX FIFO is full, we're waiting for space.
 * @irq:                IRQ number (negative if no IRQ).
 * @removing:           Indicates the device is being removed and @poll_timer
 *                      should not be restarted.
 * @poll_timer:         Timer for polling for interrupt events when @irq < 0.
 * @sysrq_pressed:      Whether the magic sysrq key combination has been
 *                      detected. See mips_ejtag_fdc_handle().
 */
struct mips_ejtag_fdc_tty {
        struct device                   *dev;
        struct tty_driver               *driver;
        unsigned int                     cpu;
        char                             fdc_name[16];
        char                             driver_name[16];
        struct mips_ejtag_fdc_tty_port   ports[NUM_TTY_CHANNELS];
        wait_queue_head_t                waitqueue;
        raw_spinlock_t                   lock;
        struct task_struct              *thread;

        void __iomem                    *reg;
        u8                               tx_fifo;

        unsigned int                     xmit_size;
        atomic_t                         xmit_total;
        unsigned int                     xmit_next;
        bool                             xmit_full;

        int                              irq;
        bool                             removing;
        struct timer_list                poll_timer;

#ifdef CONFIG_MAGIC_SYSRQ
        bool                             sysrq_pressed;
#endif
};

/* Hardware access */

static inline void mips_ejtag_fdc_write(struct mips_ejtag_fdc_tty *priv,
                                        unsigned int offs, unsigned int data)
{
        __raw_writel(data, priv->reg + offs);
}

static inline unsigned int mips_ejtag_fdc_read(struct mips_ejtag_fdc_tty *priv,
                                               unsigned int offs)
{
        return __raw_readl(priv->reg + offs);
}

/* Encoding of byte stream in FDC words */

/**
 * struct fdc_word - FDC word encoding some number of bytes of data.
 * @word:               Raw FDC word.
 * @bytes:              Number of bytes encoded by @word.
 */
struct fdc_word {
        u32             word;
        unsigned int    bytes;
};

/*
 * This is a compact encoding which allows every 1 byte, 2 byte, and 3 byte
 * sequence to be encoded in a single word, while allowing the majority of 4
 * byte sequences (including all ASCII and common binary data) to be encoded in
 * a single word too.
 *    _______________________ _____________
 *   |       FDC Word        |             |
 *   |31-24|23-16|15-8 | 7-0 |    Bytes    |
 *   |_____|_____|_____|_____|_____________|
 *   |     |     |     |     |             |
 *   |0x80 |0x80 |0x80 |  WW | WW          |
 *   |0x81 |0x81 |  XX |  WW | WW XX       |
 *   |0x82 |  YY |  XX |  WW | WW XX YY    |
 *   |  ZZ |  YY |  XX |  WW | WW XX YY ZZ |
 *   |_____|_____|_____|_____|_____________|
 *
 * Note that the 4-byte encoding can only be used where none of the other 3
 * encodings match, otherwise it must fall back to the 3 byte encoding.
 */

/* ranges >= 1 && sizes[0] >= 1 */
static struct fdc_word mips_ejtag_fdc_encode(const char **ptrs,
                                             unsigned int *sizes,
                                             unsigned int ranges)
{
        struct fdc_word word = { 0, 0 };
        const char **ptrs_end = ptrs + ranges;

        for (; ptrs < ptrs_end; ++ptrs) {
                const char *ptr = *(ptrs++);
                const char *end = ptr + *(sizes++);

                for (; ptr < end; ++ptr) {
                        word.word |= (u8)*ptr << (8*word.bytes);
                        ++word.bytes;
                        if (word.bytes == 4)
                                goto done;
                }
        }
done:
        /* Choose the appropriate encoding */
        switch (word.bytes) {
        case 4:
                /* 4 byte encoding, but don't match the 1-3 byte encodings */
                if ((word.word >> 8) != 0x808080 &&
                    (word.word >> 16) != 0x8181 &&
                    (word.word >> 24) != 0x82)
                        break;
                /* Fall back to a 3 byte encoding */
                word.bytes = 3;
                word.word &= 0x00ffffff;
        case 3:
                /* 3 byte encoding */
                word.word |= 0x82000000;
                break;
        case 2:
                /* 2 byte encoding */
                word.word |= 0x81810000;
                break;
        case 1:
                /* 1 byte encoding */
                word.word |= 0x80808000;
                break;
        }
        return word;
}

static unsigned int mips_ejtag_fdc_decode(u32 word, char *buf)
{
        buf[0] = (u8)word;
        word >>= 8;
        if (word == 0x808080)
                return 1;
        buf[1] = (u8)word;
        word >>= 8;
        if (word == 0x8181)
                return 2;
        buf[2] = (u8)word;
        word >>= 8;
        if (word == 0x82)
                return 3;
        buf[3] = (u8)word;
        return 4;
}

/* Console operations */

/**
 * struct mips_ejtag_fdc_console - Wrapper struct for FDC consoles.
 * @cons:               Console object.
 * @tty_drv:            TTY driver associated with this console.
 * @lock:               Lock to protect concurrent access to other fields.
 *                      This is raw because it may be used very early.
 * @initialised:        Whether the console is initialised.
 * @regs:               Registers base address for each CPU.
 */
struct mips_ejtag_fdc_console {
        struct console           cons;
        struct tty_driver       *tty_drv;
        raw_spinlock_t           lock;
        bool                     initialised;
        void __iomem            *regs[NR_CPUS];
};

/* Low level console write shared by early console and normal console */
static void mips_ejtag_fdc_console_write(struct console *c, const char *s,
                                         unsigned int count)
{
        struct mips_ejtag_fdc_console *cons =
                container_of(c, struct mips_ejtag_fdc_console, cons);
        void __iomem *regs;
        struct fdc_word word;
        unsigned long flags;
        unsigned int i, buf_len, cpu;
        bool done_cr = false;
        char buf[4];
        const char *buf_ptr = buf;
        /* Number of bytes of input data encoded up to each byte in buf */
        u8 inc[4];

        local_irq_save(flags);
        cpu = smp_processor_id();
        regs = cons->regs[cpu];
        /* First console output on this CPU? */
        if (!regs) {
                regs = mips_cdmm_early_probe(0xfd);
                cons->regs[cpu] = regs;
        }
        /* Already tried and failed to find FDC on this CPU? */
        if (IS_ERR(regs))
                goto out;
        while (count) {
                /*
                 * Copy the next few characters to a buffer so we can inject
                 * carriage returns before newlines.
                 */
                for (buf_len = 0, i = 0; buf_len < 4 && i < count; ++buf_len) {
                        if (s[i] == '\n' && !done_cr) {
                                buf[buf_len] = '\r';
                                done_cr = true;
                        } else {
                                buf[buf_len] = s[i];
                                done_cr = false;
                                ++i;
                        }
                        inc[buf_len] = i;
                }
                word = mips_ejtag_fdc_encode(&buf_ptr, &buf_len, 1);
                count -= inc[word.bytes - 1];
                s += inc[word.bytes - 1];

                /* Busy wait until there's space in fifo */
                while (__raw_readl(regs + REG_FDSTAT) & REG_FDSTAT_TXF)
                        ;
                __raw_writel(word.word, regs + REG_FDTX(c->index));
        }
out:
        local_irq_restore(flags);
}

static struct tty_driver *mips_ejtag_fdc_console_device(struct console *c,
                                                        int *index)
{
        struct mips_ejtag_fdc_console *cons =
                container_of(c, struct mips_ejtag_fdc_console, cons);

        *index = c->index;
        return cons->tty_drv;
}

/* Initialise an FDC console (early or normal */
static int __init mips_ejtag_fdc_console_init(struct mips_ejtag_fdc_console *c)
{
        void __iomem *regs;
        unsigned long flags;
        int ret = 0;

        raw_spin_lock_irqsave(&c->lock, flags);
        /* Don't init twice */
        if (c->initialised)
                goto out;
        /* Look for the FDC device */
        regs = mips_cdmm_early_probe(0xfd);
        if (IS_ERR(regs)) {
                ret = PTR_ERR(regs);
                goto out;
        }

        c->initialised = true;
        c->regs[smp_processor_id()] = regs;
        register_console(&c->cons);
out:
        raw_spin_unlock_irqrestore(&c->lock, flags);
        return ret;
}

static struct mips_ejtag_fdc_console mips_ejtag_fdc_con = {
        .cons   = {
                .name   = "fdc",
                .write  = mips_ejtag_fdc_console_write,
                .device = mips_ejtag_fdc_console_device,
                .flags  = CON_PRINTBUFFER,
                .index  = -1,
        },
        .lock   = __RAW_SPIN_LOCK_UNLOCKED(mips_ejtag_fdc_con.lock),
};

/* TTY RX/TX operations */

/**
 * mips_ejtag_fdc_put_chan() - Write out a block of channel data.
 * @priv:       Pointer to driver private data.
 * @chan:       Channel number.
 *
 * Write a single block of data out to the debug adapter. If the circular buffer
 * is wrapped then only the first block is written.
 *
 * Returns:     The number of bytes that were written.
 */
static unsigned int mips_ejtag_fdc_put_chan(struct mips_ejtag_fdc_tty *priv,
                                            unsigned int chan)
{
        struct mips_ejtag_fdc_tty_port *dport;
        struct tty_struct *tty;
        const char *ptrs[2];
        unsigned int sizes[2] = { 0 };
        struct fdc_word word = { .bytes = 0 };
        unsigned long flags;

        dport = &priv->ports[chan];
        spin_lock(&dport->xmit_lock);
        if (dport->xmit_cnt) {
                ptrs[0] = dport->port.xmit_buf + dport->xmit_tail;
                sizes[0] = min_t(unsigned int,
                                 priv->xmit_size - dport->xmit_tail,
                                 dport->xmit_cnt);
                ptrs[1] = dport->port.xmit_buf;
                sizes[1] = dport->xmit_cnt - sizes[0];
                word = mips_ejtag_fdc_encode(ptrs, sizes, 1 + !!sizes[1]);

                dev_dbg(priv->dev, "%s%u: out %08x: \"%*pE%*pE\"\n",
                        priv->driver_name, chan, word.word,
                        min_t(int, word.bytes, sizes[0]), ptrs[0],
                        max_t(int, 0, word.bytes - sizes[0]), ptrs[1]);

                local_irq_save(flags);
                /* Maybe we raced with the console and TX FIFO is full */
                if (mips_ejtag_fdc_read(priv, REG_FDSTAT) & REG_FDSTAT_TXF)
                        word.bytes = 0;
                else
                        mips_ejtag_fdc_write(priv, REG_FDTX(chan), word.word);
                local_irq_restore(flags);

                dport->xmit_cnt -= word.bytes;
                if (!dport->xmit_cnt) {
                        /* Reset pointers to avoid wraps */
                        dport->xmit_head = 0;
                        dport->xmit_tail = 0;
                        complete(&dport->xmit_empty);
                } else {
                        dport->xmit_tail += word.bytes;
                        if (dport->xmit_tail >= priv->xmit_size)
                                dport->xmit_tail -= priv->xmit_size;
                }
                atomic_sub(word.bytes, &priv->xmit_total);
        }
        spin_unlock(&dport->xmit_lock);

        /* If we've made more data available, wake up tty */
        if (sizes[0] && word.bytes) {
                tty = tty_port_tty_get(&dport->port);
                if (tty) {
                        tty_wakeup(tty);
                        tty_kref_put(tty);
                }
        }

        return word.bytes;
}

/**
 * mips_ejtag_fdc_put() - Kernel thread to write out channel data to FDC.
 * @arg:        Driver pointer.
 *
 * This kernel thread runs while @priv->xmit_total != 0, and round robins the
 * channels writing out blocks of buffered data to the FDC TX FIFO.
 */
static int mips_ejtag_fdc_put(void *arg)
{
        struct mips_ejtag_fdc_tty *priv = arg;
        struct mips_ejtag_fdc_tty_port *dport;
        unsigned int ret;
        u32 cfg;

        __set_current_state(TASK_RUNNING);
        while (!kthread_should_stop()) {
                /* Wait for data to actually write */
                wait_event_interruptible(priv->waitqueue,
                                         atomic_read(&priv->xmit_total) ||
                                         kthread_should_stop());
                if (kthread_should_stop())
                        break;

                /* Wait for TX FIFO space to write data */
                raw_spin_lock_irq(&priv->lock);
                if (mips_ejtag_fdc_read(priv, REG_FDSTAT) & REG_FDSTAT_TXF) {
                        priv->xmit_full = true;
                        if (priv->irq >= 0) {
                                /* Enable TX interrupt */
                                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
                                cfg &= ~REG_FDCFG_TXINTTHRES;
                                cfg |= REG_FDCFG_TXINTTHRES_NOTFULL;
                                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
                        }
                }
                raw_spin_unlock_irq(&priv->lock);
                wait_event_interruptible(priv->waitqueue,
                                         !(mips_ejtag_fdc_read(priv, REG_FDSTAT)
                                           & REG_FDSTAT_TXF) ||
                                         kthread_should_stop());
                if (kthread_should_stop())
                        break;

                /* Find next channel with data to output */
                for (;;) {
                        dport = &priv->ports[priv->xmit_next];
                        spin_lock(&dport->xmit_lock);
                        ret = dport->xmit_cnt;
                        spin_unlock(&dport->xmit_lock);
                        if (ret)
                                break;
                        /* Round robin */
                        ++priv->xmit_next;
                        if (priv->xmit_next >= NUM_TTY_CHANNELS)
                                priv->xmit_next = 0;
                }

                /* Try writing data to the chosen channel */
                ret = mips_ejtag_fdc_put_chan(priv, priv->xmit_next);

                /*
                 * If anything was output, move on to the next channel so as not
                 * to starve other channels.
                 */
                if (ret) {
                        ++priv->xmit_next;
                        if (priv->xmit_next >= NUM_TTY_CHANNELS)
                                priv->xmit_next = 0;
                }
        }

        return 0;
}

/**
 * mips_ejtag_fdc_handle() - Handle FDC events.
 * @priv:       Pointer to driver private data.
 *
 * Handle FDC events, such as new incoming data which needs draining out of the
 * RX FIFO and feeding into the appropriate TTY ports, and space becoming
 * available in the TX FIFO which would allow more data to be written out.
 */
static void mips_ejtag_fdc_handle(struct mips_ejtag_fdc_tty *priv)
{
        struct mips_ejtag_fdc_tty_port *dport;
        unsigned int stat, channel, data, cfg, i, flipped;
        int len;
        char buf[4];

        for (;;) {
                /* Find which channel the next FDC word is destined for */
                stat = mips_ejtag_fdc_read(priv, REG_FDSTAT);
                if (stat & REG_FDSTAT_RXE)
                        break;
                channel = (stat & REG_FDSTAT_RXCHAN) >> REG_FDSTAT_RXCHAN_SHIFT;
                dport = &priv->ports[channel];

                /* Read out the FDC word, decode it, and pass to tty layer */
                raw_spin_lock(&dport->rx_lock);
                data = mips_ejtag_fdc_read(priv, REG_FDRX);

                len = mips_ejtag_fdc_decode(data, buf);
                dev_dbg(priv->dev, "%s%u: in  %08x: \"%*pE\"\n",
                        priv->driver_name, channel, data, len, buf);

                flipped = 0;
                for (i = 0; i < len; ++i) {
#ifdef CONFIG_MAGIC_SYSRQ
#ifdef CONFIG_MIPS_EJTAG_FDC_KGDB
                        /* Support just Ctrl+C with KGDB channel */
                        if (channel == CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN) {
                                if (buf[i] == '\x03') { /* ^C */
                                        handle_sysrq('g');
                                        continue;
                                }
                        }
#endif
                        /* Support Ctrl+O for console channel */
                        if (channel == mips_ejtag_fdc_con.cons.index) {
                                if (buf[i] == '\x0f') { /* ^O */
                                        priv->sysrq_pressed =
                                                !priv->sysrq_pressed;
                                        if (priv->sysrq_pressed)
                                                continue;
                                } else if (priv->sysrq_pressed) {
                                        handle_sysrq(buf[i]);
                                        priv->sysrq_pressed = false;
                                        continue;
                                }
                        }
#endif /* CONFIG_MAGIC_SYSRQ */

                        /* Check the port isn't being shut down */
                        if (!dport->rx_buf)
                                continue;

                        flipped += tty_insert_flip_char(&dport->port, buf[i],
                                                        TTY_NORMAL);
                }
                if (flipped)
                        tty_flip_buffer_push(&dport->port);

                raw_spin_unlock(&dport->rx_lock);
        }

        /* If TX FIFO no longer full we may be able to write more data */
        raw_spin_lock(&priv->lock);
        if (priv->xmit_full && !(stat & REG_FDSTAT_TXF)) {
                priv->xmit_full = false;

                /* Disable TX interrupt */
                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
                cfg &= ~REG_FDCFG_TXINTTHRES;
                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);

                /* Wait the kthread so it can try writing more data */
                wake_up_interruptible(&priv->waitqueue);
        }
        raw_spin_unlock(&priv->lock);
}

/**
 * mips_ejtag_fdc_isr() - Interrupt handler.
 * @irq:        IRQ number.
 * @dev_id:     Pointer to driver private data.
 *
 * This is the interrupt handler, used when interrupts are enabled.
 *
 * It simply triggers the common FDC handler code.
 *
 * Returns:     IRQ_HANDLED if an FDC interrupt was pending.
 *              IRQ_NONE otherwise.
 */
static irqreturn_t mips_ejtag_fdc_isr(int irq, void *dev_id)
{
        struct mips_ejtag_fdc_tty *priv = dev_id;

        /*
         * We're not using proper per-cpu IRQs, so we must be careful not to
         * handle IRQs on CPUs we're not interested in.
         *
         * Ideally proper per-cpu IRQ handlers could be used, but that doesn't
         * fit well with the whole sharing of the main CPU IRQ lines. When we
         * have something with a GIC that routes the FDC IRQs (i.e. no sharing
         * between handlers) then support could be added more easily.
         */
        if (smp_processor_id() != priv->cpu)
                return IRQ_NONE;

        /* If no FDC interrupt pending, it wasn't for us */
        if (!(read_c0_cause() & CAUSEF_FDCI))
                return IRQ_NONE;

        mips_ejtag_fdc_handle(priv);
        return IRQ_HANDLED;
}

/**
 * mips_ejtag_fdc_tty_timer() - Poll FDC for incoming data.
 * @opaque:     Pointer to driver private data.
 *
 * This is the timer handler for when interrupts are disabled and polling the
 * FDC state is required.
 *
 * It simply triggers the common FDC handler code and arranges for further
 * polling.
 */
static void mips_ejtag_fdc_tty_timer(unsigned long opaque)
{
        struct mips_ejtag_fdc_tty *priv = (void *)opaque;

        mips_ejtag_fdc_handle(priv);
        if (!priv->removing)
                mod_timer_pinned(&priv->poll_timer, jiffies + FDC_TTY_POLL);
}

/* TTY Port operations */

static int mips_ejtag_fdc_tty_port_activate(struct tty_port *port,
                                            struct tty_struct *tty)
{
        struct mips_ejtag_fdc_tty_port *dport =
                container_of(port, struct mips_ejtag_fdc_tty_port, port);
        void *rx_buf;

        /* Allocate the buffer we use for writing data */
        if (tty_port_alloc_xmit_buf(port) < 0)
                goto err;

        /* Allocate the buffer we use for reading data */
        rx_buf = kzalloc(RX_BUF_SIZE, GFP_KERNEL);
        if (!rx_buf)
                goto err_free_xmit;

        raw_spin_lock_irq(&dport->rx_lock);
        dport->rx_buf = rx_buf;
        raw_spin_unlock_irq(&dport->rx_lock);

        return 0;
err_free_xmit:
        tty_port_free_xmit_buf(port);
err:
        return -ENOMEM;
}

static void mips_ejtag_fdc_tty_port_shutdown(struct tty_port *port)
{
        struct mips_ejtag_fdc_tty_port *dport =
                container_of(port, struct mips_ejtag_fdc_tty_port, port);
        struct mips_ejtag_fdc_tty *priv = dport->driver;
        void *rx_buf;
        unsigned int count;

        spin_lock(&dport->xmit_lock);
        count = dport->xmit_cnt;
        spin_unlock(&dport->xmit_lock);
        if (count) {
                /*
                 * There's still data to write out, so wake and wait for the
                 * writer thread to drain the buffer.
                 */
                wake_up_interruptible(&priv->waitqueue);
                wait_for_completion(&dport->xmit_empty);
        }

        /* Null the read buffer (timer could still be running!) */
        raw_spin_lock_irq(&dport->rx_lock);
        rx_buf = dport->rx_buf;
        dport->rx_buf = NULL;
        raw_spin_unlock_irq(&dport->rx_lock);
        /* Free the read buffer */
        kfree(rx_buf);

        /* Free the write buffer */
        tty_port_free_xmit_buf(port);
}

static const struct tty_port_operations mips_ejtag_fdc_tty_port_ops = {
        .activate       = mips_ejtag_fdc_tty_port_activate,
        .shutdown       = mips_ejtag_fdc_tty_port_shutdown,
};

/* TTY operations */

static int mips_ejtag_fdc_tty_install(struct tty_driver *driver,
                                      struct tty_struct *tty)
{
        struct mips_ejtag_fdc_tty *priv = driver->driver_state;

        tty->driver_data = &priv->ports[tty->index];
        return tty_port_install(&priv->ports[tty->index].port, driver, tty);
}

static int mips_ejtag_fdc_tty_open(struct tty_struct *tty, struct file *filp)
{
        return tty_port_open(tty->port, tty, filp);
}

static void mips_ejtag_fdc_tty_close(struct tty_struct *tty, struct file *filp)
{
        return tty_port_close(tty->port, tty, filp);
}

static void mips_ejtag_fdc_tty_hangup(struct tty_struct *tty)
{
        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
        struct mips_ejtag_fdc_tty *priv = dport->driver;

        /* Drop any data in the xmit buffer */
        spin_lock(&dport->xmit_lock);
        if (dport->xmit_cnt) {
                atomic_sub(dport->xmit_cnt, &priv->xmit_total);
                dport->xmit_cnt = 0;
                dport->xmit_head = 0;
                dport->xmit_tail = 0;
                complete(&dport->xmit_empty);
        }
        spin_unlock(&dport->xmit_lock);

        tty_port_hangup(tty->port);
}

static int mips_ejtag_fdc_tty_write(struct tty_struct *tty,
                                    const unsigned char *buf, int total)
{
        int count, block;
        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
        struct mips_ejtag_fdc_tty *priv = dport->driver;

        /*
         * Write to output buffer.
         *
         * The reason that we asynchronously write the buffer is because if we
         * were to write the buffer synchronously then because the channels are
         * per-CPU the buffer would be written to the channel of whatever CPU
         * we're running on.
         *
         * What we actually want to happen is have all input and output done on
         * one CPU.
         */
        spin_lock(&dport->xmit_lock);
        /* Work out how many bytes we can write to the xmit buffer */
        total = min(total, (int)(priv->xmit_size - dport->xmit_cnt));
        atomic_add(total, &priv->xmit_total);
        dport->xmit_cnt += total;
        /* Write the actual bytes (may need splitting if it wraps) */
        for (count = total; count; count -= block) {
                block = min(count, (int)(priv->xmit_size - dport->xmit_head));
                memcpy(dport->port.xmit_buf + dport->xmit_head, buf, block);
                dport->xmit_head += block;
                if (dport->xmit_head >= priv->xmit_size)
                        dport->xmit_head -= priv->xmit_size;
                buf += block;
        }
        count = dport->xmit_cnt;
        /* Xmit buffer no longer empty? */
        if (count)
                reinit_completion(&dport->xmit_empty);
        spin_unlock(&dport->xmit_lock);

        /* Wake up the kthread */
        if (total)
                wake_up_interruptible(&priv->waitqueue);
        return total;
}

static int mips_ejtag_fdc_tty_write_room(struct tty_struct *tty)
{
        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
        struct mips_ejtag_fdc_tty *priv = dport->driver;
        int room;

        /* Report the space in the xmit buffer */
        spin_lock(&dport->xmit_lock);
        room = priv->xmit_size - dport->xmit_cnt;
        spin_unlock(&dport->xmit_lock);

        return room;
}

static int mips_ejtag_fdc_tty_chars_in_buffer(struct tty_struct *tty)
{
        struct mips_ejtag_fdc_tty_port *dport = tty->driver_data;
        int chars;

        /* Report the number of bytes in the xmit buffer */
        spin_lock(&dport->xmit_lock);
        chars = dport->xmit_cnt;
        spin_unlock(&dport->xmit_lock);

        return chars;
}

static const struct tty_operations mips_ejtag_fdc_tty_ops = {
        .install                = mips_ejtag_fdc_tty_install,
        .open                   = mips_ejtag_fdc_tty_open,
        .close                  = mips_ejtag_fdc_tty_close,
        .hangup                 = mips_ejtag_fdc_tty_hangup,
        .write                  = mips_ejtag_fdc_tty_write,
        .write_room             = mips_ejtag_fdc_tty_write_room,
        .chars_in_buffer        = mips_ejtag_fdc_tty_chars_in_buffer,
};

static int mips_ejtag_fdc_tty_probe(struct mips_cdmm_device *dev)
{
        int ret, nport;
        struct mips_ejtag_fdc_tty_port *dport;
        struct mips_ejtag_fdc_tty *priv;
        struct tty_driver *driver;
        unsigned int cfg, tx_fifo;

        priv = devm_kzalloc(&dev->dev, sizeof(*priv), GFP_KERNEL);
        if (!priv)
                return -ENOMEM;
        priv->cpu = dev->cpu;
        priv->dev = &dev->dev;
        mips_cdmm_set_drvdata(dev, priv);
        atomic_set(&priv->xmit_total, 0);
        raw_spin_lock_init(&priv->lock);

        priv->reg = devm_ioremap_nocache(priv->dev, dev->res.start,
                                         resource_size(&dev->res));
        if (!priv->reg) {
                dev_err(priv->dev, "ioremap failed for resource %pR\n",
                        &dev->res);
                return -ENOMEM;
        }

        cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
        tx_fifo = (cfg & REG_FDCFG_TXFIFOSIZE) >> REG_FDCFG_TXFIFOSIZE_SHIFT;
        /* Disable interrupts */
        cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
        cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
        cfg |= REG_FDCFG_RXINTTHRES_DISABLED;
        mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);

        /* Make each port's xmit FIFO big enough to fill FDC TX FIFO */
        priv->xmit_size = min(tx_fifo * 4, (unsigned int)SERIAL_XMIT_SIZE);

        driver = tty_alloc_driver(NUM_TTY_CHANNELS, TTY_DRIVER_REAL_RAW);
        if (IS_ERR(driver))
                return PTR_ERR(driver);
        priv->driver = driver;

        driver->driver_name = "ejtag_fdc";
        snprintf(priv->fdc_name, sizeof(priv->fdc_name), "ttyFDC%u", dev->cpu);
        snprintf(priv->driver_name, sizeof(priv->driver_name), "%sc",
                 priv->fdc_name);
        driver->name = priv->driver_name;
        driver->major = 0; /* Auto-allocate */
        driver->minor_start = 0;
        driver->type = TTY_DRIVER_TYPE_SERIAL;
        driver->subtype = SERIAL_TYPE_NORMAL;
        driver->init_termios = tty_std_termios;
        driver->init_termios.c_cflag |= CLOCAL;
        driver->driver_state = priv;

        tty_set_operations(driver, &mips_ejtag_fdc_tty_ops);
        for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) {
                dport = &priv->ports[nport];
                dport->driver = priv;
                tty_port_init(&dport->port);
                dport->port.ops = &mips_ejtag_fdc_tty_port_ops;
                raw_spin_lock_init(&dport->rx_lock);
                spin_lock_init(&dport->xmit_lock);
                /* The xmit buffer starts empty, i.e. completely written */
                init_completion(&dport->xmit_empty);
                complete(&dport->xmit_empty);
        }

        /* Set up the console */
        mips_ejtag_fdc_con.regs[dev->cpu] = priv->reg;
        if (dev->cpu == 0)
                mips_ejtag_fdc_con.tty_drv = driver;

        init_waitqueue_head(&priv->waitqueue);
        priv->thread = kthread_create(mips_ejtag_fdc_put, priv, priv->fdc_name);
        if (IS_ERR(priv->thread)) {
                ret = PTR_ERR(priv->thread);
                dev_err(priv->dev, "Couldn't create kthread (%d)\n", ret);
                goto err_destroy_ports;
        }
        /*
         * Bind the writer thread to the right CPU so it can't migrate.
         * The channels are per-CPU and we want all channel I/O to be on a
         * single predictable CPU.
         */
        kthread_bind(priv->thread, dev->cpu);
        wake_up_process(priv->thread);

        /* Look for an FDC IRQ */
        priv->irq = -1;
        if (get_c0_fdc_int)
                priv->irq = get_c0_fdc_int();

        /* Try requesting the IRQ */
        if (priv->irq >= 0) {
                /*
                 * IRQF_SHARED, IRQF_NO_SUSPEND: The FDC IRQ may be shared with
                 * other local interrupts such as the timer which sets
                 * IRQF_TIMER (including IRQF_NO_SUSPEND).
                 *
                 * IRQF_NO_THREAD: The FDC IRQ isn't individually maskable so it
                 * cannot be deferred and handled by a thread on RT kernels. For
                 * this reason any spinlocks used from the ISR are raw.
                 */
                ret = devm_request_irq(priv->dev, priv->irq, mips_ejtag_fdc_isr,
                                       IRQF_PERCPU | IRQF_SHARED |
                                       IRQF_NO_THREAD | IRQF_NO_SUSPEND,
                                       priv->fdc_name, priv);
                if (ret)
                        priv->irq = -1;
        }
        if (priv->irq >= 0) {
                /* IRQ is usable, enable RX interrupt */
                raw_spin_lock_irq(&priv->lock);
                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
                cfg &= ~REG_FDCFG_RXINTTHRES;
                cfg |= REG_FDCFG_RXINTTHRES_NOTEMPTY;
                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
                raw_spin_unlock_irq(&priv->lock);
        } else {

                /* If we didn't get an usable IRQ, poll instead */
                setup_timer(&priv->poll_timer, mips_ejtag_fdc_tty_timer,
                            (unsigned long)priv);
                priv->poll_timer.expires = jiffies + FDC_TTY_POLL;
                /*
                 * Always attach the timer to the right CPU. The channels are
                 * per-CPU so all polling should be from a single CPU.
                 */
                add_timer_on(&priv->poll_timer, dev->cpu);

                dev_info(priv->dev, "No usable IRQ, polling enabled\n");
        }

        ret = tty_register_driver(driver);
        if (ret < 0) {
                dev_err(priv->dev, "Couldn't install tty driver (%d)\n", ret);
                goto err_stop_irq;
        }

        return 0;

err_stop_irq:
        if (priv->irq >= 0) {
                raw_spin_lock_irq(&priv->lock);
                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
                /* Disable interrupts */
                cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
                cfg |= REG_FDCFG_RXINTTHRES_DISABLED;
                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
                raw_spin_unlock_irq(&priv->lock);
        } else {
                priv->removing = true;
                del_timer_sync(&priv->poll_timer);
        }
        kthread_stop(priv->thread);
err_destroy_ports:
        if (dev->cpu == 0)
                mips_ejtag_fdc_con.tty_drv = NULL;
        for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) {
                dport = &priv->ports[nport];
                tty_port_destroy(&dport->port);
        }
        put_tty_driver(priv->driver);
        return ret;
}

static int mips_ejtag_fdc_tty_remove(struct mips_cdmm_device *dev)
{
        struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev);
        struct mips_ejtag_fdc_tty_port *dport;
        int nport;
        unsigned int cfg;

        if (priv->irq >= 0) {
                raw_spin_lock_irq(&priv->lock);
                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
                /* Disable interrupts */
                cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
                cfg |= REG_FDCFG_RXINTTHRES_DISABLED;
                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
                raw_spin_unlock_irq(&priv->lock);
        } else {
                priv->removing = true;
                del_timer_sync(&priv->poll_timer);
        }
        kthread_stop(priv->thread);
        if (dev->cpu == 0)
                mips_ejtag_fdc_con.tty_drv = NULL;
        tty_unregister_driver(priv->driver);
        for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) {
                dport = &priv->ports[nport];
                tty_port_destroy(&dport->port);
        }
        put_tty_driver(priv->driver);
        return 0;
}

static int mips_ejtag_fdc_tty_cpu_down(struct mips_cdmm_device *dev)
{
        struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev);
        unsigned int cfg;

        if (priv->irq >= 0) {
                raw_spin_lock_irq(&priv->lock);
                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
                /* Disable interrupts */
                cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
                cfg |= REG_FDCFG_RXINTTHRES_DISABLED;
                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
                raw_spin_unlock_irq(&priv->lock);
        } else {
                priv->removing = true;
                del_timer_sync(&priv->poll_timer);
        }
        kthread_stop(priv->thread);

        return 0;
}

static int mips_ejtag_fdc_tty_cpu_up(struct mips_cdmm_device *dev)
{
        struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev);
        unsigned int cfg;
        int ret = 0;

        if (priv->irq >= 0) {
                /*
                 * IRQ is usable, enable RX interrupt
                 * This must be before kthread is restarted, as kthread may
                 * enable TX interrupt.
                 */
                raw_spin_lock_irq(&priv->lock);
                cfg = mips_ejtag_fdc_read(priv, REG_FDCFG);
                cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES);
                cfg |= REG_FDCFG_TXINTTHRES_DISABLED;
                cfg |= REG_FDCFG_RXINTTHRES_NOTEMPTY;
                mips_ejtag_fdc_write(priv, REG_FDCFG, cfg);
                raw_spin_unlock_irq(&priv->lock);
        } else {
                /* Restart poll timer */
                priv->removing = false;
                add_timer_on(&priv->poll_timer, dev->cpu);
        }

        /* Restart the kthread */
        priv->thread = kthread_create(mips_ejtag_fdc_put, priv, priv->fdc_name);
        if (IS_ERR(priv->thread)) {
                ret = PTR_ERR(priv->thread);
                dev_err(priv->dev, "Couldn't re-create kthread (%d)\n", ret);
                goto out;
        }
        /* Bind it back to the right CPU and set it off */
        kthread_bind(priv->thread, dev->cpu);
        wake_up_process(priv->thread);
out:
        return ret;
}

static struct mips_cdmm_device_id mips_ejtag_fdc_tty_ids[] = {
        { .type = 0xfd },
        { }
};

static struct mips_cdmm_driver mips_ejtag_fdc_tty_driver = {
        .drv            = {
                .name   = "mips_ejtag_fdc",
        },
        .probe          = mips_ejtag_fdc_tty_probe,
        .remove         = mips_ejtag_fdc_tty_remove,
        .cpu_down       = mips_ejtag_fdc_tty_cpu_down,
        .cpu_up         = mips_ejtag_fdc_tty_cpu_up,
        .id_table       = mips_ejtag_fdc_tty_ids,
};
module_mips_cdmm_driver(mips_ejtag_fdc_tty_driver);

static int __init mips_ejtag_fdc_init_console(void)
{
        return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_con);
}
console_initcall(mips_ejtag_fdc_init_console);

#ifdef CONFIG_MIPS_EJTAG_FDC_EARLYCON
static struct mips_ejtag_fdc_console mips_ejtag_fdc_earlycon = {
        .cons   = {
                .name   = "early_fdc",
                .write  = mips_ejtag_fdc_console_write,
                .flags  = CON_PRINTBUFFER | CON_BOOT,
                .index  = CONSOLE_CHANNEL,
        },
        .lock   = __RAW_SPIN_LOCK_UNLOCKED(mips_ejtag_fdc_earlycon.lock),
};

int __init setup_early_fdc_console(void)
{
        return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_earlycon);
}
#endif

#ifdef CONFIG_MIPS_EJTAG_FDC_KGDB

/* read buffer to allow decompaction */
static unsigned int kgdbfdc_rbuflen;
static unsigned int kgdbfdc_rpos;
static char kgdbfdc_rbuf[4];

/* write buffer to allow compaction */
static unsigned int kgdbfdc_wbuflen;
static char kgdbfdc_wbuf[4];

static void __iomem *kgdbfdc_setup(void)
{
        void __iomem *regs;
        unsigned int cpu;

        /* Find address, piggy backing off console percpu regs */
        cpu = smp_processor_id();
        regs = mips_ejtag_fdc_con.regs[cpu];
        /* First console output on this CPU? */
        if (!regs) {
                regs = mips_cdmm_early_probe(0xfd);
                mips_ejtag_fdc_con.regs[cpu] = regs;
        }
        /* Already tried and failed to find FDC on this CPU? */
        if (IS_ERR(regs))
                return regs;

        return regs;
}

/* read a character from the read buffer, filling from FDC RX FIFO */
static int kgdbfdc_read_char(void)
{
        unsigned int stat, channel, data;
        void __iomem *regs;

        /* No more data, try and read another FDC word from RX FIFO */
        if (kgdbfdc_rpos >= kgdbfdc_rbuflen) {
                kgdbfdc_rpos = 0;
                kgdbfdc_rbuflen = 0;

                regs = kgdbfdc_setup();
                if (IS_ERR(regs))
                        return NO_POLL_CHAR;

                /* Read next word from KGDB channel */
                do {
                        stat = __raw_readl(regs + REG_FDSTAT);

                        /* No data waiting? */
                        if (stat & REG_FDSTAT_RXE)
                                return NO_POLL_CHAR;

                        /* Read next word */
                        channel = (stat & REG_FDSTAT_RXCHAN) >>
                                        REG_FDSTAT_RXCHAN_SHIFT;
                        data = __raw_readl(regs + REG_FDRX);
                } while (channel != CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN);

                /* Decode into rbuf */
                kgdbfdc_rbuflen = mips_ejtag_fdc_decode(data, kgdbfdc_rbuf);
        }
        pr_devel("kgdbfdc r %c\n", kgdbfdc_rbuf[kgdbfdc_rpos]);
        return kgdbfdc_rbuf[kgdbfdc_rpos++];
}

/* push an FDC word from write buffer to TX FIFO */
static void kgdbfdc_push_one(void)
{
        const char *bufs[1] = { kgdbfdc_wbuf };
        struct fdc_word word;
        void __iomem *regs;
        unsigned int i;

        /* Construct a word from any data in buffer */
        word = mips_ejtag_fdc_encode(bufs, &kgdbfdc_wbuflen, 1);
        /* Relocate any remaining data to beginnning of buffer */
        kgdbfdc_wbuflen -= word.bytes;
        for (i = 0; i < kgdbfdc_wbuflen; ++i)
                kgdbfdc_wbuf[i] = kgdbfdc_wbuf[i + word.bytes];

        regs = kgdbfdc_setup();
        if (IS_ERR(regs))
                return;

        /* Busy wait until there's space in fifo */
        while (__raw_readl(regs + REG_FDSTAT) & REG_FDSTAT_TXF)
                ;
        __raw_writel(word.word,
                     regs + REG_FDTX(CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN));
}

/* flush the whole write buffer to the TX FIFO */
static void kgdbfdc_flush(void)
{
        while (kgdbfdc_wbuflen)
                kgdbfdc_push_one();
}

/* write a character into the write buffer, writing out if full */
static void kgdbfdc_write_char(u8 chr)
{
        pr_devel("kgdbfdc w %c\n", chr);
        kgdbfdc_wbuf[kgdbfdc_wbuflen++] = chr;
        if (kgdbfdc_wbuflen >= sizeof(kgdbfdc_wbuf))
                kgdbfdc_push_one();
}

static struct kgdb_io kgdbfdc_io_ops = {
        .name           = "kgdbfdc",
        .read_char      = kgdbfdc_read_char,
        .write_char     = kgdbfdc_write_char,
        .flush          = kgdbfdc_flush,
};

static int __init kgdbfdc_init(void)
{
        kgdb_register_io_module(&kgdbfdc_io_ops);
        return 0;
}
early_initcall(kgdbfdc_init);
#endif