shithub: sms

ref: 23fe5dd09ffe2e3c2d925945bc1adcbf6fc6fc97
dir: /psg.c/

#include <u.h>
#include <libc.h>
#include <thread.h>
#include "../eui.h"
#include "dat.h"
#include "fns.h"

static int fd;
static short sbuf[2 * 2000], *sbufp;

static const short vol_table[16] = {
	8191, 6507, 5168, 4105, 3261, 2590, 2057,
	1642, 1298, 1031, 819, 650, 516, 410, 326, 0
};

static float cyclespersample;
static float cyc;
static u16int freqreg[4];
static u16int countreg[4];
static u8int attn[4];
static int flipflop[4];
static u16int noise;
static int noiseflip;
static int noiseout;
static u8int curr_reg;
static u8int curr_type;

static void
psgreset(void)
{
	memset(freqreg, 0, sizeof freqreg);
	memset(countreg, 0, sizeof countreg);
	memset(flipflop, 0, sizeof flipflop);
	memset(attn, 0x0f, sizeof attn);
	noise = 0x8000;
	noiseflip = 0;
	noiseout = 0;
	curr_reg = 0;
	curr_type = 0;
}

void
psgwrite(const u8int data)
{
	int first;

	first = data & 128;
	if(first){
		curr_reg = (data >> 5) & 3;
		curr_type = (data >> 4) & 1;
	}

	if(curr_type){
		attn[curr_reg] = data & 0x0F;
	}else if(curr_reg == 3){
		freqreg[3] = data & 7;
		noise = 0x8000;
		noiseflip = 0;
		noiseout = 0;
	}else if(first)
		freqreg[curr_reg] = (freqreg[curr_reg] & 0x3F0) | (data & 0x0F);
	else
		freqreg[curr_reg] = (freqreg[curr_reg] & 0x0F) | ((data & 0x3F) << 4);
}

static inline u16int
parity(u16int v)
{
	v ^= v >> 8;
	v ^= v >> 4;
	v ^= v >> 2;
	v ^= v >> 1;
	v &= 1;
	return v;
}

static inline s16int
vol(u8int chn)
{
	int on;

	on = chn == 3 ? noiseout : flipflop[chn];
	return (on ? 1 : -1) * vol_table[attn[chn]];
}

static inline void
toneclock(u8int chn)
{
	if(countreg[chn] == 0 || --countreg[chn] == 0){
		countreg[chn] = freqreg[chn];
		flipflop[chn] ^= 1;
	}
}

static inline void
noiseclock(void)
{
	u8int nf, fb;

	nf = freqreg[3] & 3;
	fb = (freqreg[3] >> 2) & 1;
	if(countreg[3] == 0 || --countreg[3] == 0){
		countreg[3] = nf == 3 ? freqreg[2] : (0x10 << nf);
		if((noiseflip ^= 1) != 0){
			noiseout = noise & 1;
			noise = (noise >> 1) | ((fb ? parity(noise & 0x9) : noise & 1) << 15);
		}
	}
}

u16int
psgstep(void)
{
	while(cyc > 0){
		toneclock(0);
		toneclock(1);
		toneclock(2);
		noiseclock();
		cyc--;
	}

	cyc += cyclespersample;

	return vol(0) + vol(1) + vol(2) + vol(3);
}

void
audiosample(void)
{
	s16int v;

	if(sbufp == nil)
		return;

	v = (s16int)psgstep();
	if(sbufp < sbuf + nelem(sbuf) - 1){
		*sbufp++ = v;
		*sbufp++ = v;
	}
}

int
audioout(void)
{
	int rc;

	if(sbufp == nil)
		return -1;
	if(sbufp == sbuf)
		return 0;
	rc = warp10 ? (sbufp - sbuf) * 2 : write(fd, sbuf, (sbufp - sbuf) * 2);
	if(rc > 0)
		sbufp -= (rc+1)/2;
	if(sbufp < sbuf)
		sbufp = sbuf;
	return 0;
}

void
psginit(const u16int rate, const int clockspd)
{
	cyclespersample = (double)clockspd / (double)PSGDIV/ (double)rate;
	cyc = cyclespersample;
	psgreset();

	fd = open("/dev/audio", OWRITE);
	if(fd < 0)
		return;

	sbufp = sbuf;
}