static NOINLINE void Rescale(planes data, fix_t v, int ramp, size_t length, int channels)
{
fix_t dv;
int n;
if (length < 4*sizeof(fix_t)) return;
while ((sizeof(fix_t)<<ramp) > length) --ramp;
#ifdef FIXED_POINT
dv = (v-FIXC(1)) >> ramp;
#else
dv = (fix_t)ldexp(v-FIXC(1),-ramp);
#endif
for (n=0;n<channels;++n)
{
fix_t* i=data[n];
fix_t* ie=i+(1<<ramp);
v = FIXC(1);
for (;i!=ie;++i,v+=dv)
i[0] = fix_mul(i[0],v);
ie = (fix_t*)((uint8_t*)ie+length)-(1<<ramp);
for (;i!=ie;i+=4)
{
i[0] = fix_mul(i[0],v);
i[1] = fix_mul(i[1],v);
i[2] = fix_mul(i[2],v);
i[3] = fix_mul(i[3],v);
}
}
}
static NOINLINE void UpdateScale(equalizer* p)
{
fix_t Scale;
int n;
if (!p->Attenuate)
p->Scale=FIXC(1.);
if (p->Scale<FIXC(1./65556))
p->Scale=FIXC(1./65556);
Scale = fix_mul(p->Scale,p->ScalePreamp);
p->ScaleFinal = FIXFAST_BSHIFT(Scale);
for (n=0;n<MAXFILTER;++n)
p->Filter[n].alpha = ACCFAST_BSHIFT(fix_mul(p->Filter[n].alpha0,Scale));
}
static int UpdateInput(equalizer* p)
{
PCMRelease(p->PCM);
p->PCM = NULL;
BufferClear(&p->Buffer);
if (p->Codec.In.Format.Type == PACKET_AUDIO)
{
if (!p->Enabled || p->Codec.In.Format.Format.Audio.Channels>MAXPLANES)
return ERR_INVALID_PARAM;
PacketFormatPCM(&p->Codec.Out.Format,&p->Codec.In.Format,FIX_FRACBITS+1);
p->Codec.Out.Format.Format.Audio.Bits = sizeof(fix_t)*8;
p->Codec.Out.Format.Format.Audio.Flags = PCM_PLANES;
#ifndef FIXED_POINT
p->Codec.Out.Format.Format.Audio.Flags |= PCM_FLOAT;
#endif
p->PCM = PCMCreate(&p->Codec.Out.Format.Format.Audio,&p->Codec.In.Format.Format.Audio,0,0);
if (!p->PCM)
return ERR_OUT_OF_MEMORY;
p->Scale = FIXC(1);
UpdateParam(p);
Flush(p);
}
return ERR_NONE;
}
static void
extract_ycbcr709 (const guchar *src,
gint bpp,
gint numpix,
guchar **dst)
{
register const guchar *rgb_src = src;
register guchar *y_dst = dst[0];
register guchar *cb_dst = dst[1];
register guchar *cr_dst = dst[2];
register gint count = numpix, offset = bpp-3;
while (count-- > 0)
{
register int r, g, b;
r= *(rgb_src++);
g= *(rgb_src++);
b= *(rgb_src++);
*(y_dst++) = ( FIXY(0.2126)*r + FIXY(0.7152)*g + FIXY(0.0722)*b + FIX( 16.5))>>16;
*(cb_dst++) = (-FIXC(0.1150)*r - FIXC(0.3860)*g + FIXC(0.5000)*b + FIX(128.5))>>16;
*(cr_dst++) = ( FIXC(0.5000)*r - FIXC(0.4540)*g - FIXC(0.0460)*b + FIX(128.5))>>16;
rgb_src += offset;
}
}
static void
extract_ycbcr470 (const guchar *src,
gint bpp,
gint numpix,
guchar **dst)
{
register const guchar *rgb_src = src;
register guchar *y_dst = dst[0];
register guchar *cb_dst = dst[1];
register guchar *cr_dst = dst[2];
register gint count = numpix, offset = bpp-3;
while (count-- > 0)
{
register int r, g, b;
r= *(rgb_src++);
g= *(rgb_src++);
b= *(rgb_src++);
*(y_dst++) = ( FIXY(0.2989)*r + FIXY(0.5866)*g + FIXY(0.1145)*b + FIX( 16.5))>>16;
*(cb_dst++) = (-FIXC(0.1688)*r - FIXC(0.3312)*g + FIXC(0.5000)*b + FIX(128.5))>>16;
*(cr_dst++) = ( FIXC(0.5000)*r - FIXC(0.4184)*g - FIXC(0.0816)*b + FIX(128.5))>>16;
rgb_src += offset;
}
}
static NOINLINE int UpdateParam(equalizer* p)
{
if (p->Codec.In.Format.Type == PACKET_AUDIO)
{
int n;
const eqfilter *src;
eqfilter *dst;
src = Band44100;
dst = p->Filter;
for (n=0;n<MAXFILTER;++n,++src,++dst)
{
dst->alpha0 = fix_mul(src->alpha,Pow(p->Eq[n])-FIXC(1.));
dst->beta = ACCFAST_BSHIFT(src->beta);
dst->gamma = ACCFAST_BSHIFT(src->gamma);
}
p->ScalePreamp = Pow(p->Amplify);
UpdateScale(p);
}
return ERR_NONE;
}
static int Process(equalizer* p, const packet* Packet, const flowstate* State)
{
fix_t* s;
int n,DstLength,SrcLength,Channels;
if (!Packet)
return ERR_NEED_MORE_DATA;
Channels = p->Codec.In.Format.Format.Audio.Channels;
SrcLength = Packet->Length;
DstLength = PCMDstLength(p->PCM,SrcLength);
if (DstLength*Channels*2 > p->Buffer.Allocated)
{
uint8_t* Data;
if (!BufferAlloc(&p->Buffer,DstLength*Channels*2,1024))
return ERR_OUT_OF_MEMORY;
Data = p->Buffer.Data;
for (n=0;n<Channels;++n)
{
p->Codec.Packet.Data[n] = Data;
Data += DstLength;
p->Tmp[n] = Data;
Data += DstLength;
}
}
PCMConvert(p->PCM,*(planes*)&p->Codec.Packet.Data,Packet->Data,&DstLength,&SrcLength,SPEED_ONE,256);
p->Codec.Packet.RefTime = Packet->RefTime;
p->Codec.Packet.Length = DstLength;
s = p->State[0];
for (n=0;n<Channels;++n)
{
eqfilter* f;
fix_t* i=(fix_t*)p->Codec.Packet.Data[n];
fix_t* ie=(fix_t*)((uint8_t*)i+DstLength);
fix_t* j=(fix_t*)p->Tmp[n];
fix_t a,b,c,d,e;
e = p->ScaleFinal;
a = s[2];
b = s[1];
c = s[0];
for (ie-=3;i<ie;i+=4,j+=4)
{
d=i[0];
i[0]=fixfast_mul(c,e);
j[0]=ACCFAST_ASHIFT(c-a,ACCFAST_ADJUST);
a=i[1];
i[1]=fixfast_mul(d,e);
j[1]=ACCFAST_ASHIFT(d-b,ACCFAST_ADJUST);
b=i[2];
i[2]=fixfast_mul(a,e);
j[2]=ACCFAST_ASHIFT(a-c,ACCFAST_ADJUST);
c=i[3];
i[3]=fixfast_mul(b,e);
j[3]=ACCFAST_ASHIFT(b-d,ACCFAST_ADJUST);
}
for (ie+=3;i!=ie;++i,++j)
{
j[0]=ACCFAST_ASHIFT(c-a,ACCFAST_ADJUST);
a=b;
b=c;
c=i[0];
i[0]=fixfast_mul(b,e);
}
s[2] = a;
s[1] = b;
s[0] = c;
s += 3;
for (f=p->Filter;f!=p->Filter+MAXFILTER;f+=2,s+=2*2)
if (f[0].alpha!=0 || f[1].alpha!=0)
{
accfast_var(v0);
accfast_var(v1);
i=(fix_t*)p->Codec.Packet.Data[n];
ie=(fix_t*)((uint8_t*)i+DstLength);
j=(fix_t*)p->Tmp[n];
a = s[0];
c = s[1];
b = s[2];
d = s[3];
for (--ie;i<ie;i+=2,j+=2)
{
accfast_mul(v0,a,f[0].beta);
accfast_mul(v1,b,f[1].beta);
accfast_mla(v0,j[0],f[0].alpha);
accfast_mla(v1,j[0],f[1].alpha);
accfast_mla(v0,c,f[0].gamma);
accfast_mla(v1,d,f[1].gamma);
a = accfast_get(v0,ACCFAST_ADJUST);
b = accfast_get(v1,ACCFAST_ADJUST);
i[0] += a+b;
a = ACCFAST_ASHIFT(a,ACCFAST_ADJUST);
b = ACCFAST_ASHIFT(b,ACCFAST_ADJUST);
accfast_mul(v0,c,f[0].beta);
accfast_mul(v1,d,f[1].beta);
accfast_mla(v0,j[1],f[0].alpha);
accfast_mla(v1,j[1],f[1].alpha);
accfast_mla(v0,a,f[0].gamma);
accfast_mla(v1,b,f[1].gamma);
c = accfast_get(v0,ACCFAST_ADJUST);
d = accfast_get(v1,ACCFAST_ADJUST);
i[1] += c+d;
c = ACCFAST_ASHIFT(c,ACCFAST_ADJUST);
d = ACCFAST_ASHIFT(d,ACCFAST_ADJUST);
}
if (i==ie)
{
accfast_mul(v0,a,f[0].beta);
accfast_mul(v1,b,f[1].beta);
accfast_mla(v0,j[0],f[0].alpha);
accfast_mla(v1,j[0],f[1].alpha);
accfast_mla(v0,c,f[0].gamma);
accfast_mla(v1,d,f[1].gamma);
a = c; c = accfast_get(v0,ACCFAST_ADJUST);
b = d; d = accfast_get(v1,ACCFAST_ADJUST);
i[0] += c+d;
c = ACCFAST_ASHIFT(c,ACCFAST_ADJUST);
d = ACCFAST_ASHIFT(d,ACCFAST_ADJUST);
}
s[0] = a;
s[1] = c;
s[2] = b;
s[3] = d;
}
}
if (p->Attenuate)
{
fix_t Max = 0;
for (n=0;n<Channels;++n)
{
fix_t* i=(fix_t*)p->Codec.Packet.Data[n];
fix_t* ie=(fix_t*)((uint8_t*)i+DstLength);
for (;i!=ie;++i)
{
fix_t v = *i;
if (v<0) v=-v;
if (v>Max) Max=v;
}
}
if (Max > FIXC(1.))
{
Max = fix_mul(fix_1div(Max),FIXC(15./16));
Rescale(*(planes*)&p->Codec.Packet.Data,Max,5,DstLength,Channels);
p->Scale = fix_mul(p->Scale,Max);
UpdateScale(p);
}
else
if (Max < FIXC(6./8) && p->Scale < FIXC(1-1./256))
{
Max = FIXC(32./31);
if (fix_mul(p->Scale,Max) > FIXC(1))
Max = fix_1div(p->Scale);
Rescale(*(planes*)&p->Codec.Packet.Data,Max,10,DstLength,Channels);
p->Scale = fix_mul(p->Scale,Max);
UpdateScale(p);
}
}
else
if (p->Scale != FIXC(1.))
UpdateScale(p);
return ERR_NONE;
}
fix_t Scale;
int n;
if (!p->Attenuate)
p->Scale=FIXC(1.);
if (p->Scale<FIXC(1./65556))
p->Scale=FIXC(1./65556);
Scale = fix_mul(p->Scale,p->ScalePreamp);
p->ScaleFinal = FIXFAST_BSHIFT(Scale);
for (n=0;n<MAXFILTER;++n)
p->Filter[n].alpha = ACCFAST_BSHIFT(fix_mul(p->Filter[n].alpha0,Scale));
}
static const eqfilter Band44100[MAXFILTER] =
{
{ FIXC(0.003013),FIXC(-0.993973),FIXC( 1.993901) },
{ FIXC(0.008490),FIXC(-0.983019),FIXC( 1.982437) },
{ FIXC(0.015374),FIXC(-0.969252),FIXC( 1.967331) },
{ FIXC(0.029328),FIXC(-0.941343),FIXC( 1.934254) },
{ FIXC(0.047918),FIXC(-0.904163),FIXC( 1.884869) },
{ FIXC(0.130408),FIXC(-0.739184),FIXC( 1.582718) },
{ FIXC(0.226555),FIXC(-0.546889),FIXC( 1.015267) },
{ FIXC(0.344937),FIXC(-0.310127),FIXC(-0.181410) },
{ FIXC(0.366438),FIXC(-0.267123),FIXC(-0.521151) },
{ FIXC(0.379009),FIXC(-0.241981),FIXC(-0.808451) },
};
static const fix_t PowTab[37] = //pow(10,(n-18)/20.f))
{
FIXC(0.125892541),FIXC(0.141253754),FIXC(0.158489319),FIXC(0.177827941),
FIXC(0.199526231),FIXC(0.223872114),FIXC(0.251188643),FIXC(0.281838293),