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 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 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 */ 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 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 */ 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 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 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 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 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 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 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 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 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 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 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 879 int __weak get_c0_fdc_int(void) 880 { 881 return -1; 882 } 883 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(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 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 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 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 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 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 */ 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 */ 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 */ 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 */ 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 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