~noskcaj/ubuntu/saucy/sflphone/merge-1.2.3-2

* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

* Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifdef HAVE_CONFIG_H

#include "config.h"

#endif

#include "_kiss_fft_guts.h"

#include "arch.h"

#include "os_support.h"

/* The guts header contains all the multiplication and addition macros that are defined for

fixed or floating point complex numbers. It also delares the kf_ internal functions.

static void kf_bfly2(

kiss_fft_cpx * Fout,

const size_t fstride,

const kiss_fft_cfg st,

int m,

int N,

int mm

)

{

kiss_fft_cpx * Fout2;

kiss_fft_cpx * tw1;

kiss_fft_cpx t;

if (!st->inverse) {

int i,j;

kiss_fft_cpx * Fout_beg = Fout;

for (i=0;i<N;i++)

{

Fout = Fout_beg + i*mm;

Fout2 = Fout + m;

tw1 = st->twiddles;

for(j=0;j<m;j++)

{

/* Almost the same as the code path below, except that we divide the input by two

(while keeping the best accuracy possible) */

spx_word32_t tr, ti;

tr = SHR32(SUB32(MULT16_16(Fout2->r , tw1->r),MULT16_16(Fout2->i , tw1->i)), 1);

ti = SHR32(ADD32(MULT16_16(Fout2->i , tw1->r),MULT16_16(Fout2->r , tw1->i)), 1);

tw1 += fstride;

Fout2->r = PSHR32(SUB32(SHL32(EXTEND32(Fout->r), 14), tr), 15);

Fout2->i = PSHR32(SUB32(SHL32(EXTEND32(Fout->i), 14), ti), 15);

Fout->r = PSHR32(ADD32(SHL32(EXTEND32(Fout->r), 14), tr), 15);

Fout->i = PSHR32(ADD32(SHL32(EXTEND32(Fout->i), 14), ti), 15);

++Fout2;

++Fout;

}

} else {

int i,j;

kiss_fft_cpx * Fout_beg = Fout;

for (i=0;i<N;i++)

{

Fout = Fout_beg + i*mm;

Fout2 = Fout + m;

tw1 = st->twiddles;

for(j=0;j<m;j++)

{

C_MUL (t, *Fout2 , *tw1);

tw1 += fstride;

C_SUB( *Fout2 , *Fout , t );

C_ADDTO( *Fout , t );

++Fout2;

++Fout;

}

static void kf_bfly4(

kiss_fft_cpx * Fout,

const size_t fstride,

const kiss_fft_cfg st,

int m,

int N,

int mm

)

{

kiss_fft_cpx *tw1,*tw2,*tw3;

kiss_fft_cpx scratch[6];

const size_t m2=2*m;

const size_t m3=3*m;

int i, j;

100

101

if (st->inverse)

102

{

103

kiss_fft_cpx * Fout_beg = Fout;

104

for (i=0;i<N;i++)

105

{

106

Fout = Fout_beg + i*mm;

107

tw3 = tw2 = tw1 = st->twiddles;

108

for (j=0;j<m;j++)

109

{

110

C_MUL(scratch[0],Fout[m] , *tw1 );

111

C_MUL(scratch[1],Fout[m2] , *tw2 );

112

C_MUL(scratch[2],Fout[m3] , *tw3 );

113

114

C_SUB( scratch[5] , *Fout, scratch[1] );

115

C_ADDTO(*Fout, scratch[1]);

116

C_ADD( scratch[3] , scratch[0] , scratch[2] );

117

C_SUB( scratch[4] , scratch[0] , scratch[2] );

118

C_SUB( Fout[m2], *Fout, scratch[3] );

119

tw1 += fstride;

120

tw2 += fstride*2;

121

tw3 += fstride*3;

122

C_ADDTO( *Fout , scratch[3] );

123

124

Fout[m].r = scratch[5].r - scratch[4].i;

125

Fout[m].i = scratch[5].i + scratch[4].r;

126

Fout[m3].r = scratch[5].r + scratch[4].i;

127

Fout[m3].i = scratch[5].i - scratch[4].r;

128

++Fout;

129

}

130

}

131

} else

132

{

133

kiss_fft_cpx * Fout_beg = Fout;

134

for (i=0;i<N;i++)

135

{

136

Fout = Fout_beg + i*mm;

137

tw3 = tw2 = tw1 = st->twiddles;

138

for (j=0;j<m;j++)

139

{

140

C_MUL4(scratch[0],Fout[m] , *tw1 );

141

C_MUL4(scratch[1],Fout[m2] , *tw2 );

142

C_MUL4(scratch[2],Fout[m3] , *tw3 );

143

144

Fout->r = PSHR16(Fout->r, 2);

145

Fout->i = PSHR16(Fout->i, 2);

146

C_SUB( scratch[5] , *Fout, scratch[1] );

147

C_ADDTO(*Fout, scratch[1]);

148

C_ADD( scratch[3] , scratch[0] , scratch[2] );

149

C_SUB( scratch[4] , scratch[0] , scratch[2] );

150

Fout[m2].r = PSHR16(Fout[m2].r, 2);

151

Fout[m2].i = PSHR16(Fout[m2].i, 2);

152

C_SUB( Fout[m2], *Fout, scratch[3] );

153

tw1 += fstride;

154

tw2 += fstride*2;

155

tw3 += fstride*3;

156

C_ADDTO( *Fout , scratch[3] );

157

158

Fout[m].r = scratch[5].r + scratch[4].i;

159

Fout[m].i = scratch[5].i - scratch[4].r;

160

Fout[m3].r = scratch[5].r - scratch[4].i;

161

Fout[m3].i = scratch[5].i + scratch[4].r;

162

++Fout;

163

}

164

}

165

}

166

}

167

168

static void kf_bfly3(

169

kiss_fft_cpx * Fout,

170

const size_t fstride,

171

const kiss_fft_cfg st,

172

size_t m

173

)

174

{

175

size_t k=m;

176

const size_t m2 = 2*m;

177

kiss_fft_cpx *tw1,*tw2;

178

kiss_fft_cpx scratch[5];

179

kiss_fft_cpx epi3;

180

epi3 = st->twiddles[fstride*m];

181

182

tw1=tw2=st->twiddles;

183

184

do{

185

if (!st->inverse) {

186

C_FIXDIV(*Fout,3); C_FIXDIV(Fout[m],3); C_FIXDIV(Fout[m2],3);

187

}

188

189

C_MUL(scratch[1],Fout[m] , *tw1);

190

C_MUL(scratch[2],Fout[m2] , *tw2);

191

192

C_ADD(scratch[3],scratch[1],scratch[2]);

193

C_SUB(scratch[0],scratch[1],scratch[2]);

194

tw1 += fstride;

195

tw2 += fstride*2;

196

197

Fout[m].r = Fout->r - HALF_OF(scratch[3].r);

198

Fout[m].i = Fout->i - HALF_OF(scratch[3].i);

199

200

C_MULBYSCALAR( scratch[0] , epi3.i );

201

202

C_ADDTO(*Fout,scratch[3]);

203

204

Fout[m2].r = Fout[m].r + scratch[0].i;

205

Fout[m2].i = Fout[m].i - scratch[0].r;

206

207

Fout[m].r -= scratch[0].i;

208

Fout[m].i += scratch[0].r;

209

210

++Fout;

211

}while(--k);

212

}

213

214

static void kf_bfly5(

215

kiss_fft_cpx * Fout,

216

const size_t fstride,

217

const kiss_fft_cfg st,

218

int m

219

)

220

{

221

kiss_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;

222

int u;

223

kiss_fft_cpx scratch[13];

224

kiss_fft_cpx * twiddles = st->twiddles;

225

kiss_fft_cpx *tw;

226

kiss_fft_cpx ya,yb;

227

ya = twiddles[fstride*m];

228

yb = twiddles[fstride*2*m];

229

230

Fout0=Fout;

231

Fout1=Fout0+m;

232

Fout2=Fout0+2*m;

233

Fout3=Fout0+3*m;

234

Fout4=Fout0+4*m;

235

236

tw=st->twiddles;

237

for ( u=0; u<m; ++u ) {

238

if (!st->inverse) {

239

C_FIXDIV( *Fout0,5); C_FIXDIV( *Fout1,5); C_FIXDIV( *Fout2,5); C_FIXDIV( *Fout3,5); C_FIXDIV( *Fout4,5);

240

}

241

scratch[0] = *Fout0;

242

243

C_MUL(scratch[1] ,*Fout1, tw[u*fstride]);

244

C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]);

245

C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]);

246

C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]);

247

248

C_ADD( scratch[7],scratch[1],scratch[4]);

249

C_SUB( scratch[10],scratch[1],scratch[4]);

250

C_ADD( scratch[8],scratch[2],scratch[3]);

251

C_SUB( scratch[9],scratch[2],scratch[3]);

252

253

Fout0->r += scratch[7].r + scratch[8].r;

254

Fout0->i += scratch[7].i + scratch[8].i;

255

256

scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);

257

scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);

258

259

scratch[6].r = S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i);

260

scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i);

261

262

C_SUB(*Fout1,scratch[5],scratch[6]);

263

C_ADD(*Fout4,scratch[5],scratch[6]);

264

265

scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);

266

scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);

267

scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);

268

scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);

269

270

C_ADD(*Fout2,scratch[11],scratch[12]);

271

C_SUB(*Fout3,scratch[11],scratch[12]);

272

273

++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;

274

}

275

}

276

277

/* perform the butterfly for one stage of a mixed radix FFT */

278

static void kf_bfly_generic(

279

kiss_fft_cpx * Fout,

280

const size_t fstride,

281

const kiss_fft_cfg st,

282

int m,

283

int p

284

)

285

{

286

int u,k,q1,q;

287

kiss_fft_cpx * twiddles = st->twiddles;

288

kiss_fft_cpx t;

289

kiss_fft_cpx scratchbuf[17];

290

int Norig = st->nfft;

291

292

/*CHECKBUF(scratchbuf,nscratchbuf,p);*/

293

if (p>17)

294

speex_fatal("KissFFT: max radix supported is 17");

295

296

for ( u=0; u<m; ++u ) {

297

k=u;

298

for ( q1=0 ; q1<p ; ++q1 ) {

299

scratchbuf[q1] = Fout[ k ];

300

if (!st->inverse) {

301

C_FIXDIV(scratchbuf[q1],p);

302

}

303

k += m;

304

}

305

306

k=u;

307

for ( q1=0 ; q1<p ; ++q1 ) {

308

int twidx=0;

309

Fout[ k ] = scratchbuf[0];

310

for (q=1;q<p;++q ) {

311

twidx += fstride * k;

312

if (twidx>=Norig) twidx-=Norig;

313

C_MUL(t,scratchbuf[q] , twiddles[twidx] );

314

C_ADDTO( Fout[ k ] ,t);

315

}

316

k += m;

317

}

318

}

319

}

320

321

static

322

void kf_shuffle(

323

kiss_fft_cpx * Fout,

324

const kiss_fft_cpx * f,

325

const size_t fstride,

326

int in_stride,

327

int * factors,

328

const kiss_fft_cfg st

329

)

330

{

331

const int p=*factors++; /* the radix */

332

const int m=*factors++; /* stage's fft length/p */

333

334

/*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/

335

if (m==1)

336

{

337

int j;

338

for (j=0;j<p;j++)

339

{

340

Fout[j] = *f;

341

f += fstride*in_stride;

342

}

343

} else {

344

int j;

345

for (j=0;j<p;j++)

346

{

347

kf_shuffle( Fout , f, fstride*p, in_stride, factors,st);

348

f += fstride*in_stride;

349

Fout += m;

350

}

351

}

352

}

353

354

static

355

void kf_work(

356

kiss_fft_cpx * Fout,

357

const kiss_fft_cpx * f,

358

const size_t fstride,

359

int in_stride,

360

int * factors,

361

const kiss_fft_cfg st,

362

int N,

363

int s2,

364

int m2

365

)

366

{

367

int i;

368

kiss_fft_cpx * Fout_beg=Fout;

369

const int p=*factors++; /* the radix */

370

const int m=*factors++; /* stage's fft length/p */

371

#if 0

372

/*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/

373

if (m==1)

374

{

375

/* int j;

376

for (j=0;j<p;j++)

377

{

378

Fout[j] = *f;

379

f += fstride*in_stride;

380

}*/

381

} else {

382

int j;

383

for (j=0;j<p;j++)

384

{

385

kf_work( Fout , f, fstride*p, in_stride, factors,st, N*p, fstride*in_stride, m);

386

f += fstride*in_stride;

387

Fout += m;

388

}

389

}

390

391

Fout=Fout_beg;

392

393

switch (p) {

394

case 2: kf_bfly2(Fout,fstride,st,m); break;

395

case 3: kf_bfly3(Fout,fstride,st,m); break;

396

case 4: kf_bfly4(Fout,fstride,st,m); break;

397

case 5: kf_bfly5(Fout,fstride,st,m); break;

398

default: kf_bfly_generic(Fout,fstride,st,m,p); break;

399

}

400

#else

401

/*printf ("fft %d %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N, m2);*/

402

if (m==1)

403

{

404

/*for (i=0;i<N;i++)

405

{

406

int j;

407

Fout = Fout_beg+i*m2;

408

const kiss_fft_cpx * f2 = f+i*s2;

409

for (j=0;j<p;j++)

410

{

411

*Fout++ = *f2;

412

f2 += fstride*in_stride;

413

}

414

}*/

415

}else{

416

kf_work( Fout , f, fstride*p, in_stride, factors,st, N*p, fstride*in_stride, m);

417

}

418

419

420

421

422

switch (p) {

423

case 2: kf_bfly2(Fout,fstride,st,m, N, m2); break;

424

case 3: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly3(Fout,fstride,st,m);} break;

425

case 4: kf_bfly4(Fout,fstride,st,m, N, m2); break;

426

case 5: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly5(Fout,fstride,st,m);} break;

427

default: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly_generic(Fout,fstride,st,m,p);} break;

428

}

429

#endif

430

}

431

432

/* facbuf is populated by p1,m1,p2,m2, ...

433

where

434

p[i] * m[i] = m[i-1]

435

m0 = n */

436

static

437

void kf_factor(int n,int * facbuf)

438

{

439

int p=4;

440

441

/*factor out powers of 4, powers of 2, then any remaining primes */

442

do {

443

while (n % p) {

444

switch (p) {

445

case 4: p = 2; break;

446

case 2: p = 3; break;

447

default: p += 2; break;

448

}

449

if (p>32000 || (spx_int32_t)p*(spx_int32_t)p > n)

450

p = n; /* no more factors, skip to end */

451

}

452

n /= p;

453

*facbuf++ = p;

454

*facbuf++ = n;

455

} while (n > 1);

456

}

457

458

459

* User-callable function to allocate all necessary storage space for the fft.

460

461

* The return value is a contiguous block of memory, allocated with malloc. As such,

462

* It can be freed with free(), rather than a kiss_fft-specific function.

463

* */

464

kiss_fft_cfg kiss_fft_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem )

465

{

466

kiss_fft_cfg st=NULL;

467

size_t memneeded = sizeof(struct kiss_fft_state)

468

+ sizeof(kiss_fft_cpx)*(nfft-1); /* twiddle factors*/

469

470

if ( lenmem==NULL ) {

471

st = ( kiss_fft_cfg)KISS_FFT_MALLOC( memneeded );

472

}else{

473

if (mem != NULL && *lenmem >= memneeded)

474

st = (kiss_fft_cfg)mem;

475

*lenmem = memneeded;

476

}

477

if (st) {

478

int i;

479

st->nfft=nfft;

480

st->inverse = inverse_fft;

481

#ifdef FIXED_POINT

482

for (i=0;i<nfft;++i) {

483

spx_word32_t phase = i;

484

if (!st->inverse)

485

phase = -phase;

486

kf_cexp2(st->twiddles+i, DIV32(SHL32(phase,17),nfft));

487

}

488

#else

489

for (i=0;i<nfft;++i) {

490

const double pi=3.14159265358979323846264338327;

491

double phase = ( -2*pi /nfft ) * i;

492

if (st->inverse)

493

phase *= -1;

494

kf_cexp(st->twiddles+i, phase );

495

}

496

#endif

497

kf_factor(nfft,st->factors);

498

}

499

return st;

500

}

501

502

503

504

505

void kiss_fft_stride(kiss_fft_cfg st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout,int in_stride)

506

{

507

if (fin == fout)

508

{

509

speex_fatal("In-place FFT not supported");

510

/*CHECKBUF(tmpbuf,ntmpbuf,st->nfft);

511

kf_work(tmpbuf,fin,1,in_stride, st->factors,st);

512

SPEEX_MOVE(fout,tmpbuf,st->nfft);*/

513

} else {

514

kf_shuffle( fout, fin, 1,in_stride, st->factors,st);

515

kf_work( fout, fin, 1,in_stride, st->factors,st, 1, in_stride, 1);

516

}

517

}

518

519

void kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)

520

{

521

kiss_fft_stride(cfg,fin,fout,1);

522

}

523

Older »