mips_ejtag_fdc.c

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