1 // SPDX-License-Identifier: MIT
4 Original Copyright 2020 Claude Schwarz
5 Code reorganized and rewritten by
6 Niklas Ekström 2021 (https://github.com/niklasekstrom)
16 #include <sys/types.h>
19 #include "ps_protocol.h"
22 volatile unsigned int *gpio;
23 volatile unsigned int *gpclk;
29 unsigned int gpfsel0_o;
30 unsigned int gpfsel1_o;
31 unsigned int gpfsel2_o;
33 static void setup_io() {
34 int fd = open("/dev/mem", O_RDWR | O_SYNC);
36 printf("Unable to open /dev/mem. Run as root using sudo?\n");
40 void *gpio_map = mmap(
41 NULL, // Any adddress in our space will do
42 BCM2708_PERI_SIZE, // Map length
43 PROT_READ | PROT_WRITE, // Enable reading & writting to mapped memory
44 MAP_SHARED, // Shared with other processes
46 BCM2708_PERI_BASE // Offset to GPIO peripheral
51 if (gpio_map == MAP_FAILED) {
52 printf("mmap failed, errno = %d\n", errno);
56 gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
57 gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
60 static void setup_gpclk() {
61 // Enable 200MHz CLK output on GPIO4, adjust divider and pll source depending
63 *(gpclk + (CLK_GP0_CTL / 4)) = CLK_PASSWD | (1 << 5);
65 while ((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7))
68 *(gpclk + (CLK_GP0_DIV / 4)) =
69 CLK_PASSWD | (6 << 12); // divider , 6=200MHz on pi3
71 *(gpclk + (CLK_GP0_CTL / 4)) =
72 CLK_PASSWD | 5 | (1 << 4); // pll? 6=plld, 5=pllc
74 while (((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7)) == 0)
78 SET_GPIO_ALT(PIN_CLK, 0); // gpclk0
81 void ps_setup_protocol() {
85 *(gpio + 10) = 0xffffec;
87 *(gpio + 0) = GPFSEL0_INPUT;
88 *(gpio + 1) = GPFSEL1_INPUT;
89 *(gpio + 2) = GPFSEL2_INPUT;
92 void ps_write_16(unsigned int address, unsigned int data) {
93 *(gpio + 0) = GPFSEL0_OUTPUT;
94 *(gpio + 1) = GPFSEL1_OUTPUT;
95 *(gpio + 2) = GPFSEL2_OUTPUT;
97 *(gpio + 7) = ((data & 0xffff) << 8) | (REG_DATA << PIN_A0);
98 *(gpio + 7) = 1 << PIN_WR;
99 *(gpio + 10) = 1 << PIN_WR;
100 *(gpio + 10) = 0xffffec;
102 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
103 *(gpio + 7) = 1 << PIN_WR;
104 *(gpio + 10) = 1 << PIN_WR;
105 *(gpio + 10) = 0xffffec;
107 *(gpio + 7) = ((0x0000 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
108 *(gpio + 7) = 1 << PIN_WR;
109 *(gpio + 10) = 1 << PIN_WR;
110 *(gpio + 10) = 0xffffec;
112 *(gpio + 0) = GPFSEL0_INPUT;
113 *(gpio + 1) = GPFSEL1_INPUT;
114 *(gpio + 2) = GPFSEL2_INPUT;
116 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS)) {}
119 void ps_write_8(unsigned int address, unsigned int data) {
120 if ((address & 1) == 0)
121 data = data + (data << 8); // EVEN, A0=0,UDS
123 data = data & 0xff; // ODD , A0=1,LDS
125 *(gpio + 0) = GPFSEL0_OUTPUT;
126 *(gpio + 1) = GPFSEL1_OUTPUT;
127 *(gpio + 2) = GPFSEL2_OUTPUT;
129 *(gpio + 7) = ((data & 0xffff) << 8) | (REG_DATA << PIN_A0);
130 *(gpio + 7) = 1 << PIN_WR;
131 *(gpio + 10) = 1 << PIN_WR;
132 *(gpio + 10) = 0xffffec;
134 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
135 *(gpio + 7) = 1 << PIN_WR;
136 *(gpio + 10) = 1 << PIN_WR;
137 *(gpio + 10) = 0xffffec;
139 *(gpio + 7) = ((0x0100 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
140 *(gpio + 7) = 1 << PIN_WR;
141 *(gpio + 10) = 1 << PIN_WR;
142 *(gpio + 10) = 0xffffec;
144 *(gpio + 0) = GPFSEL0_INPUT;
145 *(gpio + 1) = GPFSEL1_INPUT;
146 *(gpio + 2) = GPFSEL2_INPUT;
148 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)) {}
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)) {}
209 unsigned int value = *(gpio + 13);
211 *(gpio + 10) = 0xffffec;
213 value = (value >> 8) & 0xffff;
215 if ((address & 1) == 0)
216 return (value >> 8) & 0xff; // EVEN, A0=0,UDS
218 return value & 0xff; // ODD , A0=1,LDS
221 unsigned int ps_read_32(unsigned int address) {
222 unsigned int a = ps_read_16(address);
223 unsigned int b = ps_read_16(address + 2);
224 return (a << 16) | b;
227 void ps_write_status_reg(unsigned int value) {
228 *(gpio + 0) = GPFSEL0_OUTPUT;
229 *(gpio + 1) = GPFSEL1_OUTPUT;
230 *(gpio + 2) = GPFSEL2_OUTPUT;
232 *(gpio + 7) = ((value & 0xffff) << 8) | (REG_STATUS << PIN_A0);
234 *(gpio + 7) = 1 << PIN_WR;
235 *(gpio + 7) = 1 << PIN_WR; // delay
236 *(gpio + 10) = 1 << PIN_WR;
237 *(gpio + 10) = 0xffffec;
239 *(gpio + 0) = GPFSEL0_INPUT;
240 *(gpio + 1) = GPFSEL1_INPUT;
241 *(gpio + 2) = GPFSEL2_INPUT;
244 unsigned int ps_read_status_reg() {
245 *(gpio + 7) = (REG_STATUS << PIN_A0);
246 *(gpio + 7) = 1 << PIN_RD;
247 *(gpio + 7) = 1 << PIN_RD;
248 *(gpio + 7) = 1 << PIN_RD;
249 *(gpio + 7) = 1 << PIN_RD;
251 unsigned int value = *(gpio + 13);
253 *(gpio + 10) = 0xffffec;
255 return (value >> 8) & 0xffff;
258 void ps_reset_state_machine() {
259 ps_write_status_reg(STATUS_BIT_INIT);
261 ps_write_status_reg(0);
265 void ps_pulse_reset() {
266 ps_write_status_reg(0);
268 ps_write_status_reg(STATUS_BIT_RESET);
271 unsigned int ps_get_ipl_zero() {
272 unsigned int value = *(gpio + 13);
273 return value & (1 << PIN_IPL_ZERO);
276 #define INT2_ENABLED 1
278 void ps_update_irq() {
279 unsigned int ipl = 0;
281 if (!ps_get_ipl_zero()) {
282 unsigned int status = ps_read_status_reg();
283 ipl = (status & 0xe000) >> 13;
286 /*if (ipl < 2 && INT2_ENABLED && emu_int2_req()) {