| 1 | #include "common.h" |
| 2 | |
| 3 | void spin(int loops); |
| 4 | u16 read_frt(void); |
| 5 | |
| 6 | static void do_cmd(u16 cmd, u16 r[6], u32 is_slave) |
| 7 | { |
| 8 | u32 *rl = (u32 *)r; |
| 9 | u32 a, d; |
| 10 | u16 v; |
| 11 | |
| 12 | switch (cmd) |
| 13 | { |
| 14 | case CMD_ECHO: |
| 15 | v = read16(&r[4/2]) ^ (is_slave << 15); |
| 16 | write16(&r[6/2], v); |
| 17 | break; |
| 18 | case CMD_READ_FRT: |
| 19 | v = read_frt(); |
| 20 | write16(&r[4/2], v); |
| 21 | break; |
| 22 | case CMD_READ8: |
| 23 | a = read32(&rl[4/4]); |
| 24 | d = read8(a); |
| 25 | write32(&rl[4/4], d); |
| 26 | break; |
| 27 | case CMD_READ16: |
| 28 | a = read32(&rl[4/4]); |
| 29 | d = read16(a); |
| 30 | write32(&rl[4/4], d); |
| 31 | break; |
| 32 | case CMD_READ32: |
| 33 | a = read32(&rl[4/4]); |
| 34 | d = read32(a); |
| 35 | write32(&rl[4/4], d); |
| 36 | break; |
| 37 | case CMD_WRITE8: |
| 38 | a = read32(&rl[4/4]); |
| 39 | d = read32(&rl[8/4]); |
| 40 | write8(a, d); |
| 41 | break; |
| 42 | case CMD_WRITE16: |
| 43 | a = read32(&rl[4/4]); |
| 44 | d = read32(&rl[8/4]); |
| 45 | write16(a, d); |
| 46 | break; |
| 47 | case CMD_WRITE32: |
| 48 | a = read32(&rl[4/4]); |
| 49 | d = read32(&rl[8/4]); |
| 50 | write32(a, d); |
| 51 | break; |
| 52 | default: |
| 53 | r[2/2]++; // error |
| 54 | mem_barrier(); |
| 55 | break; |
| 56 | } |
| 57 | } |
| 58 | |
| 59 | void main_c(u32 is_slave) |
| 60 | { |
| 61 | u16 *r = (u16 *)0x20004000; |
| 62 | |
| 63 | for (;;) |
| 64 | { |
| 65 | u16 cmd, cmdr; |
| 66 | |
| 67 | mem_barrier(); |
| 68 | cmd = read16(&r[0x20/2]); |
| 69 | mem_barrier(); |
| 70 | cmdr = read16(&r[0x20/2]); |
| 71 | if (cmd == 0 |
| 72 | || cmd != cmdr // documented as "normal" case |
| 73 | || ((cmd & 0x8000) ^ (is_slave << 15)) |
| 74 | || cmd == 0x4d5f) { // 'M_' from BIOS |
| 75 | spin(64); |
| 76 | continue; |
| 77 | } |
| 78 | cmd &= 0x7fff; |
| 79 | do_cmd(cmd, &r[0x20/2], is_slave); |
| 80 | write16(&r[0x20/2], 0); |
| 81 | } |
| 82 | } |
| 83 | |
| 84 | // vim:ts=4:sw=4:expandtab |