summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorMichael Sevakis <jethead71@rockbox.org>2012-03-27 19:52:15 -0400
committerMichael Sevakis <jethead71@rockbox.org>2012-04-29 10:00:56 +0200
commitc9bcbe202d010234f76d1046880a790fe2c3a067 (patch)
tree2f15438664ad1b196d6ed8b78065cd050d351956
parentc9c13497736d8be077663f4458948f7bd526841b (diff)
downloadrockbox-c9bcbe202d010234f76d1046880a790fe2c3a067.tar.gz
rockbox-c9bcbe202d010234f76d1046880a790fe2c3a067.zip
Fundamentally rewrite much of the audio DSP.
Creates a standard buffer passing, local data passing and messaging system for processing stages. Stages can be moved to their own source files to reduce clutter and ease assimilation of new ones. dsp.c becomes dsp_core.c which supports an engine and framework for effects. Formats and change notifications are passed along with the buffer so that they arrive at the correct time at each stage in the chain regardless of the internal delays of a particular one. Removes restrictions on the number of samples that can be processed at a time and it pays attention to destination buffer size restrictions without having to limit input count, which also allows pcmbuf to remain fuller and safely set its own buffer limits as it sees fit. There is no longer a need to query input/output counts given a certain number of input samples; just give it the sizes of the source and destination buffers. Works in harmony with stages that are not deterministic in terms of sample input/output ratio (like both resamplers but most notably the timestretch). As a result it fixes quirks with timestretch hanging up with certain settings and it now operates properly throughout its full settings range. Change-Id: Ib206ec78f6f6c79259c5af9009fe021d68be9734 Reviewed-on: http://gerrit.rockbox.org/200 Reviewed-by: Michael Sevakis <jethead71@rockbox.org> Tested-by: Michael Sevakis <jethead71@rockbox.org>
Diffstat
-rw-r--r--apps/codec_thread.c56
-rw-r--r--apps/codecs.c2
-rw-r--r--apps/gui/bitmap/list-skinned.c2
-rw-r--r--apps/gui/bitmap/list.c2
-rw-r--r--apps/gui/option_select.c1
-rw-r--r--apps/main.c7
-rw-r--r--apps/menus/eq_menu.c12
-rw-r--r--apps/pcmbuf.c40
-rw-r--r--apps/plugin.c12
-rw-r--r--apps/plugin.h22
-rw-r--r--apps/plugins/SOURCES1
-rw-r--r--apps/plugins/mpegplayer/audio_thread.c56
-rw-r--r--apps/plugins/mpegplayer/mpeg_settings.c8
-rw-r--r--apps/plugins/test_codec.c51
-rw-r--r--apps/settings.c11
-rw-r--r--apps/settings.h18
-rw-r--r--apps/settings_list.c2
-rw-r--r--apps/voice_thread.c37
-rw-r--r--firmware/export/system.h16
-rw-r--r--lib/rbcodec/SOURCES21
-rw-r--r--lib/rbcodec/codecs/codecs.h6
-rw-r--r--lib/rbcodec/codecs/lib/codeclib.c14
-rw-r--r--lib/rbcodec/dsp/channel_mode.c264
-rw-r--r--lib/rbcodec/dsp/channel_mode.h27
-rw-r--r--lib/rbcodec/dsp/compressor.c70
-rw-r--r--lib/rbcodec/dsp/compressor.h14
-rw-r--r--lib/rbcodec/dsp/crossfeed.c214
-rw-r--r--lib/rbcodec/dsp/crossfeed.h28
-rw-r--r--lib/rbcodec/dsp/dsp.c1568
-rw-r--r--lib/rbcodec/dsp/dsp.h212
-rw-r--r--lib/rbcodec/dsp/dsp_arm.S621
-rw-r--r--lib/rbcodec/dsp/dsp_arm_v6.S32
-rw-r--r--lib/rbcodec/dsp/dsp_asm.h86
-rw-r--r--lib/rbcodec/dsp/dsp_cf.S502
-rw-r--r--lib/rbcodec/dsp/dsp_core.c554
-rw-r--r--lib/rbcodec/dsp/dsp_filter.c306
-rw-r--r--lib/rbcodec/dsp/dsp_filter.h57
-rw-r--r--lib/rbcodec/dsp/dsp_misc.c238
-rw-r--r--lib/rbcodec/dsp/dsp_misc.h50
-rw-r--r--lib/rbcodec/dsp/dsp_proc_database.h57
-rw-r--r--lib/rbcodec/dsp/dsp_proc_entry.h153
-rw-r--r--lib/rbcodec/dsp/dsp_proc_settings.h40
-rw-r--r--lib/rbcodec/dsp/dsp_sample_input.c334
-rw-r--r--lib/rbcodec/dsp/dsp_sample_io.h62
-rw-r--r--lib/rbcodec/dsp/dsp_sample_output.c214
-rw-r--r--lib/rbcodec/dsp/eq.c338
-rw-r--r--lib/rbcodec/dsp/eq.h38
-rw-r--r--lib/rbcodec/dsp/eq_arm.S89
-rw-r--r--lib/rbcodec/dsp/eq_cf.S91
-rw-r--r--lib/rbcodec/dsp/lin_resample.c281
-rw-r--r--lib/rbcodec/dsp/pga.c144
-rw-r--r--lib/rbcodec/dsp/pga.h40
-rw-r--r--lib/rbcodec/dsp/tdspeed.c468
-rw-r--r--lib/rbcodec/dsp/tdspeed.h22
-rw-r--r--lib/rbcodec/dsp/tone_controls.c118
-rw-r--r--lib/rbcodec/dsp/tone_controls.h28
56 files changed, 4791 insertions, 2966 deletions
diff --git a/apps/codec_thread.c b/apps/codec_thread.c
index 39db741054..17ca980e41 100644
--- a/apps/codec_thread.c
+++ b/apps/codec_thread.c
@@ -213,49 +213,41 @@ void codec_thread_do_callback(void (*fn)(void), unsigned int *id)
213static void codec_pcmbuf_insert_callback( 213static void codec_pcmbuf_insert_callback(
214 const void *ch1, const void *ch2, int count) 214 const void *ch1, const void *ch2, int count)
215{ 215{
216 const char *src[2] = { ch1, ch2 }; 216 struct dsp_buffer src;
217 217
218 while (count > 0) 218 src.remcount = count;
219 src.pin[0] = ch1;
220 src.pin[1] = ch2;
221 src.proc_mask = 0;
222
223 while (1)
219 { 224 {
220 int out_count = dsp_output_count(ci.dsp, count); 225 struct dsp_buffer dst;
221 int inp_count; 226 dst.remcount = 0;
222 char *dest; 227 dst.bufcount = MAX(src.remcount, 1024); /* Arbitrary min request */
223 228
224 while (1) 229 while ((dst.p16out = pcmbuf_request_buffer(&dst.bufcount)) == NULL)
225 { 230 {
226 if ((dest = pcmbuf_request_buffer(&out_count)) != NULL)
227 break;
228
229 cancel_cpu_boost(); 231 cancel_cpu_boost();
230 232
231 /* It will be awhile before space is available but we want 233 /* It may be awhile before space is available but we want
232 "instant" response to any message */ 234 "instant" response to any message */
233 queue_wait_w_tmo(&codec_queue, NULL, HZ/20); 235 queue_wait_w_tmo(&codec_queue, NULL, HZ/20);
234 236
235 if (!queue_empty(&codec_queue) && 237 if (!queue_empty(&codec_queue) &&
236 codec_check_queue__have_msg() < 0) 238 codec_check_queue__have_msg() < 0)
239 {
240 dsp_configure(ci.dsp, DSP_FLUSH, 0); /* Discontinuity */
237 return; 241 return;
242 }
238 } 243 }
239 244
240 /* Get the real input_size for output_size bytes, guarding 245 dsp_process(ci.dsp, &src, &dst);
241 * against resampling buffer overflows. */
242 inp_count = dsp_input_count(ci.dsp, out_count);
243
244 if (inp_count <= 0)
245 return;
246
247 /* Input size has grown, no error, just don't write more than length */
248 if (inp_count > count)
249 inp_count = count;
250 246
251 out_count = dsp_process(ci.dsp, dest, src, inp_count); 247 if (dst.remcount > 0)
252 248 pcmbuf_write_complete(dst.remcount, ci.id3->elapsed, ci.id3->offset);
253 if (out_count <= 0) 249 else if (src.remcount <= 0)
254 return; 250 break; /* No input remains and DSP purged */
255
256 pcmbuf_write_complete(out_count, ci.id3->elapsed, ci.id3->offset);
257
258 count -= inp_count;
259 } 251 }
260} 252}
261 253
@@ -352,10 +344,7 @@ static void codec_seek_complete_callback(void)
352 344
353static void codec_configure_callback(int setting, intptr_t value) 345static void codec_configure_callback(int setting, intptr_t value)
354{ 346{
355 if (!dsp_configure(ci.dsp, setting, value)) 347 dsp_configure(ci.dsp, setting, value);
356 {
357 logf("Illegal key: %d", setting);
358 }
359} 348}
360 349
361static enum codec_command_action 350static enum codec_command_action
@@ -611,8 +600,7 @@ static void NORETURN_ATTR codec_thread(void)
611void codec_thread_init(void) 600void codec_thread_init(void)
612{ 601{
613 /* Init API */ 602 /* Init API */
614 ci.dsp = (struct dsp_config *)dsp_configure(NULL, DSP_MYDSP, 603 ci.dsp = dsp_get_config(CODEC_IDX_AUDIO);
615 CODEC_IDX_AUDIO);
616 ci.codec_get_buffer = codec_get_buffer_callback; 604 ci.codec_get_buffer = codec_get_buffer_callback;
617 ci.pcmbuf_insert = codec_pcmbuf_insert_callback; 605 ci.pcmbuf_insert = codec_pcmbuf_insert_callback;
618 ci.set_elapsed = audio_codec_update_elapsed; 606 ci.set_elapsed = audio_codec_update_elapsed;
diff --git a/apps/codecs.c b/apps/codecs.c
index fafe4ac7a3..69204b7c4f 100644
--- a/apps/codecs.c
+++ b/apps/codecs.c
@@ -118,6 +118,7 @@ struct codec_api ci = {
118 118
119 commit_dcache, 119 commit_dcache,
120 commit_discard_dcache, 120 commit_discard_dcache,
121 commit_discard_idcache,
121 122
122 /* strings and memory */ 123 /* strings and memory */
123 strcpy, 124 strcpy,
@@ -166,7 +167,6 @@ struct codec_api ci = {
166 /* new stuff at the end, sort into place next time 167 /* new stuff at the end, sort into place next time
167 the API gets incompatible */ 168 the API gets incompatible */
168 169
169 commit_discard_idcache,
170}; 170};
171 171
172void codec_get_full_path(char *path, const char *codec_root_fn) 172void codec_get_full_path(char *path, const char *codec_root_fn)
diff --git a/apps/gui/bitmap/list-skinned.c b/apps/gui/bitmap/list-skinned.c
index 95430ae278..7d3620ed81 100644
--- a/apps/gui/bitmap/list-skinned.c
+++ b/apps/gui/bitmap/list-skinned.c
@@ -20,13 +20,13 @@
20 ****************************************************************************/ 20 ****************************************************************************/
21 21
22#include "config.h" 22#include "config.h"
23#include "system.h"
23#include "lcd.h" 24#include "lcd.h"
24#include "font.h" 25#include "font.h"
25#include "button.h" 26#include "button.h"
26#include "string.h" 27#include "string.h"
27#include "settings.h" 28#include "settings.h"
28#include "kernel.h" 29#include "kernel.h"
29#include "system.h"
30#include "file.h" 30#include "file.h"
31 31
32#include "action.h" 32#include "action.h"
diff --git a/apps/gui/bitmap/list.c b/apps/gui/bitmap/list.c
index 0581050091..49479c1cb3 100644
--- a/apps/gui/bitmap/list.c
+++ b/apps/gui/bitmap/list.c
@@ -22,13 +22,13 @@
22/* This file contains the code to draw the list widget on BITMAP LCDs. */ 22/* This file contains the code to draw the list widget on BITMAP LCDs. */
23 23
24#include "config.h" 24#include "config.h"
25#include "system.h"
25#include "lcd.h" 26#include "lcd.h"
26#include "font.h" 27#include "font.h"
27#include "button.h" 28#include "button.h"
28#include "string.h" 29#include "string.h"
29#include "settings.h" 30#include "settings.h"
30#include "kernel.h" 31#include "kernel.h"
31#include "system.h"
32#include "file.h" 32#include "file.h"
33 33
34#include "action.h" 34#include "action.h"
diff --git a/apps/gui/option_select.c b/apps/gui/option_select.c
index 7c3d34f024..ca206b86da 100644
--- a/apps/gui/option_select.c
+++ b/apps/gui/option_select.c
@@ -22,6 +22,7 @@
22#include <stdlib.h> 22#include <stdlib.h>
23#include "string-extra.h" 23#include "string-extra.h"
24#include "config.h" 24#include "config.h"
25#include "system.h"
25#include "option_select.h" 26#include "option_select.h"
26#include "kernel.h" 27#include "kernel.h"
27#include "lang.h" 28#include "lang.h"
diff --git a/apps/main.c b/apps/main.c
index 631236852e..0fff0846a6 100644
--- a/apps/main.c
+++ b/apps/main.c
@@ -384,6 +384,9 @@ static void init(void)
384 viewportmanager_init(); 384 viewportmanager_init();
385 385
386 storage_init(); 386 storage_init();
387#if CONFIG_CODEC == SWCODEC
388 dsp_init();
389#endif
387 settings_reset(); 390 settings_reset();
388 settings_load(SETTINGS_ALL); 391 settings_load(SETTINGS_ALL);
389 settings_apply(true); 392 settings_apply(true);
@@ -632,6 +635,10 @@ static void init(void)
632 } 635 }
633 } 636 }
634 637
638#if CONFIG_CODEC == SWCODEC
639 dsp_init();
640#endif
641
635#if defined(SETTINGS_RESET) || (CONFIG_KEYPAD == IPOD_4G_PAD) || \ 642#if defined(SETTINGS_RESET) || (CONFIG_KEYPAD == IPOD_4G_PAD) || \
636 (CONFIG_KEYPAD == IRIVER_H10_PAD) 643 (CONFIG_KEYPAD == IRIVER_H10_PAD)
637#ifdef SETTINGS_RESET 644#ifdef SETTINGS_RESET
diff --git a/apps/menus/eq_menu.c b/apps/menus/eq_menu.c
index 2cfb80f76a..60b2687050 100644
--- a/apps/menus/eq_menu.c
+++ b/apps/menus/eq_menu.c
@@ -70,13 +70,11 @@ const char* eq_precut_format(char* buffer, size_t buffer_size, int value, const
70 */ 70 */
71static void eq_apply(void) 71static void eq_apply(void)
72{ 72{
73 dsp_set_eq(global_settings.eq_enabled); 73 dsp_eq_enable(global_settings.eq_enabled);
74 dsp_set_eq_precut(global_settings.eq_precut); 74 dsp_set_eq_precut(global_settings.eq_precut);
75 /* Update all bands */ 75 /* Update all bands */
76 for(int i = 0; i < 5; i++) { 76 for(int i = 0; i < EQ_NUM_BANDS; i++) {
77 dsp_set_eq_coefs(i, global_settings.eq_band_settings[i].cutoff, 77 dsp_set_eq_coefs(i, &global_settings.eq_band_settings[i]);
78 global_settings.eq_band_settings[i].q,
79 global_settings.eq_band_settings[i].gain);
80 } 78 }
81} 79}
82 80
@@ -580,9 +578,7 @@ bool eq_menu_graphical(void)
580 /* Update the filter if the user changed something */ 578 /* Update the filter if the user changed something */
581 if (has_changed) { 579 if (has_changed) {
582 dsp_set_eq_coefs(current_band, 580 dsp_set_eq_coefs(current_band,
583 global_settings.eq_band_settings[current_band].cutoff, 581 &global_settings.eq_band_settings[current_band]);
584 global_settings.eq_band_settings[current_band].q,
585 global_settings.eq_band_settings[current_band].gain);
586 has_changed = false; 582 has_changed = false;
587 } 583 }
588 } 584 }
diff --git a/apps/pcmbuf.c b/apps/pcmbuf.c
index d36883fc5b..9cedae0b67 100644
--- a/apps/pcmbuf.c
+++ b/apps/pcmbuf.c
@@ -47,16 +47,23 @@
47 smaller math - must be < 65536 bytes */ 47 smaller math - must be < 65536 bytes */
48#define PCMBUF_CHUNK_SIZE 8192u 48#define PCMBUF_CHUNK_SIZE 8192u
49 49
50/* Massive size is a nasty temp fix */ 50/* Small guard buf to give decent space near end */
51#define PCMBUF_GUARD_SIZE (1024u*12*((NATIVE_FREQUENCY+7999)/8000)) 51#define PCMBUF_GUARD_SIZE (PCMBUF_CHUNK_SIZE / 8)
52 52
53/* Mnemonics for common data commit thresholds */ 53/* Mnemonics for common data commit thresholds */
54#define COMMIT_CHUNKS PCMBUF_CHUNK_SIZE 54#define COMMIT_CHUNKS PCMBUF_CHUNK_SIZE
55#define COMMIT_ALL_DATA 1u 55#define COMMIT_ALL_DATA 1u
56 56
57 /* Size of the crossfade buffer where codec data is written to be faded 57/* Size of the crossfade buffer where codec data is written to be faded
58 on commit */ 58 on commit */
59#define CROSSFADE_BUFSIZE 8192u 59#define CROSSFADE_BUFSIZE PCMBUF_CHUNK_SIZE
60
61/* Maximum contiguous space that PCM buffer will allow (to avoid excessive
62 draining between inserts and observe low-latency mode) */
63#define PCMBUF_MAX_BUFFER (PCMBUF_CHUNK_SIZE * 4)
64
65/* Forced buffer insert constraint can thus be from 1KB to 32KB using 8KB
66 chunks */
60 67
61/* Return data level in 1/4-second increments */ 68/* Return data level in 1/4-second increments */
62#define DATA_LEVEL(quarter_secs) (NATIVE_FREQUENCY * (quarter_secs)) 69#define DATA_LEVEL(quarter_secs) (NATIVE_FREQUENCY * (quarter_secs))
@@ -383,7 +390,11 @@ void * pcmbuf_request_buffer(int *count)
383 /* If crossfade has begun, put the new track samples in crossfade_buffer */ 390 /* If crossfade has begun, put the new track samples in crossfade_buffer */
384 if (crossfade_status != CROSSFADE_INACTIVE && size > CROSSFADE_BUFSIZE) 391 if (crossfade_status != CROSSFADE_INACTIVE && size > CROSSFADE_BUFSIZE)
385 size = CROSSFADE_BUFSIZE; 392 size = CROSSFADE_BUFSIZE;
386#endif 393 else
394#endif /* HAVE_CROSSFADE */
395
396 if (size > PCMBUF_MAX_BUFFER)
397 size = PCMBUF_MAX_BUFFER; /* constrain */
387 398
388 enum channel_status status = mixer_channel_status(PCM_MIXER_CHAN_PLAYBACK); 399 enum channel_status status = mixer_channel_status(PCM_MIXER_CHAN_PLAYBACK);
389 size_t remaining = pcmbuf_unplayed_bytes(); 400 size_t remaining = pcmbuf_unplayed_bytes();
@@ -432,11 +443,22 @@ void * pcmbuf_request_buffer(int *count)
432 pcmbuf_play_start(); 443 pcmbuf_play_start();
433 } 444 }
434 445
435 void *buf = 446 void *buf;
447
436#ifdef HAVE_CROSSFADE 448#ifdef HAVE_CROSSFADE
437 crossfade_status != CROSSFADE_INACTIVE ? crossfade_buffer : 449 if (crossfade_status != CROSSFADE_INACTIVE)
450 {
451 buf = crossfade_buffer; /* always CROSSFADE_BUFSIZE */
452 }
453 else
438#endif 454#endif
439 get_write_buffer(&size); 455 {
456 /* Give the maximum amount available if there's more */
457 if (size + PCMBUF_CHUNK_SIZE < freespace)
458 size = freespace - PCMBUF_CHUNK_SIZE;
459
460 buf = get_write_buffer(&size);
461 }
440 462
441 *count = size / 4; 463 *count = size / 4;
442 return buf; 464 return buf;
diff --git a/apps/plugin.c b/apps/plugin.c
index 5406610e23..129fd6954e 100644
--- a/apps/plugin.c
+++ b/apps/plugin.c
@@ -565,13 +565,15 @@ static const struct plugin_api rockbox_api = {
565 audio_set_output_source, 565 audio_set_output_source,
566 audio_set_input_source, 566 audio_set_input_source,
567#endif 567#endif
568 dsp_set_crossfeed, 568 dsp_crossfeed_enable,
569 dsp_set_eq, 569 dsp_eq_enable,
570 dsp_dither_enable, 570 dsp_dither_enable,
571#ifdef HAVE_PITCHSCREEN
572 dsp_set_timestretch,
573#endif
571 dsp_configure, 574 dsp_configure,
575 dsp_get_config,
572 dsp_process, 576 dsp_process,
573 dsp_input_count,
574 dsp_output_count,
575 577
576 mixer_channel_status, 578 mixer_channel_status,
577 mixer_channel_get_buffer, 579 mixer_channel_get_buffer,
@@ -584,7 +586,7 @@ static const struct plugin_api rockbox_api = {
584 586
585 system_sound_play, 587 system_sound_play,
586 keyclick_click, 588 keyclick_click,
587#endif 589#endif /* CONFIG_CODEC == SWCODEC */
588 /* playback control */ 590 /* playback control */
589 playlist_amount, 591 playlist_amount,
590 playlist_resume, 592 playlist_resume,
diff --git a/apps/plugin.h b/apps/plugin.h
index 8123414ff0..3820c7ede6 100644
--- a/apps/plugin.h
+++ b/apps/plugin.h
@@ -153,12 +153,12 @@ void* plugin_get_buffer(size_t *buffer_size);
153#define PLUGIN_MAGIC 0x526F634B /* RocK */ 153#define PLUGIN_MAGIC 0x526F634B /* RocK */
154 154
155/* increase this every time the api struct changes */ 155/* increase this every time the api struct changes */
156#define PLUGIN_API_VERSION 218 156#define PLUGIN_API_VERSION 219
157 157
158/* update this to latest version if a change to the api struct breaks 158/* update this to latest version if a change to the api struct breaks
159 backwards compatibility (and please take the opportunity to sort in any 159 backwards compatibility (and please take the opportunity to sort in any
160 new function which are "waiting" at the end of the function table) */ 160 new function which are "waiting" at the end of the function table) */
161#define PLUGIN_MIN_API_VERSION 218 161#define PLUGIN_MIN_API_VERSION 219
162 162
163/* plugin return codes */ 163/* plugin return codes */
164/* internal returns start at 0x100 to make exit(1..255) work */ 164/* internal returns start at 0x100 to make exit(1..255) work */
@@ -680,15 +680,17 @@ struct plugin_api {
680 void (*audio_set_output_source)(int monitor); 680 void (*audio_set_output_source)(int monitor);
681 void (*audio_set_input_source)(int source, unsigned flags); 681 void (*audio_set_input_source)(int source, unsigned flags);
682#endif 682#endif
683 void (*dsp_set_crossfeed)(bool enable); 683 void (*dsp_crossfeed_enable)(bool enable);
684 void (*dsp_set_eq)(bool enable); 684 void (*dsp_eq_enable)(bool enable);
685 void (*dsp_dither_enable)(bool enable); 685 void (*dsp_dither_enable)(bool enable);
686 intptr_t (*dsp_configure)(struct dsp_config *dsp, int setting, 686#ifdef HAVE_PITCHSCREEN
687 intptr_t value); 687 void (*dsp_set_timestretch)(int32_t percent);
688 int (*dsp_process)(struct dsp_config *dsp, char *dest, 688#endif
689 const char *src[], int count); 689 intptr_t (*dsp_configure)(struct dsp_config *dsp,
690 int (*dsp_input_count)(struct dsp_config *dsp, int count); 690 unsigned int setting, intptr_t value);
691 int (*dsp_output_count)(struct dsp_config *dsp, int count); 691 struct dsp_config * (*dsp_get_config)(enum dsp_ids id);
692 void (*dsp_process)(struct dsp_config *dsp, struct dsp_buffer *src,
693 struct dsp_buffer *dst);
692 694
693 enum channel_status (*mixer_channel_status)(enum pcm_mixer_channel channel); 695 enum channel_status (*mixer_channel_status)(enum pcm_mixer_channel channel);
694 const void * (*mixer_channel_get_buffer)(enum pcm_mixer_channel channel, 696 const void * (*mixer_channel_get_buffer)(enum pcm_mixer_channel channel,
diff --git a/apps/plugins/SOURCES b/apps/plugins/SOURCES
index db690a638a..e5f026c5b4 100644
--- a/apps/plugins/SOURCES
+++ b/apps/plugins/SOURCES
@@ -33,6 +33,7 @@ flipit.c
33shopper.c 33shopper.c
34resistor.c 34resistor.c
35 35
36test_codec.c
36 37
37#ifdef USB_ENABLE_HID 38#ifdef USB_ENABLE_HID
38remote_control.c 39remote_control.c
diff --git a/apps/plugins/mpegplayer/audio_thread.c b/apps/plugins/mpegplayer/audio_thread.c
index f976fd6007..b06727f759 100644
--- a/apps/plugins/mpegplayer/audio_thread.c
+++ b/apps/plugins/mpegplayer/audio_thread.c
@@ -36,6 +36,7 @@ struct audio_thread_data
36 unsigned samplerate; /* Current stream sample rate */ 36 unsigned samplerate; /* Current stream sample rate */
37 int nchannels; /* Number of audio channels */ 37 int nchannels; /* Number of audio channels */
38 struct dsp_config *dsp; /* The DSP we're using */ 38 struct dsp_config *dsp; /* The DSP we're using */
39 struct dsp_buffer src; /* Current audio data for DSP processing */
39}; 40};
40 41
41/* The audio thread is stolen from the core codec thread */ 42/* The audio thread is stolen from the core codec thread */
@@ -479,12 +480,13 @@ static void audio_thread(void)
479 /* We need this here to init the EMAC for Coldfire targets */ 480 /* We need this here to init the EMAC for Coldfire targets */
480 init_mad(); 481 init_mad();
481 482
482 td.dsp = (struct dsp_config *)rb->dsp_configure(NULL, DSP_MYDSP, 483 td.dsp = rb->dsp_get_config(CODEC_IDX_AUDIO);
483 CODEC_IDX_AUDIO);
484#ifdef HAVE_PITCHSCREEN 484#ifdef HAVE_PITCHSCREEN
485 rb->sound_set_pitch(PITCH_SPEED_100); 485 rb->sound_set_pitch(PITCH_SPEED_100);
486 rb->dsp_set_timestretch(PITCH_SPEED_100);
486#endif 487#endif
487 rb->dsp_configure(td.dsp, DSP_RESET, 0); 488 rb->dsp_configure(td.dsp, DSP_RESET, 0);
489 rb->dsp_configure(td.dsp, DSP_FLUSH, 0);
488 rb->dsp_configure(td.dsp, DSP_SET_SAMPLE_DEPTH, MAD_F_FRACBITS); 490 rb->dsp_configure(td.dsp, DSP_SET_SAMPLE_DEPTH, MAD_F_FRACBITS);
489 491
490 goto message_wait; 492 goto message_wait;
@@ -631,43 +633,53 @@ static void audio_thread(void)
631 STEREO_MONO : STEREO_NONINTERLEAVED); 633 STEREO_MONO : STEREO_NONINTERLEAVED);
632 } 634 }
633 635
636 td.src.remcount = synth.pcm.length;
637 td.src.pin[0] = synth.pcm.samples[0];
638 td.src.pin[1] = synth.pcm.samples[1];
639 td.src.proc_mask = 0;
640
634 td.state = TSTATE_RENDER_WAIT; 641 td.state = TSTATE_RENDER_WAIT;
635 642
636 /* Add a frame of audio to the pcm buffer. Maximum is 1152 samples. */ 643 /* Add a frame of audio to the pcm buffer. Maximum is 1152 samples. */
637 render_wait: 644 render_wait:
638 if (synth.pcm.length > 0) 645 rb->yield();
646
647 while (1)
639 { 648 {
640 const char *src[2] = 649 struct dsp_buffer dst;
641 { (char *)synth.pcm.samples[0], (char *)synth.pcm.samples[1] }; 650 dst.remcount = 0;
642 int out_count = (synth.pcm.length * CLOCK_RATE 651 dst.bufcount = MAX(td.src.remcount, 1024);
643 + (td.samplerate - 1)) / td.samplerate; 652
644 unsigned char *out_buf; 653 ssize_t size = dst.bufcount * 2 * sizeof(int16_t);
645 ssize_t size = out_count*4;
646 654
647 /* Wait for required amount of free buffer space */ 655 /* Wait for required amount of free buffer space */
648 while ((out_buf = pcm_output_get_buffer(&size)) == NULL) 656 while ((dst.p16out = pcm_output_get_buffer(&size)) == NULL)
649 { 657 {
650 /* Wait one frame */ 658 /* Wait one frame */
651 int timeout = out_count*HZ / td.samplerate; 659 int timeout = dst.bufcount*HZ / td.samplerate;
652 str_get_msg_w_tmo(&audio_str, &td.ev, MAX(timeout, 1)); 660 str_get_msg_w_tmo(&audio_str, &td.ev, MAX(timeout, 1));
653 if (td.ev.id != SYS_TIMEOUT) 661 if (td.ev.id != SYS_TIMEOUT)
654 goto message_process; 662 goto message_process;
655 } 663 }
656 664
657 out_count = rb->dsp_process(td.dsp, out_buf, src, synth.pcm.length); 665 dst.bufcount = size / (2 * sizeof (int16_t));
666 rb->dsp_process(td.dsp, &td.src, &dst);
658 667
659 if (out_count <= 0) 668 if (dst.remcount > 0)
660 break; 669 {
661 670 /* Make this data available to DMA */
662 /* Make this data available to DMA */ 671 pcm_output_commit_data(dst.remcount * 2 * sizeof(int16_t),
663 pcm_output_commit_data(out_count*4, audio_queue.curr->time); 672 audio_queue.curr->time);
664 673
665 /* As long as we're on this timestamp, the time is just 674 /* As long as we're on this timestamp, the time is just
666 incremented by the number of samples */ 675 incremented by the number of samples */
667 audio_queue.curr->time += out_count; 676 audio_queue.curr->time += dst.remcount;
677 }
678 else if (td.src.remcount <= 0)
679 {
680 break;
681 }
668 } 682 }
669
670 rb->yield();
671 } /* end decoding loop */ 683 } /* end decoding loop */
672} 684}
673 685
diff --git a/apps/plugins/mpegplayer/mpeg_settings.c b/apps/plugins/mpegplayer/mpeg_settings.c
index 1c3f3c0b92..7f92fb7c69 100644
--- a/apps/plugins/mpegplayer/mpeg_settings.c
+++ b/apps/plugins/mpegplayer/mpeg_settings.c
@@ -457,13 +457,13 @@ static void sync_audio_setting(int setting, bool global)
457 break; 457 break;
458 458
459 case MPEG_AUDIO_CROSSFEED: 459 case MPEG_AUDIO_CROSSFEED:
460 rb->dsp_set_crossfeed((global || settings.crossfeed) ? 460 rb->dsp_crossfeed_enable((global || settings.crossfeed) ?
461 rb->global_settings->crossfeed : false); 461 rb->global_settings->crossfeed : false);
462 break; 462 break;
463 463
464 case MPEG_AUDIO_EQUALIZER: 464 case MPEG_AUDIO_EQUALIZER:
465 rb->dsp_set_eq((global || settings.equalizer) ? 465 rb->dsp_eq_enable((global || settings.equalizer) ?
466 rb->global_settings->eq_enabled : false); 466 rb->global_settings->eq_enabled : false);
467 break; 467 break;
468 468
469 case MPEG_AUDIO_DITHERING: 469 case MPEG_AUDIO_DITHERING:
diff --git a/apps/plugins/test_codec.c b/apps/plugins/test_codec.c
index dafcf35710..920be54d56 100644
--- a/apps/plugins/test_codec.c
+++ b/apps/plugins/test_codec.c
@@ -164,6 +164,7 @@ static inline void int2le16(unsigned char* buf, int16_t x)
164 164
165static unsigned char *wavbuffer; 165static unsigned char *wavbuffer;
166static unsigned char *dspbuffer; 166static unsigned char *dspbuffer;
167static int dspbuffer_count;
167 168
168void init_wav(char* filename) 169void init_wav(char* filename)
169{ 170{
@@ -215,34 +216,31 @@ static void* codec_get_buffer(size_t *size)
215 216
216static int process_dsp(const void *ch1, const void *ch2, int count) 217static int process_dsp(const void *ch1, const void *ch2, int count)
217{ 218{
218 const char *src[2] = { ch1, ch2 }; 219 struct dsp_buffer src;
219 int written_count = 0; 220 src.remcount = count;
220 char *dest = dspbuffer; 221 src.pin[0] = ch1;
221 222 src.pin[1] = ch2;
222 while (count > 0) 223 src.proc_mask = 0;
224
225 struct dsp_buffer dst;
226 dst.remcount = 0;
227 dst.p16out = (int16_t *)dspbuffer;
228 dst.bufcount = dspbuffer_count;
229
230 while (1)
223 { 231 {
224 int out_count = rb->dsp_output_count(ci.dsp, count); 232 int old_remcount = dst.remcount;
225 233 rb->dsp_process(ci.dsp, &src, &dst);
226 int inp_count = rb->dsp_input_count(ci.dsp, out_count);
227
228 if (inp_count <= 0)
229 break;
230
231 if (inp_count > count)
232 inp_count = count;
233
234 out_count = rb->dsp_process(ci.dsp, dest, src, inp_count);
235 234
236 if (out_count <= 0) 235 if (dst.bufcount <= 0 ||
236 (src.remcount <= 0 && dst.remcount <= old_remcount))
237 {
238 /* Dest is full or no input left and DSP purged */
237 break; 239 break;
238 240 }
239 written_count += out_count;
240 dest += out_count * 4;
241
242 count -= inp_count;
243 } 241 }
244 242
245 return written_count; 243 return dst.remcount;
246} 244}
247 245
248/* Null output */ 246/* Null output */
@@ -502,7 +500,6 @@ static void configure(int setting, intptr_t value)
502 rb->dsp_configure(ci.dsp, setting, value); 500 rb->dsp_configure(ci.dsp, setting, value);
503 switch(setting) 501 switch(setting)
504 { 502 {
505 case DSP_SWITCH_FREQUENCY:
506 case DSP_SET_FREQUENCY: 503 case DSP_SET_FREQUENCY:
507 DEBUGF("samplerate=%d\n",(int)value); 504 DEBUGF("samplerate=%d\n",(int)value);
508 wavinfo.samplerate = use_dsp ? NATIVE_FREQUENCY : (int)value; 505 wavinfo.samplerate = use_dsp ? NATIVE_FREQUENCY : (int)value;
@@ -525,9 +522,7 @@ static void init_ci(void)
525{ 522{
526 /* --- Our "fake" implementations of the codec API functions. --- */ 523 /* --- Our "fake" implementations of the codec API functions. --- */
527 524
528 ci.dsp = (struct dsp_config *)rb->dsp_configure(NULL, DSP_MYDSP, 525 ci.dsp = rb->dsp_get_config(CODEC_IDX_AUDIO);
529 CODEC_IDX_AUDIO);
530
531 ci.codec_get_buffer = codec_get_buffer; 526 ci.codec_get_buffer = codec_get_buffer;
532 527
533 if (wavinfo.fd >= 0 || checksum) { 528 if (wavinfo.fd >= 0 || checksum) {
@@ -849,6 +844,8 @@ enum plugin_status plugin_start(const void* parameter)
849 844
850 wavbuffer = rb->plugin_get_buffer(&buffer_size); 845 wavbuffer = rb->plugin_get_buffer(&buffer_size);
851 dspbuffer = wavbuffer + buffer_size / 2; 846 dspbuffer = wavbuffer + buffer_size / 2;
847 dspbuffer_count = (buffer_size - (dspbuffer - wavbuffer)) /
848 (2 * sizeof (int16_t));
852 849
853 codec_mallocbuf = rb->plugin_get_audio_buffer(&audiosize); 850 codec_mallocbuf = rb->plugin_get_audio_buffer(&audiosize);
854 /* Align codec_mallocbuf to pointer size, tlsf wants that */ 851 /* Align codec_mallocbuf to pointer size, tlsf wants that */
diff --git a/apps/settings.c b/apps/settings.c
index acc38c2388..49d239a2c1 100644
--- a/apps/settings.c
+++ b/apps/settings.c
@@ -979,20 +979,17 @@ void settings_apply(bool read_disk)
979 audio_set_crossfade(global_settings.crossfade); 979 audio_set_crossfade(global_settings.crossfade);
980#endif 980#endif
981 dsp_set_replaygain(); 981 dsp_set_replaygain();
982 dsp_set_crossfeed(global_settings.crossfeed); 982 dsp_crossfeed_enable(global_settings.crossfeed);
983 dsp_set_crossfeed_direct_gain(global_settings.crossfeed_direct_gain); 983 dsp_set_crossfeed_direct_gain(global_settings.crossfeed_direct_gain);
984 dsp_set_crossfeed_cross_params(global_settings.crossfeed_cross_gain, 984 dsp_set_crossfeed_cross_params(global_settings.crossfeed_cross_gain,
985 global_settings.crossfeed_hf_attenuation, 985 global_settings.crossfeed_hf_attenuation,
986 global_settings.crossfeed_hf_cutoff); 986 global_settings.crossfeed_hf_cutoff);
987 987
988 /* Configure software equalizer, hardware eq is handled in audio_init() */ 988 /* Configure software equalizer, hardware eq is handled in audio_init() */
989 dsp_set_eq(global_settings.eq_enabled); 989 dsp_eq_enable(global_settings.eq_enabled);
990 dsp_set_eq_precut(global_settings.eq_precut); 990 dsp_set_eq_precut(global_settings.eq_precut);
991 991 for(int i = 0; i < EQ_NUM_BANDS; i++) {
992 for(int i = 0; i < 5; i++) { 992 dsp_set_eq_coefs(i, &global_settings.eq_band_settings[i]);
993 dsp_set_eq_coefs(i, global_settings.eq_band_settings[i].cutoff,
994 global_settings.eq_band_settings[i].q,
995 global_settings.eq_band_settings[i].gain);
996 } 993 }
997 994
998 dsp_dither_enable(global_settings.dithering_enabled); 995 dsp_dither_enable(global_settings.dithering_enabled);
diff --git a/apps/settings.h b/apps/settings.h
index b312c1e784..4d94ca8ba8 100644
--- a/apps/settings.h
+++ b/apps/settings.h
@@ -32,6 +32,7 @@
32#include "button.h" 32#include "button.h"
33#if CONFIG_CODEC == SWCODEC 33#if CONFIG_CODEC == SWCODEC
34#include "audio.h" 34#include "audio.h"
35#include "dsp.h"
35#endif 36#endif
36#include "rbpaths.h" 37#include "rbpaths.h"
37 38
@@ -339,13 +340,7 @@ struct user_settings
339 /* EQ */ 340 /* EQ */
340 bool eq_enabled; /* Enable equalizer */ 341 bool eq_enabled; /* Enable equalizer */
341 unsigned int eq_precut; /* dB */ 342 unsigned int eq_precut; /* dB */
342 343 struct eq_band_setting eq_band_settings[EQ_NUM_BANDS]; /* for each band */
343 struct eq_band_setting
344 {
345 int cutoff; /* Hz */
346 int q;
347 int gain; /* +/- dB */
348 } eq_band_settings[5];
349 344
350 /* Misc. swcodec */ 345 /* Misc. swcodec */
351 int beep; /* system beep volume when changing tracks etc. */ 346 int beep; /* system beep volume when changing tracks etc. */
@@ -772,14 +767,7 @@ struct user_settings
772#endif 767#endif
773 768
774#if CONFIG_CODEC == SWCODEC 769#if CONFIG_CODEC == SWCODEC
775 struct compressor_settings 770 struct compressor_settings compressor_settings;
776 {
777 int threshold;
778 int makeup_gain;
779 int ratio;
780 int knee;
781 int release_time;
782 } compressor_settings;
783#endif 771#endif
784 772
785 int sleeptimer_duration; /* In minutes; 0=off */ 773 int sleeptimer_duration; /* In minutes; 0=off */
diff --git a/apps/settings_list.c b/apps/settings_list.c
index af48d11c85..82cccd891f 100644
--- a/apps/settings_list.c
+++ b/apps/settings_list.c
@@ -1398,7 +1398,7 @@ const struct settings_list settings[] = {
1398 1398
1399 /* crossfeed */ 1399 /* crossfeed */
1400 OFFON_SETTING(F_SOUNDSETTING, crossfeed, LANG_CROSSFEED, false, 1400 OFFON_SETTING(F_SOUNDSETTING, crossfeed, LANG_CROSSFEED, false,
1401 "crossfeed", dsp_set_crossfeed), 1401 "crossfeed", dsp_crossfeed_enable),
1402 INT_SETTING_NOWRAP(F_SOUNDSETTING, crossfeed_direct_gain, 1402 INT_SETTING_NOWRAP(F_SOUNDSETTING, crossfeed_direct_gain,
1403 LANG_CROSSFEED_DIRECT_GAIN, -15, 1403 LANG_CROSSFEED_DIRECT_GAIN, -15,
1404 "crossfeed direct gain", UNIT_DB, -60, 0, 5, 1404 "crossfeed direct gain", UNIT_DB, -60, 0, 5,
diff --git a/apps/voice_thread.c b/apps/voice_thread.c
index 07a67256c4..cff703adfa 100644
--- a/apps/voice_thread.c
+++ b/apps/voice_thread.c
@@ -133,9 +133,8 @@ struct voice_thread_data
133 SpeexBits bits; /* Bit cursor */ 133 SpeexBits bits; /* Bit cursor */
134 struct dsp_config *dsp; /* DSP used for voice output */ 134 struct dsp_config *dsp; /* DSP used for voice output */
135 struct voice_info vi; /* Copy of clip data */ 135 struct voice_info vi; /* Copy of clip data */
136 const char *src[2]; /* Current output buffer pointers */
137 int lookahead; /* Number of samples to drop at start of clip */ 136 int lookahead; /* Number of samples to drop at start of clip */
138 int count; /* Count of samples remaining to send to PCM */ 137 struct dsp_buffer src; /* Speex output buffer/input to DSP */
139}; 138};
140 139
141/* Functions called in their repective state that return the next state to 140/* Functions called in their repective state that return the next state to
@@ -264,9 +263,7 @@ void voice_wait(void)
264 * setup the DSP parameters */ 263 * setup the DSP parameters */
265static void voice_data_init(struct voice_thread_data *td) 264static void voice_data_init(struct voice_thread_data *td)
266{ 265{
267 td->dsp = (struct dsp_config *)dsp_configure(NULL, DSP_MYDSP, 266 td->dsp = dsp_get_config(CODEC_IDX_VOICE);
268 CODEC_IDX_VOICE);
269
270 dsp_configure(td->dsp, DSP_RESET, 0); 267 dsp_configure(td->dsp, DSP_RESET, 0);
271 dsp_configure(td->dsp, DSP_SET_FREQUENCY, VOICE_SAMPLE_RATE); 268 dsp_configure(td->dsp, DSP_SET_FREQUENCY, VOICE_SAMPLE_RATE);
272 dsp_configure(td->dsp, DSP_SET_SAMPLE_DEPTH, VOICE_SAMPLE_DEPTH); 269 dsp_configure(td->dsp, DSP_SET_SAMPLE_DEPTH, VOICE_SAMPLE_DEPTH);
@@ -378,7 +375,8 @@ static enum voice_state voice_decode(struct voice_thread_data *td)
378 else 375 else
379 { 376 {
380 /* If all clips are done and not playing, force pcm playback. */ 377 /* If all clips are done and not playing, force pcm playback. */
381 voice_start_playback(); 378 if (voice_unplayed_frames() > 0)
379 voice_start_playback();
382 return VOICE_STATE_MESSAGE; 380 return VOICE_STATE_MESSAGE;
383 } 381 }
384 } 382 }
@@ -387,12 +385,14 @@ static enum voice_state voice_decode(struct voice_thread_data *td)
387 yield(); 385 yield();
388 386
389 /* Output the decoded frame */ 387 /* Output the decoded frame */
390 td->count = VOICE_FRAME_COUNT - td->lookahead; 388 td->src.remcount = VOICE_FRAME_COUNT - td->lookahead;
391 td->src[0] = (const char *)&voice_output_buf[td->lookahead]; 389 td->src.pin[0] = &voice_output_buf[td->lookahead];
392 td->src[1] = NULL; 390 td->src.pin[1] = NULL;
391 td->src.proc_mask = 0;
392
393 td->lookahead -= MIN(VOICE_FRAME_COUNT, td->lookahead); 393 td->lookahead -= MIN(VOICE_FRAME_COUNT, td->lookahead);
394 394
395 if (td->count > 0) 395 if (td->src.remcount > 0)
396 return VOICE_STATE_BUFFER_INSERT; 396 return VOICE_STATE_BUFFER_INSERT;
397 } 397 }
398 398
@@ -405,12 +405,21 @@ static enum voice_state voice_buffer_insert(struct voice_thread_data *td)
405 if (!queue_empty(&voice_queue)) 405 if (!queue_empty(&voice_queue))
406 return VOICE_STATE_MESSAGE; 406 return VOICE_STATE_MESSAGE;
407 407
408 char *dest = (char *)voice_buf_get(); 408 struct dsp_buffer dst;
409 409
410 if (dest != NULL) 410 if ((dst.p16out = voice_buf_get()) != NULL)
411 { 411 {
412 voice_buf_commit(dsp_process(td->dsp, dest, td->src, td->count)); 412 dst.remcount = 0;
413 return VOICE_STATE_DECODE; 413 dst.bufcount = VOICE_PCM_FRAME_COUNT;
414
415 dsp_process(td->dsp, &td->src, &dst);
416
417 voice_buf_commit(dst.remcount);
418
419 /* Unless other effects are introduced to voice that have delays,
420 all output should have been purged to dst in one call */
421 return td->src.remcount > 0 ?
422 VOICE_STATE_BUFFER_INSERT : VOICE_STATE_DECODE;
414 } 423 }
415 424
416 sleep(0); 425 sleep(0);
diff --git a/firmware/export/system.h b/firmware/export/system.h
index d93d10c9e2..b1959c496d 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -300,6 +300,12 @@ static inline uint32_t swaw32_hw(uint32_t value)
300#define BIT_N(n) (1U << (n)) 300#define BIT_N(n) (1U << (n))
301#endif 301#endif
302 302
303#ifndef MASK_N
304/* Make a mask of n contiguous bits, shifted left by 'shift' */
305#define MASK_N(type, n, shift) \
306 ((type)((((type)1 << (n)) - (type)1) << (shift)))
307#endif
308
303/* Declare this as HIGHEST_IRQ_LEVEL if they don't differ */ 309/* Declare this as HIGHEST_IRQ_LEVEL if they don't differ */
304#ifndef DISABLE_INTERRUPTS 310#ifndef DISABLE_INTERRUPTS
305#define DISABLE_INTERRUPTS HIGHEST_IRQ_LEVEL 311#define DISABLE_INTERRUPTS HIGHEST_IRQ_LEVEL
@@ -352,7 +358,7 @@ static inline uint32_t swaw32_hw(uint32_t value)
352 358
353/* Define MEM_ALIGN_ATTR which may be used to align e.g. buffers for faster 359/* Define MEM_ALIGN_ATTR which may be used to align e.g. buffers for faster
354 * access. */ 360 * access. */
355#if defined(CPU_ARM) 361#if defined(CPU_ARM)
356 /* Use ARMs cache alignment. */ 362 /* Use ARMs cache alignment. */
357 #define MEM_ALIGN_ATTR CACHEALIGN_ATTR 363 #define MEM_ALIGN_ATTR CACHEALIGN_ATTR
358 #define MEM_ALIGN_SIZE CACHEALIGN_SIZE 364 #define MEM_ALIGN_SIZE CACHEALIGN_SIZE
@@ -361,12 +367,16 @@ static inline uint32_t swaw32_hw(uint32_t value)
361 #define MEM_ALIGN_ATTR __attribute__((aligned(16))) 367 #define MEM_ALIGN_ATTR __attribute__((aligned(16)))
362 #define MEM_ALIGN_SIZE 16 368 #define MEM_ALIGN_SIZE 16
363#else 369#else
364 /* Do nothing. */
365 #define MEM_ALIGN_ATTR
366 /* Align pointer size */ 370 /* Align pointer size */
371 #define MEM_ALIGN_ATTR __attribute__((aligned(sizeof(intptr_t))))
367 #define MEM_ALIGN_SIZE sizeof(intptr_t) 372 #define MEM_ALIGN_SIZE sizeof(intptr_t)
368#endif 373#endif
369 374
375#define MEM_ALIGN_UP(x) \
376 ((typeof (x))ALIGN_UP((uintptr_t)(x), MEM_ALIGN_SIZE))
377#define MEM_ALIGN_DOWN(x) \
378 ((typeof (x))ALIGN_DOWN((uintptr_t)(x), MEM_ALIGN_SIZE))
379
370#ifdef STORAGE_WANTS_ALIGN 380#ifdef STORAGE_WANTS_ALIGN
371 #define STORAGE_ALIGN_ATTR __attribute__((aligned(CACHEALIGN_SIZE))) 381 #define STORAGE_ALIGN_ATTR __attribute__((aligned(CACHEALIGN_SIZE)))
372 #define STORAGE_ALIGN_DOWN(x) \ 382 #define STORAGE_ALIGN_DOWN(x) \
diff --git a/lib/rbcodec/SOURCES b/lib/rbcodec/SOURCES
index 3ac2660a38..c293f3c028 100644
--- a/lib/rbcodec/SOURCES
+++ b/lib/rbcodec/SOURCES
@@ -3,22 +3,31 @@ metadata/id3tags.c
3metadata/mp3.c 3metadata/mp3.c
4metadata/mp3data.c 4metadata/mp3data.c
5#if CONFIG_CODEC == SWCODEC 5#if CONFIG_CODEC == SWCODEC
6dsp/channel_mode.c
6dsp/compressor.c 7dsp/compressor.c
7dsp/dsp.c 8dsp/crossfeed.c
9dsp/dsp_core.c
10dsp/dsp_filter.c
11dsp/dsp_misc.c
12dsp/dsp_sample_input.c
13dsp/dsp_sample_output.c
8dsp/eq.c 14dsp/eq.c
15dsp/lin_resample.c
16dsp/pga.c
17# ifdef HAVE_PITCHSCREEN
18dsp/tdspeed.c
19# endif
20# ifdef HAVE_SW_TONE_CONTROLS
21dsp/tone_controls.c
22# endif
9# if defined(CPU_COLDFIRE) 23# if defined(CPU_COLDFIRE)
10dsp/dsp_cf.S 24dsp/dsp_cf.S
11dsp/eq_cf.S
12# elif defined(CPU_ARM) 25# elif defined(CPU_ARM)
13dsp/dsp_arm.S 26dsp/dsp_arm.S
14dsp/eq_arm.S
15# if ARM_ARCH >= 6 27# if ARM_ARCH >= 6
16dsp/dsp_arm_v6.S 28dsp/dsp_arm_v6.S
17# endif 29# endif
18# endif 30# endif
19# ifdef HAVE_PITCHSCREEN
20dsp/tdspeed.c
21# endif
22metadata/replaygain.c 31metadata/replaygain.c
23metadata/metadata_common.c 32metadata/metadata_common.c
24metadata/a52.c 33metadata/a52.c
diff --git a/lib/rbcodec/codecs/codecs.h b/lib/rbcodec/codecs/codecs.h
index bad8cdd469..d4a51a4aba 100644
--- a/lib/rbcodec/codecs/codecs.h
+++ b/lib/rbcodec/codecs/codecs.h
@@ -75,12 +75,12 @@
75#define CODEC_ENC_MAGIC 0x52454E43 /* RENC */ 75#define CODEC_ENC_MAGIC 0x52454E43 /* RENC */
76 76
77/* increase this every time the api struct changes */ 77/* increase this every time the api struct changes */
78#define CODEC_API_VERSION 44 78#define CODEC_API_VERSION 45
79 79
80/* update this to latest version if a change to the api struct breaks 80/* update this to latest version if a change to the api struct breaks
81 backwards compatibility (and please take the opportunity to sort in any 81 backwards compatibility (and please take the opportunity to sort in any
82 new function which are "waiting" at the end of the function table) */ 82 new function which are "waiting" at the end of the function table) */
83#define CODEC_MIN_API_VERSION 43 83#define CODEC_MIN_API_VERSION 45
84 84
85/* reasons for calling codec main entrypoint */ 85/* reasons for calling codec main entrypoint */
86enum codec_entry_call_reason { 86enum codec_entry_call_reason {
@@ -171,6 +171,7 @@ struct codec_api {
171 171
172 void (*commit_dcache)(void); 172 void (*commit_dcache)(void);
173 void (*commit_discard_dcache)(void); 173 void (*commit_discard_dcache)(void);
174 void (*commit_discard_idcache)(void);
174 175
175 /* strings and memory */ 176 /* strings and memory */
176 char* (*strcpy)(char *dst, const char *src); 177 char* (*strcpy)(char *dst, const char *src);
@@ -223,7 +224,6 @@ struct codec_api {
223 224
224 /* new stuff at the end, sort into place next time 225 /* new stuff at the end, sort into place next time
225 the API gets incompatible */ 226 the API gets incompatible */
226 void (*commit_discard_idcache)(void);
227}; 227};
228 228
229/* codec header */ 229/* codec header */
diff --git a/lib/rbcodec/codecs/lib/codeclib.c b/lib/rbcodec/codecs/lib/codeclib.c
index 36f4279941..4ca6c8c993 100644
--- a/lib/rbcodec/codecs/lib/codeclib.c
+++ b/lib/rbcodec/codecs/lib/codeclib.c
@@ -26,6 +26,7 @@
26#include "dsp.h" 26#include "dsp.h"
27#include "codeclib.h" 27#include "codeclib.h"
28#include "metadata.h" 28#include "metadata.h"
29#include "dsp_proc_entry.h"
29 30
30/* The following variables are used by codec_malloc() to make use of free RAM 31/* The following variables are used by codec_malloc() to make use of free RAM
31 * within the statically allocated codec buffer. */ 32 * within the statically allocated codec buffer. */
@@ -44,10 +45,15 @@ int codec_init(void)
44 45
45void codec_set_replaygain(const struct mp3entry *id3) 46void codec_set_replaygain(const struct mp3entry *id3)
46{ 47{
47 ci->configure(DSP_SET_TRACK_GAIN, id3->track_gain); 48 struct dsp_replay_gains gains =
48 ci->configure(DSP_SET_ALBUM_GAIN, id3->album_gain); 49 {
49 ci->configure(DSP_SET_TRACK_PEAK, id3->track_peak); 50 .track_gain = id3->track_gain,
50 ci->configure(DSP_SET_ALBUM_PEAK, id3->album_peak); 51 .album_gain = id3->album_gain,
52 .track_peak = id3->track_peak,
53 .album_peak = id3->album_peak,
54 };
55
56 ci->configure(REPLAYGAIN_SET_GAINS, (intptr_t)&gains);
51} 57}
52 58
53/* Various "helper functions" common to all the xxx2wav decoder plugins */ 59/* Various "helper functions" common to all the xxx2wav decoder plugins */
diff --git a/lib/rbcodec/dsp/channel_mode.c b/lib/rbcodec/dsp/channel_mode.c
new file mode 100644
index 0000000000..5b678887c2
--- /dev/null
+++ b/lib/rbcodec/dsp/channel_mode.c
@@ -0,0 +1,264 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Thom Johansen
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include "settings.h"
26#include "sound.h"
27#include "fixedpoint.h"
28#include "fracmul.h"
29#include "dsp_proc_entry.h"
30
31#if 0
32/* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
33 * completeness. */
34void channel_mode_proc_stereo(struct dsp_proc_entry *this,
35 struct dsp_buffer **buf_p);
36#endif
37void channel_mode_proc_mono(struct dsp_proc_entry *this,
38 struct dsp_buffer **buf_p);
39void channel_mode_proc_mono_left(struct dsp_proc_entry *this,
40 struct dsp_buffer **buf_p);
41void channel_mode_proc_mono_right(struct dsp_proc_entry *this,
42 struct dsp_buffer **buf_p);
43void channel_mode_proc_custom(struct dsp_proc_entry *this,
44 struct dsp_buffer **buf_p);
45void channel_mode_proc_karaoke(struct dsp_proc_entry *this,
46 struct dsp_buffer **buf_p);
47
48static struct channel_mode_data
49{
50 long sw_gain; /* 00h: for mode: custom */
51 long sw_cross; /* 04h: for mode: custom */
52 struct dsp_config *dsp;
53 int mode;
54 const dsp_proc_fn_type fns[SOUND_CHAN_NUM_MODES];
55} channel_mode_data =
56{
57 .sw_gain = 0,
58 .sw_cross = 0,
59 .mode = SOUND_CHAN_STEREO,
60 .fns =
61 {
62 [SOUND_CHAN_STEREO] = NULL,
63 [SOUND_CHAN_MONO] = channel_mode_proc_mono,
64 [SOUND_CHAN_CUSTOM] = channel_mode_proc_custom,
65 [SOUND_CHAN_MONO_LEFT] = channel_mode_proc_mono_left,
66 [SOUND_CHAN_MONO_RIGHT] = channel_mode_proc_mono_right,
67 [SOUND_CHAN_KARAOKE] = channel_mode_proc_karaoke,
68 },
69};
70
71static dsp_proc_fn_type get_process_fn(void)
72{
73 return channel_mode_data.fns[channel_mode_data.mode];
74}
75
76#if 0
77/* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
78 * completeness. */
79void channel_mode_proc_stereo(struct dsp_proc_entry *this,
80 struct dsp_buffer **buf_p)
81{
82 /* The channels are each just themselves */
83 (void)this; (void)buf_p;
84}
85#endif
86
87#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
88/* Unoptimized routines */
89void channel_mode_proc_mono(struct dsp_proc_entry *this,
90 struct dsp_buffer **buf_p)
91{
92 struct dsp_buffer *buf = *buf_p;
93 int32_t *sl = buf->p32[0];
94 int32_t *sr = buf->p32[1];
95 int count = buf->remcount;
96
97 do
98 {
99 int32_t lr = *sl / 2 + *sr / 2;
100 *sl++ = lr;
101 *sr++ = lr;
102 }
103 while (--count > 0);
104
105 (void)this;
106}
107
108void channel_mode_proc_custom(struct dsp_proc_entry *this,
109 struct dsp_buffer **buf_p)
110{
111 struct channel_mode_data *data = (void *)this->data;
112 struct dsp_buffer *buf = *buf_p;
113
114 int32_t *sl = buf->p32[0];
115 int32_t *sr = buf->p32[1];
116 int count = buf->remcount;
117
118 const int32_t gain = data->sw_gain;
119 const int32_t cross = data->sw_cross;
120
121 do
122 {
123 int32_t l = *sl;
124 int32_t r = *sr;
125 *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross);
126 *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross);
127 }
128 while (--count > 0);
129}
130
131void channel_mode_proc_karaoke(struct dsp_proc_entry *this,
132 struct dsp_buffer **buf_p)
133{
134 struct dsp_buffer *buf = *buf_p;
135 int32_t *sl = buf->p32[0];
136 int32_t *sr = buf->p32[1];
137 int count = buf->remcount;
138
139 do
140 {
141 int32_t ch = *sl / 2 - *sr / 2;
142 *sl++ = ch;
143 *sr++ = -ch;
144 }
145 while (--count > 0);
146
147 (void)this;
148}
149#endif /* CPU */
150
151void channel_mode_proc_mono_left(struct dsp_proc_entry *this,
152 struct dsp_buffer **buf_p)
153{
154 /* Just copy over the other channel */
155 struct dsp_buffer *buf = *buf_p;
156 memcpy(buf->p32[1], buf->p32[0], buf->remcount * sizeof (int32_t));
157 (void)this;
158}
159
160void channel_mode_proc_mono_right(struct dsp_proc_entry *this,
161 struct dsp_buffer **buf_p)
162{
163 /* Just copy over the other channel */
164 struct dsp_buffer *buf = *buf_p;
165 memcpy(buf->p32[0], buf->p32[1], buf->remcount * sizeof (int32_t));
166 (void)this;
167}
168
169/* This is the initial function pointer when first enabled/changed in order
170 * to facilitate verification of the format compatibility at the proper time
171 * This gets called for changes even if stage is inactive. */
172static void channel_mode_process_new_format(struct dsp_proc_entry *this,
173 struct dsp_buffer **buf_p)
174{
175 struct channel_mode_data *data = (void *)this->data;
176 struct dsp_buffer *buf = *buf_p;
177
178 DSP_PRINT_FORMAT(DSP_PROC_CHANNEL_MODE, DSP_PROC_CHANNEL_MODE,
179 buf->format);
180
181 bool active = buf->format.num_channels >= 2;
182 dsp_proc_activate(data->dsp, DSP_PROC_CHANNEL_MODE, active);
183
184 if (!active)
185 {
186 /* Can't do this. Sleep until next change. */
187 DEBUGF(" DSP_PROC_CHANNEL_MODE- deactivated\n");
188 return;
189 }
190
191 /* Switch to the real function and call it once */
192 this->process[0] = get_process_fn();
193 dsp_proc_call(this, buf_p, (unsigned)buf->format.changed - 1);
194}
195
196void channel_mode_set_config(int value)
197{
198 if (value < 0 || value >= SOUND_CHAN_NUM_MODES)
199 value = SOUND_CHAN_STEREO; /* Out of range */
200
201 if (value == channel_mode_data.mode)
202 return;
203
204 channel_mode_data.mode = value;
205 dsp_proc_enable(dsp_get_config(CODEC_IDX_AUDIO), DSP_PROC_CHANNEL_MODE,
206 value != SOUND_CHAN_STEREO);
207}
208
209void channel_mode_custom_set_width(int value)
210{
211 long width, straight, cross;
212
213 width = value * 0x7fffff / 100;
214
215 if (value <= 100)
216 {
217 straight = (0x7fffff + width) / 2;
218 cross = straight - width;
219 }
220 else
221 {
222 /* straight = (1 + width) / (2 * width) */
223 straight = fp_div(0x7fffff + width, width, 22);
224 cross = straight - 0x7fffff;
225 }
226
227 channel_mode_data.sw_gain = straight << 8;
228 channel_mode_data.sw_cross = cross << 8;
229}
230
231/* DSP message hook */
232static intptr_t channel_mode_configure(struct dsp_proc_entry *this,
233 struct dsp_config *dsp,
234 unsigned int setting,
235 intptr_t value)
236{
237 switch (setting)
238 {
239 case DSP_PROC_INIT:
240 if (value == 0)
241 {
242 /* New object */
243 this->data = (intptr_t)&channel_mode_data;
244 this->process[1] = channel_mode_process_new_format;
245 ((struct channel_mode_data *)this->data)->dsp = dsp;
246 }
247
248 /* Force format change call each time */
249 this->process[0] = channel_mode_process_new_format;
250 dsp_proc_activate(dsp, DSP_PROC_CHANNEL_MODE, true);
251 break;
252
253 case DSP_PROC_CLOSE:
254 ((struct channel_mode_data *)this->data)->dsp = NULL;
255 break;
256 }
257
258 return 1;
259}
260
261/* Database entry */
262DSP_PROC_DB_ENTRY(
263 CHANNEL_MODE,
264 channel_mode_configure);
diff --git a/lib/rbcodec/dsp/channel_mode.h b/lib/rbcodec/dsp/channel_mode.h
new file mode 100644
index 0000000000..7ca0f74204
--- /dev/null
+++ b/lib/rbcodec/dsp/channel_mode.h
@@ -0,0 +1,27 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef CHANNEL_MODE_H
22#define CHANNEL_MODE_H
23
24void channel_mode_set_config(int value);
25void channel_mode_custom_set_width(int value);
26
27#endif /* CHANNEL_MODE_H */
diff --git a/lib/rbcodec/dsp/compressor.c b/lib/rbcodec/dsp/compressor.c
index a6c1ac1018..1816bfef9c 100644
--- a/lib/rbcodec/dsp/compressor.c
+++ b/lib/rbcodec/dsp/compressor.c
@@ -19,16 +19,18 @@
19 * 19 *
20 ****************************************************************************/ 20 ****************************************************************************/
21#include "config.h" 21#include "config.h"
22#include "system.h"
22#include "fixedpoint.h" 23#include "fixedpoint.h"
23#include "fracmul.h" 24#include "fracmul.h"
24#include "settings.h"
25#include "dsp.h" 25#include "dsp.h"
26#include "compressor.h" 26#include <string.h>
27 27
28/* Define LOGF_ENABLE to enable logf output in this file */ 28/* Define LOGF_ENABLE to enable logf output in this file */
29/*#define LOGF_ENABLE*/ 29/*#define LOGF_ENABLE*/
30#include "logf.h" 30#include "logf.h"
31 31
32#include "dsp_proc_entry.h"
33
32static struct compressor_settings curr_set; /* Cached settings */ 34static struct compressor_settings curr_set; /* Cached settings */
33 35
34static int32_t comp_rel_slope IBSS_ATTR; /* S7.24 format */ 36static int32_t comp_rel_slope IBSS_ATTR; /* S7.24 format */
@@ -251,10 +253,10 @@ bool compressor_update(const struct compressor_settings *settings)
251 * Returns the required gain factor in S7.24 format in order to compress the 253 * Returns the required gain factor in S7.24 format in order to compress the
252 * sample in accordance with the compression curve. Always 1 or less. 254 * sample in accordance with the compression curve. Always 1 or less.
253 */ 255 */
254static inline int32_t get_compression_gain(struct dsp_data *data, 256static inline int32_t get_compression_gain(struct sample_format *format,
255 int32_t sample) 257 int32_t sample)
256{ 258{
257 const int frac_bits_offset = data->frac_bits - 15; 259 const int frac_bits_offset = format->frac_bits - 15;
258 260
259 /* sample must be positive */ 261 /* sample must be positive */
260 if (sample < 0) 262 if (sample < 0)
@@ -292,24 +294,40 @@ static inline int32_t get_compression_gain(struct dsp_data *data,
292 return -1; 294 return -1;
293} 295}
294 296
297/** DSP interface **/
298
299/** SET COMPRESSOR
300 * Enable or disable the compressor based upon the settings
301 */
302void dsp_set_compressor(const struct compressor_settings *settings)
303{
304 /* enable/disable the compressor depending upon settings */
305 bool enable = compressor_update(settings);
306 struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
307 dsp_proc_enable(dsp, DSP_PROC_COMPRESSOR, enable);
308 dsp_proc_activate(dsp, DSP_PROC_COMPRESSOR, true);
309}
310
295/** COMPRESSOR PROCESS 311/** COMPRESSOR PROCESS
296 * Changes the gain of the samples according to the compressor curve 312 * Changes the gain of the samples according to the compressor curve
297 */ 313 */
298void compressor_process(int count, struct dsp_data *data, int32_t *buf[]) 314static void compressor_process(struct dsp_proc_entry *this,
315 struct dsp_buffer **buf_p)
299{ 316{
300 const int num_chan = data->num_channels; 317 struct dsp_buffer *buf = *buf_p;
301 int32_t *in_buf[2] = {buf[0], buf[1]}; 318 int count = buf->remcount;
302 319 int32_t *in_buf[2] = { buf->p32[0], buf->p32[1] };
320 const int num_chan = buf->format.num_channels;
321
303 while (count-- > 0) 322 while (count-- > 0)
304 { 323 {
305 int ch;
306 /* use lowest (most compressed) gain factor of the output buffer 324 /* use lowest (most compressed) gain factor of the output buffer
307 sample pair for both samples (mono is also handled correctly here) 325 sample pair for both samples (mono is also handled correctly here)
308 */ 326 */
309 int32_t sample_gain = UNITY; 327 int32_t sample_gain = UNITY;
310 for (ch = 0; ch < num_chan; ch++) 328 for (int ch = 0; ch < num_chan; ch++)
311 { 329 {
312 int32_t this_gain = get_compression_gain(data, *in_buf[ch]); 330 int32_t this_gain = get_compression_gain(&buf->format, *in_buf[ch]);
313 if (this_gain < sample_gain) 331 if (this_gain < sample_gain)
314 sample_gain = this_gain; 332 sample_gain = this_gain;
315 } 333 }
@@ -345,7 +363,7 @@ void compressor_process(int count, struct dsp_data *data, int32_t *buf[])
345 output buffer sample pair/mono sample */ 363 output buffer sample pair/mono sample */
346 if (total_gain != UNITY) 364 if (total_gain != UNITY)
347 { 365 {
348 for (ch = 0; ch < num_chan; ch++) 366 for (int ch = 0; ch < num_chan; ch++)
349 { 367 {
350 *in_buf[ch] = FRACMUL_SHL(total_gain, *in_buf[ch], 7); 368 *in_buf[ch] = FRACMUL_SHL(total_gain, *in_buf[ch], 7);
351 } 369 }
@@ -353,9 +371,33 @@ void compressor_process(int count, struct dsp_data *data, int32_t *buf[])
353 in_buf[0]++; 371 in_buf[0]++;
354 in_buf[1]++; 372 in_buf[1]++;
355 } 373 }
374
375 (void)this;
356} 376}
357 377
358void compressor_reset(void) 378/* DSP message hook */
379static intptr_t compressor_configure(struct dsp_proc_entry *this,
380 struct dsp_config *dsp,
381 unsigned int setting,
382 intptr_t value)
359{ 383{
360 release_gain = UNITY; 384 switch (setting)
385 {
386 case DSP_PROC_INIT:
387 if (value != 0)
388 break; /* Already enabled */
389 this->process[0] = compressor_process;
390 case DSP_RESET:
391 case DSP_FLUSH:
392 release_gain = UNITY;
393 break;
394 }
395
396 return 1;
397 (void)dsp;
361} 398}
399
400/* Database entry */
401DSP_PROC_DB_ENTRY(
402 COMPRESSOR,
403 compressor_configure);
diff --git a/lib/rbcodec/dsp/compressor.h b/lib/rbcodec/dsp/compressor.h
index d0e33f6e2c..e41950926e 100644
--- a/lib/rbcodec/dsp/compressor.h
+++ b/lib/rbcodec/dsp/compressor.h
@@ -18,12 +18,18 @@
18 * KIND, either express or implied. 18 * KIND, either express or implied.
19 * 19 *
20 ****************************************************************************/ 20 ****************************************************************************/
21
22#ifndef COMPRESSOR_H 21#ifndef COMPRESSOR_H
23#define COMPRESSOR_H 22#define COMPRESSOR_H
24 23
25void compressor_process(int count, struct dsp_data *data, int32_t *buf[]); 24struct compressor_settings
26bool compressor_update(const struct compressor_settings *settings); 25{
27void compressor_reset(void); 26 int threshold;
27 int makeup_gain;
28 int ratio;
29 int knee;
30 int release_time;
31};
32
33void dsp_set_compressor(const struct compressor_settings *settings);
28 34
29#endif /* COMPRESSOR_H */ 35#endif /* COMPRESSOR_H */
diff --git a/lib/rbcodec/dsp/crossfeed.c b/lib/rbcodec/dsp/crossfeed.c
new file mode 100644
index 0000000000..ecb55644ee
--- /dev/null
+++ b/lib/rbcodec/dsp/crossfeed.c
@@ -0,0 +1,214 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Thom Johansen
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include "dsp_filter.h"
26#include "fixedpoint.h"
27#include "fracmul.h"
28#include "replaygain.h"
29#include <string.h>
30#include "dsp_proc_entry.h"
31
32/* Implemented here or in target assembly code */
33void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p);
34
35/**
36 * Applies crossfeed to the stereo signal.
37 *
38 * Crossfeed is a process where listening over speakers is simulated. This
39 * is good for old hard panned stereo records, which might be quite fatiguing
40 * to listen to on headphones with no crossfeed.
41 */
42
43/* Crossfeed */
44static struct crossfeed_state
45{
46 int32_t gain; /* 00h: Direct path gain */
47 int32_t coefs[3]; /* 04h: Coefficients for the shelving filter */
48 int32_t history[4]; /* 10h: Format is x[n - 1], y[n - 1] (L + R) */
49 int32_t delay[13*2]; /* 20h: Delay line buffer (L + R interleaved) */
50 int32_t *index; /* 88h: Current pointer into the delay line */
51 struct dsp_config *dsp; /* 8ch: Current DSP */
52 /* 90h */
53} crossfeed_state IBSS_ATTR;
54
55/* Discard the sample histories */
56static void crossfeed_flush(struct dsp_proc_entry *this)
57{
58 struct crossfeed_state *state = (void *)this->data;
59 memset(state->history, 0, sizeof (state->history));
60 memset(state->delay, 0, sizeof (state->delay));
61 state->index = state->delay;
62}
63
64
65/** DSP interface **/
66
67/* Crossfeed boot/format change function */
68static void crossfeed_process_new_format(struct dsp_proc_entry *this,
69 struct dsp_buffer **buf_p)
70{
71 struct crossfeed_state *state = (void *)this->data;
72 struct dsp_buffer *buf = *buf_p;
73
74 DSP_PRINT_FORMAT(DSP_PROC_CROSSFEED, DSP_PROC_CROSSFEED, buf->format);
75
76 bool active = buf->format.num_channels >= 2;
77 dsp_proc_activate(state->dsp, DSP_PROC_CROSSFEED, active);
78
79 if (!active)
80 {
81 /* Can't do this. Sleep until next change */
82 crossfeed_flush(this);
83 DEBUGF(" DSP_PROC_CROSSFEED- deactivated\n");
84 return;
85 }
86
87 /* Switch to the real function and call it once */
88 this->process[0] = crossfeed_process;
89 dsp_proc_call(this, buf_p, (unsigned)buf->format.changed - 1);
90}
91
92/* Enable or disable the crossfeed */
93void dsp_crossfeed_enable(bool enable)
94{
95 if (enable != !crossfeed_state.dsp)
96 return;
97
98 struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
99 dsp_proc_enable(dsp, DSP_PROC_CROSSFEED, enable);
100}
101
102/* Set the gain of the dry mix */
103void dsp_set_crossfeed_direct_gain(int gain)
104{
105 uint32_t gain32 = get_replaygain_int(gain * 10);
106 crossfeed_state.gain =
107 gain32 >= (0x80000000ul >> 7) ? 0x7ffffffful: (gain32 << 7);
108}
109
110/* Both gains should be below 0 dB */
111void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
112{
113 int32_t *c = crossfeed_state.coefs;
114 long scaler = get_replaygain_int(lf_gain * 10) << 7;
115
116 cutoff = 0xffffffff / NATIVE_FREQUENCY * cutoff;
117 hf_gain -= lf_gain;
118 /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
119 * point instead of shelf midpoint. This is for compatibility with the old
120 * crossfeed shelf filter and should be removed if crossfeed settings are
121 * ever made incompatible for any other good reason.
122 */
123 cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24);
124 filter_shelf_coefs(cutoff, hf_gain, false, c);
125 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
126 * over 1 and can do this safely
127 */
128 c[0] = FRACMUL_SHL(c[0], scaler, 4);
129 c[1] = FRACMUL_SHL(c[1], scaler, 4);
130 c[2] <<= 4;
131}
132
133#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
134/* Apply the crossfade to the buffer in place */
135void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
136{
137 struct crossfeed_state *state = (void *)this->data;
138 struct dsp_buffer *buf = *buf_p;
139
140 int32_t *hist_l = &state->history[0];
141 int32_t *hist_r = &state->history[2];
142 int32_t *delay = state->delay;
143 int32_t *coefs = &state->coefs[0];
144 int32_t gain = state->gain;
145 int32_t *di = state->index;
146
147 int count = buf->remcount;
148
149 for (int i = 0; i < count; i++)
150 {
151 int32_t left = buf->p32[0][i];
152 int32_t right = buf->p32[1][i];
153
154 /* Filter delayed sample from left speaker */
155 int32_t acc = FRACMUL(*di, coefs[0]);
156 acc += FRACMUL(hist_l[0], coefs[1]);
157 acc += FRACMUL(hist_l[1], coefs[2]);
158 /* Save filter history for left speaker */
159 hist_l[1] = acc;
160 hist_l[0] = *di;
161 *di++ = left;
162 /* Filter delayed sample from right speaker */
163 acc = FRACMUL(*di, coefs[0]);
164 acc += FRACMUL(hist_r[0], coefs[1]);
165 acc += FRACMUL(hist_r[1], coefs[2]);
166 /* Save filter history for right speaker */
167 hist_r[1] = acc;
168 hist_r[0] = *di;
169 *di++ = right;
170 /* Now add the attenuated direct sound and write to outputs */
171 buf->p32[0][i] = FRACMUL(left, gain) + hist_r[1];
172 buf->p32[1][i] = FRACMUL(right, gain) + hist_l[1];
173
174 /* Wrap delay line index if bigger than delay line size */
175 if (di >= delay + 13*2)
176 di = delay;
177 }
178
179 /* Write back local copies of data we've modified */
180 state->index = di;
181}
182#endif /* CPU */
183
184/* DSP message hook */
185static intptr_t crossfeed_configure(struct dsp_proc_entry *this,
186 struct dsp_config *dsp,
187 unsigned int setting,
188 intptr_t value)
189{
190 switch (setting)
191 {
192 case DSP_PROC_INIT:
193 this->data = (intptr_t)&crossfeed_state;
194 this->process[0] = crossfeed_process_new_format;
195 this->process[1] = crossfeed_process_new_format;
196 ((struct crossfeed_state *)this->data)->dsp = dsp;
197 dsp_proc_activate(dsp, DSP_PROC_CROSSFEED, true);
198 case DSP_FLUSH:
199 crossfeed_flush(this);
200 break;
201
202 case DSP_PROC_CLOSE:
203 ((struct crossfeed_state *)this->data)->dsp = NULL;
204 break;
205 }
206
207 return 1;
208 (void)value;
209}
210
211/* Database entry */
212DSP_PROC_DB_ENTRY(
213 CROSSFEED,
214 crossfeed_configure);
diff --git a/lib/rbcodec/dsp/crossfeed.h b/lib/rbcodec/dsp/crossfeed.h
new file mode 100644
index 0000000000..63261bde9f
--- /dev/null
+++ b/lib/rbcodec/dsp/crossfeed.h
@@ -0,0 +1,28 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef CROSSFEED_H
22#define CROSSFEED_H
23
24void dsp_crossfeed_enable(bool enable);
25void dsp_set_crossfeed_direct_gain(int gain);
26void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff);
27
28#endif /* CROSSFEED_H */
diff --git a/lib/rbcodec/dsp/dsp.c b/lib/rbcodec/dsp/dsp.c
deleted file mode 100644
index de647dc0dd..0000000000
--- a/lib/rbcodec/dsp/dsp.c
+++ /dev/null
@@ -1,1568 +0,0 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Miika Pekkarinen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#include "config.h"
22#include "system.h"
23#include <sound.h>
24#include "dsp.h"
25#include "dsp-util.h"
26#include "eq.h"
27#include "compressor.h"
28#include "kernel.h"
29#include "settings.h"
30#include "replaygain.h"
31#include "tdspeed.h"
32#include "core_alloc.h"
33#include "fixedpoint.h"
34#include "fracmul.h"
35
36/* Define LOGF_ENABLE to enable logf output in this file */
37/*#define LOGF_ENABLE*/
38#include "logf.h"
39
40/* 16-bit samples are scaled based on these constants. The shift should be
41 * no more than 15.
42 */
43#define WORD_SHIFT 12
44#define WORD_FRACBITS 27
45
46#define NATIVE_DEPTH 16
47#define SMALL_SAMPLE_BUF_COUNT 128 /* Per channel */
48#define DEFAULT_GAIN 0x01000000
49
50/* enums to index conversion properly with stereo mode and other settings */
51enum
52{
53 SAMPLE_INPUT_LE_NATIVE_I_STEREO = STEREO_INTERLEAVED,
54 SAMPLE_INPUT_LE_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED,
55 SAMPLE_INPUT_LE_NATIVE_MONO = STEREO_MONO,
56 SAMPLE_INPUT_GT_NATIVE_I_STEREO = STEREO_INTERLEAVED + STEREO_NUM_MODES,
57 SAMPLE_INPUT_GT_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED + STEREO_NUM_MODES,
58 SAMPLE_INPUT_GT_NATIVE_MONO = STEREO_MONO + STEREO_NUM_MODES,
59 SAMPLE_INPUT_GT_NATIVE_1ST_INDEX = STEREO_NUM_MODES
60};
61
62enum
63{
64 SAMPLE_OUTPUT_MONO = 0,
65 SAMPLE_OUTPUT_STEREO,
66 SAMPLE_OUTPUT_DITHERED_MONO,
67 SAMPLE_OUTPUT_DITHERED_STEREO
68};
69
70/* No asm...yet */
71struct dither_data
72{
73 long error[3]; /* 00h */
74 long random; /* 0ch */
75 /* 10h */
76};
77
78struct crossfeed_data
79{
80 int32_t gain; /* 00h - Direct path gain */
81 int32_t coefs[3]; /* 04h - Coefficients for the shelving filter */
82 int32_t history[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */
83 int32_t delay[13][2]; /* 20h */
84 int32_t *index; /* 88h - Current pointer into the delay line */
85 /* 8ch */
86};
87
88/* Current setup is one lowshelf filters three peaking filters and one
89 * highshelf filter. Varying the number of shelving filters make no sense,
90 * but adding peaking filters is possible.
91 */
92struct eq_state
93{
94 char enabled[5]; /* 00h - Flags for active filters */
95 struct eqfilter filters[5]; /* 08h - packing is 4? */
96 /* 10ch */
97};
98
99/* Include header with defines which functions are implemented in assembly
100 code for the target */
101#include <dsp_asm.h>
102
103/* Typedefs keep things much neater in this case */
104typedef void (*sample_input_fn_type)(int count, const char *src[],
105 int32_t *dst[]);
106typedef int (*resample_fn_type)(int count, struct dsp_data *data,
107 const int32_t *src[], int32_t *dst[]);
108typedef void (*sample_output_fn_type)(int count, struct dsp_data *data,
109 const int32_t *src[], int16_t *dst);
110
111/* Single-DSP channel processing in place */
112typedef void (*channels_process_fn_type)(int count, int32_t *buf[]);
113/* DSP local channel processing in place */
114typedef void (*channels_process_dsp_fn_type)(int count, struct dsp_data *data,
115 int32_t *buf[]);
116
117/*
118 ***************************************************************************/
119
120struct dsp_config
121{
122 struct dsp_data data; /* Config members for use in external routines */
123 long codec_frequency; /* Sample rate of data coming from the codec */
124 long frequency; /* Effective sample rate after pitch shift (if any) */
125 int sample_depth;
126 int sample_bytes;
127 int stereo_mode;
128 int32_t tdspeed_percent; /* Speed% * PITCH_SPEED_PRECISION */
129#ifdef HAVE_PITCHSCREEN
130 bool tdspeed_active; /* Timestretch is in use */
131#endif
132#ifdef HAVE_SW_TONE_CONTROLS
133 /* Filter struct for software bass/treble controls */
134 struct eqfilter tone_filter;
135#endif
136 /* Functions that change depending upon settings - NULL if stage is
137 disabled */
138 sample_input_fn_type input_samples;
139 resample_fn_type resample;
140 sample_output_fn_type output_samples;
141 /* These will be NULL for the voice codec and is more economical that
142 way */
143 channels_process_dsp_fn_type apply_gain;
144 channels_process_fn_type apply_crossfeed;
145 channels_process_fn_type eq_process;
146 channels_process_fn_type channels_process;
147 channels_process_dsp_fn_type compressor_process;
148};
149
150/* General DSP config */
151static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */
152/* Dithering */
153static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */
154static long dither_mask IBSS_ATTR;
155static long dither_bias IBSS_ATTR;
156/* Crossfeed */
157struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */
158{
159 .index = (int32_t *)crossfeed_data.delay
160};
161
162/* Equalizer */
163static struct eq_state eq_data; /* A */
164
165/* Software tone controls */
166#ifdef HAVE_SW_TONE_CONTROLS
167static int prescale; /* A/V */
168static int bass; /* A/V */
169static int treble; /* A/V */
170#endif
171
172/* Settings applicable to audio codec only */
173#ifdef HAVE_PITCHSCREEN
174static int32_t pitch_ratio = PITCH_SPEED_100;
175static int big_sample_locks;
176#endif
177static int channels_mode;
178 long dsp_sw_gain;
179 long dsp_sw_cross;
180static bool dither_enabled;
181static long eq_precut;
182static long track_gain;
183static bool new_gain;
184static long album_gain;
185static long track_peak;
186static long album_peak;
187static long replaygain;
188static bool crossfeed_enabled;
189
190#define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO])
191#define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE])
192
193/* The internal format is 32-bit samples, non-interleaved, stereo. This
194 * format is similar to the raw output from several codecs, so the amount
195 * of copying needed is minimized for that case.
196 */
197
198#define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */
199#define SMALL_RESAMPLE_BUF_COUNT (SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO)
200#define BIG_SAMPLE_BUF_COUNT SMALL_RESAMPLE_BUF_COUNT
201#define BIG_RESAMPLE_BUF_COUNT (BIG_SAMPLE_BUF_COUNT * RESAMPLE_RATIO)
202
203static int32_t small_sample_buf[2][SMALL_SAMPLE_BUF_COUNT] IBSS_ATTR;
204static int32_t small_resample_buf[2][SMALL_RESAMPLE_BUF_COUNT] IBSS_ATTR;
205
206#ifdef HAVE_PITCHSCREEN
207static int32_t (* big_sample_buf)[BIG_SAMPLE_BUF_COUNT] = NULL;
208static int32_t (* big_resample_buf)[BIG_RESAMPLE_BUF_COUNT] = NULL;
209#endif
210
211static int sample_buf_count = SMALL_SAMPLE_BUF_COUNT;
212static int32_t *sample_buf[2] = { small_sample_buf[0], small_sample_buf[1] };
213static int resample_buf_count = SMALL_RESAMPLE_BUF_COUNT;
214static int32_t *resample_buf[2] = { small_resample_buf[0], small_resample_buf[1] };
215
216#ifdef HAVE_PITCHSCREEN
217int32_t sound_get_pitch(void)
218{
219 return pitch_ratio;
220}
221
222void sound_set_pitch(int32_t percent)
223{
224 pitch_ratio = percent;
225 dsp_configure(&AUDIO_DSP, DSP_SWITCH_FREQUENCY,
226 AUDIO_DSP.codec_frequency);
227}
228
229static void tdspeed_set_pointers( bool time_stretch_active )
230{
231 if( time_stretch_active )
232 {
233 sample_buf_count = BIG_SAMPLE_BUF_COUNT;
234 resample_buf_count = BIG_RESAMPLE_BUF_COUNT;
235 sample_buf[0] = big_sample_buf[0];
236 sample_buf[1] = big_sample_buf[1];
237 resample_buf[0] = big_resample_buf[0];
238 resample_buf[1] = big_resample_buf[1];
239 }
240 else
241 {
242 sample_buf_count = SMALL_SAMPLE_BUF_COUNT;
243 resample_buf_count = SMALL_RESAMPLE_BUF_COUNT;
244 sample_buf[0] = small_sample_buf[0];
245 sample_buf[1] = small_sample_buf[1];
246 resample_buf[0] = small_resample_buf[0];
247 resample_buf[1] = small_resample_buf[1];
248 }
249}
250
251static void tdspeed_setup(struct dsp_config *dspc)
252{
253 /* Assume timestretch will not be used */
254 dspc->tdspeed_active = false;
255
256 tdspeed_set_pointers( false );
257
258 if (!dsp_timestretch_available())
259 return; /* Timestretch not enabled or buffer not allocated */
260
261 if (dspc->tdspeed_percent == 0)
262 dspc->tdspeed_percent = PITCH_SPEED_100;
263
264 if (!tdspeed_config(
265 dspc->codec_frequency == 0 ? NATIVE_FREQUENCY : dspc->codec_frequency,
266 dspc->stereo_mode != STEREO_MONO,
267 dspc->tdspeed_percent))
268 return; /* Timestretch not possible or needed with these parameters */
269
270 /* Timestretch is to be used */
271 dspc->tdspeed_active = true;
272
273 tdspeed_set_pointers( true );
274}
275
276
277static int move_callback(int handle, void* current, void* new)
278{
279 (void)handle;(void)current;
280
281 if ( big_sample_locks > 0 )
282 return BUFLIB_CB_CANNOT_MOVE;
283
284 big_sample_buf = new;
285
286 /* no allocation without timestretch enabled */
287 tdspeed_set_pointers( true );
288 return BUFLIB_CB_OK;
289}
290
291static void lock_sample_buf( bool lock )
292{
293 if ( lock )
294 big_sample_locks++;
295 else
296 big_sample_locks--;
297}
298
299static struct buflib_callbacks ops = {
300 .move_callback = move_callback,
301 .shrink_callback = NULL,
302};
303
304
305void dsp_timestretch_enable(bool enabled)
306{
307 /* Hook to set up timestretch buffer on first call to settings_apply() */
308 static int handle = -1;
309 if (enabled)
310 {
311 if (big_sample_buf)
312 return; /* already allocated and enabled */
313
314 /* Set up timestretch buffers */
315 big_sample_buf = &small_resample_buf[0];
316 handle = core_alloc_ex("resample buf",
317 2 * BIG_RESAMPLE_BUF_COUNT * sizeof(int32_t),
318 &ops);
319 big_sample_locks = 0;
320 enabled = handle >= 0;
321
322 if (enabled)
323 {
324 /* success, now setup tdspeed */
325 big_resample_buf = core_get_data(handle);
326
327 tdspeed_init();
328 tdspeed_setup(&AUDIO_DSP);
329 }
330 }
331
332 if (!enabled)
333 {
334 dsp_set_timestretch(PITCH_SPEED_100);
335 tdspeed_finish();
336
337 if (handle >= 0)
338 core_free(handle);
339
340 handle = -1;
341 big_sample_buf = NULL;
342 }
343}
344
345void dsp_set_timestretch(int32_t percent)
346{
347 AUDIO_DSP.tdspeed_percent = percent;
348 tdspeed_setup(&AUDIO_DSP);
349}
350
351int32_t dsp_get_timestretch()
352{
353 return AUDIO_DSP.tdspeed_percent;
354}
355
356bool dsp_timestretch_available()
357{
358 return (global_settings.timestretch_enabled && big_sample_buf);
359}
360#endif /* HAVE_PITCHSCREEN */
361
362/* Convert count samples to the internal format, if needed. Updates src
363 * to point past the samples "consumed" and dst is set to point to the
364 * samples to consume. Note that for mono, dst[0] equals dst[1], as there
365 * is no point in processing the same data twice.
366 */
367
368/* convert count 16-bit mono to 32-bit mono */
369static void sample_input_lte_native_mono(
370 int count, const char *src[], int32_t *dst[])
371{
372 const int16_t *s = (int16_t *) src[0];
373 const int16_t * const send = s + count;
374 int32_t *d = dst[0] = dst[1] = sample_buf[0];
375 int scale = WORD_SHIFT;
376
377 while (s < send)
378 {
379 *d++ = *s++ << scale;
380 }
381
382 src[0] = (char *)s;
383}
384
385/* convert count 16-bit interleaved stereo to 32-bit noninterleaved */
386static void sample_input_lte_native_i_stereo(
387 int count, const char *src[], int32_t *dst[])
388{
389 const int32_t *s = (int32_t *) src[0];
390 const int32_t * const send = s + count;
391 int32_t *dl = dst[0] = sample_buf[0];
392 int32_t *dr = dst[1] = sample_buf[1];
393 int scale = WORD_SHIFT;
394
395 while (s < send)
396 {
397 int32_t slr = *s++;
398#ifdef ROCKBOX_LITTLE_ENDIAN
399 *dl++ = (slr >> 16) << scale;
400 *dr++ = (int32_t)(int16_t)slr << scale;
401#else /* ROCKBOX_BIG_ENDIAN */
402 *dl++ = (int32_t)(int16_t)slr << scale;
403 *dr++ = (slr >> 16) << scale;
404#endif
405 }
406
407 src[0] = (char *)s;
408}
409
410/* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */
411static void sample_input_lte_native_ni_stereo(
412 int count, const char *src[], int32_t *dst[])
413{
414 const int16_t *sl = (int16_t *) src[0];
415 const int16_t *sr = (int16_t *) src[1];
416 const int16_t * const slend = sl + count;
417 int32_t *dl = dst[0] = sample_buf[0];
418 int32_t *dr = dst[1] = sample_buf[1];
419 int scale = WORD_SHIFT;
420
421 while (sl < slend)
422 {
423 *dl++ = *sl++ << scale;
424 *dr++ = *sr++ << scale;
425 }
426
427 src[0] = (char *)sl;
428 src[1] = (char *)sr;
429}
430
431/* convert count 32-bit mono to 32-bit mono */
432static void sample_input_gt_native_mono(
433 int count, const char *src[], int32_t *dst[])
434{
435 dst[0] = dst[1] = (int32_t *)src[0];
436 src[0] = (char *)(dst[0] + count);
437}
438
439/* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
440static void sample_input_gt_native_i_stereo(
441 int count, const char *src[], int32_t *dst[])
442{
443 const int32_t *s = (int32_t *)src[0];
444 const int32_t * const send = s + 2*count;
445 int32_t *dl = dst[0] = sample_buf[0];
446 int32_t *dr = dst[1] = sample_buf[1];
447
448 while (s < send)
449 {
450 *dl++ = *s++;
451 *dr++ = *s++;
452 }
453
454 src[0] = (char *)send;
455}
456
457/* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
458static void sample_input_gt_native_ni_stereo(
459 int count, const char *src[], int32_t *dst[])
460{
461 dst[0] = (int32_t *)src[0];
462 dst[1] = (int32_t *)src[1];
463 src[0] = (char *)(dst[0] + count);
464 src[1] = (char *)(dst[1] + count);
465}
466
467/**
468 * sample_input_new_format()
469 *
470 * set the to-native sample conversion function based on dsp sample parameters
471 *
472 * !DSPPARAMSYNC
473 * needs syncing with changes to the following dsp parameters:
474 * * dsp->stereo_mode (A/V)
475 * * dsp->sample_depth (A/V)
476 */
477static void sample_input_new_format(struct dsp_config *dsp)
478{
479 static const sample_input_fn_type sample_input_functions[] =
480 {
481 [SAMPLE_INPUT_LE_NATIVE_I_STEREO] = sample_input_lte_native_i_stereo,
482 [SAMPLE_INPUT_LE_NATIVE_NI_STEREO] = sample_input_lte_native_ni_stereo,
483 [SAMPLE_INPUT_LE_NATIVE_MONO] = sample_input_lte_native_mono,
484 [SAMPLE_INPUT_GT_NATIVE_I_STEREO] = sample_input_gt_native_i_stereo,
485 [SAMPLE_INPUT_GT_NATIVE_NI_STEREO] = sample_input_gt_native_ni_stereo,
486 [SAMPLE_INPUT_GT_NATIVE_MONO] = sample_input_gt_native_mono,
487 };
488
489 int convert = dsp->stereo_mode;
490
491 if (dsp->sample_depth > NATIVE_DEPTH)
492 convert += SAMPLE_INPUT_GT_NATIVE_1ST_INDEX;
493
494 dsp->input_samples = sample_input_functions[convert];
495}
496
497
498#ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
499/* write mono internal format to output format */
500static void sample_output_mono(int count, struct dsp_data *data,
501 const int32_t *src[], int16_t *dst)
502{
503 const int32_t *s0 = src[0];
504 const int scale = data->output_scale;
505 const int dc_bias = 1 << (scale - 1);
506
507 while (count-- > 0)
508 {
509 int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale);
510 *dst++ = lr;
511 *dst++ = lr;
512 }
513}
514#endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */
515
516/* write stereo internal format to output format */
517#ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
518static void sample_output_stereo(int count, struct dsp_data *data,
519 const int32_t *src[], int16_t *dst)
520{
521 const int32_t *s0 = src[0];
522 const int32_t *s1 = src[1];
523 const int scale = data->output_scale;
524 const int dc_bias = 1 << (scale - 1);
525
526 while (count-- > 0)
527 {
528 *dst++ = clip_sample_16((*s0++ + dc_bias) >> scale);
529 *dst++ = clip_sample_16((*s1++ + dc_bias) >> scale);
530 }
531}
532#endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */
533
534/**
535 * The "dither" code to convert the 24-bit samples produced by libmad was
536 * taken from the coolplayer project - coolplayer.sourceforge.net
537 *
538 * This function handles mono and stereo outputs.
539 */
540static void sample_output_dithered(int count, struct dsp_data *data,
541 const int32_t *src[], int16_t *dst)
542{
543 const int32_t mask = dither_mask;
544 const int32_t bias = dither_bias;
545 const int scale = data->output_scale;
546 const int32_t min = data->clip_min;
547 const int32_t max = data->clip_max;
548 const int32_t range = max - min;
549 int ch;
550 int16_t *d;
551
552 for (ch = 0; ch < data->num_channels; ch++)
553 {
554 struct dither_data * const dither = &dither_data[ch];
555 const int32_t *s = src[ch];
556 int i;
557
558 for (i = 0, d = &dst[ch]; i < count; i++, s++, d += 2)
559 {
560 int32_t output, sample;
561 int32_t random;
562
563 /* Noise shape and bias (for correct rounding later) */
564 sample = *s;
565 sample += dither->error[0] - dither->error[1] + dither->error[2];
566 dither->error[2] = dither->error[1];
567 dither->error[1] = dither->error[0]/2;
568
569 output = sample + bias;
570
571 /* Dither, highpass triangle PDF */
572 random = dither->random*0x0019660dL + 0x3c6ef35fL;
573 output += (random & mask) - (dither->random & mask);
574 dither->random = random;
575
576 /* Round sample to output range */
577 output &= ~mask;
578
579 /* Error feedback */
580 dither->error[0] = sample - output;
581
582 /* Clip */
583 if ((uint32_t)(output - min) > (uint32_t)range)
584 {
585 int32_t c = min;
586 if (output > min)
587 c += range;
588 output = c;
589 }
590
591 /* Quantize and store */
592 *d = output >> scale;
593 }
594 }
595
596 if (data->num_channels == 2)
597 return;
598
599 /* Have to duplicate left samples into the right channel since
600 pcm buffer and hardware is interleaved stereo */
601 d = &dst[0];
602
603 while (count-- > 0)
604 {
605 int16_t s = *d++;
606 *d++ = s;
607 }
608}
609
610/**
611 * sample_output_new_format()
612 *
613 * set the from-native to ouput sample conversion routine
614 *
615 * !DSPPARAMSYNC
616 * needs syncing with changes to the following dsp parameters:
617 * * dsp->stereo_mode (A/V)
618 * * dither_enabled (A)
619 */
620static void sample_output_new_format(struct dsp_config *dsp)
621{
622 static const sample_output_fn_type sample_output_functions[] =
623 {
624 sample_output_mono,
625 sample_output_stereo,
626 sample_output_dithered,
627 sample_output_dithered
628 };
629
630 int out = dsp->data.num_channels - 1;
631
632 if (dsp == &AUDIO_DSP && dither_enabled)
633 out += 2;
634
635 dsp->output_samples = sample_output_functions[out];
636}
637
638/**
639 * Linear interpolation resampling that introduces a one sample delay because
640 * of our inability to look into the future at the end of a frame.
641 */
642#ifndef DSP_HAVE_ASM_RESAMPLING
643static int dsp_downsample(int count, struct dsp_data *data,
644 const int32_t *src[], int32_t *dst[])
645{
646 int ch = data->num_channels - 1;
647 uint32_t delta = data->resample_data.delta;
648 uint32_t phase, pos;
649 int32_t *d;
650
651 /* Rolled channel loop actually showed slightly faster. */
652 do
653 {
654 /* Just initialize things and not worry too much about the relatively
655 * uncommon case of not being able to spit out a sample for the frame.
656 */
657 const int32_t *s = src[ch];
658 int32_t last = data->resample_data.last_sample[ch];
659
660 data->resample_data.last_sample[ch] = s[count - 1];
661 d = dst[ch];
662 phase = data->resample_data.phase;
663 pos = phase >> 16;
664
665 /* Do we need last sample of previous frame for interpolation? */
666 if (pos > 0)
667 last = s[pos - 1];
668
669 while (pos < (uint32_t)count)
670 {
671 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
672 phase += delta;
673 pos = phase >> 16;
674 last = s[pos - 1];
675 }
676 }
677 while (--ch >= 0);
678
679 /* Wrap phase accumulator back to start of next frame. */
680 data->resample_data.phase = phase - (count << 16);
681 return d - dst[0];
682}
683
684static int dsp_upsample(int count, struct dsp_data *data,
685 const int32_t *src[], int32_t *dst[])
686{
687 int ch = data->num_channels - 1;
688 uint32_t delta = data->resample_data.delta;
689 uint32_t phase, pos;
690 int32_t *d;
691
692 /* Rolled channel loop actually showed slightly faster. */
693 do
694 {
695 /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */
696 const int32_t *s = src[ch];
697 int32_t last = data->resample_data.last_sample[ch];
698
699 data->resample_data.last_sample[ch] = s[count - 1];
700 d = dst[ch];
701 phase = data->resample_data.phase;
702 pos = phase >> 16;
703
704 while (pos == 0)
705 {
706 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[0] - last);
707 phase += delta;
708 pos = phase >> 16;
709 }
710
711 while (pos < (uint32_t)count)
712 {
713 last = s[pos - 1];
714 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
715 phase += delta;
716 pos = phase >> 16;
717 }
718 }
719 while (--ch >= 0);
720
721 /* Wrap phase accumulator back to start of next frame. */
722 data->resample_data.phase = phase & 0xffff;
723 return d - dst[0];
724}
725#endif /* DSP_HAVE_ASM_RESAMPLING */
726
727static void resampler_new_delta(struct dsp_config *dsp)
728{
729 dsp->data.resample_data.delta = (unsigned long)
730 dsp->frequency * 65536LL / NATIVE_FREQUENCY;
731
732 if (dsp->frequency == NATIVE_FREQUENCY)
733 {
734 /* NOTE: If fully glitch-free transistions from no resampling to
735 resampling are desired, last_sample history should be maintained
736 even when not resampling. */
737 dsp->resample = NULL;
738 dsp->data.resample_data.phase = 0;
739 dsp->data.resample_data.last_sample[0] = 0;
740 dsp->data.resample_data.last_sample[1] = 0;
741 }
742 else if (dsp->frequency < NATIVE_FREQUENCY)
743 dsp->resample = dsp_upsample;
744 else
745 dsp->resample = dsp_downsample;
746}
747
748/* Resample count stereo samples. Updates the src array, if resampling is
749 * done, to refer to the resampled data. Returns number of stereo samples
750 * for further processing.
751 */
752static inline int resample(struct dsp_config *dsp, int count, int32_t *src[])
753{
754 int32_t *dst[2] =
755 {
756 resample_buf[0],
757 resample_buf[1]
758 };
759 lock_sample_buf( true );
760 count = dsp->resample(count, &dsp->data, (const int32_t **)src, dst);
761
762 src[0] = dst[0];
763 src[1] = dst[dsp->data.num_channels - 1];
764 lock_sample_buf( false );
765 return count;
766}
767
768static void dither_init(struct dsp_config *dsp)
769{
770 memset(dither_data, 0, sizeof (dither_data));
771 dither_bias = (1L << (dsp->data.frac_bits - NATIVE_DEPTH));
772 dither_mask = (1L << (dsp->data.frac_bits + 1 - NATIVE_DEPTH)) - 1;
773}
774
775void dsp_dither_enable(bool enable)
776{
777 struct dsp_config *dsp = &AUDIO_DSP;
778 dither_enabled = enable;
779 sample_output_new_format(dsp);
780}
781
782/* Applies crossfeed to the stereo signal in src.
783 * Crossfeed is a process where listening over speakers is simulated. This
784 * is good for old hard panned stereo records, which might be quite fatiguing
785 * to listen to on headphones with no crossfeed.
786 */
787#ifndef DSP_HAVE_ASM_CROSSFEED
788static void apply_crossfeed(int count, int32_t *buf[])
789{
790 int32_t *hist_l = &crossfeed_data.history[0];
791 int32_t *hist_r = &crossfeed_data.history[2];
792 int32_t *delay = &crossfeed_data.delay[0][0];
793 int32_t *coefs = &crossfeed_data.coefs[0];
794 int32_t gain = crossfeed_data.gain;
795 int32_t *di = crossfeed_data.index;
796
797 int32_t acc;
798 int32_t left, right;
799 int i;
800
801 for (i = 0; i < count; i++)
802 {
803 left = buf[0][i];
804 right = buf[1][i];
805
806 /* Filter delayed sample from left speaker */
807 acc = FRACMUL(*di, coefs[0]);
808 acc += FRACMUL(hist_l[0], coefs[1]);
809 acc += FRACMUL(hist_l[1], coefs[2]);
810 /* Save filter history for left speaker */
811 hist_l[1] = acc;
812 hist_l[0] = *di;
813 *di++ = left;
814 /* Filter delayed sample from right speaker */
815 acc = FRACMUL(*di, coefs[0]);
816 acc += FRACMUL(hist_r[0], coefs[1]);
817 acc += FRACMUL(hist_r[1], coefs[2]);
818 /* Save filter history for right speaker */
819 hist_r[1] = acc;
820 hist_r[0] = *di;
821 *di++ = right;
822 /* Now add the attenuated direct sound and write to outputs */
823 buf[0][i] = FRACMUL(left, gain) + hist_r[1];
824 buf[1][i] = FRACMUL(right, gain) + hist_l[1];
825
826 /* Wrap delay line index if bigger than delay line size */
827 if (di >= delay + 13*2)
828 di = delay;
829 }
830 /* Write back local copies of data we've modified */
831 crossfeed_data.index = di;
832}
833#endif /* DSP_HAVE_ASM_CROSSFEED */
834
835/**
836 * dsp_set_crossfeed(bool enable)
837 *
838 * !DSPPARAMSYNC
839 * needs syncing with changes to the following dsp parameters:
840 * * dsp->stereo_mode (A)
841 */
842void dsp_set_crossfeed(bool enable)
843{
844 crossfeed_enabled = enable;
845 AUDIO_DSP.apply_crossfeed = (enable && AUDIO_DSP.data.num_channels > 1)
846 ? apply_crossfeed : NULL;
847}
848
849void dsp_set_crossfeed_direct_gain(int gain)
850{
851 crossfeed_data.gain = get_replaygain_int(gain * 10) << 7;
852 /* If gain is negative, the calculation overflowed and we need to clamp */
853 if (crossfeed_data.gain < 0)
854 crossfeed_data.gain = 0x7fffffff;
855}
856
857/* Both gains should be below 0 dB */
858void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
859{
860 int32_t *c = crossfeed_data.coefs;
861 long scaler = get_replaygain_int(lf_gain * 10) << 7;
862
863 cutoff = 0xffffffff/NATIVE_FREQUENCY*cutoff;
864 hf_gain -= lf_gain;
865 /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
866 * point instead of shelf midpoint. This is for compatibility with the old
867 * crossfeed shelf filter and should be removed if crossfeed settings are
868 * ever made incompatible for any other good reason.
869 */
870 cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24);
871 filter_shelf_coefs(cutoff, hf_gain, false, c);
872 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
873 * over 1 and can do this safely
874 */
875 c[0] = FRACMUL_SHL(c[0], scaler, 4);
876 c[1] = FRACMUL_SHL(c[1], scaler, 4);
877 c[2] <<= 4;
878}
879
880/* Apply a constant gain to the samples (e.g., for ReplayGain).
881 * Note that this must be called before the resampler.
882 */
883#ifndef DSP_HAVE_ASM_APPLY_GAIN
884static void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[])
885{
886 const int32_t gain = data->gain;
887 int ch;
888
889 for (ch = 0; ch < data->num_channels; ch++)
890 {
891 int32_t *d = buf[ch];
892 int i;
893
894 for (i = 0; i < count; i++)
895 d[i] = FRACMUL_SHL(d[i], gain, 8);
896 }
897}
898#endif /* DSP_HAVE_ASM_APPLY_GAIN */
899
900/* Combine all gains to a global gain. */
901static void set_gain(struct dsp_config *dsp)
902{
903 /* gains are in S7.24 format */
904 dsp->data.gain = DEFAULT_GAIN;
905
906 /* Replay gain not relevant to voice */
907 if (dsp == &AUDIO_DSP && replaygain)
908 {
909 dsp->data.gain = replaygain;
910 }
911
912 if (dsp->eq_process && eq_precut)
913 {
914 dsp->data.gain = fp_mul(dsp->data.gain, eq_precut, 24);
915 }
916
917#ifdef HAVE_SW_VOLUME_CONTROL
918 if (global_settings.volume < SW_VOLUME_MAX ||
919 global_settings.volume > SW_VOLUME_MIN)
920 {
921 int vol_gain = get_replaygain_int(global_settings.volume * 100);
922 dsp->data.gain = (long) (((int64_t) dsp->data.gain * vol_gain) >> 24);
923 }
924#endif
925
926 if (dsp->data.gain == DEFAULT_GAIN)
927 {
928 dsp->data.gain = 0;
929 }
930 else
931 {
932 dsp->data.gain >>= 1; /* convert gain to S8.23 format */
933 }
934
935 dsp->apply_gain = dsp->data.gain != 0 ? dsp_apply_gain : NULL;
936}
937
938/**
939 * Update the amount to cut the audio before applying the equalizer.
940 *
941 * @param precut to apply in decibels (multiplied by 10)
942 */
943void dsp_set_eq_precut(int precut)
944{
945 eq_precut = get_replaygain_int(precut * -10);
946 set_gain(&AUDIO_DSP);
947}
948
949/**
950 * Synchronize the equalizer filter coefficients with the global settings.
951 *
952 * @param band the equalizer band to synchronize
953 */
954void dsp_set_eq_coefs(int band, int cutoff, int q, int gain)
955{
956 /* Convert user settings to format required by coef generator functions */
957 cutoff = 0xffffffff / NATIVE_FREQUENCY * cutoff;
958
959 if (q == 0)
960 q = 1;
961
962 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
963 which it should be, since we're executed from the main thread. */
964
965 /* Assume a band is disabled if the gain is zero */
966 if (gain == 0)
967 {
968 eq_data.enabled[band] = 0;
969 }
970 else
971 {
972 if (band == 0)
973 eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
974 else if (band == 4)
975 eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
976 else
977 eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
978
979 eq_data.enabled[band] = 1;
980 }
981}
982
983/* Apply EQ filters to those bands that have got it switched on. */
984static void eq_process(int count, int32_t *buf[])
985{
986 static const int shifts[] =
987 {
988 EQ_SHELF_SHIFT, /* low shelf */
989 EQ_PEAK_SHIFT, /* peaking */
990 EQ_PEAK_SHIFT, /* peaking */
991 EQ_PEAK_SHIFT, /* peaking */
992 EQ_SHELF_SHIFT, /* high shelf */
993 };
994 unsigned int channels = AUDIO_DSP.data.num_channels;
995 int i;
996
997 /* filter configuration currently is 1 low shelf filter, 3 band peaking
998 filters and 1 high shelf filter, in that order. we need to know this
999 so we can choose the correct shift factor.
1000 */
1001 for (i = 0; i < 5; i++)
1002 {
1003 if (!eq_data.enabled[i])
1004 continue;
1005 eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]);
1006 }
1007}
1008
1009/**
1010 * Use to enable the equalizer.
1011 *
1012 * @param enable true to enable the equalizer
1013 */
1014void dsp_set_eq(bool enable)
1015{
1016 AUDIO_DSP.eq_process = enable ? eq_process : NULL;
1017 set_gain(&AUDIO_DSP);
1018}
1019
1020static void dsp_set_stereo_width(int value)
1021{
1022 long width, straight, cross;
1023
1024 width = value * 0x7fffff / 100;
1025
1026 if (value <= 100)
1027 {
1028 straight = (0x7fffff + width) / 2;
1029 cross = straight - width;
1030 }
1031 else
1032 {
1033 /* straight = (1 + width) / (2 * width) */
1034 straight = ((int64_t)(0x7fffff + width) << 22) / width;
1035 cross = straight - 0x7fffff;
1036 }
1037
1038 dsp_sw_gain = straight << 8;
1039 dsp_sw_cross = cross << 8;
1040}
1041
1042/**
1043 * Implements the different channel configurations and stereo width.
1044 */
1045
1046/* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
1047 * completeness. */
1048#if 0
1049static void channels_process_sound_chan_stereo(int count, int32_t *buf[])
1050{
1051 /* The channels are each just themselves */
1052 (void)count; (void)buf;
1053}
1054#endif
1055
1056#ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO
1057static void channels_process_sound_chan_mono(int count, int32_t *buf[])
1058{
1059 int32_t *sl = buf[0], *sr = buf[1];
1060
1061 while (count-- > 0)
1062 {
1063 int32_t lr = *sl/2 + *sr/2;
1064 *sl++ = lr;
1065 *sr++ = lr;
1066 }
1067}
1068#endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
1069
1070#ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
1071static void channels_process_sound_chan_custom(int count, int32_t *buf[])
1072{
1073 const int32_t gain = dsp_sw_gain;
1074 const int32_t cross = dsp_sw_cross;
1075 int32_t *sl = buf[0], *sr = buf[1];
1076
1077 while (count-- > 0)
1078 {
1079 int32_t l = *sl;
1080 int32_t r = *sr;
1081 *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross);
1082 *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross);
1083 }
1084}
1085#endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */
1086
1087static void channels_process_sound_chan_mono_left(int count, int32_t *buf[])
1088{
1089 /* Just copy over the other channel */
1090 memcpy(buf[1], buf[0], count * sizeof (*buf));
1091}
1092
1093static void channels_process_sound_chan_mono_right(int count, int32_t *buf[])
1094{
1095 /* Just copy over the other channel */
1096 memcpy(buf[0], buf[1], count * sizeof (*buf));
1097}
1098
1099#ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
1100static void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
1101{
1102 int32_t *sl = buf[0], *sr = buf[1];
1103
1104 while (count-- > 0)
1105 {
1106 int32_t ch = *sl/2 - *sr/2;
1107 *sl++ = ch;
1108 *sr++ = -ch;
1109 }
1110}
1111#endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */
1112
1113static void dsp_set_channel_config(int value)
1114{
1115 static const channels_process_fn_type channels_process_functions[] =
1116 {
1117 /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */
1118 [SOUND_CHAN_STEREO] = NULL,
1119 [SOUND_CHAN_MONO] = channels_process_sound_chan_mono,
1120 [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom,
1121 [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left,
1122 [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right,
1123 [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke,
1124 };
1125
1126 if ((unsigned)value >= ARRAYLEN(channels_process_functions) ||
1127 AUDIO_DSP.stereo_mode == STEREO_MONO)
1128 {
1129 value = SOUND_CHAN_STEREO;
1130 }
1131
1132 /* This doesn't apply to voice */
1133 channels_mode = value;
1134 AUDIO_DSP.channels_process = channels_process_functions[value];
1135}
1136
1137#if CONFIG_CODEC == SWCODEC
1138
1139#ifdef HAVE_SW_TONE_CONTROLS
1140static void set_tone_controls(void)
1141{
1142 filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200,
1143 0xffffffff/NATIVE_FREQUENCY*3500,
1144 bass, treble, -prescale,
1145 AUDIO_DSP.tone_filter.coefs);
1146 /* Sync the voice dsp coefficients */
1147 memcpy(&VOICE_DSP.tone_filter.coefs, AUDIO_DSP.tone_filter.coefs,
1148 sizeof (VOICE_DSP.tone_filter.coefs));
1149}
1150#endif
1151
1152/* Hook back from firmware/ part of audio, which can't/shouldn't call apps/
1153 * code directly.
1154 */
1155int dsp_callback(int msg, intptr_t param)
1156{
1157 switch (msg)
1158 {
1159#ifdef HAVE_SW_TONE_CONTROLS
1160 case DSP_CALLBACK_SET_PRESCALE:
1161 prescale = param;
1162 set_tone_controls();
1163 break;
1164 /* prescaler is always set after calling any of these, so we wait with
1165 * calculating coefs until the above case is hit.
1166 */
1167 case DSP_CALLBACK_SET_BASS:
1168 bass = param;
1169 break;
1170 case DSP_CALLBACK_SET_TREBLE:
1171 treble = param;
1172 break;
1173#ifdef HAVE_SW_VOLUME_CONTROL
1174 case DSP_CALLBACK_SET_SW_VOLUME:
1175 set_gain(&AUDIO_DSP);
1176 break;
1177#endif
1178#endif
1179 case DSP_CALLBACK_SET_CHANNEL_CONFIG:
1180 dsp_set_channel_config(param);
1181 break;
1182 case DSP_CALLBACK_SET_STEREO_WIDTH:
1183 dsp_set_stereo_width(param);
1184 break;
1185 default:
1186 break;
1187 }
1188 return 0;
1189}
1190#endif
1191
1192/* Process and convert src audio to dst based on the DSP configuration,
1193 * reading count number of audio samples. dst is assumed to be large
1194 * enough; use dsp_output_count() to get the required number. src is an
1195 * array of pointers; for mono and interleaved stereo, it contains one
1196 * pointer to the start of the audio data and the other is ignored; for
1197 * non-interleaved stereo, it contains two pointers, one for each audio
1198 * channel. Returns number of bytes written to dst.
1199 */
1200int dsp_process(struct dsp_config *dsp, char *dst, const char *src[], int count)
1201{
1202 static int32_t *tmp[2]; /* tdspeed_doit() needs it static */
1203 static long last_yield;
1204 long tick;
1205 int written = 0;
1206
1207#if defined(CPU_COLDFIRE)
1208 /* set emac unit for dsp processing, and save old macsr, we're running in
1209 codec thread context at this point, so can't clobber it */
1210 unsigned long old_macsr = coldfire_get_macsr();
1211 coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
1212#endif
1213
1214 if (new_gain)
1215 dsp_set_replaygain(); /* Gain has changed */
1216
1217 /* Perform at least one yield before starting */
1218 last_yield = current_tick;
1219 yield();
1220
1221 /* Testing function pointers for NULL is preferred since the pointer
1222 will be preloaded to be used for the call if not. */
1223 while (count > 0)
1224 {
1225 int samples = MIN(sample_buf_count, count);
1226 count -= samples;
1227
1228 dsp->input_samples(samples, src, tmp);
1229
1230#ifdef HAVE_PITCHSCREEN
1231 if (dsp->tdspeed_active)
1232 samples = tdspeed_doit(tmp, samples);
1233#endif
1234
1235 int chunk_offset = 0;
1236 while (samples > 0)
1237 {
1238 int32_t *t2[2];
1239 t2[0] = tmp[0]+chunk_offset;
1240 t2[1] = tmp[1]+chunk_offset;
1241
1242 int chunk = MIN(sample_buf_count, samples);
1243 chunk_offset += chunk;
1244 samples -= chunk;
1245
1246 if (dsp->apply_gain)
1247 dsp->apply_gain(chunk, &dsp->data, t2);
1248
1249 if (dsp->resample && (chunk = resample(dsp, chunk, t2)) <= 0)
1250 break; /* I'm pretty sure we're downsampling here */
1251
1252 if (dsp->apply_crossfeed)
1253 dsp->apply_crossfeed(chunk, t2);
1254
1255 if (dsp->eq_process)
1256 dsp->eq_process(chunk, t2);
1257
1258#ifdef HAVE_SW_TONE_CONTROLS
1259 if ((bass | treble) != 0)
1260 eq_filter(t2, &dsp->tone_filter, chunk,
1261 dsp->data.num_channels, FILTER_BISHELF_SHIFT);
1262#endif
1263
1264 if (dsp->channels_process)
1265 dsp->channels_process(chunk, t2);
1266
1267 if (dsp->compressor_process)
1268 dsp->compressor_process(chunk, &dsp->data, t2);
1269
1270 dsp->output_samples(chunk, &dsp->data, (const int32_t **)t2, (int16_t *)dst);
1271
1272 written += chunk;
1273 dst += chunk * sizeof (int16_t) * 2;
1274
1275 /* yield at least once each tick */
1276 tick = current_tick;
1277 if (TIME_AFTER(tick, last_yield))
1278 {
1279 last_yield = tick;
1280 yield();
1281 }
1282 }
1283 }
1284
1285#if defined(CPU_COLDFIRE)
1286 /* set old macsr again */
1287 coldfire_set_macsr(old_macsr);
1288#endif
1289 return written;
1290}
1291
1292/* Given count number of input samples, calculate the maximum number of
1293 * samples of output data that would be generated (the calculation is not
1294 * entirely exact and rounds upwards to be on the safe side; during
1295 * resampling, the number of samples generated depends on the current state
1296 * of the resampler).
1297 */
1298/* dsp_input_size MUST be called afterwards */
1299int dsp_output_count(struct dsp_config *dsp, int count)
1300{
1301#ifdef HAVE_PITCHSCREEN
1302 if (dsp->tdspeed_active)
1303 count = tdspeed_est_output_size();
1304#endif
1305 if (dsp->resample)
1306 {
1307 count = (int)(((unsigned long)count * NATIVE_FREQUENCY
1308 + (dsp->frequency - 1)) / dsp->frequency);
1309 }
1310
1311 /* Now we have the resampled sample count which must not exceed
1312 * resample_buf_count to avoid resample buffer overflow. One
1313 * must call dsp_input_count() to get the correct input sample
1314 * count.
1315 */
1316 if (count > resample_buf_count)
1317 count = resample_buf_count;
1318
1319 return count;
1320}
1321
1322/* Given count output samples, calculate number of input samples
1323 * that would be consumed in order to fill the output buffer.
1324 */
1325int dsp_input_count(struct dsp_config *dsp, int count)
1326{
1327 /* count is now the number of resampled input samples. Convert to
1328 original input samples. */
1329 if (dsp->resample)
1330 {
1331 /* Use the real resampling delta =
1332 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1333 * round towards zero to avoid buffer overflows. */
1334 count = (int)(((unsigned long)count *
1335 dsp->data.resample_data.delta) >> 16);
1336 }
1337
1338#ifdef HAVE_PITCHSCREEN
1339 if (dsp->tdspeed_active)
1340 count = tdspeed_est_input_size(count);
1341#endif
1342
1343 return count;
1344}
1345
1346static void dsp_set_gain_var(long *var, long value)
1347{
1348 *var = value;
1349 new_gain = true;
1350}
1351
1352static void dsp_update_functions(struct dsp_config *dsp)
1353{
1354 sample_input_new_format(dsp);
1355 sample_output_new_format(dsp);
1356 if (dsp == &AUDIO_DSP)
1357 dsp_set_crossfeed(crossfeed_enabled);
1358}
1359
1360intptr_t dsp_configure(struct dsp_config *dsp, int setting, intptr_t value)
1361{
1362 switch (setting)
1363 {
1364 case DSP_MYDSP:
1365 switch (value)
1366 {
1367 case CODEC_IDX_AUDIO:
1368 return (intptr_t)&AUDIO_DSP;
1369 case CODEC_IDX_VOICE:
1370 return (intptr_t)&VOICE_DSP;
1371 default:
1372 return (intptr_t)NULL;
1373 }
1374
1375 case DSP_SET_FREQUENCY:
1376 memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data));
1377 /* Fall through!!! */
1378 case DSP_SWITCH_FREQUENCY:
1379 dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value;
1380 /* Account for playback speed adjustment when setting dsp->frequency
1381 if we're called from the main audio thread. Voice UI thread should
1382 not need this feature.
1383 */
1384#ifdef HAVE_PITCHSCREEN
1385 if (dsp == &AUDIO_DSP)
1386 dsp->frequency = pitch_ratio * dsp->codec_frequency / PITCH_SPEED_100;
1387 else
1388#endif
1389 dsp->frequency = dsp->codec_frequency;
1390
1391 resampler_new_delta(dsp);
1392#ifdef HAVE_PITCHSCREEN
1393 tdspeed_setup(dsp);
1394#endif
1395 break;
1396
1397 case DSP_SET_SAMPLE_DEPTH:
1398 dsp->sample_depth = value;
1399
1400 if (dsp->sample_depth <= NATIVE_DEPTH)
1401 {
1402 dsp->data.frac_bits = WORD_FRACBITS;
1403 dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */
1404 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1405 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1406 }
1407 else
1408 {
1409 dsp->data.frac_bits = value;
1410 dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */
1411 dsp->data.clip_max = (1 << value) - 1;
1412 dsp->data.clip_min = -(1 << value);
1413 }
1414
1415 dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH;
1416 sample_input_new_format(dsp);
1417 dither_init(dsp);
1418 break;
1419
1420 case DSP_SET_STEREO_MODE:
1421 dsp->stereo_mode = value;
1422 dsp->data.num_channels = value == STEREO_MONO ? 1 : 2;
1423 dsp_update_functions(dsp);
1424#ifdef HAVE_PITCHSCREEN
1425 tdspeed_setup(dsp);
1426#endif
1427 break;
1428
1429 case DSP_RESET:
1430 dsp->stereo_mode = STEREO_NONINTERLEAVED;
1431 dsp->data.num_channels = 2;
1432 dsp->sample_depth = NATIVE_DEPTH;
1433 dsp->data.frac_bits = WORD_FRACBITS;
1434 dsp->sample_bytes = sizeof (int16_t);
1435 dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH;
1436 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1437 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1438 dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY;
1439
1440 if (dsp == &AUDIO_DSP)
1441 {
1442 track_gain = 0;
1443 album_gain = 0;
1444 track_peak = 0;
1445 album_peak = 0;
1446 new_gain = true;
1447 }
1448
1449 dsp_update_functions(dsp);
1450 resampler_new_delta(dsp);
1451#ifdef HAVE_PITCHSCREEN
1452 tdspeed_setup(dsp);
1453#endif
1454 if (dsp == &AUDIO_DSP)
1455 compressor_reset();
1456 break;
1457
1458 case DSP_FLUSH:
1459 memset(&dsp->data.resample_data, 0,
1460 sizeof (dsp->data.resample_data));
1461 resampler_new_delta(dsp);
1462 dither_init(dsp);
1463#ifdef HAVE_PITCHSCREEN
1464 tdspeed_setup(dsp);
1465#endif
1466 if (dsp == &AUDIO_DSP)
1467 compressor_reset();
1468 break;
1469
1470 case DSP_SET_TRACK_GAIN:
1471 if (dsp == &AUDIO_DSP)
1472 dsp_set_gain_var(&track_gain, value);
1473 break;
1474
1475 case DSP_SET_ALBUM_GAIN:
1476 if (dsp == &AUDIO_DSP)
1477 dsp_set_gain_var(&album_gain, value);
1478 break;
1479
1480 case DSP_SET_TRACK_PEAK:
1481 if (dsp == &AUDIO_DSP)
1482 dsp_set_gain_var(&track_peak, value);
1483 break;
1484
1485 case DSP_SET_ALBUM_PEAK:
1486 if (dsp == &AUDIO_DSP)
1487 dsp_set_gain_var(&album_peak, value);
1488 break;
1489
1490 default:
1491 return 0;
1492 }
1493
1494 return 1;
1495}
1496
1497int get_replaygain_mode(bool have_track_gain, bool have_album_gain)
1498{
1499 int type;
1500
1501 bool track = ((global_settings.replaygain_type == REPLAYGAIN_TRACK)
1502 || ((global_settings.replaygain_type == REPLAYGAIN_SHUFFLE)
1503 && global_settings.playlist_shuffle));
1504
1505 type = (!track && have_album_gain) ? REPLAYGAIN_ALBUM
1506 : have_track_gain ? REPLAYGAIN_TRACK : -1;
1507
1508 return type;
1509}
1510
1511void dsp_set_replaygain(void)
1512{
1513 long gain = 0;
1514
1515 new_gain = false;
1516
1517 if ((global_settings.replaygain_type != REPLAYGAIN_OFF) ||
1518 global_settings.replaygain_noclip)
1519 {
1520 bool track_mode = get_replaygain_mode(track_gain != 0,
1521 album_gain != 0) == REPLAYGAIN_TRACK;
1522 long peak = (track_mode || !album_peak) ? track_peak : album_peak;
1523
1524 if (global_settings.replaygain_type != REPLAYGAIN_OFF)
1525 {
1526 gain = (track_mode || !album_gain) ? track_gain : album_gain;
1527
1528 if (global_settings.replaygain_preamp)
1529 {
1530 long preamp = get_replaygain_int(
1531 global_settings.replaygain_preamp * 10);
1532
1533 gain = (long) (((int64_t) gain * preamp) >> 24);
1534 }
1535 }
1536
1537 if (gain == 0)
1538 {
1539 /* So that noclip can work even with no gain information. */
1540 gain = DEFAULT_GAIN;
1541 }
1542
1543 if (global_settings.replaygain_noclip && (peak != 0)
1544 && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN))
1545 {
1546 gain = (((int64_t) DEFAULT_GAIN << 24) / peak);
1547 }
1548
1549 if (gain == DEFAULT_GAIN)
1550 {
1551 /* Nothing to do, disable processing. */
1552 gain = 0;
1553 }
1554 }
1555
1556 /* Store in S7.24 format to simplify calculations. */
1557 replaygain = gain;
1558 set_gain(&AUDIO_DSP);
1559}
1560
1561/** SET COMPRESSOR
1562 * Called by the menu system to configure the compressor process */
1563void dsp_set_compressor(const struct compressor_settings *settings)
1564{
1565 /* enable/disable the compressor */
1566 AUDIO_DSP.compressor_process = compressor_update(settings) ?
1567 compressor_process : NULL;
1568}
diff --git a/lib/rbcodec/dsp/dsp.h b/lib/rbcodec/dsp/dsp.h
index a99df17468..feac4aa845 100644
--- a/lib/rbcodec/dsp/dsp.h
+++ b/lib/rbcodec/dsp/dsp.h
@@ -18,109 +18,159 @@
18 * KIND, either express or implied. 18 * KIND, either express or implied.
19 * 19 *
20 ****************************************************************************/ 20 ****************************************************************************/
21
22#ifndef _DSP_H 21#ifndef _DSP_H
23#define _DSP_H 22#define _DSP_H
24 23
25#include <stdlib.h> 24struct dsp_config;
26#include <stdbool.h>
27 25
28#define NATIVE_FREQUENCY 44100 26/* Include all this junk here for now */
27#include "dsp_proc_settings.h"
29 28
30enum 29enum dsp_ids
31{ 30{
32 STEREO_INTERLEAVED = 0, 31 CODEC_IDX_AUDIO,
33 STEREO_NONINTERLEAVED,
34 STEREO_MONO,
35 STEREO_NUM_MODES,
36};
37
38enum
39{
40 CODEC_IDX_AUDIO = 0,
41 CODEC_IDX_VOICE, 32 CODEC_IDX_VOICE,
33 DSP_COUNT,
42}; 34};
43 35
44enum 36enum dsp_settings
45{ 37{
46 DSP_MYDSP = 1, 38 DSP_INIT, /* For dsp_init */
39 DSP_RESET,
47 DSP_SET_FREQUENCY, 40 DSP_SET_FREQUENCY,
48 DSP_SWITCH_FREQUENCY, 41 DSP_SWITCH_FREQUENCY = DSP_SET_FREQUENCY, /* deprecated */
49 DSP_SET_SAMPLE_DEPTH, 42 DSP_SET_SAMPLE_DEPTH,
50 DSP_SET_STEREO_MODE, 43 DSP_SET_STEREO_MODE,
51 DSP_RESET,
52 DSP_FLUSH, 44 DSP_FLUSH,
53 DSP_SET_TRACK_GAIN, 45 DSP_PROC_INIT,
54 DSP_SET_ALBUM_GAIN, 46 DSP_PROC_CLOSE,
55 DSP_SET_TRACK_PEAK, 47 DSP_PROC_SETTING, /* stage-specific should be this + id */
56 DSP_SET_ALBUM_PEAK,
57 DSP_CROSSFEED
58}; 48};
59 49
50#define NATIVE_FREQUENCY 44100 /* internal/output sample rate */
60 51
61/**************************************************************************** 52enum dsp_stereo_modes
62 * NOTE: Any assembly routines that use these structures must be updated
63 * if current data members are moved or changed.
64 */
65struct resample_data
66{ 53{
67 uint32_t delta; /* 00h */ 54 STEREO_INTERLEAVED,
68 uint32_t phase; /* 04h */ 55 STEREO_NONINTERLEAVED,
69 int32_t last_sample[2]; /* 08h */ 56 STEREO_MONO,
70 /* 10h */ 57 STEREO_NUM_MODES,
71}; 58};
72 59
73/* This is for passing needed data to external dsp routines. If another 60/* Format into for the buffer (if .valid == true) */
74 * dsp parameter needs to be passed, add to the end of the structure 61struct sample_format
75 * and remove from dsp_config.
76 * If another function type becomes assembly/external and requires dsp
77 * config info, add a pointer paramter of type "struct dsp_data *".
78 * If removing something from other than the end, reserve the spot or
79 * else update every implementation for every target.
80 * Be sure to add the offset of the new member for easy viewing as well. :)
81 * It is the first member of dsp_config and all members can be accessesed
82 * through the main aggregate but this is intended to make a safe haven
83 * for these items whereas the c part can be rearranged at will. dsp_data
84 * could even moved within dsp_config without disurbing the order.
85 */
86struct dsp_data
87{ 62{
88 int output_scale; /* 00h */ 63 uint8_t changed; /* 00h: 0=no change, 1=changed (is also index) */
89 int num_channels; /* 04h */ 64 uint8_t num_channels; /* 01h: number of channels of data */
90 struct resample_data resample_data; /* 08h */ 65 uint8_t frac_bits; /* 02h: number of fractional bits */
91 int32_t clip_min; /* 18h */ 66 uint8_t output_scale; /* 03h: output scaling shift */
92 int32_t clip_max; /* 1ch */ 67 int32_t frequency; /* 04h: pitch-adjusted sample rate */
93 int32_t gain; /* 20h - Note that this is in S8.23 format. */ 68 int32_t codec_frequency; /* 08h: codec-specifed sample rate */
94 int frac_bits; /* 24h */ 69 /* 0ch */
95 /* 28h */
96}; 70};
97 71
98struct dsp_config; 72/* Compare format data only */
73#define EQU_SAMPLE_FORMAT(f1, f2) \
74 (!memcmp(&(f1).num_channels, &(f2).num_channels, \
75 sizeof (f1) - sizeof ((f1).changed)))
76
77static inline void format_change_set(struct sample_format *f)
78 { f->changed = 1; }
79static inline void format_change_ack(struct sample_format *f)
80 { f->changed = 0; }
99 81
100int dsp_process(struct dsp_config *dsp, char *dest, 82/* Used by ASM routines - keep field order or else fix the functions */
101 const char *src[], int count); 83struct dsp_buffer
102int dsp_input_count(struct dsp_config *dsp, int count); 84{
103int dsp_output_count(struct dsp_config *dsp, int count); 85 int32_t remcount; /* 00h: Samples in buffer (In, Int, Out) */
104intptr_t dsp_configure(struct dsp_config *dsp, int setting, 86 union
87 {
88 const void *pin[2]; /* 04h: Channel pointers (In) */
89 int32_t *p32[2]; /* 04h: Channel pointers (Int) */
90 int16_t *p16out; /* 04h: DSP output buffer (Out) */
91 };
92 union
93 {
94 uint32_t proc_mask; /* 0Ch: In-place effects already appled to buffer
95 in order to avoid double-processing. Set
96 to zero on new buffer before passing to
97 DSP. */
98 int bufcount; /* 0Ch: Buffer length/dest buffer remaining
99 Basically, pay no attention unless it's
100 *your* new buffer and is used internally
101 or is specifically the final output
102 buffer. */
103 };
104 struct sample_format format; /* 10h: Buffer format data */
105 /* 1ch */
106};
107
108/* Remove samples from input buffer (In). Sample size is specified.
109 Provided to dsp_process(). */
110static inline void dsp_advance_buffer_input(struct dsp_buffer *buf,
111 int by_count,
112 size_t size_each)
113{
114 buf->remcount -= by_count;
115 buf->pin[0] += by_count * size_each;
116 buf->pin[1] += by_count * size_each;
117}
118
119/* Add samples to output buffer and update remaining space (Out).
120 Provided to dsp_process() */
121static inline void dsp_advance_buffer_output(struct dsp_buffer *buf,
122 int by_count)
123{
124 buf->bufcount -= by_count;
125 buf->remcount += by_count;
126 buf->p16out += 2 * by_count; /* Interleaved stereo */
127}
128
129/* Remove samples from internal input buffer (In, Int).
130 Provided to dsp_process() or by another processing stage. */
131static inline void dsp_advance_buffer32(struct dsp_buffer *buf,
132 int by_count)
133{
134 buf->remcount -= by_count;
135 buf->p32[0] += by_count;
136 buf->p32[1] += by_count;
137}
138
139/** For use by processing stages **/
140
141#define DSP_PRINT_FORMAT(name, id, format) \
142 DEBUGF("DSP format- " #name "\n" \
143 " id:%d chg:%c ch:%u fb:%u os:%u hz:%u chz:%u\n", \
144 (int)id, \
145 (format).changed ? 'y' : 'n', \
146 (unsigned int)(format).num_channels, \
147 (unsigned int)(format).frac_bits, \
148 (unsigned int)(format).output_scale, \
149 (unsigned int)(format).frequency, \
150 (unsigned int)(format).codec_frequency);
151
152/* Get DSP pointer */
153struct dsp_config * dsp_get_config(enum dsp_ids id);
154
155/* Get DSP id */
156enum dsp_ids dsp_get_id(const struct dsp_config *dsp);
157
158#if 0 /* Not needed now but enable if something must know this */
159/* Is the DSP processing a buffer? */
160bool dsp_is_busy(const struct dsp_config *dsp);
161#endif /* 0 */
162
163/** General DSP processing **/
164
165/* Process the given buffer - see implementation in dsp.c for more */
166void dsp_process(struct dsp_config *dsp, struct dsp_buffer *src,
167 struct dsp_buffer *dst);
168
169/* Change DSP settings */
170intptr_t dsp_configure(struct dsp_config *dsp, unsigned int setting,
105 intptr_t value); 171 intptr_t value);
106int get_replaygain_mode(bool have_track_gain, bool have_album_gain); 172
107void dsp_set_replaygain(void); 173/* One-time startup init that must come before settings reset/apply */
108void dsp_set_crossfeed(bool enable); 174void dsp_init(void);
109void dsp_set_crossfeed_direct_gain(int gain); 175
110void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, 176#endif /* _DSP_H */
111 long cutoff);
112void dsp_set_eq(bool enable);
113void dsp_set_eq_precut(int precut);
114void dsp_set_eq_coefs(int band, int cutoff, int q, int gain);
115void dsp_dither_enable(bool enable);
116void dsp_timestretch_enable(bool enable);
117bool dsp_timestretch_available(void);
118void sound_set_pitch(int32_t r);
119int32_t sound_get_pitch(void);
120void dsp_set_timestretch(int32_t percent);
121int32_t dsp_get_timestretch(void);
122int dsp_callback(int msg, intptr_t param);
123struct compressor_settings;
124void dsp_set_compressor(const struct compressor_settings *settings);
125
126#endif
diff --git a/lib/rbcodec/dsp/dsp_arm.S b/lib/rbcodec/dsp/dsp_arm.S
index 685aca411c..9fd19ae108 100644
--- a/lib/rbcodec/dsp/dsp_arm.S
+++ b/lib/rbcodec/dsp/dsp_arm.S
@@ -21,20 +21,19 @@
21 #include "config.h" 21 #include "config.h"
22 22
23/**************************************************************************** 23/****************************************************************************
24 * void channels_process_sound_chan_mono(int count, int32_t *buf[]) 24 * void channel_mode_proc_mono(struct dsp_proc_entry *this,
25 * struct dsp_buffer **buf_p)
25 */ 26 */
26 27 .section .icode
27#include "config.h" 28 .global channel_mode_proc_mono
28 29 .type channel_mode_proc_mono, %function
29 .section .icode, "ax", %progbits 30channel_mode_proc_mono:
30 .align 2 31 @ input: r0 = this, r1 = buf_p
31 .global channels_process_sound_chan_mono 32 ldr r1, [r1] @ r1 = buf = *buf_p;
32 .type channels_process_sound_chan_mono, %function
33channels_process_sound_chan_mono:
34 @ input: r0 = count, r1 = buf
35 stmfd sp!, { r4, lr } @ 33 stmfd sp!, { r4, lr } @
36 @ 34 @
37 ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1] 35 ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0],
36 @ r2 = buf->p32[1]
38 subs r0, r0, #1 @ odd: end at 0; even: end at -1 37 subs r0, r0, #1 @ odd: end at 0; even: end at -1
39 beq .mono_singlesample @ Zero? Only one sample! 38 beq .mono_singlesample @ Zero? Only one sample!
40 @ 39 @
@@ -61,25 +60,26 @@ channels_process_sound_chan_mono:
61 str r12, [r2] @ store Mo 60 str r12, [r2] @ store Mo
62 @ 61 @
63 ldmpc regs=r4 @ 62 ldmpc regs=r4 @
64 .size channels_process_sound_chan_mono, \ 63 .size channel_mode_proc_mono, .-channel_mode_proc_mono
65 .-channels_process_sound_chan_mono
66 64
67/**************************************************************************** 65/****************************************************************************
68 * void channels_process_sound_chan_custom(int count, int32_t *buf[]) 66 * void channel_mode_proc_custom(struct dsp_proc_entry *this,
67 * struct dsp_buffer **buf_p)
69 */ 68 */
70 .section .icode, "ax", %progbits 69 .section .icode
71 .align 2 70 .global channel_mode_proc_custom
72 .global channels_process_sound_chan_custom 71 .type channel_mode_proc_custom, %function
73 .type channels_process_sound_chan_custom, %function 72channel_mode_proc_custom:
74channels_process_sound_chan_custom: 73 @ input: r0 = this, r1 = buf_p
74 ldr r2, [r0] @ r2 = &channel_mode_data = this->data
75 ldr r1, [r1] @ r1 = buf = *buf_p;
76
75 stmfd sp!, { r4-r10, lr } 77 stmfd sp!, { r4-r10, lr }
76 78
77 ldr r3, =dsp_sw_gain 79 ldmia r2, { r3, r4 } @ r3 = sw_gain, r4 = sw_cross
78 ldr r4, =dsp_sw_cross
79 80
80 ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1] 81 ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0],
81 ldr r3, [r3] @ r3 = dsp_sw_gain 82 @ r2 = buf->p32[1]
82 ldr r4, [r4] @ r4 = dsp_sw_cross
83 83
84 subs r0, r0, #1 84 subs r0, r0, #1
85 beq .custom_single_sample @ Zero? Only one sample! 85 beq .custom_single_sample @ Zero? Only one sample!
@@ -135,21 +135,22 @@ channels_process_sound_chan_custom:
135 str r7, [r2] @ Store Rc0 135 str r7, [r2] @ Store Rc0
136 136
137 ldmpc regs=r4-r10 137 ldmpc regs=r4-r10
138 .size channels_process_sound_chan_custom, \ 138 .size channel_mode_proc_custom, .-channel_mode_proc_custom
139 .-channels_process_sound_chan_custom
140 139
141/**************************************************************************** 140/****************************************************************************
142 * void channels_process_sound_chan_karaoke(int count, int32_t *buf[]) 141 * void channel_mode_proc_karaoke(struct dsp_proc_entry *this,
142 * struct dsp_buffer **buf_p)
143 */ 143 */
144 .section .icode, "ax", %progbits 144 .section .icode
145 .align 2 145 .global channel_mode_proc_karaoke
146 .global channels_process_sound_chan_karaoke 146 .type channel_mode_proc_karaoke, %function
147 .type channels_process_sound_chan_karaoke, %function 147channel_mode_proc_karaoke:
148channels_process_sound_chan_karaoke: 148 @ input: r0 = this, r1 = buf_p
149 @ input: r0 = count, r1 = buf 149 ldr r1, [r1] @ r1 = buf = *buf_p;
150 stmfd sp!, { r4, lr } @ 150 stmfd sp!, { r4, lr } @
151 @ 151 @
152 ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1] 152 ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0],
153 @ r2 = buf->p32[1]
153 subs r0, r0, #1 @ odd: end at 0; even: end at -1 154 subs r0, r0, #1 @ odd: end at 0; even: end at -1
154 beq .karaoke_singlesample @ Zero? Only one sample! 155 beq .karaoke_singlesample @ Zero? Only one sample!
155 @ 156 @
@@ -179,24 +180,313 @@ channels_process_sound_chan_karaoke:
179 str r12, [r2] @ store Ro 180 str r12, [r2] @ store Ro
180 @ 181 @
181 ldmpc regs=r4 @ 182 ldmpc regs=r4 @
182 .size channels_process_sound_chan_karaoke, \ 183 .size channel_mode_proc_karaoke, .-channel_mode_proc_karaoke
183 .-channels_process_sound_chan_karaoke 184
185/****************************************************************************
186 * void crossfeed_process(struct dsp_proc_entry *this,
187 * struct dsp_buffer **buf_p)
188 */
189 .section .text
190 .global crossfeed_process
191crossfeed_process:
192 @ input: r0 = this, r1 = buf_p
193 @ unfortunately, we ended up in a bit of a register squeeze here, and need
194 @ to keep the count on the stack :/
195 ldr r1, [r1] @ r1 = buf = *buf_p;
196 stmfd sp!, { r4-r11, lr } @ stack modified regs
197 ldr r12, [r1] @ r12 = buf->remcount
198 ldr r14, [r0] @ r14 = this->data = &crossfeed_state
199 ldmib r1, { r2-r3 } @ r2 = buf->p32[0], r3 = buf->p32[1]
200 ldmia r14!, { r4-r11 } @ load direct gain and filter data
201 add r0, r14, #13*2*4 @ calculate end of delay
202 stmfd sp!, { r0, r12 } @ stack end of delay adr, count and state
203 ldr r0, [r0] @ fetch current delay line address
204
205 /* Register usage in loop:
206 * r0 = &delay[index][0], r1 = accumulator high, r2 = buf->p32[0],
207 * r3 = buf->p32[1], r4 = direct gain, r5-r7 = b0, b1, a1 (filter coefs),
208 * r8-r11 = filter history, r12 = temp, r14 = accumulator low
209 */
210.cfloop:
211 smull r14, r1, r6, r8 @ acc = b1*dr[n - 1]
212 smlal r14, r1, r7, r9 @ acc += a1*y_l[n - 1]
213 ldr r8, [r0, #4] @ r8 = dr[n]
214 smlal r14, r1, r5, r8 @ acc += b0*dr[n]
215 mov r9, r1, lsl #1 @ fix format for filter history
216 ldr r12, [r2] @ load left input
217 smlal r14, r1, r4, r12 @ acc += gain*x_l[n]
218 mov r1, r1, lsl #1 @ fix format
219 str r1, [r2], #4 @ save result
220
221 smull r14, r1, r6, r10 @ acc = b1*dl[n - 1]
222 smlal r14, r1, r7, r11 @ acc += a1*y_r[n - 1]
223 ldr r10, [r0] @ r10 = dl[n]
224 str r12, [r0], #4 @ save left input to delay line
225 smlal r14, r1, r5, r10 @ acc += b0*dl[n]
226 mov r11, r1, lsl #1 @ fix format for filter history
227 ldr r12, [r3] @ load right input
228 smlal r14, r1, r4, r12 @ acc += gain*x_r[n]
229 str r12, [r0], #4 @ save right input to delay line
230 mov r1, r1, lsl #1 @ fix format
231 ldmia sp, { r12, r14 } @ fetch delay line end addr and count from stack
232 str r1, [r3], #4 @ save result
233
234 cmp r0, r12 @ need to wrap to start of delay?
235 subhs r0, r12, #13*2*4 @ wrap back delay line ptr to start
236
237 subs r14, r14, #1 @ are we finished?
238 strgt r14, [sp, #4] @ nope, save count back to stack
239 bgt .cfloop
240
241 @ save data back to struct
242 str r0, [r12] @ save delay line index
243 sub r12, r12, #13*2*4 + 4*4 @ r12 = data->history
244 stmia r12, { r8-r11 } @ save filter history
245 add sp, sp, #8 @ remove temp variables from stack
246 ldmpc regs=r4-r11
247 .size crossfeed_process, .-crossfeed_process
248
249/****************************************************************************
250 * int lin_resample_resample(struct resample_data *data,
251 * struct dsp_buffer *src,
252 * struct dsp_buffer *dst)
253 */
254 .section .text
255 .global lin_resample_resample
256lin_resample_resample:
257 @input: r0 = data, r1 = src, r2 = dst
258 stmfd sp!, { r4-r11, lr } @ stack modified regs
259 ldr r4, [r0] @ r4 = data->delta
260 add r10, r0, #4 @ r10 = &data->phase
261 ldrb r3, [r1, #17] @ r3 = num_channels,
262 stmfd sp!, { r1, r10 } @ stack src, &data->phase
263.lrs_channel_loop:
264 ldr r5, [r10] @ r5 = data->phase
265 ldr r6, [r1] @ r6 = srcrem = src->remcount
266 ldr r7, [r1, r3, lsl #2] @ r7 = src->p32[ch]
267 ldr r8, [r2, r3, lsl #2] @ r8 = dst->p32[ch]
268 ldr r9, [r2, #12] @ r9 = dstrem = dst->bufcount
269
270 cmp r6, #0x8000 @ srcrem = MIN(srcrem, 0x8000)
271 movgt r6, #0x8000 @
272 mov r0, r5, lsr #16 @ pos = MIN(pos, srcrem)
273 cmp r0, r6 @
274 movgt r0, r6 @ r0 = pos = phase >> 16
275 cmp r0, #0 @
276 ldrle r11, [r10, r3, lsl #2] @ pos <= 0? r11 = last = last_sample[ch]
277 addgt r12, r7, r0, lsl #2 @ pos > 0? r1 = last = s[pos - 1]
278 ldrgt r11, [r12, #-4] @
279 cmp r0, r6 @
280 bge .lrs_channel_done @ pos >= count? channel complete
281
282 cmp r4, #0x10000 @ delta >= 1.0?
283 ldrhs r12, [r7, r0, lsl #2] @ yes? r12 = s[pos]
284 bhs .lrs_dsstart @ yes? is downsampling
285
286 /** Upsampling **/
287 mov r5, r5, lsl #16 @ Move phase into high halfword
288 add r7, r7, r0, lsl #2 @ r7 = &s[pos]
289 sub r0, r6, r0 @ r0 = dte = srcrem - pos
290.lrs_usloop_1:
291 ldr r12, [r7], #4 @ r12 = s[pos]
292 sub r14, r12, r11 @ r14 = diff = s[pos] - s[pos - 1]
293.lrs_usloop_0:
294 mov r1, r5, lsr #16 @ r1 = frac = phase >> 16
295 @ keep frac in Rs to take advantage of multiplier early termination
296 smull r1, r10, r14, r1 @ r1, r10 = diff * frac (lo, hi)
297 add r1, r11, r1, lsr #16 @ r1 = out = last + frac*diff
298 add r1, r1, r10, lsl #16 @
299 str r1, [r8], #4 @ *d++ = out
300 subs r9, r9, #1 @ destination full?
301 bls .lrs_usfull @ yes? channel is done
302 adds r5, r5, r4, lsl #16 @ phase += delta << 16
303 bcc .lrs_usloop_0 @ if carry is set, pos is incremented
304 subs r0, r0, #1 @ if srcrem > 0, do another sample
305 mov r11, r12 @ r11 = last = s[pos-1] (pos changed)
306 bgt .lrs_usloop_1
307 b .lrs_usdone
308
309.lrs_usfull:
310 adds r5, r5, r4, lsl #16 @ do missed phase increment
311 subcs r0, r0, #1 @ do missed srcrem decrement
312 movcs r11, r12 @ r11 = s[pos-1] (pos changed)
313
314.lrs_usdone:
315 sub r0, r6, r0 @ r0 = pos = srcrem - dte
316 orr r5, r5, r0 @ reconstruct swapped phase
317 mov r5, r5, ror #16 @ swap pos and frac for phase
318 b .lrs_channel_done @
319
320 /** Downsampling **/
321.lrs_dsloop:
322 add r10, r7, r0, lsl #2 @ r10 = &s[pos]
323 ldmda r10, { r11, r12 } @ r11 = last, r12 = s[pos]
324.lrs_dsstart:
325 sub r14, r12, r11 @ r14 = diff = s[pos] - s[pos - 1]
326 @ keep frac in Rs to take advantage of multiplier early termination
327 bic r1, r5, r0, lsl #16 @ frac = phase & 0xffff
328 smull r1, r10, r14, r1 @ r1, r10 = diff * frac (lo, hi)
329 add r5, r5, r4 @ phase += delta
330 subs r9, r9, #1 @ destination full? ...
331 mov r0, r5, lsr #16 @ pos = phase >> 16
332 add r1, r11, r1, lsr #16 @ r1 = out = last + frac*diff
333 add r1, r1, r10, lsl #16 @
334 str r1, [r8], #4 @ *d++ = out
335 cmpgt r6, r0 @ ... || pos >= srcrem? ...
336 bgt .lrs_dsloop @ ... no, do more samples
337
338 cmp r0, r6 @ pos = MIN(pos, srcrem)
339 movgt r0, r6 @
340 sub r1, r0, #1 @ pos must always be > 0 since step >= 1.0
341 ldr r11, [r7, r1, lsl #2] @ r11 = s[pos - 1]
342
343.lrs_channel_done:
344 ldmia sp, { r1, r10 } @ recover src, &data->phase
345 str r11, [r10, r3, lsl #2] @ last_sample[ch] = last
346 subs r3, r3, #1 @
347 bgt .lrs_channel_loop @
348
349 ldr r6, [r2, #12] @ r6 = dst->bufcount
350 sub r5, r5, r0, lsl #16 @ r5 = phase - (pos << 16)
351 str r5, [r10] @ data->phase = r5
352 sub r6, r6, r9 @ r6 = dst->bufcount - dstrem = dstcount
353 str r6, [r2] @ dst->remcount = dstcount
354 add sp, sp, #8 @ adjust stack for temp variables
355 ldmpc regs=r4-r11 @ ... and we're out
356 .size lin_resample_resample, .-lin_resample_resample
357
358/****************************************************************************
359 * void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
360 */
361 .section .icode
362 .global pga_process
363 .type pga_process, %function
364pga_process:
365 @ input: r0 = this, r1 = buf_p
366 ldr r0, [r0] @ r0 = data = this->data (&pga_data)
367 ldr r1, [r1] @ r1 = buf = *buf_p;
368 stmfd sp!, { r4-r8, lr }
369
370 ldr r4, [r0] @ r4 = data->gain
371 ldr r0, [r1], #4 @ r0 = buf->remcount, r1 = buf->p32
372 ldrb r3, [r1, #13] @ r3 = buf->format.num_channels
373
374.pga_channelloop:
375 ldr r2, [r1], #4 @ r2 = buf->p32[ch] and inc index of p32
376 subs r12, r0, #1 @ r12 = count - 1
377 beq .pga_singlesample @ Zero? Only one sample!
378
379.pga_loop:
380 ldmia r2, { r5, r6 } @ load r5, r6 from r2 (*p32[ch])
381 smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8)
382 smull r14, r5, r6, r4 @ r14 = FRACMUL_SHL(r6, r4, 8)
383 subs r12, r12, #2
384 mov r7, r7, lsr #23
385 mov r14, r14, lsr #23
386 orr r7, r7, r8, asl #9
387 orr r14, r14, r5, asl #9
388 stmia r2!, { r7, r14 } @ save r7, r14 to *p32[ch] and increment
389 bgt .pga_loop @ end of pga loop
390
391 blt .pga_evencount @ < 0? even count
392
393.pga_singlesample:
394 ldr r5, [r2] @ handle odd sample
395 smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8)
396 mov r7, r7, lsr #23
397 orr r7, r7, r8, asl #9
398 str r7, [r2]
399
400.pga_evencount:
401 subs r3, r3, #1
402 bgt .pga_channelloop @ end of channel loop
403
404 ldmpc regs=r4-r8
405 .size pga_process, .-pga_process
406
407/****************************************************************************
408 * void filter_process(struct dsp_filter *f, int32_t *buf[], int count,
409 * unsigned int channels)
410 *
411 * define HIGH_PRECISION as '1' to make filtering calculate lower bits after
412 * shifting. without this, "shift" - 1 of the lower bits will be lost here.
413 */
414#define HIGH_PRECISION 0
415
416#if CONFIG_CPU == PP5002
417 .section .icode,"ax",%progbits
418#else
419 .text
420#endif
421 .global filter_process
422filter_process:
423 @input: r0 = f, r1 = buf, r2 = count, r3 = channels
424 stmfd sp!, { r4-r11, lr } @ save all clobbered regs
425 ldmia r0!, { r4-r8 } @ load coefs, r0 = f->history
426 sub r3, r3, #1 @ r3 = ch = channels - 1
427 stmfd sp!, { r0-r3 } @ save adjusted params
428 ldrb r14, [r0, #32] @ r14 = shift
429
430 @ Channels are processed high to low while history is saved low to high
431 @ It's really noone's business how we do this
432.fp_channelloop:
433 ldmia r0, { r9-r12 } @ load history, r0 = history[channels-ch-1]
434 ldr r3, [r1, r3, lsl #2] @ r3 = buf[ch]
435
436 @ r9-r12 = history, r4-r8 = coefs, r0..r1 = accumulator,
437 @ r2 = number of samples, r3 = buf[ch], r14 = shift amount
438.fp_loop:
439 @ Direct form 1 filtering code.
440 @ y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
441 @ where y[] is output and x[] is input. This is performed out of order to
442 @ reuse registers, we're pretty short on regs.
443 smull r0, r1, r5, r9 @ acc = b1*x[i - 1]
444 smlal r0, r1, r6, r10 @ acc += b2*x[i - 2]
445 mov r10, r9 @ fix input history
446 ldr r9, [r3] @ load input and fix history
447 smlal r0, r1, r7, r11 @ acc += a1*y[i - 1]
448 smlal r0, r1, r8, r12 @ acc += a2*y[i - 2]
449 smlal r0, r1, r4, r9 @ acc += b0*x[i] /* avoid stall on arm9 */
450 mov r12, r11 @ fix output history
451 mov r11, r1, asl r14 @ get upper part of result and shift left
452#if HIGH_PRECISION
453 rsb r1, r14, #32 @ get shift amount for lower part
454 orr r11, r11, r0, lsr r1 @ then mix in correctly shifted lower part
455#endif
456 str r11, [r3], #4 @ save result
457 subs r2, r2, #1 @ are we done with this channel?
458 bgt .fp_loop @
459
460 ldr r3, [sp, #12] @ r3 = ch
461 ldr r0, [sp] @ r0 = history[channels-ch-1]
462 subs r3, r3, #1 @ all channels processed?
463 stmia r0!, { r9-r12 } @ save back history, history++
464 ldmhsib sp, { r1-r2 } @ r1 = buf, r2 = count
465 strhs r3, [sp, #12] @ store ch
466 strhs r0, [sp] @ store history[channels-ch-1]
467 bhs .fp_channelloop
468
469 add sp, sp, #16 @ compensate for temp storage
470 ldmpc regs=r4-r11
471 .size filter_process, .-filter_process
184 472
185#if ARM_ARCH < 6 473#if ARM_ARCH < 6
186/**************************************************************************** 474/****************************************************************************
187 * void sample_output_mono(int count, struct dsp_data *data, 475 * void sample_output_mono(struct sample_io_data *this,
188 * const int32_t *src[], int16_t *dst) 476 * struct dsp_buffer *src,
477 * struct dsp_buffer *dst)
189 */ 478 */
190 .section .icode, "ax", %progbits 479 .section .icode
191 .align 2
192 .global sample_output_mono 480 .global sample_output_mono
193 .type sample_output_mono, %function 481 .type sample_output_mono, %function
194sample_output_mono: 482sample_output_mono:
195 @ input: r0 = count, r1 = data, r2 = src, r3 = dst 483 @ input: r0 = this, r1 = src, r2 = dst
196 stmfd sp!, { r4-r6, lr } 484 stmfd sp!, { r4-r6, lr }
197 485
198 ldr r1, [r1] @ lr = data->output_scale 486 ldr r0, [r0] @ r0 = this->outcount
199 ldr r2, [r2] @ r2 = src[0] 487 ldr r3, [r2, #4] @ r2 = dst->p16out
488 ldr r2, [r1, #4] @ r1 = src->p32[0]
489 ldrb r1, [r1, #19] @ r2 = src->format.output_scale
200 490
201 mov r4, #1 491 mov r4, #1
202 mov r4, r4, lsl r1 @ r4 = 1 << (scale-1) 492 mov r4, r4, lsl r1 @ r4 = 1 << (scale-1)
@@ -246,19 +536,21 @@ sample_output_mono:
246 .size sample_output_mono, .-sample_output_mono 536 .size sample_output_mono, .-sample_output_mono
247 537
248/**************************************************************************** 538/****************************************************************************
249 * void sample_output_stereo(int count, struct dsp_data *data, 539 * void sample_output_stereo(struct sample_io_data *this,
250 * const int32_t *src[], int16_t *dst) 540 * struct dsp_buffer *src,
541 * struct dsp_buffer *dst)
251 */ 542 */
252 .section .icode, "ax", %progbits 543 .section .icode
253 .align 2
254 .global sample_output_stereo 544 .global sample_output_stereo
255 .type sample_output_stereo, %function 545 .type sample_output_stereo, %function
256sample_output_stereo: 546sample_output_stereo:
257 @ input: r0 = count, r1 = data, r2 = src, r3 = dst 547 @ input: r0 = this, r1 = src, r2 = dst
258 stmfd sp!, { r4-r9, lr } 548 stmfd sp!, { r4-r9, lr }
259 549
260 ldr r1, [r1] @ r1 = data->output_scale 550 ldr r0, [r0] @ r0 = this->outcount
261 ldmia r2, { r2, r5 } @ r2 = src[0], r5 = src[1] 551 ldr r3, [r2, #4] @ r3 = dsp->p16out
552 ldmib r1, { r2, r5 } @ r2 = src->p32[0], r5 = src->p32[1]
553 ldrb r1, [r1, #19] @ r1 = src->format.output_scale
262 554
263 mov r4, #1 555 mov r4, #1
264 mov r4, r4, lsl r1 @ r4 = 1 << (scale-1) 556 mov r4, r4, lsl r1 @ r4 = 1 << (scale-1)
@@ -330,232 +622,3 @@ sample_output_stereo:
330 ldmpc regs=r4-r9 622 ldmpc regs=r4-r9
331 .size sample_output_stereo, .-sample_output_stereo 623 .size sample_output_stereo, .-sample_output_stereo
332#endif /* ARM_ARCH < 6 */ 624#endif /* ARM_ARCH < 6 */
333
334/****************************************************************************
335 * void apply_crossfeed(int count, int32_t* src[])
336 */
337 .section .text
338 .global apply_crossfeed
339apply_crossfeed:
340 @ unfortunately, we ended up in a bit of a register squeeze here, and need
341 @ to keep the count on the stack :/
342 stmdb sp!, { r4-r11, lr } @ stack modified regs
343 ldmia r1, { r2-r3 } @ r2 = src[0], r3 = src[1]
344
345 ldr r1, =crossfeed_data
346 ldmia r1!, { r4-r11 } @ load direct gain and filter data
347 mov r12, r0 @ better to ldm delay + count later
348 add r0, r1, #13*4*2 @ calculate end of delay
349 stmdb sp!, { r0, r12 } @ stack end of delay adr and count
350 ldr r0, [r1, #13*4*2] @ fetch current delay line address
351
352 /* Register usage in loop:
353 * r0 = &delay[index][0], r1 = accumulator high, r2 = src[0], r3 = src[1],
354 * r4 = direct gain, r5-r7 = b0, b1, a1 (filter coefs),
355 * r8-r11 = filter history, r12 = temp, r14 = accumulator low
356 */
357.cfloop:
358 smull r14, r1, r6, r8 @ acc = b1*dr[n - 1]
359 smlal r14, r1, r7, r9 @ acc += a1*y_l[n - 1]
360 ldr r8, [r0, #4] @ r8 = dr[n]
361 smlal r14, r1, r5, r8 @ acc += b0*dr[n]
362 mov r9, r1, lsl #1 @ fix format for filter history
363 ldr r12, [r2] @ load left input
364 smlal r14, r1, r4, r12 @ acc += gain*x_l[n]
365 mov r1, r1, lsl #1 @ fix format
366 str r1, [r2], #4 @ save result
367
368 smull r14, r1, r6, r10 @ acc = b1*dl[n - 1]
369 smlal r14, r1, r7, r11 @ acc += a1*y_r[n - 1]
370 ldr r10, [r0] @ r10 = dl[n]
371 str r12, [r0], #4 @ save left input to delay line
372 smlal r14, r1, r5, r10 @ acc += b0*dl[n]
373 mov r11, r1, lsl #1 @ fix format for filter history
374 ldr r12, [r3] @ load right input
375 smlal r14, r1, r4, r12 @ acc += gain*x_r[n]
376 str r12, [r0], #4 @ save right input to delay line
377 mov r1, r1, lsl #1 @ fix format
378 ldmia sp, { r12, r14 } @ fetch delay line end addr and count from stack
379 str r1, [r3], #4 @ save result
380
381 cmp r0, r12 @ need to wrap to start of delay?
382 subeq r0, r0, #13*4*2 @ wrap back delay line ptr to start
383
384 subs r14, r14, #1 @ are we finished?
385 strne r14, [sp, #4] @ nope, save count back to stack
386 bne .cfloop
387
388 @ save data back to struct
389 ldr r12, =crossfeed_data + 4*4
390 stmia r12, { r8-r11 } @ save filter history
391 str r0, [r12, #30*4] @ save delay line index
392 add sp, sp, #8 @ remove temp variables from stack
393 ldmpc regs=r4-r11
394 .size apply_crossfeed, .-apply_crossfeed
395
396/****************************************************************************
397 * int dsp_downsample(int count, struct dsp_data *data,
398 * in32_t *src[], int32_t *dst[])
399 */
400 .section .text
401 .global dsp_downsample
402dsp_downsample:
403 stmdb sp!, { r4-r11, lr } @ stack modified regs
404 ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta
405 sub r5, r5, #1 @ pre-decrement num_channels for use
406 add r4, r1, #12 @ r4 = &resample_data.phase
407 mov r12, #0xff
408 orr r12, r12, #0xff00 @ r12 = 0xffff
409.dschannel_loop:
410 ldr r1, [r4] @ r1 = resample_data.phase
411 ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1]
412 ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1]
413 add r9, r4, #4 @ r9 = &last_sample[0]
414 ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1]
415 sub r11, r0, #1
416 ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ...
417 str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample
418 movs r9, r1, lsr #16 @ r9 = pos = phase >> 16
419 ldreq r11, [r7] @ if pos = 0, load src[0] and jump into loop
420 beq .dsuse_last_start
421 cmp r9, r0 @ if pos >= count, we're already done
422 bge .dsloop_skip
423
424 @ Register usage in loop:
425 @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel,
426 @ r6 = delta, r7 = s, r8 = d, r9 = pos, r10 = s[pos - 1], r11 = s[pos]
427.dsloop:
428 add r9, r7, r9, lsl #2 @ r9 = &s[pos]
429 ldmda r9, { r10, r11 } @ r10 = s[pos - 1], r11 = s[pos]
430.dsuse_last_start:
431 sub r11, r11, r10 @ r11 = diff = s[pos] - s[pos - 1]
432 @ keep frac in lower bits to take advantage of multiplier early termination
433 and r9, r1, r12 @ frac = phase & 0xffff
434 smull r9, r14, r11, r9
435 add r1, r1, r6 @ phase += delta
436 add r10, r10, r9, lsr #16 @ r10 = out = s[pos - 1] + frac*diff
437 add r10, r10, r14, lsl #16
438 str r10, [r8], #4 @ *d++ = out
439 mov r9, r1, lsr #16 @ pos = phase >> 16
440 cmp r9, r0 @ pos < count?
441 blt .dsloop @ yup, do more samples
442.dsloop_skip:
443 subs r5, r5, #1
444 bpl .dschannel_loop @ if (--ch) >= 0, do another channel
445 sub r1, r1, r0, lsl #16 @ wrap phase back to start
446 str r1, [r4] @ store back
447 ldr r1, [r3] @ r1 = &dst[0]
448 sub r8, r8, r1 @ dst - &dst[0]
449 mov r0, r8, lsr #2 @ convert bytes->samples
450 ldmpc regs=r4-r11 @ ... and we're out
451 .size dsp_downsample, .-dsp_downsample
452
453/****************************************************************************
454 * int dsp_upsample(int count, struct dsp_data *dsp,
455 * in32_t *src[], int32_t *dst[])
456 */
457 .section .text
458 .global dsp_upsample
459dsp_upsample:
460 stmfd sp!, { r4-r11, lr } @ stack modified regs
461 ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta
462 sub r5, r5, #1 @ pre-decrement num_channels for use
463 add r4, r1, #12 @ r4 = &resample_data.phase
464 mov r6, r6, lsl #16 @ we'll use carry to detect pos increments
465 stmfd sp!, { r0, r4 } @ stack count and &resample_data.phase
466.uschannel_loop:
467 ldr r12, [r4] @ r12 = resample_data.phase
468 ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1]
469 ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1]
470 add r9, r4, #4 @ r9 = &last_sample[0]
471 mov r1, r12, lsl #16 @ we'll use carry to detect pos increments
472 sub r11, r0, #1
473 ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ...
474 ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1]
475 str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample
476 movs r14, r12, lsr #16 @ pos = resample_data.phase >> 16
477 beq .usstart_0 @ pos = 0
478 cmp r14, r0 @ if pos >= count, we're already done
479 bge .usloop_skip
480 add r7, r7, r14, lsl #2 @ r7 = &s[pos]
481 ldr r10, [r7, #-4] @ r11 = s[pos - 1]
482 b .usstart_0
483
484 @ Register usage in loop:
485 @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel,
486 @ r6 = delta, r7 = s, r8 = d, r9 = diff, r10 = s[pos - 1], r11 = s[pos]
487.usloop_1:
488 mov r10, r11 @ r10 = previous sample
489.usstart_0:
490 ldr r11, [r7], #4 @ r11 = next sample
491 mov r4, r1, lsr #16 @ r4 = frac = phase >> 16
492 sub r9, r11, r10 @ r9 = diff = s[pos] - s[pos - 1]
493.usloop_0:
494 smull r12, r14, r4, r9
495 adds r1, r1, r6 @ phase += delta << 16
496 mov r4, r1, lsr #16 @ r4 = frac = phase >> 16
497 add r14, r10, r14, lsl #16
498 add r14, r14, r12, lsr #16 @ r14 = out = s[pos - 1] + frac*diff
499 str r14, [r8], #4 @ *d++ = out
500 bcc .usloop_0 @ if carry is set, pos is incremented
501 subs r0, r0, #1 @ if count > 0, do another sample
502 bgt .usloop_1
503.usloop_skip:
504 subs r5, r5, #1
505 ldmfd sp, { r0, r4 } @ reload count and &resample_data.phase
506 bpl .uschannel_loop @ if (--ch) >= 0, do another channel
507 mov r1, r1, lsr #16 @ wrap phase back to start of next frame
508 ldr r2, [r3] @ r1 = &dst[0]
509 str r1, [r4] @ store phase
510 sub r8, r8, r2 @ dst - &dst[0]
511 mov r0, r8, lsr #2 @ convert bytes->samples
512 add sp, sp, #8 @ adjust stack for temp variables
513 ldmpc regs=r4-r11 @ ... and we're out
514 .size dsp_upsample, .-dsp_upsample
515
516/****************************************************************************
517 * void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[])
518 */
519 .section .icode, "ax", %progbits
520 .align 2
521 .global dsp_apply_gain
522 .type dsp_apply_gain, %function
523dsp_apply_gain:
524 @ input: r0 = count, r1 = data, r2 = buf[]
525 stmfd sp!, { r4-r8, lr }
526
527 ldr r3, [r1, #4] @ r3 = data->num_channels
528 ldr r4, [r1, #32] @ r5 = data->gain
529
530.dag_outerloop:
531 ldr r1, [r2], #4 @ r1 = buf[0] and increment index of buf[]
532 subs r12, r0, #1 @ r12 = r0 = count - 1
533 beq .dag_singlesample @ Zero? Only one sample!
534
535.dag_innerloop:
536 ldmia r1, { r5, r6 } @ load r5, r6 from r1
537 smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8)
538 smull r14, r5, r6, r4 @ r14 = FRACMUL_SHL(r6, r4, 8)
539 subs r12, r12, #2
540 mov r7, r7, lsr #23
541 mov r14, r14, lsr #23
542 orr r7, r7, r8, asl #9
543 orr r14, r14, r5, asl #9
544 stmia r1!, { r7, r14 } @ save r7, r14 to [r1] and increment r1
545 bgt .dag_innerloop @ end of inner loop
546
547 blt .dag_evencount @ < 0? even count
548
549.dag_singlesample:
550 ldr r5, [r1] @ handle odd sample
551 smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8)
552 mov r7, r7, lsr #23
553 orr r7, r7, r8, asl #9
554 str r7, [r1]
555
556.dag_evencount:
557 subs r3, r3, #1
558 bgt .dag_outerloop @ end of outer loop
559
560 ldmpc regs=r4-r8
561 .size dsp_apply_gain, .-dsp_apply_gain
diff --git a/lib/rbcodec/dsp/dsp_arm_v6.S b/lib/rbcodec/dsp/dsp_arm_v6.S
index a9a88ce5bf..a36760f744 100644
--- a/lib/rbcodec/dsp/dsp_arm_v6.S
+++ b/lib/rbcodec/dsp/dsp_arm_v6.S
@@ -20,19 +20,21 @@
20 ****************************************************************************/ 20 ****************************************************************************/
21 21
22/**************************************************************************** 22/****************************************************************************
23 * void sample_output_mono(int count, struct dsp_data *data, 23 * void sample_output_mono(struct sample_io_data *this,
24 * const int32_t *src[], int16_t *dst) 24 * struct dsp_buffer *src,
25 * struct dsp_buffer *dst)
25 */ 26 */
26 .section .text, "ax", %progbits 27 .section .text
27 .align 2
28 .global sample_output_mono 28 .global sample_output_mono
29 .type sample_output_mono, %function 29 .type sample_output_mono, %function
30sample_output_mono: 30sample_output_mono:
31 @ input: r0 = count, r1 = data, r2 = src, r3 = dst 31 @ input: r0 = this, r1 = src, r2 = dst
32 stmfd sp!, { r4, lr } @ 32 stmfd sp!, { r4, lr } @
33 @ 33 @
34 ldr r1, [r1] @ r1 = data->output_scale 34 ldr r0, [r0] @ r0 = this->outcount
35 ldr r2, [r2] @ r2 = src[0] 35 ldr r3, [r2, #4] @ r3 = dst->p16out
36 ldr r2, [r1, #4] @ r2 = src->p32[0]
37 ldrb r1, [r1, #19] @ r1 = src->format.output_scale
36 @ 38 @
37 mov r4, #1 @ r4 = 1 << (scale - 1) 39 mov r4, #1 @ r4 = 1 << (scale - 1)
38 mov r4, r4, lsl r1 @ 40 mov r4, r4, lsl r1 @
@@ -68,19 +70,21 @@ sample_output_mono:
68 .size sample_output_mono, .-sample_output_mono 70 .size sample_output_mono, .-sample_output_mono
69 71
70/**************************************************************************** 72/****************************************************************************
71 * void sample_output_stereo(int count, struct dsp_data *data, 73 * void sample_output_stereo(struct sample_io_data *this,
72 * const int32_t *src[], int16_t *dst) 74 * struct dsp_buffer *src,
75 * struct dsp_buffer *dst)
73 */ 76 */
74 .section .text, "ax", %progbits 77 .section .text
75 .align 2
76 .global sample_output_stereo 78 .global sample_output_stereo
77 .type sample_output_stereo, %function 79 .type sample_output_stereo, %function
78sample_output_stereo: 80sample_output_stereo:
79 @ input: r0 = count, r1 = data, r2 = src, r3 = dst 81 @ input: r0 = this, r1 = src, r2 = dst
80 stmfd sp!, { r4-r7, lr } @ 82 stmfd sp!, { r4-r7, lr } @
81 @ 83 @
82 ldr r1, [r1] @ r1 = data->output_scale 84 ldr r0, [r0] @ r0 = this->outcount
83 ldmia r2, { r2, r4 } @ r2 = src[0], r4 = src[1] 85 ldr r3, [r2, #4] @ r3 = dst->p16out
86 ldmib r1, { r2, r4 } @ r2 = src->p32[0], r4 = src->p32[1]
87 ldrb r1, [r1, #19] @ r1 = src->format.output_scale
84 @ 88 @
85 mov r5, #1 @ r5 = 1 << (scale - 1) 89 mov r5, #1 @ r5 = 1 << (scale - 1)
86 mov r5, r5, lsl r1 @ 90 mov r5, r5, lsl r1 @
diff --git a/lib/rbcodec/dsp/dsp_asm.h b/lib/rbcodec/dsp/dsp_asm.h
deleted file mode 100644
index 7bf18370a3..0000000000
--- a/lib/rbcodec/dsp/dsp_asm.h
+++ /dev/null
@@ -1,86 +0,0 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21
22#include <config.h>
23
24#ifndef _DSP_ASM_H
25#define _DSP_ASM_H
26
27/* Set the appropriate #defines based on CPU or whatever matters */
28#if defined(CPU_ARM)
29#define DSP_HAVE_ASM_APPLY_GAIN
30#define DSP_HAVE_ASM_RESAMPLING
31#define DSP_HAVE_ASM_CROSSFEED
32#define DSP_HAVE_ASM_SOUND_CHAN_MONO
33#define DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
34#define DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
35#define DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
36#define DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
37#elif defined (CPU_COLDFIRE)
38#define DSP_HAVE_ASM_APPLY_GAIN
39#define DSP_HAVE_ASM_RESAMPLING
40#define DSP_HAVE_ASM_CROSSFEED
41#define DSP_HAVE_ASM_SOUND_CHAN_MONO
42#define DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
43#define DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
44#define DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
45#define DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
46#endif /* CPU_COLDFIRE */
47
48/* Declare prototypes based upon what's #defined above */
49#ifdef DSP_HAVE_ASM_CROSSFEED
50void apply_crossfeed(int count, int32_t *buf[]);
51#endif
52
53#ifdef DSP_HAVE_ASM_APPLY_GAIN
54void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]);
55#endif /* DSP_HAVE_ASM_APPLY_GAIN* */
56
57#ifdef DSP_HAVE_ASM_RESAMPLING
58int dsp_upsample(int count, struct dsp_data *data,
59 const int32_t *src[], int32_t *dst[]);
60int dsp_downsample(int count, struct dsp_data *data,
61 const int32_t *src[], int32_t *dst[]);
62#endif /* DSP_HAVE_ASM_RESAMPLING */
63
64#ifdef DSP_HAVE_ASM_SOUND_CHAN_MONO
65void channels_process_sound_chan_mono(int count, int32_t *buf[]);
66#endif
67
68#ifdef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
69void channels_process_sound_chan_custom(int count, int32_t *buf[]);
70#endif
71
72#ifdef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
73void channels_process_sound_chan_karaoke(int count, int32_t *buf[]);
74#endif
75
76#ifdef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
77void sample_output_stereo(int count, struct dsp_data *data,
78 const int32_t *src[], int16_t *dst);
79#endif
80
81#ifdef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
82void sample_output_mono(int count, struct dsp_data *data,
83 const int32_t *src[], int16_t *dst);
84#endif
85
86#endif /* _DSP_ASM_H */
diff --git a/lib/rbcodec/dsp/dsp_cf.S b/lib/rbcodec/dsp/dsp_cf.S
index 15ec7eb383..c710df5177 100644
--- a/lib/rbcodec/dsp/dsp_cf.S
+++ b/lib/rbcodec/dsp/dsp_cf.S
@@ -19,23 +19,27 @@
19 * KIND, either express or implied. 19 * KIND, either express or implied.
20 * 20 *
21 ****************************************************************************/ 21 ****************************************************************************/
22#include "config.h"
22 23
23/**************************************************************************** 24/****************************************************************************
24 * void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]) 25 * void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
25 */ 26 */
26 .section .text 27 .section .text
27 .align 2 28 .align 2
28 .global dsp_apply_gain 29 .global pga_process
29dsp_apply_gain: 30pga_process:
31 | input: 4(sp) = this, 8(sp) = buf_p
32 movem.l 4(%sp), %a0-%a1 | %a0 = this, %a1 = buf_p
33 move.l (%a0), %a0 | %a0 = this->data = &pga_data
34 move.l (%a0), %a0 | %a0 = data->gain
35 move.l (%a1), %a1 | %a1 = buf = *buf_p
30 lea.l -20(%sp), %sp | save registers 36 lea.l -20(%sp), %sp | save registers
31 movem.l %d2-%d4/%a2-%a3, (%sp) | 37 movem.l %d2-%d4/%a2-%a3, (%sp) |
32 movem.l 28(%sp), %a0-%a1 | %a0 = data, 38 clr.l %d1 | %d1 = buf->format.num_channels
33 | %a1 = buf 39 move.b 17(%a1), %d1 |
34 move.l 4(%a0), %d1 | %d1 = data->num_channels
35 move.l 32(%a0), %a0 | %a0 = data->gain (in s8.23)
3610: | channel loop | 4010: | channel loop |
37 move.l 24(%sp), %d0 | %d0 = count 41 move.l (%a1), %d0 | %d0 = buf->remcount
38 move.l -4(%a1, %d1.l*4), %a2 | %a2 = s = buf[ch-1] 42 move.l (%a1, %d1.l*4), %a2 | %a2 = s = buf->p32[ch-1]
39 move.l %a2, %a3 | %a3 = d = s 43 move.l %a2, %a3 | %a3 = d = s
40 move.l (%a2)+, %d2 | %d2 = *s++, 44 move.l (%a2)+, %d2 | %d2 = *s++,
41 mac.l %a0, %d2, (%a2)+, %d2, %acc0 | %acc0 = S(n)*gain, load S(n+1) 45 mac.l %a0, %d2, (%a2)+, %d2, %acc0 | %acc0 = S(n)*gain, load S(n+1)
@@ -61,25 +65,29 @@ dsp_apply_gain:
61 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers 65 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers
62 lea.l 20(%sp), %sp | cleanup stack 66 lea.l 20(%sp), %sp | cleanup stack
63 rts | 67 rts |
64 .size dsp_apply_gain,.-dsp_apply_gain 68 .size pga_process, .-pga_process
65 69
66/**************************************************************************** 70/****************************************************************************
67 * void apply_crossfeed(int count, int32_t *buf[]) 71 * void crossfeed_process(struct dsp_proc_entry *this,
72 * struct dsp_buffer **buf_p)
68 */ 73 */
69 .section .text 74 .section .text
70 .align 2 75 .align 2
71 .global apply_crossfeed 76 .global crossfeed_process
72apply_crossfeed: 77crossfeed_process:
78 | input: 4(sp) = this, 8(sp) = buf_p
73 lea.l -44(%sp), %sp | 79 lea.l -44(%sp), %sp |
74 movem.l %d2-%d7/%a2-%a6, (%sp) | save all regs 80 movem.l %d2-%d7/%a2-%a6, (%sp) | save all regs
75 movem.l 48(%sp), %d7/%a4 | %d7 = count, %a4 = src 81 movem.l 48(%sp), %a1/%a4 | %a1 = this, %a4 = buf_p
76 movem.l (%a4), %a4-%a5 | %a4 = src[0], %a5 = src[1] 82 move.l (%a4), %a4 | %a4 = buf = *buf_p
77 lea.l crossfeed_data, %a1 | %a1 = &crossfeed_data 83 movem.l (%a4), %d7/%a4-%a5 | %d7 = buf->remcount, %a4 = buf->p32[0],
84 | %a5 = buf->p32[1]
85 move.l (%a1), %a1 | %a1 = &crossfeed_state
78 move.l (%a1)+, %d6 | %d6 = direct gain 86 move.l (%a1)+, %d6 | %d6 = direct gain
79 movem.l 12(%a1), %d0-%d3 | fetch filter history samples 87 movem.l 12(%a1), %d0-%d3 | fetch filter history samples
80 move.l 132(%a1), %a0 | fetch delay line address 88 lea.l 132(%a1), %a6 | %a6 = delay line wrap limit
89 move.l (%a6), %a0 | fetch delay line address
81 movem.l (%a1), %a1-%a3 | load filter coefs 90 movem.l (%a1), %a1-%a3 | load filter coefs
82 lea.l crossfeed_data+136, %a6 | %a6 = delay line wrap limit
83 bra.b 20f | loop start | go to loop start point 91 bra.b 20f | loop start | go to loop start point
84 /* Register usage in loop: 92 /* Register usage in loop:
85 * %a0 = delay_p, %a1..%a3 = b0, b1, a1 (filter coefs), 93 * %a0 = delay_p, %a1..%a3 = b0, b1, a1 (filter coefs),
@@ -109,174 +117,181 @@ apply_crossfeed:
109 mac.l %d6, %d5, %acc1 | %acc1 += gain*x_r[n] 117 mac.l %d6, %d5, %acc1 | %acc1 += gain*x_r[n]
110 cmp.l %a6, %a0 | wrap %a0 if passed end 118 cmp.l %a6, %a0 | wrap %a0 if passed end
111 bhs.b 30f | wrap buffer | 119 bhs.b 30f | wrap buffer |
112 .word 0x51fb | tpf.l | trap the buffer wrap 120 tpf.l | trap the buffer wrap
11330: | wrap buffer | ...fwd taken branches more costly 12130: | wrap buffer | ...fwd taken branches more costly
114 lea.l -104(%a0), %a0 | wrap it up 122 lea.l -104(%a6), %a0 | wrap it up
115 subq.l #1, %d7 | --count > 0 ? 123 subq.l #1, %d7 | --count > 0 ?
116 bgt.b 10b | loop | yes? do more 124 bgt.b 10b | loop | yes? do more
117 movclr.l %acc0, %d4 | write last outputs 125 movclr.l %acc0, %d4 | write last outputs
118 move.l %d4, (%a4) | . 126 move.l %d4, (%a4) | .
119 movclr.l %acc1, %d5 | . 127 movclr.l %acc1, %d5 | .
120 move.l %d5, (%a5) | . 128 move.l %d5, (%a5) | .
121 lea.l crossfeed_data+16, %a1 | save data back to struct 129 movem.l %d0-%d3, -120(%a6) | ...history
122 movem.l %d0-%d3, (%a1) | ...history 130 move.l %a0, (%a6) | ...delay_p
123 move.l %a0, 120(%a1) | ...delay_p
124 movem.l (%sp), %d2-%d7/%a2-%a6 | restore all regs 131 movem.l (%sp), %d2-%d7/%a2-%a6 | restore all regs
125 lea.l 44(%sp), %sp | 132 lea.l 44(%sp), %sp |
126 rts | 133 rts |
127 .size apply_crossfeed,.-apply_crossfeed 134 .size crossfeed_process,.-crossfeed_process
128 135
129/**************************************************************************** 136/****************************************************************************
130 * int dsp_downsample(int count, struct dsp_data *data, 137 * int lin_resample_resample(struct resample_data *data,
131 * in32_t *src[], int32_t *dst[]) 138 * struct dsp_buffer *src,
139 * struct dsp_buffer *dst)
132 */ 140 */
133 .section .text 141 .section .text
134 .align 2 142 .align 2
135 .global dsp_downsample 143 .global lin_resample_resample
136dsp_downsample: 144lin_resample_resample:
137 lea.l -40(%sp), %sp | save non-clobberables 145 | input: 4(sp) = data, 8(sp) = src, 12(sp) = dst
138 movem.l %d2-%d7/%a2-%a5, (%sp) | 146 lea.l -44(%sp), %sp | save non-volatiles
139 movem.l 44(%sp), %d2/%a0-%a2 | %d2 = count 147 movem.l %d2-%d7/%a2-%a6, (%sp) |
140 | %a0 = data 148 movem.l 48(%sp), %a0-%a2 | %a0 = data
141 | %a1 = src 149 | %a1 = src
142 | %a2 = dst 150 | %a2 = dst
143 movem.l 4(%a0), %d3-%d4 | %d3 = ch = data->num_channels 151 clr.l %d1 | %d1 = ch = src->format.num_channels
144 | %d4 = delta = data->resample_data.delta 152 move.b 17(%a1), %d1 |
145 moveq.l #16, %d7 | %d7 = shift 153 moveq.l #16, %d7 | %d7 = shift
14610: | channel loop | 154.lrs_channel_loop: |
147 move.l 12(%a0), %d5 | %d5 = phase = data->resample_data.phase 155 movem.l (%a0), %d2-%d3 | %d2 = delta = data->delta,
148 move.l -4(%a1, %d3.l*4), %a3 | %a3 = s = src[ch-1] 156 | %d3 = phase = data->phase
149 move.l -4(%a2, %d3.l*4), %a4 | %a4 = d = dst[ch-1] 157 move.l (%a1), %d4 | %d4 = srcrem = src->remcount
150 lea.l 12(%a0, %d3.l*4), %a5 | %a5 = &data->resample_data.ast_sample[ch-1] 158 move.l 12(%a2), %d5 | %d5 = dstrem = dst->bufcount
151 move.l (%a5), %d0 | %d0 = last = data->resample_data.last_sample[ch-1] 159 cmp.l #0x8000, %d4 | %d4 = MIN(srcrem, 0x8000)
152 move.l -4(%a3, %d2.l*4), (%a5) | data->resample_data.last_sample[ch-1] = s[count-1] 160 ble.b 10f |
153 move.l %d5, %d6 | %d6 = pos = phase >> 16 161 move.l #0x8000, %d4 |
154 lsr.l %d7, %d6 | 16210: |
155 cmp.l %d2, %d6 | past end of samples? 163 move.l (%a1, %d1.l*4), %a3 | %a3 = s = src->p32[ch]
156 bge.b 40f | skip resample loop| yes? skip loop 164 move.l (%a2, %d1.l*4), %a4 | %a4 = d = dst->p32[ch]
157 tst.l %d6 | need last sample of prev. frame? 165 move.l %d3, %d0 | %d0 = pos
158 bne.b 20f | resample loop | no? start main loop 166 lsr.l %d7, %d0 | ...
159 move.l (%a3, %d6.l*4), %d1 | %d1 = s[pos] 167 beq.b 11f | pos == 0?
160 bra.b 30f | resample start last | start with last (last in %d0) 168 cmp.l %d4, %d0 | pos = MIN(pos, srcrem)
16120: | resample loop | 169 blt.b 12f |
162 lea.l -4(%a3, %d6.l*4), %a5 | load s[pos-1] and s[pos] 170 move.l %d4, %d0 | pos = srcrem
163 movem.l (%a5), %d0-%d1 | 171 move.l -4(%a3, %d0.l*4), %d6 | %d6 = last = s[pos - 1]
16430: | resample start last | 172 bra.w .lrs_channel_complete | at limit; nothing to do but next
165 sub.l %d0, %d1 | %d1 = diff = s[pos] - s[pos-1] 17311: |
166 move.l %d0, %acc0 | %acc0 = previous sample 174 move.l 4(%a0, %d1.l*4), %d6 | %d6 = last = last_sample[ch]
167 move.l %d5, %d0 | frac = (phase << 16) >> 1 175 tpf.l | trap next move.l (last = s[pos - 1])
17612: |
177 move.l -4(%a3, %d0.l*4), %d6 | %d6 = last = s[pos - 1]
178 cmp.l #0x10000, %d2 | delta >= 1.0?
179 bhs.b .lrs_downsample | yes? downsampling
180 |
181 /** Upsampling **/ |
182 lea.l (%a3, %d0.l*4), %a3 | %a3 = &s[pos]
183 sub.l %d4, %d0 | %d0 = pos - srcrem = -dte
184 lsl.l %d7, %d2 | move delta to bits 30..15
185 lsr.l #1, %d2 |
186 lsl.l %d7, %d3 | move phase to bits 30..15
187 lsr.l #1, %d3 |
188 move.l (%a3)+, %a5 | %a5 = s[pos]
189 move.l %a5, %a6 | %a6 = diff = s[pos] - last
190 sub.l %d6, %a6 |
191 bra.b 22f |
192 /* Funky loop structure is to avoid emac latency stalls */
19320: |
194 move.l (%a3)+, %a5 | %a5 = s[pos]
195 move.l %a5, %a6 | %a6 = diff = s[pos] - last
196 sub.l %d6, %a6 |
19721: |
198 movclr.l %acc0, %d7 | *d++ = %d7 = result
199 move.l %d7, (%a4)+ |
20022: |
201 move.l %d6, %acc0 | %acc0 = last
202 mac.l %d3, %a6, %acc0 | %acc0 += frac * diff
203 subq.l #1, %d5 | dstrem <= 0?
204 ble.b 23f | yes? stop
205 add.l %d2, %d3 | phase += delta
206 bpl.b 21b | load next values?
207 move.l %a5, %d6 |
208 bclr.l #31, %d3 | clear sign bit
209 addq.l #1, %d0 | dte > 0?
210 bmi.b 20b | yes? continue resampling
211 tpf.w | trap next add.l (phase += delta)
21223: |
213 add.l %d2, %d3 | phase += delta
214 lsl.l #1, %d3 | frac -> phase
215 bcs.b 24f | was sign bit set?
216 tpf.l |
21724: |
218 move.l %a5, %d6 | yes? was going to move to new s[pos]
219 addq.l #1, %d0 |
220 movclr.l %acc0, %d7 | *d = %d7 = result
221 move.l %d7, (%a4) |
222 add.l %d4, %d0 | %d0 = -dte + srcrem = pos
223 or.l %d0, %d3 | restore phase
224 swap.w %d3 |
225 moveq.l #16, %d7 | %d7 = shift
226 bra.b .lrs_channel_complete |
227 |
228 /** Downsampling **/ |
229.lrs_downsample: |
230 move.l (%a3, %d0.l*4), %a5 | %a5 = s[pos]
231 bra.b 31f |
23230: |
233 lea.l -4(%a3, %d0.l*4), %a5 | %d6 = s[pos - 1], %a5 = s[pos]
234 movem.l (%a5), %d6/%a5 |
23531: |
236 move.l %d6, %acc0 | %acc0 = last
237 sub.l %d6, %a5 | %a5 = diff = s[pos] - s[pos - 1]
238 move.l %d3, %d0 | frac = (phase << 16) >> 1
168 lsl.l %d7, %d0 | 239 lsl.l %d7, %d0 |
169 lsr.l #1, %d0 | 240 lsr.l #1, %d0 |
170 mac.l %d0, %d1, %acc0 | %acc0 += frac * diff 241 mac.l %d0, %a5, %acc0 | %acc0 += frac * diff
171 add.l %d4, %d5 | phase += delta 242 add.l %d2, %d3 | phase += delta
172 move.l %d5, %d6 | pos = phase >> 16 243 move.l %d3, %d0 | pos = phase >> 16
173 lsr.l %d7, %d6 | 244 lsr.l %d7, %d0 |
174 movclr.l %acc0, %d0 | 245 movclr.l %acc0, %a5 |
175 move.l %d0, (%a4)+ | *d++ = %d0 246 move.l %a5, (%a4)+ | *d++ = %d0
176 cmp.l %d2, %d6 | pos < count? 247 subq.l #1, %d5 | dst full?
177 blt.b 20b | resample loop | yes? continue resampling 248 ble.b 32f | yes? stop
17840: | skip resample loop | 249 cmp.l %d4, %d0 | pos < srcrem?
179 subq.l #1, %d3 | ch > 0? 250 blt.b 30b | yes? continue resampling
180 bgt.b 10b | channel loop | yes? process next channel 251 tpf.l | trap cmp.l and ble.b
181 lsl.l %d7, %d2 | wrap phase to start of next frame 25232: |
182 sub.l %d2, %d5 | data->resample_data.phase = 253 cmp.l %d4, %d0 | pos = MIN(pos, srcrem)
183 move.l %d5, 12(%a0) | ... phase - (count << 16) 254 ble.b 33f |
184 move.l %a4, %d0 | return d - d[0] 255 move.l %d4, %d0 |
185 sub.l (%a2), %d0 | 25633: |
186 asr.l #2, %d0 | convert bytes->samples 257 move.l -4(%a3, %d0.l*4), %d6 | %d6 = s[pos - 1]
187 movem.l (%sp), %d2-%d7/%a2-%a5 | restore non-clobberables 258 |
188 lea.l 40(%sp), %sp | cleanup stack 259.lrs_channel_complete: |
260 move.l %d6, 4(%a0, %d1.l*4) | last_sample[ch] = last
261 subq.l #1, %d1 | ch > 0?
262 bgt.w .lrs_channel_loop | yes? process next channel
263 |
264 move.l 12(%a2), %d1 | %d1 = dst->bufcount
265 sub.l %d5, %d1 | written = dst->bufcount - dstrem
266 move.l %d1, (%a2) | dst->remcount = written
267 move.l %d0, %d1 | wrap phase to position in next frame
268 lsl.l %d7, %d1 | data->phase = phase - (pos << 16)
269 sub.l %d1, %d3 | ...
270 move.l %d3, 4(%a0) | ...
271 movem.l (%sp), %d2-%d7/%a2-%a6 | restore non-volatiles
272 lea.l 44(%sp), %sp | cleanup stack
189 rts | buh-bye 273 rts | buh-bye
190 .size dsp_downsample,.-dsp_downsample
191 274
192/**************************************************************************** 275 .size lin_resample_resample, .-lin_resample_resample
193 * int dsp_upsample(int count, struct dsp_data *dsp, 276
194 * const int32_t *src[], int32_t *dst[])
195 */
196 .section .text
197 .align 2
198 .global dsp_upsample
199dsp_upsample:
200 lea.l -40(%sp), %sp | save non-clobberables
201 movem.l %d2-%d7/%a2-%a5, (%sp) |
202 movem.l 44(%sp), %d2/%a0-%a2 | %d2 = count
203 | %a0 = data
204 | %a1 = src
205 | %a2 = dst
206 movem.l 4(%a0), %d3-%d4 | %d3 = ch = channels
207 | %d4 = delta = data->resample_data.delta
208 swap %d4 | swap delta to high word to use...
209 | ...carries to increment position
21010: | channel loop |
211 move.l 12(%a0), %d5 | %d5 = phase = data->resample_data.phase
212 move.l -4(%a1, %d3.l*4), %a3 | %a3 = s = src[ch-1]
213 lea.l 12(%a0, %d3.l*4), %a4 | %a4 = &data->resample_data.last_sample[ch-1]
214 lea.l -4(%a3, %d2.l*4), %a5 | %a5 = src_end = &src[count-1]
215 move.l (%a4), %d0 | %d0 = last = data->resample_data.last_sample[ch-1]
216 move.l (%a5), (%a4) | data->resample_data.last_sample[ch-1] = s[count-1]
217 move.l -4(%a2, %d3.l*4), %a4 | %a4 = d = dst[ch-1]
218 move.l (%a3)+, %d1 | fetch first sample - might throw this...
219 | ...away later but we'll be preincremented
220 move.l %d1, %d6 | save sample value
221 sub.l %d0, %d1 | %d1 = diff = s[0] - last
222 swap %d5 | swap phase to high word to use
223 | carries to increment position
224 move.l %d5, %d7 | %d7 = pos = phase >> 16
225 clr.w %d5 |
226 eor.l %d5, %d7 | pos == 0?
227 beq.b 40f | loop start | yes? start loop
228 cmp.l %d2, %d7 | past end of samples?
229 bge.b 50f | skip resample loop| yes? go to next channel and collect info
230 lea.l (%a3, %d7.l*4), %a3 | %a3 = s = &s[pos+1]
231 movem.l -8(%a3), %d0-%d1 | %d0 = s[pos-1], %d1 = s[pos]
232 move.l %d1, %d6 | save sample value
233 sub.l %d0, %d1 | %d1 = diff = s[pos] - s[pos-1]
234 bra.b 40f | loop start |
23520: | next sample loop |
236 move.l %d6, %d0 | move previous sample to %d0
237 move.l (%a3)+, %d1 | fetch next sample
238 move.l %d1, %d6 | save sample value
239 sub.l %d0, %d1 | %d1 = diff = s[pos] - s[pos-1]
24030: | same sample loop |
241 movclr.l %acc0, %d7 | %d7 = result
242 move.l %d7, (%a4)+ | *d++ = %d7
24340: | loop start |
244 lsr.l #1, %d5 | make phase into frac
245 move.l %d0, %acc0 | %acc0 = s[pos-1]
246 mac.l %d1, %d5, %acc0 | %acc0 = diff * frac
247 lsl.l #1, %d5 | restore frac to phase
248 add.l %d4, %d5 | phase += delta
249 bcc.b 30b | same sample loop | load next values?
250 cmp.l %a5, %a3 | src <= src_end?
251 bls.b 20b | next sample loop | yes? continue resampling
252 movclr.l %acc0, %d7 | %d7 = result
253 move.l %d7, (%a4)+ | *d++ = %d7
25450: | skip resample loop |
255 subq.l #1, %d3 | ch > 0?
256 bgt.b 10b | channel loop | yes? process next channel
257 swap %d5 | wrap phase to start of next frame
258 move.l %d5, 12(%a0) | ...and save in data->resample_data.phase
259 move.l %a4, %d0 | return d - d[0]
260 sub.l (%a2), %d0 |
261 movem.l (%sp), %d2-%d7/%a2-%a5 | restore non-clobberables
262 asr.l #2, %d0 | convert bytes->samples
263 lea.l 40(%sp), %sp | cleanup stack
264 rts | buh-bye
265 .size dsp_upsample,.-dsp_upsample
266 277
267/**************************************************************************** 278/****************************************************************************
268 * void channels_process_sound_chan_mono(int count, int32_t *buf[]) 279 * void channel_mode_proc_mono(struct dsp_proc_entry *this,
280 * struct dsp_buffer **buf_p)
269 * 281 *
270 * Mix left and right channels 50/50 into a center channel. 282 * Mix left and right channels 50/50 into a center channel.
271 */ 283 */
272 .section .text 284 .section .text
273 .align 2 285 .align 2
274 .global channels_process_sound_chan_mono 286 .global channel_mode_proc_mono
275channels_process_sound_chan_mono: 287channel_mode_proc_mono:
276 movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf 288 | input: 4(sp) = this, 8(sp) = buf_p
289 move.l 8(%sp), %a0 | %a0 = buf_p
290 move.l (%a0), %a0 | %a0 = buf = *buf_p
277 lea.l -20(%sp), %sp | save registers 291 lea.l -20(%sp), %sp | save registers
278 movem.l %d2-%d4/%a2-%a3, (%sp) | 292 movem.l %d2-%d4/%a2-%a3, (%sp) |
279 movem.l (%a0), %a0-%a1 | get channel pointers 293 movem.l (%a0), %d0/%a0-%a1 | %d0 = buf->remcount, %a0 = buf->p32[0],
294 | %a1 = buf->p32[1]
280 move.l %a0, %a2 | use separate dst pointers since read 295 move.l %a0, %a2 | use separate dst pointers since read
281 move.l %a1, %a3 | pointers run one ahead of write 296 move.l %a1, %a3 | pointers run one ahead of write
282 move.l #0x40000000, %d3 | %d3 = 0.5 297 move.l #0x40000000, %d3 | %d3 = 0.5
@@ -301,26 +316,29 @@ channels_process_sound_chan_mono:
301 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers 316 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers
302 lea.l 20(%sp), %sp | cleanup 317 lea.l 20(%sp), %sp | cleanup
303 rts | 318 rts |
304 .size channels_process_sound_chan_mono, \ 319 .size channel_mode_proc_mono, .-channel_mode_proc_mono
305 .-channels_process_sound_chan_mono
306 320
307/**************************************************************************** 321/****************************************************************************
308 * void channels_process_sound_chan_custom(int count, int32_t *buf[]) 322 * void channel_mode_proc_custom(struct dsp_proc_entry *this,
323 * struct dsp_buffer **buf_p)
309 * 324 *
310 * Apply stereo width (narrowing/expanding) effect. 325 * Apply stereo width (narrowing/expanding) effect.
311 */ 326 */
312 .section .text 327 .section .text
313 .align 2 328 .align 2
314 .global channels_process_sound_chan_custom 329 .global channel_mode_proc_custom
315channels_process_sound_chan_custom: 330channel_mode_proc_custom:
316 movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf 331 | input: 4(sp) = this, 8(sp) = buf_p
317 lea.l -28(%sp), %sp | save registers 332 lea.l -28(%sp), %sp | save registers
318 movem.l %d2-%d6/%a2-%a3, (%sp) | 333 movem.l %d2-%d6/%a2-%a3, (%sp) |
319 movem.l (%a0), %a0-%a1 | get channel pointers 334 movem.l 32(%sp), %a0-%a1 | %a0 = this, %a1 = buf_p
335 move.l (%a1), %a1 | %a1 = buf = *buf_p
336 move.l (%a0), %a2 | %a2 = this->data = &channel_mode_data
337 movem.l (%a1), %d0/%a0-%a1 | %d0 = buf->remcount, %a0 = buf->p32[0],
338 | %a1 = buf->p32[1]
339 movem.l (%a2), %d3-%d4 | %d3 = sw_gain, %d4 = sw_cross
320 move.l %a0, %a2 | use separate dst pointers since read 340 move.l %a0, %a2 | use separate dst pointers since read
321 move.l %a1, %a3 | pointers run one ahead of write 341 move.l %a1, %a3 | pointers run one ahead of write
322 move.l dsp_sw_gain, %d3 | load straight (mid) gain
323 move.l dsp_sw_cross, %d4 | load cross (side) gain
324 move.l (%a0)+, %d1 | prime the input registers 342 move.l (%a0)+, %d1 | prime the input registers
325 move.l (%a1)+, %d2 | 343 move.l (%a1)+, %d2 |
326 mac.l %d1, %d3 , %acc0 | L = l*gain + r*cross 344 mac.l %d1, %d3 , %acc0 | L = l*gain + r*cross
@@ -348,22 +366,25 @@ channels_process_sound_chan_custom:
348 movem.l (%sp), %d2-%d6/%a2-%a3 | restore registers 366 movem.l (%sp), %d2-%d6/%a2-%a3 | restore registers
349 lea.l 28(%sp), %sp | cleanup 367 lea.l 28(%sp), %sp | cleanup
350 rts | 368 rts |
351 .size channels_process_sound_chan_custom, \ 369 .size channel_mode_proc_custom, .-channel_mode_proc_custom
352 .-channels_process_sound_chan_custom
353 370
354/**************************************************************************** 371/****************************************************************************
355 * void channels_process_sound_chan_karaoke(int count, int32_t *buf[]) 372 * void channel_mode_proc_karaoke(struct dsp_proc_entry *this,
373 * struct dsp_buffer **buf_p)
356 * 374 *
357 * Separate channels into side channels. 375 * Separate channels into side channels.
358 */ 376 */
359 .section .text 377 .section .text
360 .align 2 378 .align 2
361 .global channels_process_sound_chan_karaoke 379 .global channel_mode_proc_karaoke
362channels_process_sound_chan_karaoke: 380channel_mode_proc_karaoke:
363 movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf 381 | input: 4(sp) = this, 8(sp) = buf_p
382 move.l 8(%sp), %a0 | %a0 = buf_p
383 move.l (%a0), %a0 | %a0 = buf = *buf_p
364 lea.l -20(%sp), %sp | save registers 384 lea.l -20(%sp), %sp | save registers
365 movem.l %d2-%d4/%a2-%a3, (%sp) | 385 movem.l %d2-%d4/%a2-%a3, (%sp) |
366 movem.l (%a0), %a0-%a1 | get channel src pointers 386 movem.l (%a0), %d0/%a0-%a1 | %d0 = buf->remcount, %a0 = buf->p32[0],
387 | %a1 = buf->p32[1]
367 move.l %a0, %a2 | use separate dst pointers since read 388 move.l %a0, %a2 | use separate dst pointers since read
368 move.l %a1, %a3 | pointers run one ahead of write 389 move.l %a1, %a3 | pointers run one ahead of write
369 move.l #0x40000000, %d3 | %d3 = 0.5 390 move.l #0x40000000, %d3 | %d3 = 0.5
@@ -390,12 +411,90 @@ channels_process_sound_chan_karaoke:
390 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers 411 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers
391 lea.l 20(%sp), %sp | cleanup 412 lea.l 20(%sp), %sp | cleanup
392 rts | 413 rts |
393 .size channels_process_sound_chan_karaoke, \ 414 .size channel_mode_proc_karaoke, .-channel_mode_proc_karaoke
394 .-channels_process_sound_chan_karaoke 415
416/****************************************************************************
417 * void filter_process(struct dsp_filter *f, int32_t *buf[], int count,
418 * unsigned int channels)
419 *
420 * define HIGH_PRECISION as '1' to make filtering calculate lower bits after
421 * shifting. without this, "shift" - 1 of the lower bits will be lost here.
422 */
423#define HIGH_PRECISION 0
424 .text
425 .global filter_process
426filter_process:
427 | input: 4(sp) = f, 8(sp) = buf, 12(sp) = count, 16(sp) = channels
428 lea.l -44(%sp), %sp | save clobbered regs
429#if HIGH_PRECISION
430 movem.l %d2-%d7/%a2-%a6, (%sp) | .
431#else
432 movem.l %d2-%d6/%a2-%a6, (%sp) |
433#endif
434 move.l 48(%sp), %a5 | fetch filter structure address
435 clr.l %d6 | load shift count
436 move.b 52(%a5), %d6 | .
437 subq.l #1, %d6 | EMAC gives us one free shift
438#if HIGH_PRECISION
439 moveq.l #8, %d7
440 sub.l %d6, %d7 | shift for lower part of accumulator
441#endif
442 movem.l (%a5), %a0-%a4 | load coefs
443 lea.l 20(%a5), %a5 | point to filter history
444
44510: | channel loop
446 move.l 52(%sp), %a6 | load input channel pointer
447 addq.l #4, 52(%sp) | point x to next channel
448 move.l (%a6), %a6 |
449 move.l 56(%sp), %d5 | number of samples
450 movem.l (%a5), %d0-%d3 | load filter history
451
452 | d0-d3 = history, d4 = temp, d5 = sample count, d6 = upper shift amount,
453 | d7 = lower shift amount,a0-a4 = coefs, a5 = history pointer, a6 = buf[ch]
45420: | loop
455 | Direct form 1 filtering code. We assume DSP has put EMAC in frac mode.
456 | y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
457 | where y[] is output and x[] is input. This is performed out of order
458 | to do parallel load of input value.
459 mac.l %a2, %d1, %acc0 | acc = b2*x[i - 2]
460 move.l %d0, %d1 | fix input history
461 mac.l %a1, %d0, (%a6), %d0, %acc0 | acc += b1*x[i - 1], x[i] -> d0
462 mac.l %a0, %d0, %acc0 | acc += b0*x[i]
463 mac.l %a3, %d2, %acc0 | acc += a1*y[i - 1]
464 mac.l %a4, %d3, %acc0 | acc += a2*y[i - 2]
465 move.l %d2, %d3 | fix output history
466#if HIGH_PRECISION
467 move.l %accext01, %d2 | fetch lower part of accumulator
468 move.b %d2, %d4 | clear upper three bytes
469 lsr.l %d7, %d4 | shift lower bits
470#endif
471 movclr.l %acc0, %d2 | fetch upper part of result
472 asl.l %d6, %d2 | restore fixed point format
473#if HIGH_PRECISION
474 or.l %d2, %d4 | combine lower and upper parts
475#endif
476 move.l %d2, (%a6)+ | save result
477 subq.l #1, %d5 | are we done with this channel?
478 bgt 20b | loop
479
480 movem.l %d0-%d3, (%a5) | save history back to struct
481 lea.l 16(%a5), %a5 | point to next channel's history
482 subq.l #1, 60(%sp) | have we processed both channels?
483 bhi 10b | channel loop
484
485#if HIGH_PRECISION
486 movem.l (%sp), %d2-%d7/%a2-%a6
487#else
488 movem.l (%sp), %d2-%d6/%a2-%a6
489#endif
490 lea.l 44(%sp), %sp
491 rts
492 .size filter_process, .-filter_process
395 493
396/**************************************************************************** 494/****************************************************************************
397 * void sample_output_stereo(int count, struct dsp_data *data, 495 * void sample_output_stereo(struct sample_io_data *this,
398 * const int32_t *src[], int16_t *dst) 496 * struct dsp_buffer *src,
497 * struct dsp_buffer *dst)
399 * 498 *
400 * Framework based on the ubiquitous Rockbox line transfer logic for 499 * Framework based on the ubiquitous Rockbox line transfer logic for
401 * Coldfire CPUs. 500 * Coldfire CPUs.
@@ -417,20 +516,24 @@ channels_process_sound_chan_karaoke:
417 .align 2 516 .align 2
418 .global sample_output_stereo 517 .global sample_output_stereo
419sample_output_stereo: 518sample_output_stereo:
519 | input: 4(sp) = count, 8(sp) = src, 12(sp) = dst
420 lea.l -48(%sp), %sp | save registers 520 lea.l -48(%sp), %sp | save registers
421 move.l %macsr, %d1 | do it now as at many lines will 521 move.l %macsr, %d1 | do it now as at many lines will
422 movem.l %d1-%d7/%a2-%a6, (%sp) | be the far more common condition 522 movem.l %d1-%d7/%a2-%a6, (%sp) | be the far more common condition
423 move.l #0x80, %macsr | put emac unit in signed int mode 523 move.l #0x80, %macsr | put emac unit in signed int mode
424 movem.l 52(%sp), %a0-%a2/%a4 | 524 movem.l 52(%sp), %a0-%a2 | %a0 = this, %a1 = src, %a2 = dst
425 lea.l (%a4, %a0.l*4), %a0 | %a0 = end address 525 move.l (%a0), %a0 | %a0 = this->outcount
426 move.l (%a1), %d1 | %a1 = multiplier: (1 << (16 - scale)) 526 move.l 4(%a2), %a4 | %a4 = dst->p16out
527 lea.l (%a4, %a0.l*4), %a0 | %a0 = count -> end address
528 movem.l 4(%a1), %a2-%a3 | %a2 = src->p32[0], %a3 = src->p32[1]
529 clr.l %d1 | %a1 = multiplier: (1 << (16 - scale))
530 move.b 19(%a1), %d1 | %d1 = src->format.output_scale
427 sub.l #16, %d1 | 531 sub.l #16, %d1 |
428 neg.l %d1 | 532 neg.l %d1 |
429 moveq.l #1, %d0 | 533 moveq.l #1, %d0 |
430 asl.l %d1, %d0 | 534 asl.l %d1, %d0 |
431 move.l %d0, %a1 | 535 move.l %d0, %a1 |
432 move.l #0x8000, %a6 | %a6 = rounding term 536 move.l #0x8000, %a6 | %a6 = rounding term
433 movem.l (%a2), %a2-%a3 | get L/R channel pointers
434 moveq.l #28, %d0 | %d0 = second line bound 537 moveq.l #28, %d0 | %d0 = second line bound
435 add.l %a4, %d0 | 538 add.l %a4, %d0 |
436 and.l #0xfffffff0, %d0 | 539 and.l #0xfffffff0, %d0 |
@@ -447,7 +550,7 @@ sample_output_stereo:
447 mac.l %d2, %a1, %acc1 | shift R to high word 550 mac.l %d2, %a1, %acc1 | shift R to high word
448 movclr.l %acc0, %d1 | get possibly saturated results 551 movclr.l %acc0, %d1 | get possibly saturated results
449 movclr.l %acc1, %d2 | 552 movclr.l %acc1, %d2 |
450 swap %d2 | move R to low word 553 swap.w %d2 | move R to low word
451 move.w %d2, %d1 | interleave MS 16 bits of each 554 move.w %d2, %d1 | interleave MS 16 bits of each
452 move.l %d1, (%a4)+ | ...and write both 555 move.l %d1, (%a4)+ | ...and write both
453 cmp.l %a4, %d0 | 556 cmp.l %a4, %d0 |
@@ -477,10 +580,10 @@ sample_output_stereo:
477 mac.l %d1, %a1, (%a2)+, %d2, %acc1 | with saturation 580 mac.l %d1, %a1, (%a2)+, %d2, %acc1 | with saturation
478 mac.l %d2, %a1, (%a2)+, %d3, %acc2 | 581 mac.l %d2, %a1, (%a2)+, %d3, %acc2 |
479 mac.l %d3, %a1 , %acc3 | 582 mac.l %d3, %a1 , %acc3 |
480 swap %d4 | a) interleave most significant... 583 swap.w %d4 | a) interleave most significant...
481 swap %d5 | 584 swap.w %d5 |
482 swap %d6 | 585 swap.w %d6 |
483 swap %d7 | 586 swap.w %d7 |
484 movclr.l %acc0, %d0 | obtain L results 587 movclr.l %acc0, %d0 | obtain L results
485 movclr.l %acc1, %d1 | 588 movclr.l %acc1, %d1 |
486 movclr.l %acc2, %d2 | 589 movclr.l %acc2, %d2 |
@@ -503,7 +606,7 @@ sample_output_stereo:
503 mac.l %d2, %a1, %acc1 | 606 mac.l %d2, %a1, %acc1 |
504 movclr.l %acc0, %d1 | 607 movclr.l %acc0, %d1 |
505 movclr.l %acc1, %d2 | 608 movclr.l %acc1, %d2 |
506 swap %d2 | 609 swap.w %d2 |
507 move.w %d2, %d1 | 610 move.w %d2, %d1 |
508 move.l %d1, (%a4)+ | 611 move.l %d1, (%a4)+ |
509 cmp.l %a4, %a0 | 612 cmp.l %a4, %a0 |
@@ -516,8 +619,9 @@ sample_output_stereo:
516 .size sample_output_stereo, .-sample_output_stereo 619 .size sample_output_stereo, .-sample_output_stereo
517 620
518/**************************************************************************** 621/****************************************************************************
519 * void sample_output_mono(int count, struct dsp_data *data, 622 * void sample_output_mono(struct sample_io_data *this,
520 * const int32_t *src[], int16_t *dst) 623 * struct dsp_buffer *src,
624 * struct dsp_buffer *dst)
521 * 625 *
522 * Same treatment as sample_output_stereo but for one channel. 626 * Same treatment as sample_output_stereo but for one channel.
523 */ 627 */
@@ -525,19 +629,23 @@ sample_output_stereo:
525 .align 2 629 .align 2
526 .global sample_output_mono 630 .global sample_output_mono
527sample_output_mono: 631sample_output_mono:
632 | input: 4(sp) = count, 8(sp) = src, 12(sp) = dst
528 lea.l -32(%sp), %sp | save registers 633 lea.l -32(%sp), %sp | save registers
529 move.l %macsr, %d1 | do it now as at many lines will 634 move.l %macsr, %d1 | do it now as at many lines will
530 movem.l %d1-%d5/%a2-%a4, (%sp) | be the far more common condition 635 movem.l %d1-%d5/%a2-%a4, (%sp) | be the far more common condition
531 move.l #0x80, %macsr | put emac unit in signed int mode 636 move.l #0x80, %macsr | put emac unit in signed int mode
532 movem.l 36(%sp), %a0-%a3 | 637 movem.l 36(%sp), %a0-%a2 | %a0 = this, %a1 = src, %a2 = dst
533 lea.l (%a3, %a0.l*4), %a0 | %a0 = end address 638 move.l (%a0), %a0 | %a0 = this->outcount
534 move.l (%a1), %d1 | %d5 = multiplier: (1 << (16 - scale)) 639 move.l 4(%a2), %a3 | %a3 = dst->p16out
640 movem.l 4(%a1), %a2 | %a2 = src->p32[0]
641 lea.l (%a3, %a0.l*4), %a0 | %a0 = count -> end address
642 clr.l %d1 | %d5 = multiplier: (1 << (16 - scale))
643 move.b 19(%a1), %d1 | %d1 = src->format.output_scale
535 sub.l #16, %d1 | 644 sub.l #16, %d1 |
536 neg.l %d1 | 645 neg.l %d1 |
537 moveq.l #1, %d5 | 646 moveq.l #1, %d5 |
538 asl.l %d1, %d5 | 647 asl.l %d1, %d5 |
539 move.l #0x8000, %a4 | %a4 = rounding term 648 move.l #0x8000, %a4 | %a4 = rounding term
540 movem.l (%a2), %a2 | get source channel pointer
541 moveq.l #28, %d0 | %d0 = second line bound 649 moveq.l #28, %d0 | %d0 = second line bound
542 add.l %a3, %d0 | 650 add.l %a3, %d0 |
543 and.l #0xfffffff0, %d0 | 651 and.l #0xfffffff0, %d0 |
@@ -552,7 +660,7 @@ sample_output_mono:
552 mac.l %d1, %d5, %acc0 | shift L to high word 660 mac.l %d1, %d5, %acc0 | shift L to high word
553 movclr.l %acc0, %d1 | get possibly saturated results 661 movclr.l %acc0, %d1 | get possibly saturated results
554 move.l %d1, %d2 | 662 move.l %d1, %d2 |
555 swap %d2 | move R to low word 663 swap.w %d2 | move R to low word
556 move.w %d2, %d1 | duplicate single channel into 664 move.w %d2, %d1 | duplicate single channel into
557 move.l %d1, (%a3)+ | L and R 665 move.l %d1, (%a3)+ | L and R
558 cmp.l %a3, %d0 | 666 cmp.l %a3, %d0 |
@@ -575,16 +683,16 @@ sample_output_mono:
575 movclr.l %acc2, %d2 | 683 movclr.l %acc2, %d2 |
576 movclr.l %acc3, %d3 | 684 movclr.l %acc3, %d3 |
577 move.l %d0, %d4 | duplicate single channel 685 move.l %d0, %d4 | duplicate single channel
578 swap %d4 | into L and R 686 swap.w %d4 | into L and R
579 move.w %d4, %d0 | 687 move.w %d4, %d0 |
580 move.l %d1, %d4 | 688 move.l %d1, %d4 |
581 swap %d4 | 689 swap.w %d4 |
582 move.w %d4, %d1 | 690 move.w %d4, %d1 |
583 move.l %d2, %d4 | 691 move.l %d2, %d4 |
584 swap %d4 | 692 swap.w %d4 |
585 move.w %d4, %d2 | 693 move.w %d4, %d2 |
586 move.l %d3, %d4 | 694 move.l %d3, %d4 |
587 swap %d4 | 695 swap.w %d4 |
588 move.w %d4, %d3 | 696 move.w %d4, %d3 |
589 movem.l %d0-%d3, -16(%a3) | write four stereo samples 697 movem.l %d0-%d3, -16(%a3) | write four stereo samples
590 cmp.l %a3, %a1 | 698 cmp.l %a3, %a1 |
@@ -598,7 +706,7 @@ sample_output_mono:
598 mac.l %d1, %d5, %acc0 | the same way as leading ones 706 mac.l %d1, %d5, %acc0 | the same way as leading ones
599 movclr.l %acc0, %d1 | 707 movclr.l %acc0, %d1 |
600 move.l %d1, %d2 | 708 move.l %d1, %d2 |
601 swap %d2 | 709 swap.w %d2 |
602 move.w %d2, %d1 | 710 move.w %d2, %d1 |
603 move.l %d1, (%a3)+ | 711 move.l %d1, (%a3)+ |
604 cmp.l %a3, %a0 | 712 cmp.l %a3, %a0 |
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 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Miika Pekkarinen
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include "dsp_sample_io.h"
26#include <sys/types.h>
27
28/* Define LOGF_ENABLE to enable logf output in this file */
29/*#define LOGF_ENABLE*/
30#include "logf.h"
31
32/* Actually generate the database of stages */
33#define DSP_PROC_DB_CREATE
34#include "dsp_proc_entry.h"
35
36/* Linked lists give fewer loads in processing loop compared to some index
37 * list, which is more important than keeping occasionally executed code
38 * simple */
39
40struct dsp_config
41{
42 /** General DSP-local data **/
43 struct sample_io_data io_data; /* Sample input-output data (first) */
44 uint32_t slot_free_mask; /* Mask of free slots for this DSP */
45 uint32_t proc_masks[2]; /* Mask of active/enabled stages */
46 struct dsp_proc_slot
47 {
48 struct dsp_proc_entry proc_entry; /* This enabled stage */
49 struct dsp_proc_slot *next[2]; /* [0]=active next, [1]=enabled next */
50 const struct dsp_proc_db_entry *db_entry;
51 } *proc_slots[2]; /* Pointer to first in list of
52 active/enabled stages */
53
54 /** Misc. extra stuff **/
55#ifdef CPU_COLDFIRE
56 unsigned long old_macsr; /* Old macsr value to restore */
57#endif
58#if 0 /* Not needed now but enable if something must know this */
59 bool processing; /* DSP is processing (to thwart inopportune
60 buffer moves) */
61#endif
62};
63
64/* Pool of slots for stages - supports 32 or fewer combined as-is atm. */
65static struct dsp_proc_slot
66dsp_proc_slot_arr[DSP_NUM_PROC_STAGES+DSP_VOICE_NUM_PROC_STAGES] IBSS_ATTR;
67
68/* General DSP config */
69static struct dsp_config dsp_conf[DSP_COUNT] IBSS_ATTR;
70
71/** Processing stages support functions **/
72
73/* Find the slot for a given enabled id */
74static struct dsp_proc_slot * find_proc_slot(struct dsp_config *dsp,
75 unsigned int id)
76{
77 const uint32_t mask = BIT_N(id);
78
79 if ((dsp->proc_masks[1] & mask) == 0)
80 return NULL; /* Not enabled */
81
82 struct dsp_proc_slot *s = dsp->proc_slots[1];
83
84 while (1) /* In proc_masks == it must be there */
85 {
86 if (BIT_N(s->db_entry->id) == mask)
87 return s;
88
89 s = s->next[1];
90 }
91}
92
93/* Broadcast to all enabled stages or to the one with the specifically
94 * crafted setting */
95static intptr_t proc_broadcast(struct dsp_config *dsp, unsigned int setting,
96 intptr_t value)
97{
98 bool multi = setting < DSP_PROC_SETTING;
99 struct dsp_proc_slot *s = multi ?
100 dsp->proc_slots[1] : find_proc_slot(dsp, setting - DSP_PROC_SETTING);
101
102 while (s != NULL)
103 {
104 intptr_t ret = s->db_entry->configure(&s->proc_entry, dsp, setting,
105 value);
106 if (!multi)
107 return ret;
108
109 s = s->next[1];
110 }
111
112 return multi ? 1 : 0;
113}
114
115/* Generic handler for this->process[0] */
116static void dsp_process_null(struct dsp_proc_entry *this,
117 struct dsp_buffer **buf_p)
118{
119 (void)this; (void)buf_p;
120}
121
122/* Generic handler for this->process[1] */
123static void dsp_format_change_process(struct dsp_proc_entry *this,
124 struct dsp_buffer **buf_p)
125{
126 enum dsp_proc_ids id =
127 TYPE_FROM_MEMBER(struct dsp_proc_slot, this, proc_entry)->db_entry->id;
128
129 DSP_PRINT_FORMAT(<Default Handler>, id, (*buf_p)->format);
130
131 /* We don't keep back references to the DSP, so just search for it */
132 struct dsp_config *dsp;
133 for (int i = 0; (dsp = dsp_get_config(i)); i++)
134 {
135 struct dsp_proc_slot *slot = find_proc_slot(dsp, id);
136 /* Found one with the id, check if it's this one */
137 if (&slot->proc_entry == this && dsp_proc_active(dsp, id))
138 {
139 dsp_proc_call(this, buf_p, 0);
140 break;
141 }
142 }
143}
144
145/* Add an item to the enabled list */
146static struct dsp_proc_slot *
147dsp_proc_enable_enlink(struct dsp_config *dsp, uint32_t mask)
148{
149 /* Use the lowest-indexed available slot */
150 int slot = find_first_set_bit(dsp->slot_free_mask);
151
152 if (slot == 32)
153 {
154 /* Should NOT happen, ever, unless called before init */
155 DEBUGF("DSP %d: no slots!\n", (int)dsp_get_id(dsp));
156 return NULL;
157 }
158
159 const struct dsp_proc_db_entry *db_entry_prev = NULL;
160 const struct dsp_proc_db_entry *db_entry;
161
162 /* Order of enabled list is same as DB array */
163 for (unsigned int i = 0;; i++)
164 {
165 if (i >= DSP_NUM_PROC_STAGES)
166 return NULL;
167
168 db_entry = dsp_proc_database[i];
169
170 uint32_t m = BIT_N(db_entry->id);
171
172 if (m == mask)
173 break; /* This is the one */
174
175 if (dsp->proc_masks[1] & m)
176 db_entry_prev = db_entry;
177 }
178
179 struct dsp_proc_slot *s = &dsp_proc_slot_arr[slot];
180
181 if (db_entry_prev != NULL)
182 {
183 struct dsp_proc_slot *prev = find_proc_slot(dsp, db_entry_prev->id);
184 s->next[0] = prev->next[0];
185 s->next[1] = prev->next[1];
186 prev->next[1] = s;
187 }
188 else
189 {
190 s->next[0] = dsp->proc_slots[0];
191 s->next[1] = dsp->proc_slots[1];
192 dsp->proc_slots[1] = s;
193 }
194
195 s->db_entry = db_entry; /* record DB entry */
196 dsp->proc_masks[1] |= mask;
197 dsp->slot_free_mask &= ~BIT_N(slot);
198
199 return s;
200}
201
202/* Remove an item from the enabled list */
203static struct dsp_proc_slot *
204dsp_proc_enable_delink(struct dsp_config *dsp, uint32_t mask)
205{
206 struct dsp_proc_slot *s = dsp->proc_slots[1];
207 struct dsp_proc_slot *prev = NULL;
208
209 while (1) /* In proc_masks == it must be there */
210 {
211 if (BIT_N(s->db_entry->id) == mask)
212 {
213 if (prev)
214 prev->next[1] = s->next[1];
215 else
216 dsp->proc_slots[1] = s->next[1];
217
218 dsp->proc_masks[1] &= ~mask;
219 dsp->slot_free_mask |= BIT_N(s - dsp_proc_slot_arr);
220 return s;
221 }
222
223 prev = s;
224 s = s->next[1];
225 }
226}
227
228void dsp_proc_enable(struct dsp_config *dsp, enum dsp_proc_ids id,
229 bool enable)
230{
231 uint32_t mask = BIT_N(id);
232 bool enabled = dsp->proc_masks[1] & mask;
233
234 if (enable)
235 {
236 /* If enabled, just find it in list, if not, link a new one */
237 struct dsp_proc_slot *s = enabled ? find_proc_slot(dsp, id) :
238 dsp_proc_enable_enlink(dsp, mask);
239
240 if (s == NULL)
241 {
242 DEBUGF("DSP- proc id not valid: %d\n", (int)id);
243 return;
244 }
245
246 if (!enabled)
247 {
248 /* New entry - set defaults */
249 s->proc_entry.data = 0;
250 s->proc_entry.ip_mask = mask;
251 s->proc_entry.process[0] = dsp_process_null;
252 s->proc_entry.process[1] = dsp_format_change_process;
253 }
254
255 enabled = s->db_entry->configure(&s->proc_entry, dsp, DSP_PROC_INIT,
256 enabled) >= 0;
257 if (enabled)
258 return;
259
260 DEBUGF("DSP- proc init failed: %d\n", (int)id);
261 /* Cleanup below */
262 }
263 else if (!enabled)
264 {
265 return; /* No change */
266 }
267
268 dsp_proc_activate(dsp, id, false); /* Deactivate it first */
269 struct dsp_proc_slot *s = dsp_proc_enable_delink(dsp, mask);
270 s->db_entry->configure(&s->proc_entry, dsp, DSP_PROC_CLOSE, 0);
271}
272
273/* Maintain the list structure for the active list where each enabled entry
274 * has a link to the next active item, even if not active which facilitates
275 * switching out of format change mode by a stage during a format change.
276 * When that happens, the iterator must jump over inactive but enabled
277 * stages after its current position. */
278static struct dsp_proc_slot *
279dsp_proc_activate_link(struct dsp_config *dsp, uint32_t mask,
280 struct dsp_proc_slot *s)
281{
282 uint32_t m = BIT_N(s->db_entry->id);
283 uint32_t mor = m | mask;
284
285 if (mor == m) /* Only if same single bit in common */
286 {
287 dsp->proc_masks[0] |= mask;
288 return s;
289 }
290 else if (~mor == 0) /* Only if bits complement */
291 {
292 dsp->proc_masks[0] &= mask;
293 return s->next[0];
294 }
295
296 struct dsp_proc_slot *next = s->next[1];
297 next = dsp_proc_activate_link(dsp, mask, next);
298
299 s->next[0] = next;
300
301 return (m & dsp->proc_masks[0]) ? s : next;
302}
303
304/* Activate or deactivate a stage */
305void dsp_proc_activate(struct dsp_config *dsp, enum dsp_proc_ids id,
306 bool activate)
307{
308 const uint32_t mask = BIT_N(id);
309
310 if (!(dsp->proc_masks[1] & mask))
311 return; /* Not enabled */
312
313 if (activate != !(dsp->proc_masks[0] & mask))
314 return; /* No change in state */
315
316 /* Send mask bit if activating and ones complement if deactivating */
317 dsp->proc_slots[0] = dsp_proc_activate_link(
318 dsp, activate ? mask : ~mask, dsp->proc_slots[1]);
319}
320
321/* Is the stage specified by the id currently active? */
322bool dsp_proc_active(struct dsp_config *dsp, enum dsp_proc_ids id)
323{
324 return (dsp->proc_masks[0] & BIT_N(id)) != 0;
325}
326
327/* Determine by the rules if the processing function should be called */
328static FORCE_INLINE bool dsp_proc_should_call(struct dsp_proc_entry *this,
329 struct dsp_buffer *buf,
330 unsigned int fmt)
331{
332 uint32_t ip_mask = this->ip_mask;
333
334 return UNLIKELY(fmt != 0) || /* Also pass override value */
335 ip_mask == 0 || /* Not in-place */
336 ((ip_mask & buf->proc_mask) == 0 &&
337 (buf->proc_mask |= ip_mask, buf->remcount > 0));
338}
339
340/* Call this->process[fmt] according to the rules (for external call) */
341bool dsp_proc_call(struct dsp_proc_entry *this, struct dsp_buffer **buf_p,
342 unsigned int fmt)
343{
344 if (dsp_proc_should_call(this, *buf_p, fmt))
345 {
346 this->process[fmt == (0u-1u) ? 0 : fmt](this, buf_p);
347 return true;
348 }
349
350 return false;
351}
352
353static inline void dsp_process_start(struct dsp_config *dsp)
354{
355#if defined(CPU_COLDFIRE)
356 /* set emac unit for dsp processing, and save old macsr, we're running in
357 codec thread context at this point, so can't clobber it */
358 dsp->old_macsr = coldfire_get_macsr();
359 coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
360#endif
361#if 0 /* Not needed now but enable if something must know this */
362 dsp->processing = true;
363#endif
364 (void)dsp;
365}
366
367static inline void dsp_process_end(struct dsp_config *dsp)
368{
369#if 0 /* Not needed now but enable if something must know this */
370 dsp->processing = false;
371#endif
372#if defined(CPU_COLDFIRE)
373 /* set old macsr again */
374 coldfire_set_macsr(dsp->old_macsr);
375#endif
376 (void)dsp;
377}
378
379/**
380 * dsp_process:
381 *
382 * Process and convert src audio to dst based on the DSP configuration.
383 * dsp: the DSP instance in use
384 *
385 * src:
386 * remcount = number of input samples remaining; set to desired
387 * number of samples to be processed
388 * pin[0] = left channel if non-interleaved, audio data if
389 * interleaved or mono
390 * pin[1] = right channel if non-interleaved, ignored if
391 * interleaved or mono
392 * proc_mask = set to zero on first call, updated by this function
393 * to keep track of which in-place stages have been
394 * run on the buffers to avoid multiple applications of
395 * them
396 * format = for internal buffers, gives the relevant format
397 * details
398 *
399 * dst:
400 * remcount = number of samples placed in buffer so far; set to
401 * zero on first call
402 * p16out = current fill pointer in destination buffer; set to
403 * buffer start on first call
404 * bufcount = remaining buffer space in samples; set to maximum
405 * desired output count on first call
406 * format = ignored
407 *
408 * Processing stops when src is exhausted or dst is filled, whichever
409 * happens first. Samples can still be output when src buffer is empty
410 * if samples are held internally. Generally speaking, continue calling
411 * until no data is consumed and no data is produced to purge the DSP
412 * to the maximum extent feasible. Some internal processing stages may
413 * require more input before more output can be generated, thus there
414 * is no guarantee the DSP is free of data awaiting processing at that
415 * point.
416 *
417 * Additionally, samples consumed and samples produced do not necessarily
418 * have a direct correlation. Samples may be consumed without producing
419 * any output and samples may be produced without consuming any input.
420 * It depends on which stages are actively processing data at the time
421 * of the call and how they function internally.
422 */
423void dsp_process(struct dsp_config *dsp, struct dsp_buffer *src,
424 struct dsp_buffer *dst)
425{
426 if (dst->bufcount <= 0)
427 {
428 /* No place to put anything thus nothing may be safely consumed */
429 return;
430 }
431
432 /* At least perform one yield before starting */
433 long last_yield = current_tick;
434 yield();
435
436 dsp_process_start(dsp);
437
438 /* Tag input with codec-specified sample format */
439 src->format = dsp->io_data.format;
440
441 while (1)
442 {
443 /* Out-of-place-processing stages take the current buf as input
444 * and switch the buffer to their own output buffer */
445 struct dsp_buffer *buf = src;
446 unsigned int fmt = buf->format.changed;
447
448 /* Convert input samples to internal format */
449 dsp->io_data.input_samples[fmt](&dsp->io_data, &buf);
450 fmt = buf->format.changed;
451
452 struct dsp_proc_slot *s = dsp->proc_slots[fmt];
453
454 /* Call all active/enabled stages depending if format is
455 same/changed on the last output buffer */
456 while (s != NULL)
457 {
458 if (dsp_proc_should_call(&s->proc_entry, buf, fmt))
459 {
460 s->proc_entry.process[fmt](&s->proc_entry, &buf);
461 fmt = buf->format.changed;
462 }
463
464 /* The buffer may have changed along with the format flag */
465 s = s->next[fmt];
466 }
467
468 /* Don't overread/write src/destination */
469 int outcount = MIN(dst->bufcount, buf->remcount);
470
471 if (fmt == 0 && outcount <= 0)
472 break; /* Output full or purged internal buffers */
473
474 dsp->io_data.outcount = outcount;
475 dsp->io_data.output_samples[fmt](&dsp->io_data, buf, dst);
476
477 /* Advance buffers by what output consumed and produced */
478 dsp_advance_buffer32(buf, outcount);
479 dsp_advance_buffer_output(dst, outcount);
480
481 /* Yield at least once each tick */
482 long tick = current_tick;
483 if (TIME_AFTER(tick, last_yield))
484 {
485 last_yield = tick;
486 yield();
487 }
488 } /* while */
489
490 dsp_process_end(dsp);
491}
492
493intptr_t dsp_configure(struct dsp_config *dsp, unsigned int setting,
494 intptr_t value)
495{
496 dsp_sample_io_configure(&dsp->io_data, setting, value);
497 return proc_broadcast(dsp, setting, value);
498}
499
500struct dsp_config * dsp_get_config(enum dsp_ids id)
501{
502 if (id >= DSP_COUNT)
503 return NULL;
504
505 return &dsp_conf[id];
506}
507
508/* Return the id given a dsp pointer (or even via something within
509 the struct itself) */
510enum dsp_ids dsp_get_id(const struct dsp_config *dsp)
511{
512 ptrdiff_t id = dsp - dsp_conf;
513
514 if (id < 0 || id >= DSP_COUNT)
515 return DSP_COUNT; /* obviously invalid */
516
517 return (enum dsp_ids)id;
518}
519
520#if 0 /* Not needed now but enable if something must know this */
521bool dsp_is_busy(const struct dsp_config *dsp)
522{
523 return dsp->processing;
524}
525#endif /* 0 */
526
527/* Do what needs initializing before enable/disable calls can be made.
528 * Must be done before changing settings for the first time. */
529void INIT_ATTR dsp_init(void)
530{
531 static const uint8_t slot_count[DSP_COUNT] /* INITDATA_ATTR */ =
532 {
533 [CODEC_IDX_AUDIO] = DSP_NUM_PROC_STAGES,
534 [CODEC_IDX_VOICE] = DSP_VOICE_NUM_PROC_STAGES
535 };
536
537 for (unsigned int i = 0, count, shift = 0;
538 i < DSP_COUNT;
539 i++, shift += count)
540 {
541 struct dsp_config *dsp = &dsp_conf[i];
542
543 count = slot_count[i];
544 dsp->slot_free_mask = MASK_N(uint32_t, count, shift);
545
546 dsp_sample_io_configure(&dsp->io_data, DSP_INIT, i);
547
548 /* Notify each db entry of global init for each DSP */
549 for (unsigned int j = 0; j < DSP_NUM_PROC_STAGES; j++)
550 dsp_proc_database[j]->configure(NULL, dsp, DSP_INIT, i);
551
552 dsp_configure(dsp, DSP_RESET, 0);
553 }
554}
diff --git a/lib/rbcodec/dsp/dsp_filter.c b/lib/rbcodec/dsp/dsp_filter.c
new file mode 100644
index 0000000000..ee0ce1b18f
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_filter.c
@@ -0,0 +1,306 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006-2007 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#include <stdbool.h>
22#include <string.h>
23#include "config.h"
24#include "fixedpoint.h"
25#include "fracmul.h"
26#include "dsp_filter.h"
27#include "replaygain.h"
28
29enum filter_shift
30{
31 FILTER_BISHELF_SHIFT = 5, /* For bishelf (bass/treble) */
32 FILTER_PEAK_SHIFT = 4, /* Each peaking filter */
33 FILTER_SHELF_SHIFT = 6, /* Each high/low shelving filter */
34};
35
36/**
37 * Calculate first order shelving filter. Filter is not directly usable by the
38 * filter_process() function.
39 * @param cutoff shelf midpoint frequency. See eq_pk_coefs for format.
40 * @param A decibel value multiplied by ten, describing gain/attenuation of
41 * shelf. Max value is 24 dB.
42 * @param low true for low-shelf filter, false for high-shelf filter.
43 * @param c pointer to coefficient storage. Coefficients are s4.27 format.
44 */
45void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c)
46{
47 long sin, cos;
48 int32_t b0, b1, a0, a1; /* s3.28 */
49 const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */
50
51 sin = fp_sincos(cutoff/2, &cos);
52 if (low) {
53 const int32_t sin_div_g = fp_div(sin, g, 25);
54 const int32_t sin_g = FRACMUL(sin, g);
55 cos >>= 3;
56 b0 = sin_g + cos; /* 0.25 .. 4.10 */
57 b1 = sin_g - cos; /* -1 .. 3.98 */
58 a0 = sin_div_g + cos; /* 0.25 .. 4.10 */
59 a1 = sin_div_g - cos; /* -1 .. 3.98 */
60 } else {
61 const int32_t cos_div_g = fp_div(cos, g, 25);
62 const int32_t cos_g = FRACMUL(cos, g);
63 sin >>= 3;
64 b0 = sin + cos_g; /* 0.25 .. 4.10 */
65 b1 = sin - cos_g; /* -3.98 .. 1 */
66 a0 = sin + cos_div_g; /* 0.25 .. 4.10 */
67 a1 = sin - cos_div_g; /* -3.98 .. 1 */
68 }
69
70 const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */
71 *c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */
72 *c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */
73 *c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */
74}
75
76#ifdef HAVE_SW_TONE_CONTROLS
77/**
78 * Calculate second order section filter consisting of one low-shelf and one
79 * high-shelf section.
80 * @param cutoff_low low-shelf midpoint frequency. See filter_pk_coefs for format.
81 * @param cutoff_high high-shelf midpoint frequency.
82 * @param A_low decibel value multiplied by ten, describing gain/attenuation of
83 * low-shelf part. Max value is 24 dB.
84 * @param A_high decibel value multiplied by ten, describing gain/attenuation of
85 * high-shelf part. Max value is 24 dB.
86 * @param A decibel value multiplied by ten, describing additional overall gain.
87 * @param c pointer to coefficient storage. Coefficients are s4.27 format.
88 */
89void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high,
90 long A_low, long A_high, long A,
91 struct dsp_filter *f)
92{
93 const long g = get_replaygain_int(A*10) << 7; /* 10^(db/20), s0.31 */
94 int32_t c_ls[3], c_hs[3];
95
96 filter_shelf_coefs(cutoff_low, A_low, true, c_ls);
97 filter_shelf_coefs(cutoff_high, A_high, false, c_hs);
98 c_ls[0] = FRACMUL(g, c_ls[0]);
99 c_ls[1] = FRACMUL(g, c_ls[1]);
100
101 /* now we cascade the two first order filters to one second order filter
102 * which can be used by filter_process(). these resulting coefficients have a
103 * really wide numerical range, so we use a fixed point format which will
104 * work for the selected cutoff frequencies (in tone_controls.c) only.
105 */
106 const int32_t b0 = c_ls[0], b1 = c_ls[1], b2 = c_hs[0], b3 = c_hs[1];
107 const int32_t a0 = c_ls[2], a1 = c_hs[2];
108
109 int32_t *c = f->coefs;
110 *c++ = FRACMUL_SHL(b0, b2, 4);
111 *c++ = FRACMUL_SHL(b0, b3, 4) + FRACMUL_SHL(b1, b2, 4);
112 *c++ = FRACMUL_SHL(b1, b3, 4);
113 *c++ = a0 + a1;
114 *c = -FRACMUL_SHL(a0, a1, 4);
115
116 f->shift = FILTER_BISHELF_SHIFT;
117}
118#endif /* HAVE_SW_TONE_CONTROLS */
119
120/* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson.
121 * Slightly faster calculation can be done by deriving forms which use tan()
122 * instead of cos() and sin(), but the latter are far easier to use when doing
123 * fixed point math, and performance is not a big point in the calculation part.
124 * All the 'a' filter coefficients are negated so we can use only additions
125 * in the filtering equation.
126 */
127
128/**
129 * Calculate second order section peaking filter coefficients.
130 * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and
131 * 0x80000000 represents the Nyquist frequency (samplerate/2).
132 * @param Q Q factor value multiplied by ten. Lower bound is artificially set
133 * at 0.5.
134 * @param db decibel value multiplied by ten, describing gain/attenuation at
135 * peak freq. Max value is 24 dB.
136 * @param c pointer to coefficient storage. Coefficients are s3.28 format.
137 */
138void filter_pk_coefs(unsigned long cutoff, unsigned long Q, long db,
139 struct dsp_filter *f)
140{
141 long cs;
142 const long one = 1 << 28; /* s3.28 */
143 const long A = get_replaygain_int(db*5) << 5; /* 10^(db/40), s2.29 */
144 const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
145 int32_t a0, a1, a2; /* these are all s3.28 format */
146 int32_t b0, b1, b2;
147 const long alphadivA = fp_div(alpha, A, 27);
148 const long alphaA = FRACMUL(alpha, A);
149
150 /* possible numerical ranges are in comments by each coef */
151 b0 = one + alphaA; /* [1 .. 5] */
152 b1 = a1 = -2*(cs >> 3); /* [-2 .. 2] */
153 b2 = one - alphaA; /* [-3 .. 1] */
154 a0 = one + alphadivA; /* [1 .. 5] */
155 a2 = one - alphadivA; /* [-3 .. 1] */
156
157 /* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */
158 int32_t *c = f->coefs;
159 const long rcp_a0 = fp_div(1, a0, 59); /* s0.31 */
160 *c++ = FRACMUL(b0, rcp_a0); /* [0.25 .. 4] */
161 *c++ = FRACMUL(b1, rcp_a0); /* [-2 .. 2] */
162 *c++ = FRACMUL(b2, rcp_a0); /* [-2.4 .. 1] */
163 *c++ = FRACMUL(-a1, rcp_a0); /* [-2 .. 2] */
164 *c = FRACMUL(-a2, rcp_a0); /* [-0.6 .. 1] */
165
166 f->shift = FILTER_PEAK_SHIFT;
167}
168
169/**
170 * Calculate coefficients for lowshelf filter. Parameters are as for
171 * filter_pk_coefs, but the coefficient format is s5.26 fixed point.
172 */
173void filter_ls_coefs(unsigned long cutoff, unsigned long Q, long db,
174 struct dsp_filter *f)
175{
176 long cs;
177 const long one = 1 << 25; /* s6.25 */
178 const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */
179 const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */
180 const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
181 const long ap1 = (A >> 4) + one;
182 const long am1 = (A >> 4) - one;
183 const long ap1_cs = FRACMUL(ap1, cs);
184 const long am1_cs = FRACMUL(am1, cs);
185 const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha);
186 int32_t a0, a1, a2; /* these are all s6.25 format */
187 int32_t b0, b1, b2;
188
189 /* [0.1 .. 40] */
190 b0 = FRACMUL_SHL(A, ap1 - am1_cs + twosqrtalpha, 2);
191 /* [-16 .. 63.4] */
192 b1 = FRACMUL_SHL(A, am1 - ap1_cs, 3);
193 /* [0 .. 31.7] */
194 b2 = FRACMUL_SHL(A, ap1 - am1_cs - twosqrtalpha, 2);
195 /* [0.5 .. 10] */
196 a0 = ap1 + am1_cs + twosqrtalpha;
197 /* [-16 .. 4] */
198 a1 = -2*(am1 + ap1_cs);
199 /* [0 .. 8] */
200 a2 = ap1 + am1_cs - twosqrtalpha;
201
202 /* [0.1 .. 1.99] */
203 int32_t *c = f->coefs;
204 const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */
205 *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0.06 .. 15.9] */
206 *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-2 .. 31.7] */
207 *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 15.9] */
208 *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */
209 *c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */
210
211 f->shift = FILTER_SHELF_SHIFT;
212}
213
214/**
215 * Calculate coefficients for highshelf filter. Parameters are as for
216 * filter_pk_coefs, but the coefficient format is s5.26 fixed point.
217 */
218void filter_hs_coefs(unsigned long cutoff, unsigned long Q, long db,
219 struct dsp_filter *f)
220{
221 long cs;
222 const long one = 1 << 25; /* s6.25 */
223 const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */
224 const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */
225 const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
226 const long ap1 = (A >> 4) + one;
227 const long am1 = (A >> 4) - one;
228 const long ap1_cs = FRACMUL(ap1, cs);
229 const long am1_cs = FRACMUL(am1, cs);
230 const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha);
231 int32_t a0, a1, a2; /* these are all s6.25 format */
232 int32_t b0, b1, b2;
233
234 /* [0.1 .. 40] */
235 b0 = FRACMUL_SHL(A, ap1 + am1_cs + twosqrtalpha, 2);
236 /* [-63.5 .. 16] */
237 b1 = -FRACMUL_SHL(A, am1 + ap1_cs, 3);
238 /* [0 .. 32] */
239 b2 = FRACMUL_SHL(A, ap1 + am1_cs - twosqrtalpha, 2);
240 /* [0.5 .. 10] */
241 a0 = ap1 - am1_cs + twosqrtalpha;
242 /* [-4 .. 16] */
243 a1 = 2*(am1 - ap1_cs);
244 /* [0 .. 8] */
245 a2 = ap1 - am1_cs - twosqrtalpha;
246
247 /* [0.1 .. 1.99] */
248 int32_t *c = f->coefs;
249 const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */
250 *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0 .. 16] */
251 *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-31.7 .. 2] */
252 *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 16] */
253 *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */
254 *c = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */
255
256 f->shift = FILTER_SHELF_SHIFT;
257}
258
259/**
260 * Copy filter definition without destroying dst's history
261 */
262void filter_copy(struct dsp_filter *dst, const struct dsp_filter *src)
263{
264 memcpy(dst->coefs, src->coefs, sizeof (src->coefs));
265 dst->shift = src->shift;
266}
267
268/**
269 * Clear filter sample history
270 */
271void filter_flush(struct dsp_filter *f)
272{
273 memset(f->history, 0, sizeof (f->history));
274}
275
276/**
277 * We realise the filters as a second order direct form 1 structure. Direct
278 * form 1 was chosen because of better numerical properties for fixed point
279 * implementations.
280 */
281#if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM))
282void filter_process(struct dsp_filter *f, int32_t * const buf[], int count,
283 unsigned int channels)
284{
285 /* Direct form 1 filtering code.
286 y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
287 where y[] is output and x[] is input.
288 */
289 unsigned int shift = f->shift;
290
291 for (unsigned int c = 0; c < channels; c++) {
292 for (int i = 0; i < count; i++) {
293 long long acc = (long long) buf[c][i] * f->coefs[0];
294 acc += (long long) f->history[c][0] * f->coefs[1];
295 acc += (long long) f->history[c][1] * f->coefs[2];
296 acc += (long long) f->history[c][2] * f->coefs[3];
297 acc += (long long) f->history[c][3] * f->coefs[4];
298 f->history[c][1] = f->history[c][0];
299 f->history[c][0] = buf[c][i];
300 f->history[c][3] = f->history[c][2];
301 buf[c][i] = (acc << shift) >> 32;
302 f->history[c][2] = buf[c][i];
303 }
304 }
305}
306#endif /* CPU */
diff --git a/lib/rbcodec/dsp/dsp_filter.h b/lib/rbcodec/dsp/dsp_filter.h
new file mode 100644
index 0000000000..af6e20ce86
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_filter.h
@@ -0,0 +1,57 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006-2007 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef DSP_FILTER_H
22#define DSP_FILTER_H
23
24/** Basic filter implementations which may be used independently **/
25
26/* Used by: EQ, tone controls and crossfeed */
27
28/* These depend on the fixed point formats used by the different filter types
29 and need to be changed when they change.
30 */
31struct dsp_filter
32{
33 int32_t coefs[5]; /* 00h: Order is b0, b1, b2, a1, a2 */
34 int32_t history[2][4]; /* 14h: Order is x-1, x-2, y-1, y-2, per channel */
35 uint8_t shift; /* 34h: Final shift after computation */
36 /* 38h */
37};
38
39void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c);
40#ifdef HAVE_SW_TONE_CONTROLS
41void filter_bishelf_coefs(unsigned long cutoff_low,
42 unsigned long cutoff_high,
43 long A_low, long A_high, long A,
44 struct dsp_filter *f);
45#endif /* HAVE_SW_TONE_CONTROLS */
46void filter_pk_coefs(unsigned long cutoff, unsigned long Q, long db,
47 struct dsp_filter *f);
48void filter_ls_coefs(unsigned long cutoff, unsigned long Q, long db,
49 struct dsp_filter *f);
50void filter_hs_coefs(unsigned long cutoff, unsigned long Q, long db,
51 struct dsp_filter *f);
52void filter_copy(struct dsp_filter *dst, const struct dsp_filter *src);
53void filter_flush(struct dsp_filter *f);
54void filter_process(struct dsp_filter *f, int32_t * const buf[], int count,
55 unsigned int channels);
56
57#endif /* DSP_FILTER_H */
diff --git a/lib/rbcodec/dsp/dsp_misc.c b/lib/rbcodec/dsp/dsp_misc.c
new file mode 100644
index 0000000000..7b4589151c
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_misc.c
@@ -0,0 +1,238 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Miika Pekkarinen
11 * Copyright (C) 2005 Magnus Holmgren
12 * Copyright (C) 2007 Thom Johansen
13 * Copyright (C) 2012 Michael Sevakis
14 *
15 * This program is free software; you can redistribute it and/or
16 * modify it under the terms of the GNU General Public License
17 * as published by the Free Software Foundation; either version 2
18 * of the License, or (at your option) any later version.
19 *
20 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
21 * KIND, either express or implied.
22 *
23 ****************************************************************************/
24#include "config.h"
25#include "system.h"
26#include "dsp.h"
27#include "dsp_sample_io.h"
28#include "replaygain.h"
29#include "sound.h"
30#include "settings.h"
31#include "fixedpoint.h"
32#include <string.h>
33#include "dsp_proc_entry.h"
34
35/** Firmware callback interface **/
36
37/* Hook back from firmware/ part of audio, which can't/shouldn't call apps/
38 * code directly. */
39int dsp_callback(int msg, intptr_t param)
40{
41 switch (msg)
42 {
43#ifdef HAVE_SW_TONE_CONTROLS
44 case DSP_CALLBACK_SET_PRESCALE:
45 tone_set_prescale(param);
46 break;
47 case DSP_CALLBACK_SET_BASS:
48 tone_set_bass(param);
49 break;
50 case DSP_CALLBACK_SET_TREBLE:
51 tone_set_treble(param);
52 break;
53 /* FIXME: This must be done by bottom-level PCM driver so it works with
54 all PCM, not here and not in mixer. I won't fully support it
55 here with all streams. -- jethead71 */
56#ifdef HAVE_SW_VOLUME_CONTROL
57 case DSP_CALLBACK_SET_SW_VOLUME:
58 if (global_settings.volume < SW_VOLUME_MAX ||
59 global_settings.volume > SW_VOLUME_MIN)
60 {
61 int vol_gain = get_replaygain_int(global_settings.volume * 100);
62 pga_set_gain(PGA_VOLUME, vol_gain);
63 }
64 break;
65#endif /* HAVE_SW_VOLUME_CONTROL */
66#endif /* HAVE_SW_TONE_CONTROLS */
67 case DSP_CALLBACK_SET_CHANNEL_CONFIG:
68 channel_mode_set_config(param);
69 break;
70 case DSP_CALLBACK_SET_STEREO_WIDTH:
71 channel_mode_custom_set_width(param);
72 break;
73 default:
74 break;
75 }
76
77 return 0;
78}
79
80/** Replaygain settings **/
81static struct dsp_replay_gains current_rpgains;
82
83static void dsp_replaygain_update(const struct dsp_replay_gains *gains)
84{
85 if (gains == NULL)
86 {
87 /* Use defaults */
88 memset(&current_rpgains, 0, sizeof (current_rpgains));
89 gains = &current_rpgains;
90 }
91 else
92 {
93 current_rpgains = *gains; /* Stash settings */
94 }
95
96 int32_t gain = PGA_UNITY;
97
98 if (global_settings.replaygain_type != REPLAYGAIN_OFF ||
99 global_settings.replaygain_noclip)
100 {
101 bool track_mode =
102 get_replaygain_mode(gains->track_gain != 0,
103 gains->album_gain != 0) == REPLAYGAIN_TRACK;
104
105 int32_t peak = (track_mode || gains->album_peak == 0) ?
106 gains->track_peak : gains->album_peak;
107
108 if (global_settings.replaygain_type != REPLAYGAIN_OFF)
109 {
110 gain = (track_mode || gains->album_gain == 0) ?
111 gains->track_gain : gains->album_gain;
112
113 if (global_settings.replaygain_preamp)
114 {
115 int32_t preamp = get_replaygain_int(
116 global_settings.replaygain_preamp * 10);
117
118 gain = fp_mul(gain, preamp, 24);
119 }
120 }
121
122 if (gain == 0)
123 {
124 /* So that noclip can work even with no gain information. */
125 gain = PGA_UNITY;
126 }
127
128 if (global_settings.replaygain_noclip && peak != 0 &&
129 fp_mul(gain, peak, 24) >= PGA_UNITY)
130 {
131 gain = fp_div(PGA_UNITY, peak, 24);
132 }
133 }
134
135 pga_set_gain(PGA_REPLAYGAIN, gain);
136 pga_enable_gain(PGA_REPLAYGAIN, gain != PGA_UNITY);
137}
138
139int get_replaygain_mode(bool have_track_gain, bool have_album_gain)
140{
141 bool track = false;
142
143 switch (global_settings.replaygain_type)
144 {
145 case REPLAYGAIN_TRACK:
146 track = true;
147 break;
148
149 case REPLAYGAIN_SHUFFLE:
150 track = global_settings.playlist_shuffle;
151 break;
152 }
153
154 return (!track && have_album_gain) ?
155 REPLAYGAIN_ALBUM : (have_track_gain ? REPLAYGAIN_TRACK : -1);
156}
157
158void dsp_set_replaygain(void)
159{
160 dsp_replaygain_update(&current_rpgains);
161}
162
163
164/** Pitch Settings **/
165
166#ifdef HAVE_PITCHSCREEN
167static int32_t pitch_ratio = PITCH_SPEED_100;
168
169static void dsp_pitch_update(struct dsp_config *dsp)
170{
171 /* Account for playback speed adjustment when setting dsp->frequency
172 if we're called from the main audio thread. Voice playback thread
173 does not support this feature. */
174 struct sample_io_data *data = (void *)dsp;
175 data->format.frequency =
176 (int64_t)pitch_ratio * data->format.codec_frequency / PITCH_SPEED_100;
177}
178
179int32_t sound_get_pitch(void)
180{
181 return pitch_ratio;
182}
183
184void sound_set_pitch(int32_t percent)
185{
186 pitch_ratio = percent > 0 ? percent : PITCH_SPEED_100;
187 struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
188 struct sample_io_data *data = (void *)dsp;
189 dsp_configure(dsp, DSP_SWITCH_FREQUENCY, data->format.codec_frequency);
190}
191#endif /* HAVE_PITCHSCREEN */
192
193/* This is a null-processing stage that monitors as an enabled stage but never
194 * becomes active in processing samples. It only hooks messages. */
195
196/* DSP message hook */
197static intptr_t misc_handler_configure(struct dsp_proc_entry *this,
198 struct dsp_config *dsp,
199 unsigned setting,
200 intptr_t value)
201{
202 switch (setting)
203 {
204 case DSP_INIT:
205 /* Enable us for the audio DSP at startup */
206 if (value == CODEC_IDX_AUDIO)
207 dsp_proc_enable(dsp, DSP_PROC_MISC_HANDLER, true);
208 break;
209
210 case DSP_PROC_CLOSE:
211 /* This stage should be enabled at all times */
212 DEBUGF("DSP_PROC_MISC_HANDLER - Error: Closing!\n");
213 break;
214
215 case DSP_RESET:
216#ifdef HAVE_PITCHSCREEN
217 dsp_pitch_update(dsp);
218#endif
219 value = (intptr_t)NULL; /* Default gains */
220 case REPLAYGAIN_SET_GAINS:
221 dsp_replaygain_update((void *)value);
222 break;
223
224#ifdef HAVE_PITCHSCREEN
225 case DSP_SET_FREQUENCY:
226 dsp_pitch_update(dsp);
227 break;
228#endif
229 }
230
231 return 1;
232 (void)this;
233}
234
235/* Database entry */
236DSP_PROC_DB_ENTRY(
237 MISC_HANDLER,
238 misc_handler_configure);
diff --git a/lib/rbcodec/dsp/dsp_misc.h b/lib/rbcodec/dsp/dsp_misc.h
new file mode 100644
index 0000000000..74587cbb0e
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_misc.h
@@ -0,0 +1,50 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Miika Pekkarinen
11 * Copyright (C) 2005 Magnus Holmgren
12 * Copyright (C) 2007 Thom Johansen
13 *
14 * This program is free software; you can redistribute it and/or
15 * modify it under the terms of the GNU General Public License
16 * as published by the Free Software Foundation; either version 2
17 * of the License, or (at your option) any later version.
18 *
19 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
20 * KIND, either express or implied.
21 *
22 ****************************************************************************/
23#ifndef DSP_MISC_H
24#define DSP_MISC_H
25
26/* Set the tri-pdf dithered output */
27void dsp_dither_enable(bool enable); /* in dsp_sample_output.c */
28
29/* Structure used with REPLAYGAIN_SET_GAINS message */
30#define REPLAYGAIN_SET_GAINS (DSP_PROC_SETTING+DSP_PROC_MISC_HANDLER)
31struct dsp_replay_gains
32{
33 long track_gain;
34 long album_gain;
35 long track_peak;
36 long album_peak;
37};
38
39int get_replaygain_mode(bool have_track_gain, bool have_album_gain);
40void dsp_set_replaygain(void);
41
42#ifdef HAVE_PITCHSCREEN
43void sound_set_pitch(int32_t ratio);
44int32_t sound_get_pitch(void);
45#endif /* HAVE_PITCHSCREEN */
46
47/* Callback for firmware layers to interface */
48int dsp_callback(int msg, intptr_t param);
49
50#endif /* DSP_MISC_H */
diff --git a/lib/rbcodec/dsp/dsp_proc_database.h b/lib/rbcodec/dsp/dsp_proc_database.h
new file mode 100644
index 0000000000..55f10e684b
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_proc_database.h
@@ -0,0 +1,57 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2012 Michael Sevakis
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21
22/****************************************************************************
23 * -_-~-_-~-_-~-_-~-_-~-_- Main database of effects _-~-_-~-_-~-_-~-_-~-_-~-
24 *
25 * Order is not particularly relevant and has no intended correlation with
26 * IDs.
27 *
28 * Notable exceptions in ordering:
29 * * Sample input: which is first in line and has special responsibilities
30 * (not an effect per se).
31 * * Anything that depends on the native sample rate must go after the
32 * resampling stage.
33 * * Some bizarre dependency I didn't think of but you decided to implement.
34 * * Sample output: Naturally, this takes the final result and converts it
35 * to the target PCM format (not an effect per se).
36 */
37DSP_PROC_DB_START
38 DSP_PROC_DB_ITEM(MISC_HANDLER) /* misc stuff (null stage) */
39 DSP_PROC_DB_ITEM(PGA) /* pre-gain amp */
40#ifdef HAVE_PITCHSCREEN
41 DSP_PROC_DB_ITEM(TIMESTRETCH) /* time-stretching */
42#endif
43 DSP_PROC_DB_ITEM(RESAMPLE) /* resampler providing NATIVE_FREQUENCY */
44 DSP_PROC_DB_ITEM(CROSSFEED) /* stereo crossfeed */
45 DSP_PROC_DB_ITEM(EQUALIZER) /* n-band equalizer */
46#ifdef HAVE_SW_TONE_CONTROLS
47 DSP_PROC_DB_ITEM(TONE_CONTROLS) /* bass and treble */
48#endif
49 DSP_PROC_DB_ITEM(CHANNEL_MODE) /* channel modes */
50 DSP_PROC_DB_ITEM(COMPRESSOR) /* dynamic-range compressor */
51DSP_PROC_DB_STOP
52
53/* This file is included multiple times with different macro definitions so
54 clean up the current ones */
55#undef DSP_PROC_DB_START
56#undef DSP_PROC_DB_ITEM
57#undef DSP_PROC_DB_STOP
diff --git a/lib/rbcodec/dsp/dsp_proc_entry.h b/lib/rbcodec/dsp/dsp_proc_entry.h
new file mode 100644
index 0000000000..8bdfe5e0c9
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_proc_entry.h
@@ -0,0 +1,153 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2012 Michael Sevakis
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef DSP_PROC_ENTRY_H
22#define DSP_PROC_ENTRY_H
23
24#if 0 /* Set to '1' to enable local debug messages */
25#include <debug.h>
26#else
27#undef DEBUGF
28#define DEBUGF(...)
29#endif
30
31/* Macros to generate the right stuff */
32#ifdef DSP_PROC_DB_CREATE
33struct dsp_proc_db_entry;
34
35#define DSP_PROC_DB_START
36#define DSP_PROC_DB_ITEM(name) \
37 extern const struct dsp_proc_db_entry name##_proc_db_entry;
38#define DSP_PROC_DB_STOP
39
40/* Create database as externs to be able to build array */
41#include "dsp_proc_database.h"
42
43#define DSP_PROC_DB_START \
44 static struct dsp_proc_db_entry const * const dsp_proc_database[] = {
45
46#define DSP_PROC_DB_ITEM(name) \
47 &name##_proc_db_entry,
48
49#define DSP_PROC_DB_STOP };
50
51/* Create database as array */
52#include "dsp_proc_database.h"
53
54/* Number of effects in database - all available in audio DSP */
55#define DSP_NUM_PROC_STAGES ARRAYLEN(dsp_proc_database)
56
57/* Number of possible effects for voice DSP */
58#ifdef HAVE_SW_TONE_CONTROLS
59#define DSP_VOICE_NUM_PROC_STAGES 2 /* resample, tone */
60#else
61#define DSP_VOICE_NUM_PROC_STAGES 1 /* resample */
62#endif
63
64#else /* !DSP_PROC_DB_CREATE */
65
66#ifdef DEBUG
67#define DSP_PROC_DB_ENTRY(_name, _configure) \
68 const struct dsp_proc_db_entry _name##_proc_db_entry = \
69 { .id = DSP_PROC_##_name, .configure = _configure, \
70 .name = #_name };
71#else /* !DEBUG */
72#define DSP_PROC_DB_ENTRY(_name, _configure) \
73 const struct dsp_proc_db_entry _name##_proc_db_entry = \
74 { .id = DSP_PROC_##_name, .configure = _configure };
75#endif /* DEBUG */
76
77#endif /* DSP_PROC_DB_CREATE */
78
79#define DSP_PROC_DB_START \
80 enum dsp_proc_ids \
81 { \
82 ___DSP_PROC_ID_FIRST = -1,
83
84#define DSP_PROC_DB_ITEM(name) \
85 DSP_PROC_##name,
86
87#define DSP_PROC_DB_STOP };
88
89/* Create database as enums for use as ids */
90#include "dsp_proc_database.h"
91
92struct dsp_proc_entry;
93enum dsp_proc_ids;
94
95/* DSP sample transform function prototype */
96typedef void (*dsp_proc_fn_type)(struct dsp_proc_entry *this,
97 struct dsp_buffer **buf);
98
99/**
100 * dsp_proc_entry
101 * The structure allocated to every stage when enabled.
102 *
103 * default settings:
104 * .data = 0
105 * .ip_mask = BIT_N(dsp_proc_db_entry.id)
106 * .process[0] = dsp_process_null
107 * .process[1] = dsp_format_change_process
108 *
109 * DSP_PROC_INIT handler just has to change what it needs to change. It may
110 * also be modified at any time to implement the stage's demands.
111 */
112struct dsp_proc_entry
113{
114 intptr_t data; /* 00h: any value, at beginning for easy asm use */
115 uint32_t ip_mask; /* In-place id bit (0 or id bit flag if in-place) */
116 dsp_proc_fn_type process[2]; /* Processing normal/format changes */
117};
118
119/* DSP transform configure function prototype */
120typedef intptr_t (*dsp_proc_config_fn_type)(struct dsp_proc_entry *this,
121 struct dsp_config *dsp,
122 unsigned int setting,
123 intptr_t value);
124
125/* Enable/disable a processing stage - not to be called during processing
126 * by processing code! */
127void dsp_proc_enable(struct dsp_config *dsp, enum dsp_proc_ids id,
128 bool enable);
129/* Activate/deactivate processing stage, doesn't affect enabled status
130 * thus will not enable anything -
131 * may be called during processing to activate/deactivate for format
132 * changes */
133void dsp_proc_activate(struct dsp_config *dsp, enum dsp_proc_ids id,
134 bool activate);
135
136/* Is the specified stage active on the DSP? */
137bool dsp_proc_active(struct dsp_config *dsp, enum dsp_proc_ids id);
138
139/* Call this->process[fmt] according to the rules
140 * pass (unsigned)-1 to call function 0 with no restriction */
141bool dsp_proc_call(struct dsp_proc_entry *this, struct dsp_buffer **buf_p,
142 unsigned int fmt);
143
144struct dsp_proc_db_entry
145{
146 enum dsp_proc_ids id; /* id of this stage */
147 dsp_proc_config_fn_type configure; /* dsp_configure hook */
148#ifdef DEBUG
149 const char *name;
150#endif
151};
152
153#endif /* DSP_PROC_ENTRY_H */
diff --git a/lib/rbcodec/dsp/dsp_proc_settings.h b/lib/rbcodec/dsp/dsp_proc_settings.h
new file mode 100644
index 0000000000..769532085e
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_proc_settings.h
@@ -0,0 +1,40 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2012 Michael Sevakis
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef DSP_PROC_SETTINGS_H
22#define DSP_PROC_SETTINGS_H
23
24struct dsp_config;
25
26/* Collect all headers together */
27#include "channel_mode.h"
28#include "compressor.h"
29#include "crossfeed.h"
30#include "dsp_misc.h"
31#include "eq.h"
32#include "pga.h"
33#ifdef HAVE_PITCHSCREEN
34#include "tdspeed.h"
35#endif
36#ifdef HAVE_SW_TONE_CONTROLS
37#include "tone_controls.h"
38#endif
39
40#endif /* DSP_PROC_SETTINGS_H */ \ No newline at end of file
diff --git a/lib/rbcodec/dsp/dsp_sample_input.c b/lib/rbcodec/dsp/dsp_sample_input.c
new file mode 100644
index 0000000000..84127e1f96
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_sample_input.c
@@ -0,0 +1,334 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Miika Pekkarinen
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include "dsp_sample_io.h"
26
27#if 1
28#include <debug.h>
29#else
30#undef DEBUGF
31#define DEBUGF(...)
32#endif
33
34/* The internal format is 32-bit samples, non-interleaved, stereo. This
35 * format is similar to the raw output from several codecs, so no copying is
36 * needed for that case.
37 *
38 * Note that for mono, dst[0] equals dst[1], as there is no point in
39 * processing the same data twice nor should it be done when modifying
40 * samples in-place.
41 *
42 * When conversion is required:
43 * Updates source buffer to point past the samples "consumed" also consuming
44 * that portion of the input buffer and the destination is set to the buffer
45 * of samples for later stages to consume.
46 *
47 * Input operates similarly to how an out-of-place processing stage should
48 * behave.
49 */
50
51extern void dsp_sample_output_init(struct sample_io_data *this);
52extern void dsp_sample_output_flush(struct sample_io_data *this);
53
54/* convert count 16-bit mono to 32-bit mono */
55static void sample_input_mono16(struct sample_io_data *this,
56 struct dsp_buffer **buf_p)
57{
58 struct dsp_buffer *src = *buf_p;
59 struct dsp_buffer *dst = &this->sample_buf;
60
61 *buf_p = dst;
62
63 if (dst->remcount > 0)
64 return; /* data still remains */
65
66 int count = MIN(src->remcount, SAMPLE_BUF_COUNT);
67
68 dst->remcount = count;
69 dst->p32[0] = this->sample_buf_arr[0];
70 dst->p32[1] = this->sample_buf_arr[0];
71 dst->proc_mask = src->proc_mask;
72
73 if (count <= 0)
74 return; /* purged sample_buf */
75
76 const int16_t *s = src->pin[0];
77 int32_t *d = dst->p32[0];
78 const int scale = WORD_SHIFT;
79
80 dsp_advance_buffer_input(src, count, sizeof (int16_t));
81
82 do
83 {
84 *d++ = *s++ << scale;
85 }
86 while (--count > 0);
87}
88
89/* convert count 16-bit interleaved stereo to 32-bit noninterleaved */
90static void sample_input_i_stereo16(struct sample_io_data *this,
91 struct dsp_buffer **buf_p)
92{
93 struct dsp_buffer *src = *buf_p;
94 struct dsp_buffer *dst = &this->sample_buf;
95
96 *buf_p = dst;
97
98 if (dst->remcount > 0)
99 return; /* data still remains */
100
101 int count = MIN(src->remcount, SAMPLE_BUF_COUNT);
102
103 dst->remcount = count;
104 dst->p32[0] = this->sample_buf_arr[0];
105 dst->p32[1] = this->sample_buf_arr[1];
106 dst->proc_mask = src->proc_mask;
107
108 if (count <= 0)
109 return; /* purged sample_buf */
110
111 const int16_t *s = src->pin[0];
112 int32_t *dl = dst->p32[0];
113 int32_t *dr = dst->p32[1];
114 const int scale = WORD_SHIFT;
115
116 dsp_advance_buffer_input(src, count, 2*sizeof (int16_t));
117
118 do
119 {
120 *dl++ = *s++ << scale;
121 *dr++ = *s++ << scale;
122 }
123 while (--count > 0);
124}
125
126/* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */
127static void sample_input_ni_stereo16(struct sample_io_data *this,
128 struct dsp_buffer **buf_p)
129{
130 struct dsp_buffer *src = *buf_p;
131 struct dsp_buffer *dst = &this->sample_buf;
132
133 *buf_p = dst;
134
135 if (dst->remcount > 0)
136 return; /* data still remains */
137
138 int count = MIN(src->remcount, SAMPLE_BUF_COUNT);
139
140 dst->remcount = count;
141 dst->p32[0] = this->sample_buf_arr[0];
142 dst->p32[1] = this->sample_buf_arr[1];
143 dst->proc_mask = src->proc_mask;
144
145 if (count <= 0)
146 return; /* purged sample_buf */
147
148 const int16_t *sl = src->pin[0];
149 const int16_t *sr = src->pin[1];
150 int32_t *dl = dst->p32[0];
151 int32_t *dr = dst->p32[1];
152 const int scale = WORD_SHIFT;
153
154 dsp_advance_buffer_input(src, count, sizeof (int16_t));
155
156 do
157 {
158 *dl++ = *sl++ << scale;
159 *dr++ = *sr++ << scale;
160 }
161 while (--count > 0);
162}
163
164/* convert count 32-bit mono to 32-bit mono */
165static void sample_input_mono32(struct sample_io_data *this,
166 struct dsp_buffer **buf_p)
167{
168 struct dsp_buffer *dst = &this->sample_buf;
169
170 if (dst->remcount > 0)
171 {
172 *buf_p = dst;
173 return; /* data still remains */
174 }
175 /* else no buffer switch */
176
177 struct dsp_buffer *src = *buf_p;
178 src->p32[1] = src->p32[0];
179}
180
181
182/* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
183static void sample_input_i_stereo32(struct sample_io_data *this,
184 struct dsp_buffer **buf_p)
185{
186 struct dsp_buffer *src = *buf_p;
187 struct dsp_buffer *dst = &this->sample_buf;
188
189 *buf_p = dst;
190
191 if (dst->remcount > 0)
192 return; /* data still remains */
193
194 int count = MIN(src->remcount, SAMPLE_BUF_COUNT);
195
196 dst->remcount = count;
197 dst->p32[0] = this->sample_buf_arr[0];
198 dst->p32[1] = this->sample_buf_arr[1];
199 dst->proc_mask = src->proc_mask;
200
201 if (count <= 0)
202 return; /* purged sample_buf */
203
204 const int32_t *s = src->pin[0];
205 int32_t *dl = dst->p32[0];
206 int32_t *dr = dst->p32[1];
207
208 dsp_advance_buffer_input(src, count, 2*sizeof (int32_t));
209
210 do
211 {
212 *dl++ = *s++;
213 *dr++ = *s++;
214 }
215 while (--count > 0);
216}
217
218/* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
219static void sample_input_ni_stereo32(struct sample_io_data *this,
220 struct dsp_buffer **buf_p)
221{
222 struct dsp_buffer *dst = &this->sample_buf;
223
224 if (dst->remcount > 0)
225 *buf_p = dst; /* data still remains */
226 /* else no buffer switch */
227}
228
229/* set the to-native sample conversion function based on dsp sample
230 * parameters */
231static void dsp_sample_input_format_change(struct sample_io_data *this,
232 struct dsp_buffer **buf_p)
233{
234 static const sample_input_fn_type fns[STEREO_NUM_MODES][2] =
235 {
236 [STEREO_INTERLEAVED] =
237 { sample_input_i_stereo16,
238 sample_input_i_stereo32 },
239 [STEREO_NONINTERLEAVED] =
240 { sample_input_ni_stereo16,
241 sample_input_ni_stereo32 },
242 [STEREO_MONO] =
243 { sample_input_mono16,
244 sample_input_mono32 },
245 };
246
247 struct dsp_buffer *src = *buf_p;
248 struct dsp_buffer *dst = &this->sample_buf;
249
250 /* Ack configured format change */
251 format_change_ack(&this->format);
252
253 if (dst->remcount > 0)
254 {
255 *buf_p = dst;
256 return; /* data still remains */
257 }
258
259 DSP_PRINT_FORMAT(DSP Input, -1, src->format);
260
261 /* new format - remember it and pass it along */
262 dst->format = src->format;
263 this->input_samples[0] = fns[this->stereo_mode]
264 [this->sample_depth > NATIVE_DEPTH ? 1 : 0];
265
266 this->input_samples[0](this, buf_p);
267
268 if (*buf_p == dst) /* buffer switch? */
269 format_change_ack(&src->format);
270}
271
272static void dsp_sample_input_init(struct sample_io_data *this)
273{
274 this->input_samples[0] = sample_input_ni_stereo32;
275 this->input_samples[1] = dsp_sample_input_format_change;
276}
277
278/* discard the sample buffer */
279static void dsp_sample_input_flush(struct sample_io_data *this)
280{
281 this->sample_buf.remcount = 0;
282}
283
284void dsp_sample_io_configure(struct sample_io_data *this,
285 unsigned int setting,
286 intptr_t value)
287{
288 switch (setting)
289 {
290 case DSP_INIT:
291 dsp_sample_input_init(this);
292 dsp_sample_output_init(this);
293 break;
294
295 case DSP_RESET:
296 /* Reset all sample descriptions to default */
297 format_change_set(&this->format);
298 this->format.num_channels = 2;
299 this->format.frac_bits = WORD_FRACBITS;
300 this->format.output_scale = WORD_FRACBITS + 1 - NATIVE_DEPTH;
301 this->format.frequency = NATIVE_FREQUENCY;
302 this->format.codec_frequency = NATIVE_FREQUENCY;
303 this->sample_depth = NATIVE_DEPTH;
304 this->stereo_mode = STEREO_NONINTERLEAVED;
305 break;
306
307 case DSP_SET_FREQUENCY:
308 value = value > 0 ? value : NATIVE_FREQUENCY;
309 format_change_set(&this->format);
310 this->format.frequency = value;
311 this->format.codec_frequency = value;
312 break;
313
314 case DSP_SET_SAMPLE_DEPTH:
315 format_change_set(&this->format);
316 this->format.frac_bits =
317 value <= NATIVE_DEPTH ? WORD_FRACBITS : value;
318 this->format.output_scale =
319 this->format.frac_bits + 1 - NATIVE_DEPTH;
320 this->sample_depth = value;
321 break;
322
323 case DSP_SET_STEREO_MODE:
324 format_change_set(&this->format);
325 this->format.num_channels = value == STEREO_MONO ? 1 : 2;
326 this->stereo_mode = value;
327 break;
328
329 case DSP_FLUSH:
330 dsp_sample_input_flush(this);
331 dsp_sample_output_flush(this);
332 break;
333 }
334}
diff --git a/lib/rbcodec/dsp/dsp_sample_io.h b/lib/rbcodec/dsp/dsp_sample_io.h
new file mode 100644
index 0000000000..443038919d
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_sample_io.h
@@ -0,0 +1,62 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2012 Michael Sevakis
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef DSP_SAMPLE_IO_H
22#define DSP_SAMPLE_IO_H
23
24/* 16-bit samples are scaled based on these constants. The shift should be
25 * no more than 15.
26 */
27#define WORD_SHIFT 12
28#define WORD_FRACBITS 27
29#define NATIVE_DEPTH 16
30
31#define SAMPLE_BUF_COUNT 128 /* Per channel, per DSP */
32
33struct sample_io_data;
34
35/* DSP initial buffer input function call prototype */
36typedef void (*sample_input_fn_type)(struct sample_io_data *this,
37 struct dsp_buffer **buf_p);
38
39/* DSP final buffer output function call prototype */
40typedef void (*sample_output_fn_type)(struct sample_io_data *this,
41 struct dsp_buffer *src,
42 struct dsp_buffer *dst);
43
44/* This becomes part of the DSP aggregate */
45struct sample_io_data
46{
47 int outcount; /* 00h: Output count */
48 struct sample_format format; /* General format info */
49 int sample_depth; /* Codec-specified sample depth */
50 int stereo_mode; /* Codec-specified input format */
51 sample_input_fn_type input_samples[2]; /* input functions */
52 struct dsp_buffer sample_buf; /* Buffer descriptor for converted samples */
53 int32_t sample_buf_arr[2][SAMPLE_BUF_COUNT]; /* Internal format */
54 sample_output_fn_type output_samples[2]; /* Final output functions */
55};
56
57/* Sample IO watches the format setting from the codec */
58void dsp_sample_io_configure(struct sample_io_data *this,
59 unsigned int setting,
60 intptr_t value);
61
62#endif /* DSP_SAMPLE_IO_H */
diff --git a/lib/rbcodec/dsp/dsp_sample_output.c b/lib/rbcodec/dsp/dsp_sample_output.c
new file mode 100644
index 0000000000..47fde0440c
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_sample_output.c
@@ -0,0 +1,214 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Miika Pekkarinen
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include "dsp_sample_io.h"
26#include "dsp-util.h"
27#include <string.h>
28
29#if 0
30#include <debug.h>
31#else
32#undef DEBUGF
33#define DEBUGF(...)
34#endif
35
36/* May be implemented in here or externally.*/
37void sample_output_mono(struct sample_io_data *this,
38 struct dsp_buffer *src, struct dsp_buffer *dst);
39void sample_output_stereo(struct sample_io_data *this,
40 struct dsp_buffer *src, struct dsp_buffer *dst);
41void sample_output_dithered(struct sample_io_data *this,
42 struct dsp_buffer *src, struct dsp_buffer *dst);
43
44/** Sample output **/
45
46#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
47/* write mono internal format to output format */
48void sample_output_mono(struct sample_io_data *this,
49 struct dsp_buffer *src, struct dsp_buffer *dst)
50{
51 int count = this->outcount;
52 const int32_t *s0 = src->p32[0];
53 int16_t *d = dst->p16out;
54 int scale = src->format.output_scale;
55 int32_t dc_bias = 1L << (scale - 1);
56
57 do
58 {
59 int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale);
60 *d++ = lr;
61 *d++ = lr;
62 }
63 while (--count > 0);
64}
65
66/* write stereo internal format to output format */
67void sample_output_stereo(struct sample_io_data *this,
68 struct dsp_buffer *src, struct dsp_buffer *dst)
69{
70 int count = this->outcount;
71 const int32_t *s0 = src->p32[0];
72 const int32_t *s1 = src->p32[1];
73 int16_t *d = dst->p16out;
74 int scale = src->format.output_scale;
75 int32_t dc_bias = 1L << (scale - 1);
76
77 do
78 {
79 *d++ = clip_sample_16((*s0++ + dc_bias) >> scale);
80 *d++ = clip_sample_16((*s1++ + dc_bias) >> scale);
81 }
82 while (--count > 0);
83}
84#endif /* CPU */
85
86/**
87 * The "dither" code to convert the 24-bit samples produced by libmad was
88 * taken from the coolplayer project - coolplayer.sourceforge.net
89 *
90 * This function handles mono and stereo outputs.
91 */
92static struct dither_data
93{
94 struct dither_state
95 {
96 long error[3]; /* 00h: error term history */
97 long random; /* 0ch: last random value */
98 } state[2]; /* 0=left, 1=right */
99 bool enabled; /* 20h: dithered output enabled */
100 /* 24h */
101} dither_data IBSS_ATTR;
102
103void sample_output_dithered(struct sample_io_data *this,
104 struct dsp_buffer *src, struct dsp_buffer *dst)
105{
106 int count = this->outcount;
107 int channels = src->format.num_channels;
108 int scale = src->format.output_scale;
109 int32_t dc_bias = 1L << (scale - 1); /* 1/2 bit of significance */
110 int32_t mask = (1L << scale) - 1; /* Mask of bits quantized away */
111
112 for (int ch = 0; ch < channels; ch++)
113 {
114 struct dither_state *dither = &dither_data.state[ch];
115
116 const int32_t *s = src->p32[ch];
117 int16_t *d = &dst->p16out[ch];
118
119 for (int i = 0; i < count; i++, s++, d += 2)
120 {
121 /* Noise shape and bias (for correct rounding later) */
122 int32_t sample = *s;
123
124 sample += dither->error[0] - dither->error[1] + dither->error[2];
125 dither->error[2] = dither->error[1];
126 dither->error[1] = dither->error[0] / 2;
127
128 int32_t output = sample + dc_bias;
129
130 /* Dither, highpass triangle PDF */
131 int32_t random = dither->random*0x0019660dL + 0x3c6ef35fL;
132 output += (random & mask) - (dither->random & mask);
133 dither->random = random;
134
135 /* Quantize sample to output range */
136 output >>= scale;
137
138 /* Error feedback of quantization */
139 dither->error[0] = sample - (output << scale);
140
141 /* Clip and store */
142 *d = clip_sample_16(output);
143 }
144 }
145
146 if (channels > 1)
147 return;
148
149 /* Have to duplicate left samples into the right channel since
150 output is interleaved stereo */
151 int16_t *d = dst->p16out;
152
153 do
154 {
155 int16_t s = *d++;
156 *d++ = s;
157 }
158 while (--count > 0);
159}
160
161/* Initialize the output function for settings and format */
162static void dsp_sample_output_format_change(struct sample_io_data *this,
163 struct dsp_buffer *src,
164 struct dsp_buffer *dst)
165{
166 static const sample_output_fn_type fns[2][2] =
167 {
168 { sample_output_mono, /* DC-biased quantizing */
169 sample_output_stereo },
170 { sample_output_dithered, /* Tri-PDF dithering */
171 sample_output_dithered },
172 };
173
174 struct sample_format *format = &src->format;
175 bool dither = dsp_get_id((void *)this) == CODEC_IDX_AUDIO &&
176 dither_data.enabled;
177 int channels = format->num_channels;
178
179 DSP_PRINT_FORMAT(DSP Output, -1, *format);
180
181 this->output_samples[0] = fns[dither ? 1 : 0][channels - 1];
182 format_change_ack(format); /* always ack, we're last */
183
184 /* The real function mustn't be called with no data */
185 if (this->outcount > 0)
186 this->output_samples[0](this, src, dst);
187}
188
189void dsp_sample_output_init(struct sample_io_data *this)
190{
191 this->output_samples[0] = sample_output_stereo;
192 this->output_samples[1] = dsp_sample_output_format_change;
193}
194
195/* Flush the dither history */
196void dsp_sample_output_flush(struct sample_io_data *this)
197{
198 if (dsp_get_id((void *)this) == CODEC_IDX_AUDIO)
199 memset(dither_data.state, 0, sizeof (dither_data.state));
200}
201
202/** Output settings **/
203
204/* Set the tri-pdf dithered output */
205void dsp_dither_enable(bool enable)
206{
207 if (enable == dither_data.enabled)
208 return;
209
210 struct sample_io_data *data = (void *)dsp_get_config(CODEC_IDX_AUDIO);
211 dsp_sample_output_flush(data);
212 dither_data.enabled = enable;
213 data->output_samples[0] = dsp_sample_output_format_change;
214}
diff --git a/lib/rbcodec/dsp/eq.c b/lib/rbcodec/dsp/eq.c
index 122a46a4c5..4e7df9bf5a 100644
--- a/lib/rbcodec/dsp/eq.c
+++ b/lib/rbcodec/dsp/eq.c
@@ -7,7 +7,8 @@
7 * \/ \/ \/ \/ \/ 7 * \/ \/ \/ \/ \/
8 * $Id$ 8 * $Id$
9 * 9 *
10 * Copyright (C) 2006-2007 Thom Johansen 10 * Copyright (C) 2006-2007 Thom Johansen
11 * Copyright (C) 2012 Michael Sevakis
11 * 12 *
12 * This program is free software; you can redistribute it and/or 13 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License 14 * modify it under the terms of the GNU General Public License
@@ -18,251 +19,156 @@
18 * KIND, either express or implied. 19 * KIND, either express or implied.
19 * 20 *
20 ****************************************************************************/ 21 ****************************************************************************/
21
22#include <inttypes.h>
23#include "config.h" 22#include "config.h"
23#include "system.h"
24#include "fixedpoint.h" 24#include "fixedpoint.h"
25#include "fracmul.h" 25#include "fracmul.h"
26#include "eq.h" 26#include "dsp.h"
27#include "dsp_filter.h"
27#include "replaygain.h" 28#include "replaygain.h"
29#include <string.h>
30#include "dsp_proc_entry.h"
28 31
29/** 32/**
30 * Calculate first order shelving filter. Filter is not directly usable by the 33 * Current setup is one lowshelf filters three peaking filters and one
31 * eq_filter() function. 34 * highshelf filter. Varying the number of shelving filters make no sense,
32 * @param cutoff shelf midpoint frequency. See eq_pk_coefs for format. 35 * but adding peaking filters is possible. Check EQ_NUM_BANDS to have
33 * @param A decibel value multiplied by ten, describing gain/attenuation of 36 * 2 shelving filters and EQ_NUM_BANDS-2 peaking filters.
34 * shelf. Max value is 24 dB.
35 * @param low true for low-shelf filter, false for high-shelf filter.
36 * @param c pointer to coefficient storage. Coefficients are s4.27 format.
37 */ 37 */
38void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c) 38
39#if EQ_NUM_BANDS < 3
40/* No good. Expect at least 1 peaking and low/high shelving filters */
41#error Band count must be greater than or equal to 3
42#endif
43
44static struct eq_state
39{ 45{
40 long sin, cos; 46 uint32_t enabled; /* Mask of enabled bands */
41 int32_t b0, b1, a0, a1; /* s3.28 */ 47 uint8_t bands[EQ_NUM_BANDS+1]; /* Indexes of enabled bands */
42 const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */ 48 struct dsp_filter filters[EQ_NUM_BANDS]; /* Data for each filter */
49} eq_data IBSS_ATTR;
43 50
44 sin = fp_sincos(cutoff/2, &cos); 51/* Clear histories of all enabled bands */
45 if (low) { 52static void eq_flush(void)
46 const int32_t sin_div_g = fp_div(sin, g, 25); 53{
47 const int32_t sin_g = FRACMUL(sin, g); 54 if (eq_data.enabled == 0)
48 cos >>= 3; 55 return; /* Not initialized yet/no bands on */
49 b0 = sin_g + cos; /* 0.25 .. 4.10 */
50 b1 = sin_g - cos; /* -1 .. 3.98 */
51 a0 = sin_div_g + cos; /* 0.25 .. 4.10 */
52 a1 = sin_div_g - cos; /* -1 .. 3.98 */
53 } else {
54 const int32_t cos_div_g = fp_div(cos, g, 25);
55 const int32_t cos_g = FRACMUL(cos, g);
56 sin >>= 3;
57 b0 = sin + cos_g; /* 0.25 .. 4.10 */
58 b1 = sin - cos_g; /* -3.98 .. 1 */
59 a0 = sin + cos_div_g; /* 0.25 .. 4.10 */
60 a1 = sin - cos_div_g; /* -3.98 .. 1 */
61 }
62 56
63 const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */ 57 for (uint8_t *b = eq_data.bands; *b < EQ_NUM_BANDS; b++)
64 *c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */ 58 filter_flush(&eq_data.filters[*b]);
65 *c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */
66 *c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */
67} 59}
68 60
69#ifdef HAVE_SW_TONE_CONTROLS 61/** DSP interface **/
70/** 62
71 * Calculate second order section filter consisting of one low-shelf and one 63/* Set the precut gain value */
72 * high-shelf section. 64void dsp_set_eq_precut(int precut)
73 * @param cutoff_low low-shelf midpoint frequency. See eq_pk_coefs for format.
74 * @param cutoff_high high-shelf midpoint frequency.
75 * @param A_low decibel value multiplied by ten, describing gain/attenuation of
76 * low-shelf part. Max value is 24 dB.
77 * @param A_high decibel value multiplied by ten, describing gain/attenuation of
78 * high-shelf part. Max value is 24 dB.
79 * @param A decibel value multiplied by ten, describing additional overall gain.
80 * @param c pointer to coefficient storage. Coefficients are s4.27 format.
81 */
82void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high,
83 long A_low, long A_high, long A, int32_t *c)
84{ 65{
85 const long g = get_replaygain_int(A*10) << 7; /* 10^(db/20), s0.31 */ 66 pga_set_gain(PGA_EQ_PRECUT, get_replaygain_int(precut * -10));
86 int32_t c_ls[3], c_hs[3]; 67}
87 68
88 filter_shelf_coefs(cutoff_low, A_low, true, c_ls); 69/* Update the filter configuration for the band */
89 filter_shelf_coefs(cutoff_high, A_high, false, c_hs); 70void dsp_set_eq_coefs(int band, const struct eq_band_setting *setting)
90 c_ls[0] = FRACMUL(g, c_ls[0]); 71{
91 c_ls[1] = FRACMUL(g, c_ls[1]); 72 static void (* const coef_gen[EQ_NUM_BANDS])(unsigned long cutoff,
73 unsigned long Q, long db,
74 struct dsp_filter *f) =
75 {
76 [0] = filter_ls_coefs,
77 [1 ... EQ_NUM_BANDS-2] = filter_pk_coefs,
78 [EQ_NUM_BANDS-1] = filter_hs_coefs,
79 };
80
81 if (band < 0 || band >= EQ_NUM_BANDS)
82 return;
83
84 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
85 which it should be, since we're executed from the main thread. */
86
87 uint32_t mask = eq_data.enabled;
88 struct dsp_filter *filter = &eq_data.filters[band];
89
90 /* Assume a band is disabled if the gain is zero */
91 mask &= ~BIT_N(band);
92
93 if (setting->gain != 0)
94 {
95 mask |= BIT_N(band);
96
97 /* Convert user settings to format required by coef generator
98 functions */
99 coef_gen[band](0xffffffff / NATIVE_FREQUENCY * setting->cutoff,
100 setting->q ?: 1, setting->gain, filter);
101 }
92 102
93 /* now we cascade the two first order filters to one second order filter 103 if (mask == eq_data.enabled)
94 * which can be used by eq_filter(). these resulting coefficients have a 104 return; /* No change in band-enable state */
95 * really wide numerical range, so we use a fixed point format which will
96 * work for the selected cutoff frequencies (in dsp.c) only.
97 */
98 const int32_t b0 = c_ls[0], b1 = c_ls[1], b2 = c_hs[0], b3 = c_hs[1];
99 const int32_t a0 = c_ls[2], a1 = c_hs[2];
100 *c++ = FRACMUL_SHL(b0, b2, 4);
101 *c++ = FRACMUL_SHL(b0, b3, 4) + FRACMUL_SHL(b1, b2, 4);
102 *c++ = FRACMUL_SHL(b1, b3, 4);
103 *c++ = a0 + a1;
104 *c++ = -FRACMUL_SHL(a0, a1, 4);
105}
106#endif
107 105
108/* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson. 106 if (mask & BIT_N(band))
109 * Slightly faster calculation can be done by deriving forms which use tan() 107 filter_flush(filter); /* Coming online */
110 * instead of cos() and sin(), but the latter are far easier to use when doing
111 * fixed point math, and performance is not a big point in the calculation part.
112 * All the 'a' filter coefficients are negated so we can use only additions
113 * in the filtering equation.
114 */
115 108
116/** 109 eq_data.enabled = mask;
117 * Calculate second order section peaking filter coefficients.
118 * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and
119 * 0x80000000 represents the Nyquist frequency (samplerate/2).
120 * @param Q Q factor value multiplied by ten. Lower bound is artificially set
121 * at 0.5.
122 * @param db decibel value multiplied by ten, describing gain/attenuation at
123 * peak freq. Max value is 24 dB.
124 * @param c pointer to coefficient storage. Coefficients are s3.28 format.
125 */
126void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
127{
128 long cs;
129 const long one = 1 << 28; /* s3.28 */
130 const long A = get_replaygain_int(db*5) << 5; /* 10^(db/40), s2.29 */
131 const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
132 int32_t a0, a1, a2; /* these are all s3.28 format */
133 int32_t b0, b1, b2;
134 const long alphadivA = fp_div(alpha, A, 27);
135 const long alphaA = FRACMUL(alpha, A);
136 110
137 /* possible numerical ranges are in comments by each coef */ 111 /* Only be active if there are bands to process - if EQ is off, then
138 b0 = one + alphaA; /* [1 .. 5] */ 112 this call has no effect */
139 b1 = a1 = -2*(cs >> 3); /* [-2 .. 2] */ 113 struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
140 b2 = one - alphaA; /* [-3 .. 1] */ 114 dsp_proc_activate(dsp, DSP_PROC_EQUALIZER, mask != 0);
141 a0 = one + alphadivA; /* [1 .. 5] */ 115
142 a2 = one - alphadivA; /* [-3 .. 1] */ 116 /* Prepare list of enabled bands for efficient iteration */
117 for (band = 0; mask != 0; mask &= mask - 1, band++)
118 eq_data.bands[band] = (uint8_t)find_first_set_bit(mask);
143 119
144 /* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */ 120 eq_data.bands[band] = EQ_NUM_BANDS;
145 const long rcp_a0 = fp_div(1, a0, 59); /* s0.31 */
146 *c++ = FRACMUL(b0, rcp_a0); /* [0.25 .. 4] */
147 *c++ = FRACMUL(b1, rcp_a0); /* [-2 .. 2] */
148 *c++ = FRACMUL(b2, rcp_a0); /* [-2.4 .. 1] */
149 *c++ = FRACMUL(-a1, rcp_a0); /* [-2 .. 2] */
150 *c++ = FRACMUL(-a2, rcp_a0); /* [-0.6 .. 1] */
151} 121}
152 122
153/** 123/* Enable or disable the equalizer */
154 * Calculate coefficients for lowshelf filter. Parameters are as for 124void dsp_eq_enable(bool enable)
155 * eq_pk_coefs, but the coefficient format is s5.26 fixed point.
156 */
157void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
158{ 125{
159 long cs; 126 struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
160 const long one = 1 << 25; /* s6.25 */ 127 dsp_proc_enable(dsp, DSP_PROC_EQUALIZER, enable);
161 const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */
162 const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */
163 const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
164 const long ap1 = (A >> 4) + one;
165 const long am1 = (A >> 4) - one;
166 const long ap1_cs = FRACMUL(ap1, cs);
167 const long am1_cs = FRACMUL(am1, cs);
168 const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha);
169 int32_t a0, a1, a2; /* these are all s6.25 format */
170 int32_t b0, b1, b2;
171
172 /* [0.1 .. 40] */
173 b0 = FRACMUL_SHL(A, ap1 - am1_cs + twosqrtalpha, 2);
174 /* [-16 .. 63.4] */
175 b1 = FRACMUL_SHL(A, am1 - ap1_cs, 3);
176 /* [0 .. 31.7] */
177 b2 = FRACMUL_SHL(A, ap1 - am1_cs - twosqrtalpha, 2);
178 /* [0.5 .. 10] */
179 a0 = ap1 + am1_cs + twosqrtalpha;
180 /* [-16 .. 4] */
181 a1 = -2*(am1 + ap1_cs);
182 /* [0 .. 8] */
183 a2 = ap1 + am1_cs - twosqrtalpha;
184 128
185 /* [0.1 .. 1.99] */ 129 if (enable && eq_data.enabled != 0)
186 const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */ 130 dsp_proc_activate(dsp, DSP_PROC_EQUALIZER, true);
187 *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0.06 .. 15.9] */
188 *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-2 .. 31.7] */
189 *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 15.9] */
190 *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */
191 *c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */
192} 131}
193 132
194/** 133/* Apply EQ filters to those bands that have got it switched on. */
195 * Calculate coefficients for highshelf filter. Parameters are as for 134static void eq_process(struct dsp_proc_entry *this,
196 * eq_pk_coefs, but the coefficient format is s5.26 fixed point. 135 struct dsp_buffer **buf_p)
197 */
198void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
199{ 136{
200 long cs; 137 struct dsp_buffer *buf = *buf_p;
201 const long one = 1 << 25; /* s6.25 */ 138 int count = buf->remcount;
202 const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */ 139 unsigned int channels = buf->format.num_channels;
203 const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */
204 const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */
205 const long ap1 = (A >> 4) + one;
206 const long am1 = (A >> 4) - one;
207 const long ap1_cs = FRACMUL(ap1, cs);
208 const long am1_cs = FRACMUL(am1, cs);
209 const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha);
210 int32_t a0, a1, a2; /* these are all s6.25 format */
211 int32_t b0, b1, b2;
212 140
213 /* [0.1 .. 40] */ 141 for (uint8_t *b = eq_data.bands; *b < EQ_NUM_BANDS; b++)
214 b0 = FRACMUL_SHL(A, ap1 + am1_cs + twosqrtalpha, 2); 142 filter_process(&eq_data.filters[*b], buf->p32, count, channels);
215 /* [-63.5 .. 16] */
216 b1 = -FRACMUL_SHL(A, am1 + ap1_cs, 3);
217 /* [0 .. 32] */
218 b2 = FRACMUL_SHL(A, ap1 + am1_cs - twosqrtalpha, 2);
219 /* [0.5 .. 10] */
220 a0 = ap1 - am1_cs + twosqrtalpha;
221 /* [-4 .. 16] */
222 a1 = 2*(am1 - ap1_cs);
223 /* [0 .. 8] */
224 a2 = ap1 - am1_cs - twosqrtalpha;
225 143
226 /* [0.1 .. 1.99] */ 144 (void)this;
227 const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */
228 *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0 .. 16] */
229 *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-31.7 .. 2] */
230 *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 16] */
231 *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */
232 *c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */
233} 145}
234 146
235/* We realise the filters as a second order direct form 1 structure. Direct 147/* DSP message hook */
236 * form 1 was chosen because of better numerical properties for fixed point 148static intptr_t eq_configure(struct dsp_proc_entry *this,
237 * implementations. 149 struct dsp_config *dsp,
238 */ 150 unsigned int setting,
239 151 intptr_t value)
240#if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM))
241void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
242 unsigned channels, unsigned shift)
243{ 152{
244 unsigned c, i; 153 switch (setting)
245 long long acc; 154 {
246 155 case DSP_PROC_INIT:
247 /* Direct form 1 filtering code. 156 if (value != 0)
248 y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2], 157 break;
249 where y[] is output and x[] is input. 158 this->process[0] = eq_process;
250 */ 159 case DSP_PROC_CLOSE:
251 160 pga_enable_gain(PGA_EQ_PRECUT, setting == DSP_PROC_INIT);
252 for (c = 0; c < channels; c++) { 161 break;
253 for (i = 0; i < num; i++) { 162
254 acc = (long long) x[c][i] * f->coefs[0]; 163 case DSP_FLUSH:
255 acc += (long long) f->history[c][0] * f->coefs[1]; 164 eq_flush();
256 acc += (long long) f->history[c][1] * f->coefs[2]; 165 break;
257 acc += (long long) f->history[c][2] * f->coefs[3];
258 acc += (long long) f->history[c][3] * f->coefs[4];
259 f->history[c][1] = f->history[c][0];
260 f->history[c][0] = x[c][i];
261 f->history[c][3] = f->history[c][2];
262 x[c][i] = (acc << shift) >> 32;
263 f->history[c][2] = x[c][i];
264 }
265 } 166 }
167
168 return 1;
169 (void)dsp;
266} 170}
267#endif
268 171
172/* Database entry */
173DSP_PROC_DB_ENTRY(EQUALIZER,
174 eq_configure);
diff --git a/lib/rbcodec/dsp/eq.h b/lib/rbcodec/dsp/eq.h
index a44e9153ac..53097beb12 100644
--- a/lib/rbcodec/dsp/eq.h
+++ b/lib/rbcodec/dsp/eq.h
@@ -18,33 +18,25 @@
18 * KIND, either express or implied. 18 * KIND, either express or implied.
19 * 19 *
20 ****************************************************************************/ 20 ****************************************************************************/
21
22#ifndef _EQ_H 21#ifndef _EQ_H
23#define _EQ_H 22#define _EQ_H
24 23
25#include <inttypes.h> 24/* => support from 3 to 32 bands, inclusive
26#include <stdbool.h> 25 * Menus and screens must be updated to support changing this from 5
27 26 * without modifying other stuff (remove comment when this is no longer
28/* These depend on the fixed point formats used by the different filter types 27 * true :-) */
29 and need to be changed when they change. 28#define EQ_NUM_BANDS 5
30 */
31#define FILTER_BISHELF_SHIFT 5
32#define EQ_PEAK_SHIFT 4
33#define EQ_SHELF_SHIFT 6
34 29
35struct eqfilter { 30struct eq_band_setting
36 int32_t coefs[5]; /* Order is b0, b1, b2, a1, a2 */ 31{
37 int32_t history[2][4]; 32 int cutoff; /* Hz */
33 int q;
34 int gain; /* +/- dB */
38}; 35};
39 36
40void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c); 37/** DSP interface **/
41void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high, 38void dsp_set_eq_precut(int precut);
42 long A_low, long A_high, long A, int32_t *c); 39void dsp_set_eq_coefs(int band, const struct eq_band_setting *setting);
43void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c); 40void dsp_eq_enable(bool enable);
44void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
45void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
46void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
47 unsigned channels, unsigned shift);
48
49#endif
50 41
42#endif /* _EQ_H */
diff --git a/lib/rbcodec/dsp/eq_arm.S b/lib/rbcodec/dsp/eq_arm.S
deleted file mode 100644
index b0e1771e89..0000000000
--- a/lib/rbcodec/dsp/eq_arm.S
+++ /dev/null
@@ -1,89 +0,0 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006-2007 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21
22#include "config.h"
23
24/* uncomment this to make filtering calculate lower bits after shifting.
25 * without this, "shift" of the lower bits will be lost here.
26 */
27/* #define HIGH_PRECISION */
28
29/*
30 * void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
31 * unsigned channels, unsigned shift)
32 */
33#if CONFIG_CPU == PP5002
34 .section .icode,"ax",%progbits
35#else
36 .text
37#endif
38 .global eq_filter
39eq_filter:
40 ldr r12, [sp] @ get shift parameter
41 stmdb sp!, { r0-r11, lr } @ save all params and clobbered regs
42 ldmia r1!, { r4-r8 } @ load coefs
43 mov r10, r1 @ loop prelude expects filter struct addr in r10
44
45.filterloop:
46 ldr r9, [sp] @ get pointer to this channels data
47 add r0, r9, #4
48 str r0, [sp] @ save back pointer to next channels data
49 ldr r9, [r9] @ r9 = x[]
50 ldr r14, [sp, #8] @ r14 = numsamples
51 ldmia r10, { r0-r3 } @ load history, r10 should be filter struct addr
52 str r10, [sp, #4] @ save it for loop end
53
54 /* r0-r3 = history, r4-r8 = coefs, r9 = x[], r10..r11 = accumulator,
55 * r12 = shift amount, r14 = number of samples.
56 */
57.loop:
58 /* Direct form 1 filtering code.
59 * y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
60 * where y[] is output and x[] is input. This is performed out of order to
61 * reuse registers, we're pretty short on regs.
62 */
63 smull r10, r11, r6, r1 @ acc = b2*x[i - 2]
64 mov r1, r0 @ fix input history
65 smlal r10, r11, r5, r0 @ acc += b1*x[i - 1]
66 ldr r0, [r9] @ load input and fix history in same operation
67 smlal r10, r11, r7, r2 @ acc += a1*y[i - 1]
68 smlal r10, r11, r8, r3 @ acc += a2*y[i - 2]
69 smlal r10, r11, r4, r0 @ acc += b0*x[i] /* avoid stall on arm9*/
70 mov r3, r2 @ fix output history
71 mov r2, r11, asl r12 @ get upper part of result and shift left
72#ifdef HIGH_PRECISION
73 rsb r11, r12, #32 @ get shift amount for lower part
74 orr r2, r2, r10, lsr r11 @ then mix in correctly shifted lower part
75#endif
76 str r2, [r9], #4 @ save result
77 subs r14, r14, #1 @ are we done with this channel?
78 bne .loop
79
80 ldr r10, [sp, #4] @ load filter struct pointer
81 stmia r10!, { r0-r3 } @ save back history
82 ldr r11, [sp, #12] @ load number of channels
83 subs r11, r11, #1 @ all channels processed?
84 strne r11, [sp, #12]
85 bne .filterloop
86
87 add sp, sp, #16 @ compensate for temp storage
88 ldmpc regs=r4-r11
89
diff --git a/lib/rbcodec/dsp/eq_cf.S b/lib/rbcodec/dsp/eq_cf.S
deleted file mode 100644
index 30a28b9d99..0000000000
--- a/lib/rbcodec/dsp/eq_cf.S
+++ /dev/null
@@ -1,91 +0,0 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006-2007 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21
22/* uncomment this to make filtering calculate lower bits after shifting.
23 * without this, "shift" - 1 of the lower bits will be lost here.
24 */
25/* #define HIGH_PRECISION */
26
27/*
28 * void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
29 * unsigned channels, unsigned shift)
30 */
31 .text
32 .global eq_filter
33eq_filter:
34 lea.l (-11*4, %sp), %sp
35 movem.l %d2-%d7/%a2-%a6, (%sp) | save clobbered regs
36 move.l (11*4+8, %sp), %a5 | fetch filter structure address
37 move.l (11*4+20, %sp), %d7 | load shift count
38 subq.l #1, %d7 | EMAC gives us one free shift
39#ifdef HIGH_PRECISION
40 moveq.l #8, %d6
41 sub.l %d7, %d6 | shift for lower part of accumulator
42#endif
43 movem.l (%a5), %a0-%a4 | load coefs
44 lea.l (5*4, %a5), %a5 | point to filter history
45
46.filterloop:
47 move.l (11*4+4, %sp), %a6 | load input channel pointer
48 addq.l #4, (11*4+4, %sp) | point x to next channel
49 move.l (%a6), %a6
50 move.l (11*4+12, %sp), %d5 | number of samples
51 movem.l (%a5), %d0-%d3 | load filter history
52
53 /* d0-d3 = history, d4 = temp, d5 = sample count, d6 = lower shift amount,
54 * d7 = upper shift amount, a0-a4 = coefs, a5 = history pointer, a6 = x[]
55 */
56.loop:
57 /* Direct form 1 filtering code. We assume DSP has put EMAC in frac mode.
58 * y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
59 * where y[] is output and x[] is input. This is performed out of order
60 * to do parallel load of input value.
61 */
62 mac.l %a2, %d1, %acc0 | acc = b2*x[i - 2]
63 move.l %d0, %d1 | fix input history
64 mac.l %a1, %d0, (%a6), %d0, %acc0 | acc += b1*x[i - 1], x[i] -> d0
65 mac.l %a0, %d0, %acc0 | acc += b0*x[i]
66 mac.l %a3, %d2, %acc0 | acc += a1*y[i - 1]
67 mac.l %a4, %d3, %acc0 | acc += a2*y[i - 2]
68 move.l %d2, %d3 | fix output history
69#ifdef HIGH_PRECISION
70 move.l %accext01, %d2 | fetch lower part of accumulator
71 move.b %d2, %d4 | clear upper three bytes
72 lsr.l %d6, %d4 | shift lower bits
73#endif
74 movclr.l %acc0, %d2 | fetch upper part of result
75 asl.l %d7, %d2 | restore fixed point format
76#ifdef HIGH_PRECISION
77 or.l %d2, %d4 | combine lower and upper parts
78#endif
79 move.l %d2, (%a6)+ | save result
80 subq.l #1, %d5 | are we done with this channel?
81 jne .loop
82
83 movem.l %d0-%d3, (%a5) | save history back to struct
84 lea.l (4*4, %a5), %a5 | point to next channel's history
85 subq.l #1, (11*4+16, %sp) | have we processed both channels?
86 jne .filterloop
87
88 movem.l (%sp), %d2-%d7/%a2-%a6
89 lea.l (11*4, %sp), %sp
90 rts
91
diff --git a/lib/rbcodec/dsp/lin_resample.c b/lib/rbcodec/dsp/lin_resample.c
new file mode 100644
index 0000000000..c8be3cb1ad
--- /dev/null
+++ b/lib/rbcodec/dsp/lin_resample.c
@@ -0,0 +1,281 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Miika Pekkarinen
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include "fracmul.h"
26#include "fixedpoint.h"
27#include "dsp_sample_io.h"
28#include <string.h>
29#include "dsp_proc_entry.h"
30
31/**
32 * Linear interpolation resampling that introduces a one sample delay because
33 * of our inability to look into the future at the end of a frame.
34 */
35
36#if 0 /* Set to '1' to enable debug messages */
37#include <debug.h>
38#else
39#undef DEBUGF
40#define DEBUGF(...)
41#endif
42
43#define RESAMPLE_BUF_COUNT 192 /* Per channel, per DSP */
44
45/* Data for each resampler on each DSP */
46static struct resample_data
47{
48 uint32_t delta; /* 00h: Phase delta for each step */
49 uint32_t phase; /* 04h: Current phase [pos16|frac16] */
50 int32_t last_sample[2]; /* 08h: Last samples for interpolation (L+R) */
51 int32_t frequency; /* 10h: Virtual samplerate */
52 /* 14h */
53 struct dsp_config *dsp; /* The DSP for this resampler */
54 struct dsp_buffer resample_buf; /* Buffer descriptor for resampled data */
55 int32_t resample_buf_arr[2][RESAMPLE_BUF_COUNT]; /* Actual output data */
56} resample_data[DSP_COUNT] IBSS_ATTR;
57
58/* Actual worker function. Implemented here or in target assembly code. */
59int lin_resample_resample(struct resample_data *data, struct dsp_buffer *src,
60 struct dsp_buffer *dst);
61
62static void lin_resample_flush_data(struct resample_data *data)
63{
64 data->phase = 0;
65 data->last_sample[0] = 0;
66 data->last_sample[1] = 0;
67}
68
69static void lin_resample_flush(struct dsp_proc_entry *this)
70{
71 struct resample_data *data = (void *)this->data;
72 data->resample_buf.remcount = 0;
73 lin_resample_flush_data(data);
74}
75
76static bool lin_resample_new_delta(struct resample_data *data,
77 struct dsp_buffer *buf)
78{
79 int32_t frequency = buf->format.frequency; /* virtual samplerate */
80
81 data->frequency = frequency;
82 data->delta = fp_div(frequency, NATIVE_FREQUENCY, 16);
83
84 if (frequency == NATIVE_FREQUENCY)
85 {
86 /* NOTE: If fully glitch-free transistions from no resampling to
87 resampling are desired, last_sample history should be maintained
88 even when not resampling. */
89 lin_resample_flush_data(data);
90 return false;
91 }
92
93 return true;
94}
95
96#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
97/* Where the real work is done */
98int lin_resample_resample(struct resample_data *data, struct dsp_buffer *src,
99 struct dsp_buffer *dst)
100{
101 int ch = src->format.num_channels - 1;
102 uint32_t count = MIN(src->remcount, 0x8000);
103 uint32_t delta = data->delta;
104 uint32_t phase, pos;
105 int32_t *d;
106
107 do
108 {
109 const int32_t *s = src->p32[ch];
110
111 d = dst->p32[ch];
112 int32_t *dmax = d + dst->bufcount;
113
114 phase = data->phase;
115 pos = phase >> 16;
116 pos = MIN(pos, count);
117
118 int32_t last = pos > 0 ? s[pos - 1] : data->last_sample[ch];
119
120 if (pos < count)
121 {
122 while (1)
123 {
124 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
125 phase += delta;
126 pos = phase >> 16;
127
128 if (pos >= count || d >= dmax)
129 break;
130
131 if (pos > 0)
132 last = s[pos - 1];
133 }
134
135 if (pos > 0)
136 {
137 pos = MIN(pos, count);
138 last = s[pos - 1];
139 }
140 }
141
142 data->last_sample[ch] = last;
143 }
144 while (--ch >= 0);
145
146 /* Wrap phase accumulator back to start of next frame. */
147 data->phase = phase - (pos << 16);
148
149 dst->remcount = d - dst->p32[0];
150
151 return pos;
152}
153#endif /* CPU */
154
155/* Resample count stereo samples or stop when the destination is full.
156 * Updates the src buffer and changes to its own output buffer to refer to
157 * the resampled data. */
158static void lin_resample_process(struct dsp_proc_entry *this,
159 struct dsp_buffer **buf_p)
160{
161 struct resample_data *data = (void *)this->data;
162 struct dsp_buffer *src = *buf_p;
163 struct dsp_buffer *dst = &data->resample_buf;
164
165 *buf_p = dst;
166
167 if (dst->remcount > 0)
168 return; /* data still remains */
169
170 int channels = src->format.num_channels;
171
172 dst->remcount = 0;
173 dst->p32[0] = data->resample_buf_arr[0];
174 dst->p32[1] = data->resample_buf_arr[channels - 1];
175
176 if (src->remcount > 0)
177 {
178 dst->bufcount = RESAMPLE_BUF_COUNT;
179
180 int consumed = lin_resample_resample(data, src, dst);
181
182 /* Advance src by consumed amount */
183 if (consumed > 0)
184 dsp_advance_buffer32(src, consumed);
185 }
186 /* else purged resample_buf */
187
188 /* Inherit in-place processed mask from source buffer */
189 dst->proc_mask = src->proc_mask;
190}
191
192/* Finish draining old samples then switch format or shut off */
193static void lin_resample_new_format(struct dsp_proc_entry *this,
194 struct dsp_buffer **buf_p)
195{
196 struct resample_data *data = (void *)this->data;
197 struct dsp_buffer *src = *buf_p;
198 struct dsp_buffer *dst = &data->resample_buf;
199
200 if (dst->remcount > 0)
201 {
202 *buf_p = dst;
203 return; /* data still remains */
204 }
205
206 DSP_PRINT_FORMAT(DSP_PROC_RESAMPLE, DSP_PROC_RESAMPLE, src->format);
207
208 struct dsp_config *dsp = data->dsp;
209 int32_t frequency = data->frequency;
210 bool active = dsp_proc_active(dsp, DSP_PROC_RESAMPLE);
211
212 if (src->format.frequency != frequency)
213 {
214 DEBUGF(" DSP_PROC_RESAMPLE- new delta\n");
215 active = lin_resample_new_delta(data, src);
216 dsp_proc_activate(dsp, DSP_PROC_RESAMPLE, active);
217 }
218
219 /* Everything after us is NATIVE_FREQUENCY */
220 struct sample_format f = src->format;
221 f.frequency = NATIVE_FREQUENCY;
222 f.codec_frequency = NATIVE_FREQUENCY;
223
224 if (!active)
225 {
226 DEBUGF(" DSP_PROC_RESAMPLE- not active\n");
227 dst->format = f; /* Keep track */
228 return; /* No resampling required */
229 }
230
231 format_change_ack(&src->format);
232
233 if (EQU_SAMPLE_FORMAT(f, dst->format))
234 {
235 DEBUGF(" DSP_PROC_RESAMPLE- same dst format\n");
236 format_change_ack(&f); /* Nothing changed that matters downstream */
237 }
238
239 dst->format = f;
240 dsp_proc_call(this, buf_p, 0);
241}
242
243/* DSP message hook */
244static intptr_t lin_resample_configure(struct dsp_proc_entry *this,
245 struct dsp_config *dsp,
246 unsigned int setting,
247 intptr_t value)
248{
249 switch (setting)
250 {
251 case DSP_INIT:
252 /* Always enable resampler so that format changes may be monitored and
253 * it self-activated when required */
254 dsp_proc_enable(dsp, DSP_PROC_RESAMPLE, true);
255 break;
256
257 case DSP_FLUSH:
258 lin_resample_flush(this);
259 break;
260
261 case DSP_PROC_INIT:
262 this->data = (intptr_t)&resample_data[dsp_get_id(dsp)];
263 this->ip_mask = 0; /* Not in-place */
264 this->process[0] = lin_resample_process;
265 this->process[1] = lin_resample_new_format;
266 ((struct resample_data *)this->data)->dsp = dsp;
267 break;
268
269 case DSP_PROC_CLOSE:
270 /* This stage should be enabled at all times */
271 DEBUGF("DSP_PROC_RESAMPLE- Error: Closing!\n");
272 break;
273 }
274
275 return 1;
276 (void)value;
277}
278
279/* Database entry */
280DSP_PROC_DB_ENTRY(RESAMPLE,
281 lin_resample_configure);
diff --git a/lib/rbcodec/dsp/pga.c b/lib/rbcodec/dsp/pga.c
new file mode 100644
index 0000000000..c2c29ccfc0
--- /dev/null
+++ b/lib/rbcodec/dsp/pga.c
@@ -0,0 +1,144 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2005 Magnus Holmgren
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include "dsp-util.h"
26#include "fixedpoint.h"
27#include "fracmul.h"
28#include "dsp_proc_entry.h"
29
30/* Implemented here or in target assembly code */
31void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p);
32
33#define DEFAULT_PGA_GAIN (PGA_UNITY >> 1) /* s8.23 format */
34
35static struct pga_data
36{
37 int32_t gain; /* 00h: Final gain in s8.23 format */
38 uint32_t enabled; /* Mask of enabled gains */
39 int32_t gains[PGA_NUM_GAINS]; /* Individual gains in s7.24 format */
40} pga_data =
41{
42 .gain = DEFAULT_PGA_GAIN,
43 .enabled = 0,
44 .gains[0 ... PGA_NUM_GAINS-1] = PGA_UNITY,
45};
46
47/* Combine all gains to a global gain and enable/disable the amplifier if
48 the overall gain is not unity/unity */
49static void pga_update(void)
50{
51 int32_t gain = PGA_UNITY;
52
53 /* Multiply all gains with one another to get overall amp gain */
54 for (int i = 0; i < PGA_NUM_GAINS; i++)
55 {
56 if (pga_data.enabled & BIT_N(i)) /* Only enabled gains factor in */
57 gain = fp_mul(gain, pga_data.gains[i], 24);
58 }
59
60 gain >>= 1; /* s7.24 -> s8.23 format */
61
62 if (gain == pga_data.gain)
63 return;
64
65 struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
66 pga_data.gain = gain;
67 dsp_proc_enable(dsp, DSP_PROC_PGA, gain != DEFAULT_PGA_GAIN);
68 dsp_proc_activate(dsp, DSP_PROC_PGA, true);
69}
70
71
72/** Amp controls **/
73
74/* Set a particular gain value - doesn't have to be enabled */
75void pga_set_gain(enum pga_gain_ids id, int32_t value)
76{
77 if (value == pga_data.gains[id])
78 return;
79
80 pga_data.gains[id] = value;
81
82 if (BIT_N(id) & pga_data.enabled)
83 pga_update();
84}
85
86/* Enable or disable the specified gain stage */
87void pga_enable_gain(enum pga_gain_ids id, bool enable)
88{
89 uint32_t bit = BIT_N(id);
90
91 if (enable != !(pga_data.enabled & bit))
92 return;
93
94 pga_data.enabled ^= bit;
95 pga_update();
96}
97
98
99/** DSP interface **/
100
101#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
102/* Apply a constant gain to the samples (e.g., for ReplayGain). */
103void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
104{
105 int32_t gain = ((struct pga_data *)this->data)->gain;
106 struct dsp_buffer *buf = *buf_p;
107 unsigned int channels = buf->format.num_channels;
108
109 for (unsigned int ch = 0; ch < channels; ch++)
110 {
111 int32_t *d = buf->p32[ch];
112 int count = buf->remcount;
113
114 for (int i = 0; i < count; i++)
115 d[i] = FRACMUL_SHL(d[i], gain, 8);
116 }
117
118 (void)this;
119}
120#endif /* CPU */
121
122/* DSP message hook */
123static intptr_t pga_configure(struct dsp_proc_entry *this,
124 struct dsp_config *dsp,
125 unsigned int setting,
126 intptr_t value)
127{
128 switch (setting)
129 {
130 case DSP_PROC_INIT:
131 if (value != 0)
132 break; /* Already initialized */
133 this->data = (intptr_t)&pga_data;
134 this->process[0] = pga_process;
135 break;
136 }
137
138 return 1;
139 (void)dsp;
140}
141
142/* Database entry */
143DSP_PROC_DB_ENTRY(PGA,
144 pga_configure);
diff --git a/lib/rbcodec/dsp/pga.h b/lib/rbcodec/dsp/pga.h
new file mode 100644
index 0000000000..f0c4c4717f
--- /dev/null
+++ b/lib/rbcodec/dsp/pga.h
@@ -0,0 +1,40 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2012 Michael Sevakis
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef PGA_H
22#define PGA_H
23
24#define PGA_UNITY ((int32_t)0x01000000) /* s7.24 */
25
26/* Various gains supported by pre-gain amp */
27enum pga_gain_ids
28{
29 PGA_EQ_PRECUT = 0,
30 PGA_REPLAYGAIN,
31#ifdef HAVE_SW_VOLUME_CONTROL
32 PGA_VOLUME,
33#endif
34 PGA_NUM_GAINS,
35};
36
37void pga_set_gain(enum pga_gain_ids id, int32_t value);
38void pga_enable_gain(enum pga_gain_ids id, bool enable);
39
40#endif /* PGA_H */
diff --git a/lib/rbcodec/dsp/tdspeed.c b/lib/rbcodec/dsp/tdspeed.c
index c2f4a3f704..3aa8acc458 100644
--- a/lib/rbcodec/dsp/tdspeed.c
+++ b/lib/rbcodec/dsp/tdspeed.c
@@ -9,6 +9,7 @@
9 * 9 *
10 * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org> 10 * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org>
11 * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca> 11 * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca>
12 * Copyright (C) 2012 Michael Sevakis
12 * 13 *
13 * This program is free software; you can redistribute it and/or 14 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License 15 * modify it under the terms of the GNU General Public License
@@ -19,69 +20,42 @@
19 * KIND, either express or implied. 20 * KIND, either express or implied.
20 * 21 *
21 ****************************************************************************/ 22 ****************************************************************************/
22 23#include "config.h"
23#include <inttypes.h> 24#include "system.h"
24#include <stddef.h>
25#include <stdio.h>
26#include <string.h>
27#include "sound.h" 25#include "sound.h"
28#include "core_alloc.h" 26#include "core_alloc.h"
29#include "system.h" 27#include "system.h"
30#include "tdspeed.h" 28#include "tdspeed.h"
31#include "settings.h" 29#include "settings.h"
32#include "dsp-util.h" 30#include "dsp-util.h"
31#include "dsp_proc_entry.h"
33 32
34#define assert(cond) 33#define assert(cond)
35 34
35#define TIMESTRETCH_SET_FACTOR (DSP_PROC_SETTING+DSP_PROC_TIMESTRETCH)
36
36#define MIN_RATE 8000 37#define MIN_RATE 8000
37#define MAX_RATE 48000 /* double buffer for double rate */ 38#define MAX_RATE 48000 /* double buffer for double rate */
38#define MINFREQ 100 39#define MINFREQ 100
39 40
40#define FIXED_BUFSIZE 3072 /* 48KHz factor 3.0 */ 41#define MAX_INPUTCOUNT 512 /* Max input count so dst doesn't overflow */
41 42#define FIXED_BUFCOUNT 3072 /* 48KHz factor 3.0 */
42static int32_t** dsp_src; 43#define FIXED_OUTBUFCOUNT 4096
43static int handles[4];
44static int32_t *overlap_buffer[2] = { NULL, NULL };
45static int32_t *outbuf[2] = { NULL, NULL };
46
47static int move_callback(int handle, void* current, void* new)
48{
49 /* TODO */
50 (void)handle;
51 if (dsp_src)
52 {
53 int ch = (current == outbuf[0]) ? 0 : 1;
54 dsp_src[ch] = outbuf[ch] = new;
55 }
56 return BUFLIB_CB_OK;
57}
58
59static struct buflib_callbacks ops = {
60 .move_callback = move_callback,
61 .shrink_callback = NULL,
62};
63 44
64static int ovl_move_callback(int handle, void* current, void* new) 45enum tdspeed_ops
65{ 46{
66 /* TODO */ 47 TDSOP_PROCESS,
67 (void)handle; 48 TDSOP_LAST,
68 if (dsp_src) 49 TDSOP_PURGE,
69 {
70 int ch = (current == overlap_buffer[0]) ? 0 : 1;
71 overlap_buffer[ch] = new;
72 }
73 return BUFLIB_CB_OK;
74}
75
76static struct buflib_callbacks ovl_ops = {
77 .move_callback = ovl_move_callback,
78 .shrink_callback = NULL,
79}; 50};
80 51
81
82static struct tdspeed_state_s 52static struct tdspeed_state_s
83{ 53{
84 bool stereo; 54 struct dsp_proc_entry *this; /* this stage */
55 struct dsp_config *dsp; /* the DSP we use */
56 unsigned int channels; /* flags parameter to use in call */
57 int32_t samplerate; /* current samplerate of input data */
58 int32_t factor; /* stretch factor (perdecimille) */
85 int32_t shift_max; /* maximum displacement on a frame */ 59 int32_t shift_max; /* maximum displacement on a frame */
86 int32_t src_step; /* source window pace */ 60 int32_t src_step; /* source window pace */
87 int32_t dst_step; /* destination window pace */ 61 int32_t dst_step; /* destination window pace */
@@ -89,62 +63,132 @@ static struct tdspeed_state_s
89 int32_t ovl_shift; /* overlap buffer frame shift */ 63 int32_t ovl_shift; /* overlap buffer frame shift */
90 int32_t ovl_size; /* overlap buffer used size */ 64 int32_t ovl_size; /* overlap buffer used size */
91 int32_t ovl_space; /* overlap buffer size */ 65 int32_t ovl_space; /* overlap buffer size */
92 int32_t *ovl_buff[2]; /* overlap buffer */ 66 int32_t *ovl_buff[2]; /* overlap buffer (L+R) */
93} tdspeed_state; 67} tdspeed_state;
94 68
95void tdspeed_init(void) 69static int handles[4] = { 0, 0, 0, 0 };
70static int32_t *buffers[4] = { NULL, NULL, NULL, NULL };
71
72#define overlap_buffer (&buffers[0])
73#define outbuf (&buffers[2])
74#define out_size FIXED_OUTBUFCOUNT
75
76/* Processed buffer passed out to later stages */
77static struct dsp_buffer dsp_outbuf;
78
79static int move_callback(int handle, void *current, void *new)
96{ 80{
97 if (!global_settings.timestretch_enabled) 81#if 0
98 return; 82 /* Should not currently need to block this since DSP loop completes an
83 iteration before yielding and begins again at its input buffer */
84 if (dsp_is_busy(tdspeed_state.dsp))
85 return BUFLIB_CB_CANNOT_MOVE; /* DSP processing in progress */
86#endif
99 87
100 /* Allocate buffers */ 88 ptrdiff_t shift = (int32_t *)new - (int32_t *)current;
101 if (overlap_buffer[0] == NULL) 89 int32_t **p32 = dsp_outbuf.p32;
90
91 for (unsigned int i = 0; i < ARRAYLEN(handles); i++)
102 { 92 {
103 handles[0] = core_alloc_ex("tdspeed ovl left", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops); 93 if (handle != handles[i])
104 overlap_buffer[0] = core_get_data(handles[0]); 94 continue;
95
96 switch (i)
97 {
98 case 0: case 1:
99 /* moving overlap (input) buffers */
100 tdspeed_state.ovl_buff[i] = new;
101 break;
102
103 case 2:
104 /* moving outbuf left channel and dsp_outbuf.p32[0] */
105 if (p32[0] == p32[1])
106 p32[1] += shift; /* mono mode */
107
108 p32[0] += shift;
109 break;
110
111 case 3:
112 /* moving outbuf right channel and dsp_outbuf.p32[1] */
113 p32[1] += shift;
114 break;
115 }
116
117 buffers[i] = new;
118 break;
105 } 119 }
106 if (overlap_buffer[1] == NULL) 120
121 return BUFLIB_CB_OK;
122}
123
124static struct buflib_callbacks ops =
125{
126 .move_callback = move_callback,
127 .shrink_callback = NULL,
128};
129
130/* Allocate timestretch buffers */
131static bool tdspeed_alloc_buffers(void)
132{
133 static const struct
107 { 134 {
108 handles[1] = core_alloc_ex("tdspeed ovl right", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops); 135 const char *name;
109 overlap_buffer[1] = core_get_data(handles[1]); 136 size_t size;
110 } 137 } bufdefs[4] =
111 if (outbuf[0] == NULL)
112 { 138 {
113 handles[2] = core_alloc_ex("tdspeed left", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops); 139 { "tdspeed ovl L", FIXED_BUFCOUNT * sizeof(int32_t) },
114 outbuf[0] = core_get_data(handles[2]); 140 { "tdspeed ovl R", FIXED_BUFCOUNT * sizeof(int32_t) },
115 } 141 { "tdspeed out L", FIXED_OUTBUFCOUNT * sizeof(int32_t) },
116 if (outbuf[1] == NULL) 142 { "tdspeed out R", FIXED_OUTBUFCOUNT * sizeof(int32_t) },
143 };
144
145 for (unsigned int i = 0; i < ARRAYLEN(bufdefs); i++)
117 { 146 {
118 handles[3] = core_alloc_ex("tdspeed right", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops); 147 if (handles[i] <= 0)
119 outbuf[1] = core_get_data(handles[3]); 148 {
149 handles[i] = core_alloc_ex(bufdefs[i].name, bufdefs[i].size, &ops);
150
151 if (handles[i] <= 0)
152 return false;
153 }
154
155 if (buffers[i] == NULL)
156 {
157 buffers[i] = core_get_data(handles[i]);
158
159 if (buffers[i] == NULL)
160 return false;
161 }
120 } 162 }
163
164 return true;
121} 165}
122 166
123void tdspeed_finish(void) 167/* Free timestretch buffers */
168static void tdspeed_free_buffers(void)
124{ 169{
125 for(unsigned i = 0; i < ARRAYLEN(handles); i++) 170 for (unsigned int i = 0; i < ARRAYLEN(handles); i++)
126 { 171 {
127 if (handles[i] > 0) 172 if (handles[i] > 0)
128 {
129 core_free(handles[i]); 173 core_free(handles[i]);
130 handles[i] = 0; 174
131 } 175 handles[i] = 0;
176 buffers[i] = NULL;
132 } 177 }
133 overlap_buffer[0] = overlap_buffer[1] = NULL;
134 outbuf[0] = outbuf[1] = NULL;
135} 178}
136 179
137bool tdspeed_config(int samplerate, bool stereo, int32_t factor) 180/* Discard all data */
181static void tdspeed_flush(void)
138{ 182{
139 struct tdspeed_state_s *st = &tdspeed_state; 183 struct tdspeed_state_s *st = &tdspeed_state;
140 int src_frame_sz; 184 st->ovl_size = 0;
141 185 st->ovl_shift = 0;
142 /* Check buffers were allocated ok */ 186 dsp_outbuf.remcount = 0; /* Dump remaining output */
143 if (overlap_buffer[0] == NULL || overlap_buffer[1] == NULL) 187}
144 return false;
145 188
146 if (outbuf[0] == NULL || outbuf[1] == NULL) 189static bool tdspeed_update(int32_t samplerate, int32_t factor)
147 return false; 190{
191 struct tdspeed_state_s *st = &tdspeed_state;
148 192
149 /* Check parameters */ 193 /* Check parameters */
150 if (factor == PITCH_SPEED_100) 194 if (factor == PITCH_SPEED_100)
@@ -156,7 +200,10 @@ bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
156 if (factor < STRETCH_MIN || factor > STRETCH_MAX) 200 if (factor < STRETCH_MIN || factor > STRETCH_MAX)
157 return false; 201 return false;
158 202
159 st->stereo = stereo; 203 /* Save parameters we'll need later if format changes */
204 st->samplerate = samplerate;
205 st->factor = factor;
206
160 st->dst_step = samplerate / MINFREQ; 207 st->dst_step = samplerate / MINFREQ;
161 208
162 if (factor > PITCH_SPEED_100) 209 if (factor > PITCH_SPEED_100)
@@ -171,7 +218,7 @@ bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
171 st->src_step = st->dst_step * factor / PITCH_SPEED_100; 218 st->src_step = st->dst_step * factor / PITCH_SPEED_100;
172 st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step; 219 st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step;
173 220
174 src_frame_sz = st->shift_max + st->dst_step; 221 int src_frame_sz = st->shift_max + st->dst_step;
175 222
176 if (st->dst_step > st->src_step) 223 if (st->dst_step > st->src_step)
177 src_frame_sz += st->dst_step - st->src_step; 224 src_frame_sz += st->dst_step - st->src_step;
@@ -182,32 +229,27 @@ bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
182 if (st->src_step > st->dst_step) 229 if (st->src_step > st->dst_step)
183 st->ovl_space += 2*st->src_step - st->dst_step; 230 st->ovl_space += 2*st->src_step - st->dst_step;
184 231
185 if (st->ovl_space > FIXED_BUFSIZE) 232 if (st->ovl_space > FIXED_BUFCOUNT)
186 st->ovl_space = FIXED_BUFSIZE; 233 st->ovl_space = FIXED_BUFCOUNT;
187 234
235 /* just discard remaining input data */
188 st->ovl_size = 0; 236 st->ovl_size = 0;
189 st->ovl_shift = 0; 237 st->ovl_shift = 0;
190 238
191 st->ovl_buff[0] = overlap_buffer[0]; 239 st->ovl_buff[0] = overlap_buffer[0];
192 240 st->ovl_buff[1] = overlap_buffer[1]; /* ignored if mono */
193 if (stereo)
194 st->ovl_buff[1] = overlap_buffer[1];
195 else
196 st->ovl_buff[1] = st->ovl_buff[0];
197 241
198 return true; 242 return true;
199} 243}
200 244
201static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2], 245static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
202 int data_len, int last, int out_size) 246 int data_len, enum tdspeed_ops op, int *consumed)
203/* data_len in samples */ 247/* data_len in samples */
204{ 248{
205 struct tdspeed_state_s *st = &tdspeed_state; 249 struct tdspeed_state_s *st = &tdspeed_state;
206 int32_t *dest[2]; 250 int32_t *dest[2];
207 int32_t next_frame, prev_frame, src_frame_sz; 251 int32_t next_frame, prev_frame, src_frame_sz;
208 bool stereo = buf_in[0] != buf_in[1]; 252 bool stereo = st->channels > 1;
209
210 assert(stereo == st->stereo);
211 253
212 src_frame_sz = st->shift_max + st->dst_step; 254 src_frame_sz = st->shift_max + st->dst_step;
213 255
@@ -233,7 +275,7 @@ static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
233 if (copy > data_len) 275 if (copy > data_len)
234 copy = data_len; 276 copy = data_len;
235 277
236 assert(st->ovl_size + copy <= FIXED_BUFSIZE); 278 assert(st->ovl_size + copy <= FIXED_BUFCOUNT);
237 memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0], 279 memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0],
238 copy * sizeof(int32_t)); 280 copy * sizeof(int32_t));
239 281
@@ -241,7 +283,9 @@ static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
241 memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1], 283 memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1],
242 copy * sizeof(int32_t)); 284 copy * sizeof(int32_t));
243 285
244 if (!last && have + copy < src_frame_sz) 286 *consumed += copy;
287
288 if (op == TDSOP_PROCESS && have + copy < src_frame_sz)
245 { 289 {
246 /* still not enough to process at least one frame */ 290 /* still not enough to process at least one frame */
247 st->ovl_size += copy; 291 st->ovl_size += copy;
@@ -254,13 +298,14 @@ static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
254 298
255 if (copy == data_len) 299 if (copy == data_len)
256 { 300 {
257 assert(have + copy <= FIXED_BUFSIZE); 301 assert(have + copy <= FIXED_BUFCOUNT);
258 return tdspeed_apply(buf_out, st->ovl_buff, have+copy, last, 302 return tdspeed_apply(buf_out, st->ovl_buff, have+copy, op,
259 out_size); 303 consumed);
260 } 304 }
261 305
262 assert(have + copy <= FIXED_BUFSIZE); 306 assert(have + copy <= FIXED_BUFCOUNT);
263 int i = tdspeed_apply(buf_out, st->ovl_buff, have+copy, -1, out_size); 307 int i = tdspeed_apply(buf_out, st->ovl_buff, have+copy,
308 TDSOP_LAST, consumed);
264 309
265 dest[0] = buf_out[0] + i; 310 dest[0] = buf_out[0] + i;
266 dest[1] = buf_out[1] + i; 311 dest[1] = buf_out[1] + i;
@@ -379,12 +424,12 @@ skip:;
379 } 424 }
380 425
381 /* now deal with remaining partial frames */ 426 /* now deal with remaining partial frames */
382 if (last == -1) 427 if (op == TDSOP_LAST)
383 { 428 {
384 /* special overlap buffer processing: remember frame shift only */ 429 /* special overlap buffer processing: remember frame shift only */
385 st->ovl_shift = next_frame - prev_frame; 430 st->ovl_shift = next_frame - prev_frame;
386 } 431 }
387 else if (last != 0) 432 else if (op == TDSOP_PURGE)
388 { 433 {
389 /* last call: purge all remaining data to output buffer */ 434 /* last call: purge all remaining data to output buffer */
390 int i = data_len - prev_frame; 435 int i = data_len - prev_frame;
@@ -400,6 +445,8 @@ skip:;
400 memcpy(dest[1], buf_in[1] + prev_frame, i * sizeof(int32_t)); 445 memcpy(dest[1], buf_in[1] + prev_frame, i * sizeof(int32_t));
401 dest[1] += i; 446 dest[1] += i;
402 } 447 }
448
449 *consumed += i;
403 } 450 }
404 else 451 else
405 { 452 {
@@ -408,7 +455,7 @@ skip:;
408 int i = (st->ovl_shift < 0) ? next_frame : prev_frame; 455 int i = (st->ovl_shift < 0) ? next_frame : prev_frame;
409 st->ovl_size = data_len - i; 456 st->ovl_size = data_len - i;
410 457
411 assert(st->ovl_size <= FIXED_BUFSIZE); 458 assert(st->ovl_size <= FIXED_BUFCOUNT);
412 memcpy(st->ovl_buff[0], buf_in[0] + i, st->ovl_size * sizeof(int32_t)); 459 memcpy(st->ovl_buff[0], buf_in[0] + i, st->ovl_size * sizeof(int32_t));
413 460
414 if (stereo) 461 if (stereo)
@@ -418,32 +465,223 @@ skip:;
418 return dest[0] - buf_out[0]; 465 return dest[0] - buf_out[0];
419} 466}
420 467
421long tdspeed_est_output_size() 468
469/** DSP interface **/
470
471static void tdspeed_process_new_format(struct dsp_proc_entry *this,
472 struct dsp_buffer **buf_p);
473
474/* Enable or disable the availability of timestretch */
475void dsp_timestretch_enable(bool enabled)
422{ 476{
423 return TDSPEED_OUTBUFSIZE; 477 if (enabled != !tdspeed_state.this)
478 return; /* No change */
479
480 dsp_proc_enable(dsp_get_config(CODEC_IDX_AUDIO), DSP_PROC_TIMESTRETCH,
481 enabled);
424} 482}
425 483
426long tdspeed_est_input_size(long size) 484/* Set the timestretch ratio */
485void dsp_set_timestretch(int32_t percent)
427{ 486{
428 struct tdspeed_state_s *st = &tdspeed_state; 487 struct tdspeed_state_s *st = &tdspeed_state;
429 488
430 size = (size - st->ovl_size) * st->src_step / st->dst_step; 489 if (!st->this)
490 return; /* not enabled */
491
492 if (percent <= 0)
493 percent = PITCH_SPEED_100;
494
495 if (percent == st->factor)
496 return; /* no change */
497
498 dsp_configure(st->dsp, TIMESTRETCH_SET_FACTOR, percent);
499}
500
501/* Return the timestretch ratio */
502int32_t dsp_get_timestretch(void)
503{
504 return tdspeed_state.factor;
505}
506
507/* Return whether or not timestretch is enabled and initialized */
508bool dsp_timestretch_available(void)
509{
510 return !!tdspeed_state.this;
511}
512
513/* Apply timestretch to the input buffer and switch to our output buffer */
514static void tdspeed_process(struct dsp_proc_entry *this,
515 struct dsp_buffer **buf_p)
516{
517 struct dsp_buffer *src = *buf_p;
518 struct dsp_buffer *dst = &dsp_outbuf;
519
520 *buf_p = dst; /* switch to our buffer */
521
522 int count = dst->remcount;
523
524 if (count > 0)
525 return; /* output remains from an earlier call */
431 526
432 if (size < 0) 527 dst->p32[0] = outbuf[0];
433 size = 0; 528 dst->p32[1] = outbuf[src->format.num_channels - 1];
434 529
435 return size; 530 if (src->remcount > 0)
531 {
532 dst->bufcount = 0; /* use this to get consumed src */
533 count = tdspeed_apply(dst->p32, src->p32,
534 MIN(src->remcount, MAX_INPUTCOUNT),
535 TDSOP_PROCESS, &dst->bufcount);
536
537 /* advance src by samples consumed */
538 if (dst->bufcount > 0)
539 dsp_advance_buffer32(src, dst->bufcount);
540 }
541 /* else purged dsp_outbuf */
542
543 dst->remcount = count;
544
545 /* inherit in-place processed mask from source buffer */
546 dst->proc_mask = src->proc_mask;
547
548 (void)this;
436} 549}
437 550
438int tdspeed_doit(int32_t *src[], int count) 551/* Process format changes and settings changes */
552static void tdspeed_process_new_format(struct dsp_proc_entry *this,
553 struct dsp_buffer **buf_p)
439{ 554{
440 dsp_src = src; 555 struct dsp_buffer *src = *buf_p;
441 count = tdspeed_apply( (int32_t *[2]) { outbuf[0], outbuf[1] }, 556 struct dsp_buffer *dst = &dsp_outbuf;
442 src, count, 0, TDSPEED_OUTBUFSIZE); 557
558 if (dst->remcount > 0)
559 {
560 *buf_p = dst;
561 return; /* output remains from an earlier call */
562 }
443 563
444 src[0] = outbuf[0]; 564 DSP_PRINT_FORMAT(DSP_PROC_TIMESTRETCH, DSP_PROC_TIMESTRETCH, src->format);
445 src[1] = outbuf[1]; 565
566 struct tdspeed_state_s *st = &tdspeed_state;
567 struct dsp_config *dsp = st->dsp;
568 struct sample_format *format = &src->format;
569 unsigned int channels = format->num_channels;
570
571 if (format->codec_frequency != st->samplerate)
572 {
573 /* relevent parameters are changing - all overlap will be discarded */
574 st->channels = channels;
575
576 DEBUGF(" DSP_PROC_TIMESTRETCH- new settings: "
577 "ch:%u chz: %u, %d.%02d%%\n",
578 channels,
579 format->codec_frequency,
580 st->factor / 100, st->factor % 100);
581 bool active = tdspeed_update(format->codec_frequency, st->factor);
582 dsp_proc_activate(dsp, DSP_PROC_TIMESTRETCH, active);
583
584 if (!active)
585 {
586 DEBUGF(" DSP_PROC_RESAMPLE- not active\n");
587 dst->format = src->format; /* Keep track */
588 return; /* no more for now */
589 }
590 }
591 else if (channels != st->channels)
592 {
593 /* channel count transistion - have to make old data in overlap
594 buffer compatible with new format */
595 DEBUGF(" DSP_PROC_TIMESTRETCH- new ch count: %u=>%u\n",
596 st->channels, channels);
597
598 st->channels = channels;
599
600 if (channels > 1)
601 {
602 /* mono->stereo: Process the old mono as stereo now */
603 memcpy(st->ovl_buff[1], st->ovl_buff[0],
604 st->ovl_size * sizeof (int32_t));
605 }
606 else
607 {
608 /* stereo->mono: Process the old stereo as mono now */
609 for (int i = 0; i < st->ovl_size; i++)
610 {
611 st->ovl_buff[0][i] = st->ovl_buff[0][i] / 2 +
612 st->ovl_buff[1][i] / 2;
613 }
614 }
615 }
616
617 struct sample_format f = *format;
618 format_change_ack(format);
619
620 if (EQU_SAMPLE_FORMAT(f, dst->format))
621 {
622 DEBUGF(" DSP_PROC_TIMESTRETCH- same dst format\n");
623 format_change_ack(&f); /* nothing changed that matters downstream */
624 }
625
626 dst->format = f;
627
628 /* return to normal processing */
629 this->process[0] = tdspeed_process;
630 dsp_proc_call(this, buf_p, 0);
631}
632
633/* DSP message hook */
634static intptr_t tdspeed_configure(struct dsp_proc_entry *this,
635 struct dsp_config *dsp,
636 unsigned int setting,
637 intptr_t value)
638{
639 struct tdspeed_state_s *st = &tdspeed_state;
640
641 switch (setting)
642 {
643 case DSP_INIT:
644 /* everything is at 100% until dsp_set_timestretch is called with
645 some other value and timestretch is enabled at the time */
646 if (value == CODEC_IDX_AUDIO)
647 st->factor = PITCH_SPEED_100;
648 break;
649
650 case DSP_FLUSH:
651 tdspeed_flush();
652 break;
653
654 case DSP_PROC_INIT:
655 if (!tdspeed_alloc_buffers())
656 return -1; /* fail the init */
657
658 st->this = this;
659 st->dsp = dsp;
660 this->ip_mask = 0; /* not in-place */
661 this->process[0] = tdspeed_process;
662 this->process[1] = tdspeed_process_new_format;
663 break;
664
665 case DSP_PROC_CLOSE:
666 st->this = NULL;
667 st->factor = PITCH_SPEED_100;
668 dsp_outbuf.remcount = 0;
669 tdspeed_free_buffers();
670 break;
671
672 case TIMESTRETCH_SET_FACTOR:
673 /* force update as a format change */
674 st->samplerate = 0;
675 st->factor = (int32_t)value;
676 st->this->process[0] = tdspeed_process_new_format;
677 dsp_proc_activate(st->dsp, DSP_PROC_TIMESTRETCH, true);
678 break;
679 }
446 680
447 return count; 681 return 1;
682 (void)value;
448} 683}
449 684
685/* Database entry */
686DSP_PROC_DB_ENTRY(TIMESTRETCH,
687 tdspeed_configure);
diff --git a/lib/rbcodec/dsp/tdspeed.h b/lib/rbcodec/dsp/tdspeed.h
index e91eeb1701..ca8a7846a4 100644
--- a/lib/rbcodec/dsp/tdspeed.h
+++ b/lib/rbcodec/dsp/tdspeed.h
@@ -23,12 +23,8 @@
23#ifndef _TDSPEED_H 23#ifndef _TDSPEED_H
24#define _TDSPEED_H 24#define _TDSPEED_H
25 25
26#include "dsp.h" 26/* some #define functions to get the pitch, stretch and speed values based
27 27 * upon two known values. Remember that params are alphabetical. */
28#define TDSPEED_OUTBUFSIZE 4096
29
30/* some #define functions to get the pitch, stretch and speed values based on */
31/* two known values. Remember that params are alphabetical. */
32#define GET_SPEED(pitch, stretch) \ 28#define GET_SPEED(pitch, stretch) \
33 ((pitch * stretch + PITCH_SPEED_100 / 2L) / PITCH_SPEED_100) 29 ((pitch * stretch + PITCH_SPEED_100 / 2L) / PITCH_SPEED_100)
34#define GET_PITCH(speed, stretch) \ 30#define GET_PITCH(speed, stretch) \
@@ -36,14 +32,12 @@
36#define GET_STRETCH(pitch, speed) \ 32#define GET_STRETCH(pitch, speed) \
37 ((speed * PITCH_SPEED_100 + pitch / 2L) / pitch) 33 ((speed * PITCH_SPEED_100 + pitch / 2L) / pitch)
38 34
39void tdspeed_init(void);
40void tdspeed_finish(void);
41bool tdspeed_config(int samplerate, bool stereo, int32_t factor);
42long tdspeed_est_output_size(void);
43long tdspeed_est_input_size(long size);
44int tdspeed_doit(int32_t *src[], int count);
45
46#define STRETCH_MAX (250L * PITCH_SPEED_PRECISION) /* 250% */ 35#define STRETCH_MAX (250L * PITCH_SPEED_PRECISION) /* 250% */
47#define STRETCH_MIN (35L * PITCH_SPEED_PRECISION) /* 35% */ 36#define STRETCH_MIN (35L * PITCH_SPEED_PRECISION) /* 35% */
48 37
49#endif 38void dsp_timestretch_enable(bool enable);
39void dsp_set_timestretch(int32_t percent);
40int32_t dsp_get_timestretch(void);
41bool dsp_timestretch_available(void);
42
43#endif /* _TDSPEED_H */
diff --git a/lib/rbcodec/dsp/tone_controls.c b/lib/rbcodec/dsp/tone_controls.c
new file mode 100644
index 0000000000..0bd4a447d7
--- /dev/null
+++ b/lib/rbcodec/dsp/tone_controls.c
@@ -0,0 +1,118 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2007 Thom Johansen
11 * Copyright (C) 2012 Michael Sevakis
12 *
13 * This program is free software; you can redistribute it and/or
14 * modify it under the terms of the GNU General Public License
15 * as published by the Free Software Foundation; either version 2
16 * of the License, or (at your option) any later version.
17 *
18 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
19 * KIND, either express or implied.
20 *
21 ****************************************************************************/
22#include "config.h"
23#include "system.h"
24#include "dsp.h"
25#include <string.h>
26#include "dsp_proc_entry.h"
27#include "dsp_filter.h"
28
29/* These apply to all DSP streams to remain as consistant as possible with
30 * the behavior of hardware tone controls */
31
32/* Cutoffs in HZ - not adjustable for now */
33static const unsigned int tone_bass_cutoff = 200;
34static const unsigned int tone_treble_cutoff = 3500;
35
36/* Current bass and treble gain values */
37static int tone_bass = 0;
38static int tone_treble = 0;
39
40/* Data for each DSP */
41static struct dsp_filter tone_filters[DSP_COUNT] IBSS_ATTR;
42
43/* Update the filters' coefficients based upon the bass/treble settings */
44void tone_set_prescale(int prescale)
45{
46 int bass = tone_bass;
47 int treble = tone_treble;
48
49 struct dsp_filter tone_filter; /* Temp to hold new version */
50 filter_bishelf_coefs(0xffffffff / NATIVE_FREQUENCY * tone_bass_cutoff,
51 0xffffffff / NATIVE_FREQUENCY * tone_treble_cutoff,
52 bass, treble, -prescale, &tone_filter);
53
54 struct dsp_config *dsp;
55 for (int i = 0; (dsp = dsp_get_config(i)); i++)
56 {
57 struct dsp_filter *filter = &tone_filters[i];
58 filter_copy(filter, &tone_filter);
59
60 bool enable = bass != 0 || treble != 0;
61 dsp_proc_enable(dsp, DSP_PROC_TONE_CONTROLS, enable);
62
63 if (!dsp_proc_active(dsp, DSP_PROC_TONE_CONTROLS))
64 {
65 filter_flush(filter); /* Going online */
66 dsp_proc_activate(dsp, DSP_PROC_TONE_CONTROLS, true);
67 }
68 }
69}
70
71/* Prescaler is always set after setting bass/treble, so we wait with
72 * calculating coefs until such time. */
73
74/* Change the bass setting */
75void tone_set_bass(int bass)
76{
77 tone_bass = bass;
78}
79
80/* Change the treble setting */
81void tone_set_treble(int treble)
82{
83 tone_treble = treble;
84}
85
86/* Apply the tone control filter in-place */
87static void tone_process(struct dsp_proc_entry *this,
88 struct dsp_buffer **buf_p)
89{
90 struct dsp_buffer *buf = *buf_p;
91 filter_process((void *)this->data, buf->p32, buf->remcount,
92 buf->format.num_channels);
93}
94
95/* DSP message hook */
96static intptr_t tone_configure(struct dsp_proc_entry *this,
97 struct dsp_config *dsp,
98 unsigned int setting,
99 intptr_t value)
100{
101 switch (setting)
102 {
103 case DSP_PROC_INIT:
104 if (value != 0)
105 break;
106 this->data = (intptr_t)&tone_filters[dsp_get_id(dsp)];
107 this->process[0] = tone_process;
108 case DSP_FLUSH:
109 filter_flush((struct dsp_filter *)this->data);
110 break;
111 }
112
113 return 1;
114}
115
116/* Database entry */
117DSP_PROC_DB_ENTRY(TONE_CONTROLS,
118 tone_configure);
diff --git a/lib/rbcodec/dsp/tone_controls.h b/lib/rbcodec/dsp/tone_controls.h
new file mode 100644
index 0000000000..1e27ecf800
--- /dev/null
+++ b/lib/rbcodec/dsp/tone_controls.h
@@ -0,0 +1,28 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2007 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef TONE_CONTROLS_H
22#define TONE_CONTROLS_H
23
24void tone_set_prescale(int prescale);
25void tone_set_bass(int bass);
26void tone_set_treble(int treble);
27
28#endif /* TONE_CONTROLS_H */
generated by cgit v1.2.3 (git 2.39.1) at 2024-10-05 21:34:03 +0000