~ubuntu-branches/ubuntu/wily/sflphone/wily

const spx_word16_t Pcoef[5][3] = {{16384, -31313, 14991}, {16384, -31569, 15249}, {16384, -31677, 15328}, {16384, -32313, 15947}, {16384, -22446, 6537}};

const spx_word16_t Zcoef[5][3] = {{15672, -31344, 15672}, {15802, -31601, 15802}, {15847, -31694, 15847}, {16162, -32322, 16162}, {14418, -28836, 14418}};

#else

const spx_word16_t Pcoef[5][3] = {{1.00000f, -1.91120f, 0.91498f}, {1.00000f, -1.92683f, 0.93071f}, {1.00000f, -1.93338f, 0.93553f}, {1.00000f, -1.97226f, 0.97332f}, {1.00000f, -1.37000f, 0.39900f}};

const spx_word16_t Zcoef[5][3] = {{0.95654f, -1.91309f, 0.95654f}, {0.96446f, -1.92879f, 0.96446f}, {0.96723f, -1.93445f, 0.96723f}, {0.98645f, -1.97277f, 0.98645f}, {0.88000f, -1.76000f, 0.88000f}};

#endif

const spx_word16_t *den, *num;

if (filtID>4)

filtID=4;

den = Pcoef[filtID]; num = Zcoef[filtID];

/*return;*/

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

100

{

101

spx_word16_t yi;

102

spx_word32_t vout = ADD32(MULT16_16(num[0], x[i]),mem[0]);

103

yi = EXTRACT16(SATURATE(PSHR32(vout,14),32767));

104

mem[0] = ADD32(MAC16_16(mem[1], num[1],x[i]), SHL32(MULT16_32_Q15(-den[1],vout),1));

105

mem[1] = ADD32(MULT16_16(num[2],x[i]), SHL32(MULT16_32_Q15(-den[2],vout),1));

106

y[i] = yi;

107

}

108

}

109

110

#ifdef FIXED_POINT

111

112

/* FIXME: These functions are ugly and probably introduce too much error */

113

void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)

114

{

115

int i;

116

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

117

{

118

y[i] = SHL32(MULT16_32_Q14(EXTRACT16(SHR32(x[i],7)),scale),7);

119

}

120

}

121

122

void signal_div(const spx_word16_t *x, spx_word16_t *y, spx_word32_t scale, int len)

123

{

124

int i;

125

if (scale > SHL32(EXTEND32(SIG_SCALING), 8))

126

{

127

spx_word16_t scale_1;

128

scale = PSHR32(scale, SIG_SHIFT);

129

scale_1 = EXTRACT16(PDIV32_16(SHL32(EXTEND32(SIG_SCALING),7),scale));

130

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

131

{

132

y[i] = MULT16_16_P15(scale_1, x[i]);

133

}

134

} else if (scale > SHR32(EXTEND32(SIG_SCALING), 2)) {

135

spx_word16_t scale_1;

136

scale = PSHR32(scale, SIG_SHIFT-5);

137

scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale);

138

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

139

{

140

y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),8);

141

}

142

} else {

143

spx_word16_t scale_1;

144

scale = PSHR32(scale, SIG_SHIFT-7);

145

if (scale < 5)

146

scale = 5;

147

scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale);

148

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

149

{

150

y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),6);

151

}

152

}

153

}

154

155

#else

156

157

void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)

158

{

159

int i;

160

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

161

y[i] = scale*x[i];

162

}

163

164

void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)

165

{

166

int i;

167

float scale_1 = 1/scale;

168

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

169

y[i] = scale_1*x[i];

170

}

171

#endif

172

173

174

175

#ifdef FIXED_POINT

176

177

178

179

spx_word16_t compute_rms(const spx_sig_t *x, int len)

180

{

181

int i;

182

spx_word32_t sum=0;

183

spx_sig_t max_val=1;

184

int sig_shift;

185

186

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

187

{

188

spx_sig_t tmp = x[i];

189

if (tmp<0)

190

tmp = -tmp;

191

if (tmp > max_val)

192

max_val = tmp;

193

}

194

195

sig_shift=0;

196

while (max_val>16383)

197

{

198

sig_shift++;

199

max_val >>= 1;

200

}

201

202

for (i=0;i<len;i+=4)

203

{

204

spx_word32_t sum2=0;

205

spx_word16_t tmp;

206

tmp = EXTRACT16(SHR32(x[i],sig_shift));

207

sum2 = MAC16_16(sum2,tmp,tmp);

208

tmp = EXTRACT16(SHR32(x[i+1],sig_shift));

209

sum2 = MAC16_16(sum2,tmp,tmp);

210

tmp = EXTRACT16(SHR32(x[i+2],sig_shift));

211

sum2 = MAC16_16(sum2,tmp,tmp);

212

tmp = EXTRACT16(SHR32(x[i+3],sig_shift));

213

sum2 = MAC16_16(sum2,tmp,tmp);

214

sum = ADD32(sum,SHR32(sum2,6));

215

}

216

217

return EXTRACT16(PSHR32(SHL32(EXTEND32(spx_sqrt(DIV32(sum,len))),(sig_shift+3)),SIG_SHIFT));

218

}

219

220

spx_word16_t compute_rms16(const spx_word16_t *x, int len)

221

{

222

int i;

223

spx_word16_t max_val=10;

224

225

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

226

{

227

spx_sig_t tmp = x[i];

228

if (tmp<0)

229

tmp = -tmp;

230

if (tmp > max_val)

231

max_val = tmp;

232

}

233

if (max_val>16383)

234

{

235

spx_word32_t sum=0;

236

for (i=0;i<len;i+=4)

237

{

238

spx_word32_t sum2=0;

239

sum2 = MAC16_16(sum2,SHR16(x[i],1),SHR16(x[i],1));

240

sum2 = MAC16_16(sum2,SHR16(x[i+1],1),SHR16(x[i+1],1));

241

sum2 = MAC16_16(sum2,SHR16(x[i+2],1),SHR16(x[i+2],1));

242

sum2 = MAC16_16(sum2,SHR16(x[i+3],1),SHR16(x[i+3],1));

243

sum = ADD32(sum,SHR32(sum2,6));

244

}

245

return SHL16(spx_sqrt(DIV32(sum,len)),4);

246

} else {

247

spx_word32_t sum=0;

248

int sig_shift=0;

249

if (max_val < 8192)

250

sig_shift=1;

251

if (max_val < 4096)

252

sig_shift=2;

253

if (max_val < 2048)

254

sig_shift=3;

255

for (i=0;i<len;i+=4)

256

{

257

spx_word32_t sum2=0;

258

sum2 = MAC16_16(sum2,SHL16(x[i],sig_shift),SHL16(x[i],sig_shift));

259

sum2 = MAC16_16(sum2,SHL16(x[i+1],sig_shift),SHL16(x[i+1],sig_shift));

260

sum2 = MAC16_16(sum2,SHL16(x[i+2],sig_shift),SHL16(x[i+2],sig_shift));

261

sum2 = MAC16_16(sum2,SHL16(x[i+3],sig_shift),SHL16(x[i+3],sig_shift));

262

sum = ADD32(sum,SHR32(sum2,6));

263

}

264

return SHL16(spx_sqrt(DIV32(sum,len)),3-sig_shift);

265

}

266

}

267

268

#ifndef OVERRIDE_NORMALIZE16

269

int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len)

270

{

271

int i;

272

spx_sig_t max_val=1;

273

int sig_shift;

274

275

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

276

{

277

spx_sig_t tmp = x[i];

278

if (tmp<0)

279

tmp = NEG32(tmp);

280

if (tmp >= max_val)

281

max_val = tmp;

282

}

283

284

sig_shift=0;

285

while (max_val>max_scale)

286

{

287

sig_shift++;

288

max_val >>= 1;

289

}

290

291

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

292

y[i] = EXTRACT16(SHR32(x[i], sig_shift));

293

294

return sig_shift;

295

}

296

#endif

297

298

#else

299

300

spx_word16_t compute_rms(const spx_sig_t *x, int len)

301

{

302

int i;

303

float sum=0;

304

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

305

{

306

sum += x[i]*x[i];

307

}

308

return sqrt(.1+sum/len);

309

}

310

spx_word16_t compute_rms16(const spx_word16_t *x, int len)

311

{

312

return compute_rms(x, len);

313

}

314

#endif

315

316

317

318

#ifndef OVERRIDE_FILTER_MEM16

319

void filter_mem16(const spx_word16_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack)

320

{

321

int i,j;

322

spx_word16_t xi,yi,nyi;

323

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

324

{

325

xi= x[i];

326

yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));

327

nyi = NEG16(yi);

328

for (j=0;j<ord-1;j++)

329

{

330

mem[j] = MAC16_16(MAC16_16(mem[j+1], num[j],xi), den[j],nyi);

331

}

332

mem[ord-1] = ADD32(MULT16_16(num[ord-1],xi), MULT16_16(den[ord-1],nyi));

333

y[i] = yi;

334

}

335

}

336

#endif

337

338

#ifndef OVERRIDE_IIR_MEM16

339

void iir_mem16(const spx_word16_t *x, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack)

340

{

341

int i,j;

342

spx_word16_t yi,nyi;

343

344

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

345

{

346

yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));

347

nyi = NEG16(yi);

348

for (j=0;j<ord-1;j++)

349

{

350

mem[j] = MAC16_16(mem[j+1],den[j],nyi);

351

}

352

mem[ord-1] = MULT16_16(den[ord-1],nyi);

353

y[i] = yi;

354

}

355

}

356

#endif

357

358

#ifndef OVERRIDE_FIR_MEM16

359

void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack)

360

{

361

int i,j;

362

spx_word16_t xi,yi;

363

364

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

365

{

366

xi=x[i];

367

yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));

368

for (j=0;j<ord-1;j++)

369

{

370

mem[j] = MAC16_16(mem[j+1], num[j],xi);

371

}

372

mem[ord-1] = MULT16_16(num[ord-1],xi);

373

y[i] = yi;

374

}

375

}

376

#endif

377

378

379

void syn_percep_zero16(const spx_word16_t *xx, const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)

380

{

381

int i;

382

VARDECL(spx_mem_t *mem);

383

ALLOC(mem, ord, spx_mem_t);

384

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

385

mem[i]=0;

386

iir_mem16(xx, ak, y, N, ord, mem, stack);

387

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

388

mem[i]=0;

389

filter_mem16(y, awk1, awk2, y, N, ord, mem, stack);

390

}

391

void residue_percep_zero16(const spx_word16_t *xx, const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)

392

{

393

int i;

394

VARDECL(spx_mem_t *mem);

395

ALLOC(mem, ord, spx_mem_t);

396

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

397

mem[i]=0;

398

filter_mem16(xx, ak, awk1, y, N, ord, mem, stack);

399

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

400

mem[i]=0;

401

fir_mem16(y, awk2, y, N, ord, mem, stack);

402

}

403

404

405

#ifndef OVERRIDE_COMPUTE_IMPULSE_RESPONSE

406

void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)

407

{

408

int i,j;

409

spx_word16_t y1, ny1i, ny2i;

410

VARDECL(spx_mem_t *mem1);

411

VARDECL(spx_mem_t *mem2);

412

ALLOC(mem1, ord, spx_mem_t);

413

ALLOC(mem2, ord, spx_mem_t);

414

415

y[0] = LPC_SCALING;

416

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

417

y[i+1] = awk1[i];

418

i++;

419

for (;i<N;i++)

420

y[i] = VERY_SMALL;

421

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

422

mem1[i] = mem2[i] = 0;

423

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

424

{

425

y1 = ADD16(y[i], EXTRACT16(PSHR32(mem1[0],LPC_SHIFT)));

426

ny1i = NEG16(y1);

427

y[i] = PSHR32(ADD32(SHL32(EXTEND32(y1),LPC_SHIFT+1),mem2[0]),LPC_SHIFT);

428

ny2i = NEG16(y[i]);

429

for (j=0;j<ord-1;j++)

430

{

431

mem1[j] = MAC16_16(mem1[j+1], awk2[j],ny1i);

432

mem2[j] = MAC16_16(mem2[j+1], ak[j],ny2i);

433

}

434

mem1[ord-1] = MULT16_16(awk2[ord-1],ny1i);

435

mem2[ord-1] = MULT16_16(ak[ord-1],ny2i);

436

}

437

}

438

#endif

439

440

/* Decomposes a signal into low-band and high-band using a QMF */

441

void qmf_decomp(const spx_word16_t *xx, const spx_word16_t *aa, spx_word16_t *y1, spx_word16_t *y2, int N, int M, spx_word16_t *mem, char *stack)

442

{

443

int i,j,k,M2;

444

VARDECL(spx_word16_t *a);

445

VARDECL(spx_word16_t *x);

446

spx_word16_t *x2;

447

448

ALLOC(a, M, spx_word16_t);

449

ALLOC(x, N+M-1, spx_word16_t);

450

x2=x+M-1;

451

M2=M>>1;

452

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

453

a[M-i-1]= aa[i];

454

for (i=0;i<M-1;i++)

455

x[i]=mem[M-i-2];

456

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

457

x[i+M-1]=SHR16(xx[i],1);

458

for (i=0;i<M-1;i++)

459

mem[i]=SHR16(xx[N-i-1],1);

460

for (i=0,k=0;i<N;i+=2,k++)

461

{

462

spx_word32_t y1k=0, y2k=0;

463

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

464

{

465

y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));

466

y2k=SUB32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));

467

j++;

468

y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));

469

y2k=ADD32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));

470

}

471

y1[k] = EXTRACT16(SATURATE(PSHR32(y1k,15),32767));

472

y2[k] = EXTRACT16(SATURATE(PSHR32(y2k,15),32767));

473

}

474

}

475

476

/* Re-synthesised a signal from the QMF low-band and high-band signals */

477

void qmf_synth(const spx_word16_t *x1, const spx_word16_t *x2, const spx_word16_t *a, spx_word16_t *y, int N, int M, spx_word16_t *mem1, spx_word16_t *mem2, char *stack)

478

/* assumptions:

479

all odd x[i] are zero -- well, actually they are left out of the array now

480

N and M are multiples of 4 */

481

{

482

int i, j;

483

int M2, N2;

484

VARDECL(spx_word16_t *xx1);

485

VARDECL(spx_word16_t *xx2);

486

487

M2 = M>>1;

488

N2 = N>>1;

489

ALLOC(xx1, M2+N2, spx_word16_t);

490

ALLOC(xx2, M2+N2, spx_word16_t);

491

492

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

493

xx1[i] = x1[N2-1-i];

494

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

495

xx1[N2+i] = mem1[2*i+1];

496

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

497

xx2[i] = x2[N2-1-i];

498

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

499

xx2[N2+i] = mem2[2*i+1];

500

501

for (i = 0; i < N2; i += 2) {

502

spx_sig_t y0, y1, y2, y3;

503

spx_word16_t x10, x20;

504

505

y0 = y1 = y2 = y3 = 0;

506

x10 = xx1[N2-2-i];

507

x20 = xx2[N2-2-i];

508

509

for (j = 0; j < M2; j += 2) {

510

spx_word16_t x11, x21;

511

spx_word16_t a0, a1;

512

513

a0 = a[2*j];

514

a1 = a[2*j+1];

515

x11 = xx1[N2-1+j-i];

516

x21 = xx2[N2-1+j-i];

517

518

#ifdef FIXED_POINT

519

/* We multiply twice by the same coef to avoid overflows */

520

y0 = MAC16_16(MAC16_16(y0, a0, x11), NEG16(a0), x21);

521

y1 = MAC16_16(MAC16_16(y1, a1, x11), a1, x21);

522

y2 = MAC16_16(MAC16_16(y2, a0, x10), NEG16(a0), x20);

523

y3 = MAC16_16(MAC16_16(y3, a1, x10), a1, x20);

524

#else

525

y0 = ADD32(y0,MULT16_16(a0, x11-x21));

526

y1 = ADD32(y1,MULT16_16(a1, x11+x21));

527

y2 = ADD32(y2,MULT16_16(a0, x10-x20));

528

y3 = ADD32(y3,MULT16_16(a1, x10+x20));

529

#endif

530

a0 = a[2*j+2];

531

a1 = a[2*j+3];

532

x10 = xx1[N2+j-i];

533

x20 = xx2[N2+j-i];

534

535

#ifdef FIXED_POINT

536

/* We multiply twice by the same coef to avoid overflows */

537

y0 = MAC16_16(MAC16_16(y0, a0, x10), NEG16(a0), x20);

538

y1 = MAC16_16(MAC16_16(y1, a1, x10), a1, x20);

539

y2 = MAC16_16(MAC16_16(y2, a0, x11), NEG16(a0), x21);

540

y3 = MAC16_16(MAC16_16(y3, a1, x11), a1, x21);

541

#else

542

y0 = ADD32(y0,MULT16_16(a0, x10-x20));

543

y1 = ADD32(y1,MULT16_16(a1, x10+x20));

544

y2 = ADD32(y2,MULT16_16(a0, x11-x21));

545

y3 = ADD32(y3,MULT16_16(a1, x11+x21));

546

#endif

547

}

548

#ifdef FIXED_POINT

549

y[2*i] = EXTRACT16(SATURATE32(PSHR32(y0,15),32767));

550

y[2*i+1] = EXTRACT16(SATURATE32(PSHR32(y1,15),32767));

551

y[2*i+2] = EXTRACT16(SATURATE32(PSHR32(y2,15),32767));

552

y[2*i+3] = EXTRACT16(SATURATE32(PSHR32(y3,15),32767));

553

#else

554

/* Normalize up explicitly if we're in float */

555

y[2*i] = 2.f*y0;

556

y[2*i+1] = 2.f*y1;

557

y[2*i+2] = 2.f*y2;

558

y[2*i+3] = 2.f*y3;

559

#endif

560

}

561

562

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

563

mem1[2*i+1] = xx1[i];

564

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

565

mem2[2*i+1] = xx2[i];

566

}

567

568

#ifdef FIXED_POINT

569

#if 0

570

const spx_word16_t shift_filt[3][7] = {{-33, 1043, -4551, 19959, 19959, -4551, 1043},

571

{-98, 1133, -4425, 29179, 8895, -2328, 444},

572

{444, -2328, 8895, 29179, -4425, 1133, -98}};

573

#else

574

const spx_word16_t shift_filt[3][7] = {{-390, 1540, -4993, 20123, 20123, -4993, 1540},

575

{-1064, 2817, -6694, 31589, 6837, -990, -209},

576

{-209, -990, 6837, 31589, -6694, 2817, -1064}};

577

#endif

578

#else

579

#if 0

580

const float shift_filt[3][7] = {{-9.9369e-04, 3.1831e-02, -1.3889e-01, 6.0910e-01, 6.0910e-01, -1.3889e-01, 3.1831e-02},

581

{-0.0029937, 0.0345613, -0.1350474, 0.8904793, 0.2714479, -0.0710304, 0.0135403},

582

{0.0135403, -0.0710304, 0.2714479, 0.8904793, -0.1350474, 0.0345613, -0.0029937}};

583

#else

584

const float shift_filt[3][7] = {{-0.011915f, 0.046995f, -0.152373f, 0.614108f, 0.614108f, -0.152373f, 0.046995f},

585

{-0.0324855f, 0.0859768f, -0.2042986f, 0.9640297f, 0.2086420f, -0.0302054f, -0.0063646f},

586

{-0.0063646f, -0.0302054f, 0.2086420f, 0.9640297f, -0.2042986f, 0.0859768f, -0.0324855f}};

587

#endif

588

#endif

589

590

int interp_pitch(

591

spx_word16_t *exc, /*decoded excitation*/

592

spx_word16_t *interp, /*decoded excitation*/

593

int pitch, /*pitch period*/

594

int len

595

)

596

{

597

int i,j,k;

598

spx_word32_t corr[4][7];

599

spx_word32_t maxcorr;

600

int maxi, maxj;

601

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

602

{

603

corr[0][i] = inner_prod(exc, exc-pitch-3+i, len);

604

}

605

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

606

{

607

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

608

{

609

int i1, i2;

610

spx_word32_t tmp=0;

611

i1 = 3-j;

612

if (i1<0)

613

i1 = 0;

614

i2 = 10-j;

615

if (i2>7)

616

i2 = 7;

617

for (k=i1;k<i2;k++)

618

tmp += MULT16_32_Q15(shift_filt[i][k],corr[0][j+k-3]);

619

corr[i+1][j] = tmp;

620

}

621

}

622

maxi=maxj=0;

623

maxcorr = corr[0][0];

624

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

625

{

626

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

627

{

628

if (corr[i][j] > maxcorr)

629

{

630

maxcorr = corr[i][j];

631

maxi=i;

632

maxj=j;

633

}

634

}

635

}

636

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

637

{

638

spx_word32_t tmp = 0;

639

if (maxi>0)

640

{

641

for (k=0;k<7;k++)

642

{

643

tmp += MULT16_16(exc[i-(pitch-maxj+3)+k-3],shift_filt[maxi-1][k]);

644

}

645

} else {

646

tmp = SHL32(exc[i-(pitch-maxj+3)],15);

647

}

648

interp[i] = PSHR32(tmp,15);

649

}

650

return pitch-maxj+3;

651

}

652

653

void multicomb(

654

spx_word16_t *exc, /*decoded excitation*/

655

spx_word16_t *new_exc, /*enhanced excitation*/

656

spx_coef_t *ak, /*LPC filter coefs*/

657

int p, /*LPC order*/

658

int nsf, /*sub-frame size*/

659

int pitch, /*pitch period*/

660

int max_pitch,

661

spx_word16_t comb_gain, /*gain of comb filter*/

662

char *stack

663

)

664

{

665

int i;

666

VARDECL(spx_word16_t *iexc);

667

spx_word16_t old_ener, new_ener;

668

int corr_pitch;

669

670

spx_word16_t iexc0_mag, iexc1_mag, exc_mag;

671

spx_word32_t corr0, corr1;

672

spx_word16_t gain0, gain1;

673

spx_word16_t pgain1, pgain2;

674

spx_word16_t c1, c2;

675

spx_word16_t g1, g2;

676

spx_word16_t ngain;

677

spx_word16_t gg1, gg2;

678

#ifdef FIXED_POINT

679

int scaledown=0;

680

#endif

681

#if 0 /* Set to 1 to enable full pitch search */

682

int nol_pitch[6];

683

spx_word16_t nol_pitch_coef[6];

684

spx_word16_t ol_pitch_coef;

685

open_loop_nbest_pitch(exc, 20, 120, nsf,

686

nol_pitch, nol_pitch_coef, 6, stack);

687

corr_pitch=nol_pitch[0];

688

ol_pitch_coef = nol_pitch_coef[0];

689

/*Try to remove pitch multiples*/

690

for (i=1;i<6;i++)

691

{

692

#ifdef FIXED_POINT

693

if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],19661)) &&

694

#else

695

if ((nol_pitch_coef[i]>.6*nol_pitch_coef[0]) &&

696

#endif

697

(ABS(2*nol_pitch[i]-corr_pitch)<=2 || ABS(3*nol_pitch[i]-corr_pitch)<=3 ||

698

ABS(4*nol_pitch[i]-corr_pitch)<=4 || ABS(5*nol_pitch[i]-corr_pitch)<=5))

699

{

700

corr_pitch = nol_pitch[i];

701

}

702

}

703

#else

704

corr_pitch = pitch;

705

#endif

706

707

ALLOC(iexc, 2*nsf, spx_word16_t);

708

709

interp_pitch(exc, iexc, corr_pitch, 80);

710

if (corr_pitch>max_pitch)

711

interp_pitch(exc, iexc+nsf, 2*corr_pitch, 80);

712

else

713

interp_pitch(exc, iexc+nsf, -corr_pitch, 80);

714

715

#ifdef FIXED_POINT

716

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

717

{

718

if (ABS16(exc[i])>16383)

719

{

720

scaledown = 1;

721

break;

722

}

723

}

724

if (scaledown)

725

{

726

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

727

exc[i] = SHR16(exc[i],1);

728

for (i=0;i<2*nsf;i++)

729

iexc[i] = SHR16(iexc[i],1);

730

}

731

#endif

732

/*interp_pitch(exc, iexc+2*nsf, 2*corr_pitch, 80);*/

733

734

/*printf ("%d %d %f\n", pitch, corr_pitch, max_corr*ener_1);*/

735

iexc0_mag = spx_sqrt(1000+inner_prod(iexc,iexc,nsf));

736

iexc1_mag = spx_sqrt(1000+inner_prod(iexc+nsf,iexc+nsf,nsf));

737

exc_mag = spx_sqrt(1+inner_prod(exc,exc,nsf));

738

corr0 = inner_prod(iexc,exc,nsf);

739

if (corr0<0)

740

corr0=0;

741

corr1 = inner_prod(iexc+nsf,exc,nsf);

742

if (corr1<0)

743

corr1=0;

744

#ifdef FIXED_POINT

745

/* Doesn't cost much to limit the ratio and it makes the rest easier */

746

if (SHL32(EXTEND32(iexc0_mag),6) < EXTEND32(exc_mag))

747

iexc0_mag = ADD16(1,PSHR16(exc_mag,6));

748

if (SHL32(EXTEND32(iexc1_mag),6) < EXTEND32(exc_mag))

749

iexc1_mag = ADD16(1,PSHR16(exc_mag,6));

750

#endif

751

if (corr0 > MULT16_16(iexc0_mag,exc_mag))

752

pgain1 = QCONST16(1., 14);

753

else

754

pgain1 = PDIV32_16(SHL32(PDIV32(corr0, exc_mag),14),iexc0_mag);

755

if (corr1 > MULT16_16(iexc1_mag,exc_mag))

756

pgain2 = QCONST16(1., 14);

757

else

758

pgain2 = PDIV32_16(SHL32(PDIV32(corr1, exc_mag),14),iexc1_mag);

759

gg1 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc0_mag);

760

gg2 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc1_mag);

761

if (comb_gain>0)

762

{

763

#ifdef FIXED_POINT

764

c1 = (MULT16_16_Q15(QCONST16(.4,15),comb_gain)+QCONST16(.07,15));

765

c2 = QCONST16(.5,15)+MULT16_16_Q14(QCONST16(1.72,14),(c1-QCONST16(.07,15)));

766

#else

767

c1 = .4*comb_gain+.07;

768

c2 = .5+1.72*(c1-.07);

769

#endif

770

} else

771

{

772

c1=c2=0;

773

}

774

#ifdef FIXED_POINT

775

g1 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain1),pgain1);

776

g2 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain2),pgain2);

777

#else

778

g1 = 1-c2*pgain1*pgain1;

779

g2 = 1-c2*pgain2*pgain2;

780

#endif

781

if (g1<c1)

782

g1 = c1;

783

if (g2<c1)

784

g2 = c1;

785

g1 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g1);

786

g2 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g2);

787

if (corr_pitch>max_pitch)

788

{

789

gain0 = MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q14(g1,gg1));

790

gain1 = MULT16_16_Q15(QCONST16(.3,15),MULT16_16_Q14(g2,gg2));

791

} else {

792

gain0 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g1,gg1));

793

gain1 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g2,gg2));

794

}

795

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

796

new_exc[i] = ADD16(exc[i], EXTRACT16(PSHR32(ADD32(MULT16_16(gain0,iexc[i]), MULT16_16(gain1,iexc[i+nsf])),8)));

797

/* FIXME: compute_rms16 is currently not quite accurate enough (but close) */

798

new_ener = compute_rms16(new_exc, nsf);

799

old_ener = compute_rms16(exc, nsf);

800

801

if (old_ener < 1)

802

old_ener = 1;

803

if (new_ener < 1)

804

new_ener = 1;

805

if (old_ener > new_ener)

806

old_ener = new_ener;

807

ngain = PDIV32_16(SHL32(EXTEND32(old_ener),14),new_ener);

808

809

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

810

new_exc[i] = MULT16_16_Q14(ngain, new_exc[i]);

811

#ifdef FIXED_POINT

812

if (scaledown)

813

{

814

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

815

exc[i] = SHL16(exc[i],1);

816

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

817

new_exc[i] = SHL16(SATURATE16(new_exc[i],16383),1);

818

}

819

#endif

820

}

821

Older »