apu sortof working.

include/audio.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <common.h>
+#include <pa_ringbuffer.h>
 
 typedef struct {
 
@@ -17,16 +18,9 @@ typedef struct {
     u8 volume_left;
     u8 volume_right;
 
-    float sq1_freq;
     u8 sq1_duty;
     u8 sq1_volume;
-    float sq2_freq;
-    u8 sq2_duty;
-    u8 sq2_volume;
-
-    bool ready;
     u8 sq1_sample;
-    float sq1_value;
     u16 sq1_period_reset;
     u16 sq1_period_timer;
     bool sq1_enable;
@@ -37,31 +31,61 @@ typedef struct {
     u8 sq1_len;
     u8 sq1_initial_volume;
     bool sq1_env_direction;
+    bool sq1_env_direction_buffer;
     u8 sq1_env_pace;
+    u8 sq1_env_pace_buffer; 
     u8 sq1_env_timer;
     u8 sq1_sweep_step;
-    float sq1_audio_buffer[4096];
-    u32 sq1_write_head;
-    u32 sq1_read_head;
 
+    u8 sq1_sweep_timer;
+    bool sq1_sweep_enabled;
+    u16 sq1_sweep_period;
+    u16 sq1_calc_period;
+
+    u8 sq2_duty;
+    u8 sq2_volume;
     u8 sq2_sample;
-    float sq2_value;
     u16 sq2_period_reset;
     u16 sq2_period_timer;
     bool sq2_enable;
     bool sq2_len_enable;
-    u8 sq2_sweep_pace;
-    bool sq2_sweep_direction;
     u8 sq2_initial_len;
     u8 sq2_len;
     u8 sq2_initial_volume;
     bool sq2_env_direction;
+    bool sq2_env_direction_buffer;
     u8 sq2_env_pace;
+    u8 sq2_env_pace_buffer;
     u8 sq2_env_timer;
-    u8 sq2_sweep_step;
+
-    float sq2_audio_buffer[4096];
+    bool ch3_enable;
-    u32 sq2_write_head;
+    u8 ch3_initial_len;
-    u32 sq2_read_head;
+    u8 ch3_len;
+    u8 ch3_volume;
+    u8 ch3_initial_volume;
+    u16 ch3_period_timer;
+    u16 ch3_period_reset;
+    bool ch3_len_enable;
+    u8 ch3_last_sample;
+    u8 ch3_sample;
+
+    bool ch4_enable;
+    u8 ch4_initial_len;
+    u8 ch4_len;
+    u8 ch4_initial_volume;
+    u8 ch4_volume;
+    u8 ch4_env_pace;
+    u8 ch4_env_pace_buffer;
+    u8 ch4_env_timer;
+    bool ch4_env_direction;
+    bool ch4_env_direction_buffer;
+    u8 ch4_clock_shift;
+    bool ch4_lfsr_width;
+    u8 ch4_clock_divider;
+    bool ch4_len_enable;
+    u16 ch4_lfsr;
+
+    u8 wave_ram[16];
 
 } audio_context;
 

lib/audio.c

@@ -1,43 +1,72 @@
 #include <audio.h>
 #include <math.h>
 #include <windows.h>
+#include <emu.h>
 
 #include <portaudio.h>
+#include <pa_ringbuffer.h>
 
-#define SAMPLE_RATE 44100
+#define SAMPLE_RATE 192000
 #define FRAMES_PER_BUFFER 64
-#define TABLE_SIZE 44100
+#define TIME_PER_SAMPLE 1.0f / (SAMPLE_RATE/1000.0f/1000.0f)
-
+#define TIME_PER_AUDIO_TICK 1.0f / (1048576.0f/1000.0f/1000.0f)
-#define M_PI 3.14159265
+#define LFSR_BASE_CLOCK 262144.0f
-
-static float sine[TABLE_SIZE];
-static unsigned long ind = 0;
-static unsigned long ind2 = 0;
-
-static int sq1_on_time;
-static int sq1_off_time;
-static float sq1 = 1;
-static int sq1_timer = 0;
-
-static int sq2_on_time;
-static int sq2_off_time;
-static float sq2 = 1;
-static int sq2_timer = 0;
 
 static audio_context ctx;
 
-HANDLE data_to_read;
+static float audio_time = 0;
-HANDLE data_to_write;
+static float lfsr_timer = 0;
+static float lfsr_clock = LFSR_BASE_CLOCK;
 
-const u8 square_sample[8] = {
+const u8 square_sample_00[8] = {
+    0x0,
+    0xF,
+    0xF,
+    0xF,
+    0xF,
+    0xF,
+    0xF,
+    0xF
+};
+
+const u8 square_sample_01[8] = {
+    0x0,
+    0x0,
+    0xF,
+    0xF,
+    0xF,
+    0xF,
+    0xF,
+    0xF
+};
+
+const u8 square_sample_10[8] = {
     0x0,
     0x0,
     0x0,
     0x0,
-    0x1,
+    0xF,
-    0x1,
+    0xF,
-    0x1,
+    0xF,
-    0x1
+    0xF
+};
+
+const u8 square_sample_11[8] = {
+    0x0,
+    0x0,
+    0x0,
+    0x0,
+    0x0,
+    0x0,
+    0xF,
+    0xF
+};
+
+const u8 *square_sample[4] = {
+    square_sample_00,
+    square_sample_01,
+    square_sample_10,
+    square_sample_11
 };
 
 static int audio_callback(const void* input_uffer, void *output_buffer,
@@ -45,115 +74,152 @@ static int audio_callback(const void* input_uffer, void *output_buffer,
                             const PaStreamCallbackTimeInfo *time_info,
                             PaStreamCallbackFlags status_flags,
                             void *userData ) {
-    //printf("Audio Callback!\n");
-    //audio_context *ctx = (audio_context *) userData;
     float *out = (float *)output_buffer;
-    //static float last = 0;
-    //float val;
-    //if(!ctx.ready) {
-    //    val = last;
-    //} else {
-    //    val = ctx.sq1_value;
-    //    last = val;
-    //    ctx.ready = false;
-    //}
-    //PaTime time = time_info->currentTime;
-    //sq1_on_time = 1/(ctx->sq1_freq/(float)SAMPLE_RATE) * ctx->sq1_duty;
-    //sq1_off_time = 1/(ctx->sq1_freq/(float)SAMPLE_RATE) * (1 - ctx->sq1_duty);
-
-    //sq2_on_time = 1/(ctx->sq2_freq/(float)SAMPLE_RATE) * ctx->sq2_duty;
-    //sq2_off_time = 1/(ctx->sq2_freq/(float)SAMPLE_RATE) * (1 - ctx->sq2_duty);
-        //ctx.sq1_period_timer++;
-
-    //if(ctx.sq1_period_timer >= 0x800) {
-        //ctx.sq1_period_timer = ctx.sq1_period_reset;
-        //ctx.sq1_sample = (ctx.sq1_sample + 1) % 8;
-        //ctx.sq1_value = (float)square_sample[ctx.sq1_sample];///15.0f;
-        //ctx.sq1_value = - ctx.sq1_value;
-    //}
-    //printf("val: %f\n", val);
     float left = 0;
     float right = 0;
     for(int i = 0; i < framesPerBuffer; i++) {
-        if(!ctx.audio_enabled) {
+        audio_time += TIME_PER_SAMPLE;
-            *out++ = 0;
+        for(;audio_time >= TIME_PER_AUDIO_TICK;audio_time -= TIME_PER_AUDIO_TICK) {
-            *out++ = 0;
+            ctx.sq1_period_timer++;
-            continue;
+            ctx.sq2_period_timer++;
-        }
+            ctx.ch3_period_timer++;
-        if(WaitForSingleObject(data_to_read, INFINITE) == WAIT_TIMEOUT) {
+
-            continue;
+            if(ctx.sq1_period_timer >= 0x800) {
-        }
+                ctx.sq1_period_timer = ctx.sq1_period_reset;
-        if(ctx.sq1_write_head != ctx.sq1_read_head) {
+                ctx.sq1_sample = (ctx.sq1_sample + 1) % 8;
-            if(ctx.ch1_left){
-                //left = ((float)(ctx.sq1_volume * ctx.sq1_audio_buffer[ctx.sq1_read_head])/15.0f) - 0.5f;
             }
-            if(ctx.ch1_right){
+            if(ctx.sq2_period_timer >= 0x800) {
-                //right = ((float)(ctx.sq1_volume * ctx.sq1_audio_buffer[ctx.sq1_read_head])/15.0f) - 0.5f;
+                ctx.sq2_period_timer = ctx.sq2_period_reset;
+                ctx.sq2_sample = (ctx.sq2_sample + 1) % 8;
             }
-            ctx.sq1_read_head++;
+            if(ctx.ch3_period_timer >= 0x800) {
-            if (ctx.sq1_read_head >= 4096) {
+                ctx.ch3_period_timer = ctx.ch3_period_reset;
-                ctx.sq1_read_head = 0;
+                ctx.ch3_sample = (ctx.ch3_sample + 1) % 32;
+                if(ctx.ch3_sample & 0b1) {
+                    ctx.ch3_last_sample = ctx.wave_ram[ctx.ch3_sample << 1] & 0xF;
+                } else {
+                    ctx.ch3_last_sample = ctx.wave_ram[ctx.ch3_sample << 1] >> 4;
+                }
+            }
+        }
+        lfsr_timer += TIME_PER_SAMPLE;
+        for(;lfsr_timer >= lfsr_clock;lfsr_timer -= lfsr_clock) {
+            if(lfsr_timer >= lfsr_clock && ctx.ch4_enable) {
+                lfsr_timer = 0;
+                ctx.ch4_lfsr = ctx.ch4_lfsr >> 1;
+                u8 new = (!(ctx.ch4_lfsr & 0b1) ^ ((ctx.ch4_lfsr >> 1)) & 0b1);
+                ctx.ch4_lfsr |= (new << 15);
+                if(ctx.ch4_lfsr_width) {
+                    ctx.ch4_lfsr |= (new << 7);
+                }
+                //printf("lfsr: %02X, bit: %d\n", ctx.ch4_lfsr, new);
+            }
+        }
+        if(ctx.audio_enabled){
+            if(ctx.sq1_enable) {
+                if(ctx.ch1_left) {
+                    left += ((float)ctx.sq1_volume/15.0f) * (((float)(square_sample[ctx.sq1_duty][ctx.sq1_sample]) - 7.5f)/7.5f);
+                }
+                if(ctx.ch1_right) {
+                    right += ((float)ctx.sq1_volume/15.0f) * (((float)square_sample[ctx.sq1_duty][ctx.sq1_sample] - 7.5f)/7.5f);
+                }
+            }
+
+            if(ctx.sq2_enable) {
+                if(ctx.ch2_left) {
+                    left += ((float)ctx.sq2_volume/15.0f) * (((float)(square_sample[ctx.sq2_duty][ctx.sq2_sample]) - 7.5f)/7.5f);
+                }
+                if(ctx.ch2_right) {
+                    right += ((float)ctx.sq2_volume/15.0f) * (((float)(square_sample[ctx.sq2_duty][ctx.sq2_sample]) - 7.5f)/7.5f);
+                }
+            }
+
+            if(ctx.ch3_enable) {
+                if(ctx.ch3_left) {
+                    left += ((float)ctx.ch3_volume/4.0f) * (((float)(ctx.ch3_last_sample) - 7.5f)/7.5f);
+                    //printf("left: %d\n", ctx.ch3_volume);
+                }
+                if(ctx.ch3_right) {
+                    right += ((float)ctx.ch3_volume/4.0f) * (((float)(ctx.ch3_last_sample) - 7.5f)/7.5f);
+                }
+            }
+
+            if(ctx.ch4_enable) {
+                if(ctx.ch4_left) {
+                    left += ((float)ctx.ch4_volume/15.0f) * (ctx.ch4_lfsr & 0b1);
+                    //printf("left: %d\n", ctx.ch3_volume);
+                }
+                if(ctx.ch4_right) {
+                    right += ((float)ctx.ch4_volume/15.0f) * (ctx.ch4_lfsr & 0b1);
+                }
             }
         }
 
-        //if(ctx.sq2_write_head != ctx.sq2_read_head) {
+        left *= (float)ctx.volume_left/7.0f;
-        if(ctx.ch2_left){
+        right *= (float)ctx.volume_right/7.0f;
-            left = ctx.sq2_audio_buffer[ctx.sq2_read_head];
+
-            //printf("audio: %d\n", ctx.sq2_audio_buffer[ctx.sq2_read_head]);
+        left /= 4;
-            //left = ((float)(ctx.sq2_volume * square_sample[ctx.sq2_sample])/15.0f) - 0.5f;
+        right /= 4;
+
+        if(left > 1.0f) {
+            printf("Uh Oh! %02X\n", ctx.volume_left);
         }
-        if(ctx.ch2_right){
-            //right += ((float)(ctx.sq2_volume * ctx.sq2_audio_buffer[ctx.sq2_read_head])/15.0f) - 0.5f;
-        }
-        ctx.sq2_read_head++;
-        if (ctx.sq2_read_head >= 4096) {
-            ctx.sq2_read_head = 0;
-        }
-        //}
-        left *= (float)ctx.volume_left/8.0f;
-        right *= (float)ctx.volume_right/8.0f;
         *out++ = left;
         *out++ = right;
-        ReleaseSemaphore(data_to_write, 1, NULL);
     }
     return paContinue;
 }
-
-const int semaphore_count = 128;
 static PaStream *stream;
 
+void audio_sample();
+
 void audio_init(){
-    data_to_read = CreateSemaphore(NULL, 0, semaphore_count, NULL);
-    data_to_write = CreateSemaphore(NULL, semaphore_count, semaphore_count, NULL);
     PaStreamParameters output_parameters;
     PaError err;
 
-    ctx.sq1_volume = 1;
+    ctx.audio_enabled = false;
-    ctx.sq1_duty = 0.5;
+    ctx.ch1_left = false;
-    ctx.sq1_freq = 200;
+    ctx.ch1_right = false;
+    ctx.ch2_left = false;
+    ctx.ch2_right = false;
+    ctx.ch3_left = false;
+    ctx.ch3_right = false;
+    ctx.ch4_left = false;
+    ctx.ch4_right = false;
 
-    ctx.sq2_volume = 1;
+    ctx.volume_left = 0x1;
-    ctx.sq2_duty = 0.5;
+    ctx.volume_right = 0x1;
-    ctx.sq2_freq = 400;
-    ctx.audio_enabled = true;
 
-    ctx.sq2_period_reset = 0x700;
+    ctx.sq1_duty = 0x0;
-    ctx.sq2_period_timer = 0x700;
+    ctx.sq1_volume = 0x0;
-    ctx.sq2_enable = true;
+    ctx.sq1_sample = 0x0;
+    ctx.sq1_period_reset = 0x0;
+    ctx.sq1_period_timer = 0x0;
+    ctx.sq1_enable = false;
+    ctx.sq1_len_enable = false;
+    ctx.sq1_sweep_pace = 0x0;
+    ctx.sq1_sweep_direction = false;
+    ctx.sq1_initial_len = 0x0;
+    ctx.sq1_len = 0x0;
+    ctx.sq1_initial_volume = 0x0;
+    ctx.sq1_env_direction = false;
+    ctx.sq1_env_pace = 0x0;
+    ctx.sq1_env_timer = 0x0;
+    ctx.sq1_sweep_step = 0x0;
+
+    ctx.sq2_duty = 0x0;
+    ctx.sq2_volume = 0x0;
+    ctx.sq2_sample = 0x0;
+    ctx.sq2_period_reset = 0x0;
+    ctx.sq2_period_timer = 0x0;
+    ctx.sq2_enable = false;
     ctx.sq2_len_enable = false;
-    ctx.ch2_left = true;
+    ctx.sq2_initial_len = 0x0;
-    ctx.volume_left = 8;
+    ctx.sq2_len = 0x0;
-    ctx.sq2_volume = 1;
+    ctx.sq2_initial_volume = 0x0;
-    ctx.sq1_value = -1;
+    ctx.sq2_env_direction = false;
-    ctx.sq1_write_head = 0;
+    ctx.sq2_env_pace = 0x0;
-    ctx.sq1_read_head = 0;
+    ctx.sq2_env_timer = 0x0;
-    ctx.sq2_read_head = 0;
-    ctx.sq2_write_head = 0;
-    ctx.sq2_env_pace = 0;
-
-    for(int i = 0; i < TABLE_SIZE; i++) {
-        sine[i] = (float) sin(((double)i/(double)TABLE_SIZE) * M_PI * 2.);
-    }
     
     err = Pa_Initialize();
     if (err != paNoError) goto error;
@@ -171,8 +237,8 @@ void audio_init(){
                         NULL,
                         &output_parameters,
                         SAMPLE_RATE,
-                        FRAMES_PER_BUFFER,
+                        paFramesPerBufferUnspecified,
-                        paClipOff,
+                        paNoFlag,
                         audio_callback,
                         &ctx);
     if(err != paNoError) goto error;
@@ -184,6 +250,17 @@ error:
     fprintf(stderr, "portaudio stream error\n\tError Number: %d\n\tError Message: %s\n", err, Pa_GetErrorText(err));
 }
 
+void sq1_sweep() {
+    //frequency calculation
+    int step = (ctx.sq1_sweep_period >> ctx.sq1_sweep_step);
+    step = ctx.sq1_sweep_direction ? -step : step;
+    ctx.sq1_calc_period = ctx.sq1_sweep_period + step;
+    //overflow check
+    if(ctx.sq1_calc_period > 0x7FFF) {
+        ctx.sq1_enable = false;
+    }
+}
+
 static int change = 1;
 static u32 ticks = 0;
 
@@ -204,101 +281,83 @@ void audio_tick(){
                 ctx.sq2_enable = false;
             }
         }
+
+        if(ctx.ch3_len_enable) {
+            ctx.ch3_len++;
+            if(ctx.ch3_len >= 64) {
+                ctx.ch3_enable = false;
+            }
+        }
+
+        if(ctx.ch4_len_enable) {
+            ctx.ch4_len++;
+            if(ctx.ch4_len >= 64) {
+                ctx.ch4_enable = false;
+            }
+        }
     }
 
     if((prev_ticks & (1 << 3)) && !(ticks & (1 << 3))) {
         if(ctx.sq1_env_pace != 0){
             ctx.sq1_env_timer++;
             if(ctx.sq1_env_timer >= ctx.sq1_env_pace) {
-                ctx.sq1_volume += ctx.sq1_env_direction ? 1 : -1;
-                if(ctx.sq1_volume < 0)
-                    ctx.sq1_volume = 0;
-                if(ctx.sq1_volume > 7)
-                    ctx.sq1_volume = 7;
                 ctx.sq1_env_timer = 0;
+                if((ctx.sq1_env_direction && ctx.sq1_volume != 15) || (!ctx.sq1_env_direction && ctx.sq1_volume != 0)){
+                    ctx.sq1_volume += ctx.sq1_env_direction ? 1 : -1;
+                    if(ctx.sq1_volume < 0)
+                        ctx.sq1_volume = 0;
+                    if(ctx.sq1_volume > 15)
+                        ctx.sq1_volume = 15;
+                }
             }
         }
 
         if(ctx.sq2_env_pace != 0){
             ctx.sq2_env_timer++;
             if(ctx.sq2_env_timer >= ctx.sq2_env_pace) {
-                ctx.sq2_volume += ctx.sq2_env_direction ? 1 : -1;
-                if(ctx.sq2_volume < 0)
-                    ctx.sq2_volume = 0;
-                if(ctx.sq2_volume > 7)
-                    ctx.sq2_volume = 7;
                 ctx.sq2_env_timer = 0;
+                if((ctx.sq2_env_direction && ctx.sq2_volume != 15) || (!ctx.sq2_env_direction && ctx.sq2_volume != 0)){
+                    ctx.sq2_volume += ctx.sq2_env_direction ? 1 : -1;
+                    if(ctx.sq2_volume < 0)
+                        ctx.sq2_volume = 0;
+                    if(ctx.sq2_volume > 15)
+                        ctx.sq2_volume = 15;
+                }
+                //printf("sq2 vol: %01X\n", ctx.sq2_volume);
+            }
+        }
+
+        if(ctx.ch4_env_pace != 0){
+            ctx.ch4_env_timer++;
+            if(ctx.ch4_env_timer >= ctx.ch4_env_pace) {
+                ctx.ch4_env_timer = 0;
+                if((ctx.ch4_env_direction && ctx.ch4_volume != 15) || (!ctx.ch4_env_direction && ctx.ch4_volume != 0)){
+                    ctx.ch4_volume += ctx.ch4_env_direction ? 1 : -1;
+                    if(ctx.ch4_volume < 0)
+                        ctx.ch4_volume = 0;
+                    if(ctx.ch4_volume > 15)
+                        ctx.ch4_volume = 15;
+                }
+                //printf("sq2 vol: %01X\n", ctx.sq2_volume);
             }
         }
 
     }
-}
 
-static u32 updates;
+    if((prev_ticks & (1 << 2)) && !(ticks & (1 << 2))) {
-static u32 last;
+        ctx.sq1_sweep_timer++;
-
+        if(ctx.sq1_sweep_timer >= ctx.sq1_sweep_pace) {
-void audio_period_tick(){
+            ctx.sq1_sweep_timer = 0;
-    //DWORD dwaitResult = WaitForSingleObject(mutex, INFINITE);
+            if(ctx.sq1_enable && ctx.sq1_sweep_pace){
-    //if(dwaitResult == WAIT_ABANDONED)
+                sq1_sweep();
-    //    return;
+                if(ctx.sq1_calc_period <= 0x7FFF && ctx.sq1_sweep_step != 0) {
-    u32 now = get_ticks();
+                    ctx.sq1_sweep_period = ctx.sq1_calc_period;
-    if(now - last >= 1000) {
+                    ctx.sq1_period_reset = ctx.sq1_calc_period;
-        printf("Updates: %d\n", updates);
+                }
-        updates = 0;
+                sq1_sweep();
-        last = now;
+            }
-    }
-
-    ctx.sq1_period_timer++;
-    ctx.sq2_period_timer++;
-
-    if(ctx.sq1_period_timer >= 0x800) {
-        ctx.sq1_period_timer = ctx.sq1_period_reset;
-        ctx.sq1_sample = (ctx.sq1_sample + 1) % 8;
-        //ctx.sq1_value = (float)square_sample[ctx.sq1_sample];///15.0f;
-        //ctx.ready = true;
-    }
-    if(ctx.sq2_period_timer >= 0x800) {
-        ctx.sq2_period_timer = ctx.sq2_period_reset;
-        ctx.sq2_sample = (ctx.sq2_sample + 1) % 8;
-        //ctx.sq1_value = (float)square_sample[ctx.sq1_sample];///15.0f;
-        //ctx.ready = true;
-        if(ctx.sq2_sample == 0) {
-            updates++;
         }
     }
-    DWORD result = WaitForSingleObject(data_to_write, 0);
-    if(result != WAIT_TIMEOUT) {
-        //updates++;
-        //updates = 0;
-        //u32 read = ctx.read_head;
-        //u32 write = ctx.write_head;
-        //if(read > write) {
-        //    write += 4096;
-        //}
-        //if(write - read <= 64){
-        //    return;
-        //}
-        if(ctx.sq1_enable){
-            ctx.sq1_audio_buffer[ctx.sq1_write_head++] = ((float)(ctx.sq2_volume * square_sample[ctx.sq1_sample])/7.5f) - 1.0f;
-            
-        } else {
-            ctx.sq1_audio_buffer[ctx.sq1_write_head++] = 0x0;
-        }
-        if(ctx.sq1_write_head >= 4096) {
-            ctx.sq1_write_head = 0;
-        }
-        if(ctx.sq2_enable){
-            ctx.sq2_audio_buffer[ctx.sq2_write_head++] = ((float)(ctx.sq2_volume * (square_sample[ctx.sq2_sample] - .5f))/7.5f);
-            //printf("Audio: %f\n", ctx.sq2_audio_buffer[ctx.sq2_write_head - 1]);
-        } else {
-            ctx.sq2_audio_buffer[ctx.sq2_write_head++] = 0.0f;
-        }
-        if(ctx.sq2_write_head >= 4096) {
-            ctx.sq2_write_head = 0;
-        }
-        ReleaseSemaphore(data_to_read, 1, NULL);
-    }
-    //ReleaseMutex(mutex);
 }
 
 void enable_square1() {
@@ -308,6 +367,14 @@ void enable_square1() {
     }
     ctx.sq1_volume = ctx.sq1_initial_volume;
     ctx.sq1_env_timer = 0;
+    ctx.sq1_sweep_period = ctx.sq1_period_reset;
+    ctx.sq1_sweep_timer = 0;
+    ctx.sq1_env_direction = ctx.sq1_env_direction_buffer;
+    ctx.sq1_env_pace = ctx.sq1_env_pace_buffer;
+    ctx.sq1_sweep_enabled = (ctx.sq1_sweep_pace || ctx.sq1_sweep_step);
+    if(ctx.sq1_sweep_step) {
+        sq1_sweep();
+    }
 }
 
 void enable_square2() {
@@ -318,8 +385,31 @@ void enable_square2() {
     ctx.sq2_volume = ctx.sq2_initial_volume;
     ctx.sq2_env_timer = 0;
     ctx.sq2_sample = 0;
+    ctx.sq2_period_timer = ctx.sq2_period_reset;
+    ctx.sq2_env_direction = ctx.sq2_env_direction_buffer;
+    ctx.sq2_env_pace = ctx.sq2_env_pace_buffer;
+}
 
-    //printf("Channel 2 enabled, Timer: %d\n", ctx.sq2_len_enable);
+void enable_wave() {
+    ctx.ch3_enable = true;
+    if(ctx.ch3_len >= 64) {
+        ctx.ch3_len = ctx.ch3_initial_len;
+    }
+    ctx.ch3_sample = 0;
+    ctx.ch3_volume = ctx.ch3_initial_volume;
+    ctx.ch3_period_timer = ctx.ch3_period_reset;
+}
+
+void enable_noise() {
+    ctx.ch4_enable = true;
+    if(ctx.ch4_len >= 64) {
+        ctx.ch4_len = ctx.ch4_initial_len;
+    }
+    ctx.ch4_env_timer = 0;
+    ctx.ch4_env_direction = ctx.ch4_env_direction_buffer;
+    ctx.ch4_env_pace = ctx.ch4_env_pace_buffer;
+    ctx.ch4_volume = ctx.ch4_initial_volume;
+    ctx.ch4_lfsr = 0;
 }
 
 u8 audio_read(u16 address) {
@@ -327,7 +417,6 @@ u8 audio_read(u16 address) {
 }
 
 void audio_write(u16 address, u8 value){
-    return;
     if(address == 0xFF26) {
         ctx.audio_enabled = value & 0x80;
     }
@@ -360,16 +449,16 @@ void audio_write(u16 address, u8 value){
         ctx.sq1_initial_len = value & 0x3F;
     }
     if(address == 0xFF12) {
-        ctx.sq1_initial_volume = value >> 4;
+        ctx.sq1_initial_volume = (value >> 4) & 0x0F;
-        ctx.sq1_env_direction = value & 0x8;
+        ctx.sq1_env_direction_buffer = (value & 0x8) == 0x80;
-        ctx.sq1_env_pace = value & 0b111;
+        ctx.sq1_env_pace_buffer = value & 0b111;
     }
     if(address == 0xFF13) {
-        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0xF0) | value;
+        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0xF00) | value;
     }
     if(address == 0xFF14) {
-        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0x0F) | ((value & 0b111) << 8);
+        ctx.sq1_period_reset = (ctx.sq1_period_reset & 0x0FF) | ((value & 0b111) << 8);
-        ctx.sq1_len_enable = value & 0x40;
+        ctx.sq1_len_enable = (value & 0x40) == 0x40;
         if(value & 0x80) {
             enable_square1();
         }
@@ -381,21 +470,77 @@ void audio_write(u16 address, u8 value){
         ctx.sq2_initial_len = value & 0x3F;
     }
     if(address == 0xFF17) {
-        ctx.sq2_initial_volume = value >> 4;
+        ctx.sq2_initial_volume = (value >> 4) & 0x0F;
-        ctx.sq2_env_direction = value & 0x8;
+        ctx.sq2_env_direction_buffer = (value & 0x8) == 0x80;
-        ctx.sq2_env_pace = value * 0b111;
+        ctx.sq2_env_pace_buffer = value & 0b111;
-        if(!ctx.sq2_env_direction && !ctx.sq2_initial_volume) {
+        if(ctx.sq2_env_direction == 0 && ctx.sq2_initial_volume == 0) {
             ctx.sq2_enable = false;
         }
     }
     if(address == 0xFF18) {
-        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0xF0) | value;
+        u16 prev_period = ctx.sq2_period_reset;
+        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0xF00) | value;
+        //printf("period: %03X, old_period: %03X\n", ctx.sq2_period_reset, prev_period);
     }
     if(address == 0xFF19) {
-        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0x0F) | ((value & 0b111) << 8);
+        ctx.sq2_period_reset = (ctx.sq2_period_reset & 0x0FF) | ((value & 0b111) << 8);
-        ctx.sq2_len_enable = value & 0x40;
+        ctx.sq2_len_enable = (value & 0x40) == 0x40;
-        if(value & 0x80) {
+        if((value & 0x80) == 0x80) {
             enable_square2();
         }
     }
+
+    if(address == 0xFF1A) {
+        //ctx.ch3_enable = (value & 0x80) == 0x80;
+    }
+    
+    if(address == 0xFF1B) {
+        ctx.ch3_initial_len = value;
+    }
+
+    if(address == 0xFF1C) {
+        ctx.ch3_initial_volume = (value >> 5) & 0b11;
+    }
+
+    if(address == 0xFF1D) {
+        ctx.ch3_period_reset = (ctx.ch3_period_reset & 0xF00) | value;
+    }
+
+    if(address == 0xFF1E) {
+        ctx.ch3_period_reset = (ctx.ch3_period_reset & 0x0FF) | ((value & 0b111) << 8);
+        ctx.ch3_len_enable = (value & 0x40) == 0x40;
+        if(value & 0x80) {
+            enable_wave();
+        }
+    }
+
+    if(address == 0xFF20) {
+        ctx.ch4_initial_len = value & 0b111111;
+    }
+
+    if(address == 0xFF21) {
+        ctx.ch4_initial_volume = (value >> 4) & 0x0F;
+        ctx.ch4_env_direction_buffer = (value & 0x8) == 0x80;
+        ctx.ch4_env_pace_buffer = value & 0b111;
+    }
+
+    if(address == 0xFF22) {
+        ctx.ch4_clock_shift = (value >> 4);
+        ctx.ch4_lfsr_width = (value & 0x08);
+        ctx.ch4_clock_divider = value & 0b111;
+        float div = (ctx.ch4_clock_divider == 0 ? 0.5f : ctx.ch4_clock_divider);
+        float lfsr_rate = (LFSR_BASE_CLOCK) / div * (1 << ctx.ch4_clock_shift);
+        lfsr_clock = 1.0f / (lfsr_rate/1000.0f/1000.0f);
+    }
+
+    if(address == 0xFF23) {
+        ctx.ch4_len_enable = (value & 0x40) == 0x40;
+        if((value & 0x80) == 0x80) {
+            enable_noise();
+        }
+    }
+
+    if(BETWEEN(address, 0xFF30, 0xFF3F)) {
+        ctx.wave_ram[address - 0xFF30] = value;
+    }
 }

lib/ppu_sm.c

@@ -156,14 +156,14 @@ void ppu_mode_hblank() {
             u32 frame_time = end - prev_frame_time;
 
             if(frame_time < target_frame_time) {
-                //delay((target_frame_time - frame_time));
+                delay((target_frame_time - frame_time));
             }
 
             if (end - start_timer >= 1000) {
                 u32 fps = frame_count;
                 start_timer = end;
                 frame_count = 0;
-                //printf("FPS: %ld\n", fps);
+                printf("FPS: %ld\n", fps);
                 if(cart_need_save()){
                     cart_battery_save();
                 }

lib/timer.c

@@ -13,18 +13,15 @@ long long now;
 
 void timer_init() {
     ctx.div = 0XAC00;
-    LARGE_INTEGER freq;
-    QueryPerformanceFrequency(&freq);
-    counts_per_cycle = freq.QuadPart / (1 << 22);
 }
 
 void timer_tick() {
-    while(now - last < counts_per_cycle){
+    //while(now - last < counts_per_cycle){
-        LARGE_INTEGER current;
+    //    LARGE_INTEGER current;
-        QueryPerformanceCounter(&current);
+    //    QueryPerformanceCounter(&current);
-        now = current.QuadPart;
+    //    now = current.QuadPart;
         //printf("Last-now: %lld, counts: %lld\n", now - last, counts_per_cycle);
-    }
+    //}
     last = now;
     u16 prev_div = ctx.div;
     ctx.div++;
@@ -60,7 +57,7 @@ void timer_tick() {
     }
 
     if((prev_div & (1 << 1)) && (!(ctx.div & (1 << 1)))){
-        audio_period_tick();
+        //audio_period_tick();
     }
 }