main.c

   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"
   7 #include "pkts.h"
   8
   9 #define noinline __attribute__((noinline))
  10
  11 // use power of 2
  12 #define STREAM_BUF_SIZE 512
  13 #define STREAM_BUF_MASK (512 - 1)
  14
  15 /* ?0SA 00DU, ?1CB RLDU */
  16 #define STREAM_EL_SZ 2
  17
  18 static struct {
  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         union {
  26                 uint8_t pending_state[4];
  27                 uint32_t pending_state32;
  28         };
  29         uint32_t stream_enable_to:1;
  30         uint32_t stream_enable_from:1;
  31         uint32_t stream_started:1;
  32         uint32_t stream_ended:1;
  33         uint32_t use_readinc:1;
  34         uint32_t use_pending:1;
  35         uint32_t frame_cnt;
  36         uint32_t edge_cnt;
  37         uint32_t t_i;
  38         uint32_t t_o;
  39         uint32_t f_i;
  40         uint32_t f_o;
  41 } g;
  42
  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
  61 /* portb handles TH */
  62 static void portb_isr_fixed(void)
  63 {
  64         uint32_t isfr, th;
  65
  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
  74 static void portb_isr_do_to_inc(void)
  75 {
  76         uint32_t isfr, th;
  77
  78         isfr = PORTB_ISFR;
  79         PORTB_ISFR = isfr;
  80         th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1;
  81
  82         GPIOD_PDOR = g.stream_to[g.t_o][th];
  83         if (th) {
  84                 g.t_o = (g.t_o + 1) & STREAM_BUF_MASK;
  85                 if (g.t_o == g.t_i)
  86                         // done
  87                         attachInterruptVector(IRQ_PORTB, portb_isr_fixed);
  88                 g.frame_cnt++;
  89         }
  90         g.edge_cnt++;
  91 }
  92
  93 static void portb_isr_do_to(void)
  94 {
  95         uint32_t isfr, th;
  96
  97         isfr = PORTB_ISFR;
  98         PORTB_ISFR = isfr;
  99         th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1;
 100
 101         GPIOD_PDOR = g.stream_to[g.t_o][th];
 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
 122         g.t_o = (g.t_o + 1) & STREAM_BUF_MASK;
 123         if (g.t_o == g.t_i) {
 124                 attachInterruptVector(IRQ_PORTB, portb_isr_fixed);
 125                 attachInterruptVector(IRQ_PORTC, portc_isr_nop);
 126         }
 127         g.frame_cnt++;
 128 }
 129
 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;
 137         g.fixed_state32 = g.pending_state32;
 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
 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
 234         if (!g.stream_enable_to && !g.stream_enable_from) {
 235                 printf("got start_seq, without enable from USB\n");
 236                 return;
 237         }
 238
 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");
 246                 return;
 247         }
 248
 249         __disable_irq();
 250         g.stream_started = 1;
 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                 }
 260         }
 261         else if (g.stream_enable_from) {
 262                 g.use_pending = 1;
 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                 }
 273         }
 274         __enable_irq();
 275 }
 276
 277 // callers must disable IRQs
 278 static void clear_state(void)
 279 {
 280         g.stream_enable_to = 0;
 281         g.stream_enable_from = 0;
 282         g.stream_started = 0;
 283         g.stream_ended = 0;
 284         g.use_readinc = 0;
 285         g.use_pending = 0;
 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)
 294 {
 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;
 301 }
 302
 303 static void do_usb(void *buf)
 304 {
 305         struct tas_pkt *pkt = buf;
 306         uint32_t t_i, i;
 307         int space;
 308
 309         switch (pkt->type) {
 310         case PKT_FIXED_STATE:
 311                 memcpy(&i, pkt->data, sizeof(i));
 312                 if (g.use_pending)
 313                         g.pending_state32 = i;
 314                 else
 315                         g.fixed_state32 = i;
 316                 break;
 317         case PKT_STREAM_ENABLE:
 318                 __disable_irq();
 319                 clear_state();
 320                 /* wait for start from MD */
 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();
 329                 __enable_irq();
 330                 break;
 331         case PKT_STREAM_END:
 332                 g.stream_ended = 1;
 333                 printf("end of stream\n");
 334                 break;
 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) {
 339                         printf("got data pkt while space=%d\n", space);
 340                         return;
 341                 }
 342                 for (i = 0; i < pkt->size / STREAM_EL_SZ; i++) {
 343                         memcpy(&g.stream_to[t_i++],
 344                                pkt->data + i * STREAM_EL_SZ,
 345                                STREAM_EL_SZ);
 346                         t_i &= STREAM_BUF_MASK;
 347                 }
 348                 g.t_i = t_i;
 349                 break;
 350         default:
 351                 printf("got unknown pkt type: %04x\n", pkt->type);
 352                 break;
 353         }
 354 }
 355
 356 int main(void)
 357 {
 358         uint32_t led_time = 0;
 359         uint32_t scheck_time = 0;
 360         uint32_t edge_cnt_last;
 361         uint32_t edge_cnt;
 362         uint8_t buf[64];
 363         int ret;
 364
 365         delay(1000); // wait for usb..
 366
 367         /* ?0SA 00DU, ?1CB RLDU */
 368         g.fixed_state[0] = 0x33;
 369         g.fixed_state[1] = 0x3f;
 370
 371         printf("starting, rawhid: %d\n", usb_rawhid_available());
 372
 373         // md pin   th tr tl  r  l  d  u vsync
 374         // md bit*   6  5  4  3  2  1  0
 375         // t bit   b16 d5 d4 d3 d2 d1 d0    c6
 376         // t pin     0 20  6  8  7 14  2    11
 377         // * - note: tl/tr mixed in most docs
 378         pinMode(0, INPUT);
 379         attachInterrupt(0, portb_isr_fixed, CHANGE);
 380         attachInterruptVector(IRQ_PORTB, portb_isr_fixed);
 381         pinMode(11, INPUT);
 382         attachInterrupt(11, portc_isr_nop, RISING);
 383         attachInterruptVector(IRQ_PORTC, portc_isr_nop);
 384
 385         NVIC_SET_PRIORITY(IRQ_PORTB, 0);
 386         NVIC_SET_PRIORITY(IRQ_PORTC, 16);
 387
 388         pinMode( 2, OUTPUT);
 389         pinMode(14, OUTPUT);
 390         pinMode( 7, OUTPUT);
 391         pinMode( 8, OUTPUT);
 392         pinMode( 6, OUTPUT);
 393         pinMode(20, OUTPUT);
 394
 395         // led
 396         pinMode(13, OUTPUT);
 397
 398         // CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16
 399         printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR);
 400         printf("GPIOC PDDR, PDIR: %08x %08x\n", GPIOC_PDIR, GPIOC_PDDR);
 401         printf("GPIOD PDDR, PDIR: %08x %08x\n", GPIOD_PDIR, GPIOD_PDDR);
 402         printf("PORTB_PCR16: %08x\n", PORTB_PCR16);
 403         printf("PORTC_PCR6:  %08x\n", PORTC_PCR6);
 404
 405         asm("mrs %0, BASEPRI" : "=r"(ret));
 406         printf("BASEPRI: %d\n", ret);
 407
 408         edge_cnt_last = g.edge_cnt;
 409
 410         while (1) {
 411                 struct tas_pkt pkt;
 412                 uint32_t now;
 413
 414                 while (g.stream_enable_to && !g.stream_ended
 415                   && get_space_to() > sizeof(pkt.data) / STREAM_EL_SZ)
 416                 {
 417                         if (g.t_i == g.t_o && g.frame_cnt != 0) {
 418                                 printf("underflow detected\n");
 419                                 g.stream_enable_to = 0;
 420                                 break;
 421                         }
 422
 423                         pkt.type = PKT_STREAM_REQ;
 424                         pkt.req.frame = g.frame_cnt;
 425
 426                         ret = usb_rawhid_send(&pkt, 1000);
 427                         if (ret != sizeof(pkt)) {
 428                                 printf("send STREAM_REQ: %d\n", ret);
 429                                 break;
 430                         }
 431
 432                         ret = usb_rawhid_recv(buf, 1000);
 433                         if (ret != 64)
 434                                 printf("usb_rawhid_recv/s: %d\n", ret);
 435                         else
 436                                 do_usb(buf);
 437                 }
 438
 439                 while (g.stream_enable_from && !g.stream_ended
 440                   && get_used_from() >= sizeof(pkt.data) / STREAM_EL_SZ)
 441                 {
 442                         uint32_t f_o;
 443                         int i;
 444
 445                         f_o = g.f_o;
 446                         for (i = 0; i < sizeof(pkt.data); i += STREAM_EL_SZ) {
 447                                 memcpy(pkt.data + i, &g.stream_from[f_o++],
 448                                         STREAM_EL_SZ);
 449                                 f_o &= STREAM_BUF_MASK;
 450                         }
 451                         g.f_o = f_o;
 452
 453                         pkt.type = PKT_STREAM_DATA_FROM;
 454                         pkt.size = i;
 455
 456                         ret = usb_rawhid_send(&pkt, 1000);
 457                         if (ret != sizeof(pkt)) {
 458                                 printf("send DATA_FROM: %d\n", ret);
 459                                 break;
 460                         }
 461                 }
 462
 463                 now = millis();
 464
 465                 // start condition check
 466                 if (now - scheck_time > 1000) {
 467                         edge_cnt = g.edge_cnt;
 468                         //printf("e: %d th: %d\n", edge_cnt - edge_cnt_last,
 469                         //      (GPIOB_PDIR >> CORE_PIN0_BIT) & 1);
 470                         if ((g.stream_enable_to || g.stream_enable_from)
 471                             && !g.stream_started
 472                             && edge_cnt - edge_cnt_last > 10000)
 473                         {
 474                                 do_start_seq();
 475                                 edge_cnt = g.edge_cnt;
 476                         }
 477                         edge_cnt_last = edge_cnt;
 478                         scheck_time = now;
 479                 }
 480
 481                 // led?
 482                 if (CORE_PIN13_PORTREG & CORE_PIN13_BITMASK) {
 483                         if ((int)(now - led_time) > 10)
 484                                 CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK;
 485                 }
 486
 487                 // something on rawhid?
 488                 if (usb_rawhid_available() > 0)
 489                 {
 490                         ret = usb_rawhid_recv(buf, 20);
 491                         if (ret == 64) {
 492                                 led_time = millis();
 493                                 CORE_PIN13_PORTSET = CORE_PIN13_BITMASK;
 494
 495                                 do_usb(buf);
 496                         }
 497                         else {
 498                                 printf("usb_rawhid_recv: %d\n", ret);
 499                         }
 500                 }
 501         }
 502
 503         return 0;
 504 }