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 | |
e8e66d02 |
9 | #define noinline __attribute__((noinline)) |
10 | |
f20de073 |
11 | // use power of 2 |
12 | #define STREAM_BUF_SIZE 512 |
13 | #define STREAM_BUF_MASK (512 - 1) |
0c1e003e |
14 | |
9c4f55f4 |
15 | /* ?0SA 00DU, ?1CB RLDU */ |
e8e66d02 |
16 | #define STREAM_EL_SZ 2 |
17 | |
f20de073 |
18 | static struct { |
e8e66d02 |
19 | uint8_t stream_to[STREAM_BUF_SIZE][STREAM_EL_SZ]; |
20 | uint8_t stream_from[STREAM_BUF_SIZE][STREAM_EL_SZ]; |
21 | union { |
22 | uint8_t fixed_state[4]; |
23 | uint32_t fixed_state32; |
24 | }; |
25 | uint32_t stream_enable_to:1; |
26 | uint32_t stream_enable_from:1; |
f20de073 |
27 | uint32_t stream_started:1; |
e8e66d02 |
28 | uint32_t stream_ended:1; |
4af6d4e5 |
29 | uint32_t use_readinc:1; |
30 | uint32_t frame_cnt; |
f20de073 |
31 | uint32_t edge_cnt; |
e8e66d02 |
32 | uint32_t t_i; |
33 | uint32_t t_o; |
34 | uint32_t f_i; |
35 | uint32_t f_o; |
f20de073 |
36 | } g; |
37 | |
0c1e003e |
38 | ssize_t _write(int fd, const void *buf, size_t nbyte) |
39 | { |
40 | char tbuf[64]; |
41 | int ret; |
42 | |
43 | if (fd != 1 && fd != 2) { |
44 | snprintf(tbuf, sizeof(tbuf), "write to fd %d\n", fd); |
45 | usb_seremu_write(tbuf, strlen(tbuf)); |
46 | } |
47 | |
48 | ret = usb_seremu_write(buf, nbyte); |
49 | return ret < 0 ? ret : nbyte; |
50 | } |
51 | |
52 | void yield(void) |
53 | { |
54 | } |
55 | |
4af6d4e5 |
56 | static void portb_isr_fixed(void) |
9c4f55f4 |
57 | { |
f20de073 |
58 | uint32_t isfr, th; |
9c4f55f4 |
59 | |
4af6d4e5 |
60 | isfr = PORTB_ISFR; |
61 | PORTB_ISFR = isfr; |
62 | th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1; |
63 | |
64 | GPIOD_PDOR = g.fixed_state[th]; |
65 | g.edge_cnt++; |
66 | } |
67 | |
e8e66d02 |
68 | static void portb_isr_do_to_inc(void) |
4af6d4e5 |
69 | { |
70 | uint32_t isfr, th; |
9c4f55f4 |
71 | |
9c4f55f4 |
72 | isfr = PORTB_ISFR; |
73 | PORTB_ISFR = isfr; |
f20de073 |
74 | th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1; |
75 | |
e8e66d02 |
76 | GPIOD_PDOR = g.stream_to[g.t_o][th]; |
4af6d4e5 |
77 | if (th) { |
e8e66d02 |
78 | g.t_o = (g.t_o + 1) & STREAM_BUF_MASK; |
79 | if (g.t_o == g.t_i) |
4af6d4e5 |
80 | // done |
81 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
82 | g.frame_cnt++; |
f20de073 |
83 | } |
84 | g.edge_cnt++; |
85 | } |
86 | |
e8e66d02 |
87 | static void portb_isr_do_to(void) |
4af6d4e5 |
88 | { |
89 | uint32_t isfr, th; |
90 | |
91 | isfr = PORTB_ISFR; |
92 | PORTB_ISFR = isfr; |
93 | th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1; |
94 | |
e8e66d02 |
95 | GPIOD_PDOR = g.stream_to[g.t_o][th]; |
4af6d4e5 |
96 | g.edge_cnt++; |
97 | } |
98 | |
99 | static void portc_isr_nop(void) |
100 | { |
101 | uint32_t isfr; |
102 | |
103 | isfr = PORTC_ISFR; |
104 | PORTC_ISFR = isfr; |
105 | } |
106 | |
107 | // /vsync starts at line 235/259 (ntsc/pal), just as vcounter jumps back |
108 | // we care when it comes out (/vsync goes high) after 3 lines at 238/262 |
109 | static void portc_isr_frameinc(void) |
110 | { |
111 | uint32_t isfr; |
112 | |
113 | isfr = PORTC_ISFR; |
114 | PORTC_ISFR = isfr; |
115 | |
e8e66d02 |
116 | g.t_o = (g.t_o + 1) & STREAM_BUF_MASK; |
117 | if (g.t_o == g.t_i) { |
4af6d4e5 |
118 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
119 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
120 | } |
121 | g.frame_cnt++; |
122 | } |
123 | |
e8e66d02 |
124 | /* "recording" data */ |
125 | static noinline void do_from_step(void) |
126 | { |
127 | uint32_t s; |
128 | |
129 | // should hopefully give atomic fixed_state read.. |
130 | s = g.fixed_state32; |
131 | g.stream_from[g.f_i][0] = s; |
132 | g.stream_from[g.f_i][1] = s >> 8; |
133 | g.f_i = (g.f_i + 1) & STREAM_BUF_MASK; |
134 | } |
135 | |
136 | static void portb_isr_fixed_do_from(void) |
137 | { |
138 | uint32_t isfr, th; |
139 | |
140 | isfr = PORTB_ISFR; |
141 | PORTB_ISFR = isfr; |
142 | th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1; |
143 | |
144 | GPIOD_PDOR = g.fixed_state[th]; |
145 | if (th) |
146 | do_from_step(); |
147 | g.edge_cnt++; |
148 | } |
149 | |
150 | static void portc_isr_frameinc_do_from(void) |
151 | { |
152 | uint32_t isfr; |
153 | |
154 | isfr = PORTC_ISFR; |
155 | PORTC_ISFR = isfr; |
156 | |
157 | do_from_step(); |
158 | g.frame_cnt++; |
159 | } |
160 | |
f20de073 |
161 | static void udelay(uint32_t us) |
162 | { |
163 | uint32_t start = micros(); |
164 | |
165 | while ((micros() - start) < us) { |
166 | asm volatile("nop; nop; nop; nop"); |
167 | yield(); |
168 | } |
169 | } |
170 | |
171 | static void do_start_seq(void) |
172 | { |
173 | uint32_t edge_cnt_last; |
174 | uint32_t edge_cnt; |
175 | uint32_t start, t1, t2; |
176 | int tout; |
177 | |
178 | start = micros(); |
179 | edge_cnt = g.edge_cnt; |
180 | |
181 | /* magic value */ |
182 | g.fixed_state[0] = |
183 | g.fixed_state[1] = 0x25; |
184 | |
185 | for (tout = 10000; tout > 0; tout--) { |
186 | edge_cnt_last = edge_cnt; |
187 | udelay(100); |
188 | edge_cnt = g.edge_cnt; |
189 | |
190 | if (edge_cnt != edge_cnt_last) |
191 | continue; |
192 | if (!(GPIOB_PDIR & CORE_PIN0_BITMASK)) |
193 | break; |
194 | } |
195 | |
196 | g.fixed_state[0] = 0x33; |
197 | g.fixed_state[1] = 0x3f; |
198 | GPIOD_PDOR = 0x33; |
199 | |
200 | t1 = micros(); |
201 | if (tout == 0) { |
202 | printf("start_seq timeout1, t=%u\n", t1 - start); |
203 | return; |
204 | } |
205 | |
206 | for (tout = 100000; tout > 0; tout--) { |
207 | udelay(1); |
208 | |
209 | if (GPIOB_PDIR & CORE_PIN0_BITMASK) |
210 | break; |
211 | } |
212 | |
213 | t2 = micros(); |
214 | if (tout == 0) { |
215 | printf("start_seq timeout2, t1=%u, t2=%u\n", |
216 | t1 - start, t2 - t1); |
217 | return; |
218 | } |
219 | |
220 | //printf(" t1=%u, t2=%u\n", t1 - start, t2 - t1); |
221 | |
222 | if (g.stream_started) { |
223 | printf("got start_seq when already started\n"); |
224 | return; |
225 | } |
226 | |
e8e66d02 |
227 | if (!g.stream_enable_to && !g.stream_enable_from) { |
f20de073 |
228 | printf("got start_seq, without enable from USB\n"); |
229 | return; |
230 | } |
231 | |
e8e66d02 |
232 | if (g.stream_enable_to && g.t_i == g.t_o) { |
233 | printf("got start_seq while stream_to is empty\n"); |
234 | return; |
235 | } |
236 | |
237 | if (g.stream_enable_from && g.f_i != g.f_o) { |
238 | printf("got start_seq while stream_from is not empty\n"); |
f20de073 |
239 | return; |
240 | } |
241 | |
4af6d4e5 |
242 | __disable_irq(); |
f20de073 |
243 | g.stream_started = 1; |
e8e66d02 |
244 | if (g.stream_enable_to) { |
245 | if (g.use_readinc) { |
246 | attachInterruptVector(IRQ_PORTB, portb_isr_do_to_inc); |
247 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
248 | } |
249 | else { |
250 | attachInterruptVector(IRQ_PORTB, portb_isr_do_to); |
251 | attachInterruptVector(IRQ_PORTC, portc_isr_frameinc); |
252 | } |
4af6d4e5 |
253 | } |
e8e66d02 |
254 | else if (g.stream_enable_from) { |
255 | if (g.use_readinc) { |
256 | attachInterruptVector(IRQ_PORTB, |
257 | portb_isr_fixed_do_from); |
258 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
259 | } |
260 | else { |
261 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
262 | attachInterruptVector(IRQ_PORTC, |
263 | portc_isr_frameinc_do_from); |
264 | } |
265 | |
266 | // prep first frame already |
267 | do_from_step(); |
4af6d4e5 |
268 | } |
269 | __enable_irq(); |
f20de073 |
270 | } |
271 | |
e8e66d02 |
272 | // callers must disable IRQs |
273 | static void clear_state(void) |
274 | { |
275 | g.stream_enable_to = 0; |
276 | g.stream_enable_from = 0; |
277 | g.use_readinc = 0; |
278 | g.stream_started = 0; |
279 | g.stream_ended = 0; |
280 | g.t_i = g.t_o = 0; |
281 | g.f_i = g.f_o = 0; |
282 | g.frame_cnt = 0; |
283 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
284 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
285 | } |
286 | |
287 | static int get_space_to(void) |
f20de073 |
288 | { |
e8e66d02 |
289 | return STREAM_BUF_SIZE - ((g.t_i - g.t_o) & STREAM_BUF_MASK); |
290 | } |
291 | |
292 | static int get_used_from(void) |
293 | { |
294 | return (g.f_i - g.f_o) & STREAM_BUF_MASK; |
f20de073 |
295 | } |
296 | |
297 | static void do_usb(void *buf) |
298 | { |
299 | struct tas_pkt *pkt = buf; |
e8e66d02 |
300 | uint32_t t_i, i; |
f20de073 |
301 | int space; |
302 | |
303 | switch (pkt->type) { |
304 | case PKT_FIXED_STATE: |
305 | memcpy(g.fixed_state, pkt->data, sizeof(g.fixed_state)); |
306 | break; |
307 | case PKT_STREAM_ENABLE: |
4af6d4e5 |
308 | __disable_irq(); |
e8e66d02 |
309 | clear_state(); |
4af6d4e5 |
310 | /* wait for start from MD */ |
e8e66d02 |
311 | g.stream_enable_to = pkt->enable.stream_to; |
312 | g.stream_enable_from = pkt->enable.stream_from; |
313 | g.use_readinc = pkt->enable.use_readinc; |
314 | __enable_irq(); |
315 | break; |
316 | case PKT_STREAM_ABORT: |
317 | __disable_irq(); |
318 | clear_state(); |
4af6d4e5 |
319 | __enable_irq(); |
f20de073 |
320 | break; |
321 | case PKT_STREAM_END: |
e8e66d02 |
322 | g.stream_ended = 1; |
4af6d4e5 |
323 | printf("end of stream\n"); |
f20de073 |
324 | break; |
e8e66d02 |
325 | case PKT_STREAM_DATA_TO: |
326 | t_i = g.t_i; |
327 | space = get_space_to(); |
328 | if (space <= pkt->size / STREAM_EL_SZ) { |
f20de073 |
329 | printf("got data pkt while space=%d\n", space); |
330 | return; |
331 | } |
e8e66d02 |
332 | for (i = 0; i < pkt->size / STREAM_EL_SZ; i++) { |
333 | memcpy(&g.stream_to[t_i++], |
f20de073 |
334 | pkt->data + i * STREAM_EL_SZ, |
335 | STREAM_EL_SZ); |
e8e66d02 |
336 | t_i &= STREAM_BUF_MASK; |
f20de073 |
337 | } |
e8e66d02 |
338 | g.t_i = t_i; |
f20de073 |
339 | break; |
340 | default: |
341 | printf("got unknown pkt type: %04x\n", pkt->type); |
342 | break; |
343 | } |
9c4f55f4 |
344 | } |
345 | |
0c1e003e |
346 | int main(void) |
347 | { |
f20de073 |
348 | uint32_t edge_cnt_last; |
349 | uint32_t edge_cnt; |
350 | uint8_t buf[64]; |
1cb2822f |
351 | int timeout; |
352 | int ret; |
0c1e003e |
353 | |
354 | delay(1000); // wait for usb.. |
355 | |
f20de073 |
356 | /* ?0SA 00DU, ?1CB RLDU */ |
357 | g.fixed_state[0] = 0x33; |
358 | g.fixed_state[1] = 0x3f; |
359 | |
0c1e003e |
360 | printf("starting, rawhid: %d\n", usb_rawhid_available()); |
361 | |
4af6d4e5 |
362 | // md pin th tr tl r l d u vsync |
9c4f55f4 |
363 | // md bit* 6 5 4 3 2 1 0 |
4af6d4e5 |
364 | // t bit b16 d5 d4 d3 d2 d1 d0 c6 |
365 | // t pin 0 20 6 8 7 14 2 11 |
9c4f55f4 |
366 | // * - note: tl/tr mixed in most docs |
367 | pinMode(0, INPUT); |
4af6d4e5 |
368 | attachInterrupt(0, portb_isr_fixed, CHANGE); |
369 | attachInterruptVector(IRQ_PORTB, portb_isr_fixed); |
370 | pinMode(11, INPUT); |
371 | attachInterrupt(11, portc_isr_nop, RISING); |
372 | attachInterruptVector(IRQ_PORTC, portc_isr_nop); |
9c4f55f4 |
373 | |
4af6d4e5 |
374 | NVIC_SET_PRIORITY(IRQ_PORTB, 0); |
375 | NVIC_SET_PRIORITY(IRQ_PORTC, 16); |
f20de073 |
376 | |
9c4f55f4 |
377 | pinMode( 2, OUTPUT); |
378 | pinMode(14, OUTPUT); |
379 | pinMode( 7, OUTPUT); |
380 | pinMode( 8, OUTPUT); |
381 | pinMode( 6, OUTPUT); |
382 | pinMode(20, OUTPUT); |
383 | |
384 | // led |
385 | pinMode(13, OUTPUT); |
9c4f55f4 |
386 | |
387 | // CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16 |
4af6d4e5 |
388 | printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR); |
9c4f55f4 |
389 | printf("GPIOC PDDR, PDIR: %08x %08x\n", GPIOC_PDIR, GPIOC_PDDR); |
390 | printf("GPIOD PDDR, PDIR: %08x %08x\n", GPIOD_PDIR, GPIOD_PDDR); |
391 | printf("PORTB_PCR16: %08x\n", PORTB_PCR16); |
4af6d4e5 |
392 | printf("PORTC_PCR6: %08x\n", PORTC_PCR6); |
9c4f55f4 |
393 | |
f20de073 |
394 | asm("mrs %0, BASEPRI" : "=r"(ret)); |
395 | printf("BASEPRI: %d\n", ret); |
0c1e003e |
396 | |
1cb2822f |
397 | timeout = 1000; |
f20de073 |
398 | edge_cnt_last = g.edge_cnt; |
9c4f55f4 |
399 | |
1cb2822f |
400 | while (1) { |
f20de073 |
401 | struct tas_pkt pkt; |
e8e66d02 |
402 | |
403 | while (g.stream_enable_to && !g.stream_ended |
404 | && get_space_to() > sizeof(pkt.data) / STREAM_EL_SZ) |
f20de073 |
405 | { |
406 | pkt.type = PKT_STREAM_REQ; |
4af6d4e5 |
407 | pkt.req.frame = g.frame_cnt; |
408 | |
f20de073 |
409 | ret = usb_rawhid_send(&pkt, 1000); |
410 | if (ret != sizeof(pkt)) { |
411 | printf("send STREAM_REQ: %d\n", ret); |
412 | break; |
413 | } |
414 | |
415 | ret = usb_rawhid_recv(buf, 1000); |
416 | if (ret != 64) |
417 | printf("usb_rawhid_recv/s: %d\n", ret); |
418 | else |
419 | do_usb(buf); |
420 | } |
421 | |
e8e66d02 |
422 | while (g.stream_enable_from && !g.stream_ended |
423 | && get_used_from() >= sizeof(pkt.data) / STREAM_EL_SZ) |
424 | { |
425 | uint32_t f_o; |
426 | int i; |
427 | |
428 | f_o = g.f_o; |
429 | for (i = 0; i < sizeof(pkt.data); i += STREAM_EL_SZ) { |
430 | memcpy(pkt.data + i, &g.stream_from[f_o++], |
431 | STREAM_EL_SZ); |
432 | f_o &= STREAM_BUF_MASK; |
433 | } |
434 | g.f_o = f_o; |
435 | |
436 | pkt.type = PKT_STREAM_DATA_FROM; |
437 | pkt.size = i; |
438 | |
439 | ret = usb_rawhid_send(&pkt, 1000); |
440 | if (ret != sizeof(pkt)) { |
441 | printf("send DATA_FROM: %d\n", ret); |
442 | break; |
443 | } |
444 | } |
445 | |
f20de073 |
446 | if (timeout == 1000) { |
447 | edge_cnt = g.edge_cnt; |
448 | //printf("e: %d th: %d\n", edge_cnt - edge_cnt_last, |
449 | // (GPIOB_PDIR >> CORE_PIN0_BIT) & 1); |
450 | if (edge_cnt - edge_cnt_last > 10000) { |
451 | do_start_seq(); |
452 | edge_cnt = g.edge_cnt; |
453 | } |
454 | edge_cnt_last = edge_cnt; |
455 | } |
456 | |
1cb2822f |
457 | ret = usb_rawhid_recv(buf, timeout); |
458 | if (ret == 64) { |
459 | CORE_PIN13_PORTSET = CORE_PIN13_BITMASK; |
460 | |
f20de073 |
461 | do_usb(buf); |
1cb2822f |
462 | timeout = 20; |
463 | } |
464 | else if (ret == 0) { |
465 | CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK; |
466 | timeout = 1000; |
467 | } |
468 | else { |
469 | printf("usb_rawhid_recv: %d\n", ret); |
470 | timeout = 1000; |
471 | } |
0c1e003e |
472 | } |
1cb2822f |
473 | |
474 | return 0; |
0c1e003e |
475 | } |