mips_ejtag_fdc.c 35 KB

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