9d39a80e |
1 | #include "common.h" |
2 | |
3 | void spin(int loops); |
71b41fdd |
4 | u16 read_frt(void); |
9d39a80e |
5 | |
06d7984c |
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 | |
9d39a80e |
59 | void main_c(u32 is_slave) |
60 | { |
06d7984c |
61 | u16 *r = (u16 *)0x20004000; |
9d39a80e |
62 | |
63 | for (;;) |
64 | { |
71b41fdd |
65 | u16 cmd, cmdr; |
9d39a80e |
66 | |
67 | mem_barrier(); |
68 | cmd = read16(&r[0x20/2]); |
71b41fdd |
69 | mem_barrier(); |
70 | cmdr = read16(&r[0x20/2]); |
06d7984c |
71 | if (cmd == 0 |
72 | || cmd != cmdr // documented as "normal" case |
73 | || ((cmd & 0x8000) ^ (is_slave << 15)) |
74 | || cmd == 0x4d5f) { // 'M_' from BIOS |
9d39a80e |
75 | spin(64); |
76 | continue; |
77 | } |
78 | cmd &= 0x7fff; |
06d7984c |
79 | do_cmd(cmd, &r[0x20/2], is_slave); |
9d39a80e |
80 | write16(&r[0x20/2], 0); |
81 | } |
82 | } |
83 | |
84 | // vim:ts=4:sw=4:expandtab |