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