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