From 0868e68e0ea879652dd8841fab1d569d78345755 Mon Sep 17 00:00:00 2001
From: Wohlstand <admin@wohlnet.ru>
Date: Mon, 24 Mar 2025 13:20:42 +0300
Subject: ESFMu: Added custom queue as built-in is faulty

---
 src/chips/esfmu_opl3.cpp | 45 +++++++++++++++++++++++++++++++++++++++------
 src/chips/esfmu_opl3.h   | 14 ++++++++++++++
 2 files changed, 53 insertions(+), 6 deletions(-)

diff --git a/src/chips/esfmu_opl3.cpp b/src/chips/esfmu_opl3.cpp
index 4f4a96d..67056d0 100644
--- a/src/chips/esfmu_opl3.cpp
+++ b/src/chips/esfmu_opl3.cpp
@@ -23,10 +23,13 @@
 #include <cstring>
 
 ESFMuOPL3::ESFMuOPL3() :
-    OPLChipBaseT()
+    OPLChipBaseT(),
+    m_headPos(0),
+    m_tailPos(0),
+    m_queueCount(0)
 {
     m_chip = new esfm_chip;
-    setRate(m_rate);
+    ESFMuOPL3::setRate(m_rate);
 }
 
 ESFMuOPL3::~ESFMuOPL3()
@@ -53,19 +56,49 @@ void ESFMuOPL3::reset()
 
 void ESFMuOPL3::writeReg(uint16_t addr, uint8_t data)
 {
-    esfm_chip *chip_r = reinterpret_cast<esfm_chip*>(m_chip);
-    ESFM_write_reg_buffered_fast(chip_r, addr, data);
+    Reg &back = m_queue[m_headPos++];
+    back.addr = addr;
+    back.data = data;
+
+    if(m_headPos >= c_queueSize)
+        m_headPos = 0;
+
+    ++m_queueCount;
+    // esfm_chip *chip_r = reinterpret_cast<esfm_chip*>(m_chip);
+    // ESFM_write_reg_buffered(chip_r, addr, data);
 }
 
 void ESFMuOPL3::writePan(uint16_t addr, uint8_t data)
 {
-    esfm_chip *chip_r = reinterpret_cast<esfm_chip*>(m_chip);
-    // OPL3_WritePan(chip_r, addr, data);
+    // FIXME: Implement panning support
+    // esfm_chip *chip_r = reinterpret_cast<esfm_chip*>(m_chip);
+    // ESFM_write?pan(chip_r, addr, data);
+    (void)(addr);
+    (void)(data);
 }
 
 void ESFMuOPL3::nativeGenerate(int16_t *frame)
 {
     esfm_chip *chip_r = reinterpret_cast<esfm_chip*>(m_chip);
+    uint32_t addr = 0xffff, data;
+
+    // see if there is data to be written; if so, extract it and dequeue
+    if(m_queueCount > 0)
+    {
+        const Reg &front = m_queue[m_tailPos++];
+
+        if(m_tailPos >= c_queueSize)
+            m_tailPos = 0;
+        --m_queueCount;
+
+        addr = front.addr;
+        data = front.data;
+    }
+
+    // write to the chip
+    if(addr != 0xffff)
+        ESFM_write_reg(chip_r, addr, data);
+
     ESFM_generate(chip_r, frame);
 }
 
diff --git a/src/chips/esfmu_opl3.h b/src/chips/esfmu_opl3.h
index b01d784..43eb09a 100644
--- a/src/chips/esfmu_opl3.h
+++ b/src/chips/esfmu_opl3.h
@@ -26,6 +26,20 @@
 class ESFMuOPL3 final : public OPLChipBaseT<ESFMuOPL3>
 {
     void *m_chip;
+
+    static const size_t c_queueSize = 500;
+
+    struct Reg
+    {
+        uint32_t addr;
+        uint8_t data;
+    };
+
+    Reg m_queue[c_queueSize];
+    size_t m_headPos;
+    size_t m_tailPos;
+    long m_queueCount;
+
 public:
     ESFMuOPL3();
     ~ESFMuOPL3() override;
-- 
cgit v1.2.3