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 // use power of 2
  10 #define STREAM_BUF_SIZE 512
  11 #define STREAM_BUF_MASK (512 - 1)
  12
  13 /* ?0SA 00DU, ?1CB RLDU */
  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;
  20         uint32_t use_readinc:1;
  21         uint32_t frame_cnt;
  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])
  28
  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
  47 static void portb_isr_fixed(void)
  48 {
  49         uint32_t isfr, th;
  50
  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;
  62
  63         isfr = PORTB_ISFR;
  64         PORTB_ISFR = isfr;
  65         th = (GPIOB_PDIR >> CORE_PIN0_BIT) & 1;
  66
  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++;
  74         }
  75         g.edge_cnt++;
  76 }
  77
  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
 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
 191         __disable_irq();
 192         g.stream_started = 1;
 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();
 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:
 220                 __disable_irq();
 221                 /* wait for start from MD */
 222                 g.stream_enable = 1;
 223                 g.stream_started = 0;
 224                 g.stream_received = 0;
 225                 g.use_readinc = pkt->start.use_readinc;
 226                 g.i = g.o = 0;
 227                 g.frame_cnt = 0;
 228                 attachInterruptVector(IRQ_PORTB, portb_isr_fixed);
 229                 attachInterruptVector(IRQ_PORTC, portc_isr_nop);
 230                 __enable_irq();
 231                 break;
 232         case PKT_STREAM_END:
 233                 g.stream_received = 1;
 234                 printf("end of stream\n");
 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         }
 255 }
 256
 257 int main(void)
 258 {
 259         uint32_t edge_cnt_last;
 260         uint32_t edge_cnt;
 261         uint8_t buf[64];
 262         int timeout;
 263         int ret;
 264
 265         delay(1000); // wait for usb..
 266
 267         /* ?0SA 00DU, ?1CB RLDU */
 268         g.fixed_state[0] = 0x33;
 269         g.fixed_state[1] = 0x3f;
 270
 271         printf("starting, rawhid: %d\n", usb_rawhid_available());
 272
 273         // md pin   th tr tl  r  l  d  u vsync
 274         // md bit*   6  5  4  3  2  1  0
 275         // t bit   b16 d5 d4 d3 d2 d1 d0    c6
 276         // t pin     0 20  6  8  7 14  2    11
 277         // * - note: tl/tr mixed in most docs
 278         pinMode(0, INPUT);
 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);
 284
 285         NVIC_SET_PRIORITY(IRQ_PORTB, 0);
 286         NVIC_SET_PRIORITY(IRQ_PORTC, 16);
 287
 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);
 297
 298         // CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16
 299         printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR);
 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);
 303         printf("PORTC_PCR6:  %08x\n", PORTC_PCR6);
 304
 305         asm("mrs %0, BASEPRI" : "=r"(ret));
 306         printf("BASEPRI: %d\n", ret);
 307
 308         timeout = 1000;
 309         edge_cnt_last = g.edge_cnt;
 310
 311         while (1) {
 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;
 317                         pkt.req.frame = g.frame_cnt;
 318
 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
 343                 ret = usb_rawhid_recv(buf, timeout);
 344                 if (ret == 64) {
 345                         CORE_PIN13_PORTSET = CORE_PIN13_BITMASK;
 346
 347                         do_usb(buf);
 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                 }
 358         }
 359
 360         return 0;
 361 }