gamepad.c 2.49 KB
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>

#include <em.h>

#ifdef DEBUG

#define ERROR(s)    fprintf(stderr, s)
#define ERROR1(s, a1) fprintf(stderr, s, a1)
#define ERROR2(s, a1, a2) fprintf(stderr, s, a1, a2)

#else

#define ERROR(s)
#define ERROR1(s, a1)
#define ERROR2(s, a1)

#endif

static int desc[5];
static unsigned char tbl[] = {
    0x0, 0x8,  0x4,  0xc,  0x2,  0xa,  0x6, 0xe, 
    0x1, 0x9,  0x5,  0x9,  0x3,  0xb,  0x7, 0xf
};

static void 
TransmitSerial(int serial, int code)
{
    unsigned char cc;
    
    cc = (unsigned char)code;
    write(serial, &cc,  1);
}

static int
ReceiveSerial(int serial)
{
    unsigned char cc;
    
    if (read(serial, &cc, 1) < 1) {
	ERROR1("ReceiveSerial read error (%d)\n", serial);
	return -1;
    }

    return (int)cc;
}

static int
ReceiveSerialFlip(int serial)
{
    unsigned char cc;
    
    if (read(serial, &cc, 1) < 1) {
	ERROR1("ReceiveSerialFlip read error (%d)\n", serial);
	return -1;
    }

    /* Flip them bits */
    cc = (tbl[cc & 0xf] << 4) | tbl[(cc >> 4) & 0xf];
    
    return (int)cc;
}

int
emInitController(int port)
{
    struct termios setting;
    int fd;
    char *ttyline = "/dev/ttyd0";
    char *pp;
    
    if (port < 1 || port > 4) {
	ERROR1("emInitController bad port no. %d\n", port);
	return -1;
    }

    /* Hack to avoid sprintf */
    pp = ttyline + 9;
    *pp += port;
    if ((fd = open(ttyline, O_RDWR)) < 0) {
	ERROR1("Could not open %s\n", ttyline);
	return -1;
    }

    ioctl(fd,  TCGETS,  &setting);
    setting.c_iflag = IGNBRK | IGNPAR;
    setting.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL;
    setting.c_lflag = 0;
    setting.c_cc[VMIN] = 0;
    setting.c_cc[VTIME] = 1;
    
    ioctl(fd, TCSETSF,  &setting);
    ioctl(fd,  TCFLSH, 2);
    desc[port] = fd;
    
    return 0;
}

void
emScanController(int port)
{
    ioctl(desc[port], TCFLSH, 0);   /* flush the queue */
    TransmitSerial(desc[port], 0xff);	/* Start scanning */
}

/* Scan the game controller */
int
emGetController(int port, GamePad *pad)
{
    int code1, code2, code3, code4;
    
    emScanController(port);
    if ((code1 = ReceiveSerial(desc[port])) < 0)
	return -1;
    if ((code2 = ReceiveSerial(desc[port])) < 0)
	return -1;
    if ((code3 = ReceiveSerialFlip(desc[port])) < 0)
	return -1;
    if ((code4 = ReceiveSerialFlip(desc[port])) < 0)
	return -1;
    pad->button = (unsigned short)(code2 << 8 | code1);
    pad->stick_x = (unsigned char)code3;
    pad->stick_y = (unsigned char)code4;
    
    return 0;
}