sceSas: Resample audio according to pitch

Core/HLE/__sceAudio.cpp

@@ -194,6 +194,7 @@ void __AudioUpdate()
 
 void __AudioSetOutputFrequency(int freq)
 {
+	WARN_LOG(HLE, "Switching audio frequency to %i", freq);
 	mixFrequency = freq;
 }
 

Core/HW/SasAudio.cpp

@@ -148,28 +148,54 @@ void SasInstance::Mix(u32 outAddr)
 		if (!voice.playing)
 			continue;
 
+		// TODO: Special case no-resample case for speed
+
 		if (voice.vagAddr != 0) {
-			for (int i = 0; i < grainSize; i++) {
+			// Load resample history (so we can use a wide filter)
+			resampleBuffer[0] = voice.resampleHist[0];
+			resampleBuffer[1] = voice.resampleHist[1];
+
+			// Figure out number of samples to read.
+			int curSample = voice.samplePos / PSP_SAS_PITCH_BASE;
+			int lastSample = (voice.samplePos + grainSize * voice.pitch) / PSP_SAS_PITCH_BASE;
+			int numSamples = lastSample - curSample;
+
+			// Read N samples into the resample buffer. Could do either PCM or VAG here.
+
+			for (int i = 0; i < numSamples; i++) {
 				int sample = voice.vag.GetSample();
-				voice.samplePos++;
 				if (voice.samplePos >= voice.vagSize || voice.vag.End()) {
 					if (voice.loop) {
-						voice.vag.Loop();
+						voice.Loop();
 					} else {
 						voice.playing = false;
+						// TODO: clear rest of buffer
+						memset(resampleBuffer, 0, (numSamples - i) * sizeof(resampleBuffer[0]));
 					}
 					break;
 				}
-				int l = sample * voice.volumeLeft >> 15;
-				int r = sample * voice.volumeRight >> 15;
+				resampleBuffer[i + 2] = sample;
+			}
+
+			// Save resample history
+			voice.resampleHist[0] = resampleBuffer[numSamples - 2];
+			voice.resampleHist[1] = resampleBuffer[numSamples - 1];
+
+			// Resample to the correct pitch, writing exactly "grainSize" samples.
+
+			int bufferPos = (voice.samplePos & (PSP_SAS_PITCH_BASE - 1)) + 2 * PSP_SAS_PITCH_BASE;
+			for (int i = 0; i < grainSize; i++) {
+				// For now: nearest neighbour, not even using the resample history at all.
+				int sample = resampleBuffer[bufferPos / PSP_SAS_PITCH_BASE];
+				bufferPos += voice.pitch;
 
 				// We mix into this 32-bit temp buffer and clip in a second loop
-				mixBuffer[i * 2] += l;
-				mixBuffer[i * 2 + 1] += r;
+				mixBuffer[i * 2] += sample * voice.volumeLeft >> 15;
+				mixBuffer[i * 2 + 1] += sample * voice.volumeRight >> 15;
 			}
 		}
 		else if (voice.pcmAddr != 0) {
-			// PCM mixing should be easy, can probably share code with VAG if we predecode those
+			// PCM mixing should be easy, can share code with VAG
 		}
 		else if (voice.noiseFreq != 0) {
 			// Generate noise?
@@ -185,4 +211,18 @@ void SasInstance::Mix(u32 outAddr)
 		else if (sample < -32768) out[i] = -32768;
 		else out[i] = sample;
 	}
+}
+
+void SasVoice::Reset()
+{
+	resampleHist[0] = 0;
+	resampleHist[1] = 0;
+}
+
+void SasVoice::Loop()
+{
+	if (vagAddr) {
+		vag.Start(Memory::GetPointer(vagAddr));
+		samplePos = 0;
+	}
 }

Core/HW/SasAudio.h

@@ -91,12 +91,6 @@ public:
 		return *data_++;
 	}
 
-	void Loop() {
-		curSample = 0;
-		s_1 = 0;
-		s_2 = 0;
-	}
-
 private:
 	double samples[28];
 	int curSample;
@@ -155,6 +149,11 @@ struct SasVoice
 
 	int setPaused;
 
+	void Loop();
+	void Reset();
+
+	s16 resampleHist[2];
+
 	ADSREnvelope envelope;
 
 	VagDecoder vag;
@@ -163,8 +162,11 @@ struct SasVoice
 class SasInstance
 {
 public:
-	SasInstance() : mixBuffer(0), grainSize(256) {}
-
+	SasInstance() : mixBuffer(0), resampleBuffer(0), grainSize(0) {}
+	~SasInstance() {
+		delete [] mixBuffer;
+		delete [] resampleBuffer;
+	}
 	SasVoice voices[PSP_SAS_VOICES_MAX];
 	WaveformEffect waveformEffect;
 
@@ -174,6 +176,12 @@ public:
 		mixBuffer = new s32[newGrainSize * 2];
 		memset(mixBuffer, 0, sizeof(int) * newGrainSize * 2);
 		grainSize = newGrainSize;
+		if (resampleBuffer)
+			delete [] resampleBuffer;
+
+		// 2 samples padding at the start, that's where we copy the two last samples from the channel
+		// so that we can do bicubic resampling if necessary.
+		resampleBuffer = new s16[grainSize * 4 + 2];
 	}
 
 	int GetGrainSize() const { return grainSize; }
@@ -183,6 +191,7 @@ public:
 	int outputMode;
 	int length;
 
+	s16 *resampleBuffer;
 	int *mixBuffer;
 
 	void Mix(u32 outAddr);