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