+
+#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 = ioread32(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 = ioread32(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 (ioread32(regs + REG_FDSTAT) & REG_FDSTAT_TXF)
+ ;
+ iowrite32(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