話速変換機（速聴機）のファームウエア
話速変換機（速聴機）の全ファームウエアはかなりのステップになりますので、キーポイントとなる話速変換アルゴリズムを記載します。
機能フローの「モード選択」「話速変換再生(Ｓ再生）」に対応しています。
★メイン（モード選択）
#include <stdio.h>
#include <dsplib.h>
#include "vm.h"
#include "ui.h"
#include "fs.h"
#include "dsp.h"
#include "drv.h"
static int fs_init_p;
static int cur_sel;
extern void rec_init();
extern void rec();
extern void play_init();
extern void play_s();
extern void play_p();
extern void thru_init();
extern void thru();
extern void del_init();
extern void del();
static SETTINGS *settings;
static void init_settings()
{
if(fs_init_p)
{
settings = fs_read_settings();
dsp_set_ingain(settings->ingain);
dsp_set_outgain(settings->outgain);
dsp_set_preamp(settings->preamp);
dsp_set_speed(settings->speed);
dsp_set_pitch(settings->pitch);
dsp_set_geq(settings->geq);
}
}
static void update_settings()
{
if(fs_init_p)
{
settings->ingain = dsp_get_ingain();
settings->outgain = dsp_get_outgain();
settings->preamp = dsp_get_preamp();
settings->speed = dsp_get_speed();
settings->pitch = dsp_get_pitch();
dsp_get_geq(settings->geq);
fs_write_settings(settings);
}
}
void vm_save_settings()
{
update_settings();
}
int vm_format()
{
int yes_p;
yes_p = 0;
ui_menu_del(3,0);
ui_menu_yesno(yes_p);
while(1)
{
switch(ui_get_event())
{
case E_LEFT:
yes_p = 1;
ui_menu_yesno(yes_p);
break;
case E_RIGHT:
yes_p = 0;
ui_menu_yesno(yes_p);
break;
case E_OK:
if(yes_p)
{
lcd_begin_wait();
fs_format();
lcd_end_wait();
return 0;
}
else
return -1;
break;
}
}
}
void main()
{
int k;
int format_error;
drv_init();
ui_init();
dsp_init();
rec_init();
play_init();
thru_init();
del_init();
cur_sel = 0;
ui_menu_main(0,0);
for(k=0;k<10000;k++)
{
asm(" NOP");
asm(" NOP");
asm(" NOP");
}
dsp_disable_output();
dsp_disable_input();
fs_init_p = 0;
format_error = 0;
while(1)
{
if(!cf_exists_p())
{
fs_init_p = 0;
format_error = 0;
}
if(!cf_exists_p() || format_error)
{
ui_show_warn(WARN_NOCARD);
fs_init_p = 0;
ui_menu_main(cur_sel,0);
}
else
{
if(fs_init_p == 0)
{
if(fs_init() && vm_format())
{
format_error = 1;
}
else
{
fs_init_p = 1;
init_settings();
}
}
if(fs_full_p())
{
ui_show_warn(WARN_CARDFULL);
ui_menu_main(cur_sel,2);
}
else if(99 <= fs_max_track())
{
ui_show_warn(WARN_TRACKFULL);
ui_menu_main(cur_sel,2);
}
else
{
ui_show_warn(WARN_NONE);
ui_menu_main(cur_sel,1);
}
}
switch(ui_get_event())
{
case E_DOWN:
if(cur_sel < 4)
{
cur_sel++;
ui_menu_main(cur_sel,fs_init_p);
}
break;
case E_UP:
if(0 < cur_sel)
{
cur_sel--;
ui_menu_main(cur_sel,fs_init_p);
}
break;
case E_OK:
switch(cur_sel)
{
case 0:
if(fs_init_p && fs_max_track() < 99)
{
rec();
update_settings();
ui_menu_main(cur_sel,fs_init_p);
}
break;
case 1:
if(fs_init_p)
{
play_s();
update_settings();
ui_menu_main(cur_sel,fs_init_p);
}
break;
case 2:
if(fs_init_p)
{
play_p();
update_settings();
ui_menu_main(cur_sel,fs_init_p);
}
break;
case 3:
if(fs_init_p)
{
del();
update_settings();
ui_menu_main(cur_sel,fs_init_p);
}
break;
case 4:
thru();
update_settings();
ui_menu_main(cur_sel,fs_init_p);
break;
}
break;
}
}
}★話速変換
#include <string.h>
#include "ui.h"
#include "fs.h"
#include "dsp.h"
static int cur_sel; // 0:main 1:start/stop 2:vol 3:speed 4:filter
static int playing_p;
static int cur_vol;
static int cur_speed;
static int cur_filter;
static int exit_p;
static void update_volume()
{
dsp_set_outgain(cur_vol);
ui_menu_play(2,cur_vol,playing_p);
}
static void update_speed()
{
dsp_set_speed(cur_speed);
ui_menu_play(3,cur_speed,playing_p);
}
static void update_filter()
{
dsp_set_filter(cur_filter);
ui_menu_play(4,cur_filter,playing_p);
}
static void update_menu()
{
int p;
switch(cur_sel)
{
case 0: p = 0; break;
case 1: p = 0; break;
case 2: p = cur_vol; break;
case 3: p = cur_speed; break;
case 4: p = cur_filter; break;
}
ui_menu_play(cur_sel,p,playing_p);
}
static void play_down()
{
if(cur_sel < 4)
{
cur_sel++;
update_menu();
}
}
static void play_up()
{
if(0 < cur_sel)
{
cur_sel--;
update_menu();
}
}
static void play_left()
{
switch(cur_sel)
{
case 2:
if(-42 < cur_vol)
{
cur_vol -= 2;
update_volume();
}
break;
case 3:
if(5 < cur_speed)
{
cur_speed--;
update_speed();
}
break;
case 4:
if(0 < cur_filter)
{
cur_filter--;
update_filter();
}
break;
}
}
static void play_right()
{
switch(cur_sel)
{
case 2:
if(cur_vol < 20)
{
cur_vol += 2;
update_volume();
}
break;
case 3:
if(cur_speed < 40)
{
cur_speed++;
update_speed();
}
break;
case 4:
if(cur_filter < 8)
{
cur_filter++;
update_filter();
}
break;
}
}
static short *sig;
static void play_ok()
{
switch(cur_sel)
{
case 0:
exit_p = 1;
break;
case 1:
if(playing_p)
{
playing_p = 0;
dsp_disable_output();
}
else
{
sig = NULL;
fs_seek(0);
dsp_enable_output();
playing_p = 1;
}
update_menu();
break;
}
}
void play()
{
cur_sel = 0;
playing_p = 0;
cur_vol = dsp_get_outgain();
cur_speed = 10;
cur_filter = dsp_get_filter();
exit_p = 0;
ui_menu_play(cur_sel,0,0);
dsp_disable_output();
dsp_disable_input();
dsp_set_pitch(0);
dsp_set_speed(10);
while(!exit_p)
{
switch(ui_get_event())
{
case E_UP: play_up(); break;
case E_DOWN: play_down(); break;
case E_LEFT: play_left(); break;
case E_RIGHT: play_right(); break;
case E_OK: play_ok(); break;
}
if(playing_p)
{
if(dsp_putsig(sig) == 0)
{
sig = fs_read();
if(sig)
{
dsp(sig);
ui_show_level(dsp_get_level() >> 4);
}
else
{
playing_p = 0;
dsp_disable_output();
update_menu();
}
}
}
else
ui_show_level(0);
}
dsp_disable_output();
dsp_disable_input();
}
★ＤＳＰ
#include <dsplib.h>
#include "vm.h"
#include "codec.h"
#include "dsp.h"
const int tbl_hanning[] = {
0,0,5,11,20,31,45,61,79,100,124,150,178,209,242,278,
316,357,400,445,493,543,596,651,708,768,830,895,961,1031,1102,1176,
1252,1330,1411,1494,1579,1666,1756,1848,1942,2038,2137,2237,2340,
2445,2552,2661,
2772,2885,3000,3117,3236,3358,3481,3606,3733,3862,3993,4125,4260,
4396,4535,4675,
4816,4960,5105,5252,5401,5551,5703,5857,6012,6169,6327,6487,6648,
6811,6975,7140,
7308,7476,7646,7817,7989,8163,8338,8514,8691,8869,9049,9230,9411,
9594,9778,9963,
10149,10335,10523,10712,10901,11091,11282,11474,11667,11860,12054,
12249,12444,12640,12836,13033,
13230,13428,13627,13826,14025,14224,14424,14624,14825,15025,15226,
15427,15628,15830,16031,16232,
16535,16736,16937,17139,17340,17541,17742,17942,18143,18343,18543,
18742,18941,19140,19339,19537,
19734,19931,20127,20323,20518,20713,20907,21100,21293,21485,21676,
21866,22055,22244,22432,22618,
22804,22989,23173,23356,23537,23718,23898,24076,24253,24429,24604,
24778,24950,25121,25291,25459,
25627,25792,25956,26119,26280,26440,26598,26755,26910,27064,27216,
27366,27515,27662,27807,27951,
28092,28232,28371,28507,28642,28774,28905,29034,29161,29286,29409,
29531,29650,29767,29882,29995,
30106,30215,30322,30427,30530,30630,30729,30825,30919,31011,31101,
31188,31273,31356,31437,31515,
31591,31665,31736,31806,31872,31937,31999,32059,32116,32171,32224,
32274,32322,32367,32410,32451,
32489,32525,32558,32589,32617,32643,32667,32688,32706,32722,32736,
32747,32756,32762,32767,32767
};
static int enable_p;
static int cur_preamp;
static int cur_ingain;
static int cur_outgain;
static int cur_pitch;
static int cur_filter;
static int cur_speed;
#pragma DATA_SECTION(fftwrk,"fftbuf")
static short fftwrk[BLOCKSIZE2];
static short blurdly[BLOCKSIZE];
static short blurdlyw[BLOCKSIZE];
#pragma DATA_SECTION(_delay_buf,"dbuf")
#pragma DATA_SECTION(_delay_buf2,"dbuf")
static short _delay_buf[32];
static short *delay_buf = &_delay_buf[0];
static short _delay_buf2[16];
static short *delay_buf2 = &_delay_buf2[0];
#include "coef.c"
#pragma DATA_SECTION(geq_coef, "filt_coef")
static short geq_coef[32];
static short* filter_coef[9] = {
coef_0,
coef_1,
coef_2,
coef_3,
coef_4,
coef_5,
coef_6,
coef_7,
coef_8
};
static void clr_dbuf()
{
int i;
for(i=0;i<BLOCKSIZE2;i++) fftwrk[i] = 0;
for(i=0;i<BLOCKSIZE;i++) blurdly[i] = 0;
for(i=0;i<BLOCKSIZE;i++) blurdlyw[i] = 0;
for(i=0;i<16;i++) _delay_buf[i] = 0;
delay_buf = &_delay_buf[0];
for(i=0;i<16;i++) _delay_buf2[i] = 0;
delay_buf2 = &_delay_buf2[0];
}
unsigned long magn[BLOCKSIZE];
void group(short *sig, short *g, short *p)
{
short k;
short l,d,n;
magn[0] = ((long)sig[0] * (long)sig[0]);
for(k=1;k<BLOCKSIZE-1;k++)
magn[k] = ((long)sig[2*k ] * (long)sig[2*k ]
+ (long)sig[2*k+1] * (long)sig[2*k+1]);
magn[k] = ((long)sig[1] * (long)sig[1]);
g[0] = p[0] = n = 0;
for(k=2;k<BLOCKSIZE-2;k++)
{
if(magn[k-2] < magn[k] && magn[k-1] < magn[k] &&
magn[k] > magn[k+1] && magn[k] > magn[k+2])
{
p[n] = k;
n++;
}
}
for(k=l=0;k<n-1;k++)
{
d = (p[k] + p[k+1]) / 2 - l;
memset(&g[l],k,d);
l += d;
}
for(;l<BLOCKSIZE;l++)
g[l] = k;
}
int pc;
int pswrk[BLOCKSIZE2];
void pitch_shift_s(short s, short *sig, short *g, short *p)
{
int k;
int idx;
int rp;
int ip;
memset(pswrk,0,BLOCKSIZE2);
for(k=2;k<BLOCKSIZE;k++)
{
idx = (((long)s * (long)p[g[k]] + 8192) >> 14)
+ (k - p[g[k]]);
rp = sig[2*k+0];
ip = sig[2*k+1];
if(BLOCKSIZE - 32 < idx)
continue;
if(idx < 4)
continue;
if((pc & 0x01) && ((idx - k) & 0x01))
{
rp *= -1;
ip *= -1;
}
pswrk[2*idx+0] += rp;
pswrk[2*idx+1] += ip;
}
for(k=0;k<8;k++) pswrk[k] = sig[k];
memcpy(sig,pswrk,BLOCKSIZE2);
pc++;
}
void pitch_shift(short s, short *sig, short *g, short *p)
{
int k;
int idx;
int rp;
int ip;
memset(pswrk,0,BLOCKSIZE2);
for(k=2;k<BLOCKSIZE;k++)
{
idx = (((long)s * (long)p[g[k]] + 8192) >> 14)
+ (k - p[g[k]]);
rp = sig[2*k+0];
ip = sig[2*k+1];
if(BLOCKSIZE < idx)
{
idx = 2*BLOCKSIZE - idx;
ip *= -1;
}
if(idx < 0)
{
idx *= -1;
ip *= -1;
}
if((pc & 0x01) && ((idx - k) & 0x01))
{
rp *= -1;
ip *= -1;
}
if(BLOCKSIZE == idx)
{
pswrk[1] += rp < ip ? ip : rp;
continue;
}
if(idx == 0)
{
pswrk[0] += rp < ip ? ip : rp;
continue;
}
pswrk[2*idx+0] += rp;
pswrk[2*idx+1] += ip;
}
memcpy(sig,pswrk,BLOCKSIZE2);
pc++;
}
short geq_values[32];
short geq_parm[16];
void filter(short *sig, int f)
{
int k;
for(k=0; k < BLOCKSIZE; k++)
{
sig[2*k] = ((long)geq_values[k/8]
* (long)sig[2*k]) >> 15;
sig[2*k+1] = ((long)geq_values[k/8]
* (long)sig[2*k+1]) >> 15;
}
}
short pwrk[BLOCKSIZE2];
short gwrk[BLOCKSIZE2];
short pstbl[25] = {
8192, 8679, 9195, 9741, 10321, 10934,
11585, 12273, 13003, 13776, 14596, 15463,
16383, 17357, 18389, 19483, 20641, 21869,
23169, 24547, 26006, 27553, 29191, 30927,
32766
};
short sctbl[45] = {
0,0,0,0,0,
32766,27306,23404,20480,18204,
16383,14894,13652,12602,11702,
10922,10239,9637,9102,8623,
16383,16383,14894,14894,13652,
13652,12602,12602,11702,11702,
10922,10922,10239,10239,9637,
9637,9102,9102,8623,8623,
16383,12970,10922,9557,8192
};
void dsp_0(short *sig)
{
int i;
for(i=0;i<BLOCKSIZE;i++)
sig[i] >>= 2;
if(cur_speed < 7)
fir(sig, coef_noise_filter_1, sig,
&delay_buf2, 4, BLOCKSIZE);
if(30 <= cur_speed)
fir(sig, coef_hpf,sig,&delay_buf2,4,BLOCKSIZE);
}
void dbxcomp(short* buf)
{
int k;
int neg;
for(k=0;k<BLOCKSIZE;k++)
{
neg = buf[k] < 0 ? 1 : 0;
buf[k] = neg ? -buf[k] : buf[k];
if(buf[k] < 8192)
buf[k] = 15210 * (long)buf[k] / 8192;
else if(buf[k] < 16384)
buf[k] = 1174 * (long)(buf[k] - 8192)
/ 8192 + 15210;
else
buf[k] = 1166 * (long)(buf[k] - 16384)
/ 16384 + 16384;
buf[k] = neg ? -buf[k] : buf[k];
}
}
void dbxexp(short* buf)
{
int k;
int neg;
for(k=0;k<BLOCKSIZE;k++)
{
neg = buf[k] < 0 ? 1 : 0;
buf[k] = neg ? -buf[k] : buf[k];
if(buf[k] < 15210)
buf[k] = (long)8192 * (long)buf[k]
/ (long)15210;
else if(buf[k] < 16384)
buf[k] = (long)8192 * (long)(buf[k] - 15210)
/ (long)1174 + 8192;
else
buf[k] = (long)16384 * (long)(buf[k] - 16384)
/ (long)1166 + 16384;
buf[k] = neg ? -buf[k] : buf[k];
}
}
void dsp_test(short* sig)
{
int i;
memcpy(fftwrk,blurdly,BLOCKSIZE);
memcpy(&fftwrk[BLOCKSIZE],sig,BLOCKSIZE);
memcpy(blurdly,sig,BLOCKSIZE);
cbrev(fftwrk,fftwrk,BLOCKSIZE2/2);
rfft(fftwrk,BLOCKSIZE2,1);
unpacki(fftwrk,BLOCKSIZE2);
cbrev(fftwrk,fftwrk,BLOCKSIZE2/2);
rifft(fftwrk,BLOCKSIZE2,0);
for(i=0;i<BLOCKSIZE;i++)
fftwrk[i] = ((long)fftwrk[i] * (long)tbl_hanning[i]) >> 15;
for(i=0;i<BLOCKSIZE;i++)
fftwrk[BLOCKSIZE+i] = ((long)fftwrk[BLOCKSIZE+i]
* (long)tbl_hanning[BLOCKSIZE-1-i]) >> 15;
for(i=0;i<BLOCKSIZE;i++)
sig[i] = blurdlyw[i] + fftwrk[i];
dbxexp(sig);
memcpy(blurdlyw,&fftwrk[BLOCKSIZE],BLOCKSIZE);
}
void dsp(short* sig)
{
int i;
if(cur_speed < 10 || 0 < cur_pitch)
fir(sig, coef_noise_filter_1, sig, &delay_buf2, 4, BLOCKSIZE);
if(30 <= cur_speed || 0 > cur_pitch)
fir(sig, coef_hpf,sig,&delay_buf2,4,BLOCKSIZE);
memcpy(fftwrk,blurdly,BLOCKSIZE);
memcpy(&fftwrk[BLOCKSIZE],sig,BLOCKSIZE);
memcpy(blurdly,sig,BLOCKSIZE);
cbrev(fftwrk,fftwrk,BLOCKSIZE2/2);
rfft(fftwrk,BLOCKSIZE2,1);
filter(fftwrk,cur_filter);
group(fftwrk,gwrk,pwrk);
if(cur_pitch)
pitch_shift_s(pstbl[cur_pitch+12],fftwrk,gwrk,pwrk);
if(cur_speed != 10)
pitch_shift_s(sctbl[cur_speed],fftwrk,gwrk,pwrk);
unpacki(fftwrk,BLOCKSIZE2);
cbrev(fftwrk,fftwrk,BLOCKSIZE2/2);
rifft(fftwrk,BLOCKSIZE2,0);
for(i=0;i<BLOCKSIZE;i++)
fftwrk[i] = ((long)fftwrk[i] * (long)tbl_hanning[i]) >> 15;
for(i=0;i<BLOCKSIZE;i++)
fftwrk[BLOCKSIZE+i] = ((long)fftwrk[BLOCKSIZE+i]
* (long)tbl_hanning[BLOCKSIZE-1-i]) >> 15;
for(i=0;i<BLOCKSIZE;i++)
sig[i] = blurdlyw[i] + fftwrk[i];
memcpy(blurdlyw,&fftwrk[BLOCKSIZE],BLOCKSIZE);
}
void dsp_init()
{
int k;
cur_preamp = 12;
cur_ingain = 0;
cur_outgain = -10;
cur_pitch = 0;
cur_filter = 1;
cur_speed = 10;
for(k=0;k<32;k++)
geq_values[k] = 0x7fff;
for(k=0;k<16;k++)
geq_parm[k] = 7;
for(k=0;k<32;k++)
geq_coef[k] = 0;
geq_coef[0] = 32767;
clr_dbuf();
codec_init();
enable_p = 0;
}
void dsp_on()
{
clr_dbuf();
codec_init();
dsp_set_outgain(cur_outgain);
enable_p = 1;
}
void dsp_off()
{
codec_disable_rx();
codec_disable_tx();
enable_p = 0;
}
int dsp_enable_p()
{
return enable_p;
}
void dsp_enable_input()
{
clr_dbuf();
codec_enable_rx();
}
void dsp_disable_input()
{
codec_disable_rx();
}
void dsp_enable_output()
{
clr_dbuf();
codec_enable_tx();
}
void dsp_disable_output()
{
codec_disable_tx();
}
int dsp_putsig_ready_p()
{
return codec_putsig_ready_p();
}
int dsp_getsig_ready_p()
{
return codec_getsig_ready_p();
}
int dsp_putsig(short* sig)
{
if(sig)
return codec_putsig(sig);
else
return 0;
}
short* dsp_getsig()
{
return codec_getsig();
}
void dsp_set_preamp(int g)
{
cur_preamp = g;
switch(g)
{
case 0:
codec_set_reg(5,0xb8 | 0);
break;
case 6:
codec_set_reg(5,0xb8 | 1);
break;
case 12:
codec_set_reg(5,0xb8 | 2);
break;
case 24:
codec_set_reg(5,0xb8 | 3);
break;
default:
cur_preamp = 12;
codec_set_reg(5,0xb8 | 2);
break;
}
}
int dsp_get_preamp()
{
return cur_preamp;
}
void dsp_set_ingain(int g)
{
if(g < -42) g = -42;
if(21 < g) g = 20;
cur_ingain = g;
codec_set_reg(5,0x3f & (g + 42));
}
int dsp_get_ingain()
{
return cur_ingain;
}
void dsp_set_outgain(int g)
{
if(g < -42) g = -42;
if(21 < g) g = 20;
cur_outgain = g;
codec_set_reg(5,0x40 | (0x3f & (g + 42)));
}
void dsp_mute()
{
codec_set_reg(5,0x7f);
}
void dsp_unmute()
{
codec_set_reg(5,0x40 | (0x3f & (cur_outgain + 42)));
}
int dsp_get_outgain()
{
return cur_outgain;
}
void dsp_set_filter(int f)
{
cur_filter = f;
}
int dsp_get_filter()
{
return cur_filter;
}
void dsp_set_pitch(int p)
{
cur_pitch = p;
}
int dsp_get_pitch()
{
return cur_pitch;
}
static const int speed_ctrl_mtbl[45] = {
0,0,0,0,0,
10,10,10,10,10,
10,10,10,10,10,
10,10,10,10,10,
10,10,10,10,10,
10,10,10,10,10,
10,10,10,10,10,
10,10,10,10,10,
10,19,16,14,12
};
static const int speed_ctrl_mtbl[45] = {
0,0,0,0,0,
8,8,8,8,8,
8,8,8,8,8,
8,8,8,8,8,
8,8,8,8,8,
8,8,8,8,8,
8,8,8,8,8,
8,8,8,8,8,
8,19,16,14,12
};
static const int speed_ctrl_mtbl[45] = {
13,13,13,13,13,
13,13,13,13,13,
13,13,13,13,13,
13,13,13,13,13,
13,13,13,13,13,
13,13,13,13,13,
13,13,13,13,13,
13,13,13,13,13,
13,13,13,13,13
};
static const int speed_ctrl_mtbl[45] = {
9,9,9,9,9,
9,9,9,9,9,
9,9,9,9,9,
9,9,9,9,9,
9,9,9,9,9,
9,9,9,9,9,
9,9,9,9,9,
9,9,9,9,9,
9,9,9,9,9
};
static const int speed_ctrl_mtbl[45] = {
0,0,0,0,0,
24,21,18,16,14,
12,22,21,19,18,
17,16,15,14,13,
12,24,22,22,21,
21,19,19,18,18,
17,17,16,16,15,
15,14,14,13,13,
12,19,16,14,12
};
static const int pitch_speed_mtbl[13] = {
46, 49, 52, 55, 58, 61, 65, 69, 73, 77, 82, 87, 92
};
void set_mnp(int m, int n, int p)
{
codec_set_reg(4,0x80 | m);
codec_set_reg(4,n << 3 | p);
}
void dsp_set_pitch_speed(int p)
{
codec_set_reg(4,0x80 | pitch_speed_mtbl[p + 6]);
}
void dsp_set_speed(int s)
{
if(s == 0)
{
codec_set_reg(4,0x80 | CODEC_M);
cur_speed = 10;
}
else
{
codec_set_reg(4,0x80 | CODEC_M);
cur_speed = s;
}
}
int dsp_get_speed()
{
return cur_speed;
}
int dsp_calc_level(short* sig)
{
unsigned long x;
int level;
int k;
x = 0;
for(k=0;k<BLOCKSIZE;k++)
x += ((long)sig[k] * (long)sig[k]) >> 15;
for(level = 31; 0 <= level; level--)
{
if(0x80000000 & x)
return level;
x <<= 1;
}
return 0;
}
static int conv(int nx, short* x, int ny, short* y, short* ans)
{
int k;
int s;
int n;
n = (nx-1)+(ny-1)+1;
for(k=0;k<n;k++)
{
ans[k] = 0;
for(s=0;s<=k;s++)
{
ans[k] += (long)(s < nx ? x[s] : 0)
* (long)(k-s < ny ? y[k - s] : 0) >> 15;
}
}
return n;
}
static const short cosine[16] = {
32767,
32137,
30273,
27245,
23170,
18204,
12539,
6393,
0,
-6393,
-12539,
-18204,
-23170,
-27245,
-30273,
-32137
};
static void root_to_coef(short* r, short* a)
{
short t[32];
short x[4];
short na;
short k;
a[0] = (long)r * (long)r >> 15;
a[1] = -2 * (long)cosine[0] * (long)r;
a[2] = 1;
na = 3;
for(k=1; k < 16; k++)
{
x[0] = (long)r * (long)r;
x[1] = -2 * (long)cosine[k] * (long)r;
x[2] = 1;
na = conv(3,x, na, a, t);
memcpy(a,t,na);
}
}
static const short geq_vtbl[8] = {
0x1000,
0x2000,
0x3000,
0x4000,
0x5000,
0x6000,
0x7000,
0x7fff
};
static setup_geq_filter(short* g)
{
short r[16];
int k;
for(k=0;k<16;k++)
r[k] = geq_vtbl[g[k]];
root_to_coef(r,geq_coef);
}
void dsp_set_geq(short* values)
{
int k;
int n;
switch(cur_speed)
{
case 9:
n = 15;
break;
case 8:
n = 14;
break;
case 7:
n = 13;
break;
case 6:
n = 12;
break;
case 5:
n = 11;
break;
default:
n = 16;
break;
}
for(k=0;k<n;k++)
{
if(values[k] < 0)
geq_parm[k] = 0;
else if(values[k] > 7)
geq_parm[k] = 7;
else
geq_parm[k] = values[k];
}
for(k=0;k<15;k++)
{
geq_values[2*k+2] = geq_values[2*k+1]
= ((long)geq_vtbl[values[k]]
* (long)geq_vtbl[values[k+1]]) >> 15;
}
geq_values[0] = geq_vtbl[values[0]];
geq_values[31] = geq_vtbl[values[15]];
}
void dsp_get_geq(short* values)
{
memcpy(values,geq_parm,sizeof(geq_parm));
if(cur_speed <= 9)
values[15] = 0;
if(cur_speed <= 8)
values[14] = 0;
if(cur_speed <= 7)
values[13] = 0;
if(cur_speed <= 6)
values[12] = 0;
if(cur_speed <= 5)
values[11] = 0;
}