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