pavone@467: /*
pavone@467:  Copyright 2013 Michael Pavone
pavone@483:  This file is part of BlastEm.
pavone@467:  BlastEm is free software distributed under the terms of the GNU General Public License version 3 or greater. See COPYING for full license text.
pavone@467: */
pavone@354: #include "psg.h"
pavone@354: #include "render.h"
pavone@505: #include "blastem.h"
pavone@354: #include <string.h>
pavone@354: #include <stdlib.h>
pavone@838: #include <stdio.h>
pavone@964: #include <math.h>
pavone@964: #define LOWPASS_CUTOFF 3390
pavone@838: 
pavone@380: void psg_init(psg_context * context, uint32_t sample_rate, uint32_t master_clock, uint32_t clock_div, uint32_t samples_frame)
pavone@354: {
pavone@354: 	memset(context, 0, sizeof(*context));
pavone@354: 	context->audio_buffer = malloc(sizeof(*context->audio_buffer) * samples_frame);
pavone@354: 	context->back_buffer = malloc(sizeof(*context->audio_buffer) * samples_frame);
pavone@380: 	context->clock_inc = clock_div;
pavone@483: 	context->sample_rate = sample_rate;
pavone@354: 	context->samples_frame = samples_frame;
pavone@964: 	double rc = (1.0 / (double)LOWPASS_CUTOFF) / (2.0 * M_PI);
pavone@964: 	double dt = 1.0 / ((double)master_clock / (double)clock_div);
pavone@964: 	double alpha = dt / (dt + rc);
pavone@964: 	context->lowpass_alpha = (int32_t)(((double)0x10000) * alpha);
pavone@483: 	psg_adjust_master_clock(context, master_clock);
pavone@354: 	for (int i = 0; i < 4; i++) {
pavone@354: 		context->volume[i] = 0xF;
pavone@354: 	}
pavone@354: }
pavone@354: 
pavone@884: void psg_free(psg_context *context)
pavone@884: {
pavone@884: 	free(context->audio_buffer);
pavone@884: 	//TODO: Figure out how to make this 100% safe
pavone@884: 	//audio thread could still be using this
pavone@884: 	free(context->back_buffer);
pavone@884: 	free(context);
pavone@884: }
pavone@884: 
pavone@963: #define BUFFER_INC_RES 0x40000000UL
pavone@483: 
pavone@483: void psg_adjust_master_clock(psg_context * context, uint32_t master_clock)
pavone@483: {
pavone@483: 	uint64_t old_inc = context->buffer_inc;
pavone@483: 	context->buffer_inc = ((BUFFER_INC_RES * (uint64_t)context->sample_rate) / (uint64_t)master_clock) * (uint64_t)context->clock_inc;
pavone@483: }
pavone@483: 
pavone@354: void psg_write(psg_context * context, uint8_t value)
pavone@354: {
pavone@354: 	if (value & 0x80) {
pavone@354: 		context->latch = value & 0x70;
pavone@354: 		uint8_t channel = value >> 5 & 0x3;
pavone@354: 		if (value & 0x10) {
pavone@354: 			context->volume[channel] = value & 0xF;
pavone@354: 		} else {
pavone@354: 			if (channel == 3) {
pavone@354: 				switch(value & 0x3)
pavone@354: 				{
pavone@354: 				case 0:
pavone@354: 				case 1:
pavone@354: 				case 2:
pavone@354: 					context->counter_load[3] = 0x10 << (value & 0x3);
pavone@354: 					context->noise_use_tone = 0;
pavone@354: 					break;
pavone@354: 				default:
pavone@354: 					context->counter_load[3] = context->counter_load[2];
pavone@354: 					context->noise_use_tone = 1;
pavone@354: 				}
pavone@354: 				context->noise_type = value & 0x4;
pavone@354: 				context->lsfr = 0x8000;
pavone@354: 			} else {
pavone@354: 				context->counter_load[channel] = (context->counter_load[channel] & 0x3F0) | (value & 0xF);
pavone@354: 				if (channel == 2 && context->noise_use_tone) {
pavone@354: 					context->counter_load[3] = context->counter_load[2];
pavone@354: 				}
pavone@354: 			}
pavone@354: 		}
pavone@354: 	} else {
pavone@354: 		if (!(context->latch & 0x10)) {
pavone@354: 			uint8_t channel = context->latch >> 5 & 0x3;
pavone@354: 			if (channel != 3) {
pavone@354: 				context->counter_load[channel] = (value << 4 & 0x3F0) | (context->counter_load[channel] & 0xF);
pavone@354: 				if (channel == 2 && context->noise_use_tone) {
pavone@354: 					context->counter_load[3] = context->counter_load[2];
pavone@354: 				}
pavone@354: 			}
pavone@354: 		}
pavone@354: 	}
pavone@354: }
pavone@354: 
pavone@522: #define PSG_VOL_DIV 14
pavone@354: 
pavone@354: //table shamelessly swiped from PSG doc from smspower.org
pavone@354: int16_t volume_table[16] = {
pavone@354: 	32767/PSG_VOL_DIV, 26028/PSG_VOL_DIV, 20675/PSG_VOL_DIV, 16422/PSG_VOL_DIV, 13045/PSG_VOL_DIV, 10362/PSG_VOL_DIV,
pavone@483: 	8231/PSG_VOL_DIV, 6568/PSG_VOL_DIV, 5193/PSG_VOL_DIV, 4125/PSG_VOL_DIV, 3277/PSG_VOL_DIV, 2603/PSG_VOL_DIV,
pavone@354: 	2067/PSG_VOL_DIV, 1642/PSG_VOL_DIV, 1304/PSG_VOL_DIV, 0
pavone@354: };
pavone@354: 
pavone@354: void psg_run(psg_context * context, uint32_t cycles)
pavone@354: {
pavone@354: 	while (context->cycles < cycles) {
pavone@354: 		for (int i = 0; i < 4; i++) {
pavone@354: 			if (context->counters[i]) {
pavone@354: 				context->counters[i] -= 1;
pavone@354: 			}
pavone@354: 			if (!context->counters[i]) {
pavone@354: 				context->counters[i] = context->counter_load[i];
pavone@354: 				context->output_state[i] = !context->output_state[i];
pavone@354: 				if (i == 3 && context->output_state[i]) {
pavone@354: 					context->noise_out = context->lsfr & 1;
pavone@354: 					context->lsfr = (context->lsfr >> 1) | (context->lsfr << 15);
pavone@354: 					if (context->noise_type) {
pavone@354: 						//white noise
pavone@354: 						if (context->lsfr & 0x40) {
pavone@354: 							context->lsfr ^= 0x8000;
pavone@354: 						}
pavone@354: 					}
pavone@354: 				}
pavone@354: 			}
pavone@354: 		}
pavone@838: 
pavone@963: 		context->last_sample = context->accum;
pavone@963: 		context->accum = 0;
pavone@963: 		
pavone@838: 		for (int i = 0; i < 3; i++) {
pavone@838: 			if (context->output_state[i]) {
pavone@838: 				context->accum += volume_table[context->volume[i]];
pavone@838: 			}
pavone@838: 		}
pavone@838: 		if (context->noise_out) {
pavone@838: 			context->accum += volume_table[context->volume[3]];
pavone@838: 		}
pavone@964: 		int32_t tmp = context->accum * context->lowpass_alpha + context->last_sample * (0x10000 - context->lowpass_alpha);
pavone@964: 		context->accum = tmp >> 16;
pavone@838: 
pavone@354: 		context->buffer_fraction += context->buffer_inc;
pavone@483: 		if (context->buffer_fraction >= BUFFER_INC_RES) {
pavone@483: 			context->buffer_fraction -= BUFFER_INC_RES;
pavone@964: 			int32_t tmp = context->last_sample * ((context->buffer_fraction << 16) / context->buffer_inc);
pavone@964: 			tmp += context->accum * (0x10000 - ((context->buffer_fraction << 16) / context->buffer_inc));
pavone@963: 			context->audio_buffer[context->buffer_pos++] = tmp >> 16;
pavone@964: 			
pavone@354: 			if (context->buffer_pos == context->samples_frame) {
pavone@505: 				if (!headless) {
pavone@505: 					render_wait_psg(context);
pavone@505: 				}
pavone@354: 			}
pavone@354: 		}
pavone@380: 		context->cycles += context->clock_inc;
pavone@354: 	}
pavone@354: }
pavone@354: