0c1e003e |
1 | #include <stdint.h> |
2 | #include <stdio.h> |
3 | #include <string.h> |
4 | #include "teensy3/core_pins.h" |
5 | #include "teensy3/usb_seremu.h" |
6 | #include "teensy3/usb_rawhid.h" |
f20de073 |
7 | #include "pkts.h" |
8 | |
9 | // use power of 2 |
10 | #define STREAM_BUF_SIZE 512 |
11 | #define STREAM_BUF_MASK (512 - 1) |
0c1e003e |
12 | |
9c4f55f4 |
13 | /* ?0SA 00DU, ?1CB RLDU */ |
f20de073 |
14 | static struct { |
15 | uint8_t stream[STREAM_BUF_SIZE][2]; |
16 | uint8_t fixed_state[4]; |
17 | uint32_t stream_enable:1; |
18 | uint32_t stream_started:1; |
19 | uint32_t stream_received:1; |
4af6d4e5 |
20 | uint32_t use_readinc:1; |
21 | uint32_t frame_cnt; |
f20de073 |
22 | uint32_t edge_cnt; |
23 | uint32_t i; |
24 | uint32_t o; |
25 | } g; |
26 | |
27 | #define STREAM_EL_SZ sizeof(g.stream[0]) |
9c4f55f4 |
28 | |
0c1e003e |
29 | ssize_t _write(int fd, const void *buf, size_t nbyte) |
30 | { |
31 | char tbuf[64]; |
32 | int ret; |
33 | |
34 | if (fd != 1 && fd != 2) { |
35 | snprintf(tbuf, sizeof(tbuf), "write to fd %d\n", fd); |
36 | usb_seremu_write(tbuf, strlen(tbuf)); |
37 | } |
38 | |
39 | ret = usb_seremu_write(buf, nbyte); |
40 | return ret < 0 ? ret : nbyte; |
41 | } |
42 | |
43 | void yield(void) |
44 | { |
45 | } |
46 | |
4af6d4e5 |
47 | static void portb_isr_fixed(void) |
9c4f55f4 |
48 | { |
f20de073 |
49 | uint32_t isfr, th; |
9c4f55f4 |
50 | |
4af6d4e5 |
51 | isfr = PORTB_ISFR; |
52 | PORTB_ISFR = isfr; |
53 | th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1; |
54 | |
55 | GPIOD_PDOR = g.fixed_state[th]; |
56 | g.edge_cnt++; |
57 | } |
58 | |
59 | static void portb_isr_readinc(void) |
60 | { |
61 | uint32_t isfr, th; |
9c4f55f4 |
62 | |
9c4f55f4 |
63 | isfr = PORTB_ISFR; |
64 | PORTB_ISFR = isfr; |
f20de073 |
65 | th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1; |
66 | |
4af6d4e5 |
67 | GPIOD_PDOR = g.stream[g.o][th]; |
68 | if (th) { |
69 | g.o = (g.o + 1) & STREAM_BUF_MASK; |
70 | if (g.o == g.i) |
71 | // done |
72 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
73 | g.frame_cnt++; |
f20de073 |
74 | } |
75 | g.edge_cnt++; |
76 | } |
77 | |
4af6d4e5 |
78 | static void portb_isr_read(void) |
79 | { |
80 | uint32_t isfr, th; |
81 | |
82 | isfr = PORTB_ISFR; |
83 | PORTB_ISFR = isfr; |
84 | th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1; |
85 | |
86 | GPIOD_PDOR = g.stream[g.o][th]; |
87 | g.edge_cnt++; |
88 | } |
89 | |
90 | static void portc_isr_nop(void) |
91 | { |
92 | uint32_t isfr; |
93 | |
94 | isfr = PORTC_ISFR; |
95 | PORTC_ISFR = isfr; |
96 | } |
97 | |
98 | // /vsync starts at line 235/259 (ntsc/pal), just as vcounter jumps back |
99 | // we care when it comes out (/vsync goes high) after 3 lines at 238/262 |
100 | static void portc_isr_frameinc(void) |
101 | { |
102 | uint32_t isfr; |
103 | |
104 | isfr = PORTC_ISFR; |
105 | PORTC_ISFR = isfr; |
106 | |
107 | g.o = (g.o + 1) & STREAM_BUF_MASK; |
108 | if (g.o == g.i) { |
109 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
110 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
111 | } |
112 | g.frame_cnt++; |
113 | } |
114 | |
f20de073 |
115 | static void udelay(uint32_t us) |
116 | { |
117 | uint32_t start = micros(); |
118 | |
119 | while ((micros() - start) < us) { |
120 | asm volatile("nop; nop; nop; nop"); |
121 | yield(); |
122 | } |
123 | } |
124 | |
125 | static void do_start_seq(void) |
126 | { |
127 | uint32_t edge_cnt_last; |
128 | uint32_t edge_cnt; |
129 | uint32_t start, t1, t2; |
130 | int tout; |
131 | |
132 | start = micros(); |
133 | edge_cnt = g.edge_cnt; |
134 | |
135 | /* magic value */ |
136 | g.fixed_state[0] = |
137 | g.fixed_state[1] = 0x25; |
138 | |
139 | for (tout = 10000; tout > 0; tout--) { |
140 | edge_cnt_last = edge_cnt; |
141 | udelay(100); |
142 | edge_cnt = g.edge_cnt; |
143 | |
144 | if (edge_cnt != edge_cnt_last) |
145 | continue; |
146 | if (!(GPIOB_PDIR & CORE_PIN0_BITMASK)) |
147 | break; |
148 | } |
149 | |
150 | g.fixed_state[0] = 0x33; |
151 | g.fixed_state[1] = 0x3f; |
152 | GPIOD_PDOR = 0x33; |
153 | |
154 | t1 = micros(); |
155 | if (tout == 0) { |
156 | printf("start_seq timeout1, t=%u\n", t1 - start); |
157 | return; |
158 | } |
159 | |
160 | for (tout = 100000; tout > 0; tout--) { |
161 | udelay(1); |
162 | |
163 | if (GPIOB_PDIR & CORE_PIN0_BITMASK) |
164 | break; |
165 | } |
166 | |
167 | t2 = micros(); |
168 | if (tout == 0) { |
169 | printf("start_seq timeout2, t1=%u, t2=%u\n", |
170 | t1 - start, t2 - t1); |
171 | return; |
172 | } |
173 | |
174 | //printf(" t1=%u, t2=%u\n", t1 - start, t2 - t1); |
175 | |
176 | if (g.stream_started) { |
177 | printf("got start_seq when already started\n"); |
178 | return; |
179 | } |
180 | |
181 | if (!g.stream_enable) { |
182 | printf("got start_seq, without enable from USB\n"); |
183 | return; |
184 | } |
185 | |
186 | if (g.i == g.o) { |
187 | printf("got start_seq while buffer is empty\n"); |
188 | return; |
189 | } |
190 | |
4af6d4e5 |
191 | __disable_irq(); |
f20de073 |
192 | g.stream_started = 1; |
4af6d4e5 |
193 | if (g.use_readinc) { |
194 | attachInterruptVector(IRQ_PORTB, portb_isr_readinc); |
195 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
196 | } |
197 | else { |
198 | attachInterruptVector(IRQ_PORTB, portb_isr_read); |
199 | attachInterruptVector(IRQ_PORTC, portc_isr_frameinc); |
200 | } |
201 | __enable_irq(); |
f20de073 |
202 | } |
203 | |
204 | static int get_space(void) |
205 | { |
206 | return STREAM_BUF_SIZE - ((g.i - g.o) & STREAM_BUF_MASK); |
207 | } |
208 | |
209 | static void do_usb(void *buf) |
210 | { |
211 | struct tas_pkt *pkt = buf; |
212 | uint32_t i, g_i; |
213 | int space; |
214 | |
215 | switch (pkt->type) { |
216 | case PKT_FIXED_STATE: |
217 | memcpy(g.fixed_state, pkt->data, sizeof(g.fixed_state)); |
218 | break; |
219 | case PKT_STREAM_ENABLE: |
4af6d4e5 |
220 | __disable_irq(); |
221 | /* wait for start from MD */ |
f20de073 |
222 | g.stream_enable = 1; |
223 | g.stream_started = 0; |
224 | g.stream_received = 0; |
4af6d4e5 |
225 | g.use_readinc = pkt->start.use_readinc; |
f20de073 |
226 | g.i = g.o = 0; |
4af6d4e5 |
227 | g.frame_cnt = 0; |
228 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
229 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
230 | __enable_irq(); |
f20de073 |
231 | break; |
232 | case PKT_STREAM_END: |
233 | g.stream_received = 1; |
4af6d4e5 |
234 | printf("end of stream\n"); |
f20de073 |
235 | break; |
236 | case PKT_STREAM_DATA: |
237 | g_i = g.i; |
238 | space = get_space(); |
239 | if (space <= sizeof(pkt->data) / STREAM_EL_SZ) { |
240 | printf("got data pkt while space=%d\n", space); |
241 | return; |
242 | } |
243 | for (i = 0; i < sizeof(pkt->data) / STREAM_EL_SZ; i++) { |
244 | memcpy(&g.stream[g_i++], |
245 | pkt->data + i * STREAM_EL_SZ, |
246 | STREAM_EL_SZ); |
247 | g_i &= STREAM_BUF_MASK; |
248 | } |
249 | g.i = g_i; |
250 | break; |
251 | default: |
252 | printf("got unknown pkt type: %04x\n", pkt->type); |
253 | break; |
254 | } |
9c4f55f4 |
255 | } |
256 | |
0c1e003e |
257 | int main(void) |
258 | { |
f20de073 |
259 | uint32_t edge_cnt_last; |
260 | uint32_t edge_cnt; |
261 | uint8_t buf[64]; |
1cb2822f |
262 | int timeout; |
263 | int ret; |
0c1e003e |
264 | |
265 | delay(1000); // wait for usb.. |
266 | |
f20de073 |
267 | /* ?0SA 00DU, ?1CB RLDU */ |
268 | g.fixed_state[0] = 0x33; |
269 | g.fixed_state[1] = 0x3f; |
270 | |
0c1e003e |
271 | printf("starting, rawhid: %d\n", usb_rawhid_available()); |
272 | |
4af6d4e5 |
273 | // md pin th tr tl r l d u vsync |
9c4f55f4 |
274 | // md bit* 6 5 4 3 2 1 0 |
4af6d4e5 |
275 | // t bit b16 d5 d4 d3 d2 d1 d0 c6 |
276 | // t pin 0 20 6 8 7 14 2 11 |
9c4f55f4 |
277 | // * - note: tl/tr mixed in most docs |
278 | pinMode(0, INPUT); |
4af6d4e5 |
279 | attachInterrupt(0, portb_isr_fixed, CHANGE); |
280 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
281 | pinMode(11, INPUT); |
282 | attachInterrupt(11, portc_isr_nop, RISING); |
283 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
9c4f55f4 |
284 | |
4af6d4e5 |
285 | NVIC_SET_PRIORITY(IRQ_PORTB, 0); |
286 | NVIC_SET_PRIORITY(IRQ_PORTC, 16); |
f20de073 |
287 | |
9c4f55f4 |
288 | pinMode( 2, OUTPUT); |
289 | pinMode(14, OUTPUT); |
290 | pinMode( 7, OUTPUT); |
291 | pinMode( 8, OUTPUT); |
292 | pinMode( 6, OUTPUT); |
293 | pinMode(20, OUTPUT); |
294 | |
295 | // led |
296 | pinMode(13, OUTPUT); |
9c4f55f4 |
297 | |
298 | // CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16 |
4af6d4e5 |
299 | printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR); |
9c4f55f4 |
300 | printf("GPIOC PDDR, PDIR: %08x %08x\n", GPIOC_PDIR, GPIOC_PDDR); |
301 | printf("GPIOD PDDR, PDIR: %08x %08x\n", GPIOD_PDIR, GPIOD_PDDR); |
302 | printf("PORTB_PCR16: %08x\n", PORTB_PCR16); |
4af6d4e5 |
303 | printf("PORTC_PCR6: %08x\n", PORTC_PCR6); |
9c4f55f4 |
304 | |
f20de073 |
305 | asm("mrs %0, BASEPRI" : "=r"(ret)); |
306 | printf("BASEPRI: %d\n", ret); |
0c1e003e |
307 | |
1cb2822f |
308 | timeout = 1000; |
f20de073 |
309 | edge_cnt_last = g.edge_cnt; |
9c4f55f4 |
310 | |
1cb2822f |
311 | while (1) { |
f20de073 |
312 | struct tas_pkt pkt; |
313 | while (g.stream_enable && !g.stream_received |
314 | && get_space() > sizeof(pkt.data) / STREAM_EL_SZ) |
315 | { |
316 | pkt.type = PKT_STREAM_REQ; |
4af6d4e5 |
317 | pkt.req.frame = g.frame_cnt; |
318 | |
f20de073 |
319 | ret = usb_rawhid_send(&pkt, 1000); |
320 | if (ret != sizeof(pkt)) { |
321 | printf("send STREAM_REQ: %d\n", ret); |
322 | break; |
323 | } |
324 | |
325 | ret = usb_rawhid_recv(buf, 1000); |
326 | if (ret != 64) |
327 | printf("usb_rawhid_recv/s: %d\n", ret); |
328 | else |
329 | do_usb(buf); |
330 | } |
331 | |
332 | if (timeout == 1000) { |
333 | edge_cnt = g.edge_cnt; |
334 | //printf("e: %d th: %d\n", edge_cnt - edge_cnt_last, |
335 | // (GPIOB_PDIR >> CORE_PIN0_BIT) & 1); |
336 | if (edge_cnt - edge_cnt_last > 10000) { |
337 | do_start_seq(); |
338 | edge_cnt = g.edge_cnt; |
339 | } |
340 | edge_cnt_last = edge_cnt; |
341 | } |
342 | |
1cb2822f |
343 | ret = usb_rawhid_recv(buf, timeout); |
344 | if (ret == 64) { |
345 | CORE_PIN13_PORTSET = CORE_PIN13_BITMASK; |
346 | |
f20de073 |
347 | do_usb(buf); |
1cb2822f |
348 | timeout = 20; |
349 | } |
350 | else if (ret == 0) { |
351 | CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK; |
352 | timeout = 1000; |
353 | } |
354 | else { |
355 | printf("usb_rawhid_recv: %d\n", ret); |
356 | timeout = 1000; |
357 | } |
0c1e003e |
358 | } |
1cb2822f |
359 | |
360 | return 0; |
0c1e003e |
361 | } |