/*
 * Subroutine and test program to strobe NES controller.
 * Jan 5, 1990
 */

#include <stdio.h>
#include <sys/types.h>
#include <sys/param.h>
#include <sys/at_ansi.h>
#include <sys/inline.h>
#include <sys/kd.h>

unsigned char strobe( /* unsigned char port */ );

main() {
	unsigned char old, new;
	extern errno;

	old = new = 0;
	setbuf(stdout, (char *) 0);
	errno = 0;
	ioctl(0, KDADDIO, 0x378);
	if (errno) {
		perror("ADDIO");
	}
	ioctl(0, KDADDIO, 0x379);
	if (errno) {
		perror("ADDIO");
	}
	ioctl(0, KDADDIO, 0x37a);
	if (errno) {
		perror("ADDIO");
	}
	ioctl(0, KDENABIO);
	if (errno) {
		perror("ENABIO");
	}
	strobe_init(0x378);
	while(1) {
		new = strobe(0x378);
		if (new != old) 
			printf("%02x  ", new);
		old = new;
	}
}

#define	NES_CLOCK	0x1		/* to port */
#define	NES_RESET	0x2		/* to port */
#define	NES_DATA	0x10		/* from port + 1 */

unsigned char nes_shift[16] = { 4, 5, 6, 7, 8, 9, 10, 11 };

int c1 = 20;
int c2 = 20;
int c3 = 20;
int c4 = 20;
int c5 = 20;

strobe_init(port)
{
	outb(port, 0);
	outb(port+1, 0);
}

#define	hold(clk)		for(slow = clk; slow; slow--);

unsigned char
strobe(port)
int port;
{
	int i, slow;
	unsigned char data;
	unsigned short bits = 0;
	unsigned char xx[8];

	/* stop interrupts */
	outb(port, NES_RESET | NES_CLOCK);
	hold(c1);
	outb(port, NES_CLOCK);
	hold(c2);
	for(i = 0; i < 8; i++) {
		data = inb(port + 1);
		bits |= ((data & NES_DATA) << nes_shift[i]);
		hold(c5);
		outb(port, 0);
		hold(c3);
		outb(port, NES_CLOCK);
		hold(c4);
	}
	bits >>= 8; 
	return (unsigned char) ~bits;
}
