[megadrive.git] / teensytp / main.c

/*
 * TeensyTP, Team Player/4-Player Adaptor implementation for Teensy3
 * using a host machine with USB hub
 * Copyright (c) 2015 notaz
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "teensy3/core_pins.h"
#include "teensy3/usb_seremu.h"
#include "teensy3/usb_rawhid.h"
#include "pkts.h"

#define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0]))

#define noinline __attribute__((noclone,noinline))

ssize_t _write(int fd, const void *buf, size_t nbyte)
{
	char tbuf[64];
	int ret;

	if (fd != 1 && fd != 2) {
		snprintf(tbuf, sizeof(tbuf), "write to fd %d\n", fd);
		usb_seremu_write(tbuf, strlen(tbuf));
	}

	ret = usb_seremu_write(buf, nbyte);
	return ret < 0 ? ret : nbyte;
}

void yield(void)
{
}

/*
 * 00157:231: w 70 @ 007ef2
 * 00157:231: r 7f @ 007f06
 * 00157:231: w 30 @ 007efa
 * 00157:231: r 33 @ 007f06
 * 00157:232: w 40 @ 007f6a
 * 00157:232: r 7f @ 007f70
 */

/* ?0SA 00DU, ?1CB RLDU */

static struct {
	struct {
		union {
			uint8_t b[2];
			uint8_t b6[4 * 2];   // 6btn
			uint32_t w;          // for more atomic change
		};
		uint32_t phase;              // 6btn
		uint32_t time;
	} btn_state[2];

	uint8_t tp_state[16];
	uint32_t tp_cnt;

	uint32_t mode;
	uint32_t pl1th:1;
} g;

/* player1 TH */
#define PL1_ISFR     PORTD_ISFR
#define PL1_TH()     ((CORE_PIN21_PINREG >> CORE_PIN21_BIT) & 1)
#define PL1_TR()     ((CORE_PIN20_PINREG >> CORE_PIN20_BIT) & 1)
#define PL1_TH_MASK  CORE_PIN21_BITMASK
#define PL1_TR_MASK  CORE_PIN20_BITMASK

static void pl1_isr_3btn(void)
{
	uint32_t isfr;

	isfr = PL1_ISFR;
	PL1_ISFR = isfr;

	GPIOD_PDOR = g.btn_state[0].b[PL1_TH()];
}

static void pl1_isr_6btn(void)
{
	uint32_t isfr, th, phase = 0;

	isfr = PL1_ISFR;
	PL1_ISFR = isfr;

	th = PL1_TH();
	if (th)
		g.btn_state[0].phase++;
	if (g.btn_state[0].phase < 4)
		phase = g.btn_state[0].phase;

	GPIOD_PDOR = g.btn_state[0].b[(phase << 1) | PL1_TH()];
	g.btn_state[0].time = millis();
}

// team player
static noinline void pl1_isr_tp_do_th(void)
{
	uint32_t cnt, th;

	th = PL1_TH();
	cnt = !th;

	GPIOD_PDOR = g.tp_state[cnt] | (PL1_TR() << 4);
	g.tp_cnt = cnt;
	g.pl1th = th;
}

static void pl1_isr_tp(void)
{
	uint32_t isfr;

	isfr = PL1_ISFR;
	PL1_ISFR = isfr;

	if (isfr & PL1_TH_MASK)
		pl1_isr_tp_do_th();
	if (!(isfr & PL1_TR_MASK))
		return;
	if (g.pl1th)
		return;

	g.tp_cnt++;
	GPIOD_PDOR = g.tp_state[g.tp_cnt & 0x0f] | (PL1_TR() << 4);
}

/* player2 TH */
#define PL2_ISFR   PORTC_ISFR
#define PL2_TH()   ((CORE_PIN15_PINREG >> CORE_PIN15_BIT) & 1)
#define PL2_ADJ(x) ((x) | ((x) << 12))

static void pl2_isr_nop(void)
{
	uint32_t isfr;

	isfr = PL2_ISFR;
	PL2_ISFR = isfr;

	GPIOB_PDOR = PL2_ADJ(0x3f);
}

static void pl2_isr_3btn(void)
{
	uint32_t isfr;

	isfr = PL2_ISFR;
	PL2_ISFR = isfr;

	GPIOB_PDOR = PL2_ADJ(g.btn_state[1].b[PL2_TH()]);
}

static void pl2_isr_6btn(void)
{
	uint32_t isfr, th, phase = 0;

	isfr = PL2_ISFR;
	PL2_ISFR = isfr;

	th = PL2_TH();
	if (th)
		g.btn_state[1].phase++;
	if (g.btn_state[1].phase < 4)
		phase = g.btn_state[1].phase;

	GPIOB_PDOR = PL2_ADJ(g.btn_state[1].b[(phase << 1) | PL2_TH()]);
	g.btn_state[1].time = millis();
}

/* * */
static void clear_state()
{
	int p, i;

	// no hw connected
	for (p = 0; p < 2; p++) {
		for (i = 0; i < 4 * 2; i++)
			g.btn_state[p].b6[i] = 0x3f;
		g.btn_state[p].phase = 0;
	}

	// 4 pads, nothing pressed
	g.tp_state[0] = 0x03;
	g.tp_state[1] = 0x0f;
	g.tp_state[2] = 0x00;
	g.tp_state[3] = 0x00;
	for (i = 4; i < 8; i++)
		g.tp_state[i] = 0x00; // 3btn
	for (; i < 16; i++)
		g.tp_state[i] = 0x0f;

	g.tp_cnt = 0;
	g.pl1th = PL1_TH();
}

static void switch_mode(int mode)
{
	void (*pl1_handler)(void) = pl1_isr_3btn;
	void (*pl2_handler)(void) = pl2_isr_3btn;

	clear_state();

	switch (mode) {
	default:
	case OP_MODE_3BTN:
		pinMode(20, OUTPUT);
		break;
	case OP_MODE_6BTN:
		pinMode(20, OUTPUT);
		pl1_handler = pl1_isr_6btn;
		pl2_handler = pl2_isr_6btn;
		break;
	case OP_MODE_TEAMPLAYER:
		pinMode(20, INPUT);
		attachInterrupt(20, pl1_handler, CHANGE);
		pl1_handler = pl1_isr_tp;
		pl2_handler = pl2_isr_nop;
		GPIOB_PDOR = PL2_ADJ(0x3f);
		break;
	}

	attachInterruptVector(IRQ_PORTD, pl1_handler);
	attachInterruptVector(IRQ_PORTC, pl2_handler);
	g.mode = mode;
}

// btns: MXYZ SACB RLDU
static void update_btns_btn(const struct tp_pkt *pkt, uint32_t player)
{
	// ?1CB RLDU ?0SA 00DU
	uint32_t s_6btn0, s_6btn1;
	uint32_t s_3btn;

	s_3btn  = (~pkt->bnts[player] << 8) & 0x3f00;
	s_3btn |= (~pkt->bnts[player] >> 2) & 0x0030; // SA
	s_3btn |= (~pkt->bnts[player]     ) & 0x0003; // DU
	s_3btn |= s_3btn << 16;
	g.btn_state[player].w = s_3btn;

	// 6btn stuff
	s_6btn0 = s_3btn & 0x30;
	s_6btn1 = s_3btn >> 8;
	g.btn_state[player].b6[4] = s_6btn0;        // ?0SA 0000
	g.btn_state[player].b6[5] = s_6btn1;        // ?1CB RLDU
	g.btn_state[player].b6[6] = s_6btn0 | 0x0f; // ?0SA 1111
	s_6btn1 &= 0x30;
	s_6btn1 |= (~pkt->bnts[player] >> 8) & 0x0f;
	g.btn_state[player].b6[7] = s_6btn1;        // ?1CB MXYZ
}

static void update_btns_tp(const struct tp_pkt *pkt, uint32_t player)
{
	uint8_t b0, b1;

	b0 =  ~pkt->bnts[player] & 0x0f;
	b1 = (~pkt->bnts[player] >> 4) & 0x0f;
	g.tp_state[8 + player * 2    ] = b0;
	g.tp_state[8 + player * 2 + 1] = b1;
}

static void do_usb(const void *buf)
{
	const struct tp_pkt *pkt = buf;
	uint32_t i;

	switch (pkt->type) {
	case PKT_UPD_MODE:
		__disable_irq();
		switch_mode(pkt->mode);
		__enable_irq();
		break;
	case PKT_UPD_BTNS:
		switch (g.mode) {
		case OP_MODE_3BTN:
		case OP_MODE_6BTN:
			if (pkt->changed_players & 1)
				update_btns_btn(pkt, 0);
			if (pkt->changed_players & 2)
				update_btns_btn(pkt, 1);
			break;
		case OP_MODE_TEAMPLAYER:
			for (i = 0; i < 4; i++) {
				if (!(pkt->changed_players & (1 << i)))
					continue;
				update_btns_tp(pkt, i);
			}
		}
		break;
	default:
		printf("got unknown pkt type: %04x\n", pkt->type);
		break;
	}
}

int main(void)
{
	uint32_t led_time = 0;
	uint8_t buf[64];
	int ret;

	delay(1000); // wait for usb..

	printf("starting, rawhid: %d\n", usb_rawhid_available());

	switch_mode(OP_MODE_3BTN);

	// md pin   th tr tl  r  l  d  u vsync   th  tr  tl  r  l  d  u
	//           7  9  6  4  3  2  1          7   9   6  4  3  2  1
	// md bit*   6  5  4  3  2  1  0          6   5   4  3  2  1  0
	// t bit    d6 d5 d4 d3 d2 d1 d0   a12   c0 b17 b16 b3 b2 b1 b0
	// t pin    21 20  6  8  7 14  2     3   15   1   0 18 19 17 16
	// * - note: tl/tr mixed in most docs

	// player1
	pinMode(21, INPUT);
	// note: func is not used, see attachInterruptVector()
	attachInterrupt(21, pl1_isr_3btn, CHANGE);
	NVIC_SET_PRIORITY(IRQ_PORTD, 0);

	pinMode( 2, OUTPUT);
	pinMode(14, OUTPUT);
	pinMode( 7, OUTPUT);
	pinMode( 8, OUTPUT);
	pinMode( 6, OUTPUT);
	pinMode(20, OUTPUT);

	// player2
	pinMode(15, INPUT);
	attachInterrupt(15, pl2_isr_nop, CHANGE);
	NVIC_SET_PRIORITY(IRQ_PORTC, 0);

	pinMode(16, OUTPUT);
	pinMode(17, OUTPUT);
	pinMode(19, OUTPUT);
	pinMode(18, OUTPUT);
	pinMode( 0, OUTPUT);
	pinMode( 1, OUTPUT);

	// led
	pinMode(13, OUTPUT);

	// lower other priorities
	SCB_SHPR1 = SCB_SHPR2 = SCB_SHPR3 = 0x10101010;

	// CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16
	printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR);
	printf("GPIOC PDDR, PDIR: %08x %08x\n", GPIOC_PDIR, GPIOC_PDDR);
	printf("GPIOD PDDR, PDIR: %08x %08x\n", GPIOD_PDIR, GPIOD_PDDR);
	printf("PORTB_PCR16: %08x\n", PORTB_PCR16);
	printf("PORTC_PCR6:  %08x\n", PORTC_PCR6);
	printf("PORTD_PCR0:  %08x\n", PORTD_PCR0);

	asm("mrs %0, BASEPRI" : "=r"(ret));
	printf("BASEPRI: %d, SHPR: %08x %08x %08x\n",
		ret, SCB_SHPR1, SCB_SHPR2, SCB_SHPR3);

	while (1) {
		uint32_t i, now;

		now = millis();
		for (i = 0; i < 2; i++) {
			if (g.btn_state[i].phase == 0)
				continue;
			if (now - g.btn_state[i].time > 1)
				g.btn_state[i].phase = 0;
		}

		// led?
		if (CORE_PIN13_PORTREG & CORE_PIN13_BITMASK) {
			if ((int)(now - led_time) > 10)
				CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK;
		}

		// something on rawhid?
		if (usb_rawhid_available() > 0)
		{
			ret = usb_rawhid_recv(buf, 20);
			if (ret == 64) {
				led_time = millis();
				CORE_PIN13_PORTSET = CORE_PIN13_BITMASK;

				do_usb(buf);
			}
			else {
				printf("usb_rawhid_recv: %d\n", ret);
			}
		}
	}

	return 0;
}
Commit	Line	Data
c28a7c81	1	/*
ebeef6c6	2	* TeensyTP, Team Player/4-Player Adaptor implementation for Teensy3
	3	* using a host machine with USB hub
	4	* Copyright (c) 2015 notaz
c28a7c81	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
0c1e003e	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"
f20de073	33	#include "pkts.h"
f20de073	34
19560e5f	35	#define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0]))
19560e5f	36
ebeef6c6	37	#define noinline __attribute__((noclone,noinline))
f20de073	38
0c1e003e	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
ebeef6c6	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 {
59eb6fac	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
ebeef6c6	79	uint8_t tp_state[16];
59eb6fac	80	uint32_t tp_cnt;
ebeef6c6	81
ebeef6c6	82	uint32_t mode;
	83	uint32_t pl1th:1;
	84	} g;
19560e5f	85
19560e5f	86	/* player1 TH */
ebeef6c6	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
19560e5f	92
ebeef6c6	93	static void pl1_isr_3btn(void)
9c4f55f4	94	{
ebeef6c6	95	uint32_t isfr;
9c4f55f4	96
19560e5f	97	isfr = PL1_ISFR;
19560e5f	98	PL1_ISFR = isfr;
4af6d4e5	99
59eb6fac	100	GPIOD_PDOR = g.btn_state[0].b[PL1_TH()];
4af6d4e5	101	}
4af6d4e5	102
59eb6fac	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
ebeef6c6	121	static noinline void pl1_isr_tp_do_th(void)
8e400118	122	{
ebeef6c6	123	uint32_t cnt, th;
8e400118	124
19560e5f	125	th = PL1_TH();
ebeef6c6	126	cnt = !th;
f20de073	127
ebeef6c6	128	GPIOD_PDOR = g.tp_state[cnt] \| (PL1_TR() << 4);
59eb6fac	129	g.tp_cnt = cnt;
ebeef6c6	130	g.pl1th = th;
f20de073	131	}
f20de073	132
ebeef6c6	133	static void pl1_isr_tp(void)
4af6d4e5	134	{
ebeef6c6	135	uint32_t isfr;
4af6d4e5	136
19560e5f	137	isfr = PL1_ISFR;
19560e5f	138	PL1_ISFR = isfr;
4af6d4e5	139
ebeef6c6	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
59eb6fac	147	g.tp_cnt++;
59eb6fac	148	GPIOD_PDOR = g.tp_state[g.tp_cnt & 0x0f] \| (PL1_TR() << 4);
4af6d4e5	149	}
4af6d4e5	150
19560e5f	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
ebeef6c6	156	static void pl2_isr_nop(void)
19560e5f	157	{
ebeef6c6	158	uint32_t isfr;
19560e5f	159
	160	isfr = PL2_ISFR;
	161	PL2_ISFR = isfr;
4af6d4e5	162
59eb6fac	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();
e8e66d02	191	}
e8e66d02	192
19560e5f	193	/* * */
ebeef6c6	194	static void clear_state()
6d4349fc	195	{
ebeef6c6	196	int p, i;
	197
	198	// no hw connected
	199	for (p = 0; p < 2; p++) {
59eb6fac	200	for (i = 0; i < 4 * 2; i++)
	201	g.btn_state[p].b6[i] = 0x3f;
	202	g.btn_state[p].phase = 0;
6d4349fc	203	}
f20de073	204
ebeef6c6	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
59eb6fac	215	g.tp_cnt = 0;
ebeef6c6	216	g.pl1th = PL1_TH();
f20de073	217	}
f20de073	218
ebeef6c6	219	static void switch_mode(int mode)
f20de073	220	{
ebeef6c6	221	void (*pl1_handler)(void) = pl1_isr_3btn;
59eb6fac	222	void (*pl2_handler)(void) = pl2_isr_3btn;
f20de073	223
ebeef6c6	224	clear_state();
f20de073	225
ebeef6c6	226	switch (mode) {
	227	default:
	228	case OP_MODE_3BTN:
59eb6fac	229	pinMode(20, OUTPUT);
59eb6fac	230	break;
ebeef6c6	231	case OP_MODE_6BTN:
ebeef6c6	232	pinMode(20, OUTPUT);
59eb6fac	233	pl1_handler = pl1_isr_6btn;
59eb6fac	234	pl2_handler = pl2_isr_6btn;
ebeef6c6	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;
f20de073	243	}
f20de073	244
ebeef6c6	245	attachInterruptVector(IRQ_PORTD, pl1_handler);
	246	attachInterruptVector(IRQ_PORTC, pl2_handler);
	247	g.mode = mode;
f20de073	248	}
f20de073	249
ebeef6c6	250	// btns: MXYZ SACB RLDU
59eb6fac	251	static void update_btns_btn(const struct tp_pkt *pkt, uint32_t player)
e8e66d02	252	{
ebeef6c6	253	// ?1CB RLDU ?0SA 00DU
59eb6fac	254	uint32_t s_6btn0, s_6btn1;
59eb6fac	255	uint32_t s_3btn;
e8e66d02	256
ebeef6c6	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
59eb6fac	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
e8e66d02	272	}
e8e66d02	273
ebeef6c6	274	static void update_btns_tp(const struct tp_pkt *pkt, uint32_t player)
e8e66d02	275	{
ebeef6c6	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;
f20de073	282	}
f20de073	283
ebeef6c6	284	static void do_usb(const void *buf)
f20de073	285	{
ebeef6c6	286	const struct tp_pkt *pkt = buf;
ebeef6c6	287	uint32_t i;
f20de073	288
f20de073	289	switch (pkt->type) {
ebeef6c6	290	case PKT_UPD_MODE:
e8e66d02	291	__disable_irq();
ebeef6c6	292	switch_mode(pkt->mode);
4af6d4e5	293	__enable_irq();
f20de073	294	break;
ebeef6c6	295	case PKT_UPD_BTNS:
	296	switch (g.mode) {
	297	case OP_MODE_3BTN:
59eb6fac	298	case OP_MODE_6BTN:
ebeef6c6	299	if (pkt->changed_players & 1)
59eb6fac	300	update_btns_btn(pkt, 0);
ebeef6c6	301	if (pkt->changed_players & 2)
59eb6fac	302	update_btns_btn(pkt, 1);
ebeef6c6	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	}
f20de073	310	}
f20de073	311	break;
	312	default:
	313	printf("got unknown pkt type: %04x\n", pkt->type);
	314	break;
	315	}
9c4f55f4	316	}
9c4f55f4	317
0c1e003e	318	int main(void)
0c1e003e	319	{
beaf6d89	320	uint32_t led_time = 0;
f20de073	321	uint8_t buf[64];
ebeef6c6	322	int ret;
0c1e003e	323
	324	delay(1000); // wait for usb..
	325
	326	printf("starting, rawhid: %d\n", usb_rawhid_available());
	327
ebeef6c6	328	switch_mode(OP_MODE_3BTN);
19560e5f	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
9c4f55f4	335	// * - note: tl/tr mixed in most docs
19560e5f	336
	337	// player1
	338	pinMode(21, INPUT);
ebeef6c6	339	// note: func is not used, see attachInterruptVector()
ebeef6c6	340	attachInterrupt(21, pl1_isr_3btn, CHANGE);
19560e5f	341	NVIC_SET_PRIORITY(IRQ_PORTD, 0);
f20de073	342
9c4f55f4	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
19560e5f	350	// player2
19560e5f	351	pinMode(15, INPUT);
ebeef6c6	352	attachInterrupt(15, pl2_isr_nop, CHANGE);
19560e5f	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
9c4f55f4	362	// led
9c4f55f4	363	pinMode(13, OUTPUT);
9c4f55f4	364
19560e5f	365	// lower other priorities
	366	SCB_SHPR1 = SCB_SHPR2 = SCB_SHPR3 = 0x10101010;
	367
9c4f55f4	368	// CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16
4af6d4e5	369	printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR);
9c4f55f4	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);
4af6d4e5	373	printf("PORTC_PCR6: %08x\n", PORTC_PCR6);
8e400118	374	printf("PORTD_PCR0: %08x\n", PORTD_PCR0);
9c4f55f4	375
f20de073	376	asm("mrs %0, BASEPRI" : "=r"(ret));
8e400118	377	printf("BASEPRI: %d, SHPR: %08x %08x %08x\n",
8e400118	378	ret, SCB_SHPR1, SCB_SHPR2, SCB_SHPR3);
0c1e003e	379
1cb2822f	380	while (1) {
59eb6fac	381	uint32_t i, now;
e8e66d02	382
beaf6d89	383	now = millis();
59eb6fac	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	}
beaf6d89	390
beaf6d89	391	// led?
	392	if (CORE_PIN13_PORTREG & CORE_PIN13_BITMASK) {
	393	if ((int)(now - led_time) > 10)
	394	CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK;
1cb2822f	395	}
beaf6d89	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	}
1cb2822f	410	}
0c1e003e	411	}
1cb2822f	412
1cb2822f	413	return 0;
0c1e003e	414	}