2 Original Copyright 2020 Claude Schwarz
3 Code reorganized and rewritten by
4 Niklas Ekström 2021 (https://github.com/niklasekstrom)
14 #include <sys/types.h>
16 #include "ps_protocol.h"
18 volatile unsigned int *gpio;
19 volatile unsigned int *gpclk;
25 unsigned int gpfsel0_o;
26 unsigned int gpfsel1_o;
27 unsigned int gpfsel2_o;
29 static void setup_io() {
30 int fd = open("/dev/mem", O_RDWR | O_SYNC);
32 printf("Unable to open /dev/mem. Run as root using sudo?\n");
36 void *gpio_map = mmap(
37 NULL, // Any adddress in our space will do
38 BCM2708_PERI_SIZE, // Map length
39 PROT_READ | PROT_WRITE, // Enable reading & writting to mapped memory
40 MAP_SHARED, // Shared with other processes
42 BCM2708_PERI_BASE // Offset to GPIO peripheral
47 if (gpio_map == MAP_FAILED) {
48 printf("mmap failed, errno = %d\n", errno);
52 gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
53 gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
56 static void setup_gpclk() {
57 // Enable 200MHz CLK output on GPIO4, adjust divider and pll source depending
59 *(gpclk + (CLK_GP0_CTL / 4)) = CLK_PASSWD | (1 << 5);
61 while ((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7))
64 *(gpclk + (CLK_GP0_DIV / 4)) =
65 CLK_PASSWD | (6 << 12); // divider , 6=200MHz on pi3
67 *(gpclk + (CLK_GP0_CTL / 4)) =
68 CLK_PASSWD | 5 | (1 << 4); // pll? 6=plld, 5=pllc
70 while (((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7)) == 0)
74 SET_GPIO_ALT(PIN_CLK, 0); // gpclk0
77 void ps_setup_protocol() {
81 *(gpio + 10) = 0xffffec;
83 *(gpio + 0) = GPFSEL0_INPUT;
84 *(gpio + 1) = GPFSEL1_INPUT;
85 *(gpio + 2) = GPFSEL2_INPUT;
88 void ps_write_16(unsigned int address, unsigned int data) {
89 *(gpio + 0) = GPFSEL0_OUTPUT;
90 *(gpio + 1) = GPFSEL1_OUTPUT;
91 *(gpio + 2) = GPFSEL2_OUTPUT;
93 *(gpio + 7) = ((data & 0xffff) << 8) | (REG_DATA << PIN_A0);
94 *(gpio + 7) = 1 << PIN_WR;
95 *(gpio + 10) = 1 << PIN_WR;
96 *(gpio + 10) = 0xffffec;
98 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
99 *(gpio + 7) = 1 << PIN_WR;
100 *(gpio + 10) = 1 << PIN_WR;
101 *(gpio + 10) = 0xffffec;
103 *(gpio + 7) = ((0x0000 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
104 *(gpio + 7) = 1 << PIN_WR;
105 *(gpio + 10) = 1 << PIN_WR;
106 *(gpio + 10) = 0xffffec;
108 *(gpio + 0) = GPFSEL0_INPUT;
109 *(gpio + 1) = GPFSEL1_INPUT;
110 *(gpio + 2) = GPFSEL2_INPUT;
112 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
116 void ps_write_8(unsigned int address, unsigned int data) {
117 if ((address & 1) == 0)
118 data = data + (data << 8); // EVEN, A0=0,UDS
120 data = data & 0xff; // ODD , A0=1,LDS
122 *(gpio + 0) = GPFSEL0_OUTPUT;
123 *(gpio + 1) = GPFSEL1_OUTPUT;
124 *(gpio + 2) = GPFSEL2_OUTPUT;
126 *(gpio + 7) = ((data & 0xffff) << 8) | (REG_DATA << PIN_A0);
127 *(gpio + 7) = 1 << PIN_WR;
128 *(gpio + 10) = 1 << PIN_WR;
129 *(gpio + 10) = 0xffffec;
131 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
132 *(gpio + 7) = 1 << PIN_WR;
133 *(gpio + 10) = 1 << PIN_WR;
134 *(gpio + 10) = 0xffffec;
136 *(gpio + 7) = ((0x0100 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
137 *(gpio + 7) = 1 << PIN_WR;
138 *(gpio + 10) = 1 << PIN_WR;
139 *(gpio + 10) = 0xffffec;
141 *(gpio + 0) = GPFSEL0_INPUT;
142 *(gpio + 1) = GPFSEL1_INPUT;
143 *(gpio + 2) = GPFSEL2_INPUT;
145 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
149 void ps_write_32(unsigned int address, unsigned int value) {
150 ps_write_16(address, value >> 16);
151 ps_write_16(address + 2, value);
154 unsigned int ps_read_16(unsigned int address) {
155 *(gpio + 0) = GPFSEL0_OUTPUT;
156 *(gpio + 1) = GPFSEL1_OUTPUT;
157 *(gpio + 2) = GPFSEL2_OUTPUT;
159 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
160 *(gpio + 7) = 1 << PIN_WR;
161 *(gpio + 10) = 1 << PIN_WR;
162 *(gpio + 10) = 0xffffec;
164 *(gpio + 7) = ((0x0200 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
165 *(gpio + 7) = 1 << PIN_WR;
166 *(gpio + 10) = 1 << PIN_WR;
167 *(gpio + 10) = 0xffffec;
169 *(gpio + 0) = GPFSEL0_INPUT;
170 *(gpio + 1) = GPFSEL1_INPUT;
171 *(gpio + 2) = GPFSEL2_INPUT;
173 *(gpio + 7) = (REG_DATA << PIN_A0);
174 *(gpio + 7) = 1 << PIN_RD;
176 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
179 unsigned int value = *(gpio + 13);
181 *(gpio + 10) = 0xffffec;
183 return (value >> 8) & 0xffff;
186 unsigned int ps_read_8(unsigned int address) {
187 *(gpio + 0) = GPFSEL0_OUTPUT;
188 *(gpio + 1) = GPFSEL1_OUTPUT;
189 *(gpio + 2) = GPFSEL2_OUTPUT;
191 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
192 *(gpio + 7) = 1 << PIN_WR;
193 *(gpio + 10) = 1 << PIN_WR;
194 *(gpio + 10) = 0xffffec;
196 *(gpio + 7) = ((0x0300 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
197 *(gpio + 7) = 1 << PIN_WR;
198 *(gpio + 10) = 1 << PIN_WR;
199 *(gpio + 10) = 0xffffec;
201 *(gpio + 0) = GPFSEL0_INPUT;
202 *(gpio + 1) = GPFSEL1_INPUT;
203 *(gpio + 2) = GPFSEL2_INPUT;
205 *(gpio + 7) = (REG_DATA << PIN_A0);
206 *(gpio + 7) = 1 << PIN_RD;
208 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
211 unsigned int value = *(gpio + 13);
213 *(gpio + 10) = 0xffffec;
215 value = (value >> 8) & 0xffff;
217 if ((address & 1) == 0)
218 return (value >> 8) & 0xff; // EVEN, A0=0,UDS
220 return value & 0xff; // ODD , A0=1,LDS
223 unsigned int ps_read_32(unsigned int address) {
224 unsigned int a = ps_read_16(address);
225 unsigned int b = ps_read_16(address + 2);
226 return (a << 16) | b;
229 void ps_write_status_reg(unsigned int value) {
230 *(gpio + 0) = GPFSEL0_OUTPUT;
231 *(gpio + 1) = GPFSEL1_OUTPUT;
232 *(gpio + 2) = GPFSEL2_OUTPUT;
234 *(gpio + 7) = ((value & 0xffff) << 8) | (REG_STATUS << PIN_A0);
236 *(gpio + 7) = 1 << PIN_WR;
237 *(gpio + 7) = 1 << PIN_WR; // delay
238 *(gpio + 10) = 1 << PIN_WR;
239 *(gpio + 10) = 0xffffec;
241 *(gpio + 0) = GPFSEL0_INPUT;
242 *(gpio + 1) = GPFSEL1_INPUT;
243 *(gpio + 2) = GPFSEL2_INPUT;
246 unsigned int ps_read_status_reg() {
247 *(gpio + 7) = (REG_STATUS << PIN_A0);
248 *(gpio + 7) = 1 << PIN_RD;
249 *(gpio + 7) = 1 << PIN_RD;
250 *(gpio + 7) = 1 << PIN_RD;
251 *(gpio + 7) = 1 << PIN_RD;
253 unsigned int value = *(gpio + 13);
255 *(gpio + 10) = 0xffffec;
257 return (value >> 8) & 0xffff;
260 void ps_reset_state_machine() {
261 ps_write_status_reg(STATUS_BIT_INIT);
263 ps_write_status_reg(0);
267 void ps_pulse_reset() {
268 ps_write_status_reg(0);
270 ps_write_status_reg(STATUS_BIT_RESET);
273 unsigned int ps_get_ipl_zero() {
274 unsigned int value = *(gpio + 13);
275 return value & (1 << PIN_IPL_ZERO);
278 #define INT2_ENABLED 1
280 void ps_update_irq() {
281 unsigned int ipl = 0;
283 if (!ps_get_ipl_zero()) {
284 unsigned int status = ps_read_status_reg();
285 ipl = (status & 0xe000) >> 13;
288 /*if (ipl < 2 && INT2_ENABLED && emu_int2_req()) {