From ed367a3f7c2061905d40a26341abbf087a2a168f Mon Sep 17 00:00:00 2001
From: notaz <notasas@gmail.com>
Date: Mon, 12 May 2008 20:17:32 +0000
Subject: [PATCH] host sample rate support for Pico

git-svn-id: file:///home/notaz/opt/svn/PicoDrive@445 be3aeb3a-fb24-0410-a615-afba39da0efa
---
 Pico/Pico.h        |  2 +-
 Pico/Pico/Pico.c   |  8 +++++++
 Pico/Pico/xpcm.c   | 57 ++++++++++++++++++++++++++++------------------
 Pico/PicoInt.h     |  2 ++
 Pico/sound/sound.c |  3 +++
 5 files changed, 49 insertions(+), 23 deletions(-)

diff --git a/Pico/Pico.h b/Pico/Pico.h
index 01785903..0ae25444 100644
--- a/Pico/Pico.h
+++ b/Pico/Pico.h
@@ -71,7 +71,7 @@ extern int  (*PicoMCDcloseTray)(void);
 extern int PicoCDBuffers;
 
 // Pico/Pico.c
-#define XPCM_BUFFER_SIZE (320+32)
+#define XPCM_BUFFER_SIZE (320+160)
 typedef struct
 {
 	int pen_pos[2];
diff --git a/Pico/Pico/Pico.c b/Pico/Pico/Pico.c
index ece6d64c..00fe576c 100644
--- a/Pico/Pico/Pico.c
+++ b/Pico/Pico/Pico.c
@@ -8,6 +8,14 @@ picohw_state PicoPicohw;
 static int prev_line_cnt_irq3 = 0, prev_line_cnt_irq5 = 0;
 static int fifo_bytes_line = (16000<<16)/60/262/2; // fifo bytes/line. FIXME: other rates, modes
 
+PICO_INTERNAL void PicoReratePico(void)
+{
+  if (Pico.m.pal)
+       fifo_bytes_line = (16000<<16)/50/312/2;
+  else fifo_bytes_line = (16000<<16)/60/262/2;
+  PicoPicoPCMRerate();
+}
+
 static void PicoLinePico(int count)
 {
   PicoPicohw.line_counter += count;
diff --git a/Pico/Pico/xpcm.c b/Pico/Pico/xpcm.c
index a92f4d67..4e2de8b7 100644
--- a/Pico/Pico/xpcm.c
+++ b/Pico/Pico/xpcm.c
@@ -15,7 +15,7 @@
 	else if ( val < min ) val = min; \
 }
 
-const int TableQuant[8] =
+static const int TableQuant[8] =
 {
   ADFIX(0.8984375),
   ADFIX(0.8984375),
@@ -29,17 +29,24 @@ const int TableQuant[8] =
 
 // changed using trial and error..
 //const int quant_mul[16] = { 1, 3, 5, 7, 9, 11, 13, 15, -1, -3, -5, -7, -9, -11, -13, -15 };
-const int quant_mul[16]   = { 1, 3, 5, 7, 9, 11, 13, -1, -1, -3, -5, -7, -9, -11, -13, -15 };
+static const int quant_mul[16]   = { 1, 3, 5, 7, 9, 11, 13, -1, -1, -3, -5, -7, -9, -11, -13, -15 };
+
+static int sample = 0, quant = 0, sgn = 0;
+static int stepsamples = (44100<<10)/16000;
 
-static int sample = 0, quant = 0;
 
 PICO_INTERNAL void PicoPicoPCMReset(void)
 {
-  sample = 0;
+  sample = sgn = 0;
   quant = 0x7f;
   memset(PicoPicohw.xpcm_buffer, 0, sizeof(PicoPicohw.xpcm_buffer));
 }
 
+PICO_INTERNAL void PicoPicoPCMRerate(void)
+{
+  stepsamples = (PsndRate<<10)/16000;
+}
+
 #define XSHIFT 7
 
 #define do_sample() \
@@ -47,24 +54,16 @@ PICO_INTERNAL void PicoPicoPCMReset(void)
   sample += quant * quant_mul[srcval] >> XSHIFT; \
   quant = (quant * TableQuant[srcval&7]) >> ADPCMSHIFT; \
   Limit(quant, 0x6000, 0x7f); \
-  Limit(sample, 32767, -32768); \
+  Limit(sample, 32767/2, -32768/2); \
 }
 
 PICO_INTERNAL void PicoPicoPCMUpdate(short *buffer, int length, int stereo)
 {
   unsigned char *src = PicoPicohw.xpcm_buffer;
   unsigned char *lim = PicoPicohw.xpcm_ptr;
-  int srcval, stepsamples = (44100<<10)/16000, needsamples = 0; // TODO: stepsamples
+  int srcval, needsamples = 0;
 
-  if (src == lim)
-  {
-    if (stereo)
-      // still must expand SN76496 to stereo
-      for (; length > 0; buffer+=2, length--)
-        buffer[1] = buffer[0];
-    sample = quant = 0;
-    return;
-  }
+  if (src == lim) goto end;
 
   for (; length > 0 && src < lim; src++)
   {
@@ -72,7 +71,7 @@ PICO_INTERNAL void PicoPicoPCMUpdate(short *buffer, int length, int stereo)
     do_sample();
 
     for (needsamples += stepsamples; needsamples > (1<<10) && length > 0; needsamples -= (1<<10), length--) {
-      *buffer++ = sample;
+      *buffer++ += sample;
       if (stereo) { buffer[0] = buffer[-1]; buffer++; }
     }
 
@@ -80,9 +79,13 @@ PICO_INTERNAL void PicoPicoPCMUpdate(short *buffer, int length, int stereo)
     do_sample();
 
     for (needsamples += stepsamples; needsamples > (1<<10) && length > 0; needsamples -= (1<<10), length--) {
-      *buffer++ = sample;
+      *buffer++ += sample;
       if (stereo) { buffer[0] = buffer[-1]; buffer++; }
     }
+
+    // lame normalization stuff, needed due to wrong adpcm algo
+    sgn += (sample < 0) ? -1 : 1;
+    if (sgn < -16 || sgn > 16) sample -= sample >> 5;
   }
 
   if (src < lim) {
@@ -90,11 +93,21 @@ PICO_INTERNAL void PicoPicoPCMUpdate(short *buffer, int length, int stereo)
     memmove(PicoPicohw.xpcm_buffer, src, di);
     PicoPicohw.xpcm_ptr = PicoPicohw.xpcm_buffer + di;
     elprintf(EL_STATUS, "xpcm update: over %i", di);
+    // adjust fifo
+    PicoPicohw.fifo_bytes = di;
+    return;
   }
-  else
-  {
-    elprintf(EL_STATUS, "xpcm update: under %i", length);
-    PicoPicohw.xpcm_ptr = PicoPicohw.xpcm_buffer;
-  }
+
+  elprintf(EL_STATUS, "xpcm update: under %i", length);
+  PicoPicohw.xpcm_ptr = PicoPicohw.xpcm_buffer;
+
+end:
+  if (stereo)
+    // still must expand SN76496 to stereo
+    for (; length > 0; buffer+=2, length--)
+      buffer[1] = buffer[0];
+
+  sample = sgn = 0;
+  quant = 0x7f;
 }
 
diff --git a/Pico/PicoInt.h b/Pico/PicoInt.h
index eb78f8ca..dcbb57a0 100644
--- a/Pico/PicoInt.h
+++ b/Pico/PicoInt.h
@@ -443,10 +443,12 @@ PICO_INTERNAL int PicoFrameMCD(void);
 
 // Pico/Pico.c
 PICO_INTERNAL int PicoInitPico(void);
+PICO_INTERNAL void PicoReratePico(void);
 
 // Pico/xpcm.c
 PICO_INTERNAL void PicoPicoPCMUpdate(short *buffer, int length, int stereo);
 PICO_INTERNAL void PicoPicoPCMReset(void);
+PICO_INTERNAL void PicoPicoPCMRerate(void);
 
 // Sek.c
 PICO_INTERNAL int SekInit(void);
diff --git a/Pico/sound/sound.c b/Pico/sound/sound.c
index f0348db0..1d6c1689 100644
--- a/Pico/sound/sound.c
+++ b/Pico/sound/sound.c
@@ -158,6 +158,9 @@ void PsndRerate(int preserve_state)
 
   // set mixer
   PsndMix_32_to_16l = (PicoOpt & POPT_EN_STEREO) ? mix_32_to_16l_stereo : mix_32_to_16_mono;
+
+  if (PicoAHW & PAHW_PICO)
+    PicoReratePico();
 }
 
 
-- 
2.39.5