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 = from_timer(priv, t, poll_timer); 687 688 mips_ejtag_fdc_handle(priv); 689 if (!priv->removing) 690 mod_timer(&priv->poll_timer, jiffies + FDC_TTY_POLL); 691} 692 693/* TTY Port operations */ 694 695static int mips_ejtag_fdc_tty_port_activate(struct tty_port *port, 696 struct tty_struct *tty) 697{ 698 struct mips_ejtag_fdc_tty_port *dport = 699 container_of(port, struct mips_ejtag_fdc_tty_port, port); 700 void *rx_buf; 701 702 /* Allocate the buffer we use for writing data */ 703 if (tty_port_alloc_xmit_buf(port) < 0) 704 goto err; 705 706 /* Allocate the buffer we use for reading data */ 707 rx_buf = kzalloc(RX_BUF_SIZE, GFP_KERNEL); 708 if (!rx_buf) 709 goto err_free_xmit; 710 711 raw_spin_lock_irq(&dport->rx_lock); 712 dport->rx_buf = rx_buf; 713 raw_spin_unlock_irq(&dport->rx_lock); 714 715 return 0; 716err_free_xmit: 717 tty_port_free_xmit_buf(port); 718err: 719 return -ENOMEM; 720} 721 722static void mips_ejtag_fdc_tty_port_shutdown(struct tty_port *port) 723{ 724 struct mips_ejtag_fdc_tty_port *dport = 725 container_of(port, struct mips_ejtag_fdc_tty_port, port); 726 struct mips_ejtag_fdc_tty *priv = dport->driver; 727 void *rx_buf; 728 unsigned int count; 729 730 spin_lock(&dport->xmit_lock); 731 count = dport->xmit_cnt; 732 spin_unlock(&dport->xmit_lock); 733 if (count) { 734 /* 735 * There's still data to write out, so wake and wait for the 736 * writer thread to drain the buffer. 737 */ 738 wake_up_interruptible(&priv->waitqueue); 739 wait_for_completion(&dport->xmit_empty); 740 } 741 742 /* Null the read buffer (timer could still be running!) */ 743 raw_spin_lock_irq(&dport->rx_lock); 744 rx_buf = dport->rx_buf; 745 dport->rx_buf = NULL; 746 raw_spin_unlock_irq(&dport->rx_lock); 747 /* Free the read buffer */ 748 kfree(rx_buf); 749 750 /* Free the write buffer */ 751 tty_port_free_xmit_buf(port); 752} 753 754static const struct tty_port_operations mips_ejtag_fdc_tty_port_ops = { 755 .activate = mips_ejtag_fdc_tty_port_activate, 756 .shutdown = mips_ejtag_fdc_tty_port_shutdown, 757}; 758 759/* TTY operations */ 760 761static int mips_ejtag_fdc_tty_install(struct tty_driver *driver, 762 struct tty_struct *tty) 763{ 764 struct mips_ejtag_fdc_tty *priv = driver->driver_state; 765 766 tty->driver_data = &priv->ports[tty->index]; 767 return tty_port_install(&priv->ports[tty->index].port, driver, tty); 768} 769 770static int mips_ejtag_fdc_tty_open(struct tty_struct *tty, struct file *filp) 771{ 772 return tty_port_open(tty->port, tty, filp); 773} 774 775static void mips_ejtag_fdc_tty_close(struct tty_struct *tty, struct file *filp) 776{ 777 return tty_port_close(tty->port, tty, filp); 778} 779 780static void mips_ejtag_fdc_tty_hangup(struct tty_struct *tty) 781{ 782 struct mips_ejtag_fdc_tty_port *dport = tty->driver_data; 783 struct mips_ejtag_fdc_tty *priv = dport->driver; 784 785 /* Drop any data in the xmit buffer */ 786 spin_lock(&dport->xmit_lock); 787 if (dport->xmit_cnt) { 788 atomic_sub(dport->xmit_cnt, &priv->xmit_total); 789 dport->xmit_cnt = 0; 790 dport->xmit_head = 0; 791 dport->xmit_tail = 0; 792 complete(&dport->xmit_empty); 793 } 794 spin_unlock(&dport->xmit_lock); 795 796 tty_port_hangup(tty->port); 797} 798 799static ssize_t mips_ejtag_fdc_tty_write(struct tty_struct *tty, const u8 *buf, 800 size_t total) 801{ 802 int count, block; 803 struct mips_ejtag_fdc_tty_port *dport = tty->driver_data; 804 struct mips_ejtag_fdc_tty *priv = dport->driver; 805 806 /* 807 * Write to output buffer. 808 * 809 * The reason that we asynchronously write the buffer is because if we 810 * were to write the buffer synchronously then because the channels are 811 * per-CPU the buffer would be written to the channel of whatever CPU 812 * we're running on. 813 * 814 * What we actually want to happen is have all input and output done on 815 * one CPU. 816 */ 817 spin_lock(&dport->xmit_lock); 818 /* Work out how many bytes we can write to the xmit buffer */ 819 total = min_t(size_t, total, priv->xmit_size - dport->xmit_cnt); 820 atomic_add(total, &priv->xmit_total); 821 dport->xmit_cnt += total; 822 /* Write the actual bytes (may need splitting if it wraps) */ 823 for (count = total; count; count -= block) { 824 block = min(count, (int)(priv->xmit_size - dport->xmit_head)); 825 memcpy(dport->port.xmit_buf + dport->xmit_head, buf, block); 826 dport->xmit_head += block; 827 if (dport->xmit_head >= priv->xmit_size) 828 dport->xmit_head -= priv->xmit_size; 829 buf += block; 830 } 831 count = dport->xmit_cnt; 832 /* Xmit buffer no longer empty? */ 833 if (count) 834 reinit_completion(&dport->xmit_empty); 835 spin_unlock(&dport->xmit_lock); 836 837 /* Wake up the kthread */ 838 if (total) 839 wake_up_interruptible(&priv->waitqueue); 840 return total; 841} 842 843static unsigned int mips_ejtag_fdc_tty_write_room(struct tty_struct *tty) 844{ 845 struct mips_ejtag_fdc_tty_port *dport = tty->driver_data; 846 struct mips_ejtag_fdc_tty *priv = dport->driver; 847 unsigned int room; 848 849 /* Report the space in the xmit buffer */ 850 spin_lock(&dport->xmit_lock); 851 room = priv->xmit_size - dport->xmit_cnt; 852 spin_unlock(&dport->xmit_lock); 853 854 return room; 855} 856 857static unsigned int mips_ejtag_fdc_tty_chars_in_buffer(struct tty_struct *tty) 858{ 859 struct mips_ejtag_fdc_tty_port *dport = tty->driver_data; 860 unsigned int chars; 861 862 /* Report the number of bytes in the xmit buffer */ 863 spin_lock(&dport->xmit_lock); 864 chars = dport->xmit_cnt; 865 spin_unlock(&dport->xmit_lock); 866 867 return chars; 868} 869 870static const struct tty_operations mips_ejtag_fdc_tty_ops = { 871 .install = mips_ejtag_fdc_tty_install, 872 .open = mips_ejtag_fdc_tty_open, 873 .close = mips_ejtag_fdc_tty_close, 874 .hangup = mips_ejtag_fdc_tty_hangup, 875 .write = mips_ejtag_fdc_tty_write, 876 .write_room = mips_ejtag_fdc_tty_write_room, 877 .chars_in_buffer = mips_ejtag_fdc_tty_chars_in_buffer, 878}; 879 880int __weak get_c0_fdc_int(void) 881{ 882 return -1; 883} 884 885static int mips_ejtag_fdc_tty_probe(struct mips_cdmm_device *dev) 886{ 887 int ret, nport; 888 struct mips_ejtag_fdc_tty_port *dport; 889 struct mips_ejtag_fdc_tty *priv; 890 struct tty_driver *driver; 891 unsigned int cfg, tx_fifo; 892 893 priv = devm_kzalloc(&dev->dev, sizeof(*priv), GFP_KERNEL); 894 if (!priv) 895 return -ENOMEM; 896 priv->cpu = dev->cpu; 897 priv->dev = &dev->dev; 898 mips_cdmm_set_drvdata(dev, priv); 899 atomic_set(&priv->xmit_total, 0); 900 raw_spin_lock_init(&priv->lock); 901 902 priv->reg = devm_ioremap(priv->dev, dev->res.start, 903 resource_size(&dev->res)); 904 if (!priv->reg) { 905 dev_err(priv->dev, "ioremap failed for resource %pR\n", 906 &dev->res); 907 return -ENOMEM; 908 } 909 910 cfg = mips_ejtag_fdc_read(priv, REG_FDCFG); 911 tx_fifo = (cfg & REG_FDCFG_TXFIFOSIZE) >> REG_FDCFG_TXFIFOSIZE_SHIFT; 912 /* Disable interrupts */ 913 cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES); 914 cfg |= REG_FDCFG_TXINTTHRES_DISABLED; 915 cfg |= REG_FDCFG_RXINTTHRES_DISABLED; 916 mips_ejtag_fdc_write(priv, REG_FDCFG, cfg); 917 918 /* Make each port's xmit FIFO big enough to fill FDC TX FIFO */ 919 priv->xmit_size = min(tx_fifo * 4, (unsigned int)UART_XMIT_SIZE); 920 921 driver = tty_alloc_driver(NUM_TTY_CHANNELS, TTY_DRIVER_REAL_RAW); 922 if (IS_ERR(driver)) 923 return PTR_ERR(driver); 924 priv->driver = driver; 925 926 driver->driver_name = "ejtag_fdc"; 927 snprintf(priv->fdc_name, sizeof(priv->fdc_name), "ttyFDC%u", dev->cpu); 928 snprintf(priv->driver_name, sizeof(priv->driver_name), "%sc", 929 priv->fdc_name); 930 driver->name = priv->driver_name; 931 driver->major = 0; /* Auto-allocate */ 932 driver->minor_start = 0; 933 driver->type = TTY_DRIVER_TYPE_SERIAL; 934 driver->subtype = SERIAL_TYPE_NORMAL; 935 driver->init_termios = tty_std_termios; 936 driver->init_termios.c_cflag |= CLOCAL; 937 driver->driver_state = priv; 938 939 tty_set_operations(driver, &mips_ejtag_fdc_tty_ops); 940 for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) { 941 dport = &priv->ports[nport]; 942 dport->driver = priv; 943 tty_port_init(&dport->port); 944 dport->port.ops = &mips_ejtag_fdc_tty_port_ops; 945 raw_spin_lock_init(&dport->rx_lock); 946 spin_lock_init(&dport->xmit_lock); 947 /* The xmit buffer starts empty, i.e. completely written */ 948 init_completion(&dport->xmit_empty); 949 complete(&dport->xmit_empty); 950 } 951 952 /* Set up the console */ 953 mips_ejtag_fdc_con.regs[dev->cpu] = priv->reg; 954 if (dev->cpu == 0) 955 mips_ejtag_fdc_con.tty_drv = driver; 956 957 init_waitqueue_head(&priv->waitqueue); 958 /* 959 * Bind the writer thread to the right CPU so it can't migrate. 960 * The channels are per-CPU and we want all channel I/O to be on a 961 * single predictable CPU. 962 */ 963 priv->thread = kthread_run_on_cpu(mips_ejtag_fdc_put, priv, 964 dev->cpu, "ttyFDC/%u"); 965 if (IS_ERR(priv->thread)) { 966 ret = PTR_ERR(priv->thread); 967 dev_err(priv->dev, "Couldn't create kthread (%d)\n", ret); 968 goto err_destroy_ports; 969 } 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 1022err_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); 1037err_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 tty_driver_kref_put(priv->driver); 1045 return ret; 1046} 1047 1048static 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 1071static 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 /* Bind it back to the right CPU and set it off */ 1098 priv->thread = kthread_run_on_cpu(mips_ejtag_fdc_put, priv, 1099 dev->cpu, "ttyFDC/%u"); 1100 if (IS_ERR(priv->thread)) { 1101 ret = PTR_ERR(priv->thread); 1102 dev_err(priv->dev, "Couldn't re-create kthread (%d)\n", ret); 1103 goto out; 1104 } 1105out: 1106 return ret; 1107} 1108 1109static const struct mips_cdmm_device_id mips_ejtag_fdc_tty_ids[] = { 1110 { .type = 0xfd }, 1111 { } 1112}; 1113 1114static struct mips_cdmm_driver mips_ejtag_fdc_tty_driver = { 1115 .drv = { 1116 .name = "mips_ejtag_fdc", 1117 }, 1118 .probe = mips_ejtag_fdc_tty_probe, 1119 .cpu_down = mips_ejtag_fdc_tty_cpu_down, 1120 .cpu_up = mips_ejtag_fdc_tty_cpu_up, 1121 .id_table = mips_ejtag_fdc_tty_ids, 1122}; 1123builtin_mips_cdmm_driver(mips_ejtag_fdc_tty_driver); 1124 1125static int __init mips_ejtag_fdc_init_console(void) 1126{ 1127 return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_con); 1128} 1129console_initcall(mips_ejtag_fdc_init_console); 1130 1131#ifdef CONFIG_MIPS_EJTAG_FDC_EARLYCON 1132static struct mips_ejtag_fdc_console mips_ejtag_fdc_earlycon = { 1133 .cons = { 1134 .name = "early_fdc", 1135 .write = mips_ejtag_fdc_console_write, 1136 .flags = CON_PRINTBUFFER | CON_BOOT, 1137 .index = CONSOLE_CHANNEL, 1138 }, 1139 .lock = __RAW_SPIN_LOCK_UNLOCKED(mips_ejtag_fdc_earlycon.lock), 1140}; 1141 1142int __init setup_early_fdc_console(void) 1143{ 1144 return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_earlycon); 1145} 1146#endif 1147 1148#ifdef CONFIG_MIPS_EJTAG_FDC_KGDB 1149 1150/* read buffer to allow decompaction */ 1151static unsigned int kgdbfdc_rbuflen; 1152static unsigned int kgdbfdc_rpos; 1153static char kgdbfdc_rbuf[4]; 1154 1155/* write buffer to allow compaction */ 1156static unsigned int kgdbfdc_wbuflen; 1157static char kgdbfdc_wbuf[4]; 1158 1159static void __iomem *kgdbfdc_setup(void) 1160{ 1161 void __iomem *regs; 1162 unsigned int cpu; 1163 1164 /* Find address, piggy backing off console percpu regs */ 1165 cpu = smp_processor_id(); 1166 regs = mips_ejtag_fdc_con.regs[cpu]; 1167 /* First console output on this CPU? */ 1168 if (!regs) { 1169 regs = mips_cdmm_early_probe(0xfd); 1170 mips_ejtag_fdc_con.regs[cpu] = regs; 1171 } 1172 /* Already tried and failed to find FDC on this CPU? */ 1173 if (IS_ERR(regs)) 1174 return regs; 1175 1176 return regs; 1177} 1178 1179/* read a character from the read buffer, filling from FDC RX FIFO */ 1180static int kgdbfdc_read_char(void) 1181{ 1182 unsigned int stat, channel, data; 1183 void __iomem *regs; 1184 1185 /* No more data, try and read another FDC word from RX FIFO */ 1186 if (kgdbfdc_rpos >= kgdbfdc_rbuflen) { 1187 kgdbfdc_rpos = 0; 1188 kgdbfdc_rbuflen = 0; 1189 1190 regs = kgdbfdc_setup(); 1191 if (IS_ERR(regs)) 1192 return NO_POLL_CHAR; 1193 1194 /* Read next word from KGDB channel */ 1195 do { 1196 stat = __raw_readl(regs + REG_FDSTAT); 1197 1198 /* No data waiting? */ 1199 if (stat & REG_FDSTAT_RXE) 1200 return NO_POLL_CHAR; 1201 1202 /* Read next word */ 1203 channel = (stat & REG_FDSTAT_RXCHAN) >> 1204 REG_FDSTAT_RXCHAN_SHIFT; 1205 data = __raw_readl(regs + REG_FDRX); 1206 } while (channel != CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN); 1207 1208 /* Decode into rbuf */ 1209 kgdbfdc_rbuflen = mips_ejtag_fdc_decode(data, kgdbfdc_rbuf); 1210 } 1211 pr_devel("kgdbfdc r %c\n", kgdbfdc_rbuf[kgdbfdc_rpos]); 1212 return kgdbfdc_rbuf[kgdbfdc_rpos++]; 1213} 1214 1215/* push an FDC word from write buffer to TX FIFO */ 1216static void kgdbfdc_push_one(void) 1217{ 1218 const char *bufs[1] = { kgdbfdc_wbuf }; 1219 struct fdc_word word; 1220 void __iomem *regs; 1221 unsigned int i; 1222 1223 /* Construct a word from any data in buffer */ 1224 word = mips_ejtag_fdc_encode(bufs, &kgdbfdc_wbuflen, 1); 1225 /* Relocate any remaining data to beginning of buffer */ 1226 kgdbfdc_wbuflen -= word.bytes; 1227 for (i = 0; i < kgdbfdc_wbuflen; ++i) 1228 kgdbfdc_wbuf[i] = kgdbfdc_wbuf[i + word.bytes]; 1229 1230 regs = kgdbfdc_setup(); 1231 if (IS_ERR(regs)) 1232 return; 1233 1234 /* Busy wait until there's space in fifo */ 1235 while (__raw_readl(regs + REG_FDSTAT) & REG_FDSTAT_TXF) 1236 ; 1237 __raw_writel(word.word, 1238 regs + REG_FDTX(CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN)); 1239} 1240 1241/* flush the whole write buffer to the TX FIFO */ 1242static void kgdbfdc_flush(void) 1243{ 1244 while (kgdbfdc_wbuflen) 1245 kgdbfdc_push_one(); 1246} 1247 1248/* write a character into the write buffer, writing out if full */ 1249static void kgdbfdc_write_char(u8 chr) 1250{ 1251 pr_devel("kgdbfdc w %c\n", chr); 1252 kgdbfdc_wbuf[kgdbfdc_wbuflen++] = chr; 1253 if (kgdbfdc_wbuflen >= sizeof(kgdbfdc_wbuf)) 1254 kgdbfdc_push_one(); 1255} 1256 1257static struct kgdb_io kgdbfdc_io_ops = { 1258 .name = "kgdbfdc", 1259 .read_char = kgdbfdc_read_char, 1260 .write_char = kgdbfdc_write_char, 1261 .flush = kgdbfdc_flush, 1262}; 1263 1264static int __init kgdbfdc_init(void) 1265{ 1266 kgdb_register_io_module(&kgdbfdc_io_ops); 1267 return 0; 1268} 1269early_initcall(kgdbfdc_init); 1270#endif 1271