diff --git a/src/driver/TP2825.c b/src/driver/TP2825.c
index 66dbd6d0..823a83e5 100644
--- a/src/driver/TP2825.c
+++ b/src/driver/TP2825.c
@@ -42,203 +42,149 @@ void TP2825_Config(int ch_sel, int is_pal) // ch_sel: 0=AV in; 1=Module bay
 
     if (is_pal) {
         I2C_Write(ADDR_TP2825, 0x02, 0xCE);
-        I2C_Write(ADDR_TP2825, 0x06, 0x32);
-        I2C_Write(ADDR_TP2825, 0x07, 0xC0);
-        I2C_Write(ADDR_TP2825, 0x08, 0x00);
-        I2C_Write(ADDR_TP2825, 0x09, 0x24);
-        I2C_Write(ADDR_TP2825, 0x0A, 0x48);
-        I2C_Write(ADDR_TP2825, 0x0B, 0xC0);
+        I2C_Write(ADDR_TP2825, 0x06, 0x34);
+        I2C_Write(ADDR_TP2825, 0x08, 0xD0);
+        I2C_Write(ADDR_TP2825, 0x07, 0x80);
+        I2C_Write(ADDR_TP2825, 0x0B, 0x80);
         I2C_Write(ADDR_TP2825, 0x0C, 0x53);
         I2C_Write(ADDR_TP2825, 0x0D, 0x11);
-        I2C_Write(ADDR_TP2825, 0x0E, 0x00);
-        I2C_Write(ADDR_TP2825, 0x0F, 0x00);
-
-        // I2C_Write(ADDR_TP2825, 0x15, 0x13);
+        //I2C_Write(ADDR_TP2825, 0x15, 0x13);
         I2C_Write(ADDR_TP2825, 0x16, 0x4A);
         I2C_Write(ADDR_TP2825, 0x17, 0xC0);
         I2C_Write(ADDR_TP2825, 0x18, 0x17);
         I2C_Write(ADDR_TP2825, 0x19, 0x20);
         I2C_Write(ADDR_TP2825, 0x1A, 0x17);
-        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
-        I2C_Write(ADDR_TP2825, 0x1D, 0x48);
-
         I2C_Write(ADDR_TP2825, 0x20, g_hw_stat.IS_TP2825_L ? 0x40 : 0xB0);
-        I2C_Write(ADDR_TP2825, 0x21, 0x86);
-        I2C_Write(ADDR_TP2825, 0x22, 0x38);
-        I2C_Write(ADDR_TP2825, 0x23, 0x3C);
-        // I2C_Write(ADDR_TP2825, 0x24, 0x56);
-        I2C_Write(ADDR_TP2825, 0x25, 0xFF);
+        I2C_Write(ADDR_TP2825, 0x22, 0x39);
+        //I2C_Write(ADDR_TP2825, 0x23, 0x3c);
         I2C_Write(ADDR_TP2825, 0x26, 0x02);
-        I2C_Write(ADDR_TP2825, 0x27, 0x2D);
-        I2C_Write(ADDR_TP2825, 0x28, 0xC5); //---
-        I2C_Write(ADDR_TP2825, 0x29, 0x18); //---
-        I2C_Write(ADDR_TP2825, 0x2A, 0x30); //---
+        I2C_Write(ADDR_TP2825, 0x28, 0x41);
+        I2C_Write(ADDR_TP2825, 0x2A, 0xB0); //diable color kill
         I2C_Write(ADDR_TP2825, 0x2B, 0x70);
-        I2C_Write(ADDR_TP2825, 0x2C, 0x1A);
         I2C_Write(ADDR_TP2825, 0x2D, 0x60);
         I2C_Write(ADDR_TP2825, 0x2E, 0x5E);
-        I2C_Write(ADDR_TP2825, 0x2F, 0x00);
-
         I2C_Write(ADDR_TP2825, 0x30, 0x7A);
         I2C_Write(ADDR_TP2825, 0x31, 0x4A);
         I2C_Write(ADDR_TP2825, 0x32, 0x4D);
         I2C_Write(ADDR_TP2825, 0x33, 0xF0);
-        I2C_Write(ADDR_TP2825, 0x34, 0x00);
         I2C_Write(ADDR_TP2825, 0x35, 0x65);
-        I2C_Write(ADDR_TP2825, 0x36, 0xDC);
-        I2C_Write(ADDR_TP2825, 0x37, 0x00);
-        I2C_Write(ADDR_TP2825, 0x38, 0x40);
         I2C_Write(ADDR_TP2825, 0x39, 0x84);
-        I2C_Write(ADDR_TP2825, 0x3A, 0x00);
-        I2C_Write(ADDR_TP2825, 0x3B, 0x03);
-        I2C_Write(ADDR_TP2825, 0x3C, 0x00);
-        I2C_Write(ADDR_TP2825, 0x3D, 0x60);
-        I2C_Write(ADDR_TP2825, 0x3E, 0x00);
-        I2C_Write(ADDR_TP2825, 0x3F, 0x00);
-
-        I2C_Write(ADDR_TP2825, 0x40, 0x00);
-        // I2C_Write(ADDR_TP2825, 0x41, 0x00);
-        I2C_Write(ADDR_TP2825, 0x42, 0x00);
-        I2C_Write(ADDR_TP2825, 0x43, 0x12);
-        I2C_Write(ADDR_TP2825, 0x44, 0x07);
-        I2C_Write(ADDR_TP2825, 0x45, 0x49);
-        I2C_Write(ADDR_TP2825, 0x46, 0x00);
-        I2C_Write(ADDR_TP2825, 0x47, 0x00);
-        I2C_Write(ADDR_TP2825, 0x48, 0x00);
-        I2C_Write(ADDR_TP2825, 0x49, 0x00);
-        I2C_Write(ADDR_TP2825, 0x4A, 0x00);
-        I2C_Write(ADDR_TP2825, 0x4B, 0x00);
-        I2C_Write(ADDR_TP2825, 0x4C, 0xC3);
-        I2C_Write(ADDR_TP2825, 0x4D, 0x01);
+        I2C_Write(ADDR_TP2825, 0x4C, 0x03);
+        I2C_Write(ADDR_TP2825, 0x4D, 0x03);
         I2C_Write(ADDR_TP2825, 0x4E, 0x37);
-        I2C_Write(ADDR_TP2825, 0x4F, 0x00);
+        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
+        I2C_Write(ADDR_TP2825, 0x1D, 0x48);
     } else {
         I2C_Write(ADDR_TP2825, 0x02, 0xCF);
-        I2C_Write(ADDR_TP2825, 0x06, 0x32);
-        I2C_Write(ADDR_TP2825, 0x07, 0xC0);
-        I2C_Write(ADDR_TP2825, 0x08, 0x00);
-        I2C_Write(ADDR_TP2825, 0x09, 0x24);
-        I2C_Write(ADDR_TP2825, 0x0A, 0x48);
-        I2C_Write(ADDR_TP2825, 0x0B, 0xC0);
+        I2C_Write(ADDR_TP2825, 0x06, 0x34);
+        I2C_Write(ADDR_TP2825, 0x08, 0xD0);
+        I2C_Write(ADDR_TP2825, 0x07, 0x80);
+        I2C_Write(ADDR_TP2825, 0x0B, 0x80);
         I2C_Write(ADDR_TP2825, 0x0C, 0x53);
         I2C_Write(ADDR_TP2825, 0x0D, 0x10);
-        I2C_Write(ADDR_TP2825, 0x0E, 0x00);
-        I2C_Write(ADDR_TP2825, 0x0F, 0x00);
-
-        // I2C_Write(ADDR_TP2825, 0x15, 0x13);
+        //I2C_Write(ADDR_TP2825, 0x15, 0x13);
         I2C_Write(ADDR_TP2825, 0x16, 0x3C);
         I2C_Write(ADDR_TP2825, 0x17, 0xC0);
         I2C_Write(ADDR_TP2825, 0x18, 0x13);
         I2C_Write(ADDR_TP2825, 0x19, 0xF0);
         I2C_Write(ADDR_TP2825, 0x1A, 0x07);
-        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
-        I2C_Write(ADDR_TP2825, 0x1D, 0x38);
-
         I2C_Write(ADDR_TP2825, 0x20, g_hw_stat.IS_TP2825_L ? 0x30 : 0xA0);
-        I2C_Write(ADDR_TP2825, 0x21, 0x86);
-        I2C_Write(ADDR_TP2825, 0x22, 0x38);
-        I2C_Write(ADDR_TP2825, 0x23, 0x3c);
-        // I2C_Write(ADDR_TP2825, 0x24, 0x56);
-        I2C_Write(ADDR_TP2825, 0x25, 0xFF);
+        I2C_Write(ADDR_TP2825, 0x22, 0x39);
+        //I2C_Write(ADDR_TP2825, 0x23, 0x3c);
         I2C_Write(ADDR_TP2825, 0x26, 0x12);
-        I2C_Write(ADDR_TP2825, 0x27, 0x2D);
-        I2C_Write(ADDR_TP2825, 0x28, 0xC5); //---
-        I2C_Write(ADDR_TP2825, 0x29, 0x18); //---
-        I2C_Write(ADDR_TP2825, 0x2A, 0x30); //---
+        I2C_Write(ADDR_TP2825, 0x28, 0x41);
+        I2C_Write(ADDR_TP2825, 0x2A, 0xB0); //diable color kill
         I2C_Write(ADDR_TP2825, 0x2B, 0x70);
-        I2C_Write(ADDR_TP2825, 0x2C, 0x1A);
         I2C_Write(ADDR_TP2825, 0x2D, 0x68);
         I2C_Write(ADDR_TP2825, 0x2E, 0x5E);
-        I2C_Write(ADDR_TP2825, 0x2F, 0x00);
-
         I2C_Write(ADDR_TP2825, 0x30, 0x62);
         I2C_Write(ADDR_TP2825, 0x31, 0xBB);
         I2C_Write(ADDR_TP2825, 0x32, 0x96);
         I2C_Write(ADDR_TP2825, 0x33, 0xC0);
-        I2C_Write(ADDR_TP2825, 0x34, 0x00);
         I2C_Write(ADDR_TP2825, 0x35, 0x65);
-        I2C_Write(ADDR_TP2825, 0x36, 0xDC);
-        I2C_Write(ADDR_TP2825, 0x37, 0x00);
-        I2C_Write(ADDR_TP2825, 0x38, 0x40);
         I2C_Write(ADDR_TP2825, 0x39, 0x84);
-        I2C_Write(ADDR_TP2825, 0x3A, 0x00);
-        I2C_Write(ADDR_TP2825, 0x3B, 0x03);
-        I2C_Write(ADDR_TP2825, 0x3C, 0x00);
-        I2C_Write(ADDR_TP2825, 0x3D, 0x60);
-        I2C_Write(ADDR_TP2825, 0x3E, 0x00);
-        I2C_Write(ADDR_TP2825, 0x3F, 0x00);
-
-        I2C_Write(ADDR_TP2825, 0x40, 0x00);
-        // I2C_Write(ADDR_TP2825, 0x41, 0x00);
-        I2C_Write(ADDR_TP2825, 0x42, 0x00);
-        I2C_Write(ADDR_TP2825, 0x43, 0x12);
-        I2C_Write(ADDR_TP2825, 0x44, 0x07);
-        I2C_Write(ADDR_TP2825, 0x45, 0x49);
-        I2C_Write(ADDR_TP2825, 0x46, 0x00);
-        I2C_Write(ADDR_TP2825, 0x47, 0x00);
-        I2C_Write(ADDR_TP2825, 0x48, 0x00);
-        I2C_Write(ADDR_TP2825, 0x49, 0x00);
-        I2C_Write(ADDR_TP2825, 0x4A, 0x00);
-        I2C_Write(ADDR_TP2825, 0x4B, 0x00);
         I2C_Write(ADDR_TP2825, 0x4C, 0x03);
         I2C_Write(ADDR_TP2825, 0x4D, 0x03);
         I2C_Write(ADDR_TP2825, 0x4E, 0x37);
-        I2C_Write(ADDR_TP2825, 0x4F, 0x01);
+        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
+        I2C_Write(ADDR_TP2825, 0x1D, 0x38);
     }
-
-    I2C_Write(ADDR_TP2825, 0x06, TP2825_REG06);
+    // Disable AGC
+    // I2C_Write(ADDR_TP2825, 0x06, TP2825_REG06);
 }
 
 void TP2825_Switch_Mode(int is_pal) {
     if (is_pal) {
         I2C_Write(ADDR_TP2825, 0x02, 0xCE);
+        I2C_Write(ADDR_TP2825, 0x06, 0x34);
+        I2C_Write(ADDR_TP2825, 0x08, 0xD0);
+        I2C_Write(ADDR_TP2825, 0x07, 0x80);
+        I2C_Write(ADDR_TP2825, 0x0B, 0x80);
+        I2C_Write(ADDR_TP2825, 0x0C, 0x53);
         I2C_Write(ADDR_TP2825, 0x0D, 0x11);
-
-        I2C_Write(ADDR_TP2825, 0x19, 0x20);
-        I2C_Write(ADDR_TP2825, 0x1A, 0x17);
-        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
-        I2C_Write(ADDR_TP2825, 0x1D, 0x48);
+        //I2C_Write(ADDR_TP2825, 0x15, 0x13);
         I2C_Write(ADDR_TP2825, 0x16, 0x4A);
+        I2C_Write(ADDR_TP2825, 0x17, 0xC0);
         I2C_Write(ADDR_TP2825, 0x18, 0x17);
-
+        I2C_Write(ADDR_TP2825, 0x19, 0x20);
+        I2C_Write(ADDR_TP2825, 0x1A, 0x17);
         I2C_Write(ADDR_TP2825, 0x20, g_hw_stat.IS_TP2825_L ? 0x40 : 0xB0);
+        I2C_Write(ADDR_TP2825, 0x22, 0x39);
+        //I2C_Write(ADDR_TP2825, 0x23, 0x3c);
         I2C_Write(ADDR_TP2825, 0x26, 0x02);
+        I2C_Write(ADDR_TP2825, 0x28, 0x41);
+        I2C_Write(ADDR_TP2825, 0x2A, 0xB0); //diable color kill
+        I2C_Write(ADDR_TP2825, 0x2B, 0x70);
         I2C_Write(ADDR_TP2825, 0x2D, 0x60);
-
+        I2C_Write(ADDR_TP2825, 0x2E, 0x5E);
         I2C_Write(ADDR_TP2825, 0x30, 0x7A);
         I2C_Write(ADDR_TP2825, 0x31, 0x4A);
         I2C_Write(ADDR_TP2825, 0x32, 0x4D);
         I2C_Write(ADDR_TP2825, 0x33, 0xF0);
-
-        I2C_Write(ADDR_TP2825, 0x4C, 0xC3);
-        I2C_Write(ADDR_TP2825, 0x4D, 0x01);
-        I2C_Write(ADDR_TP2825, 0x4F, 0x00);
+        I2C_Write(ADDR_TP2825, 0x35, 0x65);
+        I2C_Write(ADDR_TP2825, 0x39, 0x84);
+        I2C_Write(ADDR_TP2825, 0x4C, 0x03);
+        I2C_Write(ADDR_TP2825, 0x4D, 0x03);
+        I2C_Write(ADDR_TP2825, 0x4E, 0x37);
+        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
+        I2C_Write(ADDR_TP2825, 0x1D, 0x48);
     } else {
         I2C_Write(ADDR_TP2825, 0x02, 0xCF);
+        I2C_Write(ADDR_TP2825, 0x06, 0x34);
+        I2C_Write(ADDR_TP2825, 0x08, 0xD0);
+        I2C_Write(ADDR_TP2825, 0x07, 0x80);
+        I2C_Write(ADDR_TP2825, 0x0B, 0x80);
+        I2C_Write(ADDR_TP2825, 0x0C, 0x53);
         I2C_Write(ADDR_TP2825, 0x0D, 0x10);
-
-        I2C_Write(ADDR_TP2825, 0x19, 0xF0);
-        I2C_Write(ADDR_TP2825, 0x1A, 0x07);
-        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
-        I2C_Write(ADDR_TP2825, 0x1D, 0x38);
+        //I2C_Write(ADDR_TP2825, 0x15, 0x13);
         I2C_Write(ADDR_TP2825, 0x16, 0x3C);
+        I2C_Write(ADDR_TP2825, 0x17, 0xC0);
         I2C_Write(ADDR_TP2825, 0x18, 0x13);
-
+        I2C_Write(ADDR_TP2825, 0x19, 0xF0);
+        I2C_Write(ADDR_TP2825, 0x1A, 0x07);
         I2C_Write(ADDR_TP2825, 0x20, g_hw_stat.IS_TP2825_L ? 0x30 : 0xA0);
+        I2C_Write(ADDR_TP2825, 0x22, 0x39);
+        //I2C_Write(ADDR_TP2825, 0x23, 0x3c);
         I2C_Write(ADDR_TP2825, 0x26, 0x12);
+        I2C_Write(ADDR_TP2825, 0x28, 0x41);
+        I2C_Write(ADDR_TP2825, 0x2A, 0xB0); //diable color kill
+        I2C_Write(ADDR_TP2825, 0x2B, 0x70);
         I2C_Write(ADDR_TP2825, 0x2D, 0x68);
-
+        I2C_Write(ADDR_TP2825, 0x2E, 0x5E);
         I2C_Write(ADDR_TP2825, 0x30, 0x62);
         I2C_Write(ADDR_TP2825, 0x31, 0xBB);
         I2C_Write(ADDR_TP2825, 0x32, 0x96);
         I2C_Write(ADDR_TP2825, 0x33, 0xC0);
-
+        I2C_Write(ADDR_TP2825, 0x35, 0x65);
+        I2C_Write(ADDR_TP2825, 0x39, 0x84);
         I2C_Write(ADDR_TP2825, 0x4C, 0x03);
         I2C_Write(ADDR_TP2825, 0x4D, 0x03);
-        I2C_Write(ADDR_TP2825, 0x4F, 0x01);
+        I2C_Write(ADDR_TP2825, 0x4E, 0x37);
+        I2C_Write(ADDR_TP2825, 0x1C, 0x09);
+        I2C_Write(ADDR_TP2825, 0x1D, 0x38);
     }
-
-    I2C_Write(ADDR_TP2825, 0x06, TP2825_REG06);
+    // Diable AGC
+    // I2C_Write(ADDR_TP2825, 0x06, TP2825_REG06);
 }
 
 void TP2825_Switch_CH(uint8_t sel) // 0 = AV in; 1 = Module bay