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_pinned(&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 static int mips_ejtag_fdc_tty_probe(struct mips_cdmm_device *dev) 883 { 884 int ret, nport; 885 struct mips_ejtag_fdc_tty_port *dport; 886 struct mips_ejtag_fdc_tty *priv; 887 struct tty_driver *driver; 888 unsigned int cfg, tx_fifo; 889 890 priv = devm_kzalloc(&dev->dev, sizeof(*priv), GFP_KERNEL); 891 if (!priv) 892 return -ENOMEM; 893 priv->cpu = dev->cpu; 894 priv->dev = &dev->dev; 895 mips_cdmm_set_drvdata(dev, priv); 896 atomic_set(&priv->xmit_total, 0); 897 raw_spin_lock_init(&priv->lock); 898 899 priv->reg = devm_ioremap_nocache(priv->dev, dev->res.start, 900 resource_size(&dev->res)); 901 if (!priv->reg) { 902 dev_err(priv->dev, "ioremap failed for resource %pR\n", 903 &dev->res); 904 return -ENOMEM; 905 } 906 907 cfg = mips_ejtag_fdc_read(priv, REG_FDCFG); 908 tx_fifo = (cfg & REG_FDCFG_TXFIFOSIZE) >> REG_FDCFG_TXFIFOSIZE_SHIFT; 909 /* Disable interrupts */ 910 cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES); 911 cfg |= REG_FDCFG_TXINTTHRES_DISABLED; 912 cfg |= REG_FDCFG_RXINTTHRES_DISABLED; 913 mips_ejtag_fdc_write(priv, REG_FDCFG, cfg); 914 915 /* Make each port's xmit FIFO big enough to fill FDC TX FIFO */ 916 priv->xmit_size = min(tx_fifo * 4, (unsigned int)SERIAL_XMIT_SIZE); 917 918 driver = tty_alloc_driver(NUM_TTY_CHANNELS, TTY_DRIVER_REAL_RAW); 919 if (IS_ERR(driver)) 920 return PTR_ERR(driver); 921 priv->driver = driver; 922 923 driver->driver_name = "ejtag_fdc"; 924 snprintf(priv->fdc_name, sizeof(priv->fdc_name), "ttyFDC%u", dev->cpu); 925 snprintf(priv->driver_name, sizeof(priv->driver_name), "%sc", 926 priv->fdc_name); 927 driver->name = priv->driver_name; 928 driver->major = 0; /* Auto-allocate */ 929 driver->minor_start = 0; 930 driver->type = TTY_DRIVER_TYPE_SERIAL; 931 driver->subtype = SERIAL_TYPE_NORMAL; 932 driver->init_termios = tty_std_termios; 933 driver->init_termios.c_cflag |= CLOCAL; 934 driver->driver_state = priv; 935 936 tty_set_operations(driver, &mips_ejtag_fdc_tty_ops); 937 for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) { 938 dport = &priv->ports[nport]; 939 dport->driver = priv; 940 tty_port_init(&dport->port); 941 dport->port.ops = &mips_ejtag_fdc_tty_port_ops; 942 raw_spin_lock_init(&dport->rx_lock); 943 spin_lock_init(&dport->xmit_lock); 944 /* The xmit buffer starts empty, i.e. completely written */ 945 init_completion(&dport->xmit_empty); 946 complete(&dport->xmit_empty); 947 } 948 949 /* Set up the console */ 950 mips_ejtag_fdc_con.regs[dev->cpu] = priv->reg; 951 if (dev->cpu == 0) 952 mips_ejtag_fdc_con.tty_drv = driver; 953 954 init_waitqueue_head(&priv->waitqueue); 955 priv->thread = kthread_create(mips_ejtag_fdc_put, priv, priv->fdc_name); 956 if (IS_ERR(priv->thread)) { 957 ret = PTR_ERR(priv->thread); 958 dev_err(priv->dev, "Couldn't create kthread (%d)\n", ret); 959 goto err_destroy_ports; 960 } 961 /* 962 * Bind the writer thread to the right CPU so it can't migrate. 963 * The channels are per-CPU and we want all channel I/O to be on a 964 * single predictable CPU. 965 */ 966 kthread_bind(priv->thread, dev->cpu); 967 wake_up_process(priv->thread); 968 969 /* Look for an FDC IRQ */ 970 priv->irq = -1; 971 if (get_c0_fdc_int) 972 priv->irq = get_c0_fdc_int(); 973 974 /* Try requesting the IRQ */ 975 if (priv->irq >= 0) { 976 /* 977 * IRQF_SHARED, IRQF_NO_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_NO_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 setup_timer(&priv->poll_timer, mips_ejtag_fdc_tty_timer, 1003 (unsigned long)priv); 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_remove(struct mips_cdmm_device *dev) 1049 { 1050 struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev); 1051 struct mips_ejtag_fdc_tty_port *dport; 1052 int nport; 1053 unsigned int cfg; 1054 1055 if (priv->irq >= 0) { 1056 raw_spin_lock_irq(&priv->lock); 1057 cfg = mips_ejtag_fdc_read(priv, REG_FDCFG); 1058 /* Disable interrupts */ 1059 cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES); 1060 cfg |= REG_FDCFG_TXINTTHRES_DISABLED; 1061 cfg |= REG_FDCFG_RXINTTHRES_DISABLED; 1062 mips_ejtag_fdc_write(priv, REG_FDCFG, cfg); 1063 raw_spin_unlock_irq(&priv->lock); 1064 } else { 1065 priv->removing = true; 1066 del_timer_sync(&priv->poll_timer); 1067 } 1068 kthread_stop(priv->thread); 1069 if (dev->cpu == 0) 1070 mips_ejtag_fdc_con.tty_drv = NULL; 1071 tty_unregister_driver(priv->driver); 1072 for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) { 1073 dport = &priv->ports[nport]; 1074 tty_port_destroy(&dport->port); 1075 } 1076 put_tty_driver(priv->driver); 1077 return 0; 1078 } 1079 1080 static int mips_ejtag_fdc_tty_cpu_down(struct mips_cdmm_device *dev) 1081 { 1082 struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev); 1083 unsigned int cfg; 1084 1085 if (priv->irq >= 0) { 1086 raw_spin_lock_irq(&priv->lock); 1087 cfg = mips_ejtag_fdc_read(priv, REG_FDCFG); 1088 /* Disable interrupts */ 1089 cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES); 1090 cfg |= REG_FDCFG_TXINTTHRES_DISABLED; 1091 cfg |= REG_FDCFG_RXINTTHRES_DISABLED; 1092 mips_ejtag_fdc_write(priv, REG_FDCFG, cfg); 1093 raw_spin_unlock_irq(&priv->lock); 1094 } else { 1095 priv->removing = true; 1096 del_timer_sync(&priv->poll_timer); 1097 } 1098 kthread_stop(priv->thread); 1099 1100 return 0; 1101 } 1102 1103 static int mips_ejtag_fdc_tty_cpu_up(struct mips_cdmm_device *dev) 1104 { 1105 struct mips_ejtag_fdc_tty *priv = mips_cdmm_get_drvdata(dev); 1106 unsigned int cfg; 1107 int ret = 0; 1108 1109 if (priv->irq >= 0) { 1110 /* 1111 * IRQ is usable, enable RX interrupt 1112 * This must be before kthread is restarted, as kthread may 1113 * enable TX interrupt. 1114 */ 1115 raw_spin_lock_irq(&priv->lock); 1116 cfg = mips_ejtag_fdc_read(priv, REG_FDCFG); 1117 cfg &= ~(REG_FDCFG_TXINTTHRES | REG_FDCFG_RXINTTHRES); 1118 cfg |= REG_FDCFG_TXINTTHRES_DISABLED; 1119 cfg |= REG_FDCFG_RXINTTHRES_NOTEMPTY; 1120 mips_ejtag_fdc_write(priv, REG_FDCFG, cfg); 1121 raw_spin_unlock_irq(&priv->lock); 1122 } else { 1123 /* Restart poll timer */ 1124 priv->removing = false; 1125 add_timer_on(&priv->poll_timer, dev->cpu); 1126 } 1127 1128 /* Restart the kthread */ 1129 priv->thread = kthread_create(mips_ejtag_fdc_put, priv, priv->fdc_name); 1130 if (IS_ERR(priv->thread)) { 1131 ret = PTR_ERR(priv->thread); 1132 dev_err(priv->dev, "Couldn't re-create kthread (%d)\n", ret); 1133 goto out; 1134 } 1135 /* Bind it back to the right CPU and set it off */ 1136 kthread_bind(priv->thread, dev->cpu); 1137 wake_up_process(priv->thread); 1138 out: 1139 return ret; 1140 } 1141 1142 static struct mips_cdmm_device_id mips_ejtag_fdc_tty_ids[] = { 1143 { .type = 0xfd }, 1144 { } 1145 }; 1146 1147 static struct mips_cdmm_driver mips_ejtag_fdc_tty_driver = { 1148 .drv = { 1149 .name = "mips_ejtag_fdc", 1150 }, 1151 .probe = mips_ejtag_fdc_tty_probe, 1152 .remove = mips_ejtag_fdc_tty_remove, 1153 .cpu_down = mips_ejtag_fdc_tty_cpu_down, 1154 .cpu_up = mips_ejtag_fdc_tty_cpu_up, 1155 .id_table = mips_ejtag_fdc_tty_ids, 1156 }; 1157 module_mips_cdmm_driver(mips_ejtag_fdc_tty_driver); 1158 1159 static int __init mips_ejtag_fdc_init_console(void) 1160 { 1161 return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_con); 1162 } 1163 console_initcall(mips_ejtag_fdc_init_console); 1164 1165 #ifdef CONFIG_MIPS_EJTAG_FDC_EARLYCON 1166 static struct mips_ejtag_fdc_console mips_ejtag_fdc_earlycon = { 1167 .cons = { 1168 .name = "early_fdc", 1169 .write = mips_ejtag_fdc_console_write, 1170 .flags = CON_PRINTBUFFER | CON_BOOT, 1171 .index = CONSOLE_CHANNEL, 1172 }, 1173 .lock = __RAW_SPIN_LOCK_UNLOCKED(mips_ejtag_fdc_earlycon.lock), 1174 }; 1175 1176 int __init setup_early_fdc_console(void) 1177 { 1178 return mips_ejtag_fdc_console_init(&mips_ejtag_fdc_earlycon); 1179 } 1180 #endif 1181 1182 #ifdef CONFIG_MIPS_EJTAG_FDC_KGDB 1183 1184 /* read buffer to allow decompaction */ 1185 static unsigned int kgdbfdc_rbuflen; 1186 static unsigned int kgdbfdc_rpos; 1187 static char kgdbfdc_rbuf[4]; 1188 1189 /* write buffer to allow compaction */ 1190 static unsigned int kgdbfdc_wbuflen; 1191 static char kgdbfdc_wbuf[4]; 1192 1193 static void __iomem *kgdbfdc_setup(void) 1194 { 1195 void __iomem *regs; 1196 unsigned int cpu; 1197 1198 /* Find address, piggy backing off console percpu regs */ 1199 cpu = smp_processor_id(); 1200 regs = mips_ejtag_fdc_con.regs[cpu]; 1201 /* First console output on this CPU? */ 1202 if (!regs) { 1203 regs = mips_cdmm_early_probe(0xfd); 1204 mips_ejtag_fdc_con.regs[cpu] = regs; 1205 } 1206 /* Already tried and failed to find FDC on this CPU? */ 1207 if (IS_ERR(regs)) 1208 return regs; 1209 1210 return regs; 1211 } 1212 1213 /* read a character from the read buffer, filling from FDC RX FIFO */ 1214 static int kgdbfdc_read_char(void) 1215 { 1216 unsigned int stat, channel, data; 1217 void __iomem *regs; 1218 1219 /* No more data, try and read another FDC word from RX FIFO */ 1220 if (kgdbfdc_rpos >= kgdbfdc_rbuflen) { 1221 kgdbfdc_rpos = 0; 1222 kgdbfdc_rbuflen = 0; 1223 1224 regs = kgdbfdc_setup(); 1225 if (IS_ERR(regs)) 1226 return NO_POLL_CHAR; 1227 1228 /* Read next word from KGDB channel */ 1229 do { 1230 stat = __raw_readl(regs + REG_FDSTAT); 1231 1232 /* No data waiting? */ 1233 if (stat & REG_FDSTAT_RXE) 1234 return NO_POLL_CHAR; 1235 1236 /* Read next word */ 1237 channel = (stat & REG_FDSTAT_RXCHAN) >> 1238 REG_FDSTAT_RXCHAN_SHIFT; 1239 data = __raw_readl(regs + REG_FDRX); 1240 } while (channel != CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN); 1241 1242 /* Decode into rbuf */ 1243 kgdbfdc_rbuflen = mips_ejtag_fdc_decode(data, kgdbfdc_rbuf); 1244 } 1245 pr_devel("kgdbfdc r %c\n", kgdbfdc_rbuf[kgdbfdc_rpos]); 1246 return kgdbfdc_rbuf[kgdbfdc_rpos++]; 1247 } 1248 1249 /* push an FDC word from write buffer to TX FIFO */ 1250 static void kgdbfdc_push_one(void) 1251 { 1252 const char *bufs[1] = { kgdbfdc_wbuf }; 1253 struct fdc_word word; 1254 void __iomem *regs; 1255 unsigned int i; 1256 1257 /* Construct a word from any data in buffer */ 1258 word = mips_ejtag_fdc_encode(bufs, &kgdbfdc_wbuflen, 1); 1259 /* Relocate any remaining data to beginnning of buffer */ 1260 kgdbfdc_wbuflen -= word.bytes; 1261 for (i = 0; i < kgdbfdc_wbuflen; ++i) 1262 kgdbfdc_wbuf[i] = kgdbfdc_wbuf[i + word.bytes]; 1263 1264 regs = kgdbfdc_setup(); 1265 if (IS_ERR(regs)) 1266 return; 1267 1268 /* Busy wait until there's space in fifo */ 1269 while (__raw_readl(regs + REG_FDSTAT) & REG_FDSTAT_TXF) 1270 ; 1271 __raw_writel(word.word, 1272 regs + REG_FDTX(CONFIG_MIPS_EJTAG_FDC_KGDB_CHAN)); 1273 } 1274 1275 /* flush the whole write buffer to the TX FIFO */ 1276 static void kgdbfdc_flush(void) 1277 { 1278 while (kgdbfdc_wbuflen) 1279 kgdbfdc_push_one(); 1280 } 1281 1282 /* write a character into the write buffer, writing out if full */ 1283 static void kgdbfdc_write_char(u8 chr) 1284 { 1285 pr_devel("kgdbfdc w %c\n", chr); 1286 kgdbfdc_wbuf[kgdbfdc_wbuflen++] = chr; 1287 if (kgdbfdc_wbuflen >= sizeof(kgdbfdc_wbuf)) 1288 kgdbfdc_push_one(); 1289 } 1290 1291 static struct kgdb_io kgdbfdc_io_ops = { 1292 .name = "kgdbfdc", 1293 .read_char = kgdbfdc_read_char, 1294 .write_char = kgdbfdc_write_char, 1295 .flush = kgdbfdc_flush, 1296 }; 1297 1298 static int __init kgdbfdc_init(void) 1299 { 1300 kgdb_register_io_module(&kgdbfdc_io_ops); 1301 return 0; 1302 } 1303 early_initcall(kgdbfdc_init); 1304 #endif 1305