#include <stdio.h>
|
#include <config.h>
|
#include <stdlib.h>
|
#include <string.h>
|
#ifdef __ADSP_21489
|
#include <sysreg.h>
|
#endif
|
#include <config.h>
|
#include "utility/biquad.h"
|
#include "jitter.h"
|
#include "eq.h"
|
#include "alg.h"
|
|
/*
|
*/
|
|
struct tag_jitter_mem {
|
int size;
|
int elem_count;
|
int w_offset;
|
int r_offset;
|
float coeffs[5];
|
float state[3];
|
float dataf[4];
|
};
|
|
|
|
jitter_mem* jitter_create(int size)
|
{
|
float b[3],a[3];
|
jitter_mem* pjitter = malloc(size+16);
|
if (pjitter == NULL) return NULL;
|
|
memset(pjitter, 0 ,size+16);
|
pjitter->size = size;
|
|
callEQcoeffs(eq_lpf, 18000,0, 0.717, 48000, b,a);
|
pjitter->coeffs[0] = b[0]/a[0];
|
pjitter->coeffs[1] = b[1]/a[0];
|
pjitter->coeffs[2] = b[2]/a[0];
|
pjitter->coeffs[3] = a[1]/a[0];
|
pjitter->coeffs[4] = a[2]/a[0];
|
|
return pjitter;
|
}
|
|
|
int jitter_write(jitter_mem* pjitter, const float* data, int num)
|
{
|
#ifdef __ADSP_21489
|
int enabled = sysreg_bit_tst(sysreg_MODE1, IRPTEN);
|
#endif
|
if (pjitter == NULL) return 0;
|
|
#ifdef __ADSP_21489
|
if (enabled){
|
sysreg_bit_clr(sysreg_MODE1, IRPTEN); // Disable
|
}
|
#endif
|
memcpy(pjitter->dataf+ pjitter->w_offset, data, num);
|
pjitter->w_offset += num;
|
if (pjitter->w_offset >= pjitter->size) {
|
pjitter->w_offset = 0;
|
}
|
pjitter->elem_count += num;
|
if(pjitter->elem_count >= pjitter->size){
|
pjitter->elem_count = pjitter->size;
|
}
|
#ifdef __ADSP_21489
|
if (enabled){
|
sysreg_bit_set(sysreg_MODE1, IRPTEN);// Re-enable interrupts
|
}
|
#endif
|
return num;
|
}
|
//p2 ,p1, x, r1
|
static float est_value(float p2,float p1, float r1)
|
{
|
float est=0;
|
float d1 = p1-p2;
|
float d2 = r1-p1;
|
float d3 = r1-p2;
|
|
if(d2 > 0 && d1 > 0){
|
est = p1 + (r1 - p1)*0.5;
|
}
|
else if(d2 < 0 && d1 < 0){
|
est = p1 + (r1 - p1)*0.5;
|
}
|
else if(d2 > 0 && d1 < 0){
|
if(d3 > 0){
|
est = p1 + (r1 - p1)*0.5;
|
}
|
else{
|
est = p1 + (p1 - p2);
|
}
|
}
|
else if(d2 < 0 && d1 > 0){
|
if(d3 > 0){
|
est = p1 + (p1 - p2);
|
}
|
else{
|
est = p1 + (r1 - p1) * 0.5;
|
}
|
}
|
|
if(est >1.0) est =1.0;
|
else if(est <-1.0) est =-1.0;
|
|
return est;
|
}
|
|
int jitter_read(jitter_mem* pjitter, float* data, int num)
|
{
|
float percent = pjitter->elem_count*1.0 / pjitter->size;
|
float pcm[128];
|
int i, size =0;
|
int flag = 0;
|
int delta;
|
#ifdef __ADSP_21489
|
int enabled = sysreg_bit_tst(sysreg_MODE1, IRPTEN);
|
#endif
|
|
if (pjitter == NULL || pjitter->elem_count <= 0) {
|
memset(data, 0 ,num);
|
return 0;
|
}
|
#ifdef __ADSP_21489
|
if (enabled){
|
sysreg_bit_clr(sysreg_MODE1, IRPTEN); // Disable
|
}
|
#endif
|
if (percent >= 0.875) {
|
//¶Á2¸öµã¶ª1¸öµã,2/3
|
size = (int)(num * 1.5);
|
flag = 1;
|
}
|
else if (percent>=0.75 && percent < 0.875) {
|
//¶Á1¸öµã¶ª1¸öµã
|
size = num *2 ;
|
flag = 2;
|
}
|
else if (percent>0.125 && percent <= 0.25) {
|
//¶Á2¸öµã²åÈë1¸öµã
|
size = (int)(num * 2.0 / 3 + 0.5);
|
flag = 3;
|
}
|
else if(percent <= 0.125 ){
|
//¶Á1¸öµã²å1¸öµã
|
size = num / 2;
|
flag = 4;
|
}
|
else {
|
size = num ;
|
flag = 0;
|
}
|
|
memset(data, 0 ,num);
|
|
|
delta = pjitter->size - pjitter->r_offset;
|
if (delta >= size) {
|
memcpy(pcm, pjitter->dataf + pjitter->r_offset, size);
|
pjitter->r_offset += size;
|
if (pjitter->r_offset >= pjitter->size) {
|
pjitter->r_offset = 0;
|
}
|
}
|
else {
|
memcpy(pcm, pjitter->dataf + pjitter->r_offset, delta);
|
memcpy(pcm + delta, pjitter->dataf, size - delta);
|
pjitter->r_offset = size - delta;
|
}
|
pjitter->elem_count -= size;
|
|
if (flag == 2) {
|
int j=0;
|
for (i = 0; i < size; i+=2) {//128
|
data[j++] = pcm[i];
|
}
|
}
|
else if (flag == 4) {
|
int j=0;
|
for (i = 0; i < size; i++) {//32
|
data[j++] = pcm[i];
|
data[j++] = pcm[i];
|
}
|
}
|
else if (flag == 1)
|
{
|
int j=0;
|
for (i = 0; i < size; i+=3) {//96
|
data[j++] = pcm[i];
|
data[j++] = pcm[i+1];
|
}
|
}
|
else if (flag == 3){
|
int j=0;
|
int len = size/2*2;
|
|
for (i = 0; i < len; i+=2) {//42
|
data[j++] = pcm[i];
|
data[j++] = pcm[i+1];
|
data[j++] = est_value(pcm[i],pcm[i+1],pcm[i+2]);
|
}
|
data[j] = pcm[len];
|
}
|
else {
|
memcpy(data, pcm, size);
|
}
|
|
biquadd_vec(data , data ,pjitter->coeffs,pjitter->state, num, 1);
|
|
#ifdef __ADSP_21489
|
if (enabled){
|
sysreg_bit_set(sysreg_MODE1, IRPTEN);// Re-enable interrupts
|
}
|
#endif
|
return num;
|
}
|
