[teensytas.git] / main.c

/*
 * TeensyTAS, TAS input player for MegaDrive
 * Copyright (c) 2014 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__((noinline))

// use power of 2
#define STREAM_BUF_SIZE 512
#define STREAM_BUF_MASK (512 - 1)

/* ?0SA 00DU, ?1CB RLDU */
#define STREAM_EL_SZ 2

static struct {
	uint8_t stream_to[2][STREAM_BUF_SIZE][STREAM_EL_SZ];
	uint8_t stream_from[STREAM_BUF_SIZE][STREAM_EL_SZ];
	struct {
		union {
			uint8_t fixed_state[4];
			uint32_t fixed_state32;
		};
		union {
			uint8_t pending_state[4];
			uint32_t pending_state32;
		};
	} pl[2];
	uint32_t stream_enable_to:1;
	uint32_t stream_enable_from:1;
	uint32_t stream_started:1;
	uint32_t stream_ended:1;
	uint32_t inc_mode:2;
	uint32_t use_pending:1;
	uint32_t frame_cnt;
	uint32_t edge_cnt;
	struct {
		uint32_t i;
		uint32_t o;
	} pos_to_p[2], pos_from;
} g;

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)
{
}

static noinline void choose_isrs_idle(void);

/* player1 TH */
#define PL1_ISFR   PORTD_ISFR
#define PL1_TH()   ((CORE_PIN21_PINREG >> CORE_PIN21_BIT) & 1)

static void pl1th_isr_fixed(void)
{
	uint32_t isfr, th;

	isfr = PL1_ISFR;
	PL1_ISFR = isfr;
	th = PL1_TH();

	GPIOD_PDOR = g.pl[0].fixed_state[th];
	g.edge_cnt++;
}

static noinline void do_to_step(void)
{
	g.frame_cnt++;

	g.pos_to_p[0].o = (g.pos_to_p[0].o + 1) & STREAM_BUF_MASK;
	if (g.pos_to_p[0].o == g.pos_to_p[0].i)
		// done
		choose_isrs_idle();
}

static void pl1th_isr_do_to_inc(void)
{
	uint32_t isfr, th;

	isfr = PL1_ISFR;
	PL1_ISFR = isfr;
	th = PL1_TH();

	GPIOD_PDOR = g.stream_to[0][g.pos_to_p[0].o][th];
	if (th)
		do_to_step();
}

static void pl1th_isr_do_to(void)
{
	uint32_t isfr, th;

	isfr = PL1_ISFR;
	PL1_ISFR = isfr;
	th = PL1_TH();

	GPIOD_PDOR = g.stream_to[0][g.pos_to_p[0].o][th];
	g.edge_cnt++;
}

/* 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 pl2th_isr_fixed(void)
{
	uint32_t isfr, th, v;

	isfr = PL2_ISFR;
	PL2_ISFR = isfr;
	th = PL2_TH();

	v = g.pl[1].fixed_state[th];
	GPIOB_PDOR = PL2_ADJ(v);
}

static void pl2th_isr_do_to(void)
{
	uint32_t isfr, th, v;

	isfr = PL2_ISFR;
	PL2_ISFR = isfr;
	th = PL2_TH();

	v = g.stream_to[1][g.pos_to_p[0].o][th];
	GPIOB_PDOR = PL2_ADJ(v);

	g.pos_to_p[1].o = g.pos_to_p[0].o;
}

static void pl2th_isr_do_to_inc_pl1(void)
{
	uint32_t isfr, th, v;

	isfr = PL2_ISFR;
	PL2_ISFR = isfr;
	th = PL2_TH();

	v = g.stream_to[1][g.pos_to_p[1].o][th];
	GPIOB_PDOR = PL2_ADJ(v);
	if (th) {
		do_to_step();
		g.pos_to_p[1].o = g.pos_to_p[0].o;
	}
}

/* vsync handler */
#define VSYNC_ISFR PORTC_ISFR

static void vsync_isr_nop(void)
{
	uint32_t isfr;

	isfr = VSYNC_ISFR;
	VSYNC_ISFR = isfr;
}

// /vsync starts at line 235/259 (ntsc/pal), just as vcounter jumps back
// we care when it comes out (/vsync goes high) after 3 lines at 238/262
static void vsync_isr_frameinc(void)
{
	uint32_t isfr;

	isfr = VSYNC_ISFR;
	VSYNC_ISFR = isfr;

	g.pos_to_p[0].o = (g.pos_to_p[0].o + 1) & STREAM_BUF_MASK;
	g.pos_to_p[1].o = g.pos_to_p[0].o;

	if (g.pos_to_p[0].o == g.pos_to_p[0].i)
		choose_isrs_idle();
	g.frame_cnt++;
}

/* "recording" data */
static noinline void do_from_step(void)
{
	uint32_t s;

	// should hopefully give atomic fixed_state read..
	s = g.pl[0].fixed_state32;
	g.pl[0].fixed_state32 = g.pl[0].pending_state32;
	g.stream_from[g.pos_from.i][0] = s;
	g.stream_from[g.pos_from.i][1] = s >> 8;
	g.pos_from.i = (g.pos_from.i + 1) & STREAM_BUF_MASK;
}

static void pl1th_isr_fixed_do_from(void)
{
	uint32_t isfr, th;

	isfr = PL1_ISFR;
	PL1_ISFR = isfr;
	th = PL1_TH();

	GPIOD_PDOR = g.pl[0].fixed_state[th];
	if (th)
		do_from_step();
	g.edge_cnt++;
}

static void vsync_isr_frameinc_do_from(void)
{
	uint32_t isfr;

	isfr = VSYNC_ISFR;
	VSYNC_ISFR = isfr;

	do_from_step();
	g.frame_cnt++;
}

/* * */
static void choose_isrs(void)
{
	void (*pl1th_handler)(void) = pl1th_isr_fixed;
	void (*pl2th_handler)(void) = pl2th_isr_fixed;
	void (*vsync_handler)(void) = vsync_isr_nop;

	if (g.stream_enable_to) {
		switch (g.inc_mode) {
		case INC_MODE_VSYNC:
			pl1th_handler = pl1th_isr_do_to;
			pl2th_handler = pl2th_isr_do_to;
			vsync_handler = vsync_isr_frameinc;
			break;
		case INC_MODE_SHARED_PL1:
			pl1th_handler = pl1th_isr_do_to_inc;
			pl2th_handler = pl2th_isr_do_to;
			break;
		case INC_MODE_SHARED_PL2:
			pl1th_handler = pl1th_isr_do_to;
			pl2th_handler = pl2th_isr_do_to_inc_pl1;
			break;
		}
	}
	else if (g.stream_enable_from) {
		g.use_pending = 1;
		switch (g.inc_mode) {
		case INC_MODE_VSYNC:
			vsync_handler = vsync_isr_frameinc_do_from;
			break;
		case INC_MODE_SHARED_PL1:
			pl1th_handler = pl1th_isr_fixed_do_from;
			break;
		case INC_MODE_SHARED_PL2:
			/* TODO */
			break;
		}
	}

	attachInterruptVector(IRQ_PORTD, pl1th_handler);
	attachInterruptVector(IRQ_PORTC, pl2th_handler);
	attachInterruptVector(IRQ_PORTA, vsync_handler);
}

static noinline void choose_isrs_idle(void)
{
	attachInterruptVector(IRQ_PORTD, pl1th_isr_fixed);
	attachInterruptVector(IRQ_PORTC, pl2th_isr_fixed);
	attachInterruptVector(IRQ_PORTA, vsync_isr_nop);
}

static void udelay(uint32_t us)
{
	uint32_t start = micros();

	while ((micros() - start) < us) {
		asm volatile("nop; nop; nop; nop");
		yield();
	}
}

static void do_start_seq(void)
{
	uint32_t edge_cnt_last;
	uint32_t edge_cnt;
	uint32_t start, t1, t2;
	int tout;

	start = micros();
	edge_cnt = g.edge_cnt;

	/* magic value */
	g.pl[0].fixed_state[0] =
	g.pl[0].fixed_state[1] = 0x25;

	for (tout = 10000; tout > 0; tout--) {
		edge_cnt_last = edge_cnt;
		udelay(100);
		edge_cnt = g.edge_cnt;

		if (edge_cnt != edge_cnt_last)
			continue;
		if (!PL1_TH())
			break;
	}

	g.pl[0].fixed_state[0] = 0x33;
	g.pl[0].fixed_state[1] = 0x3f;
	GPIOD_PDOR = 0x33;

	t1 = micros();
	if (tout == 0) {
		printf("start_seq timeout1, t=%u\n", t1 - start);
		return;
	}

	for (tout = 100000; tout > 0; tout--) {
		udelay(1);

		if (PL1_TH())
			break;
	}

	t2 = micros();
	if (tout == 0) {
		printf("start_seq timeout2, t1=%u, t2=%u\n",
			t1 - start, t2 - t1);
		return;
	}

	//printf(" t1=%u, t2=%u\n", t1 - start, t2 - t1);

	if (g.stream_started) {
		printf("got start_seq when already started\n");
		return;
	}

	if (!g.stream_enable_to && !g.stream_enable_from) {
		printf("got start_seq, without enable from USB\n");
		return;
	}

	if (g.stream_enable_to && g.pos_to_p[0].i == g.pos_to_p[0].o) {
		printf("got start_seq while stream_to is empty\n");
		return;
	}

	if (g.stream_enable_from && g.pos_from.i != g.pos_from.o) {
		printf("got start_seq while stream_from is not empty\n");
		return;
	}

	__disable_irq();
	choose_isrs();
	g.stream_started = 1;
	__enable_irq();
}

// callers must disable IRQs
static void clear_state(void)
{
	int i;

	g.stream_enable_to = 0;
	g.stream_enable_from = 0;
	g.stream_started = 0;
	g.stream_ended = 0;
	g.inc_mode = INC_MODE_VSYNC;
	g.use_pending = 0;
	for (i = 0; i < ARRAY_SIZE(g.pos_to_p); i++)
		g.pos_to_p[i].i = g.pos_to_p[i].o = 0;
	g.pos_from.i = g.pos_from.o = 0;
	g.frame_cnt = 0;
	choose_isrs_idle();
}

static int get_space_to(int p)
{
	return STREAM_BUF_SIZE - ((g.pos_to_p[p].i - g.pos_to_p[p].o)
		& STREAM_BUF_MASK);
}

static int get_used_from(void)
{
	return (g.pos_from.i - g.pos_from.o) & STREAM_BUF_MASK;
}

static void do_usb(void *buf)
{
	struct tas_pkt *pkt = buf;
	uint32_t pos_to_i, i, p;
	int space;

	switch (pkt->type) {
	case PKT_FIXED_STATE:
		memcpy(&i, pkt->data, sizeof(i));
		if (g.use_pending)
			g.pl[0].pending_state32 = i;
		else
			g.pl[0].fixed_state32 = i;
		break;
	case PKT_STREAM_ENABLE:
		__disable_irq();
		clear_state();
		/* wait for start from MD */
		g.stream_enable_to = pkt->enable.stream_to;
		g.stream_enable_from = pkt->enable.stream_from;
		g.inc_mode = pkt->enable.inc_mode;
		if (pkt->enable.no_start_seq) {
			GPIOD_PDOR = 0x3f;
			choose_isrs();
			g.stream_started = 1;
		}
		__enable_irq();
		break;
	case PKT_STREAM_ABORT:
		__disable_irq();
		clear_state();
		__enable_irq();
		break;
	case PKT_STREAM_END:
		g.stream_ended = 1;
		printf("end of stream\n");
		break;
	case PKT_STREAM_DATA_TO_P1:
	case PKT_STREAM_DATA_TO_P2:
		p = pkt->type == PKT_STREAM_DATA_TO_P1 ? 0 : 1;
		pos_to_i = g.pos_to_p[p].i;
		space = get_space_to(p);
		if (space <= pkt->size / STREAM_EL_SZ) {
			printf("got data pkt while space=%d\n", space);
			return;
		}
		for (i = 0; i < pkt->size / STREAM_EL_SZ; i++) {
			memcpy(&g.stream_to[p][pos_to_i++],
			       pkt->data + i * STREAM_EL_SZ,
			       STREAM_EL_SZ);
			pos_to_i &= STREAM_BUF_MASK;
		}
		g.pos_to_p[p].i = pos_to_i;
		break;
	default:
		printf("got unknown pkt type: %04x\n", pkt->type);
		break;
	}
}

static void check_get_data(int p)
{
	struct tas_pkt pkt;
	uint8_t buf[64];
	int ret;

	if (get_space_to(p) <= sizeof(pkt.data) / STREAM_EL_SZ)
		return;

	if (g.pos_to_p[p].i == g.pos_to_p[p].o && g.frame_cnt != 0) {
		printf("underflow detected\n");
		g.stream_enable_to = 0;
		return;
	}

	pkt.type = PKT_STREAM_REQ;
	pkt.req.frame = g.frame_cnt;
	pkt.req.is_p2 = p;

	ret = usb_rawhid_send(&pkt, 1000);
	if (ret != sizeof(pkt)) {
		printf("send STREAM_REQ/%d: %d\n", p, ret);
		return;
	}

	ret = usb_rawhid_recv(buf, 1000);
	if (ret != 64)
		printf("usb_rawhid_recv/s: %d\n", ret);
	else
		do_usb(buf);
}

int main(void)
{
	uint32_t led_time = 0;
	uint32_t scheck_time = 0;
	uint32_t edge_cnt_last;
	uint32_t edge_cnt;
	uint8_t buf[64];
	int i, ret;

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

	/* ?0SA 00DU, ?1CB RLDU */
	for (i = 0; i < 2; i++) {
		g.pl[i].fixed_state[0] = 0x33;
		g.pl[i].fixed_state[1] = 0x3f;
	}

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

	choose_isrs_idle();

	// 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);
	attachInterrupt(21, pl1th_isr_fixed, 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, pl1th_isr_fixed, 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);

	// vsync line
	pinMode(3, INPUT);
	attachInterrupt(3, vsync_isr_nop, RISING);
	NVIC_SET_PRIORITY(IRQ_PORTA, 16);

	// 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);

	edge_cnt_last = g.edge_cnt;

	while (1) {
		struct tas_pkt pkt;
		uint32_t now;

		if (g.stream_enable_to && !g.stream_ended) {
			check_get_data(0);
			if (g.inc_mode == INC_MODE_SHARED_PL2)
				check_get_data(1);
		}

		while (g.stream_enable_from && !g.stream_ended
		  && get_used_from() >= sizeof(pkt.data) / STREAM_EL_SZ)
		{
			uint32_t o;
			int i;

			o = g.pos_from.o;
			for (i = 0; i < sizeof(pkt.data); i += STREAM_EL_SZ) {
				memcpy(pkt.data + i, &g.stream_from[o++],
					STREAM_EL_SZ);
				o &= STREAM_BUF_MASK;
			}
			g.pos_from.o = o;

			pkt.type = PKT_STREAM_DATA_FROM;
			pkt.size = i;

			ret = usb_rawhid_send(&pkt, 1000);
			if (ret != sizeof(pkt)) {
				printf("send DATA_FROM: %d\n", ret);
				break;
			}
		}

		now = millis();

		// start condition check
		if (now - scheck_time > 1000) {
			edge_cnt = g.edge_cnt;
			//printf("e: %d th: %d\n", edge_cnt - edge_cnt_last,
			//      PL1_TH());
			if ((g.stream_enable_to || g.stream_enable_from)
			    && !g.stream_started
			    && edge_cnt - edge_cnt_last > 10000)
			{
				do_start_seq();
				edge_cnt = g.edge_cnt;
			}
			edge_cnt_last = edge_cnt;
			scheck_time = now;
		}

		// 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	/*
	2	* TeensyTAS, TAS input player for MegaDrive
	3	* Copyright (c) 2014 notaz
	4	*
	5	* Permission is hereby granted, free of charge, to any person obtaining
	6	* a copy of this software and associated documentation files (the
	7	* "Software"), to deal in the Software without restriction, including
	8	* without limitation the rights to use, copy, modify, merge, publish,
	9	* distribute, sublicense, and/or sell copies of the Software, and to
	10	* permit persons to whom the Software is furnished to do so, subject to
	11	* the following conditions:
	12	*
	13	* The above copyright notice and this permission notice shall be
	14	* included in all copies or substantial portions of the Software.
	15	*
	16	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
	17	* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
	18	* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
	19	* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
	20	* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
	21	* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
	22	* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
	23	* SOFTWARE.
	24	*/
	25
0c1e003e	26	#include <stdint.h>
	27	#include <stdio.h>
	28	#include <string.h>
	29	#include "teensy3/core_pins.h"
	30	#include "teensy3/usb_seremu.h"
	31	#include "teensy3/usb_rawhid.h"
f20de073	32	#include "pkts.h"
f20de073	33
19560e5f	34	#define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0]))
19560e5f	35
e8e66d02	36	#define noinline __attribute__((noinline))
e8e66d02	37
f20de073	38	// use power of 2
	39	#define STREAM_BUF_SIZE 512
	40	#define STREAM_BUF_MASK (512 - 1)
0c1e003e	41
9c4f55f4	42	/* ?0SA 00DU, ?1CB RLDU */
e8e66d02	43	#define STREAM_EL_SZ 2
e8e66d02	44
f20de073	45	static struct {
19560e5f	46	uint8_t stream_to[2][STREAM_BUF_SIZE][STREAM_EL_SZ];
e8e66d02	47	uint8_t stream_from[STREAM_BUF_SIZE][STREAM_EL_SZ];
19560e5f	48	struct {
	49	union {
	50	uint8_t fixed_state[4];
	51	uint32_t fixed_state32;
	52	};
	53	union {
	54	uint8_t pending_state[4];
	55	uint32_t pending_state32;
	56	};
	57	} pl[2];
e8e66d02	58	uint32_t stream_enable_to:1;
e8e66d02	59	uint32_t stream_enable_from:1;
f20de073	60	uint32_t stream_started:1;
e8e66d02	61	uint32_t stream_ended:1;
19560e5f	62	uint32_t inc_mode:2;
72ef318c	63	uint32_t use_pending:1;
4af6d4e5	64	uint32_t frame_cnt;
f20de073	65	uint32_t edge_cnt;
19560e5f	66	struct {
	67	uint32_t i;
	68	uint32_t o;
	69	} pos_to_p[2], pos_from;
f20de073	70	} g;
f20de073	71
0c1e003e	72	ssize_t _write(int fd, const void *buf, size_t nbyte)
	73	{
	74	char tbuf[64];
	75	int ret;
	76
	77	if (fd != 1 && fd != 2) {
	78	snprintf(tbuf, sizeof(tbuf), "write to fd %d\n", fd);
	79	usb_seremu_write(tbuf, strlen(tbuf));
	80	}
	81
	82	ret = usb_seremu_write(buf, nbyte);
	83	return ret < 0 ? ret : nbyte;
	84	}
	85
	86	void yield(void)
	87	{
	88	}
	89
19560e5f	90	static noinline void choose_isrs_idle(void);
	91
	92	/* player1 TH */
	93	#define PL1_ISFR PORTD_ISFR
	94	#define PL1_TH() ((CORE_PIN21_PINREG >> CORE_PIN21_BIT) & 1)
	95
	96	static void pl1th_isr_fixed(void)
9c4f55f4	97	{
f20de073	98	uint32_t isfr, th;
9c4f55f4	99
19560e5f	100	isfr = PL1_ISFR;
	101	PL1_ISFR = isfr;
	102	th = PL1_TH();
4af6d4e5	103
19560e5f	104	GPIOD_PDOR = g.pl[0].fixed_state[th];
4af6d4e5	105	g.edge_cnt++;
	106	}
	107
8e400118	108	static noinline void do_to_step(void)
	109	{
	110	g.frame_cnt++;
	111
19560e5f	112	g.pos_to_p[0].o = (g.pos_to_p[0].o + 1) & STREAM_BUF_MASK;
19560e5f	113	if (g.pos_to_p[0].o == g.pos_to_p[0].i)
8e400118	114	// done
19560e5f	115	choose_isrs_idle();
8e400118	116	}
8e400118	117
19560e5f	118	static void pl1th_isr_do_to_inc(void)
4af6d4e5	119	{
4af6d4e5	120	uint32_t isfr, th;
9c4f55f4	121
19560e5f	122	isfr = PL1_ISFR;
	123	PL1_ISFR = isfr;
	124	th = PL1_TH();
f20de073	125
19560e5f	126	GPIOD_PDOR = g.stream_to[0][g.pos_to_p[0].o][th];
8e400118	127	if (th)
8e400118	128	do_to_step();
f20de073	129	}
f20de073	130
19560e5f	131	static void pl1th_isr_do_to(void)
4af6d4e5	132	{
	133	uint32_t isfr, th;
	134
19560e5f	135	isfr = PL1_ISFR;
	136	PL1_ISFR = isfr;
	137	th = PL1_TH();
4af6d4e5	138
19560e5f	139	GPIOD_PDOR = g.stream_to[0][g.pos_to_p[0].o][th];
4af6d4e5	140	g.edge_cnt++;
	141	}
	142
19560e5f	143	/* player2 TH */
	144	#define PL2_ISFR PORTC_ISFR
	145	#define PL2_TH() ((CORE_PIN15_PINREG >> CORE_PIN15_BIT) & 1)
	146	#define PL2_ADJ(x) ((x) \| ((x) << 12))
	147
	148	static void pl2th_isr_fixed(void)
	149	{
	150	uint32_t isfr, th, v;
	151
	152	isfr = PL2_ISFR;
	153	PL2_ISFR = isfr;
	154	th = PL2_TH();
	155
	156	v = g.pl[1].fixed_state[th];
	157	GPIOB_PDOR = PL2_ADJ(v);
	158	}
	159
	160	static void pl2th_isr_do_to(void)
	161	{
	162	uint32_t isfr, th, v;
	163
	164	isfr = PL2_ISFR;
	165	PL2_ISFR = isfr;
	166	th = PL2_TH();
	167
	168	v = g.stream_to[1][g.pos_to_p[0].o][th];
	169	GPIOB_PDOR = PL2_ADJ(v);
	170
	171	g.pos_to_p[1].o = g.pos_to_p[0].o;
	172	}
	173
	174	static void pl2th_isr_do_to_inc_pl1(void)
	175	{
	176	uint32_t isfr, th, v;
	177
	178	isfr = PL2_ISFR;
	179	PL2_ISFR = isfr;
	180	th = PL2_TH();
	181
	182	v = g.stream_to[1][g.pos_to_p[1].o][th];
	183	GPIOB_PDOR = PL2_ADJ(v);
	184	if (th) {
	185	do_to_step();
	186	g.pos_to_p[1].o = g.pos_to_p[0].o;
	187	}
	188	}
	189
	190	/* vsync handler */
	191	#define VSYNC_ISFR PORTC_ISFR
	192
	193	static void vsync_isr_nop(void)
4af6d4e5	194	{
	195	uint32_t isfr;
	196
19560e5f	197	isfr = VSYNC_ISFR;
19560e5f	198	VSYNC_ISFR = isfr;
4af6d4e5	199	}
	200
	201	// /vsync starts at line 235/259 (ntsc/pal), just as vcounter jumps back
	202	// we care when it comes out (/vsync goes high) after 3 lines at 238/262
19560e5f	203	static void vsync_isr_frameinc(void)
4af6d4e5	204	{
	205	uint32_t isfr;
	206
19560e5f	207	isfr = VSYNC_ISFR;
19560e5f	208	VSYNC_ISFR = isfr;
4af6d4e5	209
19560e5f	210	g.pos_to_p[0].o = (g.pos_to_p[0].o + 1) & STREAM_BUF_MASK;
	211	g.pos_to_p[1].o = g.pos_to_p[0].o;
	212
	213	if (g.pos_to_p[0].o == g.pos_to_p[0].i)
	214	choose_isrs_idle();
4af6d4e5	215	g.frame_cnt++;
	216	}
	217
e8e66d02	218	/* "recording" data */
	219	static noinline void do_from_step(void)
	220	{
	221	uint32_t s;
	222
	223	// should hopefully give atomic fixed_state read..
19560e5f	224	s = g.pl[0].fixed_state32;
	225	g.pl[0].fixed_state32 = g.pl[0].pending_state32;
	226	g.stream_from[g.pos_from.i][0] = s;
	227	g.stream_from[g.pos_from.i][1] = s >> 8;
	228	g.pos_from.i = (g.pos_from.i + 1) & STREAM_BUF_MASK;
e8e66d02	229	}
e8e66d02	230
19560e5f	231	static void pl1th_isr_fixed_do_from(void)
e8e66d02	232	{
	233	uint32_t isfr, th;
	234
19560e5f	235	isfr = PL1_ISFR;
	236	PL1_ISFR = isfr;
	237	th = PL1_TH();
e8e66d02	238
19560e5f	239	GPIOD_PDOR = g.pl[0].fixed_state[th];
e8e66d02	240	if (th)
	241	do_from_step();
	242	g.edge_cnt++;
	243	}
	244
19560e5f	245	static void vsync_isr_frameinc_do_from(void)
e8e66d02	246	{
	247	uint32_t isfr;
	248
19560e5f	249	isfr = VSYNC_ISFR;
19560e5f	250	VSYNC_ISFR = isfr;
e8e66d02	251
	252	do_from_step();
	253	g.frame_cnt++;
	254	}
	255
19560e5f	256	/* * */
6d4349fc	257	static void choose_isrs(void)
6d4349fc	258	{
19560e5f	259	void (*pl1th_handler)(void) = pl1th_isr_fixed;
	260	void (*pl2th_handler)(void) = pl2th_isr_fixed;
	261	void (*vsync_handler)(void) = vsync_isr_nop;
	262
6d4349fc	263	if (g.stream_enable_to) {
19560e5f	264	switch (g.inc_mode) {
	265	case INC_MODE_VSYNC:
	266	pl1th_handler = pl1th_isr_do_to;
	267	pl2th_handler = pl2th_isr_do_to;
	268	vsync_handler = vsync_isr_frameinc;
	269	break;
	270	case INC_MODE_SHARED_PL1:
	271	pl1th_handler = pl1th_isr_do_to_inc;
	272	pl2th_handler = pl2th_isr_do_to;
	273	break;
	274	case INC_MODE_SHARED_PL2:
	275	pl1th_handler = pl1th_isr_do_to;
	276	pl2th_handler = pl2th_isr_do_to_inc_pl1;
	277	break;
6d4349fc	278	}
	279	}
	280	else if (g.stream_enable_from) {
	281	g.use_pending = 1;
19560e5f	282	switch (g.inc_mode) {
	283	case INC_MODE_VSYNC:
	284	vsync_handler = vsync_isr_frameinc_do_from;
	285	break;
	286	case INC_MODE_SHARED_PL1:
	287	pl1th_handler = pl1th_isr_fixed_do_from;
	288	break;
	289	case INC_MODE_SHARED_PL2:
	290	/* TODO */
	291	break;
6d4349fc	292	}
6d4349fc	293	}
19560e5f	294
	295	attachInterruptVector(IRQ_PORTD, pl1th_handler);
	296	attachInterruptVector(IRQ_PORTC, pl2th_handler);
	297	attachInterruptVector(IRQ_PORTA, vsync_handler);
	298	}
	299
	300	static noinline void choose_isrs_idle(void)
	301	{
	302	attachInterruptVector(IRQ_PORTD, pl1th_isr_fixed);
	303	attachInterruptVector(IRQ_PORTC, pl2th_isr_fixed);
	304	attachInterruptVector(IRQ_PORTA, vsync_isr_nop);
6d4349fc	305	}
6d4349fc	306
f20de073	307	static void udelay(uint32_t us)
	308	{
	309	uint32_t start = micros();
	310
	311	while ((micros() - start) < us) {
	312	asm volatile("nop; nop; nop; nop");
	313	yield();
	314	}
	315	}
	316
	317	static void do_start_seq(void)
	318	{
	319	uint32_t edge_cnt_last;
	320	uint32_t edge_cnt;
	321	uint32_t start, t1, t2;
	322	int tout;
	323
	324	start = micros();
	325	edge_cnt = g.edge_cnt;
	326
	327	/* magic value */
19560e5f	328	g.pl[0].fixed_state[0] =
19560e5f	329	g.pl[0].fixed_state[1] = 0x25;
f20de073	330
	331	for (tout = 10000; tout > 0; tout--) {
	332	edge_cnt_last = edge_cnt;
	333	udelay(100);
	334	edge_cnt = g.edge_cnt;
	335
	336	if (edge_cnt != edge_cnt_last)
	337	continue;
19560e5f	338	if (!PL1_TH())
f20de073	339	break;
	340	}
	341
19560e5f	342	g.pl[0].fixed_state[0] = 0x33;
19560e5f	343	g.pl[0].fixed_state[1] = 0x3f;
f20de073	344	GPIOD_PDOR = 0x33;
	345
	346	t1 = micros();
	347	if (tout == 0) {
	348	printf("start_seq timeout1, t=%u\n", t1 - start);
	349	return;
	350	}
	351
	352	for (tout = 100000; tout > 0; tout--) {
	353	udelay(1);
	354
19560e5f	355	if (PL1_TH())
f20de073	356	break;
	357	}
	358
	359	t2 = micros();
	360	if (tout == 0) {
	361	printf("start_seq timeout2, t1=%u, t2=%u\n",
	362	t1 - start, t2 - t1);
	363	return;
	364	}
	365
	366	//printf(" t1=%u, t2=%u\n", t1 - start, t2 - t1);
	367
	368	if (g.stream_started) {
	369	printf("got start_seq when already started\n");
	370	return;
	371	}
	372
e8e66d02	373	if (!g.stream_enable_to && !g.stream_enable_from) {
f20de073	374	printf("got start_seq, without enable from USB\n");
	375	return;
	376	}
	377
19560e5f	378	if (g.stream_enable_to && g.pos_to_p[0].i == g.pos_to_p[0].o) {
e8e66d02	379	printf("got start_seq while stream_to is empty\n");
	380	return;
	381	}
	382
19560e5f	383	if (g.stream_enable_from && g.pos_from.i != g.pos_from.o) {
e8e66d02	384	printf("got start_seq while stream_from is not empty\n");
f20de073	385	return;
	386	}
	387
4af6d4e5	388	__disable_irq();
6d4349fc	389	choose_isrs();
f20de073	390	g.stream_started = 1;
4af6d4e5	391	__enable_irq();
f20de073	392	}
f20de073	393
e8e66d02	394	// callers must disable IRQs
	395	static void clear_state(void)
	396	{
19560e5f	397	int i;
19560e5f	398
e8e66d02	399	g.stream_enable_to = 0;
e8e66d02	400	g.stream_enable_from = 0;
e8e66d02	401	g.stream_started = 0;
e8e66d02	402	g.stream_ended = 0;
19560e5f	403	g.inc_mode = INC_MODE_VSYNC;
72ef318c	404	g.use_pending = 0;
19560e5f	405	for (i = 0; i < ARRAY_SIZE(g.pos_to_p); i++)
	406	g.pos_to_p[i].i = g.pos_to_p[i].o = 0;
	407	g.pos_from.i = g.pos_from.o = 0;
e8e66d02	408	g.frame_cnt = 0;
19560e5f	409	choose_isrs_idle();
e8e66d02	410	}
e8e66d02	411
19560e5f	412	static int get_space_to(int p)
f20de073	413	{
19560e5f	414	return STREAM_BUF_SIZE - ((g.pos_to_p[p].i - g.pos_to_p[p].o)
19560e5f	415	& STREAM_BUF_MASK);
e8e66d02	416	}
	417
	418	static int get_used_from(void)
	419	{
19560e5f	420	return (g.pos_from.i - g.pos_from.o) & STREAM_BUF_MASK;
f20de073	421	}
	422
	423	static void do_usb(void *buf)
	424	{
	425	struct tas_pkt *pkt = buf;
19560e5f	426	uint32_t pos_to_i, i, p;
f20de073	427	int space;
	428
	429	switch (pkt->type) {
	430	case PKT_FIXED_STATE:
72ef318c	431	memcpy(&i, pkt->data, sizeof(i));
72ef318c	432	if (g.use_pending)
19560e5f	433	g.pl[0].pending_state32 = i;
72ef318c	434	else
19560e5f	435	g.pl[0].fixed_state32 = i;
f20de073	436	break;
f20de073	437	case PKT_STREAM_ENABLE:
4af6d4e5	438	__disable_irq();
e8e66d02	439	clear_state();
4af6d4e5	440	/* wait for start from MD */
e8e66d02	441	g.stream_enable_to = pkt->enable.stream_to;
e8e66d02	442	g.stream_enable_from = pkt->enable.stream_from;
19560e5f	443	g.inc_mode = pkt->enable.inc_mode;
6d4349fc	444	if (pkt->enable.no_start_seq) {
	445	GPIOD_PDOR = 0x3f;
	446	choose_isrs();
	447	g.stream_started = 1;
	448	}
e8e66d02	449	__enable_irq();
	450	break;
	451	case PKT_STREAM_ABORT:
	452	__disable_irq();
	453	clear_state();
4af6d4e5	454	__enable_irq();
f20de073	455	break;
f20de073	456	case PKT_STREAM_END:
e8e66d02	457	g.stream_ended = 1;
4af6d4e5	458	printf("end of stream\n");
f20de073	459	break;
19560e5f	460	case PKT_STREAM_DATA_TO_P1:
	461	case PKT_STREAM_DATA_TO_P2:
	462	p = pkt->type == PKT_STREAM_DATA_TO_P1 ? 0 : 1;
	463	pos_to_i = g.pos_to_p[p].i;
	464	space = get_space_to(p);
e8e66d02	465	if (space <= pkt->size / STREAM_EL_SZ) {
f20de073	466	printf("got data pkt while space=%d\n", space);
	467	return;
	468	}
e8e66d02	469	for (i = 0; i < pkt->size / STREAM_EL_SZ; i++) {
19560e5f	470	memcpy(&g.stream_to[p][pos_to_i++],
f20de073	471	pkt->data + i * STREAM_EL_SZ,
f20de073	472	STREAM_EL_SZ);
19560e5f	473	pos_to_i &= STREAM_BUF_MASK;
f20de073	474	}
19560e5f	475	g.pos_to_p[p].i = pos_to_i;
f20de073	476	break;
	477	default:
	478	printf("got unknown pkt type: %04x\n", pkt->type);
	479	break;
	480	}
9c4f55f4	481	}
9c4f55f4	482
19560e5f	483	static void check_get_data(int p)
	484	{
	485	struct tas_pkt pkt;
	486	uint8_t buf[64];
	487	int ret;
	488
	489	if (get_space_to(p) <= sizeof(pkt.data) / STREAM_EL_SZ)
	490	return;
	491
	492	if (g.pos_to_p[p].i == g.pos_to_p[p].o && g.frame_cnt != 0) {
	493	printf("underflow detected\n");
	494	g.stream_enable_to = 0;
	495	return;
	496	}
	497
	498	pkt.type = PKT_STREAM_REQ;
	499	pkt.req.frame = g.frame_cnt;
	500	pkt.req.is_p2 = p;
	501
	502	ret = usb_rawhid_send(&pkt, 1000);
	503	if (ret != sizeof(pkt)) {
	504	printf("send STREAM_REQ/%d: %d\n", p, ret);
	505	return;
	506	}
	507
	508	ret = usb_rawhid_recv(buf, 1000);
	509	if (ret != 64)
	510	printf("usb_rawhid_recv/s: %d\n", ret);
	511	else
	512	do_usb(buf);
	513	}
	514
0c1e003e	515	int main(void)
0c1e003e	516	{
beaf6d89	517	uint32_t led_time = 0;
beaf6d89	518	uint32_t scheck_time = 0;
f20de073	519	uint32_t edge_cnt_last;
	520	uint32_t edge_cnt;
	521	uint8_t buf[64];
19560e5f	522	int i, ret;
0c1e003e	523
	524	delay(1000); // wait for usb..
	525
f20de073	526	/* ?0SA 00DU, ?1CB RLDU */
19560e5f	527	for (i = 0; i < 2; i++) {
	528	g.pl[i].fixed_state[0] = 0x33;
	529	g.pl[i].fixed_state[1] = 0x3f;
	530	}
f20de073	531
0c1e003e	532	printf("starting, rawhid: %d\n", usb_rawhid_available());
0c1e003e	533
19560e5f	534	choose_isrs_idle();
	535
	536	// md pin th tr tl r l d u vsync th tr tl r l d u
	537	// 7 9 6 4 3 2 1 7 9 6 4 3 2 1
	538	// md bit* 6 5 4 3 2 1 0 6 5 4 3 2 1 0
	539	// t bit d6 d5 d4 d3 d2 d1 d0 a12 c0 b17 b16 b3 b2 b1 b0
	540	// t pin 21 20 6 8 7 14 2 3 15 1 0 18 19 17 16
9c4f55f4	541	// * - note: tl/tr mixed in most docs
19560e5f	542
	543	// player1
	544	pinMode(21, INPUT);
	545	attachInterrupt(21, pl1th_isr_fixed, CHANGE);
	546	NVIC_SET_PRIORITY(IRQ_PORTD, 0);
f20de073	547
9c4f55f4	548	pinMode( 2, OUTPUT);
	549	pinMode(14, OUTPUT);
	550	pinMode( 7, OUTPUT);
	551	pinMode( 8, OUTPUT);
	552	pinMode( 6, OUTPUT);
	553	pinMode(20, OUTPUT);
	554
19560e5f	555	// player2
	556	pinMode(15, INPUT);
	557	attachInterrupt(15, pl1th_isr_fixed, CHANGE);
	558	NVIC_SET_PRIORITY(IRQ_PORTC, 0);
	559
	560	pinMode(16, OUTPUT);
	561	pinMode(17, OUTPUT);
	562	pinMode(19, OUTPUT);
	563	pinMode(18, OUTPUT);
	564	pinMode( 0, OUTPUT);
	565	pinMode( 1, OUTPUT);
	566
	567	// vsync line
	568	pinMode(3, INPUT);
	569	attachInterrupt(3, vsync_isr_nop, RISING);
	570	NVIC_SET_PRIORITY(IRQ_PORTA, 16);
	571
9c4f55f4	572	// led
9c4f55f4	573	pinMode(13, OUTPUT);
9c4f55f4	574
19560e5f	575	// lower other priorities
	576	SCB_SHPR1 = SCB_SHPR2 = SCB_SHPR3 = 0x10101010;
	577
9c4f55f4	578	// CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16
4af6d4e5	579	printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR);
9c4f55f4	580	printf("GPIOC PDDR, PDIR: %08x %08x\n", GPIOC_PDIR, GPIOC_PDDR);
	581	printf("GPIOD PDDR, PDIR: %08x %08x\n", GPIOD_PDIR, GPIOD_PDDR);
	582	printf("PORTB_PCR16: %08x\n", PORTB_PCR16);
4af6d4e5	583	printf("PORTC_PCR6: %08x\n", PORTC_PCR6);
8e400118	584	printf("PORTD_PCR0: %08x\n", PORTD_PCR0);
9c4f55f4	585
f20de073	586	asm("mrs %0, BASEPRI" : "=r"(ret));
8e400118	587	printf("BASEPRI: %d, SHPR: %08x %08x %08x\n",
8e400118	588	ret, SCB_SHPR1, SCB_SHPR2, SCB_SHPR3);
0c1e003e	589
f20de073	590	edge_cnt_last = g.edge_cnt;
9c4f55f4	591
1cb2822f	592	while (1) {
f20de073	593	struct tas_pkt pkt;
beaf6d89	594	uint32_t now;
e8e66d02	595
19560e5f	596	if (g.stream_enable_to && !g.stream_ended) {
	597	check_get_data(0);
	598	if (g.inc_mode == INC_MODE_SHARED_PL2)
	599	check_get_data(1);
f20de073	600	}
f20de073	601
e8e66d02	602	while (g.stream_enable_from && !g.stream_ended
	603	&& get_used_from() >= sizeof(pkt.data) / STREAM_EL_SZ)
	604	{
19560e5f	605	uint32_t o;
e8e66d02	606	int i;
e8e66d02	607
19560e5f	608	o = g.pos_from.o;
e8e66d02	609	for (i = 0; i < sizeof(pkt.data); i += STREAM_EL_SZ) {
19560e5f	610	memcpy(pkt.data + i, &g.stream_from[o++],
e8e66d02	611	STREAM_EL_SZ);
19560e5f	612	o &= STREAM_BUF_MASK;
e8e66d02	613	}
19560e5f	614	g.pos_from.o = o;
e8e66d02	615
	616	pkt.type = PKT_STREAM_DATA_FROM;
	617	pkt.size = i;
	618
	619	ret = usb_rawhid_send(&pkt, 1000);
	620	if (ret != sizeof(pkt)) {
	621	printf("send DATA_FROM: %d\n", ret);
	622	break;
	623	}
	624	}
	625
beaf6d89	626	now = millis();
	627
	628	// start condition check
	629	if (now - scheck_time > 1000) {
f20de073	630	edge_cnt = g.edge_cnt;
f20de073	631	//printf("e: %d th: %d\n", edge_cnt - edge_cnt_last,
19560e5f	632	// PL1_TH());
beaf6d89	633	if ((g.stream_enable_to \|\| g.stream_enable_from)
	634	&& !g.stream_started
	635	&& edge_cnt - edge_cnt_last > 10000)
	636	{
f20de073	637	do_start_seq();
	638	edge_cnt = g.edge_cnt;
	639	}
	640	edge_cnt_last = edge_cnt;
beaf6d89	641	scheck_time = now;
f20de073	642	}
f20de073	643
beaf6d89	644	// led?
	645	if (CORE_PIN13_PORTREG & CORE_PIN13_BITMASK) {
	646	if ((int)(now - led_time) > 10)
	647	CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK;
1cb2822f	648	}
beaf6d89	649
	650	// something on rawhid?
	651	if (usb_rawhid_available() > 0)
	652	{
	653	ret = usb_rawhid_recv(buf, 20);
	654	if (ret == 64) {
	655	led_time = millis();
	656	CORE_PIN13_PORTSET = CORE_PIN13_BITMASK;
	657
	658	do_usb(buf);
	659	}
	660	else {
	661	printf("usb_rawhid_recv: %d\n", ret);
	662	}
1cb2822f	663	}
0c1e003e	664	}
1cb2822f	665
1cb2822f	666	return 0;
0c1e003e	667	}