Moar Teak LLE

Co-Authored-By: PSISP <12768103+psi-rockin@users.noreply.github.com>

include/audio/dsp_core.hpp

@@ -1,4 +1,5 @@
 #pragma once
+#include <array>
 #include <functional>
 #include <memory>
 #include <vector>
@@ -7,15 +8,19 @@
 #include "logger.hpp"
 #include "memory.hpp"
 
+// The DSP core must have access to the DSP service to be able to trigger interrupts properly
+class DSPService;
+
 namespace Audio {
 	class DSPCore {
 	  protected:
 		Memory& mem;
+		DSPService& dspService;
 		MAKE_LOG_FUNCTION(log, dspLogger)
 
 	  public:
 		enum class Type { Null, Teakra };
-		DSPCore(Memory& mem) : mem(mem) {}
+		DSPCore(Memory& mem, DSPService& dspService) : mem(mem), dspService(dspService) {}
 
 		virtual void reset() = 0;
 		virtual void runAudioFrame() = 0;
@@ -31,5 +36,5 @@ namespace Audio {
 		virtual void setSemaphoreMask(u16 value) = 0;
 	};
 
-	std::unique_ptr<DSPCore> makeDSPCore(DSPCore::Type type, Memory& mem);
+	std::unique_ptr<DSPCore> makeDSPCore(DSPCore::Type type, Memory& mem, DSPService& dspService);
 }  // namespace Audio

include/audio/null_core.hpp

@@ -20,7 +20,7 @@ namespace Audio {
 		void resetAudioPipe();
 
 	  public:
-		NullDSP(Memory& mem) : DSPCore(mem) {}
+		NullDSP(Memory& mem, DSPService& dspService) : DSPCore(mem, dspService) {}
 
 		void reset() override;
 		void runAudioFrame() override {}

include/audio/teakra_core.hpp

@@ -1,5 +1,6 @@
 #pragma once
 #include "audio/dsp_core.hpp"
+#include "swap.hpp"
 #include "teakra/teakra.h"
 
 namespace Audio {
@@ -8,8 +9,63 @@ namespace Audio {
 		u32 pipeBaseAddr;
 		bool running;
 
+		// Get a pointer to a data memory address
+		u8* getDataPointer(u32 address) { return getDspMemory() + Memory::DSP_DATA_MEMORY_OFFSET + address; }
+
+		enum class PipeDirection {
+			DSPtoCPU = 0,
+			CPUtoDSP = 1,
+		};
+
+		// A lot of Teakra integration code, especially pipe stuff are based on Citra's integration here:
+		// https://github.com/citra-emu/citra/blob/master/src/audio_core/lle/lle.cpp
+		struct PipeStatus {
+			// All addresses and sizes here refer to byte values, NOT 16-bit values.
+			u16_le address;
+			u16_le byteSize;
+			u16_le readPointer;
+			u16_le writePointer;
+			u8 slot;
+			u8 flags;
+
+			static constexpr u16 wrapBit = 0x8000;
+			static constexpr u16 pointerMask = 0x7FFF;
+
+			bool isFull() const { return (readPointer ^ writePointer) == wrapBit; }
+			bool isEmpty() const { return (readPointer ^ writePointer) == 0; }
+
+			 // isWrapped: Are read and write pointers in different memory passes.
+			 // true:   xxxx]----[xxxx (data is wrapping around the end of memory)
+			 // false:  ----[xxxx]----
+			bool isWrapped() const { return (readPointer ^ writePointer) >= wrapBit; }
+		};
+		static_assert(sizeof(PipeStatus) == 10, "Teakra: Pipe Status size is wrong");
+		static constexpr u8 pipeToSlotIndex(u8 pipe, PipeDirection direction) { return (pipe * 2) + u8(direction); }
+
+		PipeStatus getPipeStatus(u8 pipe, PipeDirection direction) {
+			PipeStatus ret;
+			const u8 index = pipeToSlotIndex(pipe, direction);
+
+			std::memcpy(&ret, getDataPointer(pipeBaseAddr * 2 + index * sizeof(PipeStatus)), sizeof(PipeStatus));
+			return ret;
+		}
+
+		void updatePipeStatus(const PipeStatus& status) {
+			u8 slot = status.slot;
+			u8* statusAddress = getDataPointer(pipeBaseAddr * 2 + slot * sizeof(PipeStatus));
+
+			if (slot % 2 == 0) {
+				std::memcpy(statusAddress + 4, &status.readPointer, sizeof(u16));
+			} else {
+				std::memcpy(statusAddress + 6, &status.writePointer, sizeof(u16));
+			}
+		}
+
+		bool signalledData;
+		bool signalledSemaphore;
+
 	  public:
-		TeakraDSP(Memory& mem);
+		TeakraDSP(Memory& mem, DSPService& dspService);
 
 		void reset() override;
 		void runAudioFrame() override {

include/scheduler.hpp

@@ -10,7 +10,8 @@ struct Scheduler {
 	enum class EventType {
 		VBlank = 0,          // End of frame event
 		UpdateTimers = 1,    // Update kernel timer objects
-		Panic = 2,           // Dummy event that is always pending and should never be triggered (Timestamp = UINT64_MAX)
+		RunDSP = 2,          // Make the emulated DSP run for one audio frame
+		Panic = 3,           // Dummy event that is always pending and should never be triggered (Timestamp = UINT64_MAX)
 		TotalNumberOfEvents  // How many event types do we have in total?
 	};
 	static constexpr usize totalNumberOfEvents = static_cast<usize>(EventType::TotalNumberOfEvents);
@@ -49,6 +50,7 @@ struct Scheduler {
 		// Clear any pending events
 		events.clear();
 		addEvent(Scheduler::EventType::VBlank, arm11Clock / 60);
+		addEvent(Scheduler::EventType::RunDSP, 16384 * 2);
 
 		// Add a dummy event to always keep the scheduler non-empty
 		addEvent(EventType::Panic, std::numeric_limits<u64>::max());

include/services/dsp.hpp

@@ -63,4 +63,8 @@ public:
 	};
 
 	void signalEvents();
+	void triggerPipeEvent(int index);
+	void triggerSemaphoreEvent();
+	void triggerInterrupt0();
+	void triggerInterrupt1();
 };

src/core/audio/dsp_core.cpp

@@ -3,16 +3,16 @@
 #include "audio/null_core.hpp"
 #include "audio/teakra_core.hpp"
 
-std::unique_ptr<Audio::DSPCore> Audio::makeDSPCore(DSPCore::Type type, Memory& mem) {
+std::unique_ptr<Audio::DSPCore> Audio::makeDSPCore(DSPCore::Type type, Memory& mem, DSPService& dspService) {
 	std::unique_ptr<DSPCore> core;
 
 	switch (type) {
-		case DSPCore::Type::Null: core = std::make_unique<NullDSP>(mem); break;
+		case DSPCore::Type::Null: core = std::make_unique<NullDSP>(mem, dspService); break;
-		case DSPCore::Type::Teakra: core = std::make_unique<TeakraDSP>(mem); break;
+		case DSPCore::Type::Teakra: core = std::make_unique<TeakraDSP>(mem, dspService); break;
 
 		default:
 			Helpers::warn("Invalid DSP core selected!");
-			core = std::make_unique<NullDSP>(mem);
+			core = std::make_unique<NullDSP>(mem, dspService);
 			break;
 	}
 

src/core/audio/teakra_core.cpp

@@ -1,10 +1,14 @@
 #include "audio/teakra_core.hpp"
 
+#include <algorithm>
+#include <cstring>
+
+#include "services/dsp.hpp"
+
 using namespace Audio;
 
 struct Dsp1 {
 	// All sizes are in bytes unless otherwise specified
-
 	u8 signature[0x100];
 	u8 magic[4];
 	u32 size;
@@ -30,7 +34,7 @@ struct Dsp1 {
 	Segment segments[10];
 };
 
-TeakraDSP::TeakraDSP(Memory& mem) : DSPCore(mem), pipeBaseAddr(0), running(false) {
+TeakraDSP::TeakraDSP(Memory& mem, DSPService& dspService) : DSPCore(mem, dspService), pipeBaseAddr(0), running(false) {
 	teakra.Reset();
 
 	// Set up callbacks for Teakra
@@ -48,30 +52,188 @@ TeakraDSP::TeakraDSP(Memory& mem) : DSPCore(mem), pipeBaseAddr(0), running(false
 	teakra.SetAudioCallback([=](std::array<s16, 2> sample) {
 		// NOP for now
 	});
+
+	// Set up event handlers
+	teakra.SetRecvDataHandler(0, [&]() {
+		if (running) {
+			dspService.triggerInterrupt0();
+		}
+	});
+
+	teakra.SetRecvDataHandler(1, [&]() {
+		if (running) {
+			dspService.triggerInterrupt1();
+		}
+	});
+
+	auto processPipeEvent = [&](bool dataEvent) {
+		if (!running) {
+			return;
+		}
+
+		if (dataEvent) {
+			signalledData = true;
+		} else {
+			if ((teakra.GetSemaphore() & 0x8000) == 0) {
+				return;
+			}
+
+			signalledSemaphore = true;
+		}
+
+		if (signalledSemaphore && signalledData) {
+			signalledSemaphore = signalledData = false;
+
+			u16 slot = teakra.RecvData(2);
+			u16 side = slot % 2;
+			u16 pipe = slot / 2;
+
+			if (side != static_cast<u16>(PipeDirection::DSPtoCPU)) {
+				return;
+			}
+
+			if (pipe == 0) {
+				Helpers::warn("Pipe event for debug pipe: Should be ignored and the data should be flushed");
+			} else {
+				dspService.triggerPipeEvent(pipe);
+			}
+		}
+	};
+
+	teakra.SetRecvDataHandler(2, [processPipeEvent]() { processPipeEvent(true); });
+	teakra.SetSemaphoreHandler([processPipeEvent]() { processPipeEvent(false); });
 }
 
 void TeakraDSP::reset() {
 	teakra.Reset();
 	running = false;
+	signalledData = signalledSemaphore = false;
 }
 
+// https://github.com/citra-emu/citra/blob/master/src/audio_core/lle/lle.cpp
 void TeakraDSP::writeProcessPipe(u32 channel, u32 size, u32 buffer) {
-	// TODO
+	Helpers::warn("Teakra: Write process pipe");
+	size &= 0xffff;
+
+	PipeStatus status = getPipeStatus(channel, PipeDirection::CPUtoDSP);
+	bool needUpdate = false;  // Do we need to update the pipe status and catch up Teakra?
+
+	std::vector<u8> data;
+	data.reserve(size);
+
+	// Read data to write
+	for (int i = 0; i < size; i++) {
+		const u8 byte = mem.read8(buffer + i);
+		data.push_back(byte);
+	}
+	u8* dataPointer = data.data();
+
+	while (size != 0) {
+		if (status.isFull()) {
+			Helpers::warn("Teakra: Writing to full pipe");
+		}
+
+		// Calculate begin/end/size for write
+		const u16 writeEnd = status.isWrapped() ? (status.readPointer & PipeStatus::pointerMask) : status.byteSize;
+		const u16 writeBegin = status.writePointer & PipeStatus::pointerMask;
+		const u16 writeSize = std::min<u16>(u16(size), writeEnd - writeBegin);
+
+		if (writeEnd <= writeBegin) [[unlikely]] {
+			Helpers::warn("Teakra: Writing to pipe but end <= start");
+		}
+
+		// Write data to pipe, increment write and buffer pointers, decrement size
+		std::memcpy(getDataPointer(status.address * 2 + writeBegin), dataPointer, writeSize);
+		dataPointer += writeSize;
+		status.writePointer += writeSize;
+		size -= writeSize;
+
+		if ((status.writePointer & PipeStatus::pointerMask) > status.byteSize) [[unlikely]] {
+			Helpers::warn("Teakra: Writing to pipe but write > size");
+		}
+
+		if ((status.writePointer & PipeStatus::pointerMask) == status.byteSize) {
+			status.writePointer &= PipeStatus::wrapBit;
+			status.writePointer ^= PipeStatus::wrapBit;
+		}
+		needUpdate = true;
+	}
+
+	if (needUpdate) {
+		updatePipeStatus(status);
+		while (!teakra.SendDataIsEmpty(2)) {
+			runAudioFrame();
+		}
+
+		teakra.SendData(2, status.slot);
+	}
 }
 
 std::vector<u8> TeakraDSP::readPipe(u32 channel, u32 peer, u32 size, u32 buffer) {
-	// TODO
+	Helpers::warn("Teakra: Read pipe");
-	return std::vector<u8>();
+	size &= 0xffff;
+
+	PipeStatus status = getPipeStatus(channel, PipeDirection::DSPtoCPU);
+
+	std::vector<u8> pipeData(size);
+	u8* dataPointer = pipeData.data();
+	bool needUpdate = false;  // Do we need to update the pipe status and catch up Teakra?
+
+	while (size != 0) {
+		if (status.isEmpty()) [[unlikely]] {
+			Helpers::warn("Teakra: Reading from empty pipe");
+			return pipeData;
+		}
+
+		// Read as many bytes as possible
+		const u16 readEnd = status.isWrapped() ? status.byteSize : (status.writePointer & PipeStatus::pointerMask);
+		const u16 readBegin = status.readPointer & PipeStatus::pointerMask;
+		const u16 readSize = std::min<u16>(u16(size), readEnd - readBegin);
+
+		// Copy bytes to the output vector, increment the read and vector pointers and decrement the size appropriately
+		std::memcpy(dataPointer, getDataPointer(status.address * 2 + readBegin), readSize);
+		dataPointer += readSize;
+		status.readPointer += readSize;
+		size -= readSize;
+
+		if ((status.readPointer & PipeStatus::pointerMask) > status.byteSize) [[unlikely]] {
+			Helpers::warn("Teakra: Reading from pipe but read > size");
+		}
+
+		if ((status.readPointer & PipeStatus::pointerMask) == status.byteSize) {
+			status.readPointer &= PipeStatus::wrapBit;
+			status.readPointer ^= PipeStatus::wrapBit;
+		}
+
+		needUpdate = true;
+	}
+
+	if (needUpdate) {
+		updatePipeStatus(status);
+		while (!teakra.SendDataIsEmpty(2)) {
+			runAudioFrame();
+		}
+
+		teakra.SendData(2, status.slot);
+	}
+
+	return pipeData;
 }
 
 void TeakraDSP::loadComponent(std::vector<u8>& data, u32 programMask, u32 dataMask) {
 	// TODO: maybe move this to the DSP service
+	if (running) {
+		Helpers::warn("Loading DSP component when already loaded");
+		return;
+	}
+
+	teakra.Reset();
 
 	u8* dspCode = teakra.GetDspMemory().data();
 	u8* dspData = dspCode + 0x40000;
 
 	Dsp1 dsp1;
-	memcpy(&dsp1, data.data(), sizeof(dsp1));
+	std::memcpy(&dsp1, data.data(), sizeof(dsp1));
 
 	// TODO: verify DSP1 signature
 
@@ -89,7 +251,7 @@ void TeakraDSP::loadComponent(std::vector<u8>& data, u32 programMask, u32 dataMa
 			default: dst = dspData + addr; break;
 		}
 
-		memcpy(dst, src, segment.size);
+		std::memcpy(dst, src, segment.size);
 	}
 
 	bool syncWithDsp = dsp1.flags & 0x1;
@@ -123,7 +285,8 @@ void TeakraDSP::loadComponent(std::vector<u8>& data, u32 programMask, u32 dataMa
 
 void TeakraDSP::unloadComponent() {
 	if (!running) {
-		Helpers::panic("Audio: unloadComponent called without a running program");
+		Helpers::warn("Audio: unloadComponent called without a running program");
+		return;
 	}
 
 	// Wait for SEND2 to be ready, then send the shutdown command to the DSP

src/core/services/dsp.cpp

@@ -269,3 +269,27 @@ void DSPService::signalEvents() {
 	if (interrupt0.has_value()) { kernel.signalEvent(interrupt0.value()); }
 	if (interrupt1.has_value()) { kernel.signalEvent(interrupt1.value()); }
 }
+
+void DSPService::triggerPipeEvent(int index) {
+	if (index < pipeCount && pipeEvents[index].has_value()) {
+		kernel.signalEvent(*pipeEvents[index]);
+	}
+}
+
+void DSPService::triggerSemaphoreEvent() {
+	if (semaphoreEvent.has_value()) {
+		kernel.signalEvent(*semaphoreEvent);
+	}
+}
+
+void DSPService::triggerInterrupt0() {
+	if (interrupt0.has_value()) {
+		kernel.signalEvent(*interrupt0);
+	}
+}
+
+void DSPService::triggerInterrupt1() {
+	if (interrupt1.has_value()) {
+		kernel.signalEvent(*interrupt1);
+	}
+}

src/core/services/gsp_gpu.cpp

@@ -125,7 +125,7 @@ void GPUService::registerInterruptRelayQueue(u32 messagePointer) {
 void GPUService::requestInterrupt(GPUInterrupt type) {
 	// HACK: Signal DSP events on GPU interrupt for now until we have the DSP since games need DSP events
 	// Maybe there's a better alternative?
-	kernel.signalDSPEvents();
+	// kernel.signalDSPEvents();
 
 	if (sharedMem == nullptr) [[unlikely]] { // Shared memory hasn't been set up yet
 		return;

src/emulator.cpp

@@ -24,8 +24,10 @@ Emulator::Emulator()
 	  httpServer(this)
 #endif
 {
-	dsp = Audio::makeDSPCore(Audio::DSPCore::Type::Teakra, memory);
+	DSPService& dspService = kernel.getServiceManager().getDSP();
-	kernel.getServiceManager().getDSP().setDSPCore(dsp.get());
+
+	dsp = Audio::makeDSPCore(Audio::DSPCore::Type::Teakra, memory, dspService);
+	dspService.setDSPCore(dsp.get());
 
 #ifdef PANDA3DS_ENABLE_DISCORD_RPC
 	if (config.discordRpcEnabled) {
@@ -49,6 +51,8 @@ void Emulator::reset(ReloadOption reload) {
 	cpu.reset();
 	gpu.reset();
 	memory.reset();
+	dsp->reset();
+
 	// Reset scheduler and add a VBlank event
 	scheduler.reset();
 
@@ -146,6 +150,11 @@ void Emulator::pollScheduler() {
 			}
 
 			case Scheduler::EventType::UpdateTimers: kernel.pollTimers(); break;
+			case Scheduler::EventType::RunDSP: {
+				scheduler.addEvent(Scheduler::EventType::RunDSP, time + 16384 * 2);
+				dsp->runAudioFrame();
+				break;
+			}
 
 			default: {
 				Helpers::panic("Scheduler: Unimplemented event type received: %d\n", static_cast<int>(eventType));