From e3f0c87399e058d1fb080737079f3937f6efc15f Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Sun, 26 Jul 2015 16:13:18 -0700
Subject: [PATCH] PSP2: Mostly functional sound

---
 src/platform/psp2/CMakeLists.txt |  2 +-
 src/platform/psp2/main.c         | 77 ++++++++++++++++++++++++++++++++
 src/platform/psp2/threading.h    |  1 +
 3 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/src/platform/psp2/CMakeLists.txt b/src/platform/psp2/CMakeLists.txt
index 1736db4ba..8a51a4cf2 100644
--- a/src/platform/psp2/CMakeLists.txt
+++ b/src/platform/psp2/CMakeLists.txt
@@ -1,6 +1,6 @@
 file(GLOB PLATFORM_SRC ${CMAKE_SOURCE_DIR}/src/platform/psp2/*.c)
 
-set(PLATFORM_LIBRARY -lvita2d -lSceCtrl_stub -lSceRtc_stub -lSceGxm_stub -lSceDisplay_stub -lm_stub)
+set(PLATFORM_LIBRARY -lvita2d -lSceCtrl_stub -lSceRtc_stub -lSceGxm_stub -lSceDisplay_stub -lSceAudio_stub -lm_stub)
 
 add_executable(${BINARY_NAME}.elf ${PLATFORM_SRC})
 target_link_libraries(${BINARY_NAME}.elf ${BINARY_NAME} ${PLATFORM_LIBRARY})
diff --git a/src/platform/psp2/main.c b/src/platform/psp2/main.c
index d9fa3e3f1..57d94e875 100644
--- a/src/platform/psp2/main.c
+++ b/src/platform/psp2/main.c
@@ -5,13 +5,18 @@
  * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
 #include "gba/gba.h"
 #include "gba/input.h"
+#include "gba/audio.h"
 #include "gba/video.h"
 
 #include "gba/renderers/video-software.h"
+#include "util/circle-buffer.h"
 #include "util/memory.h"
+#include "util/threading.h"
 #include "util/vfs.h"
 #include "platform/psp2/sce-vfs.h"
+#include "third-party/blip_buf/blip_buf.h"
 
+#include <psp2/audioout.h>
 #include <psp2/ctrl.h>
 #include <psp2/display.h>
 #include <psp2/gxm.h>
@@ -26,11 +31,46 @@ PSP2_MODULE_INFO(0, 0, "mGBA");
 #define PSP2_HORIZONTAL_PIXELS 960
 #define PSP2_VERTICAL_PIXELS 544
 #define PSP2_INPUT 0x50535032
+#define PSP2_SAMPLES 64
+#define PSP2_AUDIO_BUFFER_SIZE (PSP2_SAMPLES * 19)
+
+struct GBAPSP2AudioContext {
+	struct CircleBuffer buffer;
+	Mutex mutex;
+	Condition cond;
+	bool running;
+};
 
 static void _mapVitaKey(struct GBAInputMap* map, int pspKey, enum GBAKey key) {
 	GBAInputBindKey(map, PSP2_INPUT, __builtin_ctz(pspKey), key);
 }
 
+static THREAD_ENTRY _audioThread(void* context) {
+	struct GBAPSP2AudioContext* audio = (struct GBAPSP2AudioContext*) context;
+	struct GBAStereoSample buffer[PSP2_AUDIO_BUFFER_SIZE];
+	int audioPort = sceAudioOutOpenPort(PSP2_AUDIO_OUT_PORT_TYPE_MAIN, PSP2_AUDIO_BUFFER_SIZE, 48000, PSP2_AUDIO_OUT_MODE_STEREO);
+	while (audio->running) {
+		MutexLock(&audio->mutex);
+		int len = CircleBufferSize(&audio->buffer);
+		len /= sizeof(buffer[0]);
+		if (len > PSP2_AUDIO_BUFFER_SIZE) {
+			len = PSP2_AUDIO_BUFFER_SIZE;
+		}
+		if (len > 0) {
+			len &= ~(PSP2_AUDIO_MIN_LEN - 1);
+			CircleBufferRead(&audio->buffer, buffer, len * sizeof(buffer[0]));
+			MutexUnlock(&audio->mutex);
+			sceAudioOutSetConfig(audioPort, len, -1, -1);
+			sceAudioOutOutput(audioPort, buffer);
+			MutexLock(&audio->mutex);
+		}
+		ConditionWait(&audio->cond, &audio->mutex);
+		MutexUnlock(&audio->mutex);
+	}
+	sceAudioOutReleasePort(audioPort);
+	return 0;
+}
+
 int main() {
 	printf("%s initializing", projectName);
 	bool running = true;
@@ -89,6 +129,17 @@ int main() {
 	printf("%s loaded.", "ROM");
 
 	ARMReset(cpu);
+	double ratio = GBAAudioCalculateRatio(1, 60, 1);
+	blip_set_rates(gba->audio.left, GBA_ARM7TDMI_FREQUENCY, 48000 * ratio);
+	blip_set_rates(gba->audio.right, GBA_ARM7TDMI_FREQUENCY, 48000 * ratio);
+
+	struct GBAPSP2AudioContext audioContext;
+	CircleBufferInit(&audioContext.buffer, PSP2_AUDIO_BUFFER_SIZE * sizeof(struct GBAStereoSample));
+	MutexInit(&audioContext.mutex);
+	ConditionInit(&audioContext.cond);
+	audioContext.running = true;
+	Thread audioThread;
+	ThreadCreate(&audioThread, _audioThread, &audioContext);
 
 	printf("%s all set and ready to roll.", projectName);
 
@@ -122,6 +173,23 @@ int main() {
 				activeKeys |= 1 << angles;
 			}
 
+			MutexLock(&audioContext.mutex);
+			while (blip_samples_avail(gba->audio.left) >= PSP2_SAMPLES) {
+				if (CircleBufferSize(&audioContext.buffer) + PSP2_SAMPLES * sizeof(struct GBAStereoSample) > CircleBufferCapacity(&audioContext.buffer)) {
+					break;
+				}
+				struct GBAStereoSample samples[PSP2_SAMPLES];
+				blip_read_samples(gba->audio.left, &samples[0].left, PSP2_SAMPLES, true);
+				blip_read_samples(gba->audio.right, &samples[0].right, PSP2_SAMPLES, true);
+				int i;
+				for (i = 0; i < PSP2_SAMPLES; ++i) {
+					CircleBufferWrite16(&audioContext.buffer, samples[i].left);
+					CircleBufferWrite16(&audioContext.buffer, samples[i].right);
+				}
+			}
+			ConditionWake(&audioContext.cond);
+			MutexUnlock(&audioContext.mutex);
+
 			vita2d_start_drawing();
 			vita2d_clear_screen();
 			vita2d_draw_texture_scale(tex, 120, 32, 3.0f, 3.0f);
@@ -141,6 +209,15 @@ int main() {
 
 	GBAInputMapDeinit(&inputMap);
 
+	MutexLock(&audioContext.mutex);
+	audioContext.running = false;
+	ConditionWake(&audioContext.cond);
+	MutexUnlock(&audioContext.mutex);
+	ThreadJoin(audioThread);
+	CircleBufferDeinit(&audioContext.buffer);
+	MutexDeinit(&audioContext.mutex);
+	ConditionDeinit(&audioContext.cond);
+
 	mappedMemoryFree(gba, 0);
 	mappedMemoryFree(cpu, 0);
 
diff --git a/src/platform/psp2/threading.h b/src/platform/psp2/threading.h
index b3f7c9d5d..4ca74af13 100644
--- a/src/platform/psp2/threading.h
+++ b/src/platform/psp2/threading.h
@@ -118,6 +118,7 @@ static inline int _sceThreadEntry(SceSize args, void* argp) {
 static inline int ThreadCreate(Thread* thread, ThreadEntry entry, void* context) {
 	Thread id = sceKernelCreateThread("SceThread", _sceThreadEntry, 0x40, 0x10000, 0, 0x70000, 0);
 	if (id < 0) {
+		*thread = 0;
 		return id;
 	}
 	*thread = id;