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))
120 void ps_write_8(unsigned int address, unsigned int data) {
121 if ((address & 1) == 0)
122 data = data + (data << 8); // EVEN, A0=0,UDS
124 data = data & 0xff; // ODD , A0=1,LDS
126 *(gpio + 0) = GPFSEL0_OUTPUT;
127 *(gpio + 1) = GPFSEL1_OUTPUT;
128 *(gpio + 2) = GPFSEL2_OUTPUT;
130 *(gpio + 7) = ((data & 0xffff) << 8) | (REG_DATA << PIN_A0);
131 *(gpio + 7) = 1 << PIN_WR;
132 *(gpio + 10) = 1 << PIN_WR;
133 *(gpio + 10) = 0xffffec;
135 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
136 *(gpio + 7) = 1 << PIN_WR;
137 *(gpio + 10) = 1 << PIN_WR;
138 *(gpio + 10) = 0xffffec;
140 *(gpio + 7) = ((0x0100 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
141 *(gpio + 7) = 1 << PIN_WR;
142 *(gpio + 10) = 1 << PIN_WR;
143 *(gpio + 10) = 0xffffec;
145 *(gpio + 0) = GPFSEL0_INPUT;
146 *(gpio + 1) = GPFSEL1_INPUT;
147 *(gpio + 2) = GPFSEL2_INPUT;
149 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
153 void ps_write_32(unsigned int address, unsigned int value) {
154 ps_write_16(address, value >> 16);
155 ps_write_16(address + 2, value);
158 unsigned int ps_read_16(unsigned int address) {
159 *(gpio + 0) = GPFSEL0_OUTPUT;
160 *(gpio + 1) = GPFSEL1_OUTPUT;
161 *(gpio + 2) = GPFSEL2_OUTPUT;
163 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
164 *(gpio + 7) = 1 << PIN_WR;
165 *(gpio + 10) = 1 << PIN_WR;
166 *(gpio + 10) = 0xffffec;
168 *(gpio + 7) = ((0x0200 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
169 *(gpio + 7) = 1 << PIN_WR;
170 *(gpio + 10) = 1 << PIN_WR;
171 *(gpio + 10) = 0xffffec;
173 *(gpio + 0) = GPFSEL0_INPUT;
174 *(gpio + 1) = GPFSEL1_INPUT;
175 *(gpio + 2) = GPFSEL2_INPUT;
177 *(gpio + 7) = (REG_DATA << PIN_A0);
178 *(gpio + 7) = 1 << PIN_RD;
180 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
183 unsigned int value = *(gpio + 13);
185 *(gpio + 10) = 0xffffec;
187 return (value >> 8) & 0xffff;
190 unsigned int ps_read_8(unsigned int address) {
191 *(gpio + 0) = GPFSEL0_OUTPUT;
192 *(gpio + 1) = GPFSEL1_OUTPUT;
193 *(gpio + 2) = GPFSEL2_OUTPUT;
195 *(gpio + 7) = ((address & 0xffff) << 8) | (REG_ADDR_LO << PIN_A0);
196 *(gpio + 7) = 1 << PIN_WR;
197 *(gpio + 10) = 1 << PIN_WR;
198 *(gpio + 10) = 0xffffec;
200 *(gpio + 7) = ((0x0300 | (address >> 16)) << 8) | (REG_ADDR_HI << PIN_A0);
201 *(gpio + 7) = 1 << PIN_WR;
202 *(gpio + 10) = 1 << PIN_WR;
203 *(gpio + 10) = 0xffffec;
205 *(gpio + 0) = GPFSEL0_INPUT;
206 *(gpio + 1) = GPFSEL1_INPUT;
207 *(gpio + 2) = GPFSEL2_INPUT;
209 *(gpio + 7) = (REG_DATA << PIN_A0);
210 *(gpio + 7) = 1 << PIN_RD;
212 while (*(gpio + 13) & (1 << PIN_TXN_IN_PROGRESS))
215 unsigned int value = *(gpio + 13);
217 *(gpio + 10) = 0xffffec;
219 value = (value >> 8) & 0xffff;
221 if ((address & 1) == 0)
222 return (value >> 8) & 0xff; // EVEN, A0=0,UDS
224 return value & 0xff; // ODD , A0=1,LDS
227 unsigned int ps_read_32(unsigned int address) {
228 unsigned int a = ps_read_16(address);
229 unsigned int b = ps_read_16(address + 2);
230 return (a << 16) | b;
233 void ps_write_status_reg(unsigned int value) {
234 *(gpio + 0) = GPFSEL0_OUTPUT;
235 *(gpio + 1) = GPFSEL1_OUTPUT;
236 *(gpio + 2) = GPFSEL2_OUTPUT;
238 *(gpio + 7) = ((value & 0xffff) << 8) | (REG_STATUS << PIN_A0);
240 *(gpio + 7) = 1 << PIN_WR;
241 *(gpio + 7) = 1 << PIN_WR; // delay
242 *(gpio + 10) = 1 << PIN_WR;
243 *(gpio + 10) = 0xffffec;
245 *(gpio + 0) = GPFSEL0_INPUT;
246 *(gpio + 1) = GPFSEL1_INPUT;
247 *(gpio + 2) = GPFSEL2_INPUT;
250 unsigned int ps_read_status_reg() {
251 *(gpio + 7) = (REG_STATUS << PIN_A0);
252 *(gpio + 7) = 1 << PIN_RD;
253 *(gpio + 7) = 1 << PIN_RD;
254 *(gpio + 7) = 1 << PIN_RD;
255 *(gpio + 7) = 1 << PIN_RD;
257 unsigned int value = *(gpio + 13);
259 *(gpio + 10) = 0xffffec;
261 return (value >> 8) & 0xffff;
264 void ps_reset_state_machine() {
265 ps_write_status_reg(STATUS_BIT_INIT);
267 ps_write_status_reg(0);
271 void ps_pulse_reset() {
272 ps_write_status_reg(0);
274 ps_write_status_reg(STATUS_BIT_RESET);
277 unsigned int ps_get_ipl_zero() {
278 unsigned int value = *(gpio + 13);
279 return value & (1 << PIN_IPL_ZERO);
282 #define INT2_ENABLED 1
284 void ps_update_irq() {
285 unsigned int ipl = 0;
287 if (!ps_get_ipl_zero()) {
288 unsigned int status = ps_read_status_reg();
289 ipl = (status & 0xe000) >> 13;
292 /*if (ipl < 2 && INT2_ENABLED && emu_int2_req()) {