From 5c8855b9a1849af855923aa1b87005febd01704b Mon Sep 17 00:00:00 2001
From: Chris Robinson <chris.kcat@gmail.com>
Date: Tue, 14 Feb 2023 08:38:25 -0800
Subject: Support MSADPCM samples in the mixer

---
 core/buffer_storage.cpp |   1 +
 core/buffer_storage.h   |   2 +
 core/voice.cpp          | 106 ++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 109 insertions(+)

(limited to 'core')

diff --git a/core/buffer_storage.cpp b/core/buffer_storage.cpp
index 1e826bff..4f1480c2 100644
--- a/core/buffer_storage.cpp
+++ b/core/buffer_storage.cpp
@@ -17,6 +17,7 @@ uint BytesFromFmt(FmtType type) noexcept
     case FmtMulaw: return sizeof(uint8_t);
     case FmtAlaw: return sizeof(uint8_t);
     case FmtIMA4: break;
+    case FmtMSADPCM: break;
     }
     return 0;
 }
diff --git a/core/buffer_storage.h b/core/buffer_storage.h
index a4d1b289..b8d387a5 100644
--- a/core/buffer_storage.h
+++ b/core/buffer_storage.h
@@ -19,6 +19,7 @@ enum FmtType : unsigned char {
     FmtMulaw,
     FmtAlaw,
     FmtIMA4,
+    FmtMSADPCM,
 };
 enum FmtChannels : unsigned char {
     FmtMono,
@@ -98,6 +99,7 @@ struct BufferStorage {
     inline uint blockSizeFromFmt() const noexcept
     {
         if(mType == FmtIMA4) return ((mBlockAlign-1)/2 + 4) * channelsFromFmt();
+        if(mType == FmtMSADPCM) return ((mBlockAlign-2)/2 + 7) * channelsFromFmt();
         return frameSizeFromFmt();
     };
 
diff --git a/core/voice.cpp b/core/voice.cpp
index f6954cbe..c010ae6d 100644
--- a/core/voice.cpp
+++ b/core/voice.cpp
@@ -203,6 +203,23 @@ constexpr int IMA4Index_adjust[16] = {
    -1,-1,-1,-1, 2, 4, 6, 8
 };
 
+/* MSADPCM Adaption table */
+constexpr int MSADPCMAdaption[16] = {
+    230, 230, 230, 230, 307, 409, 512, 614,
+    768, 614, 512, 409, 307, 230, 230, 230
+};
+
+/* MSADPCM Adaption Coefficient tables */
+constexpr int MSADPCMAdaptionCoeff[7][2] = {
+    { 256,    0 },
+    { 512, -256 },
+    {   0,    0 },
+    { 192,   64 },
+    { 240,    0 },
+    { 460, -208 },
+    { 392, -232 }
+};
+
 
 void SendSourceStoppedEvent(ContextBase *context, uint id)
 {
@@ -335,6 +352,94 @@ inline void LoadSamples<FmtIMA4>(float *RESTRICT dstSamples, const al::byte *src
     } while(1);
 }
 
+template<>
+inline void LoadSamples<FmtMSADPCM>(float *RESTRICT dstSamples, const al::byte *src,
+    const size_t srcChan, const size_t srcOffset, const size_t srcStep,
+    const size_t samplesPerBlock, const size_t samples) noexcept
+{
+    const size_t blockBytes{((samplesPerBlock-2)/2 + 7)*srcStep};
+
+    src += srcOffset/samplesPerBlock*blockBytes;
+    size_t skip{srcOffset % samplesPerBlock};
+
+    size_t wrote{0};
+    do {
+        const al::byte *input{src};
+        uint8_t blockpred{std::min(input[srcChan], uint8_t{8})};
+        input += srcStep;
+        int delta{input[2*srcChan + 0] | (input[2*srcChan + 1] << 8)};
+        input += srcStep*2;
+
+        int sampleHistory[2]{};
+        sampleHistory[0] = input[2*srcChan + 0] | (input[2*srcChan + 1]<<8);
+        input += srcStep*2;
+        sampleHistory[1] = input[2*srcChan + 0] | (input[2*srcChan + 1]<<8);
+        input += srcStep*2;
+
+        delta = (delta^0x8000) - 32768;
+        sampleHistory[0] = (sampleHistory[0]^0x8000) - 32768;
+        sampleHistory[1] = (sampleHistory[1]^0x8000) - 32768;
+
+        if(skip < 2) [[likely]]
+        {
+            if(!skip) [[likely]]
+            {
+                dstSamples[wrote++] = static_cast<float>(sampleHistory[1]) / 32768.0f;
+                if(wrote == samples) return;
+            }
+            else
+                --skip;
+            dstSamples[wrote++] = static_cast<float>(sampleHistory[0]) / 32768.0f;
+            if(wrote == samples) return;
+        }
+        else
+            skip -= 2;
+
+        int tempsamples[8]{};
+        size_t nibbleOffset{srcChan};
+        for(size_t i{2};i < samplesPerBlock;)
+        {
+            const size_t todo{minz(samplesPerBlock-i, 8)};
+
+            for(size_t j{0};j < todo;++j)
+            {
+                const size_t byteOffset{nibbleOffset>>1};
+                const size_t byteShift{((nibbleOffset&1)^1) * 4};
+                const int nibble{(input[byteOffset]>>byteShift) & 15};
+                nibbleOffset += srcStep;
+
+                int pred{(sampleHistory[0]*MSADPCMAdaptionCoeff[blockpred][0] +
+                    sampleHistory[1]*MSADPCMAdaptionCoeff[blockpred][1]) / 256};
+                pred += ((nibble^0x08) - 0x08) * delta;
+                pred  = clampi(pred, -32768, 32767);
+
+                sampleHistory[1] = sampleHistory[0];
+                sampleHistory[0] = pred;
+                tempsamples[j] = pred;
+
+                delta = (MSADPCMAdaption[nibble] * delta) / 256;
+                delta = maxi(16, delta);
+            }
+
+            if(skip < todo) [[likely]]
+            {
+                const size_t towrite{minz(todo-skip, samples-wrote)};
+                for(size_t j{0};j < towrite;++j)
+                    dstSamples[wrote++] = static_cast<float>(tempsamples[j+skip]) / 32768.0f;
+                if(wrote == samples)
+                    return;
+                skip = 0;
+            }
+            else
+                skip -= todo;
+
+            i += todo;
+        }
+
+        src += blockBytes;
+    } while(true);
+}
+
 void LoadSamples(float *dstSamples, const al::byte *src, const size_t srcChan,
     const size_t srcOffset, const FmtType srcType, const size_t srcStep,
     const size_t samplesPerBlock, const size_t samples) noexcept
@@ -353,6 +458,7 @@ void LoadSamples(float *dstSamples, const al::byte *src, const size_t srcChan,
     HANDLE_FMT(FmtMulaw);
     HANDLE_FMT(FmtAlaw);
     HANDLE_FMT(FmtIMA4);
+    HANDLE_FMT(FmtMSADPCM);
     }
 #undef HANDLE_FMT
 }
-- 
cgit v1.2.3