touch/focaltech: Compressed MS and add SS_WATER.

Change list:
1. Compress the mutual-sensing data.
  The purpose is to reduce the data count of heatmap through SPI.
  To compress the mutual-sensing data in firmware side, and to
  decompress it in kernel driver side.
  It reduces the transferring data form 1088 bytes to 50 bytes/per finger).
2. Add water-on self-sensing data.

Bug: 228953908
Test: Touch, v4l, twoshay works well.

Change-Id: I18ee794aa9ef098764e8483f87f6697d5bff06d0

6 files changed, 341 insertions, 106 deletions

diff --git a/ft3658/focaltech_common.h b/ft3658/focaltech_common.h
index 5270523..9322fd5 100644
--- a/ft3658/focaltech_common.h
+++ b/ft3658/focaltech_common.h
@@ -75,6 +75,7 @@
 #define FTS_LPTW_BUF_LEN                    (max(FTS_LPTW_E1_BUF_LEN, FTS_LPTW_E2_BUF_LEN))
 
 #define FILE_NAME_LENGTH                    128
+#define FTS_MESSAGE_LENGTH                  128
 #define ENABLE                              1
 #define DISABLE                             0
 #define VALID                               1
@@ -141,10 +142,10 @@
                                                     // bit 5   : GLOVE
                                                     // bit 6   : STTW
                                                     // bit 7   : LPWG
-#define FTS_CAP_DATA_OFFSET                 91
+#define FTS_CAP_DATA_LEN                    91
 #define FTS_SELF_DATA_LEN                   68
 #define FTS_PRESSURE_SCALE                  85      // 255 / 3
-#define FTS_CUSTOMER_STATUS_LEN             2
+#define FTS_CUSTOMER_STATUS_LEN             4
 #define FTS_CUSTOMER_STATUS1_MASK           0x0F
 
 #define FTS_SYSFS_ECHO_ON(buf)      (buf[0] == '1')
@@ -222,6 +223,17 @@ enum FW_GRIP_MODE {
     FW_GRIP_FORCE_ENABLE,
 };
 
+/* Firmware Heatmap mode.
+ * 0 - Disable fw heatmap.
+ * 1 - Enable fw compressed heatmap.
+ * 2 - Enable fw uncompressed heatmap.
+ */
+enum FW_HEATMAP_MODE {
+    FW_HEATMAP_MODE_DISABLE,
+    FW_HEATMAP_MODE_COMPRESSED,
+    FW_HEATMAP_MODE_UNCOMPRESSED,
+};
+
 /* Firmware Palm rejection mode.
  * 0 - Disable fw palm rejection.
  * 1 - Enable fw palm rejection.
diff --git a/ft3658/focaltech_core.c b/ft3658/focaltech_core.c
index e49fa81..8dd8ae4 100644
--- a/ft3658/focaltech_core.c
+++ b/ft3658/focaltech_core.c
@@ -443,7 +443,7 @@ static void fts_show_touch_buffer(u8 *data, int datalen)
     }
 
     for (i = 0; i < datalen; i++) {
-        count += snprintf(tmpbuf + count, 1024 - count, "%02X,", data[i]);
+        count += scnprintf(tmpbuf + count, 1024 - count, "%02X,", data[i]);
         if (count >= 1024)
             break;
     }
@@ -796,6 +796,15 @@ static int fts_read_touchdata(struct fts_ts_data *data)
     u8 current_hopping = 0;
     u8 new_hopping = 0;
     int i;
+
+    if (data->work_mode == FTS_REG_WORKMODE_WORK_VALUE) {
+        cmd[0] = FTS_REG_CUSTOMER_STATUS;
+        fts_read(cmd,1, regB2_data, FTS_CUSTOMER_STATUS_LEN);
+#if IS_ENABLED(CONFIG_TOUCHSCREEN_OFFLOAD) || \
+    IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
+        data->compress_heatmap_wlen = (regB2_data[2] << 8) + regB2_data[3];
+#endif
+    }
 #endif
 
     cmd[0] = FTS_CMD_READ_TOUCH_DATA;
@@ -823,8 +832,6 @@ static int fts_read_touchdata(struct fts_ts_data *data)
 
 #if IS_ENABLED(GOOGLE_REPORT_MODE)
     if (data->work_mode == FTS_REG_WORKMODE_WORK_VALUE) {
-        cmd[0] = FTS_REG_CUSTOMER_STATUS;
-        fts_read(cmd,1, regB2_data, FTS_CUSTOMER_STATUS_LEN);
         check_regB2_status[0] = regB2_data[0] ^ data->current_host_status[0] ;
         if (check_regB2_status[0]) { // current_status is different with previous_status
             for(i = STATUS_HOPPING; i < STATUS_CNT_END; i++) {
@@ -1348,21 +1355,71 @@ static void fts_show_heatmap_data(struct fts_ts_data *ts_data) {
 }
 #endif /* GOOGLE_HEATMAP_DEBUG */
 
-extern void transpose_raw(u8 *src, u8 *dist, int tx, int rx);
+static int fts_ptflib_decoder(struct fts_ts_data *ts_data, const u16 *in_array,
+    const int in_array_size, u16 *out_array, const int out_array_max_size)
+{
+    const u16 ESCAPE_MASK = 0xF000;
+    const u16 ESCAPE_BIT = 0x8000;
+
+    int i;
+    int j;
+    int out_array_size = 0;
+    u16 prev_word = 0;
+    u16 repetition = 0;
+    u16 *temp_out_array = out_array;
+
+    for (i = 0; i < in_array_size; i++) {
+        /* The data form firmware is big-endian, and needs to transfer it to
+         * little-endian.
+         */
+        u16 curr_word = (u16)(*((u8*)&in_array[i]) << 8) +
+                        *((u8*)&in_array[i] + 1);
+        if ((curr_word & ESCAPE_MASK) == ESCAPE_BIT) {
+            repetition = (curr_word & ~ESCAPE_MASK);
+            if (out_array_size + repetition > out_array_max_size)
+                break;
+            for (j = 0; j < repetition; j++) {
+                *temp_out_array++ = prev_word;
+                out_array_size++;
+            }
+        } else {
+            if (out_array_size >= out_array_max_size)
+                break;
+            *temp_out_array++ = curr_word;
+            out_array_size++;
+            prev_word = curr_word;
+        }
+    }
+
+    if (i != in_array_size || out_array_size != out_array_max_size) {
+        FTS_ERROR("%d (in=%d, out=%d, rep=%d, out_max=%d).\n",
+            i, in_array_size, out_array_size,
+            repetition, out_array_max_size);
+        memset(out_array, 0, out_array_max_size * sizeof(u16));
+        return -1;
+    }
+
+    return out_array_size;
+}
+
+extern void transpose_raw(u8 *src, u8 *dist, int tx, int rx, bool big_endian);
 static int fts_get_heatmap(struct fts_ts_data *ts_data) {
     int ret = 0;
     int i;
     int idx_buff = 0;
     int node_num = 0;
     int self_node = 0;
+    int mutual_data_size = 0;
     int self_data_size = 0;
-    int total_heatmap_data_size = 0;
+    int total_data_size = 0;
     u8 cmd[1] = {0};
     u8 tx = ts_data->pdata->tx_ch_num;
     u8 rx = ts_data->pdata->rx_ch_num;
-    int idx_ms_raw = FTS_CAP_DATA_OFFSET;
+    int idx_ms_raw = 0;
     int idx_ss_tx_raw = 0;
     int idx_ss_rx_raw = 0;
+    int idx_water_ss_tx_raw = 0;
+    int idx_water_ss_rx_raw = 0;
 
 #if IS_ENABLED(GOOGLE_HEATMAP_DEBUG)
     FTS_FUNC_ENTER();
@@ -1370,19 +1427,20 @@ static int fts_get_heatmap(struct fts_ts_data *ts_data) {
 
     node_num = tx * rx;
     self_node = tx + rx;
-    idx_ss_tx_raw = FTS_CAP_DATA_OFFSET + (node_num + rx) * sizeof(u16);
-    idx_ss_rx_raw = FTS_CAP_DATA_OFFSET + node_num * sizeof(u16);
+    /* The mutual sensing raw data size : 16*34*2=1088 */
+    mutual_data_size = node_num * sizeof(u16);
+    /* The self sensing raw data size : 68*2=136 */
     self_data_size = FTS_SELF_DATA_LEN * sizeof(u16);
-    total_heatmap_data_size =
-        FTS_CAP_DATA_OFFSET + node_num * sizeof(u16) + self_data_size;
-
-    /* The format of heatmap from touch chip
-     *
-     * |- cap header (91) -|- mutual data(tx*rx*2) -|- cap(on) data(68*2) -|
-     * |-                 -|-             16*34*2  -|- (34+16)*2          -|
-     *
-     * Only needs mutual data and cap(on) data.
-     */
+    /* The index of mutual sensing data : 91+(68*2)*2=363 */
+    idx_ms_raw = FTS_CAP_DATA_LEN + self_data_size * 2;
+    /* The tx index of water self sensing data : 91+34*2=159 */
+    idx_water_ss_tx_raw = FTS_CAP_DATA_LEN + rx * sizeof(u16);
+    /* The rx index of water self sensing data : 91 */
+    idx_water_ss_rx_raw = FTS_CAP_DATA_LEN;
+    /* The tx index of normal self sensing data : 91+68*2+34*2=295 */
+    idx_ss_tx_raw = FTS_CAP_DATA_LEN + self_data_size + rx * sizeof(u16);
+    /* The rx index of normal self sensing data : 91+68*2=227 */
+    idx_ss_rx_raw = FTS_CAP_DATA_LEN + self_data_size;
 
     if (!ts_data->heatmap_buff) {
         FTS_ERROR("The heatmap_buff is not allocated!!");
@@ -1390,41 +1448,107 @@ static int fts_get_heatmap(struct fts_ts_data *ts_data) {
         goto exit;
     }
 
-    /* Get total heatmap data (cap header(91) + ms + ss). */
-    cmd[0] = FTS_CMD_READ_TOUCH_DATA;
-    ret = fts_read(cmd, 1, ts_data->heatmap_raw, total_heatmap_data_size);
-    if (ret < 0) {
-        FTS_ERROR("Failed to get heatmap raw data, ret=%d.", ret);
+    if (!ts_data->fw_heatmap_mode) {
+        FTS_ERROR("The firmware heatmap is not enabled!!");
+        ret = -EINVAL;
         goto exit;
     }
 
+    cmd[0] = FTS_CMD_READ_TOUCH_DATA;
+    if (ts_data->fw_heatmap_mode == FW_HEATMAP_MODE_UNCOMPRESSED) {
+        /* The format of uncompressed heatmap from touch chip.
+         *
+         * |- cap header (91) -|- Water-SS -|- Normal-SS -|- Normal-MS -|
+         * |-        91       -|-   68*2   -|-   68*2    -|-  16*34*2  -|
+         */
+
+        /* Total touch data: (cap header(91) + heatmap(N-MS + W-SS + N-SS)). */
+        total_data_size = FTS_CAP_DATA_LEN + self_data_size * 2 +
+                          mutual_data_size;
+        ret = fts_read(cmd, 1, ts_data->heatmap_raw, total_data_size);
+        if (ret < 0) {
+            FTS_ERROR("Failed to get heatmap raw data, ret=%d.", ret);
+            ret = -EIO;
+            goto exit;
+        }
+        /* Get the self-sensing type. */
+        ts_data->self_sensing_type =
+            ts_data->heatmap_raw[FTS_CAP_DATA_LEN - 1] & 0x80;
+
+        /*
+         * transform the order of MS from RX->TX, the output data is keep
+         * big-endian.
+         */
+        transpose_raw(ts_data->heatmap_raw + idx_ms_raw, ts_data->trans_raw,
+            tx, rx, true);
+    } else {
+        /* The format of compressed heatmap from touch chip.
+         *
+         * |- cap header -|- Water-SS -|- Normal-SS -|- compressed heatmap(MS)-|
+         * |-     91     -|-   68*2   -|-   68*2    -|- (B2[1]<<8+B2[2])*2    -|
+         */
+
+        /* Total touch data:(cap header + W-SS + N-SS + compressed heatmap(N-MS)
+         */
+        total_data_size = FTS_CAP_DATA_LEN +
+                          self_data_size * 2 +
+                          ts_data->compress_heatmap_wlen * sizeof(u16);
+
+        ret = fts_read(cmd, 1, ts_data->heatmap_raw, total_data_size);
+        if (ret < 0) {
+            FTS_ERROR("Failed to get compressed heatmap raw data,ret=%d.", ret);
+            ret = -EIO;
+            goto exit;
+        }
+        if (ts_data->compress_heatmap_wlen == 0) {
+            FTS_DEBUG("Warning : The compressed heatmap length is 0!!");
+            goto exit;
+        }
+
+        /* Get the self-sensing type. */
+        ts_data->self_sensing_type =
+            ts_data->heatmap_raw[FTS_CAP_DATA_LEN - 1] & 0x80;
+
+        /* decode the compressed data from heatmap_raw to heatmap_buff. */
+        fts_ptflib_decoder(ts_data,
+            (u16*)(&ts_data->heatmap_raw[idx_ms_raw]),
+            ts_data->compress_heatmap_wlen,
+            ts_data->heatmap_buff,
+            mutual_data_size / sizeof(u16));
+
+        /* MS: Transform the order from RX->TX. */
+        /* After decoding, the data become to little-endian, but the output of
+         * transpose_raw is big-endian.
+         */
+        transpose_raw(&((u8*)ts_data->heatmap_buff)[0], ts_data->trans_raw,
+            tx, rx, false);
+    }
 #if IS_ENABLED(GOOGLE_HEATMAP_DEBUG)
-    FTS_DEBUG("start to copy matual data,idx_buff=%d,idx_ms_raw=%d.",
+    FTS_DEBUG("Copy matual data,idx_buff=%d,idx_ms_raw=%d.",
         idx_buff, idx_ms_raw);
 #endif
-    /* MS: Transform the order from RX->TX. */
-    transpose_raw(ts_data->heatmap_raw + idx_ms_raw, ts_data->trans_raw, tx, rx);
-    /* Copy mutual data. */
+
+    /* copy mutual sensing data. */
     for (i = 0; i < node_num; i++) {
         ((u16*)ts_data->heatmap_buff)[idx_buff++] =
             (u16)(ts_data->trans_raw[(i * 2)] << 8) +
             ts_data->trans_raw[(i * 2) + 1];
     }
 
-    /* Copy tx self data first */
+    /* copy tx of Normal-SS. */
 #if IS_ENABLED(GOOGLE_HEATMAP_DEBUG)
-    FTS_DEBUG("Start to copy the tx self data,idx_buff=%d,idx_ss_tx_raw=%d.",
+    FTS_DEBUG("Copy the tx self data,idx_buff=%d,idx_ss_tx_raw=%d.",
         idx_buff, idx_ss_tx_raw);
 #endif
     for (i = 0 ; i < tx; i++) {
-          ((u16*)ts_data->heatmap_buff)[idx_buff++] =
-             (u16)(ts_data->heatmap_raw[idx_ss_tx_raw + (i * 2)] << 8) +
-             ts_data->heatmap_raw[idx_ss_tx_raw +(i * 2) + 1];
+        ((u16*)ts_data->heatmap_buff)[idx_buff++] =
+            (u16)(ts_data->heatmap_raw[idx_ss_tx_raw + (i * 2)] << 8) +
+            ts_data->heatmap_raw[idx_ss_tx_raw +(i * 2) + 1];
     }
 
-    /* Then copy rx self data */
+    /* copy rx of Normal-SS. */
 #if IS_ENABLED(GOOGLE_HEATMAP_DEBUG)
-    FTS_DEBUG("Start to copy the rx self data,idx_buff=%d,idx_ss_rx_raw=%d.",
+    FTS_DEBUG("Copy the rx self data,idx_buff=%d,idx_ss_rx_raw=%d.",
         idx_buff, idx_ss_rx_raw);
 #endif
     for (i = 0 ; i < rx; i++) {
@@ -1432,11 +1556,33 @@ static int fts_get_heatmap(struct fts_ts_data *ts_data) {
             (u16)(ts_data->heatmap_raw[idx_ss_rx_raw + (i * 2)] << 8) +
             ts_data->heatmap_raw[idx_ss_rx_raw + (i * 2) + 1];
     }
+
+    /* copy tx of Water-SS. */
 #if IS_ENABLED(GOOGLE_HEATMAP_DEBUG)
-    /* Show the heatmap data for debugging. */
-    fts_show_heatmap_data(ts_data);
+    FTS_DEBUG("Copy the tx of Water-SS,idx_buff=%d,idx_water_ss_tx_raw=%d.",
+        idx_buff, idx_water_ss_tx_raw);
 #endif
+    for (i = 0 ; i < tx; i++) {
+        ((u16*)ts_data->heatmap_buff)[idx_buff++] =
+            (u16)(ts_data->heatmap_raw[idx_water_ss_tx_raw + (i * 2)] << 8) +
+            ts_data->heatmap_raw[idx_water_ss_tx_raw +(i * 2) + 1];
+    }
 
+    /* copy rx of Water-SS. */
+#if IS_ENABLED(GOOGLE_HEATMAP_DEBUG)
+    FTS_DEBUG("Copy the rx of Water-SS,idx_buff=%d,idx_water_ss_rx_raw=%d.",
+        idx_buff, idx_water_ss_rx_raw);
+#endif
+    for (i = 0 ; i < rx; i++) {
+        ((u16*)ts_data->heatmap_buff)[idx_buff++] =
+            (u16)(ts_data->heatmap_raw[idx_water_ss_rx_raw + (i * 2)] << 8) +
+            ts_data->heatmap_raw[idx_water_ss_rx_raw + (i * 2) + 1];
+    }
+    /* The format of heatmap data (U16) of heatmap_buff is:
+     *
+     * |-    MS   -|- Normal-SS -|- Water-SS -|
+     * |-  16*34  -|-   16+34   -|-  16+34   -|
+     */
 #if IS_ENABLED(GOOGLE_HEATMAP_DEBUG)
     FTS_FUNC_EXIT();
 #endif
@@ -1845,8 +1991,7 @@ static int fts_report_buffer_init(struct fts_ts_data *ts_data)
     int events_num = 0;
 
     point_num = FTS_MAX_POINTS_SUPPORT;
-    ts_data->pnt_buf_size = FTS_TOUCH_DATA_LEN + FTS_GESTURE_DATA_LEN;
-
+    ts_data->pnt_buf_size = FTS_CAP_DATA_LEN;
     ts_data->point_buf = (u8 *)kzalloc(ts_data->pnt_buf_size + 1, GFP_KERNEL);
     if (!ts_data->point_buf) {
         FTS_ERROR("failed to alloc memory for point buf");
@@ -2704,7 +2849,8 @@ static int fts_ts_probe_entry(struct fts_ts_data *ts_data)
     memset(ts_data->current_host_status, 0, FTS_CUSTOMER_STATUS_LEN);
 #endif
 #if IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
-    ts_data->enable_fw_heatmap = false;
+    ts_data->fw_heatmap_mode = FW_HEATMAP_MODE_COMPRESSED;
+    ts_data->compress_heatmap_wlen = 0;
 #endif
     ts_data->enable_fw_grip = FW_GRIP_ENABLE;
     ts_data->enable_fw_palm = FW_GRIP_ENABLE;
@@ -2745,35 +2891,46 @@ static int fts_ts_probe_entry(struct fts_ts_data *ts_data)
 #if IS_ENABLED(CONFIG_TOUCHSCREEN_OFFLOAD) || \
     IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
 
+    /* |-   MS    -|- Normal-SS -|- Water-SS  -|
+     * |- Tx*Rx*2 -|- (Tx+Rx)*2 -|- (Tx+Rx)*2 -|
+     * Total size = 1288 bytes
+     */
     if (!ts_data->heatmap_buff) {
-        int heatmap_buff_size = sizeof(u16) *
+        ts_data->heatmap_buff_size = sizeof(u16) *
             ((ts_data->pdata->tx_ch_num * ts_data->pdata->rx_ch_num) +
-            ts_data->pdata->tx_ch_num + ts_data->pdata->rx_ch_num);
-        FTS_DEBUG("Allocate heatmap_buff size=%d\n", heatmap_buff_size);
-        ts_data->heatmap_buff = kmalloc(heatmap_buff_size, GFP_KERNEL);
+            (ts_data->pdata->tx_ch_num + ts_data->pdata->rx_ch_num) * 2);
+        FTS_DEBUG("Allocate heatmap_buff size=%d\n", ts_data->heatmap_buff_size);
+        ts_data->heatmap_buff = kmalloc(ts_data->heatmap_buff_size, GFP_KERNEL);
         if (!ts_data->heatmap_buff) {
             FTS_ERROR("allocate heatmap_buff failed\n");
             goto err_heatmap_buff;
         }
     }
 
+    /* |- cap header (91) -|- Water-SS -|- Normal-SS -|- Normal-MS -|
+     * |-        91       -|-   68*2   -|-   68*2    -|-  16*34*2  -|
+     * Total size = 1379 bytes
+     */
     if (!ts_data->heatmap_raw) {
         int node_num = ts_data->pdata->tx_ch_num * ts_data->pdata->rx_ch_num;
-        int heatmap_raw_size = FTS_CAP_DATA_OFFSET +
-            ((node_num + FTS_SELF_DATA_LEN) * sizeof(u16));
-        FTS_DEBUG("Allocate heatmap_raw size=%d\n", heatmap_raw_size);
-        ts_data->heatmap_raw = kmalloc(heatmap_raw_size, GFP_KERNEL);
+        ts_data->heatmap_raw_size = FTS_CAP_DATA_LEN +
+            ((node_num + FTS_SELF_DATA_LEN * 2) * sizeof(u16));
+        FTS_DEBUG("Allocate heatmap_raw size=%d\n", ts_data->heatmap_raw_size);
+        ts_data->heatmap_raw = kmalloc(ts_data->heatmap_raw_size, GFP_KERNEL);
         if (!ts_data->heatmap_raw) {
             FTS_ERROR("allocate heatmap_raw failed\n");
             goto err_heatmap_raw;
         }
     }
 
+    /* |-   MS    -|
+     * |- 16*34*2 -|
+     */
     if (!ts_data->trans_raw) {
         int node_num = ts_data->pdata->tx_ch_num * ts_data->pdata->rx_ch_num;
-        int trans_raw_size = sizeof(u16) * node_num;
-        FTS_DEBUG("Allocate trans_raw size=%d\n", trans_raw_size);
-        ts_data->trans_raw = kmalloc(trans_raw_size, GFP_KERNEL);
+        ts_data->trans_raw_size = node_num * sizeof(u16);
+        FTS_DEBUG("Allocate trans_raw size=%d\n", ts_data->trans_raw_size);
+        ts_data->trans_raw = kmalloc(ts_data->trans_raw_size, GFP_KERNEL);
         if (!ts_data->trans_raw) {
             FTS_ERROR("allocate trans_raw failed\n");
             goto err_trans_raw;
@@ -3115,20 +3272,53 @@ static void fts_update_host_feature_setting(struct fts_ts_data *ts_data,
         ts_data->current_host_status[1] &= ~(1 << fw_mode_setting);
 }
 
-int fts_set_heatmap_mode(struct fts_ts_data *ts_data, bool en)
+int fts_set_heatmap_mode(struct fts_ts_data *ts_data, u8 heatmap_mode)
 {
     int ret = 0;
-    u8 value = en ? ENABLE : DISABLE;
-    u8 reg = FTS_REG_HEATMAP_9E;
-
-    ret = fts_write_reg_safe(reg, value);
+    u8 value_heatmap = 0;
+    u8 value_compressed = 0;
+    u8 reg_heatmap = FTS_REG_HEATMAP_9E;
+    u8 reg_compressed = FTS_REG_HEATMAP_ED;
+    int count = 0;
+    char tmpbuf[FTS_MESSAGE_LENGTH];
+
+    switch (heatmap_mode) {
+      case FW_HEATMAP_MODE_DISABLE:
+          value_heatmap = DISABLE;
+          count += scnprintf(tmpbuf + count, FTS_MESSAGE_LENGTH - count,
+              "Disable fw_heatmap");
+          break;
+      case FW_HEATMAP_MODE_COMPRESSED:
+          value_heatmap = ENABLE;
+          value_compressed = ENABLE;
+          count += scnprintf(tmpbuf + count, FTS_MESSAGE_LENGTH - count,
+              "Enable compressed fw_heatmap");
+          break;
+      case FW_HEATMAP_MODE_UNCOMPRESSED:
+          value_heatmap = ENABLE;
+          value_compressed = DISABLE;
+          count += scnprintf(tmpbuf + count, FTS_MESSAGE_LENGTH - count,
+              "Enable uncompressed fw_heatmap");
+          break;
+      default:
+          FTS_ERROR("The input heatmap more(%d) is invalid.", heatmap_mode);
+          return -EINVAL;
+    }
+
+    ret = fts_write_reg_safe(reg_compressed, value_compressed);
+    if (ret) {
+        goto exit;
+    }
+    ret = fts_write_reg_safe(reg_heatmap, value_heatmap);
     if (ret == 0) {
-      ts_data->enable_fw_heatmap = en;
-      fts_update_host_feature_setting(ts_data, en, FW_HEATMAP);
+      ts_data->fw_heatmap_mode = heatmap_mode;
+      fts_update_host_feature_setting(ts_data, value_heatmap, FW_HEATMAP);
     }
 
-    FTS_DEBUG("%s fw_heatmap %s.\n", en ? "Enable" : "Disable",
-        (ret==0) ? "successfully" : "unsuccessfully");
+exit:
+    FTS_DEBUG("%s %s.\n", tmpbuf,
+        (ret == 0) ? "successfully" : "unsuccessfully");
+
     return ret;
 }
 
@@ -3214,7 +3404,7 @@ int fts_set_glove_mode(struct fts_ts_data *ts_data, bool en)
 void fts_update_feature_setting(struct fts_ts_data *ts_data)
 {
 #if IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
-    fts_set_heatmap_mode(ts_data, true);
+    fts_set_heatmap_mode(ts_data, FW_HEATMAP_MODE_COMPRESSED);
 #endif
 
     fts_set_grip_mode(ts_data, ts_data->enable_fw_grip);
@@ -3250,10 +3440,6 @@ static int fts_ts_suspend(struct device *dev)
     fts_esdcheck_suspend();
 #endif
 
-#if IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
-    fts_set_heatmap_mode(ts_data, false);
-#endif
-
     if (ts_data->gesture_mode) {
         fts_gesture_suspend(ts_data);
     } else {
@@ -3269,6 +3455,10 @@ static int fts_ts_suspend(struct device *dev)
 
         /* Disable irq */
         fts_irq_disable();
+
+#if IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
+    fts_set_heatmap_mode(ts_data, FW_HEATMAP_MODE_DISABLE);
+#endif
         FTS_DEBUG("make TP enter into sleep mode");
         ret = fts_write_reg(FTS_REG_POWER_MODE, FTS_REG_POWER_MODE_SLEEP);
         if (ret < 0)
diff --git a/ft3658/focaltech_core.h b/ft3658/focaltech_core.h
index cb2241e..356482f 100644
--- a/ft3658/focaltech_core.h
+++ b/ft3658/focaltech_core.h
@@ -278,7 +278,8 @@ struct fts_ts_data {
     u8 work_mode;
 
 #if IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
-    bool enable_fw_heatmap;
+    u8 fw_heatmap_mode;
+    int compress_heatmap_wlen;
 #endif
     u8 enable_fw_grip;
     u8 enable_fw_palm;
@@ -289,8 +290,12 @@ struct fts_ts_data {
 #if IS_ENABLED(CONFIG_TOUCHSCREEN_OFFLOAD) || \
     IS_ENABLED(CONFIG_TOUCHSCREEN_HEATMAP)
     u8 *heatmap_raw;
+    u16 heatmap_raw_size;
     u8 *trans_raw;
+    u16 trans_raw_size;
     u16 *heatmap_buff;
+    u16 heatmap_buff_size;
+    u8 self_sensing_type;
 #endif
 #if IS_ENABLED(CONFIG_TOUCHSCREEN_OFFLOAD)
     struct touch_offload_context offload;
@@ -389,7 +394,7 @@ int fts_gesture_suspend(struct fts_ts_data *ts_data);
 int fts_gesture_resume(struct fts_ts_data *ts_data);
 
 /* Heatmap */
-int fts_set_heatmap_mode(struct fts_ts_data *ts_data, bool en);
+int fts_set_heatmap_mode(struct fts_ts_data *ts_data, u8 heatmap_mode);
 int fts_set_grip_mode(struct fts_ts_data *ts_datam, u8 grip_mode);
 int fts_set_palm_mode(struct fts_ts_data *ts_data, u8 palm_mode);
 int fts_set_continuous_mode(struct fts_ts_data *ts_data, bool en);
diff --git a/ft3658/focaltech_ex_fun.c b/ft3658/focaltech_ex_fun.c
index dc59085..da22f2a 100644
--- a/ft3658/focaltech_ex_fun.c
+++ b/ft3658/focaltech_ex_fun.c
@@ -2090,6 +2090,7 @@ static ssize_t proc_heatmap_onoff_read(struct file *filp,
     int ret = 0;
     char tmpbuf[PROC_BUF_SIZE] = { 0 };
     u8 mode = 0;
+    u8 compressed = 0;
     loff_t pos = *ppos;
 
     if (pos)
@@ -2097,11 +2098,21 @@ static ssize_t proc_heatmap_onoff_read(struct file *filp,
 
     ret = fts_read_reg(FTS_REG_HEATMAP_9E, &mode);
     if (ret < 0) {
-        FTS_ERROR("read reg_0x9E fails");
+        FTS_ERROR("read reg_0x%X fails", FTS_REG_HEATMAP_9E);
         return ret;
     }
 
-    cnt += snprintf(tmpbuf + cnt,  PROC_BUF_SIZE - cnt, "heatmap function is %s\n",
+    if (mode) {
+        fts_read_reg(FTS_REG_HEATMAP_ED, &compressed);
+        if (ret < 0) {
+            FTS_ERROR("read reg_0x%X fails", FTS_REG_HEATMAP_ED);
+            return ret;
+        }
+        cnt += snprintf(tmpbuf + cnt,  PROC_BUF_SIZE - cnt, "%s ",
+            compressed ? "Compressed" : "Uncompressed");
+    }
+
+    cnt += snprintf(tmpbuf + cnt,  PROC_BUF_SIZE - cnt, "heatmap is %s\n",
                     mode ? "Enable" : "Disable");
 
     if (copy_to_user(buff, tmpbuf, cnt)) {
@@ -2138,9 +2149,15 @@ static ssize_t proc_heatmap_onoff_write(struct file *filp,
         return -EINVAL;
     }
 
+    if (mode < FW_HEATMAP_MODE_DISABLE || mode > FW_HEATMAP_MODE_UNCOMPRESSED) {
+        FTS_ERROR("Please input the parameters in \n \
+             0: Disable firmware heatmap. \n \
+             1: Enable firmware compressed heatmap. \n \
+             2: Enable firmware uncompressed heatmap.");
+        return -EINVAL;
+    }
     FTS_INFO("switch heatmap on/off to %d", mode);
-    fts_set_heatmap_mode(ts_data, !!mode);
-
+    fts_set_heatmap_mode(ts_data, mode);
     return count;
 }
 
diff --git a/ft3658/focaltech_test/focaltech_test.c b/ft3658/focaltech_test/focaltech_test.c
index 5e64919..9fc6d79 100644
--- a/ft3658/focaltech_test/focaltech_test.c
+++ b/ft3658/focaltech_test/focaltech_test.c
@@ -2812,15 +2812,22 @@ static const struct file_operations proc_test_baseline_fops = {
 #endif
 
 /* Strength test for full size */
-/* transpose raw */
-void transpose_raw(u8 *src, u8 *dist, int tx, int rx) {
+/* Transpose raw */
+void transpose_raw(u8 *src, u8 *dist, int tx, int rx, bool big_endian) {
     int i = 0;
     int j = 0;
 
     for (i = 0; i < tx; i++) {
         for (j = 0; j < rx; j++) {
-            dist[(j * tx + i) * 2] = src[(i * rx + j) * 2];
-            dist[(j * tx + i) * 2 + 1] = src[(i * rx + j) * 2 + 1];
+            if (big_endian) {
+                /* keep big_endian. */
+                dist[(j * tx + i) * 2] = src[(i * rx + j) * 2];
+                dist[(j * tx + i) * 2 + 1] = src[(i * rx + j) * 2 + 1];
+           } else {
+                /* transfer to big_endian. */
+                dist[(j * tx + i) * 2] = src[(i * rx + j) * 2 + 1];
+                dist[(j * tx + i) * 2 + 1] = src[(i * rx + j) * 2];
+           }
         }
     }
 }
@@ -2832,21 +2839,25 @@ static int proc_test_strength_show(struct seq_file *s, void *v)
     int i = 0;
     int node_num = 0;
     int self_node = 0;
-    int self_cap_num = 0;
-    int self_cap_num_off = 0;
-    int self_cap_offset = 91;
-    int self_cap_len = 68;
-    u8 tx = 16;
-    u8 rx = 34;
+    u8 tx = ts_data->pdata->tx_ch_num;
+    u8 rx = ts_data->pdata->rx_ch_num;
+    /* The format of uncompressed heatmap from touch chip.
+     *
+     * |- cap header (91) -|- Water-SS -|- Normal-SS -|- Normal-MS -|
+     * |-        91       -|-   68*2   -|-   68*2    -|-  16*34*2  -|
+     */
+    int ss_cap_on_idx = FTS_CAP_DATA_LEN;
+    int ss_cap_off_idx = ss_cap_on_idx + FTS_SELF_DATA_LEN * sizeof(u16);
+    int ms_cap_idx = ss_cap_off_idx + FTS_SELF_DATA_LEN * sizeof(u16);
     short base_result = 0;
 
     u8 *base_raw = NULL;
     u8 *trans_raw = NULL;
     int base_raw_len = 0;
     int base = 0;
-    int Fast_events_x = 0;
-    int Fast_events_y = 0;
-    u8 Fast_events_id = 0;
+    int fast_events_x = 0;
+    int fast_events_y = 0;
+    u8 fast_events_id = 0;
 
     fts_ts_set_bus_ref(ts_data, FTS_TS_BUS_REF_SYSFS, true);
     ret = enter_work_mode();
@@ -2856,9 +2867,9 @@ static int proc_test_strength_show(struct seq_file *s, void *v)
 
     node_num = tx * rx;
     self_node = tx + rx;
-    self_cap_num = self_cap_offset + node_num * sizeof(u16);
-    self_cap_num_off = self_cap_num + self_cap_len * sizeof(u16);
-    base_raw_len = self_cap_num + self_cap_len * 2 * sizeof(u16);
+
+    base_raw_len =
+        FTS_CAP_DATA_LEN + (FTS_SELF_DATA_LEN * 2 + node_num) * sizeof(u16);
     FTS_DEBUG("heapmap base_raw length = %d", base_raw_len);
     base_raw = fts_malloc(base_raw_len);
     if (!base_raw) {
@@ -2885,19 +2896,19 @@ static int proc_test_strength_show(struct seq_file *s, void *v)
     for (i = 0; i < base_raw[1]; i++) {
          base = FTS_ONE_TCH_LEN * i;
 
-         Fast_events_x = ((base_raw[2 + base] & 0x0F) << 8) +
+         fast_events_x = ((base_raw[2 + base] & 0x0F) << 8) +
                           (base_raw[3 + base] & 0xFF);
-         Fast_events_y = ((base_raw[4 + base] & 0x0F) << 8) +
+         fast_events_y = ((base_raw[4 + base] & 0x0F) << 8) +
                           (base_raw[5 + base] & 0xFF);
-         Fast_events_id = (base_raw[4 + base] & 0xF0) >> 4;
-         seq_printf(s, "Finger ID= %d , X= %d, y=%d\n", Fast_events_id,
-                    Fast_events_x, Fast_events_y);
+         fast_events_id = (base_raw[4 + base] & 0xF0) >> 4;
+         seq_printf(s, "Finger ID = %d , x = %d, y = %d\n", fast_events_id,
+                    fast_events_x, fast_events_y);
     }
 
     seq_printf(s, "     ");
     /* transpose data buffer. */
-    FTS_DEBUG("index(mutual) = %d", self_cap_offset);
-    transpose_raw(base_raw + self_cap_offset, trans_raw, tx, rx);
+    FTS_DEBUG("index of MS = %d", ms_cap_idx);
+    transpose_raw(base_raw + ms_cap_idx, trans_raw, tx, rx, true);
     for (i = 0; i < tx; i++)
         seq_printf(s, " TX%02d ", (i + 1));
 
@@ -2914,16 +2925,16 @@ static int proc_test_strength_show(struct seq_file *s, void *v)
     /*---------output self of strength data-----------*/
     seq_printf(s, "\n");
     seq_printf(s, "Scap raw(proof on):\n");
-    FTS_DEBUG("index(rx) = %d", self_cap_num);
+    FTS_DEBUG("index of SS_ON = %d", ss_cap_on_idx);
     for (i = 0; i < self_node; i++) {
-        base_result = (int)(base_raw[(i * 2) + self_cap_num] << 8) +
-                      (int)base_raw[(i * 2) + self_cap_num + 1];
+        base_result = (int)(base_raw[(i * 2) + ss_cap_on_idx] << 8) +
+                      (int)base_raw[(i * 2) + ss_cap_on_idx + 1];
 
         if (i == 0)
             seq_printf(s, "RX:");
 
         if (i == rx) {
-            FTS_DEBUG("index(tx) = %d", (self_cap_num + (i * 2)));
+            FTS_DEBUG("index(tx) = %d", (ss_cap_on_idx + (i * 2)));
             seq_printf(s, "\n");
             seq_printf(s, "TX:");
         }
@@ -2931,10 +2942,10 @@ static int proc_test_strength_show(struct seq_file *s, void *v)
     }
     seq_printf(s, "\n\n");
     seq_printf(s, "Scap raw(proof off):\n");
-
+    FTS_DEBUG("index of SS_OFF = %d", ss_cap_off_idx);
     for (i = 0; i < self_node; i++) {
-        base_result = (int)(base_raw[(i * 2) + self_cap_num_off] << 8) +
-                      (int)base_raw[(i * 2) + self_cap_num_off + 1];
+        base_result = (int)(base_raw[(i * 2) + ss_cap_off_idx] << 8) +
+                      (int)base_raw[(i * 2) + ss_cap_off_idx + 1];
 
         if (i == 0)
             seq_printf(s, "RX:");
diff --git a/ft3658/focaltech_test/supported_ic/focaltech_test_ft3658u.c b/ft3658/focaltech_test/supported_ic/focaltech_test_ft3658u.c
index 98931ca..0d9617a 100644
--- a/ft3658/focaltech_test/supported_ic/focaltech_test_ft3658u.c
+++ b/ft3658/focaltech_test/supported_ic/focaltech_test_ft3658u.c
@@ -2050,18 +2050,18 @@ int fts_test_get_strength(u8 *base_raw, u8 tx, u8 rx)
 
     FTS_TEST_INFO("====== Test Item: strength test start\n");
     id_cmd[0] = 0x01;
-
-    fts_set_heatmap_mode(ts_data, true);
+    /* Enable uncompressed heatmap. */
+    fts_set_heatmap_mode(ts_data, FW_HEATMAP_MODE_UNCOMPRESSED);
     sys_delay(500);
     FTS_TEST_DBG("Allocate heatmap length = %d.\n", fast_num_len);
     ret = fts_read(id_cmd, 1, base_raw, fast_num_len);
 
     if (ret < 0) {
         FTS_TEST_ERROR("get strength fail,ret=%d\n", ret);
-        goto test_err;
     }
 
-test_err:
+    /* Enable compressed heatmap. */
+    fts_set_heatmap_mode(ts_data, FW_HEATMAP_MODE_COMPRESSED);
     FTS_TEST_INFO("====== Test Item: strength test end\n");
     return ret;
 }