PD#126633: add a shelf filter to repair frequncy responds curve

Change-Id: Ifc6d8b49b3a1d0ebbd3a7058840a519fb581c1b4

4 files changed, 122 insertions, 1 deletions

diff --git a/libTVaudio/Android.mk b/libTVaudio/Android.mk
index 17b0ef5..d2d3812 100644
--- a/libTVaudio/Android.mk
+++ b/libTVaudio/Android.mk
@@ -30,6 +30,7 @@ LOCAL_SRC_FILES := \
     audio/audio_usb_check.cpp \
     audio/amaudio_main.cpp \
     audio/DDP_media_source.cpp \
+    audio/aml_shelf.c
 
 LOCAL_CFLAGS := -DANDROID_PLATFORM_SDK_VERSION=$(PLATFORM_SDK_VERSION)  -DUSE_SYS_WRITE_SERVICE=1
 
diff --git a/libTVaudio/audio/aml_audio.c b/libTVaudio/audio/aml_audio.c
index bb64508..4f862c6 100644
--- a/libTVaudio/audio/aml_audio.c
+++ b/libTVaudio/audio/aml_audio.c
@@ -28,6 +28,7 @@
 #include <cutils/properties.h>
 #include "tinyalsa/asoundlib.h"
 
+#include "aml_shelf.h"
 #include "android_out.h"
 #include "aml_audio.h"
 
@@ -277,6 +278,7 @@ static int pcm_data_counter = 0;
 static int digital_raw_enable = 0;
 int output_record_enable = 0;
 int spdif_audio_type = LPCM;
+int type_AUDIO_IN = -1;
 pthread_mutex_t device_change_lock = PTHREAD_MUTEX_INITIALIZER;
 
 static void DoDumpData(void *data_buf, int size, int aud_src_type);
@@ -744,6 +746,18 @@ static int alsa_in_read(struct aml_stream_in *in, void* buffer, size_t bytes) {
             else
                 memset(buffer, 0, bytes);
         }
+        if (type_AUDIO_IN == 2 && GetOutputdevice() != 2) {
+            short *ptr = buffer;
+            short data;
+            int i = 0;
+            int frame_size = bytes >> 2;
+            for (i = 0; i < frame_size; i++) {
+                data = (short)audio_IIR_process((int)(*ptr), 0);
+                *ptr++ = data;
+                data = (short)audio_IIR_process((int)(*ptr), 1);
+                *ptr++ = data;
+            }
+        }
         DoDumpData(buffer, bytes, CC_DUMP_SRC_TYPE_INPUT);
 
         output_size = bytes;
@@ -1288,6 +1302,8 @@ static int aml_device_init(struct aml_dev *device) {
         device->has_aml_IIR_lib = 1;
     }
 
+    audio_IIR_init();
+
     ALOGD("%s, exiting...\n", __FUNCTION__);
     return 0;
 
@@ -1384,7 +1400,6 @@ static int get_channel_status(void) {
     int card_id;
     int type_I2S = -1;
     int type_SPDIF = -1;
-    int type_AUDIO_IN = -1;
 
     card_id = get_aml_card();
     pmixer = mixer_open(card_id);
diff --git a/libTVaudio/audio/aml_shelf.c b/libTVaudio/audio/aml_shelf.c
new file mode 100644
index 0000000..83f6990
--- a/dev/null
+++ b/libTVaudio/audio/aml_shelf.c
@@ -0,0 +1,63 @@
+#include<string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include "aml_shelf.h"
+
+struct IIR_param aml_IIR;
+
+static void IIR_init(struct IIR_param *aml_iir) {
+	int i;
+
+	for (i = 0; i < COEFF_COUNT; i++) {
+		aml_iir->b[i] = IIR_coefficients[0][i];
+		aml_iir->a[i] = IIR_coefficients[1][i];
+	}
+
+	memset(aml_iir->cx, 0, sizeof(int) * CF  * 2);
+	memset(aml_iir->cy, 0, sizeof(int) * CF  * 2);
+	return;
+}
+
+static int IIR_process(int input, struct IIR_param *aml_iir, int channel) {
+	int sample = input;
+	int y = 0, i = 0;
+	long long temp = 0;
+	int cx[TMP_COEFF], cy[TMP_COEFF];
+
+	for (i = 0; i < TMP_COEFF; i++) {
+		cx[i] = aml_iir->cx[channel][i];
+		cy[i] = aml_iir->cy[channel][i];
+	}
+
+	sample <<= DATA_FRACTION_BIT;
+	temp = (long long)sample * (long long)(aml_iir->b[0]);
+	temp += (long long)cx[0] * (long long)(aml_iir->b[1]);
+	temp += (long long)cx[1] * (long long)(aml_iir->b[2]);
+	temp -= (long long)cy[0] * (long long)(aml_iir->a[1]);
+	temp -= (long long)cy[1] * (long long)(aml_iir->a[2]);
+	temp = (temp + COEFF_HALF_ERROR) >> COEFF_FRACTION_BIT;
+	y = (int)temp;
+	cx[1] = cx[0];
+	cx[0] = sample;
+	cy[1] = cy[0];
+	cy[0] = y;
+	sample = y;
+
+	for (i = 0; i < TMP_COEFF; i++) {
+		aml_iir->cx[channel][i] = cx[i];
+		aml_iir->cy[channel][i] = cy[i];
+	}
+
+	return ((y + DATA_HALF_ERROR)>>DATA_FRACTION_BIT);
+}
+
+int audio_IIR_process(int input, int channel) {
+    return Clip(IIR_process(input, &aml_IIR, channel), -32768, 32767);
+}
+
+void audio_IIR_init(void) {
+    IIR_init(&aml_IIR);
+	return;
+}
\ No newline at end of file
diff --git a/libTVaudio/audio/aml_shelf.h b/libTVaudio/audio/aml_shelf.h
new file mode 100644
index 0000000..5b9f0d2
--- a/dev/null
+++ b/libTVaudio/audio/aml_shelf.h
@@ -0,0 +1,42 @@
+#ifndef AML_SHELF_H
+#define AML_SHELF_H
+
+#define   COEFF_FRACTION_BIT    24
+#define   DATA_FRACTION_BIT     10
+#define   COEFF_HALF_ERROR      ((0x1) << (COEFF_FRACTION_BIT - 1))
+#define   DATA_HALF_ERROR       ((0x1) << (DATA_FRACTION_BIT - 1))
+
+/* Count if coefficients */
+#define   COEFF_COUNT       3
+#define   TMP_COEFF         2
+/*Channel Count*/
+#define   CF                2
+
+#define   Clip(acc,min,max) ((acc) > max ? max : ((acc) < min ? min : (acc)))
+
+struct IIR_param {
+    /*B coefficient array*/
+    int   b[COEFF_COUNT];
+    /*A coefficient array*/
+    int   a[COEFF_COUNT];
+    /*Circular buffer for channel input data*/
+    int   cx[CF][TMP_COEFF];
+    /*Circular buffer for channel output data*/
+    int   cy[CF][TMP_COEFF];
+};
+
+static int IIR_coefficients[2][COEFF_COUNT] = {
+       {/*B coefficients*/
+                36224809,    16150517,     1797672,
+       },
+       {/*A coefficients*/
+                16777216,    26740673,    10655109,
+       },
+
+};
+
+int audio_IIR_process(int input, int channel);
+void audio_IIR_init(void);
+
+#endif
+