Buffered DSP for neural-amp-modeler-lv2

BufferedDSP.hpp

/*
 * Buffered DSP
 * Copyright (C) 2022-2023 Filipe Coelho <falktx@falktx.com>
 * SPDX-License-Identifier: ISC
 */

#pragma once

#include "NAM/dsp.h"

#include <atomic>
#include <pthread.h>
#include <semaphore.h>
#include <unistd.h>

#if defined(__SSE2_MATH__)
# include <xmmintrin.h>
#endif

class BufferedDSP
{
    std::unordered_map<std::string, double>& namParams;
    DSP* activedsp = nullptr;
    float* bufferedInput = nullptr;
    float* bufferedOutput = nullptr;
    uint32_t bufferSize = 0;
    sem_t semBgProcStart = {};
    sem_t semBgProcFinished = {};
    std::atomic<bool> active{ false };

    pthread_mutex_t mutexI, mutexO;
    pthread_t thread = {};
    volatile bool running = false;

public:
    BufferedDSP(std::unordered_map<std::string, double>& namParams_)
        : namParams(namParams_)
    {
         sem_init(&semBgProcStart, 0, 0);
         sem_init(&semBgProcFinished, 0, 0);

         pthread_mutexattr_t attr;
         pthread_mutexattr_init(&attr);
         pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
         pthread_mutex_init(&mutexI, &attr);
         pthread_mutex_init(&mutexO, &attr);
         pthread_mutexattr_destroy(&attr);
    }

    ~BufferedDSP()
    {
        stop();

        pthread_mutex_destroy(&mutexI);
        pthread_mutex_destroy(&mutexO);
        sem_destroy(&semBgProcStart);
        sem_destroy(&semBgProcFinished);
        delete[] bufferedInput;
        delete[] bufferedOutput;
    }

    void setBufferSize(const uint32_t newBufferSize)
    {
        if (bufferSize == newBufferSize)
            return;

        const bool wasRunning = running;

        if (wasRunning)
            stop();

        bufferSize = newBufferSize;

        delete[] bufferedInput;
        delete[] bufferedOutput;

        bufferedInput = new float[newBufferSize];
        bufferedOutput = new float[newBufferSize];
        std::memset(bufferedOutput, 0, sizeof(float)*newBufferSize);

        if (wasRunning)
            start();
    }

    void start()
    {
        running = true;

        struct sched_param sched_param = {};
        sched_param.sched_priority = 80;

       #ifdef __MOD_DEVICES__
        int rtprio;
        const char* const srtprio = std::getenv("MOD_PLUGIN_THREAD_PRIORITY");
        if (srtprio != nullptr && (rtprio = std::atoi(srtprio)) > 0)
            sched_param.sched_priority = rtprio - 1;
       #endif

        pthread_attr_t attr;
        pthread_attr_init(&attr);
        pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);
        pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
        pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
        pthread_attr_setschedparam(&attr, &sched_param);

        if (pthread_create(&thread, &attr, _run, this) != 0)
        {
            pthread_attr_destroy(&attr);
            pthread_attr_init(&attr);
            pthread_create(&thread, &attr, _run, this);
        }

        pthread_attr_destroy(&attr);
    }

    void stop()
    {
        if (!running)
            return;

        running = false;
        sem_post(&semBgProcStart);
        pthread_join(thread, nullptr);
        thread = {};
    }

    void setDSP(DSP* const dsp)
    {
        activedsp = dsp;

        while (active.load())
            usleep(1000);
    }

    void process(float* const output, const uint32_t len)
    {
        if (len > bufferSize)
            return;

        sem_wait(&semBgProcFinished);

        pthread_mutex_lock(&mutexI);
        std::memcpy(bufferedInput, output, sizeof(float)*len);
        pthread_mutex_unlock(&mutexI);

        pthread_mutex_lock(&mutexO);
        std::memcpy(output, bufferedOutput, sizeof(float)*len);
        pthread_mutex_unlock(&mutexO);

        sem_post(&semBgProcStart);
    }

    static void* _run(void* const arg)
    {
        static_cast<BufferedDSP*>(arg)->run();
        return nullptr;
    }

    void run()
    {
        if (bufferSize == 0)
            return;

        float* tmp = new float[bufferSize];
        std::memset(tmp, 0, sizeof(float)*bufferSize);

        // disable denormals and enable flush to zero
        {
           #if defined(__SSE2_MATH__)
            _mm_setcsr(_mm_getcsr() | 0x8040);
           #elif defined(__aarch64__)
            uint64_t flags;
            __asm__ __volatile__("mrs %0, fpcr" : "=r" (flags));
            __asm__ __volatile__("msr fpcr, %0" :: "r" (flags | 0x1000000));
           #elif defined(__arm__) && !defined(__SOFTFP__)
            uint32_t flags;
            __asm__ __volatile__("vmrs %0, fpscr" : "=r" (flags));
            __asm__ __volatile__("vmsr fpscr, %0" :: "r" (flags | 0x1000000));
           #endif
        }

        while (running)
        {
            sem_post(&semBgProcFinished);
            sem_wait(&semBgProcStart);

            if (!running)
                break;

            pthread_mutex_lock(&mutexI);
            std::memcpy(tmp, bufferedInput, sizeof(float)*bufferSize);
            pthread_mutex_unlock(&mutexI);

            active.store(true);

            if (DSP* const dsp = activedsp)
            {
                dsp->process(tmp, bufferSize, namParams);
                dsp->finalize_(bufferSize);
            }

            active.store(false);

            pthread_mutex_lock(&mutexO);
            std::memcpy(bufferedOutput, tmp, sizeof(float)*bufferSize);
            pthread_mutex_unlock(&mutexO);
        }

        delete[] tmp;
    }
};