1 files changed, 554 insertions, 0 deletions

diff --git a/lib/rbcodec/dsp/dsp_core.c b/lib/rbcodec/dsp/dsp_core.c
new file mode 100644
index 0000000000..84fe64adb0
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_core.c
@@ -0,0 +1,554 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2005 Miika Pekkarinen
+ * Copyright (C) 2012 Michael Sevakis
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "config.h"
+#include "system.h"
+#include "dsp.h"
+#include "dsp_sample_io.h"
+#include <sys/types.h>
+
+/* Define LOGF_ENABLE to enable logf output in this file */
+/*#define LOGF_ENABLE*/
+#include "logf.h"
+
+/* Actually generate the database of stages */
+#define DSP_PROC_DB_CREATE
+#include "dsp_proc_entry.h"
+
+/* Linked lists give fewer loads in processing loop compared to some index
+ * list, which is more important than keeping occasionally executed code
+ * simple */
+
+struct dsp_config
+{
+    /** General DSP-local data **/
+    struct sample_io_data io_data; /* Sample input-output data (first) */
+    uint32_t slot_free_mask;       /* Mask of free slots for this DSP */
+    uint32_t proc_masks[2];        /* Mask of active/enabled stages */
+    struct dsp_proc_slot
+    {
+        struct dsp_proc_entry proc_entry; /* This enabled stage */
+        struct dsp_proc_slot *next[2]; /* [0]=active next, [1]=enabled next */
+        const struct dsp_proc_db_entry *db_entry;
+    } *proc_slots[2];              /* Pointer to first in list of
+                                      active/enabled stages */
+
+    /** Misc. extra stuff **/
+#ifdef CPU_COLDFIRE
+    unsigned long old_macsr;       /* Old macsr value to restore */
+#endif
+#if 0 /* Not needed now but enable if something must know this */
+    bool processing;               /* DSP is processing (to thwart inopportune
+                                      buffer moves) */
+#endif
+};
+
+/* Pool of slots for stages - supports 32 or fewer combined as-is atm. */
+static struct dsp_proc_slot
+dsp_proc_slot_arr[DSP_NUM_PROC_STAGES+DSP_VOICE_NUM_PROC_STAGES] IBSS_ATTR;
+
+/* General DSP config */
+static struct dsp_config dsp_conf[DSP_COUNT] IBSS_ATTR;
+
+/** Processing stages support functions **/
+
+/* Find the slot for a given enabled id */
+static struct dsp_proc_slot * find_proc_slot(struct dsp_config *dsp,
+                                             unsigned int id)
+{
+    const uint32_t mask = BIT_N(id);
+
+    if ((dsp->proc_masks[1] & mask) == 0)
+        return NULL; /* Not enabled */
+
+    struct dsp_proc_slot *s = dsp->proc_slots[1];
+
+    while (1) /* In proc_masks == it must be there */
+    {
+        if (BIT_N(s->db_entry->id) == mask)
+            return s;
+
+        s = s->next[1];
+    }
+}
+
+/* Broadcast to all enabled stages or to the one with the specifically
+ * crafted setting */
+static intptr_t proc_broadcast(struct dsp_config *dsp, unsigned int setting,
+                               intptr_t value)
+{
+    bool multi = setting < DSP_PROC_SETTING;
+    struct dsp_proc_slot *s = multi ?
+        dsp->proc_slots[1] : find_proc_slot(dsp, setting - DSP_PROC_SETTING);
+
+    while (s != NULL)
+    {
+        intptr_t ret = s->db_entry->configure(&s->proc_entry, dsp, setting,
+                                              value);
+        if (!multi)
+            return ret;
+
+        s = s->next[1];
+    }
+
+    return multi ? 1 : 0;
+}
+
+/* Generic handler for this->process[0] */
+static void dsp_process_null(struct dsp_proc_entry *this,
+                             struct dsp_buffer **buf_p)
+{
+    (void)this; (void)buf_p;
+}
+
+/* Generic handler for this->process[1] */
+static void dsp_format_change_process(struct dsp_proc_entry *this,
+                                      struct dsp_buffer **buf_p)
+{
+    enum dsp_proc_ids id =
+        TYPE_FROM_MEMBER(struct dsp_proc_slot, this, proc_entry)->db_entry->id;
+
+    DSP_PRINT_FORMAT(<Default Handler>, id, (*buf_p)->format);
+
+    /* We don't keep back references to the DSP, so just search for it */
+    struct dsp_config *dsp;
+    for (int i = 0; (dsp = dsp_get_config(i)); i++)
+    {
+        struct dsp_proc_slot *slot = find_proc_slot(dsp, id);
+        /* Found one with the id, check if it's this one */
+        if (&slot->proc_entry == this && dsp_proc_active(dsp, id))
+        {
+            dsp_proc_call(this, buf_p, 0);
+            break;
+        }
+    }
+}
+
+/* Add an item to the enabled list */
+static struct dsp_proc_slot *
+dsp_proc_enable_enlink(struct dsp_config *dsp, uint32_t mask)
+{
+    /* Use the lowest-indexed available slot */
+    int slot = find_first_set_bit(dsp->slot_free_mask);
+
+    if (slot == 32)
+    {
+        /* Should NOT happen, ever, unless called before init */
+        DEBUGF("DSP %d: no slots!\n", (int)dsp_get_id(dsp));
+        return NULL;
+    }
+
+    const struct dsp_proc_db_entry *db_entry_prev = NULL;
+    const struct dsp_proc_db_entry *db_entry;
+
+    /* Order of enabled list is same as DB array */
+    for (unsigned int i = 0;; i++)
+    {
+        if (i >= DSP_NUM_PROC_STAGES)
+            return NULL;
+
+        db_entry = dsp_proc_database[i];
+
+        uint32_t m = BIT_N(db_entry->id);
+
+        if (m == mask)
+            break; /* This is the one */
+
+        if (dsp->proc_masks[1] & m)
+            db_entry_prev = db_entry;
+    }
+
+    struct dsp_proc_slot *s = &dsp_proc_slot_arr[slot];
+
+    if (db_entry_prev != NULL)
+    {
+        struct dsp_proc_slot *prev = find_proc_slot(dsp, db_entry_prev->id);
+        s->next[0] = prev->next[0];
+        s->next[1] = prev->next[1];
+        prev->next[1] = s;
+    }
+    else
+    {
+        s->next[0] = dsp->proc_slots[0];
+        s->next[1] = dsp->proc_slots[1];
+        dsp->proc_slots[1] = s;
+    }
+
+    s->db_entry = db_entry; /* record DB entry */
+    dsp->proc_masks[1] |= mask;
+    dsp->slot_free_mask &= ~BIT_N(slot);
+
+    return s;
+}
+
+/* Remove an item from the enabled list */
+static struct dsp_proc_slot *
+dsp_proc_enable_delink(struct dsp_config *dsp, uint32_t mask)
+{
+    struct dsp_proc_slot *s = dsp->proc_slots[1];
+    struct dsp_proc_slot *prev = NULL;
+
+    while (1) /* In proc_masks == it must be there */
+    {
+        if (BIT_N(s->db_entry->id) == mask)
+        {
+            if (prev)
+                prev->next[1] = s->next[1];
+            else
+                dsp->proc_slots[1] = s->next[1];
+
+            dsp->proc_masks[1] &= ~mask;
+            dsp->slot_free_mask |= BIT_N(s - dsp_proc_slot_arr);
+            return s;
+        }
+
+        prev = s;
+        s = s->next[1];
+    }
+}
+
+void dsp_proc_enable(struct dsp_config *dsp, enum dsp_proc_ids id,
+                     bool enable)
+{
+    uint32_t mask = BIT_N(id);
+    bool enabled = dsp->proc_masks[1] & mask;
+
+    if (enable)
+    {
+        /* If enabled, just find it in list, if not, link a new one */
+        struct dsp_proc_slot *s = enabled ? find_proc_slot(dsp, id) :
+                                            dsp_proc_enable_enlink(dsp, mask);
+
+        if (s == NULL)
+        {
+            DEBUGF("DSP- proc id not valid: %d\n", (int)id);
+            return;
+        }
+
+        if (!enabled)
+        {
+            /* New entry - set defaults */
+            s->proc_entry.data = 0;
+            s->proc_entry.ip_mask = mask;
+            s->proc_entry.process[0] = dsp_process_null;
+            s->proc_entry.process[1] = dsp_format_change_process;
+        }
+
+        enabled = s->db_entry->configure(&s->proc_entry, dsp, DSP_PROC_INIT,
+                                         enabled) >= 0;
+        if (enabled)
+            return;
+
+        DEBUGF("DSP- proc init failed: %d\n", (int)id);
+        /* Cleanup below */
+    }
+    else if (!enabled)
+    {
+        return; /* No change */
+    }
+
+    dsp_proc_activate(dsp, id, false); /* Deactivate it first */
+    struct dsp_proc_slot *s = dsp_proc_enable_delink(dsp, mask);
+    s->db_entry->configure(&s->proc_entry, dsp, DSP_PROC_CLOSE, 0);
+}
+
+/* Maintain the list structure for the active list where each enabled entry
+ * has a link to the next active item, even if not active which facilitates
+ * switching out of format change mode by a stage during a format change.
+ * When that happens, the iterator must jump over inactive but enabled
+ * stages after its current position. */
+static struct dsp_proc_slot *
+dsp_proc_activate_link(struct dsp_config *dsp, uint32_t mask,
+                       struct dsp_proc_slot *s)
+{
+    uint32_t m = BIT_N(s->db_entry->id);
+    uint32_t mor = m | mask;
+
+    if (mor == m) /* Only if same single bit in common */
+    {
+        dsp->proc_masks[0] |= mask;
+        return s;
+    }
+    else if (~mor == 0) /* Only if bits complement */
+    {
+        dsp->proc_masks[0] &= mask;
+        return s->next[0];
+    }
+
+    struct dsp_proc_slot *next = s->next[1];
+    next = dsp_proc_activate_link(dsp, mask, next);
+
+    s->next[0] = next;
+
+    return (m & dsp->proc_masks[0]) ? s : next;
+}
+
+/* Activate or deactivate a stage */
+void dsp_proc_activate(struct dsp_config *dsp, enum dsp_proc_ids id,
+                       bool activate)
+{
+    const uint32_t mask = BIT_N(id);
+
+    if (!(dsp->proc_masks[1] & mask))
+        return; /* Not enabled */
+
+    if (activate != !(dsp->proc_masks[0] & mask))
+        return; /* No change in state */
+
+    /* Send mask bit if activating and ones complement if deactivating */
+    dsp->proc_slots[0] = dsp_proc_activate_link(
+            dsp, activate ? mask : ~mask, dsp->proc_slots[1]);
+}
+
+/* Is the stage specified by the id currently active? */
+bool dsp_proc_active(struct dsp_config *dsp, enum dsp_proc_ids id)
+{
+    return (dsp->proc_masks[0] & BIT_N(id)) != 0;
+}
+
+/* Determine by the rules if the processing function should be called */
+static FORCE_INLINE bool dsp_proc_should_call(struct dsp_proc_entry *this,
+                                              struct dsp_buffer *buf,
+                                              unsigned int fmt)
+{
+    uint32_t ip_mask = this->ip_mask;
+
+    return UNLIKELY(fmt != 0) || /* Also pass override value */
+           ip_mask == 0 || /* Not in-place */
+           ((ip_mask & buf->proc_mask) == 0 &&
+            (buf->proc_mask |= ip_mask, buf->remcount > 0));
+}
+
+/* Call this->process[fmt] according to the rules (for external call) */
+bool dsp_proc_call(struct dsp_proc_entry *this, struct dsp_buffer **buf_p,
+                   unsigned int fmt)
+{
+    if (dsp_proc_should_call(this, *buf_p, fmt))
+    {
+        this->process[fmt == (0u-1u) ? 0 : fmt](this, buf_p);
+        return true;
+    }
+
+    return false;
+}
+
+static inline void dsp_process_start(struct dsp_config *dsp)
+{
+#if defined(CPU_COLDFIRE)
+    /* set emac unit for dsp processing, and save old macsr, we're running in
+       codec thread context at this point, so can't clobber it */
+    dsp->old_macsr = coldfire_get_macsr();
+    coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
+#endif
+#if 0 /* Not needed now but enable if something must know this */
+    dsp->processing = true;
+#endif
+    (void)dsp;
+}
+
+static inline void dsp_process_end(struct dsp_config *dsp)
+{
+#if 0 /* Not needed now but enable if something must know this */
+    dsp->processing = false;
+#endif
+#if defined(CPU_COLDFIRE)
+    /* set old macsr again */
+    coldfire_set_macsr(dsp->old_macsr);
+#endif
+    (void)dsp;
+}
+
+/**
+ * dsp_process:
+ *
+ * Process and convert src audio to dst based on the DSP configuration.
+ * dsp:            the DSP instance in use
+ *
+ * src:
+ *     remcount  = number of input samples remaining; set to desired
+ *                 number of samples to be processed
+ *     pin[0]    = left channel if non-interleaved, audio data if
+ *                 interleaved or mono
+ *     pin[1]    = right channel if non-interleaved, ignored if
+ *                 interleaved or mono
+ *     proc_mask = set to zero on first call, updated by this function
+ *                 to keep track of which in-place stages have been
+ *                 run on the buffers to avoid multiple applications of
+ *                 them
+ *     format    = for internal buffers, gives the relevant format
+ *                 details
+ *
+ * dst:
+ *     remcount  = number of samples placed in buffer so far; set to
+ *                 zero on first call
+ *     p16out    = current fill pointer in destination buffer; set to
+ *                 buffer start on first call
+ *     bufcount  = remaining buffer space in samples; set to maximum
+ *                 desired output count on first call
+ *     format    = ignored
+ *
+ * Processing stops when src is exhausted or dst is filled, whichever
+ * happens first. Samples can still be output when src buffer is empty
+ * if samples are held internally. Generally speaking, continue calling
+ * until no data is consumed and no data is produced to purge the DSP
+ * to the maximum extent feasible. Some internal processing stages may
+ * require more input before more output can be generated, thus there
+ * is no guarantee the DSP is free of data awaiting processing at that
+ * point.
+ *
+ * Additionally, samples consumed and samples produced do not necessarily
+ * have a direct correlation. Samples may be consumed without producing
+ * any output and samples may be produced without consuming any input.
+ * It depends on which stages are actively processing data at the time
+ * of the call and how they function internally.
+ */
+void dsp_process(struct dsp_config *dsp, struct dsp_buffer *src,
+                 struct dsp_buffer *dst)
+{
+    if (dst->bufcount <= 0)
+    {
+        /* No place to put anything thus nothing may be safely consumed */
+        return;
+    }
+
+    /* At least perform one yield before starting */
+    long last_yield = current_tick;
+    yield();
+
+    dsp_process_start(dsp);
+
+    /* Tag input with codec-specified sample format */
+    src->format = dsp->io_data.format;
+
+    while (1)
+    {
+        /* Out-of-place-processing stages take the current buf as input
+         * and switch the buffer to their own output buffer */
+        struct dsp_buffer *buf = src;
+        unsigned int fmt = buf->format.changed;
+
+        /* Convert input samples to internal format */
+        dsp->io_data.input_samples[fmt](&dsp->io_data, &buf);
+        fmt = buf->format.changed;
+
+        struct dsp_proc_slot *s = dsp->proc_slots[fmt];
+
+        /* Call all active/enabled stages depending if format is
+           same/changed on the last output buffer */
+        while (s != NULL)
+        {
+            if (dsp_proc_should_call(&s->proc_entry, buf, fmt))
+            {
+                s->proc_entry.process[fmt](&s->proc_entry, &buf);
+                fmt = buf->format.changed;
+            }
+
+            /* The buffer may have changed along with the format flag */
+            s = s->next[fmt];
+        }
+
+        /* Don't overread/write src/destination */
+        int outcount = MIN(dst->bufcount, buf->remcount);
+
+        if (fmt == 0 && outcount <= 0)
+            break; /* Output full or purged internal buffers */
+
+        dsp->io_data.outcount = outcount;
+        dsp->io_data.output_samples[fmt](&dsp->io_data, buf, dst);
+
+        /* Advance buffers by what output consumed and produced */
+        dsp_advance_buffer32(buf, outcount);
+        dsp_advance_buffer_output(dst, outcount);
+
+        /* Yield at least once each tick */
+        long tick = current_tick;
+        if (TIME_AFTER(tick, last_yield))
+        {
+            last_yield = tick;
+            yield();
+        }
+    } /* while */
+
+    dsp_process_end(dsp);
+}
+
+intptr_t dsp_configure(struct dsp_config *dsp, unsigned int setting,
+                       intptr_t value)
+{
+    dsp_sample_io_configure(&dsp->io_data, setting, value);
+    return proc_broadcast(dsp, setting, value);
+}
+
+struct dsp_config * dsp_get_config(enum dsp_ids id)
+{
+    if (id >= DSP_COUNT)
+        return NULL;
+
+    return &dsp_conf[id];
+}
+
+/* Return the id given a dsp pointer (or even via something within
+   the struct itself) */
+enum dsp_ids dsp_get_id(const struct dsp_config *dsp)
+{
+    ptrdiff_t id = dsp - dsp_conf;
+
+    if (id < 0 || id >= DSP_COUNT)
+        return DSP_COUNT; /* obviously invalid */
+
+    return (enum dsp_ids)id;
+}
+
+#if 0 /* Not needed now but enable if something must know this */
+bool dsp_is_busy(const struct dsp_config *dsp)
+{
+    return dsp->processing;
+}
+#endif /* 0 */
+
+/* Do what needs initializing before enable/disable calls can be made.
+ * Must be done before changing settings for the first time. */
+void INIT_ATTR dsp_init(void)
+{
+    static const uint8_t slot_count[DSP_COUNT] /* INITDATA_ATTR */ =
+    {
+        [CODEC_IDX_AUDIO] = DSP_NUM_PROC_STAGES,
+        [CODEC_IDX_VOICE] = DSP_VOICE_NUM_PROC_STAGES
+    };
+
+    for (unsigned int i = 0, count, shift = 0;
+         i < DSP_COUNT;
+         i++, shift += count)
+    {
+        struct dsp_config *dsp = &dsp_conf[i];
+
+        count = slot_count[i];
+        dsp->slot_free_mask = MASK_N(uint32_t, count, shift);
+
+        dsp_sample_io_configure(&dsp->io_data, DSP_INIT, i);
+
+        /* Notify each db entry of global init for each DSP */
+        for (unsigned int j = 0; j < DSP_NUM_PROC_STAGES; j++)
+            dsp_proc_database[j]->configure(NULL, dsp, DSP_INIT, i);
+
+        dsp_configure(dsp, DSP_RESET, 0);
+    }
+}