teensytp/main.c

   1 /*
   2  * TeensyTP, Team Player/4-Player Adaptor implementation for Teensy3
   3  * using a host machine with USB hub
   4  * Copyright (c) 2015 notaz
   5  *
   6  * Permission is hereby granted, free of charge, to any person obtaining
   7  * a copy of this software and associated documentation files (the
   8  * "Software"), to deal in the Software without restriction, including
   9  * without limitation the rights to use, copy, modify, merge, publish,
  10  * distribute, sublicense, and/or sell copies of the Software, and to
  11  * permit persons to whom the Software is furnished to do so, subject to
  12  * the following conditions:
  13  *
  14  * The above copyright notice and this permission notice shall be
  15  * included in all copies or substantial portions of the Software.
  16  *
  17  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  18  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  19  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  20  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
  21  * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
  22  * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  23  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  24  * SOFTWARE.
  25  */
  26
  27 #include <stdint.h>
  28 #include <stdio.h>
  29 #include <string.h>
  30 #include "teensy3/core_pins.h"
  31 #include "teensy3/usb_seremu.h"
  32 #include "teensy3/usb_rawhid.h"
  33 #include "pkts.h"
  34
  35 #define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0]))
  36
  37 #define noinline __attribute__((noclone,noinline))
  38
  39 ssize_t _write(int fd, const void *buf, size_t nbyte)
  40 {
  41         char tbuf[64];
  42         int ret;
  43
  44         if (fd != 1 && fd != 2) {
  45                 snprintf(tbuf, sizeof(tbuf), "write to fd %d\n", fd);
  46                 usb_seremu_write(tbuf, strlen(tbuf));
  47         }
  48
  49         ret = usb_seremu_write(buf, nbyte);
  50         return ret < 0 ? ret : nbyte;
  51 }
  52
  53 void yield(void)
  54 {
  55 }
  56
  57 /*
  58  * 00157:231: w 70 @ 007ef2
  59  * 00157:231: r 7f @ 007f06
  60  * 00157:231: w 30 @ 007efa
  61  * 00157:231: r 33 @ 007f06
  62  * 00157:232: w 40 @ 007f6a
  63  * 00157:232: r 7f @ 007f70
  64  */
  65
  66 /* ?0SA 00DU, ?1CB RLDU */
  67
  68 static struct {
  69         struct {
  70                 union {
  71                         uint8_t b[2];
  72                         uint8_t b6[4 * 2];   // 6btn
  73                         uint32_t w;          // for more atomic change
  74                 };
  75                 uint32_t phase;              // 6btn
  76                 uint32_t time;
  77         } btn_state[2];
  78
  79         uint8_t tp_state[16];
  80         uint32_t tp_cnt;
  81
  82         uint32_t mode;
  83         uint32_t pl1th:1;
  84 } g;
  85
  86 /* player1 TH */
  87 #define PL1_ISFR     PORTD_ISFR
  88 #define PL1_TH()     ((CORE_PIN21_PINREG >> CORE_PIN21_BIT) & 1)
  89 #define PL1_TR()     ((CORE_PIN20_PINREG >> CORE_PIN20_BIT) & 1)
  90 #define PL1_TH_MASK  CORE_PIN21_BITMASK
  91 #define PL1_TR_MASK  CORE_PIN20_BITMASK
  92
  93 static void pl1_isr_3btn(void)
  94 {
  95         uint32_t isfr;
  96
  97         isfr = PL1_ISFR;
  98         PL1_ISFR = isfr;
  99
 100         GPIOD_PDOR = g.btn_state[0].b[PL1_TH()];
 101 }
 102
 103 static void pl1_isr_6btn(void)
 104 {
 105         uint32_t isfr, th, phase = 0;
 106
 107         isfr = PL1_ISFR;
 108         PL1_ISFR = isfr;
 109
 110         th = PL1_TH();
 111         if (th)
 112                 g.btn_state[0].phase++;
 113         if (g.btn_state[0].phase < 4)
 114                 phase = g.btn_state[0].phase;
 115
 116         GPIOD_PDOR = g.btn_state[0].b[(phase << 1) | PL1_TH()];
 117         g.btn_state[0].time = millis();
 118 }
 119
 120 // team player
 121 static noinline void pl1_isr_tp_do_th(void)
 122 {
 123         uint32_t cnt, th;
 124
 125         th = PL1_TH();
 126         cnt = !th;
 127
 128         GPIOD_PDOR = g.tp_state[cnt] | (PL1_TR() << 4);
 129         g.tp_cnt = cnt;
 130         g.pl1th = th;
 131 }
 132
 133 static void pl1_isr_tp(void)
 134 {
 135         uint32_t isfr;
 136
 137         isfr = PL1_ISFR;
 138         PL1_ISFR = isfr;
 139
 140         if (isfr & PL1_TH_MASK)
 141                 pl1_isr_tp_do_th();
 142         if (!(isfr & PL1_TR_MASK))
 143                 return;
 144         if (g.pl1th)
 145                 return;
 146
 147         g.tp_cnt++;
 148         GPIOD_PDOR = g.tp_state[g.tp_cnt & 0x0f] | (PL1_TR() << 4);
 149 }
 150
 151 /* player2 TH */
 152 #define PL2_ISFR   PORTC_ISFR
 153 #define PL2_TH()   ((CORE_PIN15_PINREG >> CORE_PIN15_BIT) & 1)
 154 #define PL2_ADJ(x) ((x) | ((x) << 12))
 155
 156 static void pl2_isr_nop(void)
 157 {
 158         uint32_t isfr;
 159
 160         isfr = PL2_ISFR;
 161         PL2_ISFR = isfr;
 162
 163         GPIOB_PDOR = PL2_ADJ(0x3f);
 164 }
 165
 166 static void pl2_isr_3btn(void)
 167 {
 168         uint32_t isfr;
 169
 170         isfr = PL2_ISFR;
 171         PL2_ISFR = isfr;
 172
 173         GPIOB_PDOR = PL2_ADJ(g.btn_state[1].b[PL2_TH()]);
 174 }
 175
 176 static void pl2_isr_6btn(void)
 177 {
 178         uint32_t isfr, th, phase = 0;
 179
 180         isfr = PL2_ISFR;
 181         PL2_ISFR = isfr;
 182
 183         th = PL2_TH();
 184         if (th)
 185                 g.btn_state[1].phase++;
 186         if (g.btn_state[1].phase < 4)
 187                 phase = g.btn_state[1].phase;
 188
 189         GPIOB_PDOR = PL2_ADJ(g.btn_state[1].b[(phase << 1) | PL2_TH()]);
 190         g.btn_state[1].time = millis();
 191 }
 192
 193 /* * */
 194 static void clear_state()
 195 {
 196         int p, i;
 197
 198         // no hw connected
 199         for (p = 0; p < 2; p++) {
 200                 for (i = 0; i < 4 * 2; i++)
 201                         g.btn_state[p].b6[i] = 0x3f;
 202                 g.btn_state[p].phase = 0;
 203         }
 204
 205         // 4 pads, nothing pressed
 206         g.tp_state[0] = 0x03;
 207         g.tp_state[1] = 0x0f;
 208         g.tp_state[2] = 0x00;
 209         g.tp_state[3] = 0x00;
 210         for (i = 4; i < 8; i++)
 211                 g.tp_state[i] = 0x00; // 3btn
 212         for (; i < 16; i++)
 213                 g.tp_state[i] = 0x0f;
 214
 215         g.tp_cnt = 0;
 216         g.pl1th = PL1_TH();
 217 }
 218
 219 static void switch_mode(int mode)
 220 {
 221         void (*pl1_handler)(void) = pl1_isr_3btn;
 222         void (*pl2_handler)(void) = pl2_isr_3btn;
 223
 224         clear_state();
 225
 226         switch (mode) {
 227         default:
 228         case OP_MODE_3BTN:
 229                 pinMode(20, OUTPUT);
 230                 break;
 231         case OP_MODE_6BTN:
 232                 pinMode(20, OUTPUT);
 233                 pl1_handler = pl1_isr_6btn;
 234                 pl2_handler = pl2_isr_6btn;
 235                 break;
 236         case OP_MODE_TEAMPLAYER:
 237                 pinMode(20, INPUT);
 238                 attachInterrupt(20, pl1_handler, CHANGE);
 239                 pl1_handler = pl1_isr_tp;
 240                 pl2_handler = pl2_isr_nop;
 241                 GPIOB_PDOR = PL2_ADJ(0x3f);
 242                 break;
 243         }
 244
 245         attachInterruptVector(IRQ_PORTD, pl1_handler);
 246         attachInterruptVector(IRQ_PORTC, pl2_handler);
 247         g.mode = mode;
 248 }
 249
 250 // btns: MXYZ SACB RLDU
 251 static void update_btns_btn(const struct tp_pkt *pkt, uint32_t player)
 252 {
 253         // ?1CB RLDU ?0SA 00DU
 254         uint32_t s_6btn0, s_6btn1;
 255         uint32_t s_3btn;
 256
 257         s_3btn  = (~pkt->bnts[player] << 8) & 0x3f00;
 258         s_3btn |= (~pkt->bnts[player] >> 2) & 0x0030; // SA
 259         s_3btn |= (~pkt->bnts[player]     ) & 0x0003; // DU
 260         s_3btn |= s_3btn << 16;
 261         g.btn_state[player].w = s_3btn;
 262
 263         // 6btn stuff
 264         s_6btn0 = s_3btn & 0x30;
 265         s_6btn1 = s_3btn >> 8;
 266         g.btn_state[player].b6[4] = s_6btn0;        // ?0SA 0000
 267         g.btn_state[player].b6[5] = s_6btn1;        // ?1CB RLDU
 268         g.btn_state[player].b6[6] = s_6btn0 | 0x0f; // ?0SA 1111
 269         s_6btn1 &= 0x30;
 270         s_6btn1 |= (~pkt->bnts[player] >> 8) & 0x0f;
 271         g.btn_state[player].b6[7] = s_6btn1;        // ?1CB MXYZ
 272 }
 273
 274 static void update_btns_tp(const struct tp_pkt *pkt, uint32_t player)
 275 {
 276         uint8_t b0, b1;
 277
 278         b0 =  ~pkt->bnts[player] & 0x0f;
 279         b1 = (~pkt->bnts[player] >> 4) & 0x0f;
 280         g.tp_state[8 + player * 2    ] = b0;
 281         g.tp_state[8 + player * 2 + 1] = b1;
 282 }
 283
 284 static void do_usb(const void *buf)
 285 {
 286         const struct tp_pkt *pkt = buf;
 287         uint32_t i;
 288
 289         switch (pkt->type) {
 290         case PKT_UPD_MODE:
 291                 __disable_irq();
 292                 switch_mode(pkt->mode);
 293                 __enable_irq();
 294                 break;
 295         case PKT_UPD_BTNS:
 296                 switch (g.mode) {
 297                 case OP_MODE_3BTN:
 298                 case OP_MODE_6BTN:
 299                         if (pkt->changed_players & 1)
 300                                 update_btns_btn(pkt, 0);
 301                         if (pkt->changed_players & 2)
 302                                 update_btns_btn(pkt, 1);
 303                         break;
 304                 case OP_MODE_TEAMPLAYER:
 305                         for (i = 0; i < 4; i++) {
 306                                 if (!(pkt->changed_players & (1 << i)))
 307                                         continue;
 308                                 update_btns_tp(pkt, i);
 309                         }
 310                 }
 311                 break;
 312         default:
 313                 printf("got unknown pkt type: %04x\n", pkt->type);
 314                 break;
 315         }
 316 }
 317
 318 int main(void)
 319 {
 320         uint32_t led_time = 0;
 321         uint8_t buf[64];
 322         int ret;
 323
 324         delay(1000); // wait for usb..
 325
 326         printf("starting, rawhid: %d\n", usb_rawhid_available());
 327
 328         switch_mode(OP_MODE_3BTN);
 329
 330         // md pin   th tr tl  r  l  d  u vsync   th  tr  tl  r  l  d  u
 331         //           7  9  6  4  3  2  1          7   9   6  4  3  2  1
 332         // md bit*   6  5  4  3  2  1  0          6   5   4  3  2  1  0
 333         // t bit    d6 d5 d4 d3 d2 d1 d0   a12   c0 b17 b16 b3 b2 b1 b0
 334         // t pin    21 20  6  8  7 14  2     3   15   1   0 18 19 17 16
 335         // * - note: tl/tr mixed in most docs
 336
 337         // player1
 338         pinMode(21, INPUT);
 339         // note: func is not used, see attachInterruptVector()
 340         attachInterrupt(21, pl1_isr_3btn, CHANGE);
 341         NVIC_SET_PRIORITY(IRQ_PORTD, 0);
 342
 343         pinMode( 2, OUTPUT);
 344         pinMode(14, OUTPUT);
 345         pinMode( 7, OUTPUT);
 346         pinMode( 8, OUTPUT);
 347         pinMode( 6, OUTPUT);
 348         pinMode(20, OUTPUT);
 349
 350         // player2
 351         pinMode(15, INPUT);
 352         attachInterrupt(15, pl2_isr_nop, CHANGE);
 353         NVIC_SET_PRIORITY(IRQ_PORTC, 0);
 354
 355         pinMode(16, OUTPUT);
 356         pinMode(17, OUTPUT);
 357         pinMode(19, OUTPUT);
 358         pinMode(18, OUTPUT);
 359         pinMode( 0, OUTPUT);
 360         pinMode( 1, OUTPUT);
 361
 362         // led
 363         pinMode(13, OUTPUT);
 364
 365         // lower other priorities
 366         SCB_SHPR1 = SCB_SHPR2 = SCB_SHPR3 = 0x10101010;
 367
 368         // CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16
 369         printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR);
 370         printf("GPIOC PDDR, PDIR: %08x %08x\n", GPIOC_PDIR, GPIOC_PDDR);
 371         printf("GPIOD PDDR, PDIR: %08x %08x\n", GPIOD_PDIR, GPIOD_PDDR);
 372         printf("PORTB_PCR16: %08x\n", PORTB_PCR16);
 373         printf("PORTC_PCR6:  %08x\n", PORTC_PCR6);
 374         printf("PORTD_PCR0:  %08x\n", PORTD_PCR0);
 375
 376         asm("mrs %0, BASEPRI" : "=r"(ret));
 377         printf("BASEPRI: %d, SHPR: %08x %08x %08x\n",
 378                 ret, SCB_SHPR1, SCB_SHPR2, SCB_SHPR3);
 379
 380         while (1) {
 381                 uint32_t i, now;
 382
 383                 now = millis();
 384                 for (i = 0; i < 2; i++) {
 385                         if (g.btn_state[i].phase == 0)
 386                                 continue;
 387                         if (now - g.btn_state[i].time > 1)
 388                                 g.btn_state[i].phase = 0;
 389                 }
 390
 391                 // led?
 392                 if (CORE_PIN13_PORTREG & CORE_PIN13_BITMASK) {
 393                         if ((int)(now - led_time) > 10)
 394                                 CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK;
 395                 }
 396
 397                 // something on rawhid?
 398                 if (usb_rawhid_available() > 0)
 399                 {
 400                         ret = usb_rawhid_recv(buf, 20);
 401                         if (ret == 64) {
 402                                 led_time = millis();
 403                                 CORE_PIN13_PORTSET = CORE_PIN13_BITMASK;
 404
 405                                 do_usb(buf);
 406                         }
 407                         else {
 408                                 printf("usb_rawhid_recv: %d\n", ret);
 409                         }
 410                 }
 411         }
 412
 413         return 0;
 414 }