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>
17 #include "ps_protocol.h"
20 volatile unsigned int *gpio;
21 volatile unsigned int *gpclk;
27 unsigned int gpfsel0_o;
28 unsigned int gpfsel1_o;
29 unsigned int gpfsel2_o;
31 static void setup_io() {
32 int fd = open("/dev/mem", O_RDWR | O_SYNC);
34 printf("Unable to open /dev/mem. Run as root using sudo?\n");
38 void *gpio_map = mmap(
39 NULL, // Any adddress in our space will do
40 BCM2708_PERI_SIZE, // Map length
41 PROT_READ | PROT_WRITE, // Enable reading & writting to mapped memory
42 MAP_SHARED, // Shared with other processes
44 BCM2708_PERI_BASE // Offset to GPIO peripheral
49 if (gpio_map == MAP_FAILED) {
50 printf("mmap failed, errno = %d\n", errno);
54 gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
55 gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
58 static void setup_gpclk() {
59 // Enable 200MHz CLK output on GPIO4, adjust divider and pll source depending
61 *(gpclk + (CLK_GP0_CTL / 4)) = CLK_PASSWD | (1 << 5);
63 while ((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7))
66 *(gpclk + (CLK_GP0_DIV / 4)) =
67 CLK_PASSWD | (6 << 12); // divider , 6=200MHz on pi3
69 *(gpclk + (CLK_GP0_CTL / 4)) =
70 CLK_PASSWD | 5 | (1 << 4); // pll? 6=plld, 5=pllc
72 while (((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7)) == 0)
76 SET_GPIO_ALT(PIN_CLK, 0); // gpclk0
79 void ps_setup_protocol() {
83 *(gpio + 10) = 0xffffec;
85 *(gpio + 0) = GPFSEL0_INPUT;
86 *(gpio + 1) = GPFSEL1_INPUT;
87 *(gpio + 2) = GPFSEL2_INPUT;
90 void ps_write_16(unsigned int address, unsigned int data) {
91 *(gpio + 0) = GPFSEL0_OUTPUT;
92 *(gpio + 1) = GPFSEL1_OUTPUT;
93 *(gpio + 2) = GPFSEL2_OUTPUT;
95 *(gpio + 7) = ((data & 0xffff) << 8) | (REG_DATA << PIN_A0);
96 *(gpio + 7) = 1 << PIN_WR;
97 *(gpio + 10) = 1 << PIN_WR;
98 *(gpio + 10) = 0xffffec;
100 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
101 *(gpio + 7) = 1 << PIN_WR;
102 *(gpio + 10) = 1 << PIN_WR;
103 *(gpio + 10) = 0xffffec;
105 *(gpio + 7) = ((0x0000 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
106 *(gpio + 7) = 1 << PIN_WR;
107 *(gpio + 10) = 1 << PIN_WR;
108 *(gpio + 10) = 0xffffec;
110 *(gpio + 0) = GPFSEL0_INPUT;
111 *(gpio + 1) = GPFSEL1_INPUT;
112 *(gpio + 2) = GPFSEL2_INPUT;
114 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
118 void ps_write_8(unsigned int address, unsigned int data) {
119 if ((address & 1) == 0)
120 data = data + (data << 8); // EVEN, A0=0,UDS
122 data = data & 0xff; // ODD , A0=1,LDS
124 *(gpio + 0) = GPFSEL0_OUTPUT;
125 *(gpio + 1) = GPFSEL1_OUTPUT;
126 *(gpio + 2) = GPFSEL2_OUTPUT;
128 *(gpio + 7) = ((data & 0xffff) << 8) | (REG_DATA << PIN_A0);
129 *(gpio + 7) = 1 << PIN_WR;
130 *(gpio + 10) = 1 << PIN_WR;
131 *(gpio + 10) = 0xffffec;
133 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
134 *(gpio + 7) = 1 << PIN_WR;
135 *(gpio + 10) = 1 << PIN_WR;
136 *(gpio + 10) = 0xffffec;
138 *(gpio + 7) = ((0x0100 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
139 *(gpio + 7) = 1 << PIN_WR;
140 *(gpio + 10) = 1 << PIN_WR;
141 *(gpio + 10) = 0xffffec;
143 *(gpio + 0) = GPFSEL0_INPUT;
144 *(gpio + 1) = GPFSEL1_INPUT;
145 *(gpio + 2) = GPFSEL2_INPUT;
147 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
151 void ps_write_32(unsigned int address, unsigned int value) {
152 ps_write_16(address, value >> 16);
153 ps_write_16(address + 2, value);
156 unsigned int ps_read_16(unsigned int address) {
157 *(gpio + 0) = GPFSEL0_OUTPUT;
158 *(gpio + 1) = GPFSEL1_OUTPUT;
159 *(gpio + 2) = GPFSEL2_OUTPUT;
161 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
162 *(gpio + 7) = 1 << PIN_WR;
163 *(gpio + 10) = 1 << PIN_WR;
164 *(gpio + 10) = 0xffffec;
166 *(gpio + 7) = ((0x0200 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
167 *(gpio + 7) = 1 << PIN_WR;
168 *(gpio + 10) = 1 << PIN_WR;
169 *(gpio + 10) = 0xffffec;
171 *(gpio + 0) = GPFSEL0_INPUT;
172 *(gpio + 1) = GPFSEL1_INPUT;
173 *(gpio + 2) = GPFSEL2_INPUT;
175 *(gpio + 7) = (REG_DATA << PIN_A0);
176 *(gpio + 7) = 1 << PIN_RD;
178 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
181 unsigned int value = *(gpio + 13);
183 *(gpio + 10) = 0xffffec;
185 return (value >> 8) & 0xffff;
188 unsigned int ps_read_8(unsigned int address) {
189 *(gpio + 0) = GPFSEL0_OUTPUT;
190 *(gpio + 1) = GPFSEL1_OUTPUT;
191 *(gpio + 2) = GPFSEL2_OUTPUT;
193 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
194 *(gpio + 7) = 1 << PIN_WR;
195 *(gpio + 10) = 1 << PIN_WR;
196 *(gpio + 10) = 0xffffec;
198 *(gpio + 7) = ((0x0300 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
199 *(gpio + 7) = 1 << PIN_WR;
200 *(gpio + 10) = 1 << PIN_WR;
201 *(gpio + 10) = 0xffffec;
203 *(gpio + 0) = GPFSEL0_INPUT;
204 *(gpio + 1) = GPFSEL1_INPUT;
205 *(gpio + 2) = GPFSEL2_INPUT;
207 *(gpio + 7) = (REG_DATA << PIN_A0);
208 *(gpio + 7) = 1 << PIN_RD;
210 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
213 unsigned int value = *(gpio + 13);
215 *(gpio + 10) = 0xffffec;
217 value = (value >> 8) & 0xffff;
219 if ((address & 1) == 0)
220 return (value >> 8) & 0xff; // EVEN, A0=0,UDS
222 return value & 0xff; // ODD , A0=1,LDS
225 unsigned int ps_read_32(unsigned int address) {
226 unsigned int a = ps_read_16(address);
227 unsigned int b = ps_read_16(address + 2);
228 return (a << 16) | b;
231 void ps_write_status_reg(unsigned int value) {
232 *(gpio + 0) = GPFSEL0_OUTPUT;
233 *(gpio + 1) = GPFSEL1_OUTPUT;
234 *(gpio + 2) = GPFSEL2_OUTPUT;
236 *(gpio + 7) = ((value & 0xffff) << 8) | (REG_STATUS << PIN_A0);
238 *(gpio + 7) = 1 << PIN_WR;
239 *(gpio + 7) = 1 << PIN_WR; // delay
240 *(gpio + 10) = 1 << PIN_WR;
241 *(gpio + 10) = 0xffffec;
243 *(gpio + 0) = GPFSEL0_INPUT;
244 *(gpio + 1) = GPFSEL1_INPUT;
245 *(gpio + 2) = GPFSEL2_INPUT;
248 unsigned int ps_read_status_reg() {
249 *(gpio + 7) = (REG_STATUS << PIN_A0);
250 *(gpio + 7) = 1 << PIN_RD;
251 *(gpio + 7) = 1 << PIN_RD;
252 *(gpio + 7) = 1 << PIN_RD;
253 *(gpio + 7) = 1 << PIN_RD;
255 unsigned int value = *(gpio + 13);
257 *(gpio + 10) = 0xffffec;
259 return (value >> 8) & 0xffff;
262 void ps_reset_state_machine() {
263 ps_write_status_reg(STATUS_BIT_INIT);
265 ps_write_status_reg(0);
269 void ps_pulse_reset() {
270 ps_write_status_reg(0);
272 ps_write_status_reg(STATUS_BIT_RESET);
275 unsigned int ps_get_ipl_zero() {
276 unsigned int value = *(gpio + 13);
277 return value & (1 << PIN_IPL_ZERO);
280 #define INT2_ENABLED 1
282 void ps_update_irq() {
283 unsigned int ipl = 0;
285 if (!ps_get_ipl_zero()) {
286 unsigned int status = ps_read_status_reg();
287 ipl = (status & 0xe000) >> 13;
290 /*if (ipl < 2 && INT2_ENABLED && emu_int2_req()) {