[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_pl1(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_pl1();
}

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 noinline void do_to_step_pl2(void)
{
	g.pos_to_p[1].o = (g.pos_to_p[1].o + 1) & STREAM_BUF_MASK;
	if (g.pos_to_p[1].o == g.pos_to_p[1].i)
		// done
		choose_isrs_idle();
}

static void pl2th_isr_do_to_inc(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_pl2();
}

static void pl2th_isr_do_to_p1d(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_pl1();
		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_p1d;
			vsync_handler = vsync_isr_frameinc;
			break;
		case INC_MODE_SHARED_PL1:
			pl1th_handler = pl1th_isr_do_to_inc;
			pl2th_handler = pl2th_isr_do_to_p1d;
			break;
		case INC_MODE_SHARED_PL2:
			pl1th_handler = pl1th_isr_do_to;
			pl2th_handler = pl2th_isr_do_to_inc_pl1;
			break;
		case INC_MODE_SEPARATE:
			pl1th_handler = pl1th_isr_do_to_inc;
			pl2th_handler = pl2th_isr_do_to_inc;
			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:
		case INC_MODE_SEPARATE:
			/* 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;
	memset(g.stream_to[1], 0x3f, sizeof(g.stream_to[1]));
	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
			    || g.inc_mode == INC_MODE_SEPARATE)
				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
be48e888	108	static noinline void do_to_step_pl1(void)
8e400118	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)
be48e888	128	do_to_step_pl1();
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
be48e888	160	static noinline void do_to_step_pl2(void)
	161	{
	162	g.pos_to_p[1].o = (g.pos_to_p[1].o + 1) & STREAM_BUF_MASK;
	163	if (g.pos_to_p[1].o == g.pos_to_p[1].i)
	164	// done
	165	choose_isrs_idle();
	166	}
	167
	168	static void pl2th_isr_do_to_inc(void)
	169	{
	170	uint32_t isfr, th, v;
	171
	172	isfr = PL2_ISFR;
	173	PL2_ISFR = isfr;
	174	th = PL2_TH();
	175
	176	v = g.stream_to[1][g.pos_to_p[1].o][th];
	177	GPIOB_PDOR = PL2_ADJ(v);
	178	if (th)
	179	do_to_step_pl2();
	180	}
	181
	182	static void pl2th_isr_do_to_p1d(void)
19560e5f	183	{
	184	uint32_t isfr, th, v;
	185
	186	isfr = PL2_ISFR;
	187	PL2_ISFR = isfr;
	188	th = PL2_TH();
	189
	190	v = g.stream_to[1][g.pos_to_p[0].o][th];
	191	GPIOB_PDOR = PL2_ADJ(v);
	192
	193	g.pos_to_p[1].o = g.pos_to_p[0].o;
	194	}
	195
	196	static void pl2th_isr_do_to_inc_pl1(void)
	197	{
	198	uint32_t isfr, th, v;
	199
	200	isfr = PL2_ISFR;
	201	PL2_ISFR = isfr;
	202	th = PL2_TH();
	203
	204	v = g.stream_to[1][g.pos_to_p[1].o][th];
	205	GPIOB_PDOR = PL2_ADJ(v);
	206	if (th) {
be48e888	207	do_to_step_pl1();
19560e5f	208	g.pos_to_p[1].o = g.pos_to_p[0].o;
	209	}
	210	}
	211
	212	/* vsync handler */
	213	#define VSYNC_ISFR PORTC_ISFR
	214
	215	static void vsync_isr_nop(void)
4af6d4e5	216	{
	217	uint32_t isfr;
	218
19560e5f	219	isfr = VSYNC_ISFR;
19560e5f	220	VSYNC_ISFR = isfr;
4af6d4e5	221	}
	222
	223	// /vsync starts at line 235/259 (ntsc/pal), just as vcounter jumps back
	224	// we care when it comes out (/vsync goes high) after 3 lines at 238/262
19560e5f	225	static void vsync_isr_frameinc(void)
4af6d4e5	226	{
	227	uint32_t isfr;
	228
19560e5f	229	isfr = VSYNC_ISFR;
19560e5f	230	VSYNC_ISFR = isfr;
4af6d4e5	231
19560e5f	232	g.pos_to_p[0].o = (g.pos_to_p[0].o + 1) & STREAM_BUF_MASK;
	233	g.pos_to_p[1].o = g.pos_to_p[0].o;
	234
	235	if (g.pos_to_p[0].o == g.pos_to_p[0].i)
	236	choose_isrs_idle();
4af6d4e5	237	g.frame_cnt++;
	238	}
	239
e8e66d02	240	/* "recording" data */
	241	static noinline void do_from_step(void)
	242	{
	243	uint32_t s;
	244
	245	// should hopefully give atomic fixed_state read..
19560e5f	246	s = g.pl[0].fixed_state32;
	247	g.pl[0].fixed_state32 = g.pl[0].pending_state32;
	248	g.stream_from[g.pos_from.i][0] = s;
	249	g.stream_from[g.pos_from.i][1] = s >> 8;
	250	g.pos_from.i = (g.pos_from.i + 1) & STREAM_BUF_MASK;
e8e66d02	251	}
e8e66d02	252
19560e5f	253	static void pl1th_isr_fixed_do_from(void)
e8e66d02	254	{
	255	uint32_t isfr, th;
	256
19560e5f	257	isfr = PL1_ISFR;
	258	PL1_ISFR = isfr;
	259	th = PL1_TH();
e8e66d02	260
19560e5f	261	GPIOD_PDOR = g.pl[0].fixed_state[th];
e8e66d02	262	if (th)
	263	do_from_step();
	264	g.edge_cnt++;
	265	}
	266
19560e5f	267	static void vsync_isr_frameinc_do_from(void)
e8e66d02	268	{
	269	uint32_t isfr;
	270
19560e5f	271	isfr = VSYNC_ISFR;
19560e5f	272	VSYNC_ISFR = isfr;
e8e66d02	273
	274	do_from_step();
	275	g.frame_cnt++;
	276	}
	277
19560e5f	278	/* * */
6d4349fc	279	static void choose_isrs(void)
6d4349fc	280	{
19560e5f	281	void (*pl1th_handler)(void) = pl1th_isr_fixed;
	282	void (*pl2th_handler)(void) = pl2th_isr_fixed;
	283	void (*vsync_handler)(void) = vsync_isr_nop;
	284
6d4349fc	285	if (g.stream_enable_to) {
19560e5f	286	switch (g.inc_mode) {
	287	case INC_MODE_VSYNC:
	288	pl1th_handler = pl1th_isr_do_to;
be48e888	289	pl2th_handler = pl2th_isr_do_to_p1d;
19560e5f	290	vsync_handler = vsync_isr_frameinc;
	291	break;
	292	case INC_MODE_SHARED_PL1:
	293	pl1th_handler = pl1th_isr_do_to_inc;
be48e888	294	pl2th_handler = pl2th_isr_do_to_p1d;
19560e5f	295	break;
	296	case INC_MODE_SHARED_PL2:
	297	pl1th_handler = pl1th_isr_do_to;
	298	pl2th_handler = pl2th_isr_do_to_inc_pl1;
	299	break;
be48e888	300	case INC_MODE_SEPARATE:
	301	pl1th_handler = pl1th_isr_do_to_inc;
	302	pl2th_handler = pl2th_isr_do_to_inc;
	303	break;
6d4349fc	304	}
	305	}
	306	else if (g.stream_enable_from) {
	307	g.use_pending = 1;
19560e5f	308	switch (g.inc_mode) {
	309	case INC_MODE_VSYNC:
	310	vsync_handler = vsync_isr_frameinc_do_from;
	311	break;
	312	case INC_MODE_SHARED_PL1:
	313	pl1th_handler = pl1th_isr_fixed_do_from;
	314	break;
	315	case INC_MODE_SHARED_PL2:
be48e888	316	case INC_MODE_SEPARATE:
19560e5f	317	/* TODO */
19560e5f	318	break;
6d4349fc	319	}
6d4349fc	320	}
19560e5f	321
	322	attachInterruptVector(IRQ_PORTD, pl1th_handler);
	323	attachInterruptVector(IRQ_PORTC, pl2th_handler);
	324	attachInterruptVector(IRQ_PORTA, vsync_handler);
	325	}
	326
	327	static noinline void choose_isrs_idle(void)
	328	{
	329	attachInterruptVector(IRQ_PORTD, pl1th_isr_fixed);
	330	attachInterruptVector(IRQ_PORTC, pl2th_isr_fixed);
	331	attachInterruptVector(IRQ_PORTA, vsync_isr_nop);
6d4349fc	332	}
6d4349fc	333
f20de073	334	static void udelay(uint32_t us)
	335	{
	336	uint32_t start = micros();
	337
	338	while ((micros() - start) < us) {
	339	asm volatile("nop; nop; nop; nop");
	340	yield();
	341	}
	342	}
	343
	344	static void do_start_seq(void)
	345	{
	346	uint32_t edge_cnt_last;
	347	uint32_t edge_cnt;
	348	uint32_t start, t1, t2;
	349	int tout;
	350
	351	start = micros();
	352	edge_cnt = g.edge_cnt;
	353
	354	/* magic value */
19560e5f	355	g.pl[0].fixed_state[0] =
19560e5f	356	g.pl[0].fixed_state[1] = 0x25;
f20de073	357
	358	for (tout = 10000; tout > 0; tout--) {
	359	edge_cnt_last = edge_cnt;
	360	udelay(100);
	361	edge_cnt = g.edge_cnt;
	362
	363	if (edge_cnt != edge_cnt_last)
	364	continue;
19560e5f	365	if (!PL1_TH())
f20de073	366	break;
	367	}
	368
19560e5f	369	g.pl[0].fixed_state[0] = 0x33;
19560e5f	370	g.pl[0].fixed_state[1] = 0x3f;
f20de073	371	GPIOD_PDOR = 0x33;
	372
	373	t1 = micros();
	374	if (tout == 0) {
	375	printf("start_seq timeout1, t=%u\n", t1 - start);
	376	return;
	377	}
	378
	379	for (tout = 100000; tout > 0; tout--) {
	380	udelay(1);
	381
19560e5f	382	if (PL1_TH())
f20de073	383	break;
	384	}
	385
	386	t2 = micros();
	387	if (tout == 0) {
	388	printf("start_seq timeout2, t1=%u, t2=%u\n",
	389	t1 - start, t2 - t1);
	390	return;
	391	}
	392
	393	//printf(" t1=%u, t2=%u\n", t1 - start, t2 - t1);
	394
	395	if (g.stream_started) {
	396	printf("got start_seq when already started\n");
	397	return;
	398	}
	399
e8e66d02	400	if (!g.stream_enable_to && !g.stream_enable_from) {
f20de073	401	printf("got start_seq, without enable from USB\n");
	402	return;
	403	}
	404
19560e5f	405	if (g.stream_enable_to && g.pos_to_p[0].i == g.pos_to_p[0].o) {
e8e66d02	406	printf("got start_seq while stream_to is empty\n");
	407	return;
	408	}
	409
19560e5f	410	if (g.stream_enable_from && g.pos_from.i != g.pos_from.o) {
e8e66d02	411	printf("got start_seq while stream_from is not empty\n");
f20de073	412	return;
	413	}
	414
4af6d4e5	415	__disable_irq();
6d4349fc	416	choose_isrs();
f20de073	417	g.stream_started = 1;
4af6d4e5	418	__enable_irq();
f20de073	419	}
f20de073	420
e8e66d02	421	// callers must disable IRQs
	422	static void clear_state(void)
	423	{
19560e5f	424	int i;
19560e5f	425
e8e66d02	426	g.stream_enable_to = 0;
e8e66d02	427	g.stream_enable_from = 0;
e8e66d02	428	g.stream_started = 0;
e8e66d02	429	g.stream_ended = 0;
19560e5f	430	g.inc_mode = INC_MODE_VSYNC;
72ef318c	431	g.use_pending = 0;
19560e5f	432	for (i = 0; i < ARRAY_SIZE(g.pos_to_p); i++)
	433	g.pos_to_p[i].i = g.pos_to_p[i].o = 0;
	434	g.pos_from.i = g.pos_from.o = 0;
e8e66d02	435	g.frame_cnt = 0;
be48e888	436	memset(g.stream_to[1], 0x3f, sizeof(g.stream_to[1]));
19560e5f	437	choose_isrs_idle();
e8e66d02	438	}
e8e66d02	439
19560e5f	440	static int get_space_to(int p)
f20de073	441	{
19560e5f	442	return STREAM_BUF_SIZE - ((g.pos_to_p[p].i - g.pos_to_p[p].o)
19560e5f	443	& STREAM_BUF_MASK);
e8e66d02	444	}
	445
	446	static int get_used_from(void)
	447	{
19560e5f	448	return (g.pos_from.i - g.pos_from.o) & STREAM_BUF_MASK;
f20de073	449	}
	450
	451	static void do_usb(void *buf)
	452	{
	453	struct tas_pkt *pkt = buf;
19560e5f	454	uint32_t pos_to_i, i, p;
f20de073	455	int space;
	456
	457	switch (pkt->type) {
	458	case PKT_FIXED_STATE:
72ef318c	459	memcpy(&i, pkt->data, sizeof(i));
72ef318c	460	if (g.use_pending)
19560e5f	461	g.pl[0].pending_state32 = i;
72ef318c	462	else
19560e5f	463	g.pl[0].fixed_state32 = i;
f20de073	464	break;
f20de073	465	case PKT_STREAM_ENABLE:
4af6d4e5	466	__disable_irq();
e8e66d02	467	clear_state();
4af6d4e5	468	/* wait for start from MD */
e8e66d02	469	g.stream_enable_to = pkt->enable.stream_to;
e8e66d02	470	g.stream_enable_from = pkt->enable.stream_from;
19560e5f	471	g.inc_mode = pkt->enable.inc_mode;
6d4349fc	472	if (pkt->enable.no_start_seq) {
	473	GPIOD_PDOR = 0x3f;
	474	choose_isrs();
	475	g.stream_started = 1;
	476	}
e8e66d02	477	__enable_irq();
	478	break;
	479	case PKT_STREAM_ABORT:
	480	__disable_irq();
	481	clear_state();
4af6d4e5	482	__enable_irq();
f20de073	483	break;
f20de073	484	case PKT_STREAM_END:
e8e66d02	485	g.stream_ended = 1;
4af6d4e5	486	printf("end of stream\n");
f20de073	487	break;
19560e5f	488	case PKT_STREAM_DATA_TO_P1:
	489	case PKT_STREAM_DATA_TO_P2:
	490	p = pkt->type == PKT_STREAM_DATA_TO_P1 ? 0 : 1;
	491	pos_to_i = g.pos_to_p[p].i;
	492	space = get_space_to(p);
e8e66d02	493	if (space <= pkt->size / STREAM_EL_SZ) {
f20de073	494	printf("got data pkt while space=%d\n", space);
	495	return;
	496	}
e8e66d02	497	for (i = 0; i < pkt->size / STREAM_EL_SZ; i++) {
19560e5f	498	memcpy(&g.stream_to[p][pos_to_i++],
f20de073	499	pkt->data + i * STREAM_EL_SZ,
f20de073	500	STREAM_EL_SZ);
19560e5f	501	pos_to_i &= STREAM_BUF_MASK;
f20de073	502	}
19560e5f	503	g.pos_to_p[p].i = pos_to_i;
f20de073	504	break;
	505	default:
	506	printf("got unknown pkt type: %04x\n", pkt->type);
	507	break;
	508	}
9c4f55f4	509	}
9c4f55f4	510
19560e5f	511	static void check_get_data(int p)
	512	{
	513	struct tas_pkt pkt;
	514	uint8_t buf[64];
	515	int ret;
	516
	517	if (get_space_to(p) <= sizeof(pkt.data) / STREAM_EL_SZ)
	518	return;
	519
	520	if (g.pos_to_p[p].i == g.pos_to_p[p].o && g.frame_cnt != 0) {
	521	printf("underflow detected\n");
	522	g.stream_enable_to = 0;
	523	return;
	524	}
	525
	526	pkt.type = PKT_STREAM_REQ;
	527	pkt.req.frame = g.frame_cnt;
	528	pkt.req.is_p2 = p;
	529
	530	ret = usb_rawhid_send(&pkt, 1000);
	531	if (ret != sizeof(pkt)) {
	532	printf("send STREAM_REQ/%d: %d\n", p, ret);
	533	return;
	534	}
	535
	536	ret = usb_rawhid_recv(buf, 1000);
	537	if (ret != 64)
	538	printf("usb_rawhid_recv/s: %d\n", ret);
	539	else
	540	do_usb(buf);
	541	}
	542
0c1e003e	543	int main(void)
0c1e003e	544	{
beaf6d89	545	uint32_t led_time = 0;
beaf6d89	546	uint32_t scheck_time = 0;
f20de073	547	uint32_t edge_cnt_last;
	548	uint32_t edge_cnt;
	549	uint8_t buf[64];
19560e5f	550	int i, ret;
0c1e003e	551
	552	delay(1000); // wait for usb..
	553
f20de073	554	/* ?0SA 00DU, ?1CB RLDU */
19560e5f	555	for (i = 0; i < 2; i++) {
	556	g.pl[i].fixed_state[0] = 0x33;
	557	g.pl[i].fixed_state[1] = 0x3f;
	558	}
f20de073	559
0c1e003e	560	printf("starting, rawhid: %d\n", usb_rawhid_available());
0c1e003e	561
19560e5f	562	choose_isrs_idle();
	563
	564	// md pin th tr tl r l d u vsync th tr tl r l d u
	565	// 7 9 6 4 3 2 1 7 9 6 4 3 2 1
	566	// md bit* 6 5 4 3 2 1 0 6 5 4 3 2 1 0
	567	// t bit d6 d5 d4 d3 d2 d1 d0 a12 c0 b17 b16 b3 b2 b1 b0
	568	// t pin 21 20 6 8 7 14 2 3 15 1 0 18 19 17 16
9c4f55f4	569	// * - note: tl/tr mixed in most docs
19560e5f	570
	571	// player1
	572	pinMode(21, INPUT);
	573	attachInterrupt(21, pl1th_isr_fixed, CHANGE);
	574	NVIC_SET_PRIORITY(IRQ_PORTD, 0);
f20de073	575
9c4f55f4	576	pinMode( 2, OUTPUT);
	577	pinMode(14, OUTPUT);
	578	pinMode( 7, OUTPUT);
	579	pinMode( 8, OUTPUT);
	580	pinMode( 6, OUTPUT);
	581	pinMode(20, OUTPUT);
	582
19560e5f	583	// player2
	584	pinMode(15, INPUT);
	585	attachInterrupt(15, pl1th_isr_fixed, CHANGE);
	586	NVIC_SET_PRIORITY(IRQ_PORTC, 0);
	587
	588	pinMode(16, OUTPUT);
	589	pinMode(17, OUTPUT);
	590	pinMode(19, OUTPUT);
	591	pinMode(18, OUTPUT);
	592	pinMode( 0, OUTPUT);
	593	pinMode( 1, OUTPUT);
	594
	595	// vsync line
	596	pinMode(3, INPUT);
	597	attachInterrupt(3, vsync_isr_nop, RISING);
	598	NVIC_SET_PRIORITY(IRQ_PORTA, 16);
	599
9c4f55f4	600	// led
9c4f55f4	601	pinMode(13, OUTPUT);
9c4f55f4	602
19560e5f	603	// lower other priorities
	604	SCB_SHPR1 = SCB_SHPR2 = SCB_SHPR3 = 0x10101010;
	605
9c4f55f4	606	// CORE_PIN0_PORTSET CORE_PIN0_BITMASK PORTB_PCR16
4af6d4e5	607	printf("GPIOB PDDR, PDIR: %08x %08x\n", GPIOB_PDIR, GPIOB_PDDR);
9c4f55f4	608	printf("GPIOC PDDR, PDIR: %08x %08x\n", GPIOC_PDIR, GPIOC_PDDR);
	609	printf("GPIOD PDDR, PDIR: %08x %08x\n", GPIOD_PDIR, GPIOD_PDDR);
	610	printf("PORTB_PCR16: %08x\n", PORTB_PCR16);
4af6d4e5	611	printf("PORTC_PCR6: %08x\n", PORTC_PCR6);
8e400118	612	printf("PORTD_PCR0: %08x\n", PORTD_PCR0);
9c4f55f4	613
f20de073	614	asm("mrs %0, BASEPRI" : "=r"(ret));
8e400118	615	printf("BASEPRI: %d, SHPR: %08x %08x %08x\n",
8e400118	616	ret, SCB_SHPR1, SCB_SHPR2, SCB_SHPR3);
0c1e003e	617
f20de073	618	edge_cnt_last = g.edge_cnt;
9c4f55f4	619
1cb2822f	620	while (1) {
f20de073	621	struct tas_pkt pkt;
beaf6d89	622	uint32_t now;
e8e66d02	623
19560e5f	624	if (g.stream_enable_to && !g.stream_ended) {
19560e5f	625	check_get_data(0);
be48e888	626	if (g.inc_mode == INC_MODE_SHARED_PL2
be48e888	627	\|\| g.inc_mode == INC_MODE_SEPARATE)
19560e5f	628	check_get_data(1);
f20de073	629	}
f20de073	630
e8e66d02	631	while (g.stream_enable_from && !g.stream_ended
	632	&& get_used_from() >= sizeof(pkt.data) / STREAM_EL_SZ)
	633	{
19560e5f	634	uint32_t o;
e8e66d02	635	int i;
e8e66d02	636
19560e5f	637	o = g.pos_from.o;
e8e66d02	638	for (i = 0; i < sizeof(pkt.data); i += STREAM_EL_SZ) {
19560e5f	639	memcpy(pkt.data + i, &g.stream_from[o++],
e8e66d02	640	STREAM_EL_SZ);
19560e5f	641	o &= STREAM_BUF_MASK;
e8e66d02	642	}
19560e5f	643	g.pos_from.o = o;
e8e66d02	644
	645	pkt.type = PKT_STREAM_DATA_FROM;
	646	pkt.size = i;
	647
	648	ret = usb_rawhid_send(&pkt, 1000);
	649	if (ret != sizeof(pkt)) {
	650	printf("send DATA_FROM: %d\n", ret);
	651	break;
	652	}
	653	}
	654
beaf6d89	655	now = millis();
	656
	657	// start condition check
	658	if (now - scheck_time > 1000) {
f20de073	659	edge_cnt = g.edge_cnt;
f20de073	660	//printf("e: %d th: %d\n", edge_cnt - edge_cnt_last,
19560e5f	661	// PL1_TH());
beaf6d89	662	if ((g.stream_enable_to \|\| g.stream_enable_from)
	663	&& !g.stream_started
	664	&& edge_cnt - edge_cnt_last > 10000)
	665	{
f20de073	666	do_start_seq();
	667	edge_cnt = g.edge_cnt;
	668	}
	669	edge_cnt_last = edge_cnt;
beaf6d89	670	scheck_time = now;
f20de073	671	}
f20de073	672
beaf6d89	673	// led?
	674	if (CORE_PIN13_PORTREG & CORE_PIN13_BITMASK) {
	675	if ((int)(now - led_time) > 10)
	676	CORE_PIN13_PORTCLEAR = CORE_PIN13_BITMASK;
1cb2822f	677	}
beaf6d89	678
	679	// something on rawhid?
	680	if (usb_rawhid_available() > 0)
	681	{
	682	ret = usb_rawhid_recv(buf, 20);
	683	if (ret == 64) {
	684	led_time = millis();
	685	CORE_PIN13_PORTSET = CORE_PIN13_BITMASK;
	686
	687	do_usb(buf);
	688	}
	689	else {
	690	printf("usb_rawhid_recv: %d\n", ret);
	691	}
1cb2822f	692	}
0c1e003e	693	}
1cb2822f	694
1cb2822f	695	return 0;
0c1e003e	696	}