FFT-то го направих, но за съжаление за 128 точки ми отнема 80ms при положение, че работя с PIC18F452 на 40Mhz. Дайте идея как да го оптимизирам

. Ето го и кода:
inline sint16 fix_mpy(sint16 a, sint16 b)
{
sint32 c;
if(!a)
return 0;
if(!b)
return 0;
c = ((sint32) a * (sint32) b) >> 14;
b = c & 0x01;
a = (c >> 1) + b;
return (sint16) a;
}
void fix_fft(sint16 fr[], sint16 fi[], sint16 m)
{
uint16 i_counter, j_counter, mr, nn, l, k, istep, n;
sint16 qr, qi, tr, ti, wr, wi;
n = 1 << m;
mr = 0;
nn = n - 1;
for(m = 1; m <= nn; ++m) {
l = n;
do {
l >>= 1;
} while(mr + l > nn);
mr = (mr & (l-1)) + l;
if(mr <= m)
continue;
tr = fr[m];
fr[m] = fr[mr];
fr[mr] = tr;
}
l = 1;
k = LOG2_N_WAVE-1;
while(l < n) {
istep = l << 1;
for(m = 0; m < l; ++m) {
j_counter = m << k;
wr = signed_shift(Sinewave[j_counter + signed_shift(N_WAVE, 2)], 1);
wi = signed_shift(-Sinewave[j_counter], 1);
for(i_counter = m; i_counter < n; i_counter += istep) {
j_counter = i_counter + l;
tr = fix_mpy(wr, fr[j_counter]) - fix_mpy(wi, fi[j_counter]);
ti = fix_mpy(wr, fi[j_counter]) + fix_mpy(wi, fr[j_counter]);
qr = signed_shift(fr[i_counter], 1);
qi = signed_shift(fi[i_counter], 1);
fr[j_counter] = qr - tr;
fi[j_counter] = qi - ti;
fr[i_counter] = qr + tr;
fi[i_counter] = qi + ti;
}
}
--k;
l = istep;
}
}