Add SPU memory allocation

include/PSX/Auxiliary/math_helper.hpp

@@ -34,4 +34,14 @@ namespace JabyEngine {
         }
         return result;
     }
+
+    template<typename T>
+    static constexpr T min_of(T a, T b) {
+        return (a < b) ? a : b;
+    }
+
+    template<typename T>
+    static constexpr T max_of(T a, T b) {
+        return (a > b) ? a : b;
+    }
 }

include/PSX/SPU/spu.hpp

@@ -0,0 +1,11 @@
+#pragma once
+#include "../jabyengine.hpp"
+
+namespace JabyEngine {
+    namespace SPU {
+        const uint8_t* allocate_voice(uint8_t voice, size_t size);
+        void           deallocate_voice(uint8_t voice);
+
+        void dump();
+    }
+}

include/PSX/System/IOPorts/spu_io.hpp

@@ -4,6 +4,10 @@
 
 namespace JabyEngine {
     namespace SPU_IO {
+        namespace MemoryMap {
+            static constexpr uintptr_t ADPCM = 0x01000;
+        }
+        
         enum struct Mode {
             Linear      = 0,
             Exponential = 1,
@@ -126,7 +130,8 @@ namespace JabyEngine {
             static constexpr auto EchoBits = BitRange::from_to(0, 23);
         };
 
-        static constexpr size_t VoiceCount = 24;
+        static constexpr size_t VoiceCount  = 24;
+        static constexpr size_t ReverbCount = 1;
 
         struct Key {            
             __declare_io_port_w_type(inline, ubus32_t, On,     0x1F801D88);

src/Library/internal-include/SPU/spu_internal.hpp

@@ -0,0 +1,2 @@
+#pragma once
+

src/Library/src/BootLoader/start_boot.cpp

@@ -3,6 +3,8 @@
 #include <stdio.hpp>
 
 #include <PSX/GTE/gte.hpp>
+#include <PSX/SPU/spu.hpp>
+#include <PSX/System/IOPorts/spu_io.hpp>
 #include <PSX/System/syscalls.hpp>
 
 extern "C" uint32_t __heap_start;
@@ -39,6 +41,26 @@ namespace JabyEngine {
         }
     }
 
+    static void test_spu_alloc() {
+        static const auto calculate_spu_adr = [](size_t size) -> const uint8_t* {
+            return reinterpret_cast<const uint8_t*>(SPU_IO::MemoryMap::ADPCM + size);
+        }; 
+        static const auto simple_assert = [](uint32_t test_id, const uint8_t* adr, const uint8_t* expected) {
+            static const char* ok_text     = "Test %i: 0x%p == 0x%p; OK\n";
+            static const char* failed_text = "Test %i: 0x%p != 0x%p; Failed\n";
+
+            printf(adr == expected ? ok_text : failed_text, test_id, adr, expected);
+        };
+
+        printf("=== SPU test ===\n");
+        simple_assert(0, SPU::allocate_voice(0, 0x600), calculate_spu_adr(0x0));
+        simple_assert(1, SPU::allocate_voice(1, 0x800), calculate_spu_adr(0x600));
+        simple_assert(2, SPU::allocate_voice(0, 0x300), calculate_spu_adr(0x0));
+        simple_assert(3, SPU::allocate_voice(2, 0x300), calculate_spu_adr(0x300));
+
+        // TODO: More tests      
+    }
+
     namespace boot {
         namespace Start {
             // Thanks to Nicolas Noble!
@@ -77,6 +99,7 @@ namespace JabyEngine {
                 GTE::setup();
                 test_bios_font();
                 test_gte_scale();
+                test_spu_alloc();
 
                 SPU::setup();
             }

src/Library/src/File/Processor/vag_processor.cpp

@@ -5,7 +5,6 @@
 #ifdef __SUPPORT_VAG__
 namespace JabyEngine {
     namespace FileProcessor {
-        #pragma pack(push, 1)
         struct VAGHeader {
             char     id[4];
             uint32_t version;
@@ -27,7 +26,6 @@ namespace JabyEngine {
                 return read_be(this->sample_frequency);
             }
         };
-        #pragma pack(pop)
 
         struct VAGState {
             static constexpr VAGState create() {
@@ -39,7 +37,8 @@ namespace JabyEngine {
             if(config.data_bytes >= sizeof(VAGHeader)) {
                 const auto& header = *reinterpret_cast<const VAGHeader*>(config.data_adr);
 
-                printf("VAG-ID: %s\nName: %s\nSample size: %i\nSample rate: %i\n", header.id, header.name, header.get_sample_size(), header.get_sample_frequency());
+                //printf("VAG-ID: %s\nName: %s\nSample size: %i\nSample rate: %i\n", header.id, header.name, header.get_sample_size(), header.get_sample_frequency());
+                // TODO: Allocate SPU memory
                 return Progress::Error;
             }
 

src/Library/src/SPU/spu.cpp

@@ -0,0 +1,127 @@
+#include "../../internal-include/SPU/spu_internal.hpp"
+#include <PSX/System/IOPorts/spu_io.hpp>
+#include <PSX/Auxiliary/math_helper.hpp>
+#include <PSX/SPU/spu.hpp>
+#include <stddef.hpp>
+
+#include <stdio.hpp>
+
+namespace JabyEngine {
+    namespace SPU {
+        namespace SPU_MemoryMap = SPU_IO::MemoryMap;
+
+        struct SPUMemory {
+            const uint8_t* adr;
+            size_t         size;
+
+            static SPUMemory create(size_t size) {
+                return SPUMemory{.adr = reinterpret_cast<const uint8_t*>(SPU_MemoryMap::ADPCM), .size = size};
+            }
+
+            constexpr void clear() {
+                this->adr  = nullptr;
+                this->size = 0;
+            }
+
+            constexpr const uint8_t* get_end_adr() const {
+                return (this->adr + this->size);
+            }
+
+            constexpr bool is_free() const {
+                return this->adr == nullptr;
+            }
+
+            constexpr bool intersects(const SPUMemory& other) const {
+                const auto* min = max_of(this->adr,           other.adr);
+                const auto* max = min_of(this->get_end_adr(), other.get_end_adr());
+                return min < max;
+            }
+        };
+
+        struct AllocatedVoice {
+            SPUMemory       memory;
+            AllocatedVoice* next_entry;
+        };
+
+        struct VoiceManager {
+            struct Iterator {
+                AllocatedVoice* *prev_voice;
+                AllocatedVoice*  current_voice;
+
+                void next() {
+                    this->prev_voice    = &this->current_voice->next_entry;
+                    this->current_voice = this->current_voice->next_entry;
+                }
+
+                bool has_next() const {
+                    return this->current_voice;
+                }
+
+                operator bool() const {
+                    return Iterator::has_next();
+                }
+            };
+
+            AllocatedVoice  allocated_voice_buffer[SPU_IO::VoiceCount + SPU_IO::ReverbCount] = {0};
+            AllocatedVoice* first_allocated_voice = nullptr;
+
+            Iterator iterator() {
+                return Iterator{.prev_voice = &this->first_allocated_voice, .current_voice = this->first_allocated_voice};
+            }
+
+            AllocatedVoice& get_voice(uint8_t id) {
+                return this->allocated_voice_buffer[id];
+            }
+        };
+
+        static VoiceManager voice_mgr;
+
+        using MemoryFoundCallback = const uint8_t* (AllocatedVoice& new_entry, AllocatedVoice* &prev_entry, AllocatedVoice* next_entry);
+
+        static const uint8_t* verify_and_add(AllocatedVoice& new_entry, AllocatedVoice* &prev_entry, AllocatedVoice* next_entry) {
+            // TODO: Verify that we are not crashing into reverb or that we are higher then SPU
+            prev_entry           = &new_entry;
+            new_entry.next_entry = next_entry;
+
+            return new_entry.memory.adr;
+        }
+
+        static const uint8_t* find_first_fit(AllocatedVoice &new_entry, MemoryFoundCallback memory_found) {
+            auto iterator = voice_mgr.iterator();
+            while(iterator) {
+                if(!iterator.current_voice->memory.intersects(new_entry.memory)) {
+                    break;
+                }
+                new_entry.memory.adr = iterator.current_voice->memory.get_end_adr();
+                iterator.next();
+            }
+
+            return memory_found(new_entry, *iterator.prev_voice, iterator.current_voice);
+        }
+
+        const uint8_t* allocate_voice(uint8_t voice, size_t size) {
+            auto& voice_entry = voice_mgr.get_voice(voice);
+            if(!voice_entry.memory.is_free()) {
+                deallocate_voice(voice);
+            }
+
+            voice_entry.memory = SPUMemory::create(size);
+            return find_first_fit(voice_entry, verify_and_add);
+            // TODO: Set memory adr for voice in SPU
+        }
+
+        void deallocate_voice(uint8_t voice) {
+            auto* voice_adr = &voice_mgr.get_voice(voice);
+            auto  iterator  = voice_mgr.iterator();
+
+            voice_adr->memory.clear();
+            while(iterator) {
+                if(iterator.current_voice == voice_adr) {
+                    *iterator.prev_voice = voice_adr->next_entry;
+                    break;
+                }
+                iterator.next();
+            }
+        }
+    }
+}