touch: add drivers

Add all drivers for new platforms.

Change-Id: Ie9947b0c6f8ddfee7dab6dfa80d6aca62323f4da
Signed-off-by: Fei Mao <[email protected]>

focaltech_touch/focaltech_common.h

@@ -0,0 +1,167 @@
+/*
+ *
+ * FocalTech fts TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, Focaltech Ltd. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+/*****************************************************************************
+*
+* File Name: focaltech_common.h
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-16
+*
+* Abstract:
+*
+* Reference:
+*
+*****************************************************************************/
+
+#ifndef __LINUX_FOCALTECH_COMMON_H__
+#define __LINUX_FOCALTECH_COMMON_H__
+
+#include "focaltech_config.h"
+
+/*****************************************************************************
+* Macro definitions using #define
+*****************************************************************************/
+#define FTS_DRIVER_VERSION      "Focaltech V3.2 20200422"
+
+#define BYTE_OFF_0(x)           (u8)((x) & 0xFF)
+#define BYTE_OFF_8(x)           (u8)(((x) >> 8) & 0xFF)
+#define BYTE_OFF_16(x)          (u8)(((x) >> 16) & 0xFF)
+#define BYTE_OFF_24(x)          (u8)(((x) >> 24) & 0xFF)
+#define FLAGBIT(x)              (0x00000001 << (x))
+#define FLAGBITS(x, y)          ((0xFFFFFFFF >> (32 - (y) - 1)) & (0xFFFFFFFF << (x)))
+
+#define FLAG_ICSERIALS_LEN      8
+#define FLAG_HID_BIT            10
+#define FLAG_IDC_BIT            11
+
+#define IC_SERIALS(type)        ((type) & FLAGBITS(0, FLAG_ICSERIALS_LEN-1))
+#define IC_TO_SERIALS(x)        ((x) & FLAGBITS(0, FLAG_ICSERIALS_LEN-1))
+#define FTS_CHIP_IDC(type)            (((type) & FLAGBIT(FLAG_IDC_BIT)) == FLAGBIT(FLAG_IDC_BIT))
+#define FTS_HID_SUPPORTTED(type)      (((type) & FLAGBIT(FLAG_HID_BIT)) == FLAGBIT(FLAG_HID_BIT))
+
+#define FILE_NAME_LENGTH                    128
+#define ENABLE                              1
+#define DISABLE                             0
+#define VALID                               1
+#define INVALID                             0
+#define FTS_CMD_START1                      0x55
+#define FTS_CMD_START2                      0xAA
+#define FTS_CMD_START_DELAY                 12
+#define FTS_CMD_READ_ID                     0x90
+#define FTS_CMD_READ_ID_LEN                 4
+#define FTS_CMD_READ_ID_LEN_INCELL          1
+#define FTS_CMD_READ_FW_CONF                0xA8
+/*register address*/
+#define FTS_REG_INT_CNT                     0x8F
+#define FTS_REG_FLOW_WORK_CNT               0x91
+#define FTS_REG_WORKMODE                    0x00
+#define FTS_REG_WORKMODE_FACTORY_VALUE      0x40
+#define FTS_REG_WORKMODE_WORK_VALUE         0x00
+#define FTS_REG_ESDCHECK_DISABLE            0x8D
+#define FTS_REG_CHIP_ID                     0xA3
+#define FTS_REG_CHIP_ID2                    0x9F
+#define FTS_REG_POWER_MODE                  0xA5
+#define FTS_REG_POWER_MODE_SLEEP            0x03
+#define FTS_REG_FW_VER                      0xA6
+#define FTS_REG_VENDOR_ID                   0xA8
+#define FTS_REG_LCD_BUSY_NUM                0xAB
+#define FTS_REG_FACE_DEC_MODE_EN            0xB0
+#define FTS_REG_FACTORY_MODE_DETACH_FLAG    0xB4
+#define FTS_REG_FACE_DEC_MODE_STATUS        0x01
+#define FTS_REG_IDE_PARA_VER_ID             0xB5
+#define FTS_REG_IDE_PARA_STATUS             0xB6
+#define FTS_REG_GLOVE_MODE_EN               0xC0
+#define FTS_REG_COVER_MODE_EN               0xC1
+#define FTS_REG_REPORT_RATE                 0x88
+#define FTS_REG_CHARGER_MODE_EN             0x8B
+#define FTS_REG_GESTURE_EN                  0xD0
+#define FTS_REG_GESTURE_OUTPUT_ADDRESS      0xD3
+#define FTS_REG_MODULE_ID                   0xE3
+#define FTS_REG_LIC_VER                     0xE4
+#define FTS_REG_ESD_SATURATE                0xED
+
+#define FTS_SYSFS_ECHO_ON(buf)      (buf[0] == '1')
+#define FTS_SYSFS_ECHO_OFF(buf)     (buf[0] == '0')
+
+#define kfree_safe(pbuf) do {\
+	if (pbuf) {\
+		kfree(pbuf);\
+		pbuf = NULL;\
+	}\
+} while(0)
+
+/*****************************************************************************
+*  Alternative mode (When something goes wrong,
+* the modules may be able to solve the problem.)
+*****************************************************************************/
+/*
+ * point report check
+ * default: disable
+ */
+#define FTS_POINT_REPORT_CHECK_EN               0
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+struct ft_chip_t {
+	u64 type;
+	u8 chip_idh;
+	u8 chip_idl;
+	u8 rom_idh;
+	u8 rom_idl;
+	u8 pb_idh;
+	u8 pb_idl;
+	u8 bl_idh;
+	u8 bl_idl;
+};
+
+struct ts_ic_info {
+	bool is_incell;
+	bool hid_supported;
+	struct ft_chip_t ids;
+};
+
+/*****************************************************************************
+* DEBUG function define here
+*****************************************************************************/
+#if FTS_DEBUG_EN
+#define FTS_DEBUG(fmt, args...) do { \
+	printk("[FTS_TS]%s:"fmt"\n", __func__, ##args); \
+} while (0)
+
+#define FTS_FUNC_ENTER() do { \
+	printk("[FTS_TS]%s: Enter\n", __func__); \
+} while (0)
+
+#define FTS_FUNC_EXIT() do { \
+	printk("[FTS_TS]%s: Exit(%d)\n", __func__, __LINE__); \
+} while (0)
+#else /* #if FTS_DEBUG_EN*/
+#define FTS_DEBUG(fmt, args...)
+#define FTS_FUNC_ENTER()
+#define FTS_FUNC_EXIT()
+#endif
+
+#define FTS_INFO(fmt, args...) do { \
+	printk(KERN_INFO "[FTS_TS/I]%s:"fmt"\n", __func__, ##args); \
+} while (0)
+
+#define FTS_ERROR(fmt, args...) do { \
+	printk(KERN_ERR "[FTS_TS/E]%s:"fmt"\n", __func__, ##args); \
+} while (0)
+#endif /* __LINUX_FOCALTECH_COMMON_H__ */

focaltech_touch/focaltech_config.h

@@ -0,0 +1,277 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, FocalTech Systems, Ltd., all rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/************************************************************************
+*
+* File Name: focaltech_config.h
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-08
+*
+* Abstract: global configurations
+*
+* Version: v1.0
+*
+************************************************************************/
+#ifndef _LINUX_FOCLATECH_CONFIG_H_
+#define _LINUX_FOCLATECH_CONFIG_H_
+
+/**************************************************/
+/****** G: A, I: B, S: C, U: D  ******************/
+/****** chip type defines, do not modify *********/
+#define _FT8716             0x87160805
+#define _FT8736             0x87360806
+#define _FT8006M            0x80060807
+#define _FT8607             0x86070809
+#define _FT8006U            0x8006D80B
+#define _FT8006S            0x8006A80B
+#define _FT8613             0x8613080C
+#define _FT8719             0x8719080D
+#define _FT8739             0x8739080E
+#define _FT8615             0x8615080F
+#define _FT8201             0x82010810
+#define _FT8006P            0x86220811
+#define _FT7251             0x72510812
+#define _FT7252             0x72520813
+#define _FT8613S            0x8613C814
+#define _FT8756             0x87560815
+#define _FT8302             0x83020816
+#define _FT8009             0x80090817
+#define _FT8656             0x86560818
+#define _FT8006S_AA         0x86320819
+#define _FT7250             0x7250081A
+#define _FT7120             0x7120081B
+#define _FT8720             0x8720081C
+#define _FT8016             0x8016081D
+
+
+#define _FT5416             0x54160402
+#define _FT5426             0x54260402
+#define _FT5435             0x54350402
+#define _FT5436             0x54360402
+#define _FT5526             0x55260402
+#define _FT5526I            0x5526B402
+#define _FT5446             0x54460402
+#define _FT5346             0x53460402
+#define _FT5446I            0x5446B402
+#define _FT5346I            0x5346B402
+#define _FT7661             0x76610402
+#define _FT7511             0x75110402
+#define _FT7421             0x74210402
+#define _FT7681             0x76810402
+#define _FT3C47U            0x3C47D402
+#define _FT3417             0x34170402
+#define _FT3517             0x35170402
+#define _FT3327             0x33270402
+#define _FT3427             0x34270402
+#define _FT7311             0x73110402
+#define _FT5526_V00         0x5526C402
+
+#define _FT5626             0x56260401
+#define _FT5726             0x57260401
+#define _FT5826B            0x5826B401
+#define _FT5826S            0x5826C401
+#define _FT7811             0x78110401
+#define _FT3D47             0x3D470401
+#define _FT3617             0x36170401
+#define _FT3717             0x37170401
+#define _FT3817B            0x3817B401
+#define _FT3517U            0x3517D401
+
+#define _FT6236U            0x6236D003
+#define _FT6336G            0x6336A003
+#define _FT6336U            0x6336D003
+#define _FT6436U            0x6436D003
+#define _FT6436T            0x6436E003
+
+#define _FT3267             0x32670004
+#define _FT3367             0x33670004
+
+#define _FT3327DQQ_XXX      0x3327D482
+#define _FT5446DQS_XXX      0x5446D482
+
+#define _FT3427_003         0x3427D482
+#define _FT3427G_003        0x3427A482
+#define _FT5446_003         0x5446D482
+#define _FT5446_Q03         0x5446C482
+#define _FT5446_P03         0x5446A481
+#define _FT5426_003         0x5426D482
+#define _FT5526_003         0x5526D482
+
+#define _FT3518             0x35180481
+#define _FT3518U            0x3518D481
+#define _FT3558             0x35580481
+#define _FT3528             0x35280481
+#define _FT5536             0x55360481
+#define _FT5536L            0x5536E481
+#define _FT3418             0x34180481
+
+#define _FT5446U            0x5446D083
+#define _FT5456U            0x5456D083
+#define _FT3417U            0x3417D083
+#define _FT5426U            0x5426D083
+#define _FT3428             0x34280083
+#define _FT3437U            0x3437D083
+
+#define _FT7302             0x73020084
+#define _FT7202             0x72020084
+#define _FT3308             0x33080084
+#define _FT6446             0x64460084
+
+#define _FT6346U            0x6346D085
+#define _FT6346G            0x6346A085
+#define _FT3067             0x30670085
+#define _FT3068             0x30680085
+#define _FT3168             0x31680085
+#define _FT3268             0x32680085
+#define _FT6146             0x61460085
+
+#define _FT5726_003         0x5726D486
+#define _FT5726_V03         0x5726C486
+
+#define _FT3618             0x36180487
+#define _FT5646             0x56460487
+#define _FT3A58             0x3A580487
+#define _FT3B58             0x3B580487
+#define _FT3D58             0x3D580487
+#define _FT5936             0x59360487
+#define _FT5A36             0x5A360487
+#define _FT5B36             0x5B360487
+#define _FT5D36             0x5D360487
+#define _FT5946             0x59460487
+#define _FT5A46             0x5A460487
+#define _FT5B46             0x5B460487
+#define _FT5D46             0x5D460487
+
+#define _FT3658U            0x3658D488
+
+/******************* Enables *********************/
+/*********** 1 to enable, 0 to disable ***********/
+
+/*
+ * show debug log info
+ * enable it for debug, disable it for release
+ */
+#define FTS_DEBUG_EN                            0
+
+/*
+ * Linux MultiTouch Protocol
+ * 1: Protocol B(default), 0: Protocol A
+ */
+#define FTS_MT_PROTOCOL_B_EN                    1
+
+/*
+ * Report Pressure in multitouch
+ * 1:enable(default),0:disable
+*/
+#define FTS_REPORT_PRESSURE_EN                  1
+
+/*
+ * Gesture function enable
+ * default: disable
+ */
+#define FTS_GESTURE_EN                          0
+
+/*
+ * ESD check & protection
+ * default: disable
+ */
+#define FTS_ESDCHECK_EN                         0
+
+/*
+ * Production test enable
+ * 1: enable, 0:disable(default)
+ */
+#define FTS_TEST_EN                             0
+
+/*
+ * Pinctrl enable
+ * default: disable
+ */
+#define FTS_PINCTRL_EN                          1
+
+/*
+ * Customer power enable
+ * enable it when customer need control TP power
+ * default: disable
+ */
+#define FTS_POWER_SOURCE_CUST_EN                1
+
+/****************************************************/
+
+/********************** Upgrade ****************************/
+/*
+ * auto upgrade
+ */
+#define FTS_AUTO_UPGRADE_EN                     1
+
+/*
+ * auto upgrade for lcd cfg
+ */
+#define FTS_AUTO_LIC_UPGRADE_EN                 0
+
+/*
+ * Numbers of modules support
+ */
+#define FTS_GET_MODULE_NUM                      0
+
+/*
+ * module_id: mean vendor_id generally, also maybe gpio or lcm_id...
+ * If means vendor_id, the FTS_MODULE_ID = PANEL_ID << 8 + VENDOR_ID
+ * FTS_GET_MODULE_NUM == 0/1, no check module id, you may ignore them
+ * FTS_GET_MODULE_NUM >= 2, compatible with FTS_MODULE2_ID
+ * FTS_GET_MODULE_NUM >= 3, compatible with FTS_MODULE3_ID
+ */
+#define FTS_MODULE_ID                          0x0000
+#define FTS_MODULE2_ID                         0x0000
+#define FTS_MODULE3_ID                         0x0000
+
+/*
+ * Need set the following when get firmware via firmware_request()
+ * For example: if module'vendor is tianma,
+ * #define FTS_MODULE_NAME                        "tianma"
+ * then file_name will be "focaltech_ts_fw_tianma"
+ * You should rename fw to "focaltech_ts_fw_tianma", and push it into
+ * etc/firmware or by customers
+ */
+#define FTS_MODULE_NAME                        "gvo"
+#define FTS_MODULE2_NAME                       ""
+#define FTS_MODULE3_NAME                       ""
+
+/*
+ * FW.i file for auto upgrade, you must replace it with your own
+ * define your own fw_file, the sample one to be replaced is invalid
+ * NOTE: if FTS_GET_MODULE_NUM > 1, it's the fw corresponding with FTS_VENDOR_ID
+ */
+#define FTS_UPGRADE_FW_FILE                    "include/firmware/fw_sample.i"
+
+/*
+ * if FTS_GET_MODULE_NUM >= 2, fw corrsponding with FTS_VENDOR_ID2
+ * define your own fw_file, the sample one is invalid
+ */
+#define FTS_UPGRADE_FW2_FILE                   "include/firmware/fw_sample.i"
+
+/*
+ * if FTS_GET_MODULE_NUM >= 3, fw corrsponding with FTS_VENDOR_ID3
+ * define your own fw_file, the sample one is invalid
+ */
+#define FTS_UPGRADE_FW3_FILE                   "include/firmware/fw_sample.i"
+
+/*********************************************************/
+
+#endif /* _LINUX_FOCLATECH_CONFIG_H_ */

focaltech_touch/focaltech_core.c

@@ -0,0 +1,3364 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, FocalTech Systems, Ltd., all rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+/*****************************************************************************
+*
+* File Name: focaltech_core.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-08
+*
+* Abstract: entrance for focaltech ts driver
+*
+* Version: V1.0
+*
+*****************************************************************************/
+
+/*****************************************************************************
+* Included header files
+*****************************************************************************/
+#include <linux/module.h>
+#include <linux/irq.h>
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+#include <linux/of_irq.h>
+#include <linux/soc/qcom/panel_event_notifier.h>
+#if defined(CONFIG_DRM)
+#include <drm/drm_panel.h>
+#elif defined(CONFIG_FB)
+#include <linux/notifier.h>
+#include <linux/fb.h>
+#elif defined(CONFIG_HAS_EARLYSUSPEND)
+#include <linux/earlysuspend.h>
+#define FTS_SUSPEND_LEVEL 1     /* Early-suspend level */
+#endif
+#include "focaltech_core.h"
+
+#if defined(CONFIG_FTS_TRUSTED_TOUCH)
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+#include <linux/debugfs.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/kobject.h>
+#include <linux/sysfs.h>
+#include "linux/gunyah/gh_msgq.h"
+#include "linux/gunyah/gh_rm_drv.h"
+#include <linux/sort.h>
+#include <linux/pinctrl/qcom-pinctrl.h>
+#endif
+
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define FTS_DRIVER_NAME                     "fts_ts"
+#define INTERVAL_READ_REG                   200  /* unit:ms */
+#define TIMEOUT_READ_REG                    1000 /* unit:ms */
+#if FTS_POWER_SOURCE_CUST_EN
+#define FTS_VTG_MIN_UV                      3000000
+#define FTS_VTG_MAX_UV                      3300000
+#define FTS_LOAD_MAX_UA                     30000
+#define FTS_LOAD_AVDD_UA                    10000
+#define FTS_LOAD_DISABLE_UA                 0
+#define FTS_I2C_VTG_MIN_UV                  1800000
+#define FTS_I2C_VTG_MAX_UV                  1800000
+#endif
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+struct fts_ts_data *fts_data;
+
+#if defined(CONFIG_DRM)
+static struct drm_panel *active_panel;
+static void fts_ts_panel_notifier_callback(enum panel_event_notifier_tag tag,
+		 struct panel_event_notification *event, void *client_data);
+#endif
+
+static struct ft_chip_t ctype[] = {
+	{0x88, 0x56, 0x52, 0x00, 0x00, 0x00, 0x00, 0x56, 0xB2},
+	{0x81, 0x54, 0x52, 0x54, 0x52, 0x00, 0x00, 0x54, 0x5C},
+};
+
+/*****************************************************************************
+* Static function prototypes
+*****************************************************************************/
+static int fts_ts_suspend(struct device *dev);
+static int fts_ts_resume(struct device *dev);
+static irqreturn_t fts_irq_handler(int irq, void *data);
+static int fts_ts_probe_delayed(struct fts_ts_data *fts_data);
+static int fts_ts_enable_reg(struct fts_ts_data *ts_data, bool enable);
+
+static void fts_ts_register_for_panel_events(struct device_node *dp,
+					struct fts_ts_data *ts_data)
+{
+	const char *touch_type;
+	int rc = 0;
+	void *cookie = NULL;
+
+	rc = of_property_read_string(dp, "focaltech,touch-type",
+						&touch_type);
+	if (rc) {
+		dev_warn(&fts_data->client->dev,
+			"%s: No touch type\n", __func__);
+		return;
+	}
+	if (strcmp(touch_type, "primary")) {
+		pr_err("Invalid touch type\n");
+		return;
+	}
+
+	cookie = panel_event_notifier_register(PANEL_EVENT_NOTIFICATION_PRIMARY,
+			PANEL_EVENT_NOTIFIER_CLIENT_PRIMARY_TOUCH, active_panel,
+			&fts_ts_panel_notifier_callback, ts_data);
+	if (!cookie) {
+		pr_err("Failed to register for panel events\n");
+		return;
+	}
+
+	FTS_DEBUG("registered for panel notifications panel: 0x%x\n",
+			active_panel);
+
+	ts_data->notifier_cookie = cookie;
+}
+
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+
+static void fts_ts_trusted_touch_abort_handler(struct fts_ts_data *fts_data,
+						int error);
+static struct gh_acl_desc *fts_ts_vm_get_acl(enum gh_vm_names vm_name)
+{
+	struct gh_acl_desc *acl_desc;
+	gh_vmid_t vmid;
+
+	gh_rm_get_vmid(vm_name, &vmid);
+
+	acl_desc = kzalloc(offsetof(struct gh_acl_desc, acl_entries[1]),
+			GFP_KERNEL);
+	if (!acl_desc)
+		return ERR_PTR(ENOMEM);
+
+	acl_desc->n_acl_entries = 1;
+	acl_desc->acl_entries[0].vmid = vmid;
+	acl_desc->acl_entries[0].perms = GH_RM_ACL_R | GH_RM_ACL_W;
+
+	return acl_desc;
+}
+
+static struct gh_sgl_desc *fts_ts_vm_get_sgl(
+				struct trusted_touch_vm_info *vm_info)
+{
+	struct gh_sgl_desc *sgl_desc;
+	int i;
+
+	sgl_desc = kzalloc(offsetof(struct gh_sgl_desc,
+			sgl_entries[vm_info->iomem_list_size]), GFP_KERNEL);
+	if (!sgl_desc)
+		return ERR_PTR(ENOMEM);
+
+	sgl_desc->n_sgl_entries = vm_info->iomem_list_size;
+
+	for (i = 0; i < vm_info->iomem_list_size; i++) {
+		sgl_desc->sgl_entries[i].ipa_base = vm_info->iomem_bases[i];
+		sgl_desc->sgl_entries[i].size = vm_info->iomem_sizes[i];
+	}
+
+	return sgl_desc;
+}
+
+static int fts_ts_populate_vm_info_iomem(struct fts_ts_data *fts_data)
+{
+	int i, gpio, rc = 0;
+	int num_regs, num_sizes, num_gpios, list_size;
+	struct resource res;
+	struct device_node *np = fts_data->dev->of_node;
+	struct trusted_touch_vm_info *vm_info = fts_data->vm_info;
+
+	num_regs = of_property_count_u32_elems(np, "focaltech,trusted-touch-io-bases");
+	if (num_regs < 0) {
+		FTS_ERROR("Invalid number of IO regions specified\n");
+		return -EINVAL;
+	}
+
+	num_sizes = of_property_count_u32_elems(np, "focaltech,trusted-touch-io-sizes");
+	if (num_sizes < 0) {
+		FTS_ERROR("Invalid number of IO regions specified\n");
+		return -EINVAL;
+	}
+
+	if (num_regs != num_sizes) {
+		FTS_ERROR("IO bases and sizes array lengths mismatch\n");
+		return -EINVAL;
+	}
+
+	num_gpios = of_gpio_named_count(np, "focaltech,trusted-touch-vm-gpio-list");
+	if (num_gpios < 0) {
+		dev_warn(fts_data->dev, "Ignoring invalid trusted gpio list: %d\n", num_gpios);
+		num_gpios = 0;
+	}
+
+	list_size = num_regs + num_gpios;
+	vm_info->iomem_list_size = list_size;
+	vm_info->iomem_bases = devm_kcalloc(fts_data->dev, list_size, sizeof(*vm_info->iomem_bases),
+			GFP_KERNEL);
+	if (!vm_info->iomem_bases)
+		return -ENOMEM;
+
+	vm_info->iomem_sizes = devm_kcalloc(fts_data->dev, list_size, sizeof(*vm_info->iomem_sizes),
+			GFP_KERNEL);
+	if (!vm_info->iomem_sizes)
+		return -ENOMEM;
+
+	for (i = 0; i < num_gpios; ++i) {
+		gpio = of_get_named_gpio(np, "focaltech,trusted-touch-vm-gpio-list", i);
+		if (gpio < 0 || !gpio_is_valid(gpio)) {
+			FTS_ERROR("Invalid gpio %d at position %d\n", gpio, i);
+			return gpio;
+		}
+
+		if (!msm_gpio_get_pin_address(gpio, &res)) {
+			FTS_ERROR("Failed to retrieve gpio-%d resource\n", gpio);
+			return -ENODATA;
+		}
+
+		vm_info->iomem_bases[i] = res.start;
+		vm_info->iomem_sizes[i] = resource_size(&res);
+	}
+
+	rc = of_property_read_u32_array(np, "focaltech,trusted-touch-io-bases",
+			&vm_info->iomem_bases[i], list_size - i);
+	if (rc) {
+		FTS_ERROR("Failed to read trusted touch io bases:%d\n", rc);
+		return rc;
+	}
+
+	rc = of_property_read_u32_array(np, "focaltech,trusted-touch-io-sizes",
+			&vm_info->iomem_sizes[i], list_size - i);
+	if (rc) {
+		FTS_ERROR("Failed to read trusted touch io sizes:%d\n", rc);
+		return rc;
+	}
+
+	return 0;
+}
+
+static int fts_ts_populate_vm_info(struct fts_ts_data *fts_data)
+{
+	int rc;
+	struct trusted_touch_vm_info *vm_info;
+	struct device_node *np = fts_data->dev->of_node;
+
+	vm_info = devm_kzalloc(fts_data->dev, sizeof(struct trusted_touch_vm_info), GFP_KERNEL);
+	if (!vm_info)
+		return -ENOMEM;
+
+	fts_data->vm_info = vm_info;
+	vm_info->vm_name = GH_TRUSTED_VM;
+	rc = of_property_read_u32(np, "focaltech,trusted-touch-spi-irq", &vm_info->hw_irq);
+	if (rc) {
+		pr_err("Failed to read trusted touch SPI irq:%d\n", rc);
+		return rc;
+	}
+
+	rc = fts_ts_populate_vm_info_iomem(fts_data);
+	if (rc) {
+		pr_err("Failed to read trusted touch mmio ranges:%d\n", rc);
+		return rc;
+	}
+
+	rc = of_property_read_string(np, "focaltech,trusted-touch-type",
+						&vm_info->trusted_touch_type);
+	if (rc) {
+		pr_warn("%s: No trusted touch type selection made\n", __func__);
+		vm_info->mem_tag = GH_MEM_NOTIFIER_TAG_TOUCH_PRIMARY;
+		vm_info->irq_label = GH_IRQ_LABEL_TRUSTED_TOUCH_PRIMARY;
+		rc = 0;
+	} else if (!strcmp(vm_info->trusted_touch_type, "primary")) {
+		vm_info->mem_tag = GH_MEM_NOTIFIER_TAG_TOUCH_PRIMARY;
+		vm_info->irq_label = GH_IRQ_LABEL_TRUSTED_TOUCH_PRIMARY;
+	} else if (!strcmp(vm_info->trusted_touch_type, "secondary")) {
+		vm_info->mem_tag = GH_MEM_NOTIFIER_TAG_TOUCH_SECONDARY;
+		vm_info->irq_label = GH_IRQ_LABEL_TRUSTED_TOUCH_SECONDARY;
+	}
+
+	return 0;
+}
+
+static void fts_ts_destroy_vm_info(struct fts_ts_data *fts_data)
+{
+	kfree(fts_data->vm_info->iomem_sizes);
+	kfree(fts_data->vm_info->iomem_bases);
+	kfree(fts_data->vm_info);
+}
+
+static void fts_ts_vm_deinit(struct fts_ts_data *fts_data)
+{
+	if (fts_data->vm_info->mem_cookie)
+		gh_mem_notifier_unregister(fts_data->vm_info->mem_cookie);
+	fts_ts_destroy_vm_info(fts_data);
+}
+
+static int fts_ts_trusted_touch_get_vm_state(struct fts_ts_data *fts_data)
+{
+	return atomic_read(&fts_data->vm_info->vm_state);
+}
+
+static void fts_ts_trusted_touch_set_vm_state(struct fts_ts_data *fts_data,
+							int state)
+{
+	atomic_set(&fts_data->vm_info->vm_state, state);
+}
+
+#ifdef CONFIG_ARCH_QTI_VM
+static int fts_ts_vm_mem_release(struct fts_ts_data *fts_data);
+static void fts_ts_trusted_touch_tvm_vm_mode_disable(struct fts_ts_data *fts_data);
+static void fts_ts_trusted_touch_abort_tvm(struct fts_ts_data *fts_data);
+static void fts_ts_trusted_touch_event_notify(struct fts_ts_data *fts_data, int event);
+
+void fts_ts_trusted_touch_tvm_i2c_failure_report(struct fts_ts_data *fts_data)
+{
+	pr_err("initiating trusted touch abort due to i2c failure\n");
+	fts_ts_trusted_touch_abort_handler(fts_data,
+			TRUSTED_TOUCH_EVENT_I2C_FAILURE);
+}
+
+static void fts_ts_trusted_touch_reset_gpio_toggle(struct fts_ts_data *fts_data)
+{
+	void __iomem *base;
+
+	if (fts_data->bus_type != BUS_TYPE_I2C)
+		return;
+
+	base = ioremap(TOUCH_RESET_GPIO_BASE, TOUCH_RESET_GPIO_SIZE);
+	writel_relaxed(0x1, base + TOUCH_RESET_GPIO_OFFSET);
+	/* wait until toggle to finish*/
+	wmb();
+	writel_relaxed(0x0, base + TOUCH_RESET_GPIO_OFFSET);
+	/* wait until toggle to finish*/
+	wmb();
+	iounmap(base);
+}
+
+static void fts_trusted_touch_intr_gpio_toggle(struct fts_ts_data *fts_data,
+		bool enable)
+{
+	void __iomem *base;
+	u32 val;
+
+	if (fts_data->bus_type != BUS_TYPE_I2C)
+		return;
+
+	base = ioremap(TOUCH_INTR_GPIO_BASE, TOUCH_INTR_GPIO_SIZE);
+	val = readl_relaxed(base + TOUCH_RESET_GPIO_OFFSET);
+	if (enable) {
+		val |= BIT(0);
+		writel_relaxed(val, base + TOUCH_INTR_GPIO_OFFSET);
+		/* wait until toggle to finish*/
+		wmb();
+	} else {
+		val &= ~BIT(0);
+		writel_relaxed(val, base + TOUCH_INTR_GPIO_OFFSET);
+		/* wait until toggle to finish*/
+		wmb();
+	}
+	iounmap(base);
+}
+
+static int fts_ts_sgl_cmp(const void *a, const void *b)
+{
+	struct gh_sgl_entry *left = (struct gh_sgl_entry *)a;
+	struct gh_sgl_entry *right = (struct gh_sgl_entry *)b;
+
+	return (left->ipa_base - right->ipa_base);
+}
+
+static int fts_ts_vm_compare_sgl_desc(struct gh_sgl_desc *expected,
+		struct gh_sgl_desc *received)
+{
+	int idx;
+
+	if (expected->n_sgl_entries != received->n_sgl_entries)
+		return -E2BIG;
+	sort(received->sgl_entries, received->n_sgl_entries,
+			sizeof(received->sgl_entries[0]), fts_ts_sgl_cmp, NULL);
+	sort(expected->sgl_entries, expected->n_sgl_entries,
+			sizeof(expected->sgl_entries[0]), fts_ts_sgl_cmp, NULL);
+
+	for (idx = 0; idx < expected->n_sgl_entries; idx++) {
+		struct gh_sgl_entry *left = &expected->sgl_entries[idx];
+		struct gh_sgl_entry *right = &received->sgl_entries[idx];
+
+		if ((left->ipa_base != right->ipa_base) ||
+				(left->size != right->size)) {
+			pr_err("sgl mismatch: left_base:%d right base:%d left size:%d right size:%d\n",
+					left->ipa_base, right->ipa_base,
+					left->size, right->size);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int fts_ts_vm_handle_vm_hardware(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+
+	if (atomic_read(&fts_data->delayed_vm_probe_pending)) {
+		rc = fts_ts_probe_delayed(fts_data);
+		if (rc) {
+			pr_err(" Delayed probe failure on VM!\n");
+			return rc;
+		}
+		atomic_set(&fts_data->delayed_vm_probe_pending, 0);
+		return rc;
+	}
+
+	fts_irq_enable();
+	fts_ts_trusted_touch_set_vm_state(fts_data, TVM_INTERRUPT_ENABLED);
+	return rc;
+}
+
+static void fts_ts_trusted_touch_tvm_vm_mode_enable(struct fts_ts_data *fts_data)
+{
+
+	struct gh_sgl_desc *sgl_desc, *expected_sgl_desc;
+	struct gh_acl_desc *acl_desc;
+	struct irq_data *irq_data;
+	int rc = 0;
+	int irq = 0;
+
+	if (fts_ts_trusted_touch_get_vm_state(fts_data) != TVM_ALL_RESOURCES_LENT_NOTIFIED) {
+		pr_err("All lend notifications not received\n");
+		fts_ts_trusted_touch_event_notify(fts_data,
+				TRUSTED_TOUCH_EVENT_NOTIFICATIONS_PENDING);
+		return;
+	}
+
+	acl_desc = fts_ts_vm_get_acl(GH_TRUSTED_VM);
+	if (IS_ERR(acl_desc)) {
+		pr_err("failed to populated acl data:rc=%d\n",
+				PTR_ERR(acl_desc));
+		goto accept_fail;
+	}
+
+	sgl_desc = gh_rm_mem_accept(fts_data->vm_info->vm_mem_handle,
+			GH_RM_MEM_TYPE_IO,
+			GH_RM_TRANS_TYPE_LEND,
+			GH_RM_MEM_ACCEPT_VALIDATE_ACL_ATTRS |
+			GH_RM_MEM_ACCEPT_VALIDATE_LABEL |
+			GH_RM_MEM_ACCEPT_DONE,  TRUSTED_TOUCH_MEM_LABEL,
+			acl_desc, NULL, NULL, 0);
+	if (IS_ERR_OR_NULL(sgl_desc)) {
+		pr_err("failed to do mem accept :rc=%d\n",
+				PTR_ERR(sgl_desc));
+		goto acl_fail;
+	}
+	fts_ts_trusted_touch_set_vm_state(fts_data, TVM_IOMEM_ACCEPTED);
+
+	/* Initiate session on tvm */
+	if (fts_data->bus_type == BUS_TYPE_I2C)
+		rc = pm_runtime_get_sync(fts_data->client->adapter->dev.parent);
+	else
+		rc = pm_runtime_get_sync(fts_data->spi->master->dev.parent);
+
+	if (rc < 0) {
+		pr_err("failed to get sync rc:%d\n", rc);
+		goto acl_fail;
+	}
+	fts_ts_trusted_touch_set_vm_state(fts_data, TVM_I2C_SESSION_ACQUIRED);
+
+	expected_sgl_desc = fts_ts_vm_get_sgl(fts_data->vm_info);
+	if (fts_ts_vm_compare_sgl_desc(expected_sgl_desc, sgl_desc)) {
+		pr_err("IO sg list does not match\n");
+		goto sgl_cmp_fail;
+	}
+
+	kfree(expected_sgl_desc);
+	kfree(acl_desc);
+
+	irq = gh_irq_accept(fts_data->vm_info->irq_label, -1, IRQ_TYPE_EDGE_RISING);
+	fts_trusted_touch_intr_gpio_toggle(fts_data, false);
+	if (irq < 0) {
+		pr_err("failed to accept irq\n");
+		goto accept_fail;
+	}
+	fts_ts_trusted_touch_set_vm_state(fts_data, TVM_IRQ_ACCEPTED);
+
+
+	irq_data = irq_get_irq_data(irq);
+	if (!irq_data) {
+		pr_err("Invalid irq data for trusted touch\n");
+		goto accept_fail;
+	}
+	if (!irq_data->hwirq) {
+		pr_err("Invalid irq in irq data\n");
+		goto accept_fail;
+	}
+	if (irq_data->hwirq != fts_data->vm_info->hw_irq) {
+		pr_err("Invalid irq lent\n");
+		goto accept_fail;
+	}
+
+	pr_debug("irq:returned from accept:%d\n", irq);
+	fts_data->irq = irq;
+
+	rc = fts_ts_vm_handle_vm_hardware(fts_data);
+	if (rc) {
+		pr_err(" Delayed probe failure on VM!\n");
+		goto accept_fail;
+	}
+	atomic_set(&fts_data->trusted_touch_enabled, 1);
+	pr_info("trusted touch enabled\n");
+
+	return;
+sgl_cmp_fail:
+	kfree(expected_sgl_desc);
+acl_fail:
+	kfree(acl_desc);
+accept_fail:
+	fts_ts_trusted_touch_abort_handler(fts_data,
+			TRUSTED_TOUCH_EVENT_ACCEPT_FAILURE);
+}
+static void fts_ts_vm_irq_on_lend_callback(void *data,
+					unsigned long notif_type,
+					enum gh_irq_label label)
+{
+	struct fts_ts_data *fts_data = data;
+
+	pr_debug("received irq lend request for label:%d\n", label);
+	if (fts_ts_trusted_touch_get_vm_state(fts_data) == TVM_IOMEM_LENT_NOTIFIED)
+		fts_ts_trusted_touch_set_vm_state(fts_data, TVM_ALL_RESOURCES_LENT_NOTIFIED);
+	else
+		fts_ts_trusted_touch_set_vm_state(fts_data, TVM_IRQ_LENT_NOTIFIED);
+}
+
+static void fts_ts_vm_mem_on_lend_handler(enum gh_mem_notifier_tag tag,
+		unsigned long notif_type, void *entry_data, void *notif_msg)
+{
+	struct gh_rm_notif_mem_shared_payload *payload;
+	struct trusted_touch_vm_info *vm_info;
+	struct fts_ts_data *fts_data;
+
+	fts_data = (struct fts_ts_data *)entry_data;
+	vm_info = fts_data->vm_info;
+	if (!vm_info) {
+		pr_err("Invalid vm_info\n");
+		return;
+	}
+
+	if (notif_type != GH_RM_NOTIF_MEM_SHARED ||
+			tag != vm_info->mem_tag) {
+		pr_err("Invalid command passed from rm\n");
+		return;
+	}
+
+	if (!entry_data || !notif_msg) {
+		pr_err("Invalid entry data passed from rm\n");
+		return;
+	}
+
+
+	payload = (struct gh_rm_notif_mem_shared_payload  *)notif_msg;
+	if (payload->trans_type != GH_RM_TRANS_TYPE_LEND ||
+			payload->label != TRUSTED_TOUCH_MEM_LABEL) {
+		pr_err("Invalid label or transaction type\n");
+		return;
+	}
+
+	vm_info->vm_mem_handle = payload->mem_handle;
+	pr_debug("received mem lend request with handle:%d\n",
+		vm_info->vm_mem_handle);
+	if (fts_ts_trusted_touch_get_vm_state(fts_data) == TVM_IRQ_LENT_NOTIFIED)
+		fts_ts_trusted_touch_set_vm_state(fts_data, TVM_ALL_RESOURCES_LENT_NOTIFIED);
+	else
+		fts_ts_trusted_touch_set_vm_state(fts_data, TVM_IOMEM_LENT_NOTIFIED);
+}
+
+static int fts_ts_vm_mem_release(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+
+	if (!fts_data->vm_info->vm_mem_handle) {
+		pr_err("Invalid memory handle\n");
+		return -EINVAL;
+	}
+
+	rc = gh_rm_mem_release(fts_data->vm_info->vm_mem_handle, 0);
+	if (rc)
+		pr_err("VM mem release failed: rc=%d\n", rc);
+
+	rc = gh_rm_mem_notify(fts_data->vm_info->vm_mem_handle,
+				GH_RM_MEM_NOTIFY_OWNER_RELEASED,
+				fts_data->vm_info->mem_tag, 0);
+	if (rc)
+		pr_err("Failed to notify mem release to PVM: rc=%d\n");
+	pr_debug("vm mem release succeded\n");
+
+	fts_data->vm_info->vm_mem_handle = 0;
+	return rc;
+}
+
+static void fts_ts_trusted_touch_tvm_vm_mode_disable(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+
+	if (atomic_read(&fts_data->trusted_touch_abort_status)) {
+		fts_ts_trusted_touch_abort_tvm(fts_data);
+		return;
+	}
+
+	fts_irq_disable();
+	fts_ts_trusted_touch_set_vm_state(fts_data, TVM_INTERRUPT_DISABLED);
+
+	rc = gh_irq_release(fts_data->vm_info->irq_label);
+	if (rc) {
+		pr_err("Failed to release irq rc:%d\n", rc);
+		goto error;
+	} else {
+		fts_ts_trusted_touch_set_vm_state(fts_data, TVM_IRQ_RELEASED);
+	}
+	rc = gh_irq_release_notify(fts_data->vm_info->irq_label);
+	if (rc)
+		pr_err("Failed to notify release irq rc:%d\n", rc);
+
+	pr_debug("vm irq release succeded\n");
+
+	fts_release_all_finger();
+
+	if (fts_data->bus_type == BUS_TYPE_I2C)
+		pm_runtime_put_sync(fts_data->client->adapter->dev.parent);
+	else
+		pm_runtime_put_sync(fts_data->spi->master->dev.parent);
+
+	fts_ts_trusted_touch_set_vm_state(fts_data, TVM_I2C_SESSION_RELEASED);
+	rc = fts_ts_vm_mem_release(fts_data);
+	if (rc) {
+		pr_err("Failed to release mem rc:%d\n", rc);
+		goto error;
+	} else {
+		fts_ts_trusted_touch_set_vm_state(fts_data, TVM_IOMEM_RELEASED);
+	}
+	fts_ts_trusted_touch_set_vm_state(fts_data, TRUSTED_TOUCH_TVM_INIT);
+	atomic_set(&fts_data->trusted_touch_enabled, 0);
+	pr_info("trusted touch disabled\n");
+	return;
+error:
+	fts_ts_trusted_touch_abort_handler(fts_data,
+			TRUSTED_TOUCH_EVENT_RELEASE_FAILURE);
+}
+
+int fts_ts_handle_trusted_touch_tvm(struct fts_ts_data *fts_data, int value)
+{
+	int err = 0;
+
+	switch (value) {
+	case 0:
+		if ((atomic_read(&fts_data->trusted_touch_enabled) == 0) &&
+			(atomic_read(&fts_data->trusted_touch_abort_status) == 0)) {
+			pr_err("Trusted touch is already disabled\n");
+			break;
+		}
+		if (atomic_read(&fts_data->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			fts_ts_trusted_touch_tvm_vm_mode_disable(fts_data);
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	case 1:
+		if (atomic_read(&fts_data->trusted_touch_enabled)) {
+			pr_err("Trusted touch usecase underway\n");
+			err = -EBUSY;
+			break;
+		}
+		if (atomic_read(&fts_data->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			fts_ts_trusted_touch_tvm_vm_mode_enable(fts_data);
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	default:
+		FTS_ERROR("unsupported value: %lu\n", value);
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+static void fts_ts_trusted_touch_abort_tvm(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+	int vm_state = fts_ts_trusted_touch_get_vm_state(fts_data);
+
+	if (vm_state >= TRUSTED_TOUCH_TVM_STATE_MAX) {
+		pr_err("invalid tvm driver state: %d\n", vm_state);
+		return;
+	}
+
+	switch (vm_state) {
+	case TVM_INTERRUPT_ENABLED:
+		fts_irq_disable();
+	case TVM_IRQ_ACCEPTED:
+	case TVM_INTERRUPT_DISABLED:
+		rc = gh_irq_release(fts_data->vm_info->irq_label);
+		if (rc)
+			pr_err("Failed to release irq rc:%d\n", rc);
+		rc = gh_irq_release_notify(fts_data->vm_info->irq_label);
+		if (rc)
+			pr_err("Failed to notify irq release rc:%d\n", rc);
+	case TVM_I2C_SESSION_ACQUIRED:
+	case TVM_IOMEM_ACCEPTED:
+	case TVM_IRQ_RELEASED:
+		fts_release_all_finger();
+		if (fts_data->bus_type == BUS_TYPE_I2C)
+			pm_runtime_put_sync(fts_data->client->adapter->dev.parent);
+		else
+			pm_runtime_put_sync(fts_data->spi->master->dev.parent);
+	case TVM_I2C_SESSION_RELEASED:
+		rc = fts_ts_vm_mem_release(fts_data);
+		if (rc)
+			pr_err("Failed to release mem rc:%d\n", rc);
+	case TVM_IOMEM_RELEASED:
+	case TVM_ALL_RESOURCES_LENT_NOTIFIED:
+	case TRUSTED_TOUCH_TVM_INIT:
+	case TVM_IRQ_LENT_NOTIFIED:
+	case TVM_IOMEM_LENT_NOTIFIED:
+		atomic_set(&fts_data->trusted_touch_enabled, 0);
+	}
+
+	atomic_set(&fts_data->trusted_touch_abort_status, 0);
+	fts_ts_trusted_touch_set_vm_state(fts_data, TRUSTED_TOUCH_TVM_INIT);
+}
+
+#else
+
+static void fts_ts_bus_put(struct fts_ts_data *fts_data);
+
+static void fts_ts_trusted_touch_abort_pvm(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+	int vm_state = fts_ts_trusted_touch_get_vm_state(fts_data);
+
+	if (vm_state >= TRUSTED_TOUCH_PVM_STATE_MAX) {
+		pr_err("Invalid driver state: %d\n", vm_state);
+		return;
+	}
+
+	switch (vm_state) {
+	case PVM_IRQ_RELEASE_NOTIFIED:
+	case PVM_ALL_RESOURCES_RELEASE_NOTIFIED:
+	case PVM_IRQ_LENT:
+	case PVM_IRQ_LENT_NOTIFIED:
+		rc = gh_irq_reclaim(fts_data->vm_info->irq_label);
+		if (rc)
+			pr_err("failed to reclaim irq on pvm rc:%d\n", rc);
+	case PVM_IRQ_RECLAIMED:
+	case PVM_IOMEM_LENT:
+	case PVM_IOMEM_LENT_NOTIFIED:
+	case PVM_IOMEM_RELEASE_NOTIFIED:
+		rc = gh_rm_mem_reclaim(fts_data->vm_info->vm_mem_handle, 0);
+		if (rc)
+			pr_err("failed to reclaim iomem on pvm rc:%d\n", rc);
+		fts_data->vm_info->vm_mem_handle = 0;
+	case PVM_IOMEM_RECLAIMED:
+	case PVM_INTERRUPT_DISABLED:
+		fts_irq_enable();
+	case PVM_I2C_RESOURCE_ACQUIRED:
+	case PVM_INTERRUPT_ENABLED:
+		fts_ts_bus_put(fts_data);
+	case TRUSTED_TOUCH_PVM_INIT:
+	case PVM_I2C_RESOURCE_RELEASED:
+		atomic_set(&fts_data->trusted_touch_enabled, 0);
+		atomic_set(&fts_data->trusted_touch_transition, 0);
+	}
+
+	atomic_set(&fts_data->trusted_touch_abort_status, 0);
+
+	fts_ts_trusted_touch_set_vm_state(fts_data, TRUSTED_TOUCH_PVM_INIT);
+}
+
+static int fts_ts_clk_prepare_enable(struct fts_ts_data *fts_data)
+{
+	int ret;
+
+	ret = clk_prepare_enable(fts_data->iface_clk);
+	if (ret) {
+		FTS_ERROR("error on clk_prepare_enable(iface_clk):%d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(fts_data->core_clk);
+	if (ret) {
+		clk_disable_unprepare(fts_data->iface_clk);
+		FTS_ERROR("error clk_prepare_enable(core_clk):%d\n", ret);
+	}
+	return ret;
+}
+
+static void fts_ts_clk_disable_unprepare(struct fts_ts_data *fts_data)
+{
+	clk_disable_unprepare(fts_data->core_clk);
+	clk_disable_unprepare(fts_data->iface_clk);
+}
+
+static int fts_ts_bus_get(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+	struct device *dev = NULL;
+
+	cancel_work_sync(&fts_data->resume_work);
+	reinit_completion(&fts_data->trusted_touch_powerdown);
+	fts_ts_enable_reg(fts_data, true);
+
+	if (fts_data->bus_type == BUS_TYPE_I2C)
+		dev = fts_data->client->adapter->dev.parent;
+	else
+		dev = fts_data->spi->master->dev.parent;
+
+	mutex_lock(&fts_data->fts_clk_io_ctrl_mutex);
+	rc = pm_runtime_get_sync(dev);
+	if (rc >= 0 &&  fts_data->core_clk != NULL &&
+				fts_data->iface_clk != NULL) {
+		rc = fts_ts_clk_prepare_enable(fts_data);
+		if (rc)
+			pm_runtime_put_sync(dev);
+	}
+
+	mutex_unlock(&fts_data->fts_clk_io_ctrl_mutex);
+	return rc;
+}
+
+static void fts_ts_bus_put(struct fts_ts_data *fts_data)
+{
+	struct device *dev = NULL;
+
+	if (fts_data->bus_type == BUS_TYPE_I2C)
+		dev = fts_data->client->adapter->dev.parent;
+	else
+		dev = fts_data->spi->master->dev.parent;
+
+	mutex_lock(&fts_data->fts_clk_io_ctrl_mutex);
+	if (fts_data->core_clk != NULL && fts_data->iface_clk != NULL)
+		fts_ts_clk_disable_unprepare(fts_data);
+	pm_runtime_put_sync(dev);
+	mutex_unlock(&fts_data->fts_clk_io_ctrl_mutex);
+	complete(&fts_data->trusted_touch_powerdown);
+	fts_ts_enable_reg(fts_data, false);
+}
+
+static struct gh_notify_vmid_desc *fts_ts_vm_get_vmid(gh_vmid_t vmid)
+{
+	struct gh_notify_vmid_desc *vmid_desc;
+
+	vmid_desc = kzalloc(offsetof(struct gh_notify_vmid_desc,
+				vmid_entries[1]), GFP_KERNEL);
+	if (!vmid_desc)
+		return ERR_PTR(ENOMEM);
+
+	vmid_desc->n_vmid_entries = 1;
+	vmid_desc->vmid_entries[0].vmid = vmid;
+	return vmid_desc;
+}
+
+static void fts_trusted_touch_pvm_vm_mode_disable(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+
+	atomic_set(&fts_data->trusted_touch_transition, 1);
+
+	if (atomic_read(&fts_data->trusted_touch_abort_status)) {
+		fts_ts_trusted_touch_abort_pvm(fts_data);
+		return;
+	}
+
+	if (fts_ts_trusted_touch_get_vm_state(fts_data) != PVM_ALL_RESOURCES_RELEASE_NOTIFIED)
+		pr_err("all release notifications are not received yet\n");
+
+	rc = gh_rm_mem_reclaim(fts_data->vm_info->vm_mem_handle, 0);
+	if (rc) {
+		pr_err("Trusted touch VM mem reclaim failed rc:%d\n", rc);
+		goto error;
+	}
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IOMEM_RECLAIMED);
+	fts_data->vm_info->vm_mem_handle = 0;
+	pr_debug("vm mem reclaim succeded!\n");
+
+	rc = gh_irq_reclaim(fts_data->vm_info->irq_label);
+	if (rc) {
+		pr_err("failed to reclaim irq on pvm rc:%d\n", rc);
+		goto error;
+	}
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IRQ_RECLAIMED);
+	pr_debug("vm irq reclaim succeded!\n");
+
+	fts_irq_enable();
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_INTERRUPT_ENABLED);
+	fts_ts_bus_put(fts_data);
+	atomic_set(&fts_data->trusted_touch_transition, 0);
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_I2C_RESOURCE_RELEASED);
+	fts_ts_trusted_touch_set_vm_state(fts_data, TRUSTED_TOUCH_PVM_INIT);
+	atomic_set(&fts_data->trusted_touch_enabled, 0);
+	pr_info("trusted touch disabled\n");
+	return;
+error:
+	fts_ts_trusted_touch_abort_handler(fts_data,
+			TRUSTED_TOUCH_EVENT_RECLAIM_FAILURE);
+}
+
+static void fts_ts_vm_irq_on_release_callback(void *data,
+					unsigned long notif_type,
+					enum gh_irq_label label)
+{
+	struct fts_ts_data *fts_data = data;
+
+	if (notif_type != GH_RM_NOTIF_VM_IRQ_RELEASED) {
+		pr_err("invalid notification type\n");
+		return;
+	}
+
+	if (fts_ts_trusted_touch_get_vm_state(fts_data) == PVM_IOMEM_RELEASE_NOTIFIED)
+		fts_ts_trusted_touch_set_vm_state(fts_data, PVM_ALL_RESOURCES_RELEASE_NOTIFIED);
+	else
+		fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IRQ_RELEASE_NOTIFIED);
+}
+
+static void fts_ts_vm_mem_on_release_handler(enum gh_mem_notifier_tag tag,
+		unsigned long notif_type, void *entry_data, void *notif_msg)
+{
+	struct gh_rm_notif_mem_released_payload *release_payload;
+	struct trusted_touch_vm_info *vm_info;
+	struct fts_ts_data *fts_data;
+
+	fts_data = (struct fts_ts_data *)entry_data;
+	vm_info = fts_data->vm_info;
+	if (!vm_info) {
+		pr_err(" Invalid vm_info\n");
+		return;
+	}
+
+	if (notif_type != GH_RM_NOTIF_MEM_RELEASED) {
+		pr_err(" Invalid notification type\n");
+		return;
+	}
+
+	if (tag != vm_info->mem_tag) {
+		pr_err(" Invalid tag\n");
+		return;
+	}
+
+	if (!entry_data || !notif_msg) {
+		pr_err(" Invalid data or notification message\n");
+		return;
+	}
+
+	release_payload = (struct gh_rm_notif_mem_released_payload  *)notif_msg;
+	if (release_payload->mem_handle != vm_info->vm_mem_handle) {
+		pr_err("Invalid mem handle detected\n");
+		return;
+	}
+
+	if (fts_ts_trusted_touch_get_vm_state(fts_data) == PVM_IRQ_RELEASE_NOTIFIED)
+		fts_ts_trusted_touch_set_vm_state(fts_data, PVM_ALL_RESOURCES_RELEASE_NOTIFIED);
+	else
+		fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IOMEM_RELEASE_NOTIFIED);
+}
+
+static int fts_ts_vm_mem_lend(struct fts_ts_data *fts_data)
+{
+	struct gh_acl_desc *acl_desc;
+	struct gh_sgl_desc *sgl_desc;
+	struct gh_notify_vmid_desc *vmid_desc;
+	gh_memparcel_handle_t mem_handle;
+	gh_vmid_t trusted_vmid;
+	int rc = 0;
+
+	acl_desc = fts_ts_vm_get_acl(GH_TRUSTED_VM);
+	if (IS_ERR(acl_desc)) {
+		pr_err("Failed to get acl of IO memories for Trusted touch\n");
+		PTR_ERR(acl_desc);
+		return -EINVAL;
+	}
+
+	sgl_desc = fts_ts_vm_get_sgl(fts_data->vm_info);
+	if (IS_ERR(sgl_desc)) {
+		pr_err("Failed to get sgl of IO memories for Trusted touch\n");
+		PTR_ERR(sgl_desc);
+		rc = -EINVAL;
+		goto sgl_error;
+	}
+
+	rc = gh_rm_mem_lend(GH_RM_MEM_TYPE_IO, 0, TRUSTED_TOUCH_MEM_LABEL,
+			acl_desc, sgl_desc, NULL, &mem_handle);
+	if (rc) {
+		pr_err("Failed to lend IO memories for Trusted touch rc:%d\n",
+							rc);
+		goto error;
+	}
+
+	pr_info("vm mem lend succeded\n");
+
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IOMEM_LENT);
+
+	gh_rm_get_vmid(GH_TRUSTED_VM, &trusted_vmid);
+
+	vmid_desc = fts_ts_vm_get_vmid(trusted_vmid);
+
+	rc = gh_rm_mem_notify(mem_handle, GH_RM_MEM_NOTIFY_RECIPIENT_SHARED,
+			fts_data->vm_info->mem_tag, vmid_desc);
+	if (rc) {
+		pr_err("Failed to notify mem lend to hypervisor rc:%d\n", rc);
+		goto vmid_error;
+	}
+
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IOMEM_LENT_NOTIFIED);
+
+	fts_data->vm_info->vm_mem_handle = mem_handle;
+vmid_error:
+	kfree(vmid_desc);
+error:
+	kfree(sgl_desc);
+sgl_error:
+	kfree(acl_desc);
+
+	return rc;
+}
+
+static int fts_ts_trusted_touch_pvm_vm_mode_enable(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+	struct trusted_touch_vm_info *vm_info = fts_data->vm_info;
+
+	atomic_set(&fts_data->trusted_touch_transition, 1);
+	mutex_lock(&fts_data->transition_lock);
+
+	if (fts_data->suspended) {
+		FTS_ERROR("Invalid power state for operation\n");
+		atomic_set(&fts_data->trusted_touch_transition, 0);
+		rc =  -EPERM;
+		goto error;
+	}
+
+	/* i2c session start and resource acquire */
+	if (fts_ts_bus_get(fts_data) < 0) {
+		FTS_ERROR("fts_ts_bus_get failed\n");
+		rc = -EIO;
+		goto error;
+	}
+
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_I2C_RESOURCE_ACQUIRED);
+	/* flush pending interurpts from FIFO */
+	fts_irq_disable();
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_INTERRUPT_DISABLED);
+	fts_release_all_finger();
+
+	rc = fts_ts_vm_mem_lend(fts_data);
+	if (rc) {
+		pr_err("Failed to lend memory\n");
+		goto abort_handler;
+	}
+	pr_debug("vm mem lend succeded\n");
+	rc = gh_irq_lend_v2(vm_info->irq_label, vm_info->vm_name,
+		fts_data->irq, &fts_ts_vm_irq_on_release_callback, fts_data);
+	if (rc) {
+		pr_err("Failed to lend irq\n");
+		goto abort_handler;
+	}
+
+	pr_debug("vm irq lend succeded for irq:%d\n", fts_data->irq);
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IRQ_LENT);
+
+	rc = gh_irq_lend_notify(vm_info->irq_label);
+	if (rc) {
+		pr_err("Failed to notify irq\n");
+		goto abort_handler;
+	}
+	fts_ts_trusted_touch_set_vm_state(fts_data, PVM_IRQ_LENT_NOTIFIED);
+
+	mutex_unlock(&fts_data->transition_lock);
+	atomic_set(&fts_data->trusted_touch_transition, 0);
+	atomic_set(&fts_data->trusted_touch_enabled, 1);
+	pr_info("trusted touch enabled\n");
+	return rc;
+
+abort_handler:
+	fts_ts_trusted_touch_abort_handler(fts_data, TRUSTED_TOUCH_EVENT_LEND_FAILURE);
+
+error:
+	mutex_unlock(&fts_data->transition_lock);
+	return rc;
+}
+
+int fts_ts_handle_trusted_touch_pvm(struct fts_ts_data *fts_data, int value)
+{
+	int err = 0;
+
+	switch (value) {
+	case 0:
+		if (atomic_read(&fts_data->trusted_touch_enabled) == 0 &&
+			(atomic_read(&fts_data->trusted_touch_abort_status) == 0)) {
+			pr_err("Trusted touch is already disabled\n");
+			break;
+		}
+		if (atomic_read(&fts_data->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			fts_trusted_touch_pvm_vm_mode_disable(fts_data);
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	case 1:
+		if (atomic_read(&fts_data->trusted_touch_enabled)) {
+			pr_err("Trusted touch usecase underway\n");
+			err = -EBUSY;
+			break;
+		}
+		if (atomic_read(&fts_data->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			err = fts_ts_trusted_touch_pvm_vm_mode_enable(fts_data);
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	default:
+		FTS_ERROR("unsupported value: %lu\n", value);
+		err = -EINVAL;
+		break;
+	}
+	return err;
+}
+
+#endif
+
+static void fts_ts_trusted_touch_event_notify(struct fts_ts_data *fts_data, int event)
+{
+	atomic_set(&fts_data->trusted_touch_event, event);
+	sysfs_notify(&fts_data->dev->kobj, NULL, "trusted_touch_event");
+}
+
+static void fts_ts_trusted_touch_abort_handler(struct fts_ts_data *fts_data, int error)
+{
+	atomic_set(&fts_data->trusted_touch_abort_status, error);
+	pr_err("TUI session aborted with failure:%d\n", error);
+	fts_ts_trusted_touch_event_notify(fts_data, error);
+#ifdef CONFIG_ARCH_QTI_VM
+	pr_err("Resetting touch controller\n");
+	if (fts_ts_trusted_touch_get_vm_state(fts_data) >= TVM_IOMEM_ACCEPTED &&
+			error == TRUSTED_TOUCH_EVENT_I2C_FAILURE) {
+		pr_err("Resetting touch controller\n");
+		fts_ts_trusted_touch_reset_gpio_toggle(fts_data);
+	}
+#endif
+}
+
+static int fts_ts_vm_init(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+	struct trusted_touch_vm_info *vm_info;
+	void *mem_cookie;
+
+	rc = fts_ts_populate_vm_info(fts_data);
+	if (rc) {
+		pr_err("Cannot setup vm pipeline\n");
+		rc = -EINVAL;
+		goto fail;
+	}
+
+	vm_info = fts_data->vm_info;
+#ifdef CONFIG_ARCH_QTI_VM
+	mem_cookie = gh_mem_notifier_register(vm_info->mem_tag,
+			fts_ts_vm_mem_on_lend_handler, fts_data);
+	if (!mem_cookie) {
+		pr_err("Failed to register on lend mem notifier\n");
+		rc = -EINVAL;
+		goto init_fail;
+	}
+	vm_info->mem_cookie = mem_cookie;
+	rc = gh_irq_wait_for_lend_v2(vm_info->irq_label, GH_PRIMARY_VM,
+			&fts_ts_vm_irq_on_lend_callback, fts_data);
+	fts_ts_trusted_touch_set_vm_state(fts_data, TRUSTED_TOUCH_TVM_INIT);
+#else
+	mem_cookie = gh_mem_notifier_register(vm_info->mem_tag,
+			fts_ts_vm_mem_on_release_handler, fts_data);
+	if (!mem_cookie) {
+		pr_err("Failed to register on release mem notifier\n");
+		rc = -EINVAL;
+		goto init_fail;
+	}
+	vm_info->mem_cookie = mem_cookie;
+	fts_ts_trusted_touch_set_vm_state(fts_data, TRUSTED_TOUCH_PVM_INIT);
+#endif
+	return rc;
+init_fail:
+	fts_ts_vm_deinit(fts_data);
+fail:
+	return rc;
+}
+
+static void fts_ts_dt_parse_trusted_touch_info(struct fts_ts_data *fts_data)
+{
+	struct device_node *np = fts_data->dev->of_node;
+	int rc = 0;
+	const char *selection;
+	const char *environment;
+
+	rc = of_property_read_string(np, "focaltech,trusted-touch-mode",
+								&selection);
+	if (rc) {
+		dev_warn(fts_data->dev,
+			"%s: No trusted touch mode selection made\n", __func__);
+		atomic_set(&fts_data->trusted_touch_mode,
+						TRUSTED_TOUCH_MODE_NONE);
+		return;
+	}
+
+	if (!strcmp(selection, "vm_mode")) {
+		atomic_set(&fts_data->trusted_touch_mode,
+						TRUSTED_TOUCH_VM_MODE);
+		pr_err("Selected trusted touch mode to VM mode\n");
+	} else {
+		atomic_set(&fts_data->trusted_touch_mode,
+						TRUSTED_TOUCH_MODE_NONE);
+		pr_err("Invalid trusted_touch mode\n");
+	}
+
+	rc = of_property_read_string(np, "focaltech,touch-environment",
+						&environment);
+	if (rc) {
+		dev_warn(fts_data->dev,
+			"%s: No trusted touch mode environment\n", __func__);
+	}
+	fts_data->touch_environment = environment;
+	pr_err("Trusted touch environment:%s\n",
+			fts_data->touch_environment);
+}
+
+static void fts_ts_trusted_touch_init(struct fts_ts_data *fts_data)
+{
+	int rc = 0;
+
+	atomic_set(&fts_data->trusted_touch_initialized, 0);
+	fts_ts_dt_parse_trusted_touch_info(fts_data);
+
+	if (atomic_read(&fts_data->trusted_touch_mode) ==
+						TRUSTED_TOUCH_MODE_NONE)
+		return;
+
+	init_completion(&fts_data->trusted_touch_powerdown);
+
+	/* Get clocks */
+	fts_data->core_clk = devm_clk_get(fts_data->dev->parent,
+						"m-ahb");
+	if (IS_ERR(fts_data->core_clk)) {
+		fts_data->core_clk = NULL;
+		dev_warn(fts_data->dev,
+				"%s: core_clk is not defined\n", __func__);
+	}
+
+	fts_data->iface_clk = devm_clk_get(fts_data->dev->parent,
+						"se-clk");
+	if (IS_ERR(fts_data->iface_clk)) {
+		fts_data->iface_clk = NULL;
+		dev_warn(fts_data->dev,
+			"%s: iface_clk is not defined\n", __func__);
+	}
+
+	if (atomic_read(&fts_data->trusted_touch_mode) ==
+						TRUSTED_TOUCH_VM_MODE) {
+		rc = fts_ts_vm_init(fts_data);
+		if (rc)
+			pr_err("Failed to init VM\n");
+	}
+	atomic_set(&fts_data->trusted_touch_initialized, 1);
+}
+
+#endif
+
+/*****************************************************************************
+*  Name: fts_wait_tp_to_valid
+*  Brief: Read chip id until TP FW become valid(Timeout: TIMEOUT_READ_REG),
+*         need call when reset/power on/resume...
+*  Input:
+*  Output:
+*  Return: return 0 if tp valid, otherwise return error code
+*****************************************************************************/
+int fts_wait_tp_to_valid(void)
+{
+	int ret = 0;
+	int cnt = 0;
+	u8 idh = 0;
+	u8 idl = 0;
+	u8 chip_idh = fts_data->ic_info.ids.chip_idh;
+	u8 chip_idl = fts_data->ic_info.ids.chip_idl;
+
+	do {
+		ret = fts_read_reg(FTS_REG_CHIP_ID, &idh);
+		ret = fts_read_reg(FTS_REG_CHIP_ID2, &idl);
+		if ((ret < 0) || (idh != chip_idh) || (idl != chip_idl)) {
+			FTS_DEBUG("TP Not Ready,ReadData:0x%02x%02x", idh, idl);
+		} else if ((idh == chip_idh) && (idl == chip_idl)) {
+			FTS_INFO("TP Ready,Device ID:0x%02x%02x", idh, idl);
+			return 0;
+		}
+		cnt++;
+		msleep(INTERVAL_READ_REG);
+	} while ((cnt * INTERVAL_READ_REG) < TIMEOUT_READ_REG);
+
+	return -EIO;
+}
+
+/*****************************************************************************
+*  Name: fts_tp_state_recovery
+*  Brief: Need execute this function when reset
+*  Input:
+*  Output:
+*  Return:
+*****************************************************************************/
+void fts_tp_state_recovery(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+	/* wait tp stable */
+	fts_wait_tp_to_valid();
+	/* recover TP charger state 0x8B */
+	/* recover TP glove state 0xC0 */
+	/* recover TP cover state 0xC1 */
+	fts_ex_mode_recovery(ts_data);
+	/* recover TP gesture state 0xD0 */
+	fts_gesture_recovery(ts_data);
+	FTS_FUNC_EXIT();
+}
+
+int fts_reset_proc(int hdelayms)
+{
+	FTS_DEBUG("tp reset");
+	gpio_direction_output(fts_data->pdata->reset_gpio, 0);
+	msleep(1);
+	gpio_direction_output(fts_data->pdata->reset_gpio, 1);
+	if (hdelayms) {
+		msleep(hdelayms);
+	}
+
+	return 0;
+}
+
+void fts_irq_disable(void)
+{
+	unsigned long irqflags;
+
+	FTS_FUNC_ENTER();
+	spin_lock_irqsave(&fts_data->irq_lock, irqflags);
+
+	if (!fts_data->irq_disabled) {
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+		if (atomic_read(&fts_data->trusted_touch_transition))
+			disable_irq_wake(fts_data->irq);
+		else
+			disable_irq_nosync(fts_data->irq);
+#else
+		disable_irq_nosync(fts_data->irq);
+#endif
+		fts_data->irq_disabled = true;
+	}
+
+	spin_unlock_irqrestore(&fts_data->irq_lock, irqflags);
+	FTS_FUNC_EXIT();
+}
+
+void fts_irq_enable(void)
+{
+	unsigned long irqflags = 0;
+
+	FTS_FUNC_ENTER();
+	spin_lock_irqsave(&fts_data->irq_lock, irqflags);
+
+	if (fts_data->irq_disabled) {
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+		if (atomic_read(&fts_data->trusted_touch_transition))
+			enable_irq_wake(fts_data->irq);
+		else
+			enable_irq(fts_data->irq);
+#else
+		enable_irq(fts_data->irq);
+#endif
+		fts_data->irq_disabled = false;
+	}
+
+	spin_unlock_irqrestore(&fts_data->irq_lock, irqflags);
+	FTS_FUNC_EXIT();
+}
+
+void fts_hid2std(void)
+{
+	int ret = 0;
+	u8 buf[3] = {0xEB, 0xAA, 0x09};
+
+	if (fts_data->bus_type != BUS_TYPE_I2C)
+		return;
+
+	ret = fts_write(buf, 3);
+	if (ret < 0) {
+		FTS_ERROR("hid2std cmd write fail");
+		return;
+	}
+
+	msleep(20);
+	buf[0] = buf[1] = buf[2] = 0;
+	ret = fts_read(NULL, 0, buf, 3);
+	if (ret < 0)
+		FTS_ERROR("hid2std cmd read fail");
+	else if ((buf[0] == 0xEB) && (buf[1] == 0xAA) && (buf[2] == 0x08))
+		FTS_DEBUG("hidi2c change to stdi2c successful");
+	else
+		FTS_DEBUG("hidi2c change to stdi2c not support or fail");
+
+}
+
+static int fts_get_chip_types(
+	struct fts_ts_data *ts_data,
+	u8 id_h, u8 id_l, bool fw_valid)
+{
+	int i = 0;
+	u32 ctype_entries = sizeof(ctype) / sizeof(struct ft_chip_t);
+
+	if ((0x0 == id_h) || (0x0 == id_l)) {
+		FTS_ERROR("id_h/id_l is 0");
+		return -EINVAL;
+	}
+
+	FTS_DEBUG("verify id:0x%02x%02x", id_h, id_l);
+	for (i = 0; i < ctype_entries; i++) {
+		if (VALID == fw_valid) {
+			if ((id_h == ctype[i].chip_idh) && (id_l == ctype[i].chip_idl))
+				break;
+		} else {
+			if (((id_h == ctype[i].rom_idh) && (id_l == ctype[i].rom_idl))
+				|| ((id_h == ctype[i].pb_idh) && (id_l == ctype[i].pb_idl))
+				|| ((id_h == ctype[i].bl_idh) && (id_l == ctype[i].bl_idl)))
+			break;
+		}
+	}
+
+	if (i >= ctype_entries)
+		return -ENODATA;
+
+	ts_data->ic_info.ids = ctype[i];
+	return 0;
+}
+
+static int fts_read_bootid(struct fts_ts_data *ts_data, u8 *id)
+{
+	int ret = 0;
+	u8 chip_id[2] = { 0 };
+	u8 id_cmd[4] = { 0 };
+	u32 id_cmd_len = 0;
+
+	id_cmd[0] = FTS_CMD_START1;
+	id_cmd[1] = FTS_CMD_START2;
+	ret = fts_write(id_cmd, 2);
+	if (ret < 0) {
+		FTS_ERROR("start cmd write fail");
+		return ret;
+	}
+
+	msleep(FTS_CMD_START_DELAY);
+	id_cmd[0] = FTS_CMD_READ_ID;
+	id_cmd[1] = id_cmd[2] = id_cmd[3] = 0x00;
+	if (ts_data->ic_info.is_incell)
+		id_cmd_len = FTS_CMD_READ_ID_LEN_INCELL;
+	else
+		id_cmd_len = FTS_CMD_READ_ID_LEN;
+	ret = fts_read(id_cmd, id_cmd_len, chip_id, 2);
+	if ((ret < 0) || (0x0 == chip_id[0]) || (0x0 == chip_id[1])) {
+		FTS_ERROR("read boot id fail,read:0x%02x%02x", chip_id[0], chip_id[1]);
+		return -EIO;
+	}
+
+	id[0] = chip_id[0];
+	id[1] = chip_id[1];
+	return 0;
+}
+
+/*****************************************************************************
+* Name: fts_get_ic_information
+* Brief: read chip id to get ic information, after run the function, driver w-
+*        ill know which IC is it.
+*        If cant get the ic information, maybe not focaltech's touch IC, need
+*        unregister the driver
+* Input:
+* Output:
+* Return: return 0 if get correct ic information, otherwise return error code
+*****************************************************************************/
+static int fts_get_ic_information(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+	int cnt = 0;
+	u8 chip_id[2] = { 0 };
+	u32 type = ts_data->pdata->type;
+
+	ts_data->ic_info.is_incell = FTS_CHIP_IDC(type);
+	ts_data->ic_info.hid_supported = FTS_HID_SUPPORTTED(type);
+
+	do {
+		ret = fts_read_reg(FTS_REG_CHIP_ID, &chip_id[0]);
+		ret = fts_read_reg(FTS_REG_CHIP_ID2, &chip_id[1]);
+		if ((ret < 0) || (0x0 == chip_id[0]) || (0x0 == chip_id[1])) {
+			FTS_DEBUG("i2c read invalid, read:0x%02x%02x",
+				chip_id[0], chip_id[1]);
+		} else {
+			ret = fts_get_chip_types(ts_data, chip_id[0], chip_id[1], VALID);
+			if (!ret)
+				break;
+			else
+				FTS_DEBUG("TP not ready, read:0x%02x%02x",
+						chip_id[0], chip_id[1]);
+		}
+
+		cnt++;
+		msleep(INTERVAL_READ_REG);
+	} while ((cnt * INTERVAL_READ_REG) < TIMEOUT_READ_REG);
+
+	if ((cnt * INTERVAL_READ_REG) >= TIMEOUT_READ_REG) {
+		FTS_INFO("fw is invalid, need read boot id");
+		if (ts_data->ic_info.hid_supported) {
+			fts_hid2std();
+		}
+
+		ret = fts_read_bootid(ts_data, &chip_id[0]);
+		if (ret <  0) {
+			FTS_ERROR("read boot id fail");
+			return ret;
+		}
+
+		ret = fts_get_chip_types(ts_data, chip_id[0], chip_id[1], INVALID);
+		if (ret < 0) {
+			FTS_ERROR("can't get ic informaton");
+			return ret;
+		}
+	}
+
+	FTS_INFO("get ic information, chip id = 0x%02x%02x",
+		ts_data->ic_info.ids.chip_idh, ts_data->ic_info.ids.chip_idl);
+
+	return 0;
+}
+
+/*****************************************************************************
+*  Reprot related
+*****************************************************************************/
+static void fts_show_touch_buffer(u8 *data, int datalen)
+{
+	int i = 0;
+	int count = 0;
+	char *tmpbuf = NULL;
+
+	tmpbuf = kzalloc(1024, GFP_KERNEL);
+	if (!tmpbuf) {
+		FTS_ERROR("tmpbuf zalloc fail");
+		return;
+	}
+
+	for (i = 0; i < datalen; i++) {
+		count += snprintf(tmpbuf + count, 1024 - count, "%02X,", data[i]);
+		if (count >= 1024)
+			break;
+	}
+	FTS_DEBUG("point buffer:%s", tmpbuf);
+
+	if (tmpbuf) {
+		kfree(tmpbuf);
+		tmpbuf = NULL;
+	}
+}
+
+void fts_release_all_finger(void)
+{
+	struct input_dev *input_dev = fts_data->input_dev;
+#if FTS_MT_PROTOCOL_B_EN
+	u32 finger_count = 0;
+	u32 max_touches = fts_data->pdata->max_touch_number;
+#endif
+
+	FTS_FUNC_ENTER();
+	mutex_lock(&fts_data->report_mutex);
+#if FTS_MT_PROTOCOL_B_EN
+	for (finger_count = 0; finger_count < max_touches; finger_count++) {
+		input_mt_slot(input_dev, finger_count);
+		input_mt_report_slot_state(input_dev, MT_TOOL_FINGER, false);
+	}
+#else
+	input_mt_sync(input_dev);
+#endif
+	input_report_key(input_dev, BTN_TOUCH, 0);
+	input_sync(input_dev);
+
+	fts_data->touchs = 0;
+	fts_data->key_state = 0;
+	mutex_unlock(&fts_data->report_mutex);
+	FTS_FUNC_EXIT();
+}
+
+/*****************************************************************************
+* Name: fts_input_report_key
+* Brief: process key events,need report key-event if key enable.
+*        if point's coordinate is in (x_dim-50,y_dim-50) ~ (x_dim+50,y_dim+50),
+*        need report it to key event.
+*        x_dim: parse from dts, means key x_coordinate, dimension:+-50
+*        y_dim: parse from dts, means key y_coordinate, dimension:+-50
+* Input:
+* Output:
+* Return: return 0 if it's key event, otherwise return error code
+*****************************************************************************/
+static int fts_input_report_key(struct fts_ts_data *data, int index)
+{
+	int i = 0;
+	int x = data->events[index].x;
+	int y = data->events[index].y;
+	int *x_dim = &data->pdata->key_x_coords[0];
+	int *y_dim = &data->pdata->key_y_coords[0];
+
+	if (!data->pdata->have_key) {
+		return -EINVAL;
+	}
+
+	for (i = 0; i < data->pdata->key_number; i++) {
+		if ((x >= x_dim[i] - FTS_KEY_DIM) && (x <= x_dim[i] + FTS_KEY_DIM) &&
+			(y >= y_dim[i] - FTS_KEY_DIM) && (y <= y_dim[i] + FTS_KEY_DIM)) {
+			if (EVENT_DOWN(data->events[index].flag)
+				&& !(data->key_state & (1 << i))) {
+				input_report_key(data->input_dev, data->pdata->keys[i], 1);
+				data->key_state |= (1 << i);
+				FTS_DEBUG("Key%d(%d,%d) DOWN!", i, x, y);
+			} else if (EVENT_UP(data->events[index].flag)
+				&& (data->key_state & (1 << i))) {
+				input_report_key(data->input_dev, data->pdata->keys[i], 0);
+				data->key_state &= ~(1 << i);
+				FTS_DEBUG("Key%d(%d,%d) Up!", i, x, y);
+			}
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+#if FTS_MT_PROTOCOL_B_EN
+static int fts_input_report_b(struct fts_ts_data *data)
+{
+	int i = 0;
+	int uppoint = 0;
+	int touchs = 0;
+	bool va_reported = false;
+	u32 max_touch_num = data->pdata->max_touch_number;
+	struct ts_event *events = data->events;
+
+	for (i = 0; i < data->touch_point; i++) {
+		if (fts_input_report_key(data, i) == 0)
+			continue;
+
+		va_reported = true;
+		input_mt_slot(data->input_dev, events[i].id);
+
+		if (EVENT_DOWN(events[i].flag)) {
+			input_mt_report_slot_state(data->input_dev, MT_TOOL_FINGER, true);
+
+#if FTS_REPORT_PRESSURE_EN
+			if (events[i].p <= 0) {
+				events[i].p = 0x3f;
+			}
+			input_report_abs(data->input_dev, ABS_MT_PRESSURE, events[i].p);
+#endif
+			if (events[i].area <= 0) {
+				events[i].area = 0x09;
+			}
+			input_report_abs(data->input_dev, ABS_MT_TOUCH_MAJOR, events[i].area);
+			input_report_abs(data->input_dev, ABS_MT_POSITION_X, events[i].x);
+			input_report_abs(data->input_dev, ABS_MT_POSITION_Y, events[i].y);
+
+			touchs |= BIT(events[i].id);
+			data->touchs |= BIT(events[i].id);
+
+			if ((data->log_level >= 2) ||
+				((1 == data->log_level) && (FTS_TOUCH_DOWN == events[i].flag))) {
+				FTS_DEBUG("[B]P%d(%d, %d)[p:%d,tm:%d] DOWN!",
+					events[i].id,
+					events[i].x, events[i].y,
+					events[i].p, events[i].area);
+			}
+		} else {
+			uppoint++;
+			input_mt_report_slot_state(data->input_dev, MT_TOOL_FINGER, false);
+			data->touchs &= ~BIT(events[i].id);
+			if (data->log_level >= 1) {
+				FTS_DEBUG("[B]P%d UP!", events[i].id);
+			}
+		}
+	}
+
+	if (unlikely(data->touchs ^ touchs)) {
+		for (i = 0; i < max_touch_num; i++)  {
+			if (BIT(i) & (data->touchs ^ touchs)) {
+				if (data->log_level >= 1) {
+					FTS_DEBUG("[B]P%d UP!", i);
+				}
+			va_reported = true;
+			input_mt_slot(data->input_dev, i);
+			input_mt_report_slot_state(data->input_dev, MT_TOOL_FINGER, false);
+			}
+		}
+	}
+	data->touchs = touchs;
+
+	if (va_reported) {
+		/* touchs==0, there's no point but key */
+		if (EVENT_NO_DOWN(data) || (!touchs)) {
+			if (data->log_level >= 1) {
+				FTS_DEBUG("[B]Points All Up!");
+			}
+			input_report_key(data->input_dev, BTN_TOUCH, 0);
+		} else {
+			input_report_key(data->input_dev, BTN_TOUCH, 1);
+		}
+	}
+
+	input_sync(data->input_dev);
+	return 0;
+}
+
+#else
+static int fts_input_report_a(struct fts_ts_data *data)
+{
+	int i = 0;
+	int touchs = 0;
+	bool va_reported = false;
+	struct ts_event *events = data->events;
+
+	for (i = 0; i < data->touch_point; i++) {
+		if (fts_input_report_key(data, i) == 0) {
+			continue;
+		}
+
+		va_reported = true;
+		if (EVENT_DOWN(events[i].flag)) {
+			input_report_abs(data->input_dev, ABS_MT_TRACKING_ID, events[i].id);
+#if FTS_REPORT_PRESSURE_EN
+			if (events[i].p <= 0) {
+				events[i].p = 0x3f;
+			}
+			input_report_abs(data->input_dev, ABS_MT_PRESSURE, events[i].p);
+#endif
+			if (events[i].area <= 0) {
+				events[i].area = 0x09;
+			}
+			input_report_abs(data->input_dev, ABS_MT_TOUCH_MAJOR, events[i].area);
+
+			input_report_abs(data->input_dev, ABS_MT_POSITION_X, events[i].x);
+			input_report_abs(data->input_dev, ABS_MT_POSITION_Y, events[i].y);
+
+			input_mt_sync(data->input_dev);
+
+			if ((data->log_level >= 2) ||
+				((1 == data->log_level) && (FTS_TOUCH_DOWN == events[i].flag))) {
+				FTS_DEBUG("[A]P%d(%d, %d)[p:%d,tm:%d] DOWN!",
+					events[i].id,
+					events[i].x, events[i].y,
+					events[i].p, events[i].area);
+			}
+			touchs++;
+		}
+	}
+
+	/* last point down, current no point but key */
+	if (data->touchs && !touchs) {
+		va_reported = true;
+	}
+	data->touchs = touchs;
+
+	if (va_reported) {
+		if (EVENT_NO_DOWN(data)) {
+			if (data->log_level >= 1) {
+				FTS_DEBUG("[A]Points All Up!");
+			}
+			input_report_key(data->input_dev, BTN_TOUCH, 0);
+			input_mt_sync(data->input_dev);
+		} else {
+			input_report_key(data->input_dev, BTN_TOUCH, 1);
+		}
+	}
+
+	input_sync(data->input_dev);
+	return 0;
+}
+#endif
+
+static int fts_read_touchdata(struct fts_ts_data *data)
+{
+	int ret = 0;
+	u8 *buf = data->point_buf;
+
+	memset(buf, 0xFF, data->pnt_buf_size);
+	buf[0] = 0x01;
+
+	if (data->gesture_mode) {
+		if (0 == fts_gesture_readdata(data, NULL)) {
+			FTS_INFO("succuss to get gesture data in irq handler");
+			return 1;
+		}
+	}
+
+	ret = fts_read(buf, 1, buf + 1, data->pnt_buf_size - 1);
+	if (ret < 0) {
+		FTS_ERROR("read touchdata failed, ret:%d", ret);
+		return ret;
+	}
+
+	if (data->log_level >= 3) {
+		fts_show_touch_buffer(buf, data->pnt_buf_size);
+	}
+
+	return 0;
+}
+
+static int fts_read_parse_touchdata(struct fts_ts_data *data)
+{
+	int ret = 0;
+	int i = 0;
+	u8 pointid = 0;
+	int base = 0;
+	struct ts_event *events = data->events;
+	int max_touch_num = data->pdata->max_touch_number;
+	u8 *buf = data->point_buf;
+
+	ret = fts_read_touchdata(data);
+	if (ret) {
+		return ret;
+	}
+
+	data->point_num = buf[FTS_TOUCH_POINT_NUM] & 0x0F;
+	data->touch_point = 0;
+
+	if (data->ic_info.is_incell) {
+		if ((data->point_num == 0x0F) && (buf[2] == 0xFF) && (buf[3] == 0xFF)
+			&& (buf[4] == 0xFF) && (buf[5] == 0xFF) && (buf[6] == 0xFF)) {
+			FTS_DEBUG("touch buff is 0xff, need recovery state");
+			fts_release_all_finger();
+			fts_tp_state_recovery(data);
+			return -EIO;
+		}
+	}
+
+	if (data->point_num > max_touch_num) {
+		FTS_INFO("invalid point_num(%d)", data->point_num);
+		return -EIO;
+	}
+
+	for (i = 0; i < max_touch_num; i++) {
+		base = FTS_ONE_TCH_LEN * i;
+		pointid = (buf[FTS_TOUCH_ID_POS + base]) >> 4;
+		if (pointid >= FTS_MAX_ID)
+			break;
+		else if (pointid >= max_touch_num) {
+			FTS_ERROR("ID(%d) beyond max_touch_number", pointid);
+			return -EINVAL;
+		}
+
+		data->touch_point++;
+		events[i].x = ((buf[FTS_TOUCH_X_H_POS + base] & 0x0F) << 8) +
+				(buf[FTS_TOUCH_X_L_POS + base] & 0xFF);
+		events[i].y = ((buf[FTS_TOUCH_Y_H_POS + base] & 0x0F) << 8) +
+				(buf[FTS_TOUCH_Y_L_POS + base] & 0xFF);
+		events[i].flag = buf[FTS_TOUCH_EVENT_POS + base] >> 6;
+		events[i].id = buf[FTS_TOUCH_ID_POS + base] >> 4;
+		events[i].area = buf[FTS_TOUCH_AREA_POS + base] >> 4;
+		events[i].p =  buf[FTS_TOUCH_PRE_POS + base];
+
+		if (EVENT_DOWN(events[i].flag) && (data->point_num == 0)) {
+			FTS_INFO("abnormal touch data from fw");
+			return -EIO;
+		}
+	}
+
+	if (data->touch_point == 0) {
+		FTS_INFO("no touch point information");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void fts_irq_read_report(void)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = fts_data;
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_set_intr(1);
+#endif
+
+#if FTS_POINT_REPORT_CHECK_EN
+	fts_prc_queue_work(ts_data);
+#endif
+
+	ret = fts_read_parse_touchdata(ts_data);
+	if (ret == 0) {
+		mutex_lock(&ts_data->report_mutex);
+#if FTS_MT_PROTOCOL_B_EN
+		fts_input_report_b(ts_data);
+#else
+		fts_input_report_a(ts_data);
+#endif
+		mutex_unlock(&ts_data->report_mutex);
+	}
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_set_intr(0);
+#endif
+}
+
+static irqreturn_t fts_irq_handler(int irq, void *data)
+{
+	struct fts_ts_data *fts_data = data;
+
+	if (!fts_data) {
+		pr_err("%s: Invalid fts_data\n", __func__);
+		return IRQ_HANDLED;
+	}
+
+	if (!mutex_trylock(&fts_data->transition_lock))
+		return IRQ_HANDLED;
+
+	fts_irq_read_report();
+	mutex_unlock(&fts_data->transition_lock);
+
+	return IRQ_HANDLED;
+}
+
+static int fts_irq_registration(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+	struct fts_ts_platform_data *pdata = ts_data->pdata;
+
+#ifdef CONFIG_ARCH_QTI_VM
+	pdata->irq_gpio_flags = IRQF_TRIGGER_RISING | IRQF_ONESHOT;
+	FTS_INFO("irq:%d, flag:%x", ts_data->irq, pdata->irq_gpio_flags);
+	ret = request_threaded_irq(ts_data->irq, NULL, fts_irq_handler,
+				pdata->irq_gpio_flags,
+				FTS_DRIVER_NAME, ts_data);
+#else
+	ts_data->irq = gpio_to_irq(pdata->irq_gpio);
+	pdata->irq_gpio_flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT;
+	FTS_INFO("irq:%d, flag:%x", ts_data->irq, pdata->irq_gpio_flags);
+	ret = request_threaded_irq(ts_data->irq, NULL, fts_irq_handler,
+				pdata->irq_gpio_flags,
+				FTS_DRIVER_NAME, ts_data);
+#endif
+	return ret;
+}
+
+static int fts_input_init(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+	int key_num = 0;
+	struct fts_ts_platform_data *pdata = ts_data->pdata;
+	struct input_dev *input_dev;
+
+	FTS_FUNC_ENTER();
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		FTS_ERROR("Failed to allocate memory for input device");
+		return -ENOMEM;
+	}
+
+	/* Init and register Input device */
+	input_dev->name = FTS_DRIVER_NAME;
+	if (ts_data->bus_type == BUS_TYPE_I2C)
+		input_dev->id.bustype = BUS_I2C;
+	else
+		input_dev->id.bustype = BUS_SPI;
+	input_dev->dev.parent = ts_data->dev;
+
+	input_set_drvdata(input_dev, ts_data);
+
+	__set_bit(EV_SYN, input_dev->evbit);
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+	__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
+
+	if (pdata->have_key) {
+		FTS_INFO("set key capabilities");
+		for (key_num = 0; key_num < pdata->key_number; key_num++)
+			input_set_capability(input_dev, EV_KEY, pdata->keys[key_num]);
+	}
+
+#if FTS_MT_PROTOCOL_B_EN
+	input_mt_init_slots(input_dev, pdata->max_touch_number, INPUT_MT_DIRECT);
+#else
+	input_set_abs_params(input_dev, ABS_MT_TRACKING_ID, 0, 0x0F, 0, 0);
+#endif
+	input_set_abs_params(input_dev, ABS_MT_POSITION_X, pdata->x_min, pdata->x_max, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_Y, pdata->y_min, pdata->y_max, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR, 0, 0xFF, 0, 0);
+#if FTS_REPORT_PRESSURE_EN
+	input_set_abs_params(input_dev, ABS_MT_PRESSURE, 0, 0xFF, 0, 0);
+#endif
+
+	ret = input_register_device(input_dev);
+	if (ret) {
+		FTS_ERROR("Input device registration failed");
+		input_set_drvdata(input_dev, NULL);
+		input_free_device(input_dev);
+		input_dev = NULL;
+		return ret;
+	}
+
+	ts_data->input_dev = input_dev;
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+static int fts_report_buffer_init(struct fts_ts_data *ts_data)
+{
+	int point_num = 0;
+	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->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");
+		return -ENOMEM;
+	}
+
+	events_num = point_num * sizeof(struct ts_event);
+	ts_data->events = (struct ts_event *)kzalloc(events_num, GFP_KERNEL);
+	if (!ts_data->events) {
+		FTS_ERROR("failed to alloc memory for point events");
+		kfree_safe(ts_data->point_buf);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+#if FTS_POWER_SOURCE_CUST_EN
+/*****************************************************************************
+* Power Control
+*****************************************************************************/
+#if FTS_PINCTRL_EN
+static int fts_pinctrl_init(struct fts_ts_data *ts)
+{
+	int ret = 0;
+
+	ts->pinctrl = devm_pinctrl_get(ts->dev);
+	if (IS_ERR_OR_NULL(ts->pinctrl)) {
+		FTS_ERROR("Failed to get pinctrl, please check dts");
+		ret = PTR_ERR(ts->pinctrl);
+		goto err_pinctrl_get;
+	}
+
+	ts->pins_active = pinctrl_lookup_state(ts->pinctrl, "pmx_ts_active");
+	if (IS_ERR_OR_NULL(ts->pins_active)) {
+		FTS_ERROR("Pin state[active] not found");
+		ret = PTR_ERR(ts->pins_active);
+		goto err_pinctrl_lookup;
+	}
+
+	ts->pins_suspend = pinctrl_lookup_state(ts->pinctrl, "pmx_ts_suspend");
+	if (IS_ERR_OR_NULL(ts->pins_suspend)) {
+		FTS_ERROR("Pin state[suspend] not found");
+		ret = PTR_ERR(ts->pins_suspend);
+		goto err_pinctrl_lookup;
+	}
+
+	ts->pins_release = pinctrl_lookup_state(ts->pinctrl, "pmx_ts_release");
+	if (IS_ERR_OR_NULL(ts->pins_release)) {
+		FTS_ERROR("Pin state[release] not found");
+		ret = PTR_ERR(ts->pins_release);
+	}
+
+	return 0;
+err_pinctrl_lookup:
+	if (ts->pinctrl) {
+		devm_pinctrl_put(ts->pinctrl);
+	}
+err_pinctrl_get:
+	ts->pinctrl = NULL;
+	ts->pins_release = NULL;
+	ts->pins_suspend = NULL;
+	ts->pins_active = NULL;
+	return ret;
+}
+
+static int fts_pinctrl_select_normal(struct fts_ts_data *ts)
+{
+	int ret = 0;
+
+	if (ts->pinctrl && ts->pins_active) {
+		ret = pinctrl_select_state(ts->pinctrl, ts->pins_active);
+		if (ret < 0) {
+			FTS_ERROR("Set normal pin state error:%d", ret);
+		}
+	}
+
+	return ret;
+}
+
+static int fts_pinctrl_select_suspend(struct fts_ts_data *ts)
+{
+	int ret = 0;
+
+	if (ts->pinctrl && ts->pins_suspend) {
+		ret = pinctrl_select_state(ts->pinctrl, ts->pins_suspend);
+		if (ret < 0) {
+			FTS_ERROR("Set suspend pin state error:%d", ret);
+		}
+	}
+
+	return ret;
+}
+
+static int fts_pinctrl_select_release(struct fts_ts_data *ts)
+{
+	int ret = 0;
+
+	if (ts->pinctrl) {
+		if (IS_ERR_OR_NULL(ts->pins_release)) {
+			devm_pinctrl_put(ts->pinctrl);
+			ts->pinctrl = NULL;
+		} else {
+			ret = pinctrl_select_state(ts->pinctrl, ts->pins_release);
+			if (ret < 0)
+				FTS_ERROR("Set gesture pin state error:%d", ret);
+		}
+	}
+
+	return ret;
+}
+#endif /* FTS_PINCTRL_EN */
+
+static int fts_power_configure(struct fts_ts_data *ts_data, bool enable)
+{
+	int ret = 0;
+
+	FTS_FUNC_ENTER();
+
+	if (enable) {
+		if (regulator_count_voltages(ts_data->vdd) > 0) {
+			ret = regulator_set_load(ts_data->vdd, FTS_LOAD_MAX_UA);
+			if (ret) {
+				FTS_ERROR("vdd regulator set_load failed ret=%d", ret);
+				return ret;
+			}
+
+			ret = regulator_set_voltage(ts_data->vdd, FTS_VTG_MIN_UV,
+						FTS_VTG_MAX_UV);
+			if (ret) {
+				FTS_ERROR("vdd regulator set_vtg failed ret=%d", ret);
+				goto err_vdd_load;
+			}
+		}
+
+		if (!IS_ERR_OR_NULL(ts_data->vcc_i2c)) {
+			if (regulator_count_voltages(ts_data->vcc_i2c) > 0) {
+				ret = regulator_set_load(ts_data->vcc_i2c, FTS_LOAD_AVDD_UA);
+				if (ret) {
+					FTS_ERROR("vcc_i2c regulator set_load failed ret=%d", ret);
+					goto err_vdd_load;
+				}
+
+				ret = regulator_set_voltage(ts_data->vcc_i2c,
+							FTS_I2C_VTG_MIN_UV,
+							FTS_I2C_VTG_MAX_UV);
+				if (ret) {
+					FTS_ERROR("vcc_i2c regulator set_vtg failed,ret=%d", ret);
+					goto err_vcc_load;
+				}
+			}
+		}
+	} else {
+		if (regulator_count_voltages(ts_data->vdd) > 0) {
+			ret = regulator_set_load(ts_data->vdd, FTS_LOAD_DISABLE_UA);
+			if (ret) {
+				FTS_ERROR("vdd regulator set_load failed ret=%d", ret);
+				return ret;
+			}
+		}
+
+		if (!IS_ERR_OR_NULL(ts_data->vcc_i2c)) {
+			if (regulator_count_voltages(ts_data->vcc_i2c) > 0) {
+				ret = regulator_set_load(ts_data->vcc_i2c, FTS_LOAD_DISABLE_UA);
+				if (ret) {
+					FTS_ERROR("vcc_i2c regulator set_load failed ret=%d", ret);
+					return ret;
+				}
+			}
+		}
+	}
+
+	FTS_FUNC_EXIT();
+	return ret;
+
+err_vcc_load:
+	regulator_set_load(ts_data->vcc_i2c, FTS_LOAD_DISABLE_UA);
+err_vdd_load:
+	regulator_set_load(ts_data->vdd, FTS_LOAD_DISABLE_UA);
+	return ret;
+}
+
+static int fts_ts_enable_reg(struct fts_ts_data *ts_data, bool enable)
+{
+	int ret = 0;
+
+	if (IS_ERR_OR_NULL(ts_data->vdd)) {
+		FTS_ERROR("vdd is invalid");
+		return -EINVAL;
+	}
+
+	if (enable) {
+		fts_power_configure(ts_data, true);
+		ret = regulator_enable(ts_data->vdd);
+		if (ret)
+			FTS_ERROR("enable vdd regulator failed,ret=%d", ret);
+
+		if (!IS_ERR_OR_NULL(ts_data->vcc_i2c)) {
+			ret = regulator_enable(ts_data->vcc_i2c);
+			if (ret)
+				FTS_ERROR("enable vcc_i2c regulator failed,ret=%d", ret);
+		}
+	} else {
+		ret = regulator_disable(ts_data->vdd);
+		if (ret)
+			FTS_ERROR("disable vdd regulator failed,ret=%d", ret);
+		if (!IS_ERR_OR_NULL(ts_data->vcc_i2c)) {
+			ret = regulator_disable(ts_data->vcc_i2c);
+			if (ret)
+				FTS_ERROR("disable vcc_i2c regulator failed,ret=%d", ret);
+		}
+		fts_power_configure(ts_data, false);
+	}
+
+	return ret;
+}
+
+static int fts_power_source_ctrl(struct fts_ts_data *ts_data, int enable)
+{
+	int ret = 0;
+
+	if (IS_ERR_OR_NULL(ts_data->vdd)) {
+		FTS_ERROR("vdd is invalid");
+		return -EINVAL;
+	}
+
+	FTS_FUNC_ENTER();
+	if (enable) {
+		if (ts_data->power_disabled) {
+			FTS_DEBUG("regulator enable !");
+			gpio_direction_output(ts_data->pdata->reset_gpio, 0);
+			msleep(1);
+			ret = fts_ts_enable_reg(ts_data, true);
+			if (ret)
+				FTS_ERROR("Touch reg enable failed\n");
+			ts_data->power_disabled = false;
+		}
+	} else {
+		if (!ts_data->power_disabled) {
+			FTS_DEBUG("regulator disable !");
+			gpio_direction_output(ts_data->pdata->reset_gpio, 0);
+			msleep(1);
+			ret = fts_ts_enable_reg(ts_data, false);
+			if (ret)
+				FTS_ERROR("Touch reg disable failed");
+			ts_data->power_disabled = true;
+		}
+	}
+
+	FTS_FUNC_EXIT();
+	return ret;
+}
+
+/*****************************************************************************
+* Name: fts_power_source_init
+* Brief: Init regulator power:vdd/vcc_io(if have), generally, no vcc_io
+*        vdd---->vdd-supply in dts, kernel will auto add "-supply" to parse
+*        Must be call after fts_gpio_configure() execute,because this function
+*        will operate reset-gpio which request gpio in fts_gpio_configure()
+* Input:
+* Output:
+* Return: return 0 if init power successfully, otherwise return error code
+*****************************************************************************/
+static int fts_power_source_init(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+
+	FTS_FUNC_ENTER();
+	ts_data->vdd = regulator_get(ts_data->dev, "vdd");
+	if (IS_ERR_OR_NULL(ts_data->vdd)) {
+		ret = PTR_ERR(ts_data->vdd);
+		FTS_ERROR("get vdd regulator failed,ret=%d", ret);
+		return ret;
+	}
+
+	ts_data->vcc_i2c = regulator_get(ts_data->dev, "vcc_i2c");
+	if (IS_ERR_OR_NULL(ts_data->vcc_i2c))
+		FTS_INFO("get vcc_i2c regulator failed");
+
+#if FTS_PINCTRL_EN
+	fts_pinctrl_init(ts_data);
+	fts_pinctrl_select_normal(ts_data);
+#endif
+
+	ts_data->power_disabled = true;
+	ret = fts_power_source_ctrl(ts_data, ENABLE);
+	if (ret) {
+		FTS_ERROR("fail to enable power(regulator)");
+	}
+
+	FTS_FUNC_EXIT();
+	return ret;
+}
+
+static int fts_power_source_exit(struct fts_ts_data *ts_data)
+{
+#if FTS_PINCTRL_EN
+	fts_pinctrl_select_release(ts_data);
+#endif
+
+	fts_power_source_ctrl(ts_data, DISABLE);
+
+	if (!IS_ERR_OR_NULL(ts_data->vdd)) {
+		if (regulator_count_voltages(ts_data->vdd) > 0)
+			regulator_set_voltage(ts_data->vdd, 0, FTS_VTG_MAX_UV);
+		regulator_put(ts_data->vdd);
+	}
+
+	if (!IS_ERR_OR_NULL(ts_data->vcc_i2c)) {
+		if (regulator_count_voltages(ts_data->vcc_i2c) > 0)
+			regulator_set_voltage(ts_data->vcc_i2c, 0, FTS_I2C_VTG_MAX_UV);
+		regulator_put(ts_data->vcc_i2c);
+	}
+
+	return 0;
+}
+
+static int fts_power_source_suspend(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+
+#if FTS_PINCTRL_EN
+	fts_pinctrl_select_suspend(ts_data);
+#endif
+
+	ret = fts_power_source_ctrl(ts_data, DISABLE);
+	if (ret < 0) {
+		FTS_ERROR("power off fail, ret=%d", ret);
+	}
+
+	return ret;
+}
+
+static int fts_power_source_resume(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+
+#if FTS_PINCTRL_EN
+	fts_pinctrl_select_normal(ts_data);
+#endif
+
+	ret = fts_power_source_ctrl(ts_data, ENABLE);
+	if (ret < 0) {
+		FTS_ERROR("power on fail, ret=%d", ret);
+	}
+
+	return ret;
+}
+#endif /* FTS_POWER_SOURCE_CUST_EN */
+
+static int fts_gpio_configure(struct fts_ts_data *data)
+{
+	int ret = 0;
+
+	FTS_FUNC_ENTER();
+	/* request irq gpio */
+	if (gpio_is_valid(data->pdata->irq_gpio)) {
+		ret = gpio_request(data->pdata->irq_gpio, "fts_irq_gpio");
+		if (ret) {
+			FTS_ERROR("[GPIO]irq gpio request failed");
+			goto err_irq_gpio_req;
+		}
+
+		ret = gpio_direction_input(data->pdata->irq_gpio);
+		if (ret) {
+			FTS_ERROR("[GPIO]set_direction for irq gpio failed");
+			goto err_irq_gpio_dir;
+		}
+	}
+
+	/* request reset gpio */
+	if (gpio_is_valid(data->pdata->reset_gpio)) {
+		ret = gpio_request(data->pdata->reset_gpio, "fts_reset_gpio");
+		if (ret) {
+			FTS_ERROR("[GPIO]reset gpio request failed");
+			goto err_irq_gpio_dir;
+		}
+
+		ret = gpio_direction_output(data->pdata->reset_gpio, 1);
+		if (ret) {
+			FTS_ERROR("[GPIO]set_direction for reset gpio failed");
+			goto err_reset_gpio_dir;
+		}
+	}
+
+	FTS_FUNC_EXIT();
+	return 0;
+
+err_reset_gpio_dir:
+	if (gpio_is_valid(data->pdata->reset_gpio))
+		gpio_free(data->pdata->reset_gpio);
+err_irq_gpio_dir:
+	if (gpio_is_valid(data->pdata->irq_gpio))
+		gpio_free(data->pdata->irq_gpio);
+err_irq_gpio_req:
+	FTS_FUNC_EXIT();
+	return ret;
+}
+
+static int fts_get_dt_coords(struct device *dev, char *name,
+				struct fts_ts_platform_data *pdata)
+{
+	int ret = 0;
+	u32 coords[FTS_COORDS_ARR_SIZE] = { 0 };
+	struct property *prop;
+	struct device_node *np = dev->of_node;
+	int coords_size;
+
+	prop = of_find_property(np, name, NULL);
+	if (!prop)
+		return -EINVAL;
+	if (!prop->value)
+		return -ENODATA;
+
+	coords_size = prop->length / sizeof(u32);
+	if (coords_size != FTS_COORDS_ARR_SIZE) {
+		FTS_ERROR("invalid:%s, size:%d", name, coords_size);
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32_array(np, name, coords, coords_size);
+	if (ret < 0) {
+		FTS_ERROR("Unable to read %s, please check dts", name);
+		pdata->x_min = FTS_X_MIN_DISPLAY_DEFAULT;
+		pdata->y_min = FTS_Y_MIN_DISPLAY_DEFAULT;
+		pdata->x_max = FTS_X_MAX_DISPLAY_DEFAULT;
+		pdata->y_max = FTS_Y_MAX_DISPLAY_DEFAULT;
+		return -ENODATA;
+	} else {
+		pdata->x_min = coords[0];
+		pdata->y_min = coords[1];
+		pdata->x_max = coords[2];
+		pdata->y_max = coords[3];
+	}
+
+	FTS_INFO("display x(%d %d) y(%d %d)", pdata->x_min, pdata->x_max,
+		pdata->y_min, pdata->y_max);
+	return 0;
+}
+
+static int fts_parse_dt(struct device *dev, struct fts_ts_platform_data *pdata)
+{
+	int ret = 0;
+	struct device_node *np = dev->of_node;
+	u32 temp_val = 0;
+
+	FTS_FUNC_ENTER();
+
+	ret = fts_get_dt_coords(dev, "focaltech,display-coords", pdata);
+	if (ret < 0)
+		FTS_ERROR("Unable to get display-coords");
+
+	/* key */
+	pdata->have_key = of_property_read_bool(np, "focaltech,have-key");
+	if (pdata->have_key) {
+		ret = of_property_read_u32(np, "focaltech,key-number", &pdata->key_number);
+		if (ret < 0)
+			FTS_ERROR("Key number undefined!");
+
+		ret = of_property_read_u32_array(np, "focaltech,keys",
+						pdata->keys, pdata->key_number);
+		if (ret < 0)
+			FTS_ERROR("Keys undefined!");
+		else if (pdata->key_number > FTS_MAX_KEYS)
+			pdata->key_number = FTS_MAX_KEYS;
+
+		ret = of_property_read_u32_array(np, "focaltech,key-x-coords",
+						pdata->key_x_coords,
+						pdata->key_number);
+		if (ret < 0)
+			FTS_ERROR("Key Y Coords undefined!");
+
+		ret = of_property_read_u32_array(np, "focaltech,key-y-coords",
+						pdata->key_y_coords,
+						pdata->key_number);
+		if (ret < 0)
+			FTS_ERROR("Key X Coords undefined!");
+
+		FTS_INFO("VK Number:%d, key:(%d,%d,%d), "
+			"coords:(%d,%d),(%d,%d),(%d,%d)",
+			pdata->key_number,
+			pdata->keys[0], pdata->keys[1], pdata->keys[2],
+			pdata->key_x_coords[0], pdata->key_y_coords[0],
+			pdata->key_x_coords[1], pdata->key_y_coords[1],
+			pdata->key_x_coords[2], pdata->key_y_coords[2]);
+	}
+
+	/* reset, irq gpio info */
+	pdata->reset_gpio = of_get_named_gpio_flags(np, "focaltech,reset-gpio",
+			0, &pdata->reset_gpio_flags);
+	if (pdata->reset_gpio < 0)
+		FTS_ERROR("Unable to get reset_gpio");
+
+	pdata->irq_gpio = of_get_named_gpio_flags(np, "focaltech,irq-gpio",
+			0, &pdata->irq_gpio_flags);
+	if (pdata->irq_gpio < 0)
+		FTS_ERROR("Unable to get irq_gpio");
+
+	ret = of_property_read_u32(np, "focaltech,max-touch-number", &temp_val);
+	if (ret < 0) {
+		FTS_ERROR("Unable to get max-touch-number, please check dts");
+		pdata->max_touch_number = FTS_MAX_POINTS_SUPPORT;
+	} else {
+		if (temp_val < 2)
+			pdata->max_touch_number = 2; /* max_touch_number must >= 2 */
+		else if (temp_val > FTS_MAX_POINTS_SUPPORT)
+			pdata->max_touch_number = FTS_MAX_POINTS_SUPPORT;
+		else
+			pdata->max_touch_number = temp_val;
+	}
+
+	FTS_INFO("max touch number:%d, irq gpio:%d, reset gpio:%d",
+		pdata->max_touch_number, pdata->irq_gpio, pdata->reset_gpio);
+
+	ret = of_property_read_u32(np, "focaltech,ic-type", &temp_val);
+	if (ret < 0)
+		pdata->type = _FT3518;
+	else
+		pdata->type = temp_val;
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+#if defined(CONFIG_DRM)
+static void fts_resume_work(struct work_struct *work)
+{
+	struct fts_ts_data *ts_data = container_of(work, struct fts_ts_data,
+					resume_work);
+
+	fts_ts_resume(ts_data->dev);
+}
+
+static void fts_ts_panel_notifier_callback(enum panel_event_notifier_tag tag,
+		 struct panel_event_notification *notification, void *client_data)
+{
+	struct fts_ts_data *ts_data = client_data;
+
+	if (!notification) {
+		pr_err("Invalid notification\n");
+		return;
+	}
+
+	FTS_DEBUG("Notification type:%d, early_trigger:%d",
+			notification->notif_type,
+			notification->notif_data.early_trigger);
+	switch (notification->notif_type) {
+	case DRM_PANEL_EVENT_UNBLANK:
+		if (notification->notif_data.early_trigger)
+			FTS_DEBUG("resume notification pre commit\n");
+		else
+			queue_work(fts_data->ts_workqueue, &fts_data->resume_work);
+		break;
+	case DRM_PANEL_EVENT_BLANK:
+		if (notification->notif_data.early_trigger) {
+			cancel_work_sync(&fts_data->resume_work);
+			fts_ts_suspend(ts_data->dev);
+		} else {
+			FTS_DEBUG("suspend notification post commit\n");
+		}
+		break;
+	case DRM_PANEL_EVENT_BLANK_LP:
+		FTS_DEBUG("received lp event\n");
+		break;
+	case DRM_PANEL_EVENT_FPS_CHANGE:
+		FTS_DEBUG("shashank:Received fps change old fps:%d new fps:%d\n",
+				notification->notif_data.old_fps,
+				notification->notif_data.new_fps);
+		break;
+	default:
+		FTS_DEBUG("notification serviced :%d\n",
+				notification->notif_type);
+		break;
+	}
+}
+
+#elif defined(CONFIG_FB)
+static void fts_resume_work(struct work_struct *work)
+{
+	struct fts_ts_data *ts_data = container_of(work, struct fts_ts_data,
+					resume_work);
+
+	fts_ts_resume(ts_data->dev);
+}
+
+static int fb_notifier_callback(struct notifier_block *self,
+				unsigned long event, void *data)
+{
+	struct fb_event *evdata = data;
+	int *blank = NULL;
+	struct fts_ts_data *ts_data = container_of(self, struct fts_ts_data,
+					fb_notif);
+
+	if (!(event == FB_EARLY_EVENT_BLANK || event == FB_EVENT_BLANK)) {
+		FTS_INFO("event(%lu) do not need process\n", event);
+		return 0;
+	}
+
+	blank = evdata->data;
+	FTS_INFO("FB event:%lu,blank:%d", event, *blank);
+	switch (*blank) {
+	case FB_BLANK_UNBLANK:
+		if (FB_EARLY_EVENT_BLANK == event) {
+			FTS_INFO("resume: event = %lu, not care\n", event);
+		} else if (FB_EVENT_BLANK == event) {
+			queue_work(fts_data->ts_workqueue, &fts_data->resume_work);
+		}
+		break;
+
+	case FB_BLANK_POWERDOWN:
+		if (FB_EARLY_EVENT_BLANK == event) {
+			cancel_work_sync(&fts_data->resume_work);
+			fts_ts_suspend(ts_data->dev);
+		} else if (FB_EVENT_BLANK == event) {
+			FTS_INFO("suspend: event = %lu, not care\n", event);
+		}
+		break;
+
+	default:
+		FTS_INFO("FB BLANK(%d) do not need process\n", *blank);
+		break;
+	}
+
+	return 0;
+}
+#elif defined(CONFIG_HAS_EARLYSUSPEND)
+static void fts_ts_early_suspend(struct early_suspend *handler)
+{
+	struct fts_ts_data *ts_data = container_of(handler, struct fts_ts_data,
+					early_suspend);
+
+	fts_ts_suspend(ts_data->dev);
+}
+
+static void fts_ts_late_resume(struct early_suspend *handler)
+{
+	struct fts_ts_data *ts_data = container_of(handler, struct fts_ts_data,
+					early_suspend);
+
+	fts_ts_resume(ts_data->dev);
+}
+#endif
+
+static int fts_ts_probe_delayed(struct fts_ts_data *fts_data)
+{
+	int ret = 0;
+
+/* Avoid setting up hardware for TVM during probe */
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+#ifdef CONFIG_ARCH_QTI_VM
+	if (!atomic_read(&fts_data->delayed_vm_probe_pending)) {
+		atomic_set(&fts_data->delayed_vm_probe_pending, 1);
+		return 0;
+	}
+	goto tvm_setup;
+#endif
+#endif
+	ret = fts_gpio_configure(fts_data);
+	if (ret) {
+		FTS_ERROR("configure the gpios fail");
+		goto err_gpio_config;
+	}
+
+#if FTS_POWER_SOURCE_CUST_EN
+	ret = fts_power_source_init(fts_data);
+	if (ret) {
+		FTS_ERROR("fail to get power(regulator)");
+		goto err_power_init;
+	}
+#endif
+
+	if (!FTS_CHIP_IDC(fts_data->pdata->type))
+		fts_reset_proc(200);
+
+	ret = fts_get_ic_information(fts_data);
+	if (ret) {
+		FTS_ERROR("not focal IC, unregister driver");
+		goto err_irq_req;
+	}
+
+#ifdef CONFIG_ARCH_QTI_VM
+tvm_setup:
+#endif
+	ret = fts_irq_registration(fts_data);
+	if (ret) {
+		FTS_ERROR("request irq failed");
+#ifdef CONFIG_ARCH_QTI_VM
+		return ret;
+#endif
+		goto err_irq_req;
+	}
+
+#ifdef CONFIG_ARCH_QTI_VM
+	return ret;
+#endif
+
+	ret = fts_fwupg_init(fts_data);
+	if (ret)
+		FTS_ERROR("init fw upgrade fail");
+
+	return 0;
+
+err_irq_req:
+	if (gpio_is_valid(fts_data->pdata->reset_gpio))
+		gpio_free(fts_data->pdata->reset_gpio);
+	if (gpio_is_valid(fts_data->pdata->irq_gpio))
+		gpio_free(fts_data->pdata->irq_gpio);
+#if FTS_POWER_SOURCE_CUST_EN
+err_power_init:
+	fts_power_source_exit(fts_data);
+#endif
+err_gpio_config:
+	return ret;
+}
+
+static int fts_ts_probe_entry(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+	int pdata_size = sizeof(struct fts_ts_platform_data);
+
+	FTS_FUNC_ENTER();
+	FTS_INFO("%s", FTS_DRIVER_VERSION);
+	ts_data->pdata = kzalloc(pdata_size, GFP_KERNEL);
+	if (!ts_data->pdata) {
+		FTS_ERROR("allocate memory for platform_data fail");
+		return -ENOMEM;
+	}
+
+	if (ts_data->dev->of_node) {
+		ret = fts_parse_dt(ts_data->dev, ts_data->pdata);
+		if (ret)
+			FTS_ERROR("device-tree parse fail");
+	} else {
+		if (ts_data->dev->platform_data) {
+			memcpy(ts_data->pdata, ts_data->dev->platform_data, pdata_size);
+		} else {
+			FTS_ERROR("platform_data is null");
+			return -ENODEV;
+		}
+	}
+
+	ts_data->ts_workqueue = create_singlethread_workqueue("fts_wq");
+	if (!ts_data->ts_workqueue) {
+		FTS_ERROR("create fts workqueue fail");
+	}
+
+	spin_lock_init(&ts_data->irq_lock);
+	mutex_init(&ts_data->report_mutex);
+	mutex_init(&ts_data->bus_lock);
+	mutex_init(&ts_data->transition_lock);
+
+	/* Init communication interface */
+	ret = fts_bus_init(ts_data);
+	if (ret) {
+		FTS_ERROR("bus initialize fail");
+		goto err_bus_init;
+	}
+
+	ret = fts_input_init(ts_data);
+	if (ret) {
+		FTS_ERROR("input initialize fail");
+		goto err_input_init;
+	}
+
+	ret = fts_report_buffer_init(ts_data);
+	if (ret) {
+		FTS_ERROR("report buffer init fail");
+		goto err_report_buffer;
+	}
+
+	ret = fts_create_apk_debug_channel(ts_data);
+	if (ret) {
+		FTS_ERROR("create apk debug node fail");
+	}
+
+	ret = fts_create_sysfs(ts_data);
+	if (ret) {
+		FTS_ERROR("create sysfs node fail");
+	}
+
+#if FTS_POINT_REPORT_CHECK_EN
+	ret = fts_point_report_check_init(ts_data);
+	if (ret) {
+		FTS_ERROR("init point report check fail");
+	}
+#endif
+
+	ret = fts_ex_mode_init(ts_data);
+	if (ret) {
+		FTS_ERROR("init glove/cover/charger fail");
+	}
+
+	ret = fts_gesture_init(ts_data);
+	if (ret) {
+		FTS_ERROR("init gesture fail");
+	}
+
+
+#if FTS_ESDCHECK_EN
+	ret = fts_esdcheck_init(ts_data);
+	if (ret) {
+		FTS_ERROR("init esd check fail");
+	}
+#endif
+
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+	fts_ts_trusted_touch_init(ts_data);
+	mutex_init(&(ts_data->fts_clk_io_ctrl_mutex));
+#endif
+	ret = fts_ts_probe_delayed(ts_data);
+	if (ret) {
+		FTS_ERROR("Failed to enable resources\n");
+		goto err_probe_delayed;
+	}
+
+#if defined(CONFIG_DRM)
+	if (ts_data->ts_workqueue)
+		INIT_WORK(&ts_data->resume_work, fts_resume_work);
+
+	if (!strcmp(fts_data->touch_environment, "pvm"))
+		fts_ts_register_for_panel_events(ts_data->dev->of_node, ts_data);
+#elif defined(CONFIG_FB)
+	if (ts_data->ts_workqueue) {
+		INIT_WORK(&ts_data->resume_work, fts_resume_work);
+	}
+	ts_data->fb_notif.notifier_call = fb_notifier_callback;
+	ret = fb_register_client(&ts_data->fb_notif);
+	if (ret) {
+		FTS_ERROR("[FB]Unable to register fb_notifier: %d", ret);
+	}
+#elif defined(CONFIG_HAS_EARLYSUSPEND)
+	ts_data->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + FTS_SUSPEND_LEVEL;
+	ts_data->early_suspend.suspend = fts_ts_early_suspend;
+	ts_data->early_suspend.resume = fts_ts_late_resume;
+	register_early_suspend(&ts_data->early_suspend);
+#endif
+
+	FTS_FUNC_EXIT();
+	return 0;
+
+err_probe_delayed:
+	kfree_safe(ts_data->point_buf);
+	kfree_safe(ts_data->events);
+err_report_buffer:
+	input_unregister_device(ts_data->input_dev);
+err_input_init:
+	if (ts_data->ts_workqueue)
+		destroy_workqueue(ts_data->ts_workqueue);
+err_bus_init:
+	kfree_safe(ts_data->bus_tx_buf);
+	kfree_safe(ts_data->bus_rx_buf);
+	kfree_safe(ts_data->pdata);
+
+	FTS_FUNC_EXIT();
+	return ret;
+}
+
+static int fts_ts_remove_entry(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+
+#if FTS_POINT_REPORT_CHECK_EN
+	fts_point_report_check_exit(ts_data);
+#endif
+
+	fts_release_apk_debug_channel(ts_data);
+	fts_remove_sysfs(ts_data);
+	fts_ex_mode_exit(ts_data);
+
+	fts_fwupg_exit(ts_data);
+
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_exit(ts_data);
+#endif
+
+	fts_gesture_exit(ts_data);
+	fts_bus_exit(ts_data);
+
+	free_irq(ts_data->irq, ts_data);
+	input_unregister_device(ts_data->input_dev);
+
+	if (ts_data->ts_workqueue)
+		destroy_workqueue(ts_data->ts_workqueue);
+
+#if defined(CONFIG_DRM)
+	if (active_panel && ts_data->notifier_cookie)
+		panel_event_notifier_unregister(ts_data->notifier_cookie);
+
+#elif defined(CONFIG_FB)
+	if (fb_unregister_client(&ts_data->fb_notif))
+		FTS_ERROR("Error occurred while unregistering fb_notifier.");
+#elif defined(CONFIG_HAS_EARLYSUSPEND)
+	unregister_early_suspend(&ts_data->early_suspend);
+#endif
+
+	if (gpio_is_valid(ts_data->pdata->reset_gpio))
+		gpio_free(ts_data->pdata->reset_gpio);
+
+	if (gpio_is_valid(ts_data->pdata->irq_gpio))
+		gpio_free(ts_data->pdata->irq_gpio);
+
+#if FTS_POWER_SOURCE_CUST_EN
+	fts_power_source_exit(ts_data);
+#endif
+
+	kfree_safe(ts_data->point_buf);
+	kfree_safe(ts_data->events);
+
+	kfree_safe(ts_data->pdata);
+	kfree_safe(ts_data);
+
+	FTS_FUNC_EXIT();
+
+	return 0;
+}
+
+static int fts_ts_suspend(struct device *dev)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = fts_data;
+
+	FTS_FUNC_ENTER();
+	if (ts_data->suspended) {
+		FTS_INFO("Already in suspend state");
+		return 0;
+	}
+
+	if (ts_data->fw_loading) {
+		FTS_INFO("fw upgrade in process, can't suspend");
+		return 0;
+	}
+
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+	if (atomic_read(&fts_data->trusted_touch_transition)
+			|| atomic_read(&fts_data->trusted_touch_enabled))
+		wait_for_completion_interruptible(
+			&fts_data->trusted_touch_powerdown);
+#endif
+
+	mutex_lock(&ts_data->transition_lock);
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_suspend();
+#endif
+
+	if (ts_data->gesture_mode) {
+		fts_gesture_suspend(ts_data);
+	} else {
+		fts_irq_disable();
+
+		FTS_INFO("make TP enter into sleep mode");
+		ret = fts_write_reg(FTS_REG_POWER_MODE, FTS_REG_POWER_MODE_SLEEP);
+		if (ret < 0)
+			FTS_ERROR("set TP to sleep mode fail, ret=%d", ret);
+
+		if (!ts_data->ic_info.is_incell) {
+#if FTS_POWER_SOURCE_CUST_EN
+			ret = fts_power_source_suspend(ts_data);
+			if (ret < 0) {
+				FTS_ERROR("power enter suspend fail");
+			}
+#endif
+		}
+	}
+
+	fts_release_all_finger();
+	ts_data->suspended = true;
+	mutex_unlock(&ts_data->transition_lock);
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+static int fts_ts_resume(struct device *dev)
+{
+	struct fts_ts_data *ts_data = fts_data;
+
+	FTS_FUNC_ENTER();
+	if (!ts_data->suspended) {
+		FTS_DEBUG("Already in awake state");
+		return 0;
+	}
+
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+
+	if (atomic_read(&ts_data->trusted_touch_transition))
+		wait_for_completion_interruptible(
+			&ts_data->trusted_touch_powerdown);
+#endif
+
+	mutex_lock(&ts_data->transition_lock);
+
+	fts_release_all_finger();
+
+	if (!ts_data->ic_info.is_incell) {
+#if FTS_POWER_SOURCE_CUST_EN
+		fts_power_source_resume(ts_data);
+#endif
+		fts_reset_proc(200);
+	}
+
+	fts_wait_tp_to_valid();
+	fts_ex_mode_recovery(ts_data);
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_resume();
+#endif
+
+	if (ts_data->gesture_mode) {
+		fts_gesture_resume(ts_data);
+	} else {
+		fts_irq_enable();
+	}
+
+	ts_data->suspended = false;
+	mutex_unlock(&ts_data->transition_lock);
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+/*****************************************************************************
+* TP Driver
+*****************************************************************************/
+static int fts_ts_check_dt(struct device_node *np)
+{
+	int i;
+	int count;
+	struct device_node *node;
+	struct drm_panel *panel;
+
+	count = of_count_phandle_with_args(np, "panel", NULL);
+	if (count <= 0)
+		return 0;
+
+	for (i = 0; i < count; i++) {
+		node = of_parse_phandle(np, "panel", i);
+		panel = of_drm_find_panel(node);
+		of_node_put(node);
+		if (!IS_ERR(panel)) {
+			active_panel = panel;
+			return 0;
+		}
+	}
+
+	return PTR_ERR(panel);
+}
+
+static int fts_ts_check_default_tp(struct device_node *dt, const char *prop)
+{
+	const char **active_tp = NULL;
+	int count, tmp, score = 0;
+	const char *active;
+	int ret, i;
+
+	count = of_property_count_strings(dt->parent, prop);
+	if (count <= 0 || count > 3)
+		return -ENODEV;
+
+	active_tp = kcalloc(count, sizeof(char *),  GFP_KERNEL);
+	if (!active_tp) {
+		FTS_ERROR("FTS alloc failed\n");
+		return -ENOMEM;
+	}
+
+	ret = of_property_read_string_array(dt->parent, prop,
+			active_tp, count);
+	if (ret < 0) {
+		FTS_ERROR("fail to read %s %d\n", prop, ret);
+		ret = -ENODEV;
+		goto out;
+	}
+
+	for (i = 0; i < count; i++) {
+		active = active_tp[i];
+		if (active != NULL) {
+			tmp = of_device_is_compatible(dt, active);
+			if (tmp > 0)
+				score++;
+		}
+	}
+
+	if (score <= 0) {
+		FTS_INFO("not match this driver\n");
+		ret = -ENODEV;
+		goto out;
+	}
+	ret = 0;
+out:
+	kfree(active_tp);
+	return ret;
+}
+
+static int fts_ts_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = NULL;
+	struct device_node *dp = client->dev.of_node;
+
+	FTS_INFO("Touch Screen(I2C BUS) driver prboe...");
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		FTS_ERROR("I2C not supported");
+		return -ENODEV;
+	}
+
+	ret = fts_ts_check_dt(dp);
+	if (ret == -EPROBE_DEFER)
+		return ret;
+
+	if (ret) {
+		if (!fts_ts_check_default_tp(dp, "qcom,i2c-touch-active"))
+			ret = -EPROBE_DEFER;
+		else
+			ret = -ENODEV;
+
+		return ret;
+	}
+
+	/* malloc memory for global struct variable */
+	ts_data = (struct fts_ts_data *)kzalloc(sizeof(*ts_data), GFP_KERNEL);
+	if (!ts_data) {
+		FTS_ERROR("allocate memory for fts_data fail");
+		return -ENOMEM;
+	}
+
+	fts_data = ts_data;
+	ts_data->client = client;
+	ts_data->dev = &client->dev;
+	ts_data->log_level = 1;
+	ts_data->fw_is_running = 0;
+	ts_data->bus_type = BUS_TYPE_I2C;
+	i2c_set_clientdata(client, ts_data);
+
+	ret = fts_ts_probe_entry(ts_data);
+	if (ret) {
+		FTS_ERROR("Touch Screen(I2C BUS) driver probe fail");
+		kfree_safe(ts_data);
+		return ret;
+	}
+
+	FTS_INFO("Touch Screen(I2C BUS) driver prboe successfully");
+	return 0;
+}
+
+static int fts_ts_i2c_remove(struct i2c_client *client)
+{
+	return fts_ts_remove_entry(i2c_get_clientdata(client));
+}
+
+static const struct i2c_device_id fts_ts_i2c_id[] = {
+	{FTS_DRIVER_NAME, 0},
+	{},
+};
+static const struct of_device_id fts_dt_match[] = {
+	{.compatible = "focaltech,fts_ts", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, fts_dt_match);
+
+static struct i2c_driver fts_ts_i2c_driver = {
+	.probe = fts_ts_i2c_probe,
+	.remove = fts_ts_i2c_remove,
+	.driver = {
+		.name = FTS_DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(fts_dt_match),
+	},
+	.id_table = fts_ts_i2c_id,
+};
+
+static int __init fts_ts_i2c_init(void)
+{
+	int ret = 0;
+
+	FTS_FUNC_ENTER();
+	ret = i2c_add_driver(&fts_ts_i2c_driver);
+	if (ret != 0)
+		FTS_ERROR("Focaltech touch screen driver init failed!");
+
+	FTS_FUNC_EXIT();
+	return ret;
+}
+
+static void __exit fts_ts_i2c_exit(void)
+{
+	i2c_del_driver(&fts_ts_i2c_driver);
+}
+
+static int fts_ts_spi_probe(struct spi_device *spi)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = NULL;
+	struct device_node *dp = spi->dev.of_node;
+
+	FTS_INFO("Touch Screen(SPI BUS) driver prboe...");
+
+	ret = fts_ts_check_dt(dp);
+	if (ret == -EPROBE_DEFER)
+		return ret;
+
+	if (ret) {
+		if (!fts_ts_check_default_tp(dp, "qcom,spi-touch-active"))
+			ret = -EPROBE_DEFER;
+		else
+			ret = -ENODEV;
+
+		return ret;
+	}
+
+	spi->mode = SPI_MODE_0;
+	spi->bits_per_word = 8;
+	ret = spi_setup(spi);
+	if (ret) {
+		FTS_ERROR("spi setup fail");
+		return ret;
+	}
+
+	/* malloc memory for global struct variable */
+	ts_data = kzalloc(sizeof(*ts_data), GFP_KERNEL);
+	if (!ts_data) {
+		FTS_ERROR("allocate memory for fts_data fail");
+		return -ENOMEM;
+	}
+
+	fts_data = ts_data;
+	ts_data->spi = spi;
+	ts_data->dev = &spi->dev;
+	ts_data->log_level = 1;
+
+	ts_data->bus_type = BUS_TYPE_SPI_V2;
+	spi_set_drvdata(spi, ts_data);
+
+	ret = fts_ts_probe_entry(ts_data);
+	if (ret) {
+		FTS_ERROR("Touch Screen(SPI BUS) driver probe fail");
+		kfree_safe(ts_data);
+		return ret;
+	}
+
+	FTS_INFO("Touch Screen(SPI BUS) driver prboe successfully");
+	return 0;
+}
+
+static int fts_ts_spi_remove(struct spi_device *spi)
+{
+	return fts_ts_remove_entry(spi_get_drvdata(spi));
+}
+
+static const struct spi_device_id fts_ts_spi_id[] = {
+	{FTS_DRIVER_NAME, 0},
+	{},
+};
+
+static struct spi_driver fts_ts_spi_driver = {
+	.probe = fts_ts_spi_probe,
+	.remove = fts_ts_spi_remove,
+	.driver = {
+		.name = FTS_DRIVER_NAME,
+		.owner = THIS_MODULE,
+#if defined(CONFIG_PM) && FTS_PATCH_COMERR_PM
+		.pm = &fts_dev_pm_ops,
+#endif
+		.of_match_table = of_match_ptr(fts_dt_match),
+	},
+	.id_table = fts_ts_spi_id,
+};
+
+static int __init fts_ts_spi_init(void)
+{
+	int ret = 0;
+
+	FTS_FUNC_ENTER();
+	ret = spi_register_driver(&fts_ts_spi_driver);
+	if (ret != 0)
+		FTS_ERROR("Focaltech touch screen driver init failed!");
+
+	FTS_FUNC_EXIT();
+	return ret;
+}
+
+static void __exit fts_ts_spi_exit(void)
+{
+	spi_unregister_driver(&fts_ts_spi_driver);
+}
+
+static int __init fts_ts_init(void)
+{
+	int ret = 0;
+
+	ret = fts_ts_i2c_init();
+	if (ret)
+		FTS_ERROR("Focaltech I2C driver init failed!");
+
+	ret = fts_ts_spi_init();
+	if (ret)
+		FTS_ERROR("Focaltech SPI driver init failed!");
+
+	return ret;
+}
+
+static void __exit fts_ts_exit(void)
+{
+	fts_ts_i2c_exit();
+	fts_ts_spi_exit();
+}
+
+#ifdef CONFIG_ARCH_QTI_VM
+module_init(fts_ts_init);
+#else
+late_initcall(fts_ts_init);
+#endif
+module_exit(fts_ts_exit);
+
+MODULE_AUTHOR("FocalTech Driver Team");
+MODULE_DESCRIPTION("FocalTech Touchscreen Driver");
+MODULE_LICENSE("GPL v2");

focaltech_touch/focaltech_core.h

@@ -0,0 +1,389 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, Focaltech Ltd. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+/*****************************************************************************
+*
+* File Name: focaltech_core.h
+
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-08
+*
+* Abstract:
+*
+* Reference:
+*
+*****************************************************************************/
+
+#ifndef __LINUX_FOCALTECH_CORE_H__
+#define __LINUX_FOCALTECH_CORE_H__
+/*****************************************************************************
+* Included header files
+*****************************************************************************/
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/spi/spi.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <asm/uaccess.h>
+#include <linux/firmware.h>
+#include <linux/debugfs.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/jiffies.h>
+#include <linux/fs.h>
+#include <linux/proc_fs.h>
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/sched.h>
+#include <linux/kthread.h>
+#include <linux/dma-mapping.h>
+#include <linux/gunyah/gh_irq_lend.h>
+#include <linux/gunyah/gh_mem_notifier.h>
+#include "focaltech_common.h"
+
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define FTS_MAX_POINTS_SUPPORT              10 /* constant value, can't be changed */
+#define FTS_MAX_KEYS                        4
+#define FTS_KEY_DIM                         10
+#define FTS_ONE_TCH_LEN                     6
+#define FTS_TOUCH_DATA_LEN  (FTS_MAX_POINTS_SUPPORT * FTS_ONE_TCH_LEN + 3)
+
+#define FTS_GESTURE_POINTS_MAX              6
+#define FTS_GESTURE_DATA_LEN               (FTS_GESTURE_POINTS_MAX * 4 + 4)
+
+#define FTS_MAX_ID                          0x0A
+#define FTS_TOUCH_X_H_POS                   3
+#define FTS_TOUCH_X_L_POS                   4
+#define FTS_TOUCH_Y_H_POS                   5
+#define FTS_TOUCH_Y_L_POS                   6
+#define FTS_TOUCH_PRE_POS                   7
+#define FTS_TOUCH_AREA_POS                  8
+#define FTS_TOUCH_POINT_NUM                 2
+#define FTS_TOUCH_EVENT_POS                 3
+#define FTS_TOUCH_ID_POS                    5
+#define FTS_COORDS_ARR_SIZE                 4
+#define FTS_X_MIN_DISPLAY_DEFAULT           0
+#define FTS_Y_MIN_DISPLAY_DEFAULT           0
+#define FTS_X_MAX_DISPLAY_DEFAULT           720
+#define FTS_Y_MAX_DISPLAY_DEFAULT           1280
+
+#define FTS_TOUCH_DOWN                      0
+#define FTS_TOUCH_UP                        1
+#define FTS_TOUCH_CONTACT                   2
+#define EVENT_DOWN(flag)                    ((FTS_TOUCH_DOWN == flag) || (FTS_TOUCH_CONTACT == flag))
+#define EVENT_UP(flag)                      (FTS_TOUCH_UP == flag)
+#define EVENT_NO_DOWN(data)                 (!data->point_num)
+
+#define FTX_MAX_COMPATIBLE_TYPE             4
+#define FTX_MAX_COMMMAND_LENGTH             16
+
+
+/*****************************************************************************
+*  Alternative mode (When something goes wrong, the modules may be able to solve the problem.)
+*****************************************************************************/
+/*
+ * For commnication error in PM(deep sleep) state
+ */
+#define FTS_PATCH_COMERR_PM                     0
+#define FTS_TIMEOUT_COMERR_PM                   700
+
+
+/*****************************************************************************
+* Private enumerations, structures and unions using typedef
+*****************************************************************************/
+struct ftxxxx_proc {
+	struct proc_dir_entry *proc_entry;
+	u8 opmode;
+	u8 cmd_len;
+	u8 cmd[FTX_MAX_COMMMAND_LENGTH];
+};
+
+struct fts_ts_platform_data {
+	u32 type;
+	u32 irq_gpio;
+	u32 irq_gpio_flags;
+	u32 reset_gpio;
+	u32 reset_gpio_flags;
+	bool have_key;
+	u32 key_number;
+	u32 keys[FTS_MAX_KEYS];
+	u32 key_y_coords[FTS_MAX_KEYS];
+	u32 key_x_coords[FTS_MAX_KEYS];
+	u32 x_max;
+	u32 y_max;
+	u32 x_min;
+	u32 y_min;
+	u32 max_touch_number;
+};
+
+struct ts_event {
+	int x;      /*x coordinate */
+	int y;      /*y coordinate */
+	int p;      /* pressure */
+	int flag;   /* touch event flag: 0 -- down; 1-- up; 2 -- contact */
+	int id;     /*touch ID */
+	int area;
+};
+
+enum trusted_touch_mode_config {
+	TRUSTED_TOUCH_VM_MODE,
+	TRUSTED_TOUCH_MODE_NONE
+};
+
+enum trusted_touch_pvm_states {
+	TRUSTED_TOUCH_PVM_INIT,
+	PVM_I2C_RESOURCE_ACQUIRED,
+	PVM_INTERRUPT_DISABLED,
+	PVM_IOMEM_LENT,
+	PVM_IOMEM_LENT_NOTIFIED,
+	PVM_IRQ_LENT,
+	PVM_IRQ_LENT_NOTIFIED,
+	PVM_IOMEM_RELEASE_NOTIFIED,
+	PVM_IRQ_RELEASE_NOTIFIED,
+	PVM_ALL_RESOURCES_RELEASE_NOTIFIED,
+	PVM_IRQ_RECLAIMED,
+	PVM_IOMEM_RECLAIMED,
+	PVM_INTERRUPT_ENABLED,
+	PVM_I2C_RESOURCE_RELEASED,
+	TRUSTED_TOUCH_PVM_STATE_MAX
+};
+
+enum trusted_touch_tvm_states {
+	TRUSTED_TOUCH_TVM_INIT,
+	TVM_IOMEM_LENT_NOTIFIED,
+	TVM_IRQ_LENT_NOTIFIED,
+	TVM_ALL_RESOURCES_LENT_NOTIFIED,
+	TVM_IOMEM_ACCEPTED,
+	TVM_I2C_SESSION_ACQUIRED,
+	TVM_IRQ_ACCEPTED,
+	TVM_INTERRUPT_ENABLED,
+	TVM_INTERRUPT_DISABLED,
+	TVM_IRQ_RELEASED,
+	TVM_I2C_SESSION_RELEASED,
+	TVM_IOMEM_RELEASED,
+	TRUSTED_TOUCH_TVM_STATE_MAX
+};
+
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+#define TRUSTED_TOUCH_MEM_LABEL 0x7
+
+#define TOUCH_RESET_GPIO_BASE 0xF114000
+#define TOUCH_RESET_GPIO_SIZE 0x1000
+#define TOUCH_RESET_GPIO_OFFSET 0x4
+#define TOUCH_INTR_GPIO_BASE 0xF115000
+#define TOUCH_INTR_GPIO_SIZE 0x1000
+#define TOUCH_INTR_GPIO_OFFSET 0x8
+
+#define TRUSTED_TOUCH_EVENT_LEND_FAILURE -1
+#define TRUSTED_TOUCH_EVENT_LEND_NOTIFICATION_FAILURE -2
+#define TRUSTED_TOUCH_EVENT_ACCEPT_FAILURE -3
+#define	TRUSTED_TOUCH_EVENT_FUNCTIONAL_FAILURE -4
+#define	TRUSTED_TOUCH_EVENT_RELEASE_FAILURE -5
+#define	TRUSTED_TOUCH_EVENT_RECLAIM_FAILURE -6
+#define	TRUSTED_TOUCH_EVENT_I2C_FAILURE -7
+#define	TRUSTED_TOUCH_EVENT_NOTIFICATIONS_PENDING 5
+
+struct trusted_touch_vm_info {
+	enum gh_irq_label irq_label;
+	enum gh_mem_notifier_tag mem_tag;
+	enum gh_vm_names vm_name;
+	const char *trusted_touch_type;
+	u32 hw_irq;
+	gh_memparcel_handle_t vm_mem_handle;
+	u32 *iomem_bases;
+	u32 *iomem_sizes;
+	u32 iomem_list_size;
+	void *mem_cookie;
+	atomic_t vm_state;
+};
+#endif
+
+struct fts_ts_data {
+	struct i2c_client *client;
+	struct spi_device *spi;
+	struct device *dev;
+	struct input_dev *input_dev;
+	struct fts_ts_platform_data *pdata;
+	struct ts_ic_info ic_info;
+	struct workqueue_struct *ts_workqueue;
+	struct work_struct fwupg_work;
+	struct delayed_work esdcheck_work;
+	struct delayed_work prc_work;
+	struct work_struct resume_work;
+	struct work_struct suspend_work;
+	struct ftxxxx_proc proc;
+	spinlock_t irq_lock;
+	struct mutex report_mutex;
+	struct mutex bus_lock;
+	struct mutex transition_lock;
+	int irq;
+	int log_level;
+	int fw_is_running;      /* confirm fw is running when using spi:default 0 */
+	int dummy_byte;
+#if defined(CONFIG_PM) && FTS_PATCH_COMERR_PM
+	struct completion pm_completion;
+	bool pm_suspend;
+#endif
+	bool suspended;
+	bool fw_loading;
+	bool irq_disabled;
+	bool power_disabled;
+	bool glove_mode;
+	bool cover_mode;
+	bool charger_mode;
+	bool gesture_mode;      /* gesture enable or disable, default: disable */
+	int report_rate;
+	/* multi-touch */
+	struct ts_event *events;
+	u8 *bus_tx_buf;
+	u8 *bus_rx_buf;
+	int bus_type;
+	u8 *point_buf;
+	void *notifier_cookie;
+	int pnt_buf_size;
+	int touchs;
+	int key_state;
+	int touch_point;
+	int point_num;
+	struct regulator *vdd;
+	struct regulator *vcc_i2c;
+#if FTS_PINCTRL_EN
+	struct pinctrl *pinctrl;
+	struct pinctrl_state *pins_active;
+	struct pinctrl_state *pins_suspend;
+	struct pinctrl_state *pins_release;
+#endif
+#if defined(CONFIG_FB) || defined(CONFIG_DRM)
+	struct notifier_block fb_notif;
+#elif defined(CONFIG_HAS_EARLYSUSPEND)
+	struct early_suspend early_suspend;
+#endif
+
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+	struct trusted_touch_vm_info *vm_info;
+	struct mutex fts_clk_io_ctrl_mutex;
+	const char *touch_environment;
+	struct completion trusted_touch_powerdown;
+	struct clk *core_clk;
+	struct clk *iface_clk;
+	atomic_t trusted_touch_initialized;
+	atomic_t trusted_touch_enabled;
+	atomic_t trusted_touch_transition;
+	atomic_t trusted_touch_event;
+	atomic_t trusted_touch_abort_status;
+	atomic_t delayed_vm_probe_pending;
+	atomic_t trusted_touch_mode;
+#endif
+};
+
+enum _FTS_BUS_TYPE {
+	BUS_TYPE_NONE,
+	BUS_TYPE_I2C,
+	BUS_TYPE_SPI,
+	BUS_TYPE_SPI_V2,
+};
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+extern struct fts_ts_data *fts_data;
+
+/* communication interface */
+int fts_read(u8 *cmd, u32 cmdlen, u8 *data, u32 datalen);
+int fts_read_reg(u8 addr, u8 *value);
+int fts_write(u8 *writebuf, u32 writelen);
+int fts_write_reg(u8 addr, u8 value);
+void fts_hid2std(void);
+int fts_bus_init(struct fts_ts_data *ts_data);
+int fts_bus_exit(struct fts_ts_data *ts_data);
+
+/* Gesture functions */
+int fts_gesture_init(struct fts_ts_data *ts_data);
+int fts_gesture_exit(struct fts_ts_data *ts_data);
+void fts_gesture_recovery(struct fts_ts_data *ts_data);
+int fts_gesture_readdata(struct fts_ts_data *ts_data, u8 *data);
+int fts_gesture_suspend(struct fts_ts_data *ts_data);
+int fts_gesture_resume(struct fts_ts_data *ts_data);
+
+/* Apk and functions */
+int fts_create_apk_debug_channel(struct fts_ts_data *);
+void fts_release_apk_debug_channel(struct fts_ts_data *);
+
+/* ADB functions */
+int fts_create_sysfs(struct fts_ts_data *ts_data);
+int fts_remove_sysfs(struct fts_ts_data *ts_data);
+
+/* ESD */
+#if FTS_ESDCHECK_EN
+int fts_esdcheck_init(struct fts_ts_data *ts_data);
+int fts_esdcheck_exit(struct fts_ts_data *ts_data);
+int fts_esdcheck_switch(bool enable);
+int fts_esdcheck_proc_busy(bool proc_debug);
+int fts_esdcheck_set_intr(bool intr);
+int fts_esdcheck_suspend(void);
+int fts_esdcheck_resume(void);
+#endif
+
+/* Production test */
+#if FTS_TEST_EN
+int fts_test_init(struct fts_ts_data *ts_data);
+int fts_test_exit(struct fts_ts_data *ts_data);
+#endif
+
+/* Point Report Check*/
+#if FTS_POINT_REPORT_CHECK_EN
+int fts_point_report_check_init(struct fts_ts_data *ts_data);
+int fts_point_report_check_exit(struct fts_ts_data *ts_data);
+void fts_prc_queue_work(struct fts_ts_data *ts_data);
+#endif
+
+/* FW upgrade */
+int fts_fwupg_init(struct fts_ts_data *ts_data);
+int fts_fwupg_exit(struct fts_ts_data *ts_data);
+int fts_enter_test_environment(bool test_state);
+
+/* Other */
+int fts_reset_proc(int hdelayms);
+int fts_wait_tp_to_valid(void);
+void fts_release_all_finger(void);
+void fts_tp_state_recovery(struct fts_ts_data *ts_data);
+int fts_ex_mode_init(struct fts_ts_data *ts_data);
+int fts_ex_mode_exit(struct fts_ts_data *ts_data);
+int fts_ex_mode_recovery(struct fts_ts_data *ts_data);
+
+void fts_irq_disable(void);
+void fts_irq_enable(void);
+int fts_ts_handle_trusted_touch_pvm(struct fts_ts_data *ts_data, int value);
+int fts_ts_handle_trusted_touch_tvm(struct fts_ts_data *ts_data, int value);
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+#ifdef CONFIG_ARCH_QTI_VM
+void fts_ts_trusted_touch_tvm_i2c_failure_report(struct fts_ts_data *fts_data);
+#endif
+#endif
+#endif /* __LINUX_FOCALTECH_CORE_H__ */

focaltech_touch/focaltech_esdcheck.c

@@ -0,0 +1,465 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, FocalTech Systems, Ltd., all rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*****************************************************************************
+*
+* File Name: focaltech_esdcheck.c
+*
+*    Author: Focaltech Driver Team
+*
+*   Created: 2016-08-03
+*
+*  Abstract: ESD check function
+*
+*   Version: v1.0
+*
+* Revision History:
+*        v1.0:
+*            First release. By luougojin 2016-08-03
+*        v1.1: By luougojin 2017-02-15
+*            1. Add LCD_ESD_PATCH to control idc_esdcheck_lcderror
+*****************************************************************************/
+
+/*****************************************************************************
+* Included header files
+*****************************************************************************/
+#include "focaltech_core.h"
+
+#if FTS_ESDCHECK_EN
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define ESDCHECK_WAIT_TIME              1000    /* ms */
+#define LCD_ESD_PATCH                   0
+#define ESDCHECK_INTRCNT_MAX            2
+
+/*****************************************************************************
+* Private enumerations, structures and unions using typedef
+*****************************************************************************/
+struct fts_esdcheck_st {
+	u8  mode                : 1;    /* 1- need check esd 0- no esd check */
+	u8  suspend             : 1;
+	u8  proc_debug          : 1;    /* apk or adb use */
+	u8  intr                : 1;    /* 1- Interrupt trigger */
+	u8  unused              : 4;
+	u8  intr_cnt;
+	u8  flow_work_hold_cnt;         /* Flow Work Cnt(reg0x91) keep a same value for x times. >=5 times is ESD, need reset */
+	u8  flow_work_cnt_last;         /* Save Flow Work Cnt(reg0x91) value */
+	u32 hardware_reset_cnt;
+	u32 nack_cnt;
+	u32 dataerror_cnt;
+};
+
+/*****************************************************************************
+* Static variables
+*****************************************************************************/
+static struct fts_esdcheck_st fts_esdcheck_data;
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+
+/*****************************************************************************
+* Static function prototypes
+*****************************************************************************/
+
+/*****************************************************************************
+* functions body
+*****************************************************************************/
+#if LCD_ESD_PATCH
+int lcd_need_reset;
+static int tp_need_recovery; /* LCD reset cause Tp reset */
+int idc_esdcheck_lcderror(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+	u8 val = 0;
+
+	FTS_DEBUG("check LCD ESD");
+	if ( (tp_need_recovery == 1) && (lcd_need_reset == 0) ) {
+		tp_need_recovery = 0;
+		/* LCD reset, need recover TP state */
+		fts_release_all_finger();
+		fts_tp_state_recovery(ts_data);
+	}
+
+	ret = fts_read_reg(FTS_REG_ESD_SATURATE, &val);
+	if ( ret < 0) {
+		FTS_ERROR("read reg0xED fail,ret:%d", ret);
+		return -EIO;
+	}
+
+	if (val == 0xAA) {
+		/*
+		* 1. Set flag lcd_need_reset = 1;
+		* 2. LCD driver need reset(recovery) LCD and set lcd_need_reset to 0
+		* 3. recover TP state
+		*/
+		FTS_INFO("LCD ESD, need execute LCD reset");
+		lcd_need_reset = 1;
+		tp_need_recovery = 1;
+	}
+
+	return 0;
+}
+#endif
+
+static int fts_esdcheck_tp_reset(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+
+	fts_esdcheck_data.flow_work_hold_cnt = 0;
+	fts_esdcheck_data.hardware_reset_cnt++;
+
+	fts_reset_proc(200);
+	fts_release_all_finger();
+	fts_tp_state_recovery(ts_data);
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+static bool get_chip_id(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+	int i = 0;
+	u8 reg_value = 0;
+	u8 reg_addr = 0;
+	u8 chip_id = ts_data->ic_info.ids.chip_idh;
+
+	for (i = 0; i < 3; i++) {
+		reg_addr = FTS_REG_CHIP_ID;
+		ret = fts_read(&reg_addr, 1, &reg_value, 1);
+		if (ret < 0) {
+			FTS_ERROR("read chip id fail,ret:%d", ret);
+			fts_esdcheck_data.nack_cnt++;
+		} else {
+			if (reg_value == chip_id) {
+				break;
+			} else {
+				FTS_DEBUG("read chip_id:%x,retry:%d", reg_value, i);
+				fts_esdcheck_data.dataerror_cnt++;
+			}
+		}
+		msleep(10);
+	}
+
+	/* if can't get correct data in 3 times, then need hardware reset */
+	if (i >= 3) {
+		FTS_ERROR("read chip id 3 times fail, need execute TP reset");
+		return true;
+	}
+
+	return false;
+}
+
+/*****************************************************************************
+*  Name: get_flow_cnt
+*  Brief: Read flow cnt(0x91)
+*  Input:
+*  Output:
+*  Return:  1(true) - Reg 0x91(flow cnt) abnormal: hold a value for 5 times
+*           0(false) - Reg 0x91(flow cnt) normal
+*****************************************************************************/
+static bool get_flow_cnt(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+	u8  reg_value = 0;
+	u8  reg_addr = 0;
+
+	reg_addr = FTS_REG_FLOW_WORK_CNT;
+	ret = fts_read(&reg_addr, 1, &reg_value, 1);
+	if (ret < 0) {
+		FTS_ERROR("read reg0x91 fail,ret:%d", ret);
+		fts_esdcheck_data.nack_cnt++;
+	} else {
+		if ( reg_value == fts_esdcheck_data.flow_work_cnt_last ) {
+			FTS_DEBUG("reg0x91,val:%x,last:%x", reg_value,
+				fts_esdcheck_data.flow_work_cnt_last);
+			fts_esdcheck_data.flow_work_hold_cnt++;
+		} else {
+			fts_esdcheck_data.flow_work_hold_cnt = 0;
+		}
+
+		fts_esdcheck_data.flow_work_cnt_last = reg_value;
+	}
+
+		/* Flow Work Cnt keep a value for 5 times, need execute TP reset */
+	if (fts_esdcheck_data.flow_work_hold_cnt >= 5) {
+		FTS_DEBUG("reg0x91 keep a value for 5 times, need execute TP reset");
+		return true;
+	}
+
+	return false;
+}
+
+static int esdcheck_algorithm(struct fts_ts_data *ts_data)
+{
+	int  ret = 0;
+	u8   reg_value = 0;
+	u8   reg_addr = 0;
+	bool hardware_reset = 0;
+
+	/* 1. esdcheck is interrupt, then return */
+	if (fts_esdcheck_data.intr == 1) {
+		fts_esdcheck_data.intr_cnt++;
+		if (fts_esdcheck_data.intr_cnt > ESDCHECK_INTRCNT_MAX)
+			fts_esdcheck_data.intr = 0;
+		else
+			return 0;
+	}
+
+	/* 2. check power state, if suspend, no need check esd */
+	if (fts_esdcheck_data.suspend == 1) {
+		FTS_DEBUG("In suspend, not check esd");
+		/* because in suspend state, adb can be used, when upgrade FW, will
+		 * active ESD check(active = 1); But in suspend, then will don't
+		 * queue_delayed_work, when resume, don't check ESD again
+		 */
+		return 0;
+	}
+
+	/* 3. check fts_esdcheck_data.proc_debug state, if 1-proc busy, no need check esd*/
+	if (fts_esdcheck_data.proc_debug == 1) {
+		FTS_INFO("In apk/adb command mode, not check esd");
+		return 0;
+	}
+
+	/* 4. In factory mode, can't check esd */
+	reg_addr = FTS_REG_WORKMODE;
+	ret = fts_read_reg(reg_addr, &reg_value);
+	if ( ret < 0 ) {
+		fts_esdcheck_data.nack_cnt++;
+	} else if ( (reg_value & 0x70) !=  FTS_REG_WORKMODE_WORK_VALUE) {
+		FTS_DEBUG("not in work mode(%x), no check esd", reg_value);
+		return 0;
+	}
+
+	/* 5. IDC esd check lcd  default:close */
+#if LCD_ESD_PATCH
+	idc_esdcheck_lcderror(ts_data);
+#endif
+
+	/* 6. Get Chip ID */
+	hardware_reset = get_chip_id(ts_data);
+
+	/* 7. get Flow work cnt: 0x91 If no change for 5 times, then ESD and reset */
+	if (!hardware_reset) {
+		hardware_reset = get_flow_cnt(ts_data);
+	}
+
+	/* 8. If need hardware reset, then handle it here */
+	if (hardware_reset == 1) {
+		FTS_DEBUG("NoACK=%d, Error Data=%d, Hardware Reset=%d",
+				fts_esdcheck_data.nack_cnt,
+				fts_esdcheck_data.dataerror_cnt,
+				fts_esdcheck_data.hardware_reset_cnt);
+		fts_esdcheck_tp_reset(ts_data);
+	}
+
+	return 0;
+}
+
+static void esdcheck_func(struct work_struct *work)
+{
+	struct fts_ts_data *ts_data = container_of(work,
+					struct fts_ts_data, esdcheck_work.work);
+
+	if (ENABLE == fts_esdcheck_data.mode) {
+		esdcheck_algorithm(ts_data);
+		queue_delayed_work(ts_data->ts_workqueue,
+				&ts_data->esdcheck_work,
+				msecs_to_jiffies(ESDCHECK_WAIT_TIME));
+	}
+}
+
+int fts_esdcheck_set_intr(bool intr)
+{
+	/* interrupt don't add debug message */
+	fts_esdcheck_data.intr = intr;
+	fts_esdcheck_data.intr_cnt = (u8)intr;
+	return 0;
+}
+
+static int fts_esdcheck_get_status(void)
+{
+	/* interrupt don't add debug message */
+	return fts_esdcheck_data.mode;
+}
+
+/*****************************************************************************
+*  Name: fts_esdcheck_proc_busy
+*  Brief: When APK or ADB command access TP via driver, then need set proc_debug,
+*         then will not check ESD.
+*  Input:
+*  Output:
+*  Return:
+*****************************************************************************/
+int fts_esdcheck_proc_busy(bool proc_debug)
+{
+	fts_esdcheck_data.proc_debug = proc_debug;
+	return 0;
+}
+
+/*****************************************************************************
+*  Name: fts_esdcheck_switch
+*  Brief: FTS esd check function switch.
+*  Input:   enable:  1 - Enable esd check
+*                    0 - Disable esd check
+*  Output:
+*  Return:
+*****************************************************************************/
+int fts_esdcheck_switch(bool enable)
+{
+	struct fts_ts_data *ts_data = fts_data;
+	FTS_FUNC_ENTER();
+	if (fts_esdcheck_data.mode == ENABLE) {
+		if (enable) {
+			FTS_DEBUG("ESD check start");
+			fts_esdcheck_data.flow_work_hold_cnt = 0;
+			fts_esdcheck_data.flow_work_cnt_last = 0;
+			fts_esdcheck_data.intr = 0;
+			fts_esdcheck_data.intr_cnt = 0;
+			queue_delayed_work(ts_data->ts_workqueue,
+					&ts_data->esdcheck_work,
+					msecs_to_jiffies(ESDCHECK_WAIT_TIME));
+		} else {
+			FTS_DEBUG("ESD check stop");
+			cancel_delayed_work_sync(&ts_data->esdcheck_work);
+		}
+	}
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+int fts_esdcheck_suspend(void)
+{
+	FTS_FUNC_ENTER();
+	fts_esdcheck_switch(DISABLE);
+	fts_esdcheck_data.suspend = 1;
+	fts_esdcheck_data.intr = 0;
+	fts_esdcheck_data.intr_cnt = 0;
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+int fts_esdcheck_resume( void )
+{
+	FTS_FUNC_ENTER();
+	fts_esdcheck_switch(ENABLE);
+	fts_esdcheck_data.suspend = 0;
+	fts_esdcheck_data.intr = 0;
+	fts_esdcheck_data.intr_cnt = 0;
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+static ssize_t fts_esdcheck_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	if (FTS_SYSFS_ECHO_ON(buf)) {
+		FTS_DEBUG("enable esdcheck");
+		fts_esdcheck_data.mode = ENABLE;
+		fts_esdcheck_switch(ENABLE);
+	} else if (FTS_SYSFS_ECHO_OFF(buf)) {
+		FTS_DEBUG("disable esdcheck");
+		fts_esdcheck_switch(DISABLE);
+		fts_esdcheck_data.mode = DISABLE;
+	}
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_esdcheck_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count;
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	count = snprintf(buf, PAGE_SIZE, "Esd check: %s\n", \
+			fts_esdcheck_get_status() ? "On" : "Off");
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+/* sysfs esd node
+ *   read example: cat  fts_esd_mode        ---read esd mode
+ *   write example:echo 01 > fts_esd_mode   ---make esdcheck enable
+ *
+ */
+static DEVICE_ATTR (fts_esd_mode, S_IRUGO | S_IWUSR, fts_esdcheck_show, fts_esdcheck_store);
+
+static struct attribute *fts_esd_mode_attrs[] = {
+
+	&dev_attr_fts_esd_mode.attr,
+	NULL,
+};
+
+static struct attribute_group fts_esd_group = {
+	.attrs = fts_esd_mode_attrs,
+};
+
+int fts_create_esd_sysfs(struct device *dev)
+{
+	int ret = 0;
+
+	ret = sysfs_create_group(&dev->kobj, &fts_esd_group);
+	if ( ret != 0) {
+		FTS_ERROR("fts_create_esd_sysfs(sysfs) create fail");
+		sysfs_remove_group(&dev->kobj, &fts_esd_group);
+		return ret;
+	}
+	return 0;
+}
+
+int fts_esdcheck_init(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+
+	if (ts_data->ts_workqueue) {
+		INIT_DELAYED_WORK(&ts_data->esdcheck_work, esdcheck_func);
+	} else {
+		FTS_ERROR("fts workqueue is NULL, can't run esd check func!");
+		return -EINVAL;
+	}
+
+	memset((u8 *)&fts_esdcheck_data, 0, sizeof(struct fts_esdcheck_st));
+
+	fts_esdcheck_data.mode = ENABLE;
+	fts_esdcheck_data.intr = 0;
+	fts_esdcheck_data.intr_cnt = 0;
+	fts_esdcheck_switch(ENABLE);
+	fts_create_esd_sysfs(ts_data->dev);
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+int fts_esdcheck_exit(struct fts_ts_data *ts_data)
+{
+	sysfs_remove_group(&ts_data->dev->kobj, &fts_esd_group);
+	return 0;
+}
+
+#endif /* FTS_ESDCHECK_EN */
+

focaltech_touch/focaltech_ex_fun.c

@@ -0,0 +1,1229 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, Focaltech Ltd. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*****************************************************************************
+*
+* File Name: Focaltech_ex_fun.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-08
+*
+* Abstract:
+*
+* Reference:
+*
+*****************************************************************************/
+
+/*****************************************************************************
+* 1.Included header files
+*****************************************************************************/
+#include <linux/uaccess.h>
+#include "focaltech_core.h"
+
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define PROC_UPGRADE                            0
+#define PROC_READ_REGISTER                      1
+#define PROC_WRITE_REGISTER                     2
+#define PROC_AUTOCLB                            4
+#define PROC_UPGRADE_INFO                       5
+#define PROC_WRITE_DATA                         6
+#define PROC_READ_DATA                          7
+#define PROC_SET_TEST_FLAG                      8
+#define PROC_SET_SLAVE_ADDR                     10
+#define PROC_HW_RESET                           11
+#define PROC_READ_STATUS                        12
+#define PROC_SET_BOOT_MODE                      13
+#define PROC_ENTER_TEST_ENVIRONMENT             14
+#define PROC_NAME                               "ftxxxx-debug"
+#define PROC_BUF_SIZE                           256
+
+/*****************************************************************************
+* Private enumerations, structures and unions using typedef
+*****************************************************************************/
+enum {
+	RWREG_OP_READ = 0,
+	RWREG_OP_WRITE = 1,
+};
+
+/*****************************************************************************
+* Static variables
+*****************************************************************************/
+static struct rwreg_operation_t {
+	int type;        /*  0: read, 1: write */
+	int reg;         /*  register */
+	int len;         /*  read/write length */
+	int val;         /*  length = 1; read: return value, write: op return */
+	int res;         /*  0: success, otherwise: fail */
+	char *opbuf;     /*  length >= 1, read return value, write: op return */
+} rw_op;
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+
+/*****************************************************************************
+* Static function prototypes
+*****************************************************************************/
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
+static ssize_t fts_debug_write(
+	struct file *filp, const char __user *buff, size_t count, loff_t *ppos)
+{
+	u8 *writebuf = NULL;
+	u8 tmpbuf[PROC_BUF_SIZE] = { 0 };
+	int buflen = count;
+	int writelen = 0;
+	int ret = 0;
+	char tmp[PROC_BUF_SIZE];
+	struct fts_ts_data *ts_data = fts_data;
+	struct ftxxxx_proc *proc = &ts_data->proc;
+
+	if ((buflen <= 1) || (buflen > PAGE_SIZE)) {
+		FTS_ERROR("apk proc wirte count(%d>%d) fail", buflen, (int)PAGE_SIZE);
+		return -EINVAL;
+	}
+
+	if (buflen > PROC_BUF_SIZE) {
+		writebuf = (u8 *)kzalloc(buflen * sizeof(u8), GFP_KERNEL);
+		if (NULL == writebuf) {
+			FTS_ERROR("apk proc wirte buf zalloc fail");
+			return -ENOMEM;
+		}
+	} else {
+		writebuf = tmpbuf;
+	}
+
+	if (copy_from_user(writebuf, buff, buflen)) {
+		FTS_ERROR("[APK]: copy from user error!!");
+		ret = -EFAULT;
+		goto proc_write_err;
+	}
+
+	proc->opmode = writebuf[0];
+	switch (proc->opmode) {
+	case PROC_SET_TEST_FLAG:
+		FTS_DEBUG("[APK]: PROC_SET_TEST_FLAG = %x", writebuf[1]);
+		if (writebuf[1] == 0) {
+#if FTS_ESDCHECK_EN
+			fts_esdcheck_switch(ENABLE);
+#endif
+		} else {
+#if FTS_ESDCHECK_EN
+			fts_esdcheck_switch(DISABLE);
+#endif
+		}
+		break;
+
+	case PROC_READ_REGISTER:
+		proc->cmd[0] = writebuf[1];
+		break;
+
+	case PROC_WRITE_REGISTER:
+		ret = fts_write_reg(writebuf[1], writebuf[2]);
+		if (ret < 0) {
+			FTS_ERROR("PROC_WRITE_REGISTER write error");
+			goto proc_write_err;
+		}
+		break;
+
+	case PROC_READ_DATA:
+		writelen = buflen - 1;
+		if (writelen >= FTX_MAX_COMMMAND_LENGTH) {
+			FTS_ERROR("cmd(PROC_READ_DATA) len(%d) fail", writelen);
+			goto proc_write_err;
+		}
+		memcpy(proc->cmd, writebuf + 1, writelen);
+		proc->cmd_len = writelen;
+		ret = fts_write(writebuf + 1, writelen);
+		if (ret < 0) {
+			FTS_ERROR("PROC_READ_DATA write error");
+			goto proc_write_err;
+		}
+		break;
+
+	case PROC_WRITE_DATA:
+		writelen = buflen - 1;
+		ret = fts_write(writebuf + 1, writelen);
+		if (ret < 0) {
+			FTS_ERROR("PROC_WRITE_DATA write error");
+			goto proc_write_err;
+		}
+		break;
+
+	case PROC_SET_SLAVE_ADDR:
+		break;
+
+	case PROC_HW_RESET:
+		snprintf(tmp, PROC_BUF_SIZE, "%s", writebuf + 1);
+		tmp[buflen - 1] = '\0';
+		if (strncmp(tmp, "focal_driver", 12) == 0) {
+			FTS_INFO("APK execute HW Reset");
+			fts_reset_proc(0);
+		}
+		break;
+
+	case PROC_SET_BOOT_MODE:
+		FTS_DEBUG("[APK]: PROC_SET_BOOT_MODE = %x", writebuf[1]);
+		if (0 == writebuf[1]) {
+			ts_data->fw_is_running = true;
+		} else {
+			ts_data->fw_is_running = false;
+		}
+		break;
+
+	case PROC_ENTER_TEST_ENVIRONMENT:
+		FTS_DEBUG("[APK]: PROC_ENTER_TEST_ENVIRONMENT = %x", writebuf[1]);
+		if (0 == writebuf[1]) {
+			fts_enter_test_environment(0);
+		} else {
+			fts_enter_test_environment(1);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	ret = buflen;
+proc_write_err:
+	if ((buflen > PROC_BUF_SIZE) && writebuf) {
+		kfree(writebuf);
+		writebuf = NULL;
+	}
+	return ret;
+}
+
+static ssize_t fts_debug_read(
+	struct file *filp, char __user *buff, size_t count, loff_t *ppos)
+{
+	int ret = 0;
+	int num_read_chars = 0;
+	int buflen = count;
+	u8 *readbuf = NULL;
+	u8 tmpbuf[PROC_BUF_SIZE] = { 0 };
+	struct fts_ts_data *ts_data = fts_data;
+	struct ftxxxx_proc *proc = &ts_data->proc;
+
+	if ((buflen <= 0) || (buflen > PAGE_SIZE)) {
+		FTS_ERROR("apk proc read count(%d>%d) fail", buflen, (int)PAGE_SIZE);
+		return -EINVAL;
+	}
+
+	if (buflen > PROC_BUF_SIZE) {
+		readbuf = (u8 *)kzalloc(buflen * sizeof(u8), GFP_KERNEL);
+		if (NULL == readbuf) {
+			FTS_ERROR("apk proc wirte buf zalloc fail");
+			return -ENOMEM;
+		}
+	} else {
+		readbuf = tmpbuf;
+	}
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(1);
+#endif
+
+	switch (proc->opmode) {
+	case PROC_READ_REGISTER:
+		num_read_chars = 1;
+		ret = fts_read_reg(proc->cmd[0], &readbuf[0]);
+		if (ret < 0) {
+			FTS_ERROR("PROC_READ_REGISTER read error");
+			goto proc_read_err;
+		}
+		break;
+
+	case PROC_WRITE_REGISTER:
+		break;
+
+	case PROC_READ_DATA:
+		num_read_chars = buflen;
+		ret = fts_read(NULL, 0, readbuf, num_read_chars);
+		if (ret < 0) {
+			FTS_ERROR("PROC_READ_DATA read error");
+			goto proc_read_err;
+		}
+		break;
+
+	case PROC_WRITE_DATA:
+		break;
+
+	default:
+		break;
+	}
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(0);
+#endif
+
+	if (copy_to_user(buff, readbuf, num_read_chars)) {
+		FTS_ERROR("copy to user error");
+		ret = -EFAULT;
+		goto proc_read_err;
+	}
+
+	ret = num_read_chars;
+proc_read_err:
+	if ((buflen > PROC_BUF_SIZE) && readbuf) {
+		kfree(readbuf);
+		readbuf = NULL;
+	}
+	return ret;
+}
+
+static const struct proc_ops fts_proc_fops = {
+	.proc_read  = fts_debug_read,
+	.proc_write = fts_debug_write,
+};
+#else
+static int fts_debug_write(struct file *filp,
+	const char __user *buff, unsigned long len, void *data)
+{
+	u8 *writebuf = NULL;
+	u8 tmpbuf[PROC_BUF_SIZE] = { 0 };
+	int buflen = count;
+	int writelen = 0;
+	int ret = 0;
+	char tmp[PROC_BUF_SIZE];
+	struct fts_ts_data *ts_data = fts_data;
+	struct ftxxxx_proc *proc = &ts_data->proc;
+
+	if ((buflen <= 1) || (buflen > PAGE_SIZE)) {
+		FTS_ERROR("apk proc wirte count(%d>%d) fail", buflen, (int)PAGE_SIZE);
+		return -EINVAL;
+	}
+
+	if (buflen > PROC_BUF_SIZE) {
+		writebuf = (u8 *)kzalloc(buflen * sizeof(u8), GFP_KERNEL);
+		if (NULL == writebuf) {
+			FTS_ERROR("apk proc wirte buf zalloc fail");
+			return -ENOMEM;
+		}
+	} else {
+		writebuf = tmpbuf;
+	}
+
+	if (copy_from_user(writebuf, buff, buflen)) {
+		FTS_ERROR("[APK]: copy from user error!!");
+		ret = -EFAULT;
+		goto proc_write_err;
+	}
+
+	proc->opmode = writebuf[0];
+	switch (proc->opmode) {
+	case PROC_SET_TEST_FLAG:
+		FTS_DEBUG("[APK]: PROC_SET_TEST_FLAG = %x", writebuf[1]);
+		if (writebuf[1] == 0) {
+#if FTS_ESDCHECK_EN
+			fts_esdcheck_switch(ENABLE);
+#endif
+		} else {
+#if FTS_ESDCHECK_EN
+			fts_esdcheck_switch(DISABLE);
+#endif
+		}
+		break;
+
+	case PROC_READ_REGISTER:
+		proc->cmd[0] = writebuf[1];
+		break;
+
+	case PROC_WRITE_REGISTER:
+		ret = fts_write_reg(writebuf[1], writebuf[2]);
+		if (ret < 0) {
+			FTS_ERROR("PROC_WRITE_REGISTER write error");
+			goto proc_write_err;
+		}
+		break;
+
+	case PROC_READ_DATA:
+		writelen = buflen - 1;
+		if (writelen >= FTX_MAX_COMMMAND_LENGTH) {
+			FTS_ERROR("cmd(PROC_READ_DATA) length(%d) fail", writelen);
+			goto proc_write_err;
+		}
+		memcpy(proc->cmd, writebuf + 1, writelen);
+		proc->cmd_len = writelen;
+		ret = fts_write(writebuf + 1, writelen);
+		if (ret < 0) {
+			FTS_ERROR("PROC_READ_DATA write error");
+			goto proc_write_err;
+		}
+		break;
+
+	case PROC_WRITE_DATA:
+		writelen = buflen - 1;
+		ret = fts_write(writebuf + 1, writelen);
+		if (ret < 0) {
+			FTS_ERROR("PROC_WRITE_DATA write error");
+			goto proc_write_err;
+		}
+		break;
+
+	case PROC_SET_SLAVE_ADDR:
+		break;
+
+	case PROC_HW_RESET:
+		snprintf(tmp, PROC_BUF_SIZE, "%s", writebuf + 1);
+		tmp[buflen - 1] = '\0';
+		if (strncmp(tmp, "focal_driver", 12) == 0) {
+			FTS_INFO("APK execute HW Reset");
+			fts_reset_proc(0);
+		}
+		break;
+
+	case PROC_SET_BOOT_MODE:
+		FTS_DEBUG("[APK]: PROC_SET_BOOT_MODE = %x", writebuf[1]);
+		if (0 == writebuf[1]) {
+			ts_data->fw_is_running = true;
+		} else {
+			ts_data->fw_is_running = false;
+		}
+		break;
+
+	case PROC_ENTER_TEST_ENVIRONMENT:
+		FTS_DEBUG("[APK]: PROC_ENTER_TEST_ENVIRONMENT = %x", writebuf[1]);
+		if (0 == writebuf[1]) {
+			fts_enter_test_environment(0);
+		} else {
+			fts_enter_test_environment(1);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	ret = buflen;
+proc_write_err:
+	if ((buflen > PROC_BUF_SIZE) && writebuf) {
+		kfree(writebuf);
+		writebuf = NULL;
+	}
+	return ret;
+}
+
+static int fts_debug_read(
+	char *page, char **start, off_t off, int count, int *eof, void *data )
+{
+	int ret = 0;
+	int num_read_chars = 0;
+	int buflen = count;
+	u8 *readbuf = NULL;
+	u8 tmpbuf[PROC_BUF_SIZE] = { 0 };
+	struct fts_ts_data *ts_data = fts_data;
+	struct ftxxxx_proc *proc = &ts_data->proc;
+
+	if ((buflen <= 0) || (buflen > PAGE_SIZE)) {
+		FTS_ERROR("apk proc read count(%d>%d) fail", buflen, (int)PAGE_SIZE);
+		return -EINVAL;
+	}
+
+	if (buflen > PROC_BUF_SIZE) {
+		readbuf = (u8 *)kzalloc(buflen * sizeof(u8), GFP_KERNEL);
+		if (NULL == readbuf) {
+			FTS_ERROR("apk proc wirte buf zalloc fail");
+			return -ENOMEM;
+		}
+	} else {
+		readbuf = tmpbuf;
+	}
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(1);
+#endif
+
+	switch (proc->opmode) {
+	case PROC_READ_REGISTER:
+		num_read_chars = 1;
+		ret = fts_read_reg(proc->cmd[0], &readbuf[0]);
+		if (ret < 0) {
+			FTS_ERROR("PROC_READ_REGISTER read error");
+			goto proc_read_err;
+		}
+		break;
+
+	case PROC_WRITE_REGISTER:
+		break;
+
+	case PROC_READ_DATA:
+		num_read_chars = buflen;
+		ret = fts_read(NULL, 0, readbuf, num_read_chars);
+		if (ret < 0) {
+			FTS_ERROR("PROC_READ_DATA read error");
+			goto proc_read_err;
+		}
+		break;
+
+	case PROC_WRITE_DATA:
+		break;
+
+	default:
+		break;
+	}
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(0);
+#endif
+
+	if (copy_to_user(buff, readbuf, num_read_chars)) {
+		FTS_ERROR("copy to user error");
+		ret = -EFAULT;
+		goto proc_read_err;
+	}
+
+	ret = num_read_chars;
+proc_read_err:
+	if ((buflen > PROC_BUF_SIZE) && readbuf) {
+		kfree(readbuf);
+		readbuf = NULL;
+	}
+	return ret;
+}
+#endif
+
+int fts_create_apk_debug_channel(struct fts_ts_data *ts_data)
+{
+	struct ftxxxx_proc *proc = &ts_data->proc;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
+	proc->proc_entry = proc_create(PROC_NAME, 0777, NULL, &fts_proc_fops);
+	if (NULL == proc->proc_entry) {
+		FTS_ERROR("create proc entry fail");
+		return -ENOMEM;
+	}
+#else
+	proc->proc_entry = create_proc_entry(PROC_NAME, 0777, NULL);
+	if (NULL == proc->proc_entry) {
+		FTS_ERROR("create proc entry fail");
+		return -ENOMEM;
+	}
+	proc->proc_entry->write_proc = fts_debug_write;
+	proc->proc_entry->read_proc = fts_debug_read;
+#endif
+
+	FTS_INFO("Create proc entry success!");
+	return 0;
+}
+
+void fts_release_apk_debug_channel(struct fts_ts_data *ts_data)
+{
+	struct ftxxxx_proc *proc = &ts_data->proc;
+
+	if (proc->proc_entry) {
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
+		proc_remove(proc->proc_entry);
+#else
+		remove_proc_entry(PROC_NAME, NULL);
+#endif
+	}
+}
+
+/************************************************************************
+ * sysfs interface
+ ***********************************************************************/
+/* fts_hw_reset interface */
+static ssize_t fts_hw_reset_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct input_dev *input_dev = fts_data->input_dev;
+	ssize_t count = 0;
+
+	mutex_lock(&input_dev->mutex);
+	fts_reset_proc(0);
+	count = snprintf(buf, PAGE_SIZE, "hw reset executed\n");
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_hw_reset_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	return -EPERM;
+}
+
+/* fts_irq interface */
+static ssize_t fts_irq_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	ssize_t count = 0;
+	struct irq_desc *desc = irq_to_desc(fts_data->irq);
+
+	count = snprintf(buf, PAGE_SIZE, "irq_depth:%d\n", desc->depth);
+
+	return count;
+}
+
+static ssize_t fts_irq_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	if (FTS_SYSFS_ECHO_ON(buf)) {
+		FTS_INFO("enable irq");
+		fts_irq_enable();
+	} else if (FTS_SYSFS_ECHO_OFF(buf)) {
+		FTS_INFO("disable irq");
+		fts_irq_disable();
+	}
+	mutex_unlock(&input_dev->mutex);
+	return count;
+}
+
+/* fts_boot_mode interface */
+static ssize_t fts_bootmode_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	FTS_FUNC_ENTER();
+	mutex_lock(&input_dev->mutex);
+	if (FTS_SYSFS_ECHO_ON(buf)) {
+		FTS_INFO("[EX-FUN]set to boot mode");
+		fts_data->fw_is_running = false;
+	} else if (FTS_SYSFS_ECHO_OFF(buf)) {
+		FTS_INFO("[EX-FUN]set to fw mode");
+		fts_data->fw_is_running = true;
+	}
+	mutex_unlock(&input_dev->mutex);
+	FTS_FUNC_EXIT();
+
+	return count;
+}
+
+static ssize_t fts_bootmode_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	ssize_t count = 0;
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	FTS_FUNC_ENTER();
+	mutex_lock(&input_dev->mutex);
+	if (true == fts_data->fw_is_running) {
+		count = snprintf(buf, PAGE_SIZE, "tp is in fw mode\n");
+	} else {
+		count = snprintf(buf, PAGE_SIZE, "tp is in boot mode\n");
+	}
+	mutex_unlock(&input_dev->mutex);
+	FTS_FUNC_EXIT();
+
+	return count;
+}
+
+/* fts_tpfwver interface */
+static ssize_t fts_fw_version_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct fts_ts_data *ts_data = fts_data;
+	struct input_dev *input_dev = ts_data->input_dev;
+	ssize_t num_read_chars = 0;
+	u8 fwver = 0;
+
+	mutex_lock(&input_dev->mutex);
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(1);
+#endif
+	fts_read_reg(FTS_REG_FW_VER, &fwver);
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(0);
+#endif
+	if ((fwver == 0xFF) || (fwver == 0x00))
+		num_read_chars = snprintf(buf, PAGE_SIZE, "get tp fw version fail!\n");
+	else
+		num_read_chars = snprintf(buf, PAGE_SIZE, "%02x\n", fwver);
+
+	mutex_unlock(&input_dev->mutex);
+	return num_read_chars;
+}
+
+static ssize_t fts_fw_version_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	return -EPERM;
+}
+
+/* fts_rw_reg */
+static ssize_t fts_tprwreg_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count;
+	int i;
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+
+	if (rw_op.len < 0) {
+		count = snprintf(buf, PAGE_SIZE, "Invalid cmd line\n");
+	} else if (rw_op.len == 1) {
+		if (RWREG_OP_READ == rw_op.type) {
+			if (rw_op.res == 0) {
+				count = snprintf(buf, PAGE_SIZE, "Read %02X: %02X\n", rw_op.reg, rw_op.val);
+			} else {
+				count = snprintf(buf, PAGE_SIZE, "Read %02X failed, ret: %d\n", rw_op.reg,  rw_op.res);
+			}
+		} else {
+			if (rw_op.res == 0) {
+				count = snprintf(buf, PAGE_SIZE, "Write %02X, %02X success\n", rw_op.reg,  rw_op.val);
+			} else {
+				count = snprintf(buf, PAGE_SIZE, "Write %02X failed, ret: %d\n", rw_op.reg,  rw_op.res);
+			}
+		}
+	} else {
+		if (RWREG_OP_READ == rw_op.type) {
+			count = snprintf(buf, PAGE_SIZE, "Read Reg: [%02X]-[%02X]\n", rw_op.reg, rw_op.reg + rw_op.len);
+			count += snprintf(buf + count, PAGE_SIZE, "Result: ");
+			if (rw_op.res) {
+				count += snprintf(buf + count, PAGE_SIZE, "failed, ret: %d\n", rw_op.res);
+			} else {
+				if (rw_op.opbuf) {
+					for (i = 0; i < rw_op.len; i++) {
+						count += snprintf(buf + count, PAGE_SIZE, "%02X ", rw_op.opbuf[i]);
+					}
+					count += snprintf(buf + count, PAGE_SIZE, "\n");
+				}
+			}
+		} else {
+			count = snprintf(buf, PAGE_SIZE, "Write Reg: [%02X]-[%02X]\n", rw_op.reg, rw_op.reg + rw_op.len - 1);
+			count += snprintf(buf + count, PAGE_SIZE, "Write Data: ");
+			if (rw_op.opbuf) {
+				for (i = 1; i < rw_op.len; i++) {
+					count += snprintf(buf + count, PAGE_SIZE, "%02X ", rw_op.opbuf[i]);
+				}
+				count += snprintf(buf + count, PAGE_SIZE, "\n");
+			}
+			if (rw_op.res) {
+				count += snprintf(buf + count, PAGE_SIZE, "Result: failed, ret: %d\n", rw_op.res);
+			} else {
+				count += snprintf(buf + count, PAGE_SIZE, "Result: success\n");
+			}
+		}
+		/*if (rw_op.opbuf) {
+		 *  kfree(rw_op.opbuf);
+		 *  rw_op.opbuf = NULL;
+		 *}
+		 */
+	}
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static int shex_to_int(const char *hex_buf, int size)
+{
+	int i;
+	int base = 1;
+	int value = 0;
+	char single;
+
+	for (i = size - 1; i >= 0; i--) {
+		single = hex_buf[i];
+
+		if ((single >= '0') && (single <= '9')) {
+			value += (single - '0') * base;
+		} else if ((single >= 'a') && (single <= 'z')) {
+			value += (single - 'a' + 10) * base;
+		} else if ((single >= 'A') && (single <= 'Z')) {
+			value += (single - 'A' + 10) * base;
+		} else {
+			return -EINVAL;
+		}
+
+		base *= 16;
+	}
+
+	return value;
+}
+
+
+static u8 shex_to_u8(const char *hex_buf, int size)
+{
+	return (u8)shex_to_int(hex_buf, size);
+}
+/*
+ * Format buf:
+ * [0]: '0' write, '1' read(reserved)
+ * [1-2]: addr, hex
+ * [3-4]: length, hex
+ * [5-6]...[n-(n+1)]: data, hex
+ */
+static int fts_parse_buf(const char *buf, size_t cmd_len)
+{
+	int length;
+	int i;
+	char *tmpbuf;
+
+	rw_op.reg = shex_to_u8(buf + 1, 2);
+	length = shex_to_int(buf + 3, 2);
+
+	if (buf[0] == '1') {
+		rw_op.len = length;
+		rw_op.type = RWREG_OP_READ;
+		FTS_DEBUG("read %02X, %d bytes", rw_op.reg, rw_op.len);
+	} else {
+		if (cmd_len < (length * 2 + 5)) {
+			pr_err("data invalided!\n");
+			return -EINVAL;
+		}
+		FTS_DEBUG("write %02X, %d bytes", rw_op.reg, length);
+
+		/* first byte is the register addr */
+		rw_op.type = RWREG_OP_WRITE;
+		rw_op.len = length + 1;
+	}
+
+	if (rw_op.len > 0) {
+		tmpbuf = (char *)kzalloc(rw_op.len, GFP_KERNEL);
+		if (!tmpbuf) {
+			FTS_ERROR("allocate memory failed!\n");
+			return -ENOMEM;
+		}
+
+		if (RWREG_OP_WRITE == rw_op.type) {
+			tmpbuf[0] = rw_op.reg & 0xFF;
+			FTS_DEBUG("write buffer: ");
+			for (i = 1; i < rw_op.len; i++) {
+				tmpbuf[i] = shex_to_u8(buf + 5 + i * 2 - 2, 2);
+				FTS_DEBUG("buf[%d]: %02X", i, tmpbuf[i] & 0xFF);
+			}
+		}
+		rw_op.opbuf = tmpbuf;
+	}
+
+	return rw_op.len;
+}
+
+static ssize_t fts_tprwreg_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct input_dev *input_dev = fts_data->input_dev;
+	ssize_t cmd_length = 0;
+
+	mutex_lock(&input_dev->mutex);
+	cmd_length = count - 1;
+
+	if (rw_op.opbuf) {
+		kfree(rw_op.opbuf);
+		rw_op.opbuf = NULL;
+	}
+
+	FTS_DEBUG("cmd len: %d, buf: %s", (int)cmd_length, buf);
+	/* compatible old ops */
+	if (2 == cmd_length) {
+		rw_op.type = RWREG_OP_READ;
+		rw_op.len = 1;
+		rw_op.reg = shex_to_int(buf, 2);
+	} else if (4 == cmd_length) {
+		rw_op.type = RWREG_OP_WRITE;
+		rw_op.len = 1;
+		rw_op.reg = shex_to_int(buf, 2);
+		rw_op.val = shex_to_int(buf + 2, 2);
+	} else if (cmd_length < 5) {
+		FTS_ERROR("Invalid cmd buffer");
+		mutex_unlock(&input_dev->mutex);
+		return -EINVAL;
+	} else {
+		rw_op.len = fts_parse_buf(buf, cmd_length);
+	}
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(1);
+#endif
+	if (rw_op.len < 0) {
+		FTS_ERROR("cmd buffer error!");
+		goto exit;
+	}
+
+	if (RWREG_OP_READ == rw_op.type) {
+		if (rw_op.len == 1) {
+			u8 reg, val;
+			reg = rw_op.reg & 0xFF;
+			rw_op.res = fts_read_reg(reg, &val);
+			rw_op.val = val;
+		} else {
+			char reg;
+			reg = rw_op.reg & 0xFF;
+
+			rw_op.res = fts_read(&reg, 1, rw_op.opbuf, rw_op.len);
+		}
+
+		if (rw_op.res < 0) {
+			FTS_ERROR("Could not read 0x%02x", rw_op.reg);
+		} else {
+			FTS_INFO("read 0x%02x, %d bytes successful", rw_op.reg, rw_op.len);
+			rw_op.res = 0;
+		}
+
+	} else {
+		if (rw_op.len == 1) {
+			u8 reg, val;
+			reg = rw_op.reg & 0xFF;
+			val = rw_op.val & 0xFF;
+			rw_op.res = fts_write_reg(reg, val);
+		} else {
+			rw_op.res = fts_write(rw_op.opbuf, rw_op.len);
+		}
+		if (rw_op.res < 0) {
+			FTS_ERROR("Could not write 0x%02x", rw_op.reg);
+
+		} else {
+			FTS_INFO("Write 0x%02x, %d bytes successful", rw_op.val, rw_op.len);
+			rw_op.res = 0;
+		}
+	}
+
+exit:
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(0);
+#endif
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+/* fts_driver_info interface */
+static ssize_t fts_driverinfo_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	struct fts_ts_data *ts_data = fts_data;
+	struct fts_ts_platform_data *pdata = ts_data->pdata;
+	struct input_dev *input_dev = ts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	count += scnprintf(buf + count, PAGE_SIZE, "Driver Ver:%s\n", FTS_DRIVER_VERSION);
+
+	count += snprintf(buf + count, PAGE_SIZE, "Resolution:(%d,%d)~(%d,%d)\n",
+			pdata->x_min, pdata->y_min, pdata->x_max, pdata->y_max);
+
+	count += snprintf(buf + count, PAGE_SIZE, "Max Touchs:%d\n", pdata->max_touch_number);
+
+	count += scnprintf(buf + count, PAGE_SIZE, "reset gpio:%d,int gpio:%d,irq:%d\n",
+			pdata->reset_gpio, pdata->irq_gpio, ts_data->irq);
+
+	count += scnprintf(buf + count, PAGE_SIZE, "IC ID:0x%02x%02x\n",
+			ts_data->ic_info.ids.chip_idh, ts_data->ic_info.ids.chip_idl);
+	if (ts_data->bus_type == BUS_TYPE_I2C)
+		count += scnprintf(buf + count, PAGE_SIZE, "BUS:%s,addr:0x%x\n",
+				"I2C", ts_data->client->addr);
+	else
+		count += scnprintf(buf + count, PAGE_SIZE,
+				"BUS:%s,mode:%d,max_freq:%d\n", "SPI",
+				ts_data->spi->mode, ts_data->spi->max_speed_hz);
+
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_driverinfo_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	return -EPERM;
+}
+
+/* fts_dump_reg interface */
+static ssize_t fts_dumpreg_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	u8 val = 0;
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(1);
+#endif
+	fts_read_reg(FTS_REG_POWER_MODE, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "Power Mode:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_FW_VER, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "FW Ver:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_LIC_VER, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "LCD Initcode Ver:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_IDE_PARA_VER_ID, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "Param Ver:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_IDE_PARA_STATUS, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "Param status:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_VENDOR_ID, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "Vendor ID:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_LCD_BUSY_NUM, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "LCD Busy Number:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_GESTURE_EN, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "Gesture Mode:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_CHARGER_MODE_EN, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "charge stat:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_INT_CNT, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "INT count:0x%02x\n", val);
+
+	fts_read_reg(FTS_REG_FLOW_WORK_CNT, &val);
+	count += snprintf(buf + count, PAGE_SIZE, "ESD count:0x%02x\n", val);
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_proc_busy(0);
+#endif
+
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_dumpreg_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	return -EPERM;
+}
+
+/* fts_dump_reg interface */
+static ssize_t fts_tpbuf_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	int i = 0;
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	count += snprintf(buf + count, PAGE_SIZE, "touch point buffer:\n");
+	for (i = 0; i < fts_data->pnt_buf_size; i++) {
+		count += snprintf(buf + count, PAGE_SIZE, "%02x ",
+			fts_data->point_buf[i]);
+	}
+	count += snprintf(buf + count, PAGE_SIZE, "\n");
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_tpbuf_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	return -EPERM;
+}
+
+/* fts_log_level interface */
+static ssize_t fts_log_level_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	count += snprintf(buf + count, PAGE_SIZE, "log level:%d\n",
+			fts_data->log_level);
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_log_level_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int value = 0;
+	struct input_dev *input_dev = fts_data->input_dev;
+
+	FTS_FUNC_ENTER();
+	mutex_lock(&input_dev->mutex);
+	sscanf(buf, "%d", &value);
+	FTS_DEBUG("log level:%d->%d", fts_data->log_level, value);
+	fts_data->log_level = value;
+	mutex_unlock(&input_dev->mutex);
+	FTS_FUNC_EXIT();
+
+	return count;
+}
+
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+
+static ssize_t trusted_touch_enable_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct fts_ts_data *info = fts_data;
+
+	return scnprintf(buf, PAGE_SIZE, "%d",
+			atomic_read(&info->trusted_touch_enabled));
+}
+
+static ssize_t trusted_touch_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fts_ts_data *info = fts_data;
+	unsigned long value;
+	int err = 0;
+
+	if (count > 2)
+		return -EINVAL;
+	err = kstrtoul(buf, 10, &value);
+	if (err != 0)
+		return err;
+
+	if (!atomic_read(&info->trusted_touch_initialized))
+		return -EIO;
+
+#ifdef CONFIG_ARCH_QTI_VM
+	err = fts_ts_handle_trusted_touch_tvm(info, value);
+	if (err) {
+		pr_err("Failed to handle trusted touch in tvm\n");
+		return -EINVAL;
+	}
+#else
+	err = fts_ts_handle_trusted_touch_pvm(info, value);
+	if (err) {
+		pr_err("Failed to handle trusted touch in pvm\n");
+		return -EINVAL;
+	}
+#endif
+	err = count;
+	return err;
+}
+
+static ssize_t trusted_touch_event_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct fts_ts_data *info = fts_data;
+
+	return scnprintf(buf, PAGE_SIZE, "%d",
+			atomic_read(&info->trusted_touch_event));
+}
+
+static ssize_t trusted_touch_event_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fts_ts_data *info = fts_data;
+	unsigned long value;
+	int err = 0;
+
+	if (count > 2)
+		return -EINVAL;
+
+	err = kstrtoul(buf, 10, &value);
+	if (err != 0)
+		return err;
+
+	if (!atomic_read(&info->trusted_touch_initialized))
+		return -EIO;
+
+	if (value)
+		return -EIO;
+
+	atomic_set(&info->trusted_touch_event, value);
+
+	return count;
+}
+
+static ssize_t trusted_touch_type_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct fts_ts_data *info = fts_data;
+
+	return scnprintf(buf, PAGE_SIZE, "%s", info->vm_info->trusted_touch_type);
+}
+
+#endif
+
+/* get the fw version  example:cat fw_version */
+static DEVICE_ATTR_RW(fts_fw_version);
+
+/* read and write register(s)
+*   All data type is **HEX**
+*   Single Byte:
+*       read:   echo 88 > rw_reg ---read register 0x88
+*       write:  echo 8807 > rw_reg ---write 0x07 into register 0x88
+*   Multi-bytes:
+*       [0:rw-flag][1-2: reg addr, hex][3-4: length, hex][5-6...n-n+1: write data, hex]
+*       rw-flag: 0, write; 1, read
+*       read:  echo 10005           > rw_reg ---read reg 0x00-0x05
+*       write: echo 000050102030405 > rw_reg ---write reg 0x00-0x05 as 01,02,03,04,05
+*  Get result:
+*       cat rw_reg
+*/
+static DEVICE_ATTR(fts_rw_reg, S_IRUGO | S_IWUSR, fts_tprwreg_show, fts_tprwreg_store);
+static DEVICE_ATTR(fts_driver_info, S_IRUGO | S_IWUSR, fts_driverinfo_show, fts_driverinfo_store);
+static DEVICE_ATTR(fts_dump_reg, S_IRUGO | S_IWUSR, fts_dumpreg_show, fts_dumpreg_store);
+static DEVICE_ATTR(fts_hw_reset, S_IRUGO | S_IWUSR, fts_hw_reset_show, fts_hw_reset_store);
+static DEVICE_ATTR(fts_irq, S_IRUGO | S_IWUSR, fts_irq_show, fts_irq_store);
+static DEVICE_ATTR(fts_boot_mode, S_IRUGO | S_IWUSR, fts_bootmode_show, fts_bootmode_store);
+static DEVICE_ATTR(fts_touch_point, S_IRUGO | S_IWUSR, fts_tpbuf_show, fts_tpbuf_store);
+static DEVICE_ATTR(fts_log_level, S_IRUGO | S_IWUSR, fts_log_level_show, fts_log_level_store);
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+static DEVICE_ATTR_RW(trusted_touch_enable);
+static DEVICE_ATTR_RW(trusted_touch_event);
+static DEVICE_ATTR_RO(trusted_touch_type);
+#endif
+
+/* add your attr in here*/
+static struct attribute *fts_attributes[] = {
+	&dev_attr_fts_fw_version.attr,
+	&dev_attr_fts_rw_reg.attr,
+	&dev_attr_fts_dump_reg.attr,
+	&dev_attr_fts_driver_info.attr,
+	&dev_attr_fts_hw_reset.attr,
+	&dev_attr_fts_irq.attr,
+	&dev_attr_fts_boot_mode.attr,
+	&dev_attr_fts_touch_point.attr,
+	&dev_attr_fts_log_level.attr,
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+	&dev_attr_trusted_touch_enable.attr,
+	&dev_attr_trusted_touch_event.attr,
+	&dev_attr_trusted_touch_type.attr,
+#endif
+	NULL
+};
+
+static struct attribute_group fts_attribute_group = {
+	.attrs = fts_attributes
+};
+
+int fts_create_sysfs(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+
+	ret = sysfs_create_group(&ts_data->dev->kobj, &fts_attribute_group);
+	if (ret) {
+		FTS_ERROR("[EX]: sysfs_create_group() failed!!");
+		sysfs_remove_group(&ts_data->dev->kobj, &fts_attribute_group);
+		return -ENOMEM;
+	} else {
+		FTS_INFO("[EX]: sysfs_create_group() succeeded!!");
+	}
+
+	return ret;
+}
+
+int fts_remove_sysfs(struct fts_ts_data *ts_data)
+{
+	sysfs_remove_group(&ts_data->dev->kobj, &fts_attribute_group);
+	return 0;
+}

focaltech_touch/focaltech_ex_mode.c

@@ -0,0 +1,359 @@
+/*
+ *
+ * FocalTech ftxxxx TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, Focaltech Ltd. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*****************************************************************************
+*
+* File Name: focaltech_ex_mode.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-31
+*
+* Abstract:
+*
+* Reference:
+*
+*****************************************************************************/
+
+/*****************************************************************************
+* 1.Included header files
+*****************************************************************************/
+#include "focaltech_core.h"
+
+/*****************************************************************************
+* 2.Private constant and macro definitions using #define
+*****************************************************************************/
+
+/*****************************************************************************
+* 3.Private enumerations, structures and unions using typedef
+*****************************************************************************/
+enum _ex_mode {
+	MODE_GLOVE = 0,
+	MODE_COVER,
+	MODE_CHARGER,
+	REPORT_RATE,
+};
+
+/*****************************************************************************
+* 4.Static variables
+*****************************************************************************/
+
+/*****************************************************************************
+* 5.Global variable or extern global variabls/functions
+*****************************************************************************/
+
+/*****************************************************************************
+* 6.Static function prototypes
+*******************************************************************************/
+static int fts_ex_mode_switch(enum _ex_mode mode, u8 value)
+{
+	int ret = 0;
+
+	switch (mode) {
+	case MODE_GLOVE:
+		ret = fts_write_reg(FTS_REG_GLOVE_MODE_EN, value > 0 ? 1 : 0);
+		if (ret < 0)
+			FTS_ERROR("MODE_GLOVE switch to %d fail", value);
+		break;
+
+	case MODE_COVER:
+		ret = fts_write_reg(FTS_REG_COVER_MODE_EN, value > 0 ? 1 : 0);
+		if (ret < 0)
+			FTS_ERROR("MODE_COVER switch to %d fail", value);
+		break;
+
+	case MODE_CHARGER:
+		ret = fts_write_reg(FTS_REG_CHARGER_MODE_EN, value > 0 ? 1 : 0);
+		if (ret < 0)
+			FTS_ERROR("MODE_CHARGER switch to %d fail", value);
+		break;
+
+	case REPORT_RATE:
+		ret = fts_write_reg(FTS_REG_REPORT_RATE, value);
+		if (ret < 0)
+			FTS_ERROR("REPORT_RATE switch to %d fail", value);
+		break;
+
+	default:
+		FTS_ERROR("mode(%d) unsupport", mode);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static ssize_t fts_glove_mode_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	u8 val = 0;
+	struct fts_ts_data *ts_data = fts_data;
+	struct input_dev *input_dev = ts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	fts_read_reg(FTS_REG_GLOVE_MODE_EN, &val);
+	count = scnprintf(buf + count, PAGE_SIZE, "Glove Mode:%s\n",
+			ts_data->glove_mode ? "On" : "Off");
+	count += scnprintf(buf + count, PAGE_SIZE - count,
+			"Glove Reg(0xC0):%d\n", val);
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_glove_mode_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = fts_data;
+
+	if (FTS_SYSFS_ECHO_ON(buf)) {
+		if (!ts_data->glove_mode) {
+			FTS_DEBUG("enter glove mode");
+			ret = fts_ex_mode_switch(MODE_GLOVE, ENABLE);
+			if (ret >= 0) {
+				ts_data->glove_mode = ENABLE;
+			}
+		}
+	} else if (FTS_SYSFS_ECHO_OFF(buf)) {
+		if (ts_data->glove_mode) {
+			FTS_DEBUG("exit glove mode");
+			ret = fts_ex_mode_switch(MODE_GLOVE, DISABLE);
+			if (ret >= 0) {
+				ts_data->glove_mode = DISABLE;
+			}
+		}
+	}
+
+	FTS_DEBUG("glove mode:%d", ts_data->glove_mode);
+	return count;
+}
+
+
+static ssize_t fts_cover_mode_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	u8 val = 0;
+	struct fts_ts_data *ts_data = fts_data;
+	struct input_dev *input_dev = ts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	fts_read_reg(FTS_REG_COVER_MODE_EN, &val);
+	count = scnprintf(buf + count, PAGE_SIZE, "Cover Mode:%s\n",
+			ts_data->cover_mode ? "On" : "Off");
+	count += scnprintf(buf + count, PAGE_SIZE - count,
+			"Cover Reg(0xC1):%d\n", val);
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_cover_mode_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = fts_data;
+
+	if (FTS_SYSFS_ECHO_ON(buf)) {
+		if (!ts_data->cover_mode) {
+			FTS_DEBUG("enter cover mode");
+			ret = fts_ex_mode_switch(MODE_COVER, ENABLE);
+			if (ret >= 0) {
+				ts_data->cover_mode = ENABLE;
+			}
+		}
+	} else if (FTS_SYSFS_ECHO_OFF(buf)) {
+		if (ts_data->cover_mode) {
+			FTS_DEBUG("exit cover mode");
+			ret = fts_ex_mode_switch(MODE_COVER, DISABLE);
+			if (ret >= 0) {
+				ts_data->cover_mode = DISABLE;
+			}
+		}
+	}
+
+	FTS_DEBUG("cover mode:%d", ts_data->cover_mode);
+	return count;
+}
+
+static ssize_t fts_charger_mode_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	u8 val = 0;
+	struct fts_ts_data *ts_data = fts_data;
+	struct input_dev *input_dev = ts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	fts_read_reg(FTS_REG_CHARGER_MODE_EN, &val);
+	count = scnprintf(buf + count, PAGE_SIZE, "Charger Mode:%s\n",
+			ts_data->charger_mode ? "On" : "Off");
+	count += scnprintf(buf + count, PAGE_SIZE - count,
+			"Charger Reg(0x8B):%d\n", val);
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_charger_mode_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = fts_data;
+
+	if (FTS_SYSFS_ECHO_ON(buf)) {
+		if (!ts_data->charger_mode) {
+			FTS_DEBUG("enter charger mode");
+			ret = fts_ex_mode_switch(MODE_CHARGER, ENABLE);
+			if (ret >= 0) {
+				ts_data->charger_mode = ENABLE;
+			}
+		}
+	} else if (FTS_SYSFS_ECHO_OFF(buf)) {
+		if (ts_data->charger_mode) {
+			FTS_DEBUG("exit charger mode");
+			ret = fts_ex_mode_switch(MODE_CHARGER, DISABLE);
+			if (ret >= 0) {
+				ts_data->charger_mode = DISABLE;
+			}
+		}
+	}
+
+	FTS_DEBUG("charger mode:%d", ts_data->glove_mode);
+	return count;
+}
+
+static ssize_t fts_report_rate_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	u8 val = 0;
+	struct fts_ts_data *ts_data = fts_data;
+	struct input_dev *input_dev = ts_data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+	fts_read_reg(FTS_REG_REPORT_RATE, &val);
+	count = scnprintf(buf + count, PAGE_SIZE,
+			"Report Rate:%d\n", ts_data->report_rate);
+	count += scnprintf(buf + count, PAGE_SIZE - count,
+			"Report Rate Reg(0x88):%d\n", val);
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_report_rate_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret = 0;
+	struct fts_ts_data *ts_data = fts_data;
+	int rate;
+
+	ret = kstrtoint(buf, 16, &rate);
+	if (ret)
+		return ret;
+
+	if (rate != ts_data->report_rate) {
+		ret = fts_ex_mode_switch(REPORT_RATE, (u8)rate);
+		if (ret >= 0)
+			ts_data->report_rate = rate;
+	}
+
+	FTS_DEBUG("report rate:%d", ts_data->report_rate);
+	return count;
+}
+
+
+/* read and write charger mode
+ * read example: cat fts_glove_mode        ---read  glove mode
+ * write example:echo 1 > fts_glove_mode   ---write glove mode to 01
+ */
+static DEVICE_ATTR(fts_glove_mode, S_IRUGO | S_IWUSR,
+			fts_glove_mode_show, fts_glove_mode_store);
+
+static DEVICE_ATTR(fts_cover_mode, S_IRUGO | S_IWUSR,
+			fts_cover_mode_show, fts_cover_mode_store);
+
+static DEVICE_ATTR(fts_charger_mode, S_IRUGO | S_IWUSR,
+			fts_charger_mode_show, fts_charger_mode_store);
+
+static DEVICE_ATTR_RW(fts_report_rate);
+
+static struct attribute *fts_touch_mode_attrs[] = {
+	&dev_attr_fts_glove_mode.attr,
+	&dev_attr_fts_cover_mode.attr,
+	&dev_attr_fts_charger_mode.attr,
+	&dev_attr_fts_report_rate.attr,
+	NULL,
+};
+
+static struct attribute_group fts_touch_mode_group = {
+	.attrs = fts_touch_mode_attrs,
+};
+
+int fts_ex_mode_recovery(struct fts_ts_data *ts_data)
+{
+	if (ts_data->glove_mode) {
+		fts_ex_mode_switch(MODE_GLOVE, ENABLE);
+	}
+
+	if (ts_data->cover_mode) {
+		fts_ex_mode_switch(MODE_COVER, ENABLE);
+	}
+
+	if (ts_data->charger_mode) {
+		fts_ex_mode_switch(MODE_CHARGER, ENABLE);
+	}
+
+	if (ts_data->report_rate > 0)
+		fts_ex_mode_switch(REPORT_RATE, ts_data->report_rate);
+
+	return 0;
+}
+
+int fts_ex_mode_init(struct fts_ts_data *ts_data)
+{
+	int ret = 0;
+
+	ts_data->glove_mode = DISABLE;
+	ts_data->cover_mode = DISABLE;
+	ts_data->charger_mode = DISABLE;
+	ts_data->report_rate = 0;
+
+	ret = sysfs_create_group(&ts_data->dev->kobj, &fts_touch_mode_group);
+	if (ret < 0) {
+		FTS_ERROR("create sysfs(ex_mode) fail");
+		sysfs_remove_group(&ts_data->dev->kobj, &fts_touch_mode_group);
+		return ret;
+	} else {
+		FTS_DEBUG("create sysfs(ex_mode) succeedfully");
+	}
+
+	return 0;
+}
+
+int fts_ex_mode_exit(struct fts_ts_data *ts_data)
+{
+	sysfs_remove_group(&ts_data->dev->kobj, &fts_touch_mode_group);
+	return 0;
+}

focaltech_touch/focaltech_flash.c

@@ -0,0 +1,2079 @@
+/*
+ *
+ * FocalTech fts TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, Focaltech Ltd. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*****************************************************************************
+*
+* File Name: focaltech_flash.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-08
+*
+* Abstract:
+*
+* Reference:
+*
+*****************************************************************************/
+
+/*****************************************************************************
+* 1.Included header files
+*****************************************************************************/
+#include "focaltech_core.h"
+#include "focaltech_flash.h"
+
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define FTS_FW_REQUEST_SUPPORT                      1
+/* Example: focaltech_ts_fw_tianma.bin */
+#define FTS_FW_NAME_PREX_WITH_REQUEST               "focaltech_ts_fw_"
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+u8 fw_file[1] = {
+0,
+};
+
+struct upgrade_module module_list[] = {
+	{FTS_MODULE_ID, FTS_MODULE_NAME, fw_file, sizeof(fw_file)},
+};
+
+struct upgrade_func *upgrade_func_list[] = {
+	&upgrade_func_ft5452,
+	&upgrade_func_ft5652,
+};
+
+struct fts_upgrade *fwupgrade;
+
+/*****************************************************************************
+* Static function prototypes
+*****************************************************************************/
+static bool fts_fwupg_check_state(
+	struct fts_upgrade *upg, enum FW_STATUS rstate);
+
+/************************************************************************
+* Name: fts_fwupg_get_boot_state
+* Brief: read boot id(rom/pram/bootloader), confirm boot environment
+* Input:
+* Output:
+* Return: return 0 if success, otherwise return error code
+***********************************************************************/
+static int fts_fwupg_get_boot_state(
+	struct fts_upgrade *upg,
+	enum FW_STATUS *fw_sts)
+{
+	int ret = 0;
+	u8 cmd[4] = { 0 };
+	u32 cmd_len = 0;
+	u8 val[2] = { 0 };
+	struct ft_chip_t *ids = NULL;
+
+	FTS_INFO("**********read boot id**********");
+	if ((!upg) || (!upg->func) || (!upg->ts_data) || (!fw_sts)) {
+		FTS_ERROR("upg/func/ts_data/fw_sts is null");
+		return -EINVAL;
+	}
+
+	if (upg->func->hid_supported)
+		fts_hid2std();
+
+	cmd[0] = FTS_CMD_START1;
+	cmd[1] = FTS_CMD_START2;
+	if (upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0)
+		cmd_len = 1;
+	else
+		cmd_len = 2;
+	ret = fts_write(cmd, cmd_len);
+	if (ret < 0) {
+		FTS_ERROR("write 55 cmd fail");
+		return ret;
+	}
+
+	msleep(FTS_CMD_START_DELAY);
+	cmd[0] = FTS_CMD_READ_ID;
+	cmd[1] = cmd[2] = cmd[3] = 0x00;
+	if (fts_data->ic_info.is_incell ||
+		(upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0))
+		cmd_len = FTS_CMD_READ_ID_LEN_INCELL;
+	else
+		cmd_len = FTS_CMD_READ_ID_LEN;
+	ret = fts_read(cmd, cmd_len, val, 2);
+	if (ret < 0) {
+		FTS_ERROR("write 90 cmd fail");
+		return ret;
+	}
+	FTS_INFO("read boot id:0x%02x%02x", val[0], val[1]);
+
+	ids = &upg->ts_data->ic_info.ids;
+	if ((val[0] == ids->rom_idh) && (val[1] == ids->rom_idl)) {
+		FTS_INFO("tp run in romboot");
+		*fw_sts = FTS_RUN_IN_ROM;
+	} else if ((val[0] == ids->pb_idh) && (val[1] == ids->pb_idl)) {
+		FTS_INFO("tp run in pramboot");
+		*fw_sts = FTS_RUN_IN_PRAM;
+	} else if ((val[0] == ids->bl_idh) && (val[1] == ids->bl_idl)) {
+		FTS_INFO("tp run in bootloader");
+		*fw_sts = FTS_RUN_IN_BOOTLOADER;
+	}
+
+	return 0;
+}
+
+static int fts_fwupg_reset_to_boot(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	u8 reg = FTS_REG_UPGRADE;
+
+	FTS_INFO("send 0xAA and 0x55 to FW, reset to boot environment");
+	if (upg && upg->func && upg->func->is_reset_register_BC) {
+		reg = FTS_REG_UPGRADE2;
+	}
+
+	ret = fts_write_reg(reg, FTS_UPGRADE_AA);
+	if (ret < 0) {
+		FTS_ERROR("write FC=0xAA fail");
+		return ret;
+	}
+	msleep(FTS_DELAY_UPGRADE_AA);
+
+	ret = fts_write_reg(reg, FTS_UPGRADE_55);
+	if (ret < 0) {
+		FTS_ERROR("write FC=0x55 fail");
+		return ret;
+	}
+
+	msleep(FTS_DELAY_UPGRADE_RESET);
+	return 0;
+}
+
+/************************************************************************
+* Name: fts_fwupg_reset_to_romboot
+* Brief: reset to romboot, to load pramboot
+* Input:
+* Output:
+* Return: return 0 if success, otherwise return error code
+***********************************************************************/
+static int fts_fwupg_reset_to_romboot(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	int i = 0;
+	u8 cmd = FTS_CMD_RESET;
+	enum FW_STATUS state = FTS_RUN_IN_ERROR;
+
+	ret = fts_write(&cmd, 1);
+	if (ret < 0) {
+		FTS_ERROR("pram/rom/bootloader reset cmd write fail");
+		return ret;
+	}
+	mdelay(10);
+
+	for (i = 0; i < FTS_UPGRADE_LOOP; i++) {
+		ret = fts_fwupg_get_boot_state(upg, &state);
+		if (FTS_RUN_IN_ROM == state)
+			break;
+		mdelay(5);
+	}
+	if (i >= FTS_UPGRADE_LOOP) {
+		FTS_ERROR("reset to romboot fail");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static u16 fts_crc16_calc_host(u8 *pbuf, u32 length)
+{
+	u16 ecc = 0;
+	u32 i = 0;
+	u32 j = 0;
+
+	for ( i = 0; i < length; i += 2 ) {
+		ecc ^= ((pbuf[i] << 8) | (pbuf[i + 1]));
+		for (j = 0; j < 16; j ++) {
+			if (ecc & 0x01)
+				ecc = (u16)((ecc >> 1) ^ AL2_FCS_COEF);
+			else
+				ecc >>= 1;
+		}
+	}
+
+	return ecc;
+}
+
+static u16 fts_pram_ecc_calc_host(u8 *pbuf, u32 length)
+{
+	return fts_crc16_calc_host(pbuf, length);
+}
+
+static int fts_pram_ecc_cal_algo(
+	struct fts_upgrade *upg,
+	u32 start_addr,
+	u32 ecc_length)
+{
+	int ret = 0;
+	int i = 0;
+	int ecc = 0;
+	u8 val[2] = { 0 };
+	u8 tmp = 0;
+	u8 cmd[FTS_ROMBOOT_CMD_ECC_NEW_LEN] = { 0 };
+
+	FTS_INFO("read out pramboot checksum");
+	if ((!upg) || (!upg->func)) {
+		FTS_ERROR("upg/func is null");
+		return -EINVAL;
+	}
+
+	cmd[0] = FTS_ROMBOOT_CMD_ECC;
+	cmd[1] = BYTE_OFF_16(start_addr);
+	cmd[2] = BYTE_OFF_8(start_addr);
+	cmd[3] = BYTE_OFF_0(start_addr);
+	cmd[4] = BYTE_OFF_16(ecc_length);
+	cmd[5] = BYTE_OFF_8(ecc_length);
+	cmd[6] = BYTE_OFF_0(ecc_length);
+	ret = fts_write(cmd, FTS_ROMBOOT_CMD_ECC_NEW_LEN);
+	if (ret < 0) {
+		FTS_ERROR("write pramboot ecc cal cmd fail");
+		return ret;
+	}
+
+	cmd[0] = FTS_ROMBOOT_CMD_ECC_FINISH;
+	for (i = 0; i < FTS_ECC_FINISH_TIMEOUT; i++) {
+		msleep(1);
+		ret = fts_read(cmd, 1, val, 1);
+		if (ret < 0) {
+			FTS_ERROR("ecc_finish read cmd fail");
+			return ret;
+		}
+		if (upg->func->new_return_value_from_ic ||
+			(upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0)) {
+			tmp = FTS_ROMBOOT_CMD_ECC_FINISH_OK_A5;
+		} else {
+			tmp = FTS_ROMBOOT_CMD_ECC_FINISH_OK_00;
+		}
+		if (tmp == val[0])
+			break;
+	}
+	if (i >= FTS_ECC_FINISH_TIMEOUT) {
+		FTS_ERROR("wait ecc finish fail");
+		return -EIO;
+	}
+
+	cmd[0] = FTS_ROMBOOT_CMD_ECC_READ;
+	ret = fts_read(cmd, 1, val, 2);
+	if (ret < 0) {
+		FTS_ERROR("read pramboot ecc fail");
+		return ret;
+	}
+
+	ecc = ((u16)(val[0] << 8) + val[1]) & 0x0000FFFF;
+	return ecc;
+}
+
+static int fts_pram_ecc_cal_xor(void)
+{
+	int ret = 0;
+	u8 reg_val = 0;
+
+	FTS_INFO("read out pramboot checksum");
+
+	ret = fts_read_reg(FTS_ROMBOOT_CMD_ECC, &reg_val);
+	if (ret < 0) {
+		FTS_ERROR("read pramboot ecc fail");
+		return ret;
+	}
+
+	return (int)reg_val;
+}
+
+static int fts_pram_ecc_cal(struct fts_upgrade *upg, u32 saddr, u32 len)
+{
+	if ((!upg) || (!upg->func)) {
+		FTS_ERROR("upg/func is null");
+		return -EINVAL;
+	}
+
+	if ((upg->func->pram_ecc_check_mode == ECC_CHECK_MODE_CRC16) ||
+		(upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0))
+		return fts_pram_ecc_cal_algo(upg, saddr, len);
+
+	return fts_pram_ecc_cal_xor();
+}
+
+static int fts_pram_write_buf(struct fts_upgrade *upg, u8 *buf, u32 len)
+{
+	int ret = 0;
+	u32 i = 0;
+	u32 j = 0;
+	u32 offset = 0;
+	u32 remainder = 0;
+	u32 packet_number;
+	u32 packet_len = 0;
+	u8 packet_buf[FTS_FLASH_PACKET_LENGTH + FTS_CMD_WRITE_LEN] = { 0 };
+	u8 ecc_tmp = 0;
+	int ecc_in_host = 0;
+	u32 cmdlen = 0;
+
+	FTS_INFO("write pramboot to pram");
+	if ((!upg) || (!upg->func) || !buf) {
+		FTS_ERROR("upg/func/buf is null");
+		return -EINVAL;
+	}
+
+	FTS_INFO("pramboot len=%d", len);
+	if ((len < PRAMBOOT_MIN_SIZE) || (len > PRAMBOOT_MAX_SIZE)) {
+		FTS_ERROR("pramboot length(%d) fail", len);
+		return -EINVAL;
+	}
+
+	packet_number = len / FTS_FLASH_PACKET_LENGTH;
+	remainder = len % FTS_FLASH_PACKET_LENGTH;
+	if (remainder > 0)
+		packet_number++;
+	packet_len = FTS_FLASH_PACKET_LENGTH;
+
+	for (i = 0; i < packet_number; i++) {
+		offset = i * FTS_FLASH_PACKET_LENGTH;
+		/* last packet */
+		if ((i == (packet_number - 1)) && remainder)
+			packet_len = remainder;
+
+		if (upg->ts_data->bus_type == BUS_TYPE_SPI_V2) {
+			packet_buf[0] = FTS_ROMBOOT_CMD_SET_PRAM_ADDR;
+			packet_buf[1] = BYTE_OFF_16(offset);
+			packet_buf[2] = BYTE_OFF_8(offset);
+			packet_buf[3] = BYTE_OFF_0(offset);
+
+			ret = fts_write(packet_buf, FTS_ROMBOOT_CMD_SET_PRAM_ADDR_LEN);
+			if (ret < 0) {
+				FTS_ERROR("pramboot set write address(%d) fail", i);
+				return ret;
+			}
+
+			packet_buf[0] = FTS_ROMBOOT_CMD_WRITE;
+			cmdlen = 1;
+		} else {
+			packet_buf[0] = FTS_ROMBOOT_CMD_WRITE;
+			packet_buf[1] = BYTE_OFF_16(offset);
+			packet_buf[2] = BYTE_OFF_8(offset);
+			packet_buf[3] = BYTE_OFF_0(offset);
+
+			packet_buf[4] = BYTE_OFF_8(packet_len);
+			packet_buf[5] = BYTE_OFF_0(packet_len);
+			cmdlen = 6;
+		}
+
+		for (j = 0; j < packet_len; j++) {
+			packet_buf[cmdlen + j] = buf[offset + j];
+			if (upg->func->pram_ecc_check_mode == ECC_CHECK_MODE_XOR)
+				ecc_tmp ^= packet_buf[cmdlen + j];
+		}
+
+		ret = fts_write(packet_buf, packet_len + cmdlen);
+		if (ret < 0) {
+			FTS_ERROR("pramboot write data(%d) fail", i);
+			return ret;
+		}
+	}
+
+	if ((upg->func->pram_ecc_check_mode == ECC_CHECK_MODE_CRC16) ||
+		(upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0))
+		ecc_in_host = (int)fts_pram_ecc_calc_host(buf, len);
+	else
+		ecc_in_host = (int)ecc_tmp;
+
+	return ecc_in_host;
+}
+
+static int fts_pram_start(void)
+{
+	u8 cmd = FTS_ROMBOOT_CMD_START_APP;
+	int ret = 0;
+
+	FTS_INFO("remap to start pramboot");
+
+	ret = fts_write(&cmd, 1);
+	if (ret < 0) {
+		FTS_ERROR("write start pram cmd fail");
+		return ret;
+	}
+	msleep(FTS_DELAY_PRAMBOOT_START);
+
+	return 0;
+}
+
+static int fts_pram_write_remap(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	int ecc_in_host = 0;
+	int ecc_in_tp = 0;
+	u8 *pb_buf = NULL;
+	u32 pb_len = 0;
+
+	FTS_INFO("write pram and remap");
+	if (!upg || !upg->func || !upg->func->pramboot) {
+		FTS_ERROR("upg/func/pramboot is null");
+		return -EINVAL;
+	}
+
+	if (upg->func->pb_length < FTS_MIN_LEN) {
+		FTS_ERROR("pramboot length(%d) fail", upg->func->pb_length);
+		return -EINVAL;
+	}
+
+	pb_buf = upg->func->pramboot;
+	pb_len = upg->func->pb_length;
+
+	/* write pramboot to pram */
+	ecc_in_host = fts_pram_write_buf(upg, pb_buf, pb_len);
+	if (ecc_in_host < 0) {
+		FTS_ERROR( "write pramboot fail");
+		return ecc_in_host;
+	}
+
+	/* read out checksum */
+	ecc_in_tp = fts_pram_ecc_cal(upg, 0, pb_len);
+	if (ecc_in_tp < 0) {
+		FTS_ERROR( "read pramboot ecc fail");
+		return ecc_in_tp;
+	}
+
+	FTS_INFO("pram ecc in tp:%x, host:%x", ecc_in_tp, ecc_in_host);
+	/*  pramboot checksum != fw checksum, upgrade fail */
+	if (ecc_in_host != ecc_in_tp) {
+		FTS_ERROR("pramboot ecc check fail");
+		return -EIO;
+	}
+
+	/*start pram*/
+	ret = fts_pram_start();
+	if (ret < 0) {
+		FTS_ERROR("pram start fail");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int fts_pram_init(void)
+{
+	int ret = 0;
+	u8 reg_val = 0;
+	u8 wbuf[3] = { 0 };
+
+	FTS_INFO("pramboot initialization");
+
+	/* read flash ID */
+	wbuf[0] = FTS_CMD_FLASH_TYPE;
+	ret = fts_read(wbuf, 1, &reg_val, 1);
+	if (ret < 0) {
+		FTS_ERROR("read flash type fail");
+		return ret;
+	}
+
+	/* set flash clk */
+	wbuf[0] = FTS_CMD_FLASH_TYPE;
+	wbuf[1] = reg_val;
+	wbuf[2] = 0x00;
+	ret = fts_write(wbuf, 3);
+	if (ret < 0) {
+		FTS_ERROR("write flash type fail");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int fts_pram_write_init(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	bool state = 0;
+	enum FW_STATUS status = FTS_RUN_IN_ERROR;
+
+	FTS_INFO("**********pram write and init**********");
+	if ((NULL == upg) || (NULL == upg->func)) {
+		FTS_ERROR("upgrade/func is null");
+		return -EINVAL;
+	}
+
+	if (!upg->func->pramboot_supported) {
+		FTS_ERROR("ic not support pram");
+		return -EINVAL;
+	}
+
+	FTS_DEBUG("check whether tp is in romboot or not ");
+	/* need reset to romboot when non-romboot state */
+	ret = fts_fwupg_get_boot_state(upg, &status);
+	if (status != FTS_RUN_IN_ROM) {
+		if (FTS_RUN_IN_PRAM == status) {
+			FTS_INFO("tp is in pramboot, need send reset cmd before upgrade");
+			ret = fts_pram_init();
+			if (ret < 0) {
+				FTS_ERROR("pramboot(before) init fail");
+				return ret;
+			}
+		}
+
+		FTS_INFO("tp isn't in romboot, need send reset to romboot");
+		ret = fts_fwupg_reset_to_romboot(upg);
+		if (ret < 0) {
+			FTS_ERROR("reset to romboot fail");
+			return ret;
+		}
+	}
+
+	/* check the length of the pramboot */
+	ret = fts_pram_write_remap(upg);
+	if (ret < 0) {
+		FTS_ERROR("pram write fail, ret=%d", ret);
+		return ret;
+	}
+
+	FTS_DEBUG("after write pramboot, confirm run in pramboot");
+	state = fts_fwupg_check_state(upg, FTS_RUN_IN_PRAM);
+	if (!state) {
+		FTS_ERROR("not in pramboot");
+		return -EIO;
+	}
+
+	ret = fts_pram_init();
+	if (ret < 0) {
+		FTS_ERROR("pramboot init fail");
+		return ret;
+	}
+
+	return 0;
+}
+
+static bool fts_fwupg_check_fw_valid(void)
+{
+	int ret = 0;
+
+	ret = fts_wait_tp_to_valid();
+	if (ret < 0) {
+		FTS_INFO("tp fw invaild");
+		return false;
+	}
+
+	FTS_INFO("tp fw vaild");
+	return true;
+}
+
+/************************************************************************
+* Name: fts_fwupg_check_state
+* Brief: confirm tp run in which mode: romboot/pramboot/bootloader
+* Input:
+* Output:
+* Return: return true if state is match, otherwise return false
+***********************************************************************/
+static bool fts_fwupg_check_state(
+	struct fts_upgrade *upg, enum FW_STATUS rstate)
+{
+	int ret = 0;
+	int i = 0;
+	enum FW_STATUS cstate = FTS_RUN_IN_ERROR;
+
+	for (i = 0; i < FTS_UPGRADE_LOOP; i++) {
+		ret = fts_fwupg_get_boot_state(upg, &cstate);
+		/* FTS_DEBUG("fw state=%d, retries=%d", cstate, i); */
+		if (cstate == rstate)
+			return true;
+		msleep(FTS_DELAY_READ_ID);
+	}
+
+	return false;
+}
+
+/************************************************************************
+* Name: fts_fwupg_reset_in_boot
+* Brief: RST CMD(07), reset to romboot(bootloader) in boot environment
+* Input:
+* Output:
+* Return: return 0 if success, otherwise return error code
+***********************************************************************/
+int fts_fwupg_reset_in_boot(void)
+{
+	int ret = 0;
+	u8 cmd = FTS_CMD_RESET;
+
+	FTS_INFO("reset in boot environment");
+	ret = fts_write(&cmd, 1);
+	if (ret < 0) {
+		FTS_ERROR("pram/rom/bootloader reset cmd write fail");
+		return ret;
+	}
+
+	msleep(FTS_DELAY_UPGRADE_RESET);
+	return 0;
+}
+
+static int fts_fwupg_enter_into_boot_old(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	bool fwvalid = false;
+	bool state = false;
+
+	FTS_INFO("***********enter into pramboot/bootloader***********");
+	if ((!upg) || (NULL == upg->func)) {
+		FTS_ERROR("upgrade/func is null");
+		return -EINVAL;
+	}
+
+	fwvalid = fts_fwupg_check_fw_valid();
+	if (fwvalid) {
+		ret = fts_fwupg_reset_to_boot(upg);
+		if (ret < 0) {
+			FTS_ERROR("enter into romboot/bootloader fail");
+			return ret;
+		}
+	} else if (upg->func->read_boot_id_need_reset) {
+		ret = fts_fwupg_reset_in_boot();
+		if (ret < 0) {
+			FTS_ERROR("reset before read boot id when fw invalid fail");
+			return ret;
+		}
+	}
+
+	if (upg->func->pramboot_supported) {
+		FTS_INFO("pram supported, write pramboot and init");
+		/* pramboot */
+		ret = fts_pram_write_init(upg);
+		if (ret < 0) {
+			FTS_ERROR("pram write_init fail");
+			return ret;
+		}
+	} else {
+		FTS_DEBUG("pram not supported, confirm in bootloader");
+		/* bootloader */
+		state = fts_fwupg_check_state(upg, FTS_RUN_IN_BOOTLOADER);
+		if (!state) {
+			FTS_ERROR("fw not in bootloader, fail");
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int fts_hardware_reset_to_boot(void)
+{
+	u8 cmd[2] = {0};
+	int i = 0;
+	u8 chip_id[2] = { 0 };
+	u8 id_cmd[4] = { 0 };
+	int ret = 0;
+
+	fts_reset_proc(0);
+	FTS_INFO("reset finish!!");
+	usleep_range(10000, 11000);
+
+	for (i = 0; i < 20; i++) {
+		FTS_INFO("start send 55 AA");
+		cmd[0] = 0x55;
+		cmd[1] = 0xAA;
+		ret = fts_write(cmd, 2);
+		if (ret < 0)
+			FTS_ERROR("send 55 AA cmd fail");
+
+		usleep_range(8000, 9000);
+		FTS_INFO("start send 90 00 00 00");
+		id_cmd[0] = 0x90;
+		id_cmd[1] = id_cmd[2] = id_cmd[3] = 0x00;
+		ret = fts_read(id_cmd, 1, chip_id, 2);
+		if (ret < 0)
+			FTS_ERROR("send 90 cmd fail");
+
+		FTS_INFO("read boot id = %x--%x", chip_id[0], chip_id[1]);
+		if (chip_id[1] == 0xB2)
+			break;
+
+		usleep_range(1000, 2000);
+
+		if (i == 10) {
+			fts_reset_proc(0);
+			FTS_INFO("reset again finish!!");
+			usleep_range(10000, 11000);
+		}
+
+	}
+
+	if (i >= 20) {
+		FTS_INFO("enter into romboot fail");
+		return -EIO;
+	}
+	return 0;
+
+}
+
+static int fts_fwupg_enter_into_boot_new(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	bool state = false;
+
+	FTS_INFO("***********enter into pramboot/bootloader***********");
+	if ((!upg) || (upg->func == NULL)) {
+		FTS_ERROR("upgrade/func is null");
+		return -EINVAL;
+	}
+
+	ret = fts_hardware_reset_to_boot();
+	if (!ret) {
+		FTS_INFO("tp run in bootloader success");
+		return ret;
+	}
+
+	if (upg->func->pramboot_supported) {
+		FTS_INFO("pram supported, write pramboot and init");
+		/* pramboot */
+		if (upg->func->write_pramboot_private)
+			ret = upg->func->write_pramboot_private();
+		else
+			ret = fts_pram_write_init(upg);
+
+		if (ret < 0) {
+			FTS_ERROR("pram write_init fail");
+			return ret;
+		}
+	} else {
+		FTS_DEBUG("pram not supported, confirm in bootloader");
+		/* bootloader */
+		state = fts_fwupg_check_state(upg, FTS_RUN_IN_BOOTLOADER);
+		if (!state) {
+			FTS_ERROR("fw not in bootloader, fail");
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+int fts_fwupg_enter_into_boot(void)
+{
+	struct fts_upgrade *upg = fwupgrade;
+
+	if (upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0)
+		return fts_fwupg_enter_into_boot_new(upg);
+
+	return fts_fwupg_enter_into_boot_old(upg);
+}
+
+/************************************************************************
+ * Name: fts_fwupg_check_flash_status
+ * Brief: read status from tp
+ * Input: flash_status: correct value from tp
+ *        retries: read retry times
+ *        retries_delay: retry delay
+ * Output:
+ * Return: return true if flash status check pass, otherwise return false
+ ***********************************************************************/
+static bool fts_fwupg_check_flash_status(
+	u16 flash_status,
+	int retries,
+	int retries_delay)
+{
+	int ret = 0;
+	int i = 0;
+	u8 cmd = 0;
+	u8 val[FTS_CMD_FLASH_STATUS_LEN] = { 0 };
+	u16 read_status = 0;
+
+	for (i = 0; i < retries; i++) {
+		cmd = FTS_CMD_FLASH_STATUS;
+		ret = fts_read(&cmd , 1, val, FTS_CMD_FLASH_STATUS_LEN);
+		read_status = (((u16)val[0]) << 8) + val[1];
+		if (flash_status == read_status) {
+			/* FTS_DEBUG("[UPGRADE]flash status ok"); */
+			return true;
+		}
+		/* FTS_DEBUG("flash status fail,ok:%04x read:%04x, retries:%d", flash_status, read_status, i); */
+		msleep(retries_delay);
+	}
+
+	return false;
+}
+
+/************************************************************************
+ * Name: fts_fwupg_erase
+ * Brief: erase flash area
+ * Input: delay - delay after erase
+ * Output:
+ * Return: return 0 if success, otherwise return error code
+ ***********************************************************************/
+int fts_fwupg_erase(u32 delay)
+{
+	int ret = 0;
+	u8 cmd = 0;
+	bool flag = false;
+
+	FTS_INFO("**********erase now**********");
+
+	/*send to erase flash*/
+	cmd = FTS_CMD_ERASE_APP;
+	ret = fts_write(&cmd, 1);
+	if (ret < 0) {
+		FTS_ERROR("erase cmd fail");
+		return ret;
+	}
+	msleep(delay);
+
+	/* read status 0xF0AA: success */
+	flag = fts_fwupg_check_flash_status(FTS_CMD_FLASH_STATUS_ERASE_OK,
+						FTS_RETRIES_REASE,
+						FTS_RETRIES_DELAY_REASE);
+	if (!flag) {
+		FTS_ERROR("ecc flash status check fail");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/************************************************************************
+ * Name: fts_fwupg_ecc_cal
+ * Brief: calculate and get ecc from tp
+ * Input: saddr - start address need calculate ecc
+ *        len - length need calculate ecc
+ * Output:
+ * Return: return data ecc of tp if success, otherwise return error code
+ ***********************************************************************/
+int fts_fwupg_ecc_cal(u32 saddr, u32 len)
+{
+	int ret = 0;
+	u32 i = 0;
+	u32 cmdlen = FTS_CMD_ECC_CAL_LEN;
+	u8 wbuf[FTS_CMD_ECC_CAL_LEN] = { 0 };
+	u8 val[FTS_CMD_FLASH_STATUS_LEN] = { 0 };
+	int ecc = 0;
+	int ecc_len = 0;
+	u32 packet_num = 0;
+	u32 packet_len = 0;
+	u32 remainder = 0;
+	u32 addr = 0;
+	u32 offset = 0;
+	bool bflag = false;
+	struct fts_upgrade *upg = fwupgrade;
+
+	FTS_INFO( "**********read out checksum**********");
+	if ((NULL == upg) || (NULL == upg->func)) {
+		FTS_ERROR("upgrade/func is null");
+		return -EINVAL;
+	}
+
+	/* check sum init */
+	wbuf[0] = FTS_CMD_ECC_INIT;
+	ret = fts_write(wbuf, 1);
+	if (ret < 0) {
+		FTS_ERROR("ecc init cmd write fail");
+		return ret;
+	}
+
+	if (upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0) {
+		packet_num = 1;
+		remainder = 0;
+		packet_len = len;
+	} else {
+		packet_num = len / FTS_MAX_LEN_ECC_CALC;
+		remainder = len % FTS_MAX_LEN_ECC_CALC;
+		if (remainder)
+			packet_num++;
+		packet_len = FTS_MAX_LEN_ECC_CALC;
+	}
+	FTS_INFO("ecc calc num:%d, remainder:%d", packet_num, remainder);
+
+	/* send commond to start checksum */
+	wbuf[0] = FTS_CMD_ECC_CAL;
+	for (i = 0; i < packet_num; i++) {
+		offset = FTS_MAX_LEN_ECC_CALC * i;
+		addr = saddr + offset;
+		wbuf[1] = BYTE_OFF_16(addr);
+		wbuf[2] = BYTE_OFF_8(addr);
+		wbuf[3] = BYTE_OFF_0(addr);
+
+		if (upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0) {
+			wbuf[4] = BYTE_OFF_16(packet_len);
+			wbuf[5] = BYTE_OFF_8(packet_len);
+			wbuf[6] = BYTE_OFF_0(packet_len);
+			cmdlen = FTS_CMD_ECC_CAL_LEN;
+		} else {
+			if ((i == (packet_num - 1)) && remainder)
+				packet_len = remainder;
+			wbuf[4] = BYTE_OFF_8(packet_len);
+			wbuf[5] = BYTE_OFF_0(packet_len);
+			cmdlen = FTS_CMD_ECC_CAL_LEN - 1;
+		}
+
+		FTS_DEBUG("ecc calc startaddr:0x%04x, len:%d", addr, packet_len);
+		ret = fts_write(wbuf, cmdlen);
+		if (ret < 0) {
+			FTS_ERROR("ecc calc cmd write fail");
+			return ret;
+		}
+
+		msleep(packet_len / 256);
+
+		/* read status if check sum is finished */
+		bflag = fts_fwupg_check_flash_status(FTS_CMD_FLASH_STATUS_ECC_OK,
+						FTS_RETRIES_ECC_CAL,
+						FTS_RETRIES_DELAY_ECC_CAL);
+		if (!bflag) {
+			FTS_ERROR("ecc flash status read fail");
+			return -EIO;
+		}
+	}
+
+	ecc_len = 1;
+	if ((upg->func->fw_ecc_check_mode == ECC_CHECK_MODE_CRC16) ||
+		(upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0))
+		ecc_len = 2;
+
+	/* read out check sum */
+	wbuf[0] = FTS_CMD_ECC_READ;
+	ret = fts_read(wbuf, 1, val, ecc_len);
+	if (ret < 0) {
+		FTS_ERROR( "ecc read cmd write fail");
+		return ret;
+	}
+
+	if ((upg->func->fw_ecc_check_mode == ECC_CHECK_MODE_CRC16) ||
+		(upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0))
+		ecc = (int)((u16)(val[0] << 8) + val[1]);
+	else
+		ecc = (int)val[0];
+
+	return ecc;
+}
+
+/************************************************************************
+ * Name: fts_flash_write_buf
+ * Brief: write buf data to flash address
+ * Input: saddr - start address data write to flash
+ *        buf - data buffer
+ *        len - data length
+ *        delay - delay after write
+ * Output:
+ * Return: return data ecc of host if success, otherwise return error code
+ ***********************************************************************/
+int fts_flash_write_buf(
+	u32 saddr,
+	u8 *buf,
+	u32 len,
+	u32 delay)
+{
+	int ret = 0;
+	u32 i = 0;
+	u32 j = 0;
+	u32 packet_number = 0;
+	u32 packet_len = 0;
+	u32 addr = 0;
+	u32 offset = 0;
+	u32 remainder = 0;
+	u32 cmdlen = 0;
+	u8 packet_buf[FTS_FLASH_PACKET_LENGTH + FTS_CMD_WRITE_LEN] = { 0 };
+	u8 ecc_tmp = 0;
+	int ecc_in_host = 0;
+	u8 cmd = 0;
+	u8 val[FTS_CMD_FLASH_STATUS_LEN] = { 0 };
+	u16 read_status = 0;
+	u16 wr_ok = 0;
+	struct fts_upgrade *upg = fwupgrade;
+
+	FTS_INFO( "**********write data to flash**********");
+	if ((!upg) || (!upg->func || !buf || !len)) {
+		FTS_ERROR("upgrade/func/buf/len is invalid");
+		return -EINVAL;
+	}
+
+	FTS_INFO("data buf start addr=0x%x, len=0x%x", saddr, len);
+	packet_number = len / FTS_FLASH_PACKET_LENGTH;
+	remainder = len % FTS_FLASH_PACKET_LENGTH;
+	if (remainder > 0)
+		packet_number++;
+	packet_len = FTS_FLASH_PACKET_LENGTH;
+	FTS_INFO("write data, num:%d remainder:%d", packet_number, remainder);
+
+	for (i = 0; i < packet_number; i++) {
+		offset = i * FTS_FLASH_PACKET_LENGTH;
+		addr = saddr + offset;
+
+		/* last packet */
+		if ((i == (packet_number - 1)) && remainder)
+			packet_len = remainder;
+
+		if (upg->ts_data->bus_type == BUS_TYPE_SPI_V2) {
+			packet_buf[0] = FTS_CMD_SET_WFLASH_ADDR;
+			packet_buf[1] = BYTE_OFF_16(addr);
+			packet_buf[2] = BYTE_OFF_8(addr);
+			packet_buf[3] = BYTE_OFF_0(addr);
+			ret = fts_write(packet_buf, FTS_LEN_SET_ADDR);
+			if (ret < 0) {
+				FTS_ERROR("set flash address fail");
+				return ret;
+			}
+
+			packet_buf[0] = FTS_CMD_WRITE;
+			cmdlen = 1;
+		} else {
+			packet_buf[0] = FTS_CMD_WRITE;
+			packet_buf[1] = BYTE_OFF_16(addr);
+			packet_buf[2] = BYTE_OFF_8(addr);
+			packet_buf[3] = BYTE_OFF_0(addr);
+			packet_buf[4] = BYTE_OFF_8(packet_len);
+			packet_buf[5] = BYTE_OFF_0(packet_len);
+			cmdlen = 6;
+		}
+
+		for (j = 0; j < packet_len; j++) {
+			packet_buf[cmdlen + j] = buf[offset + j];
+			ecc_tmp ^= packet_buf[cmdlen + j];
+		}
+
+		ret = fts_write(packet_buf, packet_len + cmdlen);
+		if (ret < 0) {
+			FTS_ERROR("app write fail");
+			return ret;
+		}
+		mdelay(delay);
+
+		/* read status */
+		wr_ok = FTS_CMD_FLASH_STATUS_WRITE_OK + addr / packet_len;
+		for (j = 0; j < FTS_RETRIES_WRITE; j++) {
+			cmd = FTS_CMD_FLASH_STATUS;
+			ret = fts_read(&cmd , 1, val, FTS_CMD_FLASH_STATUS_LEN);
+			read_status = (((u16)val[0]) << 8) + val[1];
+			/*  FTS_INFO("%x %x", wr_ok, read_status); */
+			if (read_status == wr_ok)
+				break;
+
+			mdelay(FTS_RETRIES_DELAY_WRITE);
+		}
+	}
+
+	ecc_in_host = (int)ecc_tmp;
+	if ((upg->func->fw_ecc_check_mode == ECC_CHECK_MODE_CRC16) ||
+		(upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0))
+		ecc_in_host = (int)fts_crc16_calc_host(buf, len);
+
+	return ecc_in_host;
+}
+
+/************************************************************************
+ * Name: fts_flash_read_buf
+ * Brief: read data from flash
+ * Input: saddr - start address data write to flash
+ *        buf - buffer to store data read from flash
+ *        len - read length
+ * Output:
+ * Return: return 0 if success, otherwise return error code
+ *
+ * Warning: can't call this function directly, need call in boot environment
+ ***********************************************************************/
+static int fts_flash_read_buf(u32 saddr, u8 *buf, u32 len)
+{
+	int ret = 0;
+	u32 i = 0;
+	u32 packet_number = 0;
+	u32 packet_len = 0;
+	u32 addr = 0;
+	u32 offset = 0;
+	u32 remainder = 0;
+	u8 wbuf[FTS_CMD_READ_LEN_SPI] = { 0 };
+	struct fts_upgrade *upg = fwupgrade;
+
+	if (!upg || !buf || !len) {
+		FTS_ERROR("upgrade/buf is NULL or len is 0");
+		return -EINVAL;
+	}
+
+	packet_number = len / FTS_FLASH_PACKET_LENGTH;
+	remainder = len % FTS_FLASH_PACKET_LENGTH;
+	if (remainder > 0) {
+		packet_number++;
+	}
+	packet_len = FTS_FLASH_PACKET_LENGTH;
+	FTS_INFO("read packet_number:%d, remainder:%d", packet_number, remainder);
+
+
+	for (i = 0; i < packet_number; i++) {
+		offset = i * FTS_FLASH_PACKET_LENGTH;
+		addr = saddr + offset;
+		/* last packet */
+		if ((i == (packet_number - 1)) && remainder)
+			packet_len = remainder;
+
+		if (upg->ts_data->bus_type == BUS_TYPE_I2C) {
+			wbuf[0] = FTS_CMD_READ;
+			wbuf[1] = BYTE_OFF_16(addr);
+			wbuf[2] = BYTE_OFF_8(addr);
+			wbuf[3] = BYTE_OFF_0(addr);
+			ret = fts_write(wbuf, FTS_CMD_READ_LEN);
+			if (ret < 0) {
+				FTS_ERROR("pram/bootloader write 03 command fail");
+				return ret;
+			}
+
+			msleep(FTS_CMD_READ_DELAY); /* must wait, otherwise read wrong data */
+			ret = fts_read(NULL, 0, buf + offset, packet_len);
+			if (ret < 0) {
+				FTS_ERROR("pram/bootloader read 03 command fail");
+				return ret;
+			}
+		} else if (upg->ts_data->bus_type == BUS_TYPE_SPI_V2) {
+			wbuf[0] = FTS_CMD_SET_RFLASH_ADDR;
+			wbuf[1] = BYTE_OFF_16(addr);
+			wbuf[2] = BYTE_OFF_8(addr);
+			wbuf[3] = BYTE_OFF_0(addr);
+			ret = fts_write(wbuf, FTS_LEN_SET_ADDR);
+			if (ret < 0) {
+				FTS_ERROR("set flash address fail");
+				return ret;
+			}
+
+			msleep(FTS_CMD_READ_DELAY);
+			wbuf[0] = FTS_CMD_READ;
+			ret = fts_read(wbuf, 1, buf + offset, packet_len);
+			if (ret < 0) {
+				FTS_ERROR("pram/bootloader read 03(SPI_V2) command fail");
+				return ret;
+			}
+		} else if (upg->ts_data->bus_type == BUS_TYPE_SPI) {
+			wbuf[0] = FTS_CMD_READ;
+			wbuf[1] = BYTE_OFF_16(addr);
+			wbuf[2] = BYTE_OFF_8(addr);
+			wbuf[3] = BYTE_OFF_0(addr);
+			wbuf[4] = BYTE_OFF_8(packet_len);
+			wbuf[5] = BYTE_OFF_0(packet_len);
+			ret = fts_read(wbuf, FTS_CMD_READ_LEN_SPI,
+						buf + offset, packet_len);
+			if (ret < 0) {
+				FTS_ERROR("pram/bootloader read 03(SPI) command fail");
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/************************************************************************
+ * Name: fts_flash_read
+ * Brief:
+ * Input:  addr  - address of flash
+ *         len   - length of read
+ * Output: buf   - data read from flash
+ * Return: return 0 if success, otherwise return error code
+ ***********************************************************************/
+static int fts_flash_read(u32 addr, u8 *buf, u32 len)
+{
+	int ret = 0;
+
+	FTS_INFO("***********read flash***********");
+	if ((NULL == buf) || (0 == len)) {
+		FTS_ERROR("buf is NULL or len is 0");
+		return -EINVAL;
+	}
+
+	ret = fts_fwupg_enter_into_boot();
+	if (ret < 0) {
+		FTS_ERROR("enter into pramboot/bootloader fail");
+		goto read_flash_err;
+	}
+
+	ret = fts_flash_read_buf(addr, buf, len);
+	if (ret < 0) {
+		FTS_ERROR("read flash fail");
+		goto read_flash_err;
+	}
+
+read_flash_err:
+	/* reset to normal boot */
+	ret = fts_fwupg_reset_in_boot();
+	if (ret < 0)
+		FTS_ERROR("reset to normal boot fail");
+
+	return ret;
+}
+
+int fts_enter_test_environment(bool test_state)
+{
+	return 0;
+}
+#if FTS_AUTO_LIC_UPGRADE_EN
+static int fts_lic_get_vid_in_tp(u16 *vid)
+{
+	int ret = 0;
+	u8 val[2] = { 0 };
+
+	if (NULL == vid) {
+		FTS_ERROR("vid is NULL");
+		return -EINVAL;
+	}
+
+	ret = fts_read_reg(FTS_REG_VENDOR_ID, &val[0]);
+	if (fts_data->ic_info.is_incell)
+		ret = fts_read_reg(FTS_REG_MODULE_ID, &val[1]);
+	if (ret < 0) {
+		FTS_ERROR("read vid from tp fail");
+		return ret;
+	}
+
+	*vid = *(u16 *)val;
+	return 0;
+}
+
+static int fts_lic_get_vid_in_host(struct fts_upgrade *upg, u16 *vid)
+{
+	u8 val[2] = { 0 };
+	u8 *licbuf = NULL;
+	u32 conf_saddr = 0;
+
+	if (!upg || !upg->func || !upg->lic || !vid) {
+		FTS_ERROR("upgrade/func/get_hlic_ver/lic/vid is null");
+		return -EINVAL;
+	}
+
+	if (upg->lic_length < FTS_MAX_LEN_SECTOR) {
+		FTS_ERROR("lic length(%x) fail", upg->lic_length);
+		return -EINVAL;
+	}
+
+	licbuf  = upg->lic;
+	conf_saddr = upg->func->fwcfgoff;
+	val[0] = licbuf[conf_saddr + FTS_CONIFG_VENDORID_OFF];
+	if (fts_data->ic_info.is_incell)
+		val[1] = licbuf[conf_saddr + FTS_CONIFG_MODULEID_OFF];
+
+	*vid = *(u16 *)val;
+	return 0;
+}
+
+static int fts_lic_get_ver_in_tp(u8 *ver)
+{
+	int ret = 0;
+
+	if (NULL == ver) {
+		FTS_ERROR("ver is NULL");
+		return -EINVAL;
+	}
+
+	ret = fts_read_reg(FTS_REG_LIC_VER, ver);
+	if (ret < 0) {
+		FTS_ERROR("read lcd initcode ver from tp fail");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int fts_lic_get_ver_in_host(struct fts_upgrade *upg, u8 *ver)
+{
+	int ret = 0;
+
+	if (!upg || !upg->func || !upg->func->get_hlic_ver || !upg->lic) {
+		FTS_ERROR("upgrade/func/get_hlic_ver/lic is null");
+		return -EINVAL;
+	}
+
+	ret = upg->func->get_hlic_ver(upg->lic);
+	if (ret < 0) {
+		FTS_ERROR("get host lcd initial code version fail");
+		return ret;
+	}
+
+	*ver = (u8)ret;
+	return ret;
+}
+
+static bool fts_lic_need_upgrade(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	u8 initcode_ver_in_tp = 0;
+	u8 initcode_ver_in_host = 0;
+	u16 vid_in_tp = 0;
+	u16 vid_in_host = 0;
+	bool fwvalid = false;
+
+	fwvalid = fts_fwupg_check_fw_valid();
+	if ( !fwvalid) {
+		FTS_INFO("fw is invalid, no upgrade lcd init code");
+		return false;
+	}
+
+	ret = fts_lic_get_vid_in_host(upg, &vid_in_host);
+	if (ret < 0) {
+		FTS_ERROR("vendor id in host invalid");
+		return false;
+	}
+
+	ret = fts_lic_get_vid_in_tp(&vid_in_tp);
+	if (ret < 0) {
+		FTS_ERROR("vendor id in tp invalid");
+		return false;
+	}
+
+	FTS_DEBUG("vid in tp:0x%04x, host:0x%04x", vid_in_tp, vid_in_host);
+	if (vid_in_tp != vid_in_host) {
+		FTS_INFO("vendor id in tp&host are different, no upgrade lic");
+		return false;
+	}
+
+	ret = fts_lic_get_ver_in_host(upg, &initcode_ver_in_host);
+	if (ret < 0) {
+		FTS_ERROR("init code in host invalid");
+		return false;
+	}
+
+	ret = fts_lic_get_ver_in_tp(&initcode_ver_in_tp);
+	if (ret < 0) {
+		FTS_ERROR("read reg0xE4 fail");
+		return false;
+	}
+
+	FTS_DEBUG("lcd initial code version in tp:%x, host:%x",
+		initcode_ver_in_tp, initcode_ver_in_host);
+	if (0xA5 == initcode_ver_in_tp) {
+		FTS_INFO("lcd init code ver is 0xA5, don't upgade init code");
+		return false;
+	} else if (0xFF == initcode_ver_in_tp) {
+		FTS_DEBUG("lcd init code in tp is invalid, need upgrade init code");
+		return true;
+	} else if (initcode_ver_in_tp < initcode_ver_in_host)
+		return true;
+	else
+		return false;
+}
+
+static int fts_lic_upgrade(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	bool hlic_upgrade = false;
+	int upgrade_count = 0;
+	u8 ver = 0;
+
+	FTS_INFO("lcd initial code auto upgrade function");
+	if ((!upg) || (!upg->func) || (!upg->func->lic_upgrade)) {
+		FTS_ERROR("lcd upgrade function is null");
+		return -EINVAL;
+	}
+
+	hlic_upgrade = fts_lic_need_upgrade(upg);
+	FTS_INFO("lcd init code upgrade flag:%d", hlic_upgrade);
+	if (hlic_upgrade) {
+		FTS_INFO("lcd initial code need upgrade, upgrade begin...");
+		do {
+			FTS_INFO("lcd initial code upgrade times:%d", upgrade_count);
+			upgrade_count++;
+
+			ret = upg->func->lic_upgrade(upg->lic, upg->lic_length);
+			if (ret < 0) {
+				fts_fwupg_reset_in_boot();
+			} else {
+				fts_lic_get_ver_in_tp(&ver);
+				FTS_INFO("success upgrade to lcd initcode ver:%02x", ver);
+				break;
+			}
+		} while (upgrade_count < 2);
+	} else {
+		FTS_INFO("lcd initial code don't need upgrade");
+	}
+
+	return ret;
+}
+#endif /* FTS_AUTO_LIC_UPGRADE_EN */
+
+
+static int fts_param_get_ver_in_tp(u8 *ver)
+{
+	int ret = 0;
+
+	if (NULL == ver) {
+		FTS_ERROR("ver is NULL");
+		return -EINVAL;
+	}
+
+	ret = fts_read_reg(FTS_REG_IDE_PARA_VER_ID, ver);
+	if (ret < 0) {
+		FTS_ERROR("read fw param ver from tp fail");
+		return ret;
+	}
+
+	if ((0x00 == *ver) || (0xFF == *ver)) {
+		FTS_INFO("param version in tp invalid");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int fts_param_get_ver_in_host(struct fts_upgrade *upg, u8 *ver)
+{
+	if ((!upg) || (!upg->func) || (!upg->fw) || (!ver)) {
+		FTS_ERROR("fts_data/upgrade/func/fw/ver is NULL");
+		return -EINVAL;
+	}
+
+	if (upg->fw_length < upg->func->paramcfgveroff) {
+		FTS_ERROR("fw len(%x) < paramcfg ver offset(%x)",
+			upg->fw_length, upg->func->paramcfgveroff);
+		return -EINVAL;
+	}
+
+	FTS_INFO("fw paramcfg version offset:%x", upg->func->paramcfgveroff);
+	*ver = upg->fw[upg->func->paramcfgveroff];
+
+	if ((0x00 == *ver) || (0xFF == *ver)) {
+		FTS_INFO("param version in host invalid");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * return: < 0 : error
+ *         == 0: no ide
+ *         == 1: ide
+ */
+static int fts_param_ide_in_host(struct fts_upgrade *upg)
+{
+	u32 off = 0;
+
+	if ((!upg) || (!upg->func) || (!upg->fw)) {
+		FTS_ERROR("fts_data/upgrade/func/fw is NULL");
+		return -EINVAL;
+	}
+
+	if (upg->fw_length < upg->func->paramcfgoff + FTS_FW_IDE_SIG_LEN) {
+		FTS_INFO("fw len(%x) < paramcfg offset(%x), no IDE",
+			upg->fw_length, upg->func->paramcfgoff + FTS_FW_IDE_SIG_LEN);
+		return 0;
+	}
+
+	off = upg->func->paramcfgoff;
+	if (0 == memcmp(&upg->fw[off], FTS_FW_IDE_SIG, FTS_FW_IDE_SIG_LEN)) {
+		FTS_INFO("fw in host is IDE version");
+		return 1;
+	}
+
+	FTS_INFO("fw in host isn't IDE version");
+	return 0;
+}
+
+/*
+ * return: < 0 : error
+ *         0   : no ide
+ *         1   : ide
+ */
+static int fts_param_ide_in_tp(u8 *val)
+{
+	int ret = 0;
+
+	ret = fts_read_reg(FTS_REG_IDE_PARA_STATUS, val);
+	if (ret < 0) {
+		FTS_ERROR("read IDE PARAM STATUS in tp fail");
+		return ret;
+	}
+
+	if ((*val != 0xFF) && ((*val & 0x80) == 0x80)) {
+		FTS_INFO("fw in tp is IDE version");
+		return 1;
+	}
+
+	FTS_INFO("fw in tp isn't IDE version");
+	return 0;
+}
+
+/************************************************************************
+ * fts_param_need_upgrade - check fw paramcfg need upgrade or not
+ *
+ * Return:  < 0 : error if paramcfg need upgrade
+ *          0   : no need upgrade
+ *          1   : need upgrade app + param
+ *          2   : need upgrade param
+ ***********************************************************************/
+static int fts_param_need_upgrade(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	u8 val = 0;
+	int ide_in_host = 0;
+	int ide_in_tp = 0;
+	u8 ver_in_host = 0;
+	u8 ver_in_tp = 0;
+	bool fwvalid = false;
+
+	fwvalid = fts_fwupg_check_fw_valid();
+	if ( !fwvalid) {
+		FTS_INFO("fw is invalid, upgrade app+param");
+		return 1;
+	}
+
+	ide_in_host = fts_param_ide_in_host(upg);
+	if (ide_in_host < 0) {
+		FTS_INFO("fts_param_ide_in_host fail");
+		return ide_in_host;
+	}
+
+	ide_in_tp = fts_param_ide_in_tp(&val);
+	if (ide_in_tp < 0) {
+		FTS_INFO("fts_param_ide_in_tp fail");
+		return ide_in_tp;
+	}
+
+	if ((0 == ide_in_host) && (0 == ide_in_tp)) {
+		FTS_INFO("fw in host&tp are both no ide");
+		return 0;
+	} else if (ide_in_host != ide_in_tp) {
+		FTS_INFO("fw in host&tp not equal, need upgrade app+param");
+		return 1;
+	} else if ((1 == ide_in_host) && (1 == ide_in_tp)) {
+		FTS_INFO("fw in host&tp are both ide");
+		if ((val & 0x7F) != 0x00) {
+			FTS_INFO("param invalid, need upgrade param");
+			return 2;
+		}
+
+		ret = fts_param_get_ver_in_host(upg, &ver_in_host);
+		if (ret < 0) {
+			FTS_ERROR("param version in host invalid");
+		return ret;
+		}
+
+		ret = fts_param_get_ver_in_tp(&ver_in_tp);
+		if (ret < 0) {
+			FTS_ERROR("get IDE param ver in tp fail");
+		return ret;
+		}
+
+		FTS_INFO("fw paramcfg version in tp:%x, host:%x",
+			ver_in_tp, ver_in_host);
+		if (ver_in_tp != ver_in_host) {
+			return 2;
+		}
+	}
+
+	return 0;
+}
+
+static int fts_fwupg_get_ver_in_tp(u8 *ver)
+{
+	int ret = 0;
+
+	if (NULL == ver) {
+		FTS_ERROR("ver is NULL");
+		return -EINVAL;
+	}
+
+	ret = fts_read_reg(FTS_REG_FW_VER, ver);
+	if (ret < 0) {
+		FTS_ERROR("read fw ver from tp fail");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int fts_fwupg_get_ver_in_host(struct fts_upgrade *upg, u8 *ver)
+{
+	if ((!upg) || (!upg->func) || (!upg->fw) || (!ver)) {
+		FTS_ERROR("fts_data/upgrade/func/fw/ver is NULL");
+		return -EINVAL;
+	}
+
+	if (upg->fw_length < upg->func->fwveroff) {
+		FTS_ERROR("fw len(0x%0x) < fw ver offset(0x%x)",
+			upg->fw_length, upg->func->fwveroff);
+		return -EINVAL;
+	}
+
+	FTS_INFO("fw version offset:0x%x", upg->func->fwveroff);
+	*ver = upg->fw[upg->func->fwveroff];
+	return 0;
+}
+
+static bool fts_fwupg_need_upgrade(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	bool fwvalid = false;
+	u8 fw_ver_in_host = 0;
+	u8 fw_ver_in_tp = 0;
+
+	fwvalid = fts_fwupg_check_fw_valid();
+	if (fwvalid) {
+		ret = fts_fwupg_get_ver_in_host(upg, &fw_ver_in_host);
+		if (ret < 0) {
+			FTS_ERROR("get fw ver in host fail");
+			return false;
+		}
+
+		ret = fts_fwupg_get_ver_in_tp(&fw_ver_in_tp);
+		if (ret < 0) {
+			FTS_ERROR("get fw ver in tp fail");
+			return false;
+		}
+
+		FTS_INFO("fw version in tp:%x, host:%x", fw_ver_in_tp, fw_ver_in_host);
+		if (fw_ver_in_tp != fw_ver_in_host) {
+			return true;
+		}
+	} else {
+		FTS_INFO("fw invalid, need upgrade fw");
+		return true;
+	}
+
+	return false;
+}
+
+/************************************************************************
+ * Name: fts_fw_upgrade
+ * Brief: fw upgrade main entry, run in following steps
+ *        1. check fw version(A6), not equal, will upgrade app(+param)
+ *        2. if fw version equal, will check ide, will upgrade app(+param)
+ *        in the follow situation
+ *          a. host&tp IDE's type are not equal, will upgrade app+param
+ *          b. host&tp are both IDE's type, and param's version are not
+ *          equal, will upgrade param
+ * Input:
+ * Output:
+ * Return: return 0 if success, otherwise return error code
+ ***********************************************************************/
+int fts_fwupg_upgrade(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	bool upgrade_flag = false;
+	int upgrade_count = 0;
+	u8 ver = 0;
+
+	FTS_INFO("fw auto upgrade function");
+	if ((NULL == upg) || (NULL == upg->func)) {
+		FTS_ERROR("upg/upg->func is null");
+		return -EINVAL;
+	}
+
+	upgrade_flag = fts_fwupg_need_upgrade(upg);
+	FTS_INFO("fw upgrade flag:%d", upgrade_flag);
+	do {
+		upgrade_count++;
+		if (upgrade_flag) {
+			FTS_INFO("upgrade fw app(times:%d)", upgrade_count);
+			if (upg->func->upgrade) {
+				ret = upg->func->upgrade(upg->fw, upg->fw_length);
+				if (ret < 0) {
+					fts_fwupg_reset_in_boot();
+				} else {
+					fts_fwupg_get_ver_in_tp(&ver);
+					FTS_INFO("success upgrade to fw version %02x", ver);
+					break;
+				}
+			} else {
+				FTS_ERROR("upgrade func/upgrade is null, return immediately");
+				ret = -ENODATA;
+				break;
+			}
+		} else {
+			if (upg->func->param_upgrade) {
+				ret = fts_param_need_upgrade(upg);
+				if (ret <= 0) {
+					FTS_INFO("param don't need upgrade");
+					break;
+				} else if (1 == ret) {
+					FTS_INFO("force upgrade fw app(times:%d)", upgrade_count);
+					if (upg->func->upgrade) {
+						ret = upg->func->upgrade(upg->fw, upg->fw_length);
+						if (ret < 0) {
+							fts_fwupg_reset_in_boot();
+						} else {
+							break;
+						}
+					}
+				} else if (2 == ret) {
+					FTS_INFO("upgrade param area(times:%d)", upgrade_count);
+					ret = upg->func->param_upgrade(upg->fw, upg->fw_length);
+					if (ret < 0) {
+						fts_fwupg_reset_in_boot();
+					} else {
+						fts_param_get_ver_in_tp(&ver);
+						FTS_INFO("success upgrade to fw param version %02x", ver);
+						break;
+					}
+				} else
+					break;
+			} else {
+				break;
+			}
+		}
+	} while (upgrade_count < 2);
+
+	return ret;
+}
+
+/************************************************************************
+ * fts_fwupg_auto_upgrade - upgrade main entry
+ ***********************************************************************/
+static void fts_fwupg_auto_upgrade(struct fts_upgrade *upg)
+{
+	int ret = 0;
+
+	FTS_INFO("********************FTS enter upgrade********************");
+	if (!upg || !upg->ts_data) {
+		FTS_ERROR("upg/ts_data is null");
+		return ;
+	}
+
+	ret = fts_fwupg_upgrade(upg);
+	if (ret < 0)
+		FTS_ERROR("**********tp fw(app/param) upgrade failed**********");
+	else
+		FTS_INFO("**********tp fw(app/param) no upgrade/upgrade success**********");
+
+#if FTS_AUTO_LIC_UPGRADE_EN
+	ret = fts_lic_upgrade(upg);
+	if (ret < 0)
+		FTS_ERROR("**********lcd init code upgrade failed**********");
+	else
+		FTS_INFO("**********lcd init code no upgrade/upgrade success**********");
+#endif
+
+	FTS_INFO("********************FTS exit upgrade********************");
+}
+
+static int fts_fwupg_get_vendorid(struct fts_upgrade *upg, int *vid)
+{
+	int ret = 0;
+	bool fwvalid = false;
+	u8 vendor_id = 0;
+	u8 module_id = 0;
+	u32 fwcfg_addr = 0;
+	u8 cmd = 0;
+	u8 cfgbuf[FTS_HEADER_LEN] = { 0 };
+
+	FTS_INFO("read vendor id from tp");
+	if ((!upg) || (!upg->func) || (!upg->ts_data) || (!vid)) {
+		FTS_ERROR("upgrade/func/ts_data/vid is null");
+		return -EINVAL;
+	}
+
+	fwvalid = fts_fwupg_check_fw_valid();
+	if (fwvalid) {
+		ret = fts_read_reg(FTS_REG_VENDOR_ID, &vendor_id);
+		if (upg->ts_data->ic_info.is_incell)
+			ret = fts_read_reg(FTS_REG_MODULE_ID, &module_id);
+	} else {
+		if (upg->func->upgspec_version >= UPGRADE_SPEC_V_1_0) {
+			cmd = FTS_CMD_READ_FW_CONF;
+			ret = fts_read(&cmd, 1, cfgbuf, FTS_HEADER_LEN);
+		} else {
+			fwcfg_addr =  upg->func->fwcfgoff;
+			ret = fts_flash_read(fwcfg_addr, cfgbuf, FTS_HEADER_LEN);
+		}
+
+		if ((cfgbuf[FTS_CONIFG_VENDORID_OFF] +
+			cfgbuf[FTS_CONIFG_VENDORID_OFF + 1]) == 0xFF)
+			vendor_id = cfgbuf[FTS_CONIFG_VENDORID_OFF];
+		if (upg->ts_data->ic_info.is_incell) {
+			if ((cfgbuf[FTS_CONIFG_MODULEID_OFF] +
+				cfgbuf[FTS_CONIFG_MODULEID_OFF + 1]) == 0xFF)
+				module_id = cfgbuf[FTS_CONIFG_MODULEID_OFF];
+		}
+	}
+
+	if (ret < 0) {
+		FTS_ERROR("fail to get vendor id from tp");
+		return ret;
+	}
+
+	*vid = (int)((module_id << 8) + vendor_id);
+	return 0;
+}
+
+static int fts_fwupg_get_module_info(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	int i = 0;
+	struct upgrade_module *info = &module_list[0];
+
+	if (!upg || !upg->ts_data) {
+		FTS_ERROR("upg/ts_data is null");
+		return -EINVAL;
+	}
+
+	if (FTS_GET_MODULE_NUM > 1) {
+		/* support multi modules, must read correct module id(vendor id) */
+		ret = fts_fwupg_get_vendorid(upg, &upg->module_id);
+		if (ret < 0) {
+			FTS_ERROR("get vendor id failed");
+			return ret;
+		}
+		FTS_INFO("module id:%04x", upg->module_id);
+		for (i = 0; i < FTS_GET_MODULE_NUM; i++) {
+			info = &module_list[i];
+			if (upg->module_id == info->id) {
+				FTS_INFO("module id match, get module info pass");
+				break;
+			}
+		}
+		if (i >= FTS_GET_MODULE_NUM) {
+			FTS_ERROR("no module id match, don't get file");
+			return -ENODATA;
+		}
+	}
+
+	upg->module_info = info;
+	return 0;
+}
+
+static int fts_get_fw_file_via_request_firmware(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	const struct firmware *fw = NULL;
+	u8 *tmpbuf = NULL;
+	char fwname[FILE_NAME_LENGTH] = { 0 };
+
+	if (!upg || !upg->ts_data || !upg->ts_data->dev) {
+		FTS_ERROR("upg/ts_data/dev is null");
+		return -EINVAL;
+	}
+
+	if (upg->ts_data->pdata->type == _FT3658U)
+		snprintf(fwname, FILE_NAME_LENGTH, "%s%s_ft3658.bin",
+				FTS_FW_NAME_PREX_WITH_REQUEST,
+				upg->module_info->vendor_name);
+	else
+		snprintf(fwname, FILE_NAME_LENGTH, "%s%s.bin",
+				FTS_FW_NAME_PREX_WITH_REQUEST,
+				upg->module_info->vendor_name);
+
+	ret = request_firmware(&fw, fwname, upg->ts_data->dev);
+	if (ret == 0) {
+		FTS_INFO("firmware(%s) request successfully", fwname);
+		tmpbuf = vmalloc(fw->size);
+		if (NULL == tmpbuf) {
+			FTS_ERROR("fw buffer vmalloc fail");
+			ret = -ENOMEM;
+		} else {
+			memcpy(tmpbuf, fw->data, fw->size);
+			upg->fw = tmpbuf;
+			upg->fw_length = fw->size;
+			upg->fw_from_request = 1;
+		}
+	} else {
+		FTS_INFO("firmware(%s) request fail,ret=%d", fwname, ret);
+	}
+
+	if (fw != NULL) {
+		release_firmware(fw);
+		fw = NULL;
+	}
+
+	return ret;
+}
+
+static int fts_get_fw_file_via_i(struct fts_upgrade *upg)
+{
+	upg->fw = upg->module_info->fw_file;
+	upg->fw_length = upg->module_info->fw_len;
+	upg->fw_from_request = 0;
+
+	return 0;
+}
+
+/*****************************************************************************
+ *  Name: fts_fwupg_get_fw_file
+ *  Brief: get fw image/file,
+ *         If support muitl modules, please set FTS_GET_MODULE_NUM, and FTS_-
+ *         MODULE_ID/FTS_MODULE_NAME;
+ *         If get fw via .i file, please set FTS_FW_REQUEST_SUPPORT=0, and F-
+ *         TS_MODULE_ID; will use module id to distingwish different modules;
+ *         If get fw via reques_firmware(), please set FTS_FW_REQUEST_SUPPORT
+ *         =1, and FTS_MODULE_NAME; fw file name will be composed of "focalt-
+ *         ech_ts_fw_" & FTS_VENDOR_NAME;
+ *
+ *         If have flash, module_id=vendor_id, If non-flash,module_id need
+ *         transfer from LCD driver(gpio or lcm_id or ...);
+ *  Input:
+ *  Output:
+ *  Return: return 0 if success, otherwise return error code
+ *****************************************************************************/
+static int fts_fwupg_get_fw_file(struct fts_upgrade *upg)
+{
+	int ret = 0;
+	bool get_fw_i_flag = false;
+
+	FTS_DEBUG("get upgrade fw file");
+	if (!upg || !upg->ts_data) {
+		FTS_ERROR("upg/ts_data is null");
+		return -EINVAL;
+	}
+
+	ret = fts_fwupg_get_module_info(upg);
+	if ((ret < 0) || (!upg->module_info)) {
+		FTS_ERROR("get module info fail");
+		return ret;
+	}
+
+	if (FTS_FW_REQUEST_SUPPORT) {
+		ret = fts_get_fw_file_via_request_firmware(upg);
+		if (ret != 0) {
+			get_fw_i_flag = true;
+		}
+	} else {
+		get_fw_i_flag = true;
+	}
+
+	if (get_fw_i_flag) {
+		ret = fts_get_fw_file_via_i(upg);
+	}
+
+	upg->lic = upg->fw;
+	upg->lic_length = upg->fw_length;
+
+	FTS_INFO("upgrade fw file len:%d", upg->fw_length);
+	if ((upg->fw_length < FTS_MIN_LEN)
+		|| (upg->fw_length > FTS_MAX_LEN_FILE)) {
+		FTS_ERROR("fw file len(%d) fail", upg->fw_length);
+		return -ENODATA;
+	}
+
+	return ret;
+}
+
+static void fts_fwupg_init_ic_detail(struct fts_upgrade *upg)
+{
+	if (upg && upg->func && upg->func->init) {
+		upg->func->init(upg->fw, upg->fw_length);
+	}
+}
+
+/*****************************************************************************
+ *  Name: fts_fwupg_work
+ *  Brief: 1. get fw image/file
+ *         2. ic init if have
+ *         3. call upgrade main function(fts_fwupg_auto_upgrade)
+ *  Input:
+ *  Output:
+ *  Return:
+ *****************************************************************************/
+static void fts_fwupg_work(struct work_struct *work)
+{
+	int ret = 0;
+	struct fts_upgrade *upg = fwupgrade;
+
+#if !FTS_AUTO_UPGRADE_EN
+	FTS_INFO("FTS_AUTO_UPGRADE_EN is disabled, not upgrade when power on");
+	return ;
+#endif
+
+	FTS_INFO("fw upgrade work function");
+	if (!upg || !upg->ts_data) {
+		FTS_ERROR("upg/ts_data is null");
+		return ;
+	}
+
+	upg->ts_data->fw_loading = 1;
+	fts_irq_disable();
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_switch(DISABLE);
+#endif
+
+	/* get fw */
+	ret = fts_fwupg_get_fw_file(upg);
+	if (ret < 0) {
+		FTS_ERROR("get file fail, can't upgrade");
+	} else {
+		/* ic init if have */
+		fts_fwupg_init_ic_detail(upg);
+		/* run auto upgrade */
+		fts_fwupg_auto_upgrade(upg);
+	}
+
+#if FTS_ESDCHECK_EN
+	fts_esdcheck_switch(ENABLE);
+#endif
+	fts_irq_enable();
+	upg->ts_data->fw_loading = 0;
+}
+
+int fts_fwupg_init(struct fts_ts_data *ts_data)
+{
+	int i = 0;
+	int j = 0;
+	int ic_stype = 0;
+	struct upgrade_func *func = upgrade_func_list[0];
+	int func_count = sizeof(upgrade_func_list) / sizeof(upgrade_func_list[0]);
+
+	FTS_INFO("fw upgrade init function");
+
+	if (!ts_data || !ts_data->ts_workqueue) {
+		FTS_ERROR("ts_data/workqueue is NULL, can't run upgrade function");
+		return -EINVAL;
+	}
+
+	if (0 == func_count) {
+		FTS_ERROR("no upgrade function in tp driver");
+		return -ENODATA;
+	}
+
+	fwupgrade = (struct fts_upgrade *)kzalloc(sizeof(*fwupgrade), GFP_KERNEL);
+	if (NULL == fwupgrade) {
+		FTS_ERROR("malloc memory for upgrade fail");
+		return -ENOMEM;
+	}
+
+	ic_stype = ts_data->ic_info.ids.type;
+	if (1 == func_count) {
+		fwupgrade->func = func;
+	} else {
+		for (i = 0; i < func_count; i++) {
+			func = upgrade_func_list[i];
+			for (j = 0; j < FTX_MAX_COMPATIBLE_TYPE; j++) {
+				if (0 == func->ctype[j])
+					break;
+				else if (func->ctype[j] == ic_stype) {
+					FTS_INFO("match upgrade function,type:%x", (int)func->ctype[j]);
+					fwupgrade->func = func;
+				}
+			}
+		}
+	}
+
+	if (NULL == fwupgrade->func) {
+		FTS_ERROR("no upgrade function match, can't upgrade");
+		kfree(fwupgrade);
+		fwupgrade = NULL;
+		return -ENODATA;
+	}
+
+	fwupgrade->ts_data = ts_data;
+	INIT_WORK(&ts_data->fwupg_work, fts_fwupg_work);
+	queue_work(ts_data->ts_workqueue, &ts_data->fwupg_work);
+
+	return 0;
+}
+
+int fts_fwupg_exit(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+	if (fwupgrade) {
+		if (fwupgrade->fw_from_request) {
+			vfree(fwupgrade->fw);
+			fwupgrade->fw = NULL;
+		}
+
+		kfree(fwupgrade);
+		fwupgrade = NULL;
+	}
+	FTS_FUNC_EXIT();
+	return 0;
+}

focaltech_touch/focaltech_flash.h

@@ -0,0 +1,216 @@
+/************************************************************************
+* Copyright (C) 2012-2019, Focaltech Systems (R)£¬All Rights Reserved.
+*
+* File Name: focaltech_flash.h
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-07
+*
+* Abstract:
+*
+************************************************************************/
+#ifndef __LINUX_FOCALTECH_FLASH_H__
+#define __LINUX_FOCALTECH_FLASH_H__
+
+/*****************************************************************************
+* 1.Included header files
+*****************************************************************************/
+#include "focaltech_core.h"
+
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define FTS_CMD_RESET                               0x07
+#define FTS_ROMBOOT_CMD_SET_PRAM_ADDR               0xAD
+#define FTS_ROMBOOT_CMD_SET_PRAM_ADDR_LEN           4
+#define FTS_ROMBOOT_CMD_WRITE                       0xAE
+#define FTS_ROMBOOT_CMD_START_APP                   0x08
+#define FTS_DELAY_PRAMBOOT_START                    100
+#define FTS_ROMBOOT_CMD_ECC                         0xCC
+#define FTS_PRAM_SADDR                              0x000000
+#define FTS_DRAM_SADDR                              0xD00000
+
+#define FTS_CMD_READ                                0x03
+#define FTS_CMD_READ_DELAY                          1
+#define FTS_CMD_READ_LEN                            4
+#define FTS_CMD_READ_LEN_SPI                        6
+#define FTS_CMD_FLASH_TYPE                          0x05
+#define FTS_CMD_FLASH_MODE                          0x09
+#define FLASH_MODE_WRITE_FLASH_VALUE                0x0A
+#define FLASH_MODE_UPGRADE_VALUE                    0x0B
+#define FLASH_MODE_LIC_VALUE                        0x0C
+#define FLASH_MODE_PARAM_VALUE                      0x0D
+#define FTS_CMD_ERASE_APP                           0x61
+#define FTS_REASE_APP_DELAY                         1350
+#define FTS_ERASE_SECTOR_DELAY                      60
+#define FTS_RETRIES_REASE                           50
+#define FTS_RETRIES_DELAY_REASE                     400
+#define FTS_CMD_FLASH_STATUS                        0x6A
+#define FTS_CMD_FLASH_STATUS_LEN                    2
+#define FTS_CMD_FLASH_STATUS_NOP                    0x0000
+#define FTS_CMD_FLASH_STATUS_ECC_OK                 0xF055
+#define FTS_CMD_FLASH_STATUS_ERASE_OK               0xF0AA
+#define FTS_CMD_FLASH_STATUS_WRITE_OK               0x1000
+#define FTS_CMD_ECC_INIT                            0x64
+#define FTS_CMD_ECC_CAL                             0x65
+#define FTS_CMD_ECC_CAL_LEN                         7
+#define FTS_RETRIES_ECC_CAL                         10
+#define FTS_RETRIES_DELAY_ECC_CAL                   50
+#define FTS_CMD_ECC_READ                            0x66
+#define FTS_CMD_SET_WFLASH_ADDR                     0xAB
+#define FTS_CMD_SET_RFLASH_ADDR                     0xAC
+#define FTS_LEN_SET_ADDR                            4
+#define FTS_CMD_DATA_LEN                            0xB0
+#define FTS_CMD_APP_DATA_LEN_INCELL                 0x7A
+#define FTS_CMD_DATA_LEN_LEN                        4
+#define FTS_CMD_WRITE                               0xBF
+#define FTS_RETRIES_WRITE                           100
+#define FTS_RETRIES_DELAY_WRITE                     1
+#define FTS_CMD_WRITE_LEN                           6
+#define FTS_DELAY_READ_ID                           20
+#define FTS_DELAY_UPGRADE_RESET                     80
+#define PRAMBOOT_MIN_SIZE                           0x120
+#define PRAMBOOT_MAX_SIZE                           (64*1024)
+#define FTS_FLASH_PACKET_LENGTH                     32     /* max=128 */
+#define FTS_MAX_LEN_ECC_CALC                        0xFFFE /* must be even */
+#define FTS_MIN_LEN                                 0x120
+#define FTS_MAX_LEN_FILE                            (128 * 1024)
+#define FTS_MAX_LEN_APP                             (64 * 1024)
+#define FTS_MAX_LEN_SECTOR                          (4 * 1024)
+#define FTS_CONIFG_VENDORID_OFF                     0x04
+#define FTS_CONIFG_MODULEID_OFF                     0x1E
+#define FTS_CONIFG_PROJECTID_OFF                    0x20
+#define FTS_APPINFO_OFF                             0x100
+#define FTS_APPINFO_APPLEN_OFF                      0x00
+#define FTS_APPINFO_APPLEN2_OFF                     0x12
+#define FTS_REG_UPGRADE                             0xFC
+#define FTS_REG_UPGRADE2                            0xBC
+#define FTS_UPGRADE_AA                              0xAA
+#define FTS_UPGRADE_55                              0x55
+#define FTS_DELAY_UPGRADE_AA                        10
+#define FTS_UPGRADE_LOOP                            30
+#define FTS_HEADER_LEN                              32
+#define FTS_FW_BIN_FILEPATH                         "/sdcard/"
+#define FTS_FW_IDE_SIG                              "IDE_"
+#define FTS_FW_IDE_SIG_LEN                          4
+#define MAX_MODULE_VENDOR_NAME_LEN                  16
+
+#define FTS_ROMBOOT_CMD_ECC_NEW_LEN                 7
+#define FTS_ECC_FINISH_TIMEOUT                      100
+#define FTS_ROMBOOT_CMD_ECC_FINISH                  0xCE
+#define FTS_ROMBOOT_CMD_ECC_FINISH_OK_A5            0xA5
+#define FTS_ROMBOOT_CMD_ECC_FINISH_OK_00            0x00
+#define FTS_ROMBOOT_CMD_ECC_READ                    0xCD
+#define AL2_FCS_COEF                ((1 << 15) + (1 << 10) + (1 << 3))
+
+#define FTS_APP_INFO_OFFSET                         0x100
+
+enum FW_STATUS {
+	FTS_RUN_IN_ERROR,
+	FTS_RUN_IN_APP,
+	FTS_RUN_IN_ROM,
+	FTS_RUN_IN_PRAM,
+	FTS_RUN_IN_BOOTLOADER,
+};
+
+enum FW_FLASH_MODE {
+	FLASH_MODE_APP,
+	FLASH_MODE_LIC,
+	FLASH_MODE_PARAM,
+	FLASH_MODE_ALL,
+};
+
+enum ECC_CHECK_MODE {
+	ECC_CHECK_MODE_XOR,
+	ECC_CHECK_MODE_CRC16,
+};
+
+enum UPGRADE_SPEC {
+	UPGRADE_SPEC_V_1_0 = 0x0100,
+};
+
+/*****************************************************************************
+* Private enumerations, structures and unions using typedef
+*****************************************************************************/
+/* IC info */
+struct upgrade_func {
+	u64 ctype[FTX_MAX_COMPATIBLE_TYPE];
+	u32 fwveroff;
+	u32 fwcfgoff;
+	u32 appoff;
+	u32 licoff;
+	u32 paramcfgoff;
+	u32 paramcfgveroff;
+	u32 paramcfg2off;
+	int pram_ecc_check_mode;
+	int fw_ecc_check_mode;
+	int upgspec_version;
+	bool new_return_value_from_ic;
+	bool appoff_handle_in_ic;
+	bool is_reset_register_BC;
+	bool read_boot_id_need_reset;
+	bool hid_supported;
+	bool pramboot_supported;
+	u8 *pramboot;
+	u32 pb_length;
+	int (*init)(u8 *, u32);
+	int (*write_pramboot_private)(void);
+	int (*upgrade)(u8 *, u32);
+	int (*get_hlic_ver)(u8 *);
+	int (*lic_upgrade)(u8 *, u32);
+	int (*param_upgrade)(u8 *, u32);
+	int (*force_upgrade)(u8 *, u32);
+};
+
+struct upgrade_setting_nf {
+	u8 rom_idh;
+	u8 rom_idl;
+	u16 reserved;
+	u32 app2_offset;
+	u32 ecclen_max;
+	u8 eccok_val;
+	u8 upgsts_boot;
+	u8 delay_init;
+	bool spi_pe;
+	bool half_length;
+	bool fd_check;
+	bool drwr_support;
+};
+
+struct upgrade_module {
+	int id;
+	char vendor_name[MAX_MODULE_VENDOR_NAME_LEN];
+	u8 *fw_file;
+	u32 fw_len;
+};
+
+struct fts_upgrade {
+	struct fts_ts_data *ts_data;
+	struct upgrade_module *module_info;
+	struct upgrade_func *func;
+	struct upgrade_setting_nf *setting_nf;
+	int module_id;
+	bool fw_from_request;
+	u8 *fw;
+	u32 fw_length;
+	u8 *lic;
+	u32 lic_length;
+};
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+extern struct upgrade_func upgrade_func_ft5452;
+extern struct upgrade_func upgrade_func_ft5652;
+
+/*****************************************************************************
+* Static function prototypes
+*****************************************************************************/
+int fts_fwupg_reset_in_boot(void);
+int fts_fwupg_enter_into_boot(void);
+int fts_fwupg_erase(u32 delay);
+int fts_fwupg_ecc_cal(u32 saddr, u32 len);
+int fts_flash_write_buf(u32 saddr, u8 *buf, u32 len, u32 delay);
+int fts_fwupg_upgrade(struct fts_upgrade *upg);
+#endif

focaltech_touch/focaltech_flash/focaltech_upgrade_ft3518.c

@@ -0,0 +1,292 @@
+/*
+ *
+ * FocalTech fts TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, Focaltech Ltd. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*****************************************************************************
+*
+* File Name: focaltech_upgrade_ft5452.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-15
+*
+* Abstract:
+*
+* Reference:
+*
+*****************************************************************************/
+/*****************************************************************************
+* 1.Included header files
+*****************************************************************************/
+#include "../focaltech_flash.h"
+
+/************************************************************************
+* Name: fts_ft5452_upgrade
+* Brief:
+* Input:
+* Output:
+* Return: return 0 if success, otherwise return error code
+***********************************************************************/
+static int fts_ft5452_upgrade(u8 *buf, u32 len)
+{
+	int ret = 0;
+	u32 start_addr = 0;
+	u8 cmd[4] = { 0 };
+	int ecc_in_host = 0;
+	int ecc_in_tp = 0;
+
+	int i = 0;
+	u8 wbuf[7] = { 0 };
+	u8 reg_val[4] = {0};
+
+	if (NULL == buf) {
+		FTS_ERROR("fw buf is null");
+		return -EINVAL;
+	}
+
+	if ((len < FTS_MIN_LEN) || (len > (60 * 1024))) {
+		FTS_ERROR("fw buffer len(%x) fail", len);
+		return -EINVAL;
+	}
+
+	/* enter into upgrade environment */
+	ret = fts_fwupg_enter_into_boot();
+	if (ret < 0) {
+		FTS_ERROR("enter into pramboot/bootloader fail,ret=%d", ret);
+		goto fw_reset;
+	}
+
+	cmd[0] = FTS_CMD_FLASH_MODE;
+	cmd[1] = FLASH_MODE_UPGRADE_VALUE;
+	ret = fts_write(cmd, 2);
+	if (ret < 0) {
+		FTS_ERROR("upgrade mode(09) cmd write fail");
+		goto fw_reset;
+	}
+
+	cmd[0] = FTS_CMD_DATA_LEN;
+	cmd[1] = BYTE_OFF_16(len);
+	cmd[2] = BYTE_OFF_8(len);
+	cmd[3] = BYTE_OFF_0(len);
+	ret = fts_write(cmd, FTS_CMD_DATA_LEN_LEN);
+	if (ret < 0) {
+		FTS_ERROR("data len cmd write fail");
+		goto fw_reset;
+	}
+
+	ret = fts_fwupg_erase(FTS_REASE_APP_DELAY);
+	if (ret < 0) {
+		FTS_ERROR("erase cmd write fail");
+		goto fw_reset;
+	}
+
+	/* write app */
+	start_addr = upgrade_func_ft5452.appoff;
+	ecc_in_host = fts_flash_write_buf(start_addr, buf, len, 1);
+	if (ecc_in_host < 0 ) {
+		FTS_ERROR("lcd initial code write fail");
+		goto fw_reset;
+	}
+
+	FTS_INFO( "**********read out checksum**********");
+
+	/* check sum init */
+	wbuf[0] = FTS_CMD_ECC_INIT;
+	ret = fts_write(wbuf, 1);
+	if (ret < 0) {
+		FTS_ERROR("ecc init cmd write fail");
+		return ret;
+	}
+
+	/* send commond to start checksum */
+	wbuf[0] = FTS_CMD_ECC_CAL;
+	wbuf[1] = BYTE_OFF_16(start_addr);
+	wbuf[2] = BYTE_OFF_8(start_addr);
+	wbuf[3] = BYTE_OFF_0(start_addr);
+
+	wbuf[4] = BYTE_OFF_16(len);
+	wbuf[5] = BYTE_OFF_8(len);
+	wbuf[6] = BYTE_OFF_0(len);
+
+	FTS_DEBUG("ecc calc startaddr:0x%04x, len:%d", start_addr, len);
+	ret = fts_write(wbuf, 7);
+	if (ret < 0) {
+		FTS_ERROR("ecc calc cmd write fail");
+		return ret;
+	}
+
+	msleep(len / 256);
+
+	/* read status if check sum is finished */
+	for (i = 0; i < FTS_RETRIES_ECC_CAL; i++) {
+		wbuf[0] = FTS_CMD_FLASH_STATUS;
+		reg_val[0] = reg_val[1] = 0x00;
+		fts_read(wbuf, 1, reg_val, 2);
+		FTS_DEBUG("[UPGRADE]: reg_val[0]=%02x reg_val[0]=%02x!!", reg_val[0], reg_val[1]);
+		if ((0xF0 == reg_val[0]) && (0x55 == reg_val[1])) {
+			break;
+		}
+		msleep(FTS_RETRIES_DELAY_ECC_CAL);
+	}
+
+	/* read out check sum */
+	wbuf[0] = FTS_CMD_ECC_READ;
+	ret = fts_read(wbuf, 1, reg_val, 1);
+	if (ret < 0) {
+		FTS_ERROR( "ecc read cmd write fail");
+		return ret;
+	}
+	ecc_in_tp = reg_val[0];
+
+	FTS_INFO("ecc in tp:%x, host:%x", ecc_in_tp, ecc_in_host);
+	if (ecc_in_tp != ecc_in_host) {
+		FTS_ERROR("ecc check fail");
+		goto fw_reset;
+	}
+
+	FTS_INFO("upgrade success, reset to normal boot");
+	ret = fts_fwupg_reset_in_boot();
+	if (ret < 0) {
+		FTS_ERROR("reset to normal boot fail");
+	}
+
+	msleep(200);
+	return 0;
+
+fw_reset:
+	FTS_INFO("upgrade fail, reset to normal boot");
+	ret = fts_fwupg_reset_in_boot();
+	if (ret < 0) {
+		FTS_ERROR("reset to normal boot fail");
+	}
+	return -EIO;
+}
+
+struct upgrade_func upgrade_func_ft5452 = {
+	.ctype = {0x81},
+	.fwveroff = 0x010E,
+	.fwcfgoff = 0x1FFB0,
+	.appoff = 0x0000,
+	.pramboot_supported = false,
+	.hid_supported = true,
+	.upgrade = fts_ft5452_upgrade,
+};
+
+
+#define FTS_DELAY_ERASE_PAGE_2K         80
+#define FTS_SIZE_PAGE_2K                2048
+
+/************************************************************************
+ * Name: fts_ft5652_upgrade
+ * Brief:
+ * Input:
+ * Output:
+ * Return: return 0 if success, otherwise return error code
+ **********************************************************************/
+static int fts_ft5652_upgrade(u8 *buf, u32 len)
+{
+	int ret = 0;
+	u32 start_addr = 0;
+	u8 cmd[4] = { 0 };
+	u32 delay = 0;
+	int ecc_in_host = 0;
+	int ecc_in_tp = 0;
+
+	if ((buf == NULL) || (len < FTS_MIN_LEN)) {
+		FTS_ERROR("buffer/len(%x) is invalid", len);
+		return -EINVAL;
+	}
+
+	/* enter into upgrade environment */
+	ret = fts_fwupg_enter_into_boot();
+	if (ret < 0) {
+		FTS_ERROR("enter into pramboot/bootloader fail,ret=%d", ret);
+		goto fw_reset;
+	}
+
+	cmd[0] = FTS_CMD_APP_DATA_LEN_INCELL;
+	cmd[1] = BYTE_OFF_16(len);
+	cmd[2] = BYTE_OFF_8(len);
+	cmd[3] = BYTE_OFF_0(len);
+	ret = fts_write(cmd, FTS_CMD_DATA_LEN_LEN);
+	if (ret < 0) {
+		FTS_ERROR("data len cmd write fail");
+		goto fw_reset;
+	}
+
+	cmd[0] = FTS_CMD_FLASH_MODE;
+	cmd[1] = FLASH_MODE_UPGRADE_VALUE;
+	ret = fts_write(cmd, 2);
+	if (ret < 0) {
+		FTS_ERROR("upgrade mode(09) cmd write fail");
+		goto fw_reset;
+	}
+
+	delay = FTS_DELAY_ERASE_PAGE_2K * (len / FTS_SIZE_PAGE_2K);
+	ret = fts_fwupg_erase(delay);
+	if (ret < 0) {
+		FTS_ERROR("erase cmd write fail");
+		goto fw_reset;
+	}
+
+	/* write app */
+	start_addr = upgrade_func_ft5652.appoff;
+	ecc_in_host = fts_flash_write_buf(start_addr, buf, len, 1);
+	if (ecc_in_host < 0) {
+		FTS_ERROR("flash write fail");
+		goto fw_reset;
+	}
+
+	/* ecc */
+	ecc_in_tp = fts_fwupg_ecc_cal(start_addr, len);
+	if (ecc_in_tp < 0) {
+		FTS_ERROR("ecc read fail");
+		goto fw_reset;
+	}
+
+	FTS_INFO("ecc in tp:%x, host:%x", ecc_in_tp, ecc_in_host);
+	if (ecc_in_tp != ecc_in_host) {
+		FTS_ERROR("ecc check fail");
+		goto fw_reset;
+	}
+
+	FTS_INFO("upgrade success, reset to normal boot");
+	ret = fts_fwupg_reset_in_boot();
+	if (ret < 0)
+		FTS_ERROR("reset to normal boot fail");
+
+	msleep(200);
+	return 0;
+
+fw_reset:
+	FTS_INFO("upgrade fail, reset to normal boot");
+	ret = fts_fwupg_reset_in_boot();
+	if (ret < 0)
+		FTS_ERROR("reset to normal boot fail");
+
+	return -EIO;
+}
+
+struct upgrade_func upgrade_func_ft5652 = {
+	.ctype = {0x88},
+	.fwveroff = 0x010E,
+	.fwcfgoff = 0x1F80,
+	.appoff = 0x0000,
+	.upgspec_version = UPGRADE_SPEC_V_1_0,
+	.pramboot_supported = false,
+	.hid_supported = true,
+	.upgrade = fts_ft5652_upgrade,
+};

focaltech_touch/focaltech_gesture.c

@@ -0,0 +1,477 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, Focaltech Ltd. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*****************************************************************************
+*
+* File Name: focaltech_gestrue.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-08
+*
+* Abstract:
+*
+* Reference:
+*
+*****************************************************************************/
+
+/*****************************************************************************
+* 1.Included header files
+*****************************************************************************/
+#include "focaltech_core.h"
+
+/******************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define KEY_GESTURE_U                           KEY_U
+#define KEY_GESTURE_UP                          KEY_UP
+#define KEY_GESTURE_DOWN                        KEY_DOWN
+#define KEY_GESTURE_LEFT                        KEY_LEFT
+#define KEY_GESTURE_RIGHT                       KEY_RIGHT
+#define KEY_GESTURE_O                           KEY_O
+#define KEY_GESTURE_E                           KEY_E
+#define KEY_GESTURE_M                           KEY_M
+#define KEY_GESTURE_L                           KEY_L
+#define KEY_GESTURE_W                           KEY_W
+#define KEY_GESTURE_S                           KEY_S
+#define KEY_GESTURE_V                           KEY_V
+#define KEY_GESTURE_C                           KEY_C
+#define KEY_GESTURE_Z                           KEY_Z
+
+#define GESTURE_LEFT                            0x20
+#define GESTURE_RIGHT                           0x21
+#define GESTURE_UP                              0x22
+#define GESTURE_DOWN                            0x23
+#define GESTURE_DOUBLECLICK                     0x24
+#define GESTURE_O                               0x30
+#define GESTURE_W                               0x31
+#define GESTURE_M                               0x32
+#define GESTURE_E                               0x33
+#define GESTURE_L                               0x44
+#define GESTURE_S                               0x46
+#define GESTURE_V                               0x54
+#define GESTURE_Z                               0x41
+#define GESTURE_C                               0x34
+
+/*****************************************************************************
+* Private enumerations, structures and unions using typedef
+*****************************************************************************/
+/*
+* gesture_id    - mean which gesture is recognised
+* point_num     - points number of this gesture
+* coordinate_x  - All gesture point x coordinate
+* coordinate_y  - All gesture point y coordinate
+* mode          - gesture enable/disable, need enable by host
+*               - 1:enable gesture function(default)  0:disable
+* active        - gesture work flag,
+*                 always set 1 when suspend, set 0 when resume
+*/
+struct fts_gesture_st {
+	u8 gesture_id;
+	u8 point_num;
+	u16 coordinate_x[FTS_GESTURE_POINTS_MAX];
+	u16 coordinate_y[FTS_GESTURE_POINTS_MAX];
+};
+
+/*****************************************************************************
+* Static variables
+*****************************************************************************/
+static struct fts_gesture_st fts_gesture_data;
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+
+/*****************************************************************************
+* Static function prototypes
+*****************************************************************************/
+static ssize_t fts_gesture_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	u8 val = 0;
+	struct fts_ts_data *ts_data = fts_data;
+
+	mutex_lock(&ts_data->input_dev->mutex);
+	fts_read_reg(FTS_REG_GESTURE_EN, &val);
+	count = snprintf(buf, PAGE_SIZE, "Gesture Mode:%s\n",
+			ts_data->gesture_mode ? "On" : "Off");
+	count += snprintf(buf + count, PAGE_SIZE, "Reg(0xD0)=%d\n", val);
+	mutex_unlock(&ts_data->input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_gesture_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fts_ts_data *ts_data = fts_data;
+
+	mutex_lock(&ts_data->input_dev->mutex);
+	if (FTS_SYSFS_ECHO_ON(buf)) {
+		FTS_DEBUG("enable gesture");
+		ts_data->gesture_mode = ENABLE;
+	} else if (FTS_SYSFS_ECHO_OFF(buf)) {
+		FTS_DEBUG("disable gesture");
+		ts_data->gesture_mode = DISABLE;
+	}
+	mutex_unlock(&ts_data->input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_gesture_buf_show(
+	struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int count = 0;
+	int i = 0;
+	struct input_dev *input_dev = fts_data->input_dev;
+	struct fts_gesture_st *gesture = &fts_gesture_data;
+
+	mutex_lock(&input_dev->mutex);
+	count = snprintf(buf, PAGE_SIZE, "Gesture ID:%d\n", gesture->gesture_id);
+	count += snprintf(buf + count, PAGE_SIZE, "Gesture PointNum:%d\n",
+			gesture->point_num);
+	count += snprintf(buf + count, PAGE_SIZE, "Gesture Points Buffer:\n");
+
+	/* save point data,max:6 */
+	for (i = 0; i < FTS_GESTURE_POINTS_MAX; i++) {
+		count += snprintf(buf + count, PAGE_SIZE, "%3d(%4d,%4d) ", i,
+			gesture->coordinate_x[i], gesture->coordinate_y[i]);
+
+		if ((i + 1) % 4 == 0)
+			count += snprintf(buf + count, PAGE_SIZE, "\n");
+	}
+	count += snprintf(buf + count, PAGE_SIZE, "\n");
+	mutex_unlock(&input_dev->mutex);
+
+	return count;
+}
+
+static ssize_t fts_gesture_buf_store(
+	struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	return -EPERM;
+}
+
+
+/* sysfs gesture node
+ *   read example: cat  fts_gesture_mode       ---read gesture mode
+ *   write example:echo 1 > fts_gesture_mode   --- write gesture mode to 1
+ *
+ */
+static DEVICE_ATTR(fts_gesture_mode, S_IRUGO | S_IWUSR, fts_gesture_show,
+			fts_gesture_store);
+/*
+ *   read example: cat fts_gesture_buf        --- read gesture buf
+ */
+static DEVICE_ATTR(fts_gesture_buf, S_IRUGO | S_IWUSR,
+			fts_gesture_buf_show, fts_gesture_buf_store);
+
+static struct attribute *fts_gesture_mode_attrs[] = {
+	&dev_attr_fts_gesture_mode.attr,
+	&dev_attr_fts_gesture_buf.attr,
+	NULL,
+};
+
+static struct attribute_group fts_gesture_group = {
+	.attrs = fts_gesture_mode_attrs,
+};
+
+static int fts_create_gesture_sysfs(struct device *dev)
+{
+	int ret = 0;
+
+	ret = sysfs_create_group(&dev->kobj, &fts_gesture_group);
+	if (ret) {
+		FTS_ERROR("gesture sys node create fail");
+		sysfs_remove_group(&dev->kobj, &fts_gesture_group);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void fts_gesture_report(struct input_dev *input_dev, int gesture_id)
+{
+	int gesture;
+
+	FTS_DEBUG("gesture_id:0x%x", gesture_id);
+	switch (gesture_id) {
+	case GESTURE_LEFT:
+		gesture = KEY_GESTURE_LEFT;
+		break;
+
+	case GESTURE_RIGHT:
+		gesture = KEY_GESTURE_RIGHT;
+		break;
+
+	case GESTURE_UP:
+		gesture = KEY_GESTURE_UP;
+		break;
+
+	case GESTURE_DOWN:
+		gesture = KEY_GESTURE_DOWN;
+		break;
+
+	case GESTURE_DOUBLECLICK:
+		gesture = KEY_POWER;
+
+		break;
+
+	case GESTURE_O:
+		gesture = KEY_GESTURE_O;
+		break;
+
+	case GESTURE_W:
+		gesture = KEY_GESTURE_W;
+		break;
+
+	case GESTURE_M:
+		gesture = KEY_GESTURE_M;
+		break;
+
+	case GESTURE_E:
+		gesture = KEY_GESTURE_E;
+		break;
+
+	case GESTURE_L:
+		gesture = KEY_GESTURE_L;
+		break;
+
+	case GESTURE_S:
+		gesture = KEY_GESTURE_S;
+		break;
+
+	case GESTURE_V:
+		gesture = KEY_GESTURE_V;
+		break;
+
+	case GESTURE_Z:
+		gesture = KEY_GESTURE_Z;
+		break;
+
+	case GESTURE_C:
+		gesture = KEY_GESTURE_C;
+		break;
+
+	default:
+		gesture = -1;
+		break;
+	}
+
+	/* report event key */
+	if (gesture != -1) {
+		FTS_DEBUG("Gesture Code=%d", gesture);
+		input_report_key(input_dev, gesture, 1);
+		input_sync(input_dev);
+		input_report_key(input_dev, gesture, 0);
+		input_sync(input_dev);
+	}
+}
+
+/*****************************************************************************
+* Name: fts_gesture_readdata
+* Brief: Read information about gesture: enable flag/gesture points..., if ges-
+*        ture enable, save gesture points' information, and report to OS.
+*        It will be called this function every intrrupt when FTS_GESTURE_EN = 1
+*
+*        gesture data length: 1(enable) + 1(reserve) + 2(header) + 6 * 4
+* Input: ts_data - global struct data
+*        data    - gesture data buffer if non-flash, else NULL
+* Output:
+* Return: 0 - read gesture data successfully, the report data is gesture data
+*         1 - tp not in suspend/gesture not enable in TP FW
+*         -Exx - error
+*****************************************************************************/
+int fts_gesture_readdata(struct fts_ts_data *ts_data, u8 *data)
+{
+	int ret = 0;
+	int i = 0;
+	int index = 0;
+	u8 buf[FTS_GESTURE_DATA_LEN] = { 0 };
+	struct input_dev *input_dev = ts_data->input_dev;
+	struct fts_gesture_st *gesture = &fts_gesture_data;
+
+	if (!ts_data->suspended || !ts_data->gesture_mode) {
+		return 1;
+	}
+
+
+	ret = fts_read_reg(FTS_REG_GESTURE_EN, &buf[0]);
+	if ((ret < 0) || (buf[0] != ENABLE)) {
+		FTS_DEBUG("gesture not enable in fw, don't process gesture");
+		return 1;
+	}
+
+	buf[2] = FTS_REG_GESTURE_OUTPUT_ADDRESS;
+	ret = fts_read(&buf[2], 1, &buf[2], FTS_GESTURE_DATA_LEN - 2);
+	if (ret < 0) {
+		FTS_ERROR("read gesture header data fail");
+		return ret;
+	}
+
+	/* init variable before read gesture point */
+	memset(gesture->coordinate_x, 0, FTS_GESTURE_POINTS_MAX * sizeof(u16));
+	memset(gesture->coordinate_y, 0, FTS_GESTURE_POINTS_MAX * sizeof(u16));
+	gesture->gesture_id = buf[2];
+	gesture->point_num = buf[3];
+	FTS_DEBUG("gesture_id=%d, point_num=%d",
+		gesture->gesture_id, gesture->point_num);
+
+	/* save point data,max:6 */
+	for (i = 0; i < FTS_GESTURE_POINTS_MAX; i++) {
+		index = 4 * i + 4;
+		gesture->coordinate_x[i] = (u16)(((buf[0 + index] & 0x0F) << 8)
+						+ buf[1 + index]);
+		gesture->coordinate_y[i] = (u16)(((buf[2 + index] & 0x0F) << 8)
+						+ buf[3 + index]);
+	}
+
+	/* report gesture to OS */
+	fts_gesture_report(input_dev, gesture->gesture_id);
+	return 0;
+}
+
+void fts_gesture_recovery(struct fts_ts_data *ts_data)
+{
+	if (ts_data->gesture_mode && ts_data->suspended) {
+		FTS_DEBUG("gesture recovery...");
+		fts_write_reg(0xD1, 0xFF);
+		fts_write_reg(0xD2, 0xFF);
+		fts_write_reg(0xD5, 0xFF);
+		fts_write_reg(0xD6, 0xFF);
+		fts_write_reg(0xD7, 0xFF);
+		fts_write_reg(0xD8, 0xFF);
+		fts_write_reg(FTS_REG_GESTURE_EN, ENABLE);
+	}
+}
+
+int fts_gesture_suspend(struct fts_ts_data *ts_data)
+{
+	int i = 0;
+	u8 state = 0xFF;
+
+	FTS_FUNC_ENTER();
+	if (enable_irq_wake(ts_data->irq)) {
+		FTS_DEBUG("enable_irq_wake(irq:%d) fail", ts_data->irq);
+	}
+
+	for (i = 0; i < 5; i++) {
+		fts_write_reg(0xD1, 0xFF);
+		fts_write_reg(0xD2, 0xFF);
+		fts_write_reg(0xD5, 0xFF);
+		fts_write_reg(0xD6, 0xFF);
+		fts_write_reg(0xD7, 0xFF);
+		fts_write_reg(0xD8, 0xFF);
+		fts_write_reg(FTS_REG_GESTURE_EN, ENABLE);
+		msleep(1);
+		fts_read_reg(FTS_REG_GESTURE_EN, &state);
+		if (state == ENABLE)
+			break;
+	}
+
+	if (i >= 5)
+		FTS_ERROR("make IC enter into gesture(suspend) fail,state:%x", state);
+	else
+		FTS_INFO("Enter into gesture(suspend) successfully");
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+int fts_gesture_resume(struct fts_ts_data *ts_data)
+{
+	int i = 0;
+	u8 state = 0xFF;
+
+	FTS_FUNC_ENTER();
+	if (disable_irq_wake(ts_data->irq)) {
+		FTS_DEBUG("disable_irq_wake(irq:%d) fail", ts_data->irq);
+	}
+
+	for (i = 0; i < 5; i++) {
+		fts_write_reg(FTS_REG_GESTURE_EN, DISABLE);
+		msleep(1);
+		fts_read_reg(FTS_REG_GESTURE_EN, &state);
+		if (state == DISABLE)
+			break;
+	}
+
+	if (i >= 5)
+		FTS_ERROR("make IC exit gesture(resume) fail,state:%x", state);
+	else
+		FTS_INFO("resume from gesture successfully");
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+int fts_gesture_init(struct fts_ts_data *ts_data)
+{
+	struct input_dev *input_dev = ts_data->input_dev;
+
+	FTS_FUNC_ENTER();
+	input_set_capability(input_dev, EV_KEY, KEY_POWER);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_U);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_UP);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_DOWN);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_LEFT);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_RIGHT);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_O);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_E);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_M);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_L);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_W);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_S);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_V);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_Z);
+	input_set_capability(input_dev, EV_KEY, KEY_GESTURE_C);
+
+	__set_bit(KEY_GESTURE_RIGHT, input_dev->keybit);
+	__set_bit(KEY_GESTURE_LEFT, input_dev->keybit);
+	__set_bit(KEY_GESTURE_UP, input_dev->keybit);
+	__set_bit(KEY_GESTURE_DOWN, input_dev->keybit);
+	__set_bit(KEY_GESTURE_U, input_dev->keybit);
+	__set_bit(KEY_GESTURE_O, input_dev->keybit);
+	__set_bit(KEY_GESTURE_E, input_dev->keybit);
+	__set_bit(KEY_GESTURE_M, input_dev->keybit);
+	__set_bit(KEY_GESTURE_W, input_dev->keybit);
+	__set_bit(KEY_GESTURE_L, input_dev->keybit);
+	__set_bit(KEY_GESTURE_S, input_dev->keybit);
+	__set_bit(KEY_GESTURE_V, input_dev->keybit);
+	__set_bit(KEY_GESTURE_C, input_dev->keybit);
+	__set_bit(KEY_GESTURE_Z, input_dev->keybit);
+
+	fts_create_gesture_sysfs(ts_data->dev);
+
+	memset(&fts_gesture_data, 0, sizeof(struct fts_gesture_st));
+	ts_data->gesture_mode = FTS_GESTURE_EN;
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+int fts_gesture_exit(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+	sysfs_remove_group(&ts_data->dev->kobj, &fts_gesture_group);
+	FTS_FUNC_EXIT();
+	return 0;
+}

focaltech_touch/focaltech_i2c.c

@@ -0,0 +1,548 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, FocalTech Systems, Ltd., all rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/************************************************************************
+*
+* File Name: focaltech_i2c.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-08-04
+*
+* Abstract: i2c communication with TP
+*
+* Version: v1.0
+*
+* Revision History:
+*
+************************************************************************/
+
+/*****************************************************************************
+* Included header files
+*****************************************************************************/
+#include "focaltech_core.h"
+#include <linux/pm_runtime.h>
+
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define I2C_RETRY_NUMBER                    3
+#define I2C_BUF_LENGTH                      256
+
+/*****************************************************************************
+* Private enumerations, structures and unions using typedef
+*****************************************************************************/
+
+/*****************************************************************************
+* Static variables
+*****************************************************************************/
+static struct fts_ts_data *ts_data;
+
+/*****************************************************************************
+* Global variable or extern global variabls/functions
+*****************************************************************************/
+
+/*****************************************************************************
+* Static function prototypes
+*****************************************************************************/
+
+/*****************************************************************************
+* functions body
+*****************************************************************************/
+static int fts_i2c_read(u8 *cmd, u32 cmdlen, u8 *data, u32 datalen)
+{
+	int ret = 0;
+	int i = 0;
+	struct i2c_msg msg_list[2];
+	struct i2c_msg *msg = NULL;
+	int msg_num = 0;
+
+	/* must have data when read */
+	if (!ts_data || !ts_data->client || !data || !datalen
+		|| (datalen >= I2C_BUF_LENGTH) || (cmdlen >= I2C_BUF_LENGTH)) {
+		FTS_ERROR("fts_data/client/cmdlen(%d)/data/datalen(%d) is invalid",
+			cmdlen, datalen);
+		return -EINVAL;
+	}
+
+	mutex_lock(&ts_data->bus_lock);
+	memset(&msg_list[0], 0, sizeof(struct i2c_msg));
+	memset(&msg_list[1], 0, sizeof(struct i2c_msg));
+	memcpy(ts_data->bus_tx_buf, cmd, cmdlen);
+	msg_list[0].addr = ts_data->client->addr;
+	msg_list[0].flags = 0;
+	msg_list[0].len = cmdlen;
+	msg_list[0].buf = ts_data->bus_tx_buf;
+	msg_list[1].addr = ts_data->client->addr;
+	msg_list[1].flags = I2C_M_RD;
+	msg_list[1].len = datalen;
+	msg_list[1].buf = ts_data->bus_rx_buf;
+	if (cmd && cmdlen) {
+		msg = &msg_list[0];
+		msg_num = 2;
+	} else {
+		msg = &msg_list[1];
+		msg_num = 1;
+	}
+
+	for (i = 0; i < I2C_RETRY_NUMBER; i++) {
+		ret = i2c_transfer(ts_data->client->adapter, msg, msg_num);
+		if (ret < 0) {
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+#ifdef CONFIG_ARCH_QTI_VM
+			if (atomic_read(&ts_data->trusted_touch_enabled) &&
+					ret == -ECONNRESET) {
+				pr_err("failed i2c read reacquiring session\n");
+				pm_runtime_put_sync(
+					ts_data->client->adapter->dev.parent);
+				pm_runtime_get_sync(
+					ts_data->client->adapter->dev.parent);
+			}
+#endif
+#endif
+			FTS_ERROR("i2c_transfer(read) fail,ret:%d", ret);
+		} else {
+			memcpy(data, ts_data->bus_rx_buf, datalen);
+			break;
+		}
+	}
+
+	if (ret < 0) {
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+#ifdef CONFIG_ARCH_QTI_VM
+		pr_err("initiating abort due to i2c xfer failure\n");
+		fts_ts_trusted_touch_tvm_i2c_failure_report(ts_data);
+#endif
+#endif
+	}
+
+	mutex_unlock(&ts_data->bus_lock);
+	return ret;
+}
+
+static int fts_i2c_write(u8 *writebuf, u32 writelen)
+{
+	int ret = 0;
+	int i = 0;
+	struct i2c_msg msgs;
+
+	if (!ts_data || !ts_data->client || !writebuf || !writelen
+		|| (writelen >= I2C_BUF_LENGTH)) {
+		FTS_ERROR("fts_data/client/data/datalen(%d) is invalid", writelen);
+		return -EINVAL;
+	}
+
+	mutex_lock(&ts_data->bus_lock);
+	memset(&msgs, 0, sizeof(struct i2c_msg));
+	memcpy(ts_data->bus_tx_buf, writebuf, writelen);
+	msgs.addr = ts_data->client->addr;
+	msgs.flags = 0;
+	msgs.len = writelen;
+	msgs.buf = ts_data->bus_tx_buf;
+	for (i = 0; i < I2C_RETRY_NUMBER; i++) {
+		ret = i2c_transfer(ts_data->client->adapter, &msgs, 1);
+		if (ret < 0) {
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+#ifdef CONFIG_ARCH_QTI_VM
+			if (atomic_read(&ts_data->trusted_touch_enabled) &&
+				ret == -ECONNRESET){
+				pr_err("failed i2c write reacquiring session\n");
+				pm_runtime_put_sync(
+					ts_data->client->adapter->dev.parent);
+				pm_runtime_get_sync(
+					ts_data->client->adapter->dev.parent);
+			}
+#endif
+#endif
+			FTS_ERROR("i2c_transfer(write) fail,ret:%d", ret);
+		} else {
+			break;
+		}
+	}
+
+	if (ret < 0) {
+#ifdef CONFIG_FTS_TRUSTED_TOUCH
+#ifdef CONFIG_ARCH_QTI_VM
+		pr_err("initiating abort due to i2c xfer failure\n");
+		fts_ts_trusted_touch_tvm_i2c_failure_report(ts_data);
+#endif
+#endif
+	}
+	mutex_unlock(&ts_data->bus_lock);
+	return ret;
+}
+
+static int fts_i2c_init(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+	ts_data->bus_tx_buf = kzalloc(I2C_BUF_LENGTH, GFP_KERNEL);
+	if (ts_data->bus_tx_buf == NULL) {
+		FTS_ERROR("failed to allocate memory for bus_tx_buf");
+		return -ENOMEM;
+	}
+
+	ts_data->bus_rx_buf = kzalloc(I2C_BUF_LENGTH, GFP_KERNEL);
+	if (ts_data->bus_rx_buf == NULL) {
+		FTS_ERROR("failed to allocate memory for bus_rx_buf");
+		return -ENOMEM;
+	}
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+static int fts_i2c_exit(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+	if (ts_data && ts_data->bus_tx_buf) {
+		kfree(ts_data->bus_tx_buf);
+		ts_data->bus_tx_buf = NULL;
+	}
+
+	if (ts_data && ts_data->bus_rx_buf) {
+		kfree(ts_data->bus_rx_buf);
+		ts_data->bus_rx_buf = NULL;
+	}
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+/*****************************************************************************
+ * Private constant and macro definitions using #define
+ ****************************************************************************/
+#define SPI_RETRY_NUMBER            3
+#define CS_HIGH_DELAY               150 /* unit: us */
+#define SPI_BUF_LENGTH              256
+
+#define DATA_CRC_EN                 0x20
+#define WRITE_CMD                   0x00
+#define READ_CMD                    (0x80 | DATA_CRC_EN)
+
+#define SPI_DUMMY_BYTE              3
+#define SPI_HEADER_LENGTH           6   /*CRC*/
+
+/*****************************************************************************
+ * functions body
+ ****************************************************************************/
+/* spi interface */
+static int fts_spi_transfer(u8 *tx_buf, u8 *rx_buf, u32 len)
+{
+	int ret = 0;
+	struct spi_device *spi = fts_data->spi;
+	struct spi_message msg;
+	struct spi_transfer xfer = {
+		.tx_buf = tx_buf,
+		.rx_buf = rx_buf,
+		.len    = len,
+	};
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	ret = spi_sync(spi, &msg);
+	if (ret) {
+		FTS_ERROR("spi_sync fail,ret:%d", ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static void crckermit(u8 *data, u32 len, u16 *crc_out)
+{
+	u32 i = 0;
+	u32 j = 0;
+	u16 crc = 0xFFFF;
+
+	for (i = 0; i < len; i++) {
+		crc ^= data[i];
+		for (j = 0; j < 8; j++) {
+			if (crc & 0x01)
+				crc = (crc >> 1) ^ 0x8408;
+			else
+				crc = (crc >> 1);
+		}
+	}
+
+	*crc_out = crc;
+}
+
+static int rdata_check(u8 *rdata, u32 rlen)
+{
+	u16 crc_calc = 0;
+	u16 crc_read = 0;
+
+	crckermit(rdata, rlen - 2, &crc_calc);
+	crc_read = (u16)(rdata[rlen - 1] << 8) + rdata[rlen - 2];
+	if (crc_calc != crc_read)
+		return -EIO;
+
+	return 0;
+}
+
+static int fts_spi_write(u8 *writebuf, u32 writelen)
+{
+	int ret = 0;
+	int i = 0;
+	struct fts_ts_data *ts_data = fts_data;
+	u8 *txbuf = NULL;
+	u8 *rxbuf = NULL;
+	u32 txlen = 0;
+	u32 txlen_need = writelen + SPI_HEADER_LENGTH + ts_data->dummy_byte;
+	u32 datalen = writelen - 1;
+
+	if (!writebuf || !writelen) {
+		FTS_ERROR("writebuf/len is invalid");
+		return -EINVAL;
+	}
+
+	mutex_lock(&ts_data->bus_lock);
+	if (txlen_need > SPI_BUF_LENGTH) {
+		txbuf = kzalloc(txlen_need, GFP_KERNEL);
+		if (txbuf == NULL) {
+			FTS_ERROR("txbuf malloc fail");
+			ret = -ENOMEM;
+			goto err_write;
+		}
+
+		rxbuf = kzalloc(txlen_need, GFP_KERNEL);
+		if (rxbuf == NULL) {
+			FTS_ERROR("rxbuf malloc fail");
+			ret = -ENOMEM;
+			goto err_write;
+		}
+	} else {
+		txbuf = ts_data->bus_tx_buf;
+		rxbuf = ts_data->bus_rx_buf;
+		memset(txbuf, 0x0, SPI_BUF_LENGTH);
+		memset(rxbuf, 0x0, SPI_BUF_LENGTH);
+	}
+
+	txbuf[txlen++] = writebuf[0];
+	txbuf[txlen++] = WRITE_CMD;
+	txbuf[txlen++] = (datalen >> 8) & 0xFF;
+	txbuf[txlen++] = datalen & 0xFF;
+	if (datalen > 0) {
+		txlen = txlen + SPI_DUMMY_BYTE;
+		memcpy(&txbuf[txlen], &writebuf[1], datalen);
+		txlen = txlen + datalen;
+	}
+
+	for (i = 0; i < SPI_RETRY_NUMBER; i++) {
+		ret = fts_spi_transfer(txbuf, rxbuf, txlen);
+		if ((ret == 0) && ((rxbuf[3] & 0xA0) == 0))
+			break;
+
+		FTS_DEBUG("data write(addr:%x),status:%x,retry:%d,ret:%d",
+				writebuf[0], rxbuf[3], i, ret);
+		ret = -EIO;
+		udelay(CS_HIGH_DELAY);
+	}
+
+	if (ret < 0) {
+		FTS_ERROR("data write(addr:%x) fail,status:%x,ret:%d",
+				writebuf[0], rxbuf[3], ret);
+	}
+
+err_write:
+	if (txlen_need > SPI_BUF_LENGTH) {
+		kfree(txbuf);
+		kfree(rxbuf);
+	}
+
+	udelay(CS_HIGH_DELAY);
+	mutex_unlock(&ts_data->bus_lock);
+	return ret;
+}
+
+static int fts_spi_read(u8 *cmd, u32 cmdlen, u8 *data, u32 datalen)
+{
+	int ret = 0;
+	int i = 0;
+	u8 *txbuf = NULL;
+	u8 *rxbuf = NULL;
+	u32 txlen = 0;
+	u32 txlen_need = datalen + SPI_HEADER_LENGTH + ts_data->dummy_byte;
+	u8 ctrl = READ_CMD;
+	u32 dp = 0;
+
+	if (!cmd || !cmdlen || !data || !datalen) {
+		FTS_ERROR("cmd/cmdlen/data/datalen is invalid");
+		return -EINVAL;
+	}
+
+	mutex_lock(&ts_data->bus_lock);
+	if (txlen_need > SPI_BUF_LENGTH) {
+		txbuf = kzalloc(txlen_need, GFP_KERNEL);
+		if (txbuf == NULL) {
+			FTS_ERROR("txbuf malloc fail");
+			ret = -ENOMEM;
+			goto err_read;
+		}
+
+		rxbuf = kzalloc(txlen_need, GFP_KERNEL);
+		if (rxbuf == NULL) {
+			FTS_ERROR("rxbuf malloc fail");
+			ret = -ENOMEM;
+			goto err_read;
+		}
+	} else {
+		txbuf = ts_data->bus_tx_buf;
+		rxbuf = ts_data->bus_rx_buf;
+		memset(txbuf, 0x0, SPI_BUF_LENGTH);
+		memset(rxbuf, 0x0, SPI_BUF_LENGTH);
+	}
+
+	txbuf[txlen++] = cmd[0];
+	txbuf[txlen++] = ctrl;
+	txbuf[txlen++] = (datalen >> 8) & 0xFF;
+	txbuf[txlen++] = datalen & 0xFF;
+	dp = txlen + SPI_DUMMY_BYTE;
+	txlen = dp + datalen;
+	if (ctrl & DATA_CRC_EN)
+		txlen = txlen + 2;
+
+	for (i = 0; i < SPI_RETRY_NUMBER; i++) {
+		ret = fts_spi_transfer(txbuf, rxbuf, txlen);
+		if ((ret == 0) && ((rxbuf[3] & 0xA0) == 0)) {
+			memcpy(data, &rxbuf[dp], datalen);
+			/* crc check */
+			if (ctrl & DATA_CRC_EN) {
+				ret = rdata_check(&rxbuf[dp], txlen - dp);
+				if (ret < 0) {
+					FTS_DEBUG("data read(addr:%x) crc abnormal,retry:%d",
+							cmd[0], i);
+					udelay(CS_HIGH_DELAY);
+					continue;
+				}
+			}
+			break;
+		}
+
+		FTS_DEBUG("data read(addr:%x) status:%x,retry:%d,ret:%d",
+				cmd[0], rxbuf[3], i, ret);
+		ret = -EIO;
+		udelay(CS_HIGH_DELAY);
+	}
+
+	if (ret < 0) {
+		FTS_ERROR("data read(addr:%x) %s,status:%x,ret:%d", cmd[0],
+				(i >= SPI_RETRY_NUMBER) ? "crc abnormal" : "fail",
+				rxbuf[3], ret);
+	}
+
+err_read:
+	if (txlen_need > SPI_BUF_LENGTH) {
+		kfree(txbuf);
+		kfree(rxbuf);
+	}
+
+	udelay(CS_HIGH_DELAY);
+	mutex_unlock(&ts_data->bus_lock);
+	return ret;
+}
+
+static int fts_spi_init(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+	ts_data->bus_tx_buf = kzalloc(SPI_BUF_LENGTH, GFP_KERNEL);
+	if (ts_data->bus_tx_buf == NULL) {
+		FTS_ERROR("failed to allocate memory for bus_tx_buf");
+		return -ENOMEM;
+	}
+
+	ts_data->bus_rx_buf = kzalloc(SPI_BUF_LENGTH, GFP_KERNEL);
+	if (ts_data->bus_rx_buf == NULL) {
+		FTS_ERROR("failed to allocate memory for bus_rx_buf");
+		return -ENOMEM;
+	}
+
+	ts_data->dummy_byte = SPI_DUMMY_BYTE;
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+static int fts_spi_exit(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+	if (ts_data && ts_data->bus_tx_buf) {
+		kfree(ts_data->bus_tx_buf);
+		ts_data->bus_tx_buf = NULL;
+	}
+
+	if (ts_data && ts_data->bus_rx_buf) {
+		kfree(ts_data->bus_rx_buf);
+		ts_data->bus_rx_buf = NULL;
+	}
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+int fts_read(u8 *cmd, u32 cmdlen, u8 *data, u32 datalen)
+{
+	int ret = 0;
+
+	if (ts_data->bus_type == BUS_TYPE_I2C)
+		ret = fts_i2c_read(cmd, cmdlen, data, datalen);
+	else
+		ret = fts_spi_read(cmd, cmdlen, data, datalen);
+
+	return ret;
+}
+
+int fts_write(u8 *writebuf, u32 writelen)
+{
+	int ret = 0;
+
+	if (ts_data->bus_type == BUS_TYPE_I2C)
+		ret = fts_i2c_write(writebuf, writelen);
+	else
+		ret = fts_spi_write(writebuf, writelen);
+
+	return ret;
+}
+
+int fts_read_reg(u8 addr, u8 *value)
+{
+	return fts_read(&addr, 1, value, 1);
+}
+
+int fts_write_reg(u8 addr, u8 value)
+{
+	u8 buf[2] = { 0 };
+
+	buf[0] = addr;
+	buf[1] = value;
+	return fts_write(buf, sizeof(buf));
+}
+
+int fts_bus_init(struct fts_ts_data *_ts_data)
+{
+	ts_data = _ts_data;
+
+	if (ts_data->bus_type == BUS_TYPE_I2C)
+		return fts_i2c_init(ts_data);
+
+	return fts_spi_init(ts_data);
+}
+
+int fts_bus_exit(struct fts_ts_data *ts_data)
+{
+	if (ts_data->bus_type == BUS_TYPE_I2C)
+		return fts_i2c_exit(ts_data);
+
+	return fts_spi_exit(ts_data);
+}

focaltech_touch/focaltech_point_report_check.c

@@ -0,0 +1,135 @@
+/*
+ *
+ * FocalTech TouchScreen driver.
+ *
+ * Copyright (c) 2012-2019, FocalTech Systems, Ltd., all rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/*****************************************************************************
+*
+* File Name: focaltech_point_report_check.c
+*
+* Author: Focaltech Driver Team
+*
+* Created: 2016-11-16
+*
+* Abstract: point report check function
+*
+* Version: v1.0
+*
+* Revision History:
+*
+*****************************************************************************/
+
+/*****************************************************************************
+* Included header files
+*****************************************************************************/
+#include "focaltech_core.h"
+
+#if FTS_POINT_REPORT_CHECK_EN
+/*****************************************************************************
+* Private constant and macro definitions using #define
+*****************************************************************************/
+#define POINT_REPORT_CHECK_WAIT_TIME              200    /* unit:ms */
+
+/*****************************************************************************
+* functions body
+*****************************************************************************/
+/*****************************************************************************
+*  Name: fts_prc_func
+*  Brief: fts point report check work func, report whole up of points
+*  Input:
+*  Output:
+*  Return:
+*****************************************************************************/
+static void fts_prc_func(struct work_struct *work)
+{
+	struct fts_ts_data *ts_data = container_of(work,
+					struct fts_ts_data, prc_work.work);
+	struct input_dev *input_dev = ts_data->input_dev;
+#if FTS_MT_PROTOCOL_B_EN
+	u32 finger_count = 0;
+	u32 max_touches = fts_data->pdata->max_touch_number;
+#endif
+
+	FTS_FUNC_ENTER();
+	mutex_lock(&ts_data->report_mutex);
+
+#if FTS_MT_PROTOCOL_B_EN
+	for (finger_count = 0; finger_count < max_touches; finger_count++) {
+		input_mt_slot(input_dev, finger_count);
+		input_mt_report_slot_state(input_dev, MT_TOOL_FINGER, false);
+	}
+#else
+	input_mt_sync(input_dev);
+#endif
+	input_report_key(input_dev, BTN_TOUCH, 0);
+	input_sync(input_dev);
+
+	mutex_unlock(&ts_data->report_mutex);
+
+	FTS_FUNC_EXIT();
+}
+
+/*****************************************************************************
+*  Name: fts_prc_queue_work
+*  Brief: fts point report check queue work, call it when interrupt comes
+*  Input:
+*  Output:
+*  Return:
+*****************************************************************************/
+void fts_prc_queue_work(struct fts_ts_data *ts_data)
+{
+	cancel_delayed_work_sync(&ts_data->prc_work);
+	queue_delayed_work(ts_data->ts_workqueue, &ts_data->prc_work,
+			msecs_to_jiffies(POINT_REPORT_CHECK_WAIT_TIME));
+}
+
+/*****************************************************************************
+*  Name: fts_point_report_check_init
+*  Brief:
+*  Input:
+*  Output:
+*  Return: < 0: Fail to create esd check queue
+*****************************************************************************/
+int fts_point_report_check_init(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+
+	if (ts_data->ts_workqueue) {
+		INIT_DELAYED_WORK(&ts_data->prc_work, fts_prc_func);
+	} else {
+		FTS_ERROR("fts workqueue is NULL, can't run point report check function");
+		return -EINVAL;
+	}
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+
+/*****************************************************************************
+*  Name: fts_point_report_check_exit
+*  Brief:
+*  Input:
+*  Output:
+*  Return:
+*****************************************************************************/
+int fts_point_report_check_exit(struct fts_ts_data *ts_data)
+{
+	FTS_FUNC_ENTER();
+
+	FTS_FUNC_EXIT();
+	return 0;
+}
+#endif /* FTS_POINT_REPORT_CHECK_EN */
+

nt36xxx/nt36xxx.c

@@ -0,0 +1,4601 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2010 - 2018 Novatek, Inc.
+ *
+ * $Revision: 47247 $
+ * $Date: 2019-07-10 10:41:36 +0800 (Wed, 10 Jul 2019) $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+
+#if !defined(NVT_NT36XXX_SPI) /* NT36XXX I2C */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/proc_fs.h>
+#include <linux/input/mt.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+
+#if defined(CONFIG_DRM)
+#include <linux/soc/qcom/panel_event_notifier.h>
+#endif
+
+#if defined(CONFIG_DRM_PANEL)
+#include <drm/drm_panel.h>
+#elif defined(CONFIG_FB)
+#include <linux/notifier.h>
+#include <linux/fb.h>
+#elif defined(CONFIG_HAS_EARLYSUSPEND)
+#include <linux/earlysuspend.h>
+#endif
+
+#include "nt36xxx.h"
+#if NVT_TOUCH_ESD_PROTECT
+#include <linux/jiffies.h>
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+#if NVT_TOUCH_ESD_PROTECT
+static struct delayed_work nvt_esd_check_work;
+static struct workqueue_struct *nvt_esd_check_wq;
+static unsigned long irq_timer = 0;
+uint8_t esd_check = false;
+uint8_t esd_retry = 0;
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+#if NVT_TOUCH_EXT_PROC
+extern int32_t nvt_extra_proc_init(void);
+extern void nvt_extra_proc_deinit(void);
+#endif
+
+#if NVT_TOUCH_MP
+extern int32_t nvt_mp_proc_init(void);
+extern void nvt_mp_proc_deinit(void);
+#endif
+
+struct nvt_ts_data *ts;
+
+#if BOOT_UPDATE_FIRMWARE
+static struct workqueue_struct *nvt_fwu_wq;
+extern void Boot_Update_Firmware(struct work_struct *work);
+#endif
+
+#if defined(CONFIG_DRM)
+static struct drm_panel *active_panel;
+static void nvt_i2c_panel_notifier_callback(enum panel_event_notifier_tag tag,
+			struct panel_event_notification *notification, void *client_data);
+
+#elif defined(_MSM_DRM_NOTIFY_H_)
+static int nvt_drm_notifier_callback(struct notifier_block *self, unsigned long event, void *data);
+
+#else
+static int nvt_fb_notifier_callback(struct notifier_block *self, unsigned long event, void *data);
+#endif
+
+#if TOUCH_KEY_NUM > 0
+const uint16_t touch_key_array[TOUCH_KEY_NUM] = {
+	KEY_BACK,
+	KEY_HOME,
+	KEY_MENU
+};
+#endif
+
+#if WAKEUP_GESTURE
+const uint16_t gesture_key_array[] = {
+	KEY_POWER,  //GESTURE_WORD_C
+	KEY_POWER,  //GESTURE_WORD_W
+	KEY_POWER,  //GESTURE_WORD_V
+	KEY_POWER,  //GESTURE_DOUBLE_CLICK
+	KEY_POWER,  //GESTURE_WORD_Z
+	KEY_POWER,  //GESTURE_WORD_M
+	KEY_POWER,  //GESTURE_WORD_O
+	KEY_POWER,  //GESTURE_WORD_e
+	KEY_POWER,  //GESTURE_WORD_S
+	KEY_POWER,  //GESTURE_SLIDE_UP
+	KEY_POWER,  //GESTURE_SLIDE_DOWN
+	KEY_POWER,  //GESTURE_SLIDE_LEFT
+	KEY_POWER,  //GESTURE_SLIDE_RIGHT
+};
+#endif
+
+static uint8_t bTouchIsAwake = 0;
+
+#if defined(CONFIG_DRM)
+static void nvt_i2c_register_for_panel_events(struct device_node *dp,
+		struct nvt_ts_data *ts)
+{
+	void *cookie = NULL;
+
+	cookie = panel_event_notifier_register(PANEL_EVENT_NOTIFICATION_PRIMARY,
+			PANEL_EVENT_NOTIFIER_CLIENT_PRIMARY_TOUCH, active_panel,
+			&nvt_i2c_panel_notifier_callback, ts);
+	if (!cookie) {
+		pr_err("Failed to register for panel events\n");
+		return;
+	}
+	ts->notifier_cookie = cookie;
+}
+#endif
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen irq enable/disable function.
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+static void nvt_irq_enable(bool enable)
+{
+	struct irq_desc *desc;
+
+	if (enable) {
+		if (!ts->irq_enabled) {
+			enable_irq(ts->client->irq);
+			ts->irq_enabled = true;
+		}
+	} else {
+		if (ts->irq_enabled) {
+			disable_irq(ts->client->irq);
+			ts->irq_enabled = false;
+		}
+	}
+
+	desc = irq_to_desc(ts->client->irq);
+	NVT_LOG("enable=%d, desc->depth=%d\n", enable, desc->depth);
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen i2c read function.
+ *
+ * return:
+ *     Executive outcomes. 2---succeed. -5---I/O error
+ *******************************************************/
+int32_t CTP_I2C_READ(struct i2c_client *client, uint16_t address, uint8_t *buf,
+		uint16_t len)
+{
+	struct i2c_msg msgs[2];
+	int32_t ret = -1;
+	int32_t retries = 0;
+
+	mutex_lock(&ts->xbuf_lock);
+
+	msgs[0].flags = !I2C_M_RD;
+	msgs[0].addr  = address;
+	msgs[0].len   = 1;
+	msgs[0].buf   = &buf[0];
+
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].addr  = address;
+	msgs[1].len   = len - 1;
+	msgs[1].buf   = ts->xbuf;
+
+	while (retries < 5) {
+		ret = i2c_transfer(client->adapter, msgs, 2);
+		if (ret == 2)	break;
+		retries++;
+	}
+
+	if (unlikely(retries == 5)) {
+		NVT_ERR("error, ret=%d\n", ret);
+		ret = -EIO;
+	}
+
+	memcpy(buf + 1, ts->xbuf, len - 1);
+
+	mutex_unlock(&ts->xbuf_lock);
+
+	return ret;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen i2c write function.
+ *
+ * return:
+ *     Executive outcomes. 1---succeed. -5---I/O error
+ *******************************************************/
+int32_t CTP_I2C_WRITE(struct i2c_client *client, uint16_t address, uint8_t *buf,
+		uint16_t len)
+{
+	struct i2c_msg msg;
+	int32_t ret = -1;
+	int32_t retries = 0;
+
+	mutex_lock(&ts->xbuf_lock);
+
+	msg.flags = !I2C_M_RD;
+	msg.addr  = address;
+	msg.len   = len;
+	memcpy(ts->xbuf, buf, len);
+	msg.buf   = ts->xbuf;
+
+	while (retries < 5) {
+		ret = i2c_transfer(client->adapter, &msg, 1);
+		if (ret == 1)	break;
+		retries++;
+	}
+
+	if (unlikely(retries == 5)) {
+		NVT_ERR("error, ret=%d\n", ret);
+		ret = -EIO;
+	}
+
+	mutex_unlock(&ts->xbuf_lock);
+
+	return ret;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen set index/page/addr address.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. -5---access fail.
+ ********************************************************/
+int32_t nvt_set_page(uint16_t i2c_addr, uint32_t addr)
+{
+	uint8_t buf[4] = {0};
+
+	buf[0] = 0xFF;	//set index/page/addr command
+	buf[1] = (addr >> 16) & 0xFF;
+	buf[2] = (addr >> 8) & 0xFF;
+
+	return CTP_I2C_WRITE(ts->client, i2c_addr, buf, 3);
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen reset MCU then into idle mode
+ * function.
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+void nvt_sw_reset_idle(void)
+{
+	uint8_t buf[4]={0};
+
+	//---write i2c cmds to reset idle---
+	buf[0]=0x00;
+	buf[1]=0xA5;
+	CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+
+	msleep(15);
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen reset MCU (boot) function.
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+void nvt_bootloader_reset(void)
+{
+	uint8_t buf[8] = {0};
+
+	NVT_LOG("start\n");
+
+	//---write i2c cmds to reset---
+	buf[0] = 0x00;
+	buf[1] = 0x69;
+	CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+
+	// need 35ms delay after bootloader reset
+	msleep(35);
+
+	NVT_LOG("end\n");
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen clear FW status function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. -1---fail.
+ *******************************************************/
+int32_t nvt_clear_fw_status(void)
+{
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 20;
+
+	for (i = 0; i < retry; i++) {
+		//---set xdata index to EVENT BUF ADDR---
+		nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE);
+
+		//---clear fw status---
+		buf[0] = EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = 0x00;
+		CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 2);
+
+		//---read fw status---
+		buf[0] = EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = 0xFF;
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 2);
+
+		if (buf[1] == 0x00)
+			break;
+
+		usleep_range(10000, 10000);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("failed, i=%d, buf[1]=0x%02X\n", i, buf[1]);
+		return -1;
+	} else {
+		return 0;
+	}
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen check FW status function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. -1---failed.
+ *******************************************************/
+int32_t nvt_check_fw_status(void)
+{
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 50;
+
+	for (i = 0; i < retry; i++) {
+		//---set xdata index to EVENT BUF ADDR---
+		nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE);
+
+		//---read fw status---
+		buf[0] = EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = 0x00;
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 2);
+
+		if ((buf[1] & 0xF0) == 0xA0)
+			break;
+
+		usleep_range(10000, 10000);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("failed, i=%d, buf[1]=0x%02X\n", i, buf[1]);
+		return -1;
+	} else {
+		return 0;
+	}
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen check FW reset state function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. -1---failed.
+ ******************************************************/
+int32_t nvt_check_fw_reset_state(RST_COMPLETE_STATE check_reset_state)
+{
+	uint8_t buf[8] = {0};
+	int32_t ret = 0;
+	int32_t retry = 0;
+
+	while (1) {
+		usleep_range(10000, 10000);
+
+		//---read reset state---
+		buf[0] = EVENT_MAP_RESET_COMPLETE;
+		buf[1] = 0x00;
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 6);
+
+		if ((buf[1] >= check_reset_state) && (buf[1] <= RESET_STATE_MAX)) {
+			ret = 0;
+			break;
+		}
+
+		retry++;
+		if(unlikely(retry > 100)) {
+			NVT_ERR("error, retry=%d, buf[1]=0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X\n",
+				retry, buf[1], buf[2], buf[3], buf[4], buf[5]);
+			ret = -1;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen get novatek project id information
+ * function.
+ *
+ * return:
+ *     Executive outcomes. 0---success. -1---fail.
+ *******************************************************/
+int32_t nvt_read_pid(void)
+{
+	uint8_t buf[3] = {0};
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_PROJECTID);
+
+	//---read project id---
+	buf[0] = EVENT_MAP_PROJECTID;
+	buf[1] = 0x00;
+	buf[2] = 0x00;
+	CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 3);
+
+	ts->nvt_pid = (buf[2] << 8) + buf[1];
+
+	NVT_LOG("PID=%04X\n", ts->nvt_pid);
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen get firmware related information
+	function.
+
+return:
+	Executive outcomes. 0---success. -1---fail.
+*******************************************************/
+int32_t nvt_get_fw_info(void)
+{
+	uint8_t buf[64] = {0};
+	uint32_t retry_count = 0;
+	int32_t ret = 0;
+
+info_retry:
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_FWINFO);
+
+	//---read fw info---
+	buf[0] = EVENT_MAP_FWINFO;
+	CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 17);
+	ts->fw_ver = buf[1];
+	ts->x_num = buf[3];
+	ts->y_num = buf[4];
+	ts->abs_x_max = (uint16_t)((buf[5] << 8) | buf[6]);
+	ts->abs_y_max = (uint16_t)((buf[7] << 8) | buf[8]);
+	ts->max_button_num = buf[11];
+
+	//---clear x_num, y_num if fw info is broken---
+	if ((buf[1] + buf[2]) != 0xFF) {
+		NVT_ERR("FW info is broken! fw_ver=0x%02X, ~fw_ver=0x%02X\n", buf[1], buf[2]);
+		ts->fw_ver = 0;
+		ts->x_num = 18;
+		ts->y_num = 32;
+		ts->abs_x_max = TOUCH_DEFAULT_MAX_WIDTH;
+		ts->abs_y_max = TOUCH_DEFAULT_MAX_HEIGHT;
+		ts->max_button_num = TOUCH_KEY_NUM;
+
+		if(retry_count < 3) {
+			retry_count++;
+			NVT_ERR("retry_count=%d\n", retry_count);
+			goto info_retry;
+		} else {
+			NVT_ERR("Set default fw_ver=%d, x_num=%d, y_num=%d, "
+					"abs_x_max=%d, abs_y_max=%d, max_button_num=%d!\n",
+					ts->fw_ver, ts->x_num, ts->y_num,
+					ts->abs_x_max, ts->abs_y_max, ts->max_button_num);
+			ret = -1;
+		}
+	} else {
+		ret = 0;
+	}
+
+	//---Get Novatek PID---
+	nvt_read_pid();
+
+	return ret;
+}
+
+/*******************************************************
+ * Create Device Node (Proc Entry)
+ *******************************************************/
+#if NVT_TOUCH_PROC
+static struct proc_dir_entry *NVT_proc_entry;
+#define DEVICE_NAME	"NVTflash"
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen /proc/NVTflash read function.
+ *
+ * return:
+ *     Executive outcomes. 2---succeed. -5,-14---failed.
+ *******************************************************/
+static ssize_t nvt_flash_read(struct file *file, char __user *buff,
+	size_t count, loff_t *offp)
+{
+	uint8_t str[68] = {0};
+	int32_t ret = -1;
+	int32_t retries = 0;
+	int8_t i2c_wr = 0;
+
+	if (count > sizeof(str)) {
+		NVT_ERR("error count=%zu\n", count);
+		return -EFAULT;
+	}
+
+	if (copy_from_user(str, buff, count)) {
+		NVT_ERR("copy from user error\n");
+		return -EFAULT;
+	}
+
+#if NVT_TOUCH_ESD_PROTECT
+	/*
+	 * stop esd check work to avoid case that 0x77 report righ after here to enable esd check again
+	 * finally lead to trigger esd recovery bootloader reset
+	 */
+	cancel_delayed_work_sync(&nvt_esd_check_work);
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	i2c_wr = str[0] >> 7;
+
+	if (i2c_wr == 0) {	//I2C write
+		while (retries < 20) {
+			ret = CTP_I2C_WRITE(ts->client, (str[0] & 0x7F), &str[2], str[1]);
+			if (ret == 1)
+				break;
+			else
+				NVT_ERR("error, retries=%d, ret=%d\n", retries, ret);
+
+			retries++;
+		}
+
+		if (unlikely(retries == 20)) {
+			NVT_ERR("error, ret = %d\n", ret);
+			return -EIO;
+		}
+
+		return ret;
+	} else if (i2c_wr == 1) {	//I2C read
+		while (retries < 20) {
+			ret = CTP_I2C_READ(ts->client, (str[0] & 0x7F), &str[2], str[1]);
+			if (ret == 2)
+				break;
+			else
+				NVT_ERR("error, retries=%d, ret=%d\n", retries, ret);
+
+			retries++;
+		}
+
+		// copy buff to user if i2c transfer
+		if (retries < 20) {
+			if (copy_to_user(buff, str, count))
+				return -EFAULT;
+		}
+
+		if (unlikely(retries == 20)) {
+			NVT_ERR("error, ret = %d\n", ret);
+			return -EIO;
+		}
+
+		return ret;
+	} else {
+		NVT_ERR("Call error, str[0]=%d\n", str[0]);
+		return -EFAULT;
+	}
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen /proc/NVTflash open function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. -12---failed.
+ *******************************************************/
+static int32_t nvt_flash_open(struct inode *inode, struct file *file)
+{
+	struct nvt_flash_data *dev;
+
+	dev = kmalloc(sizeof(struct nvt_flash_data), GFP_KERNEL);
+	if (dev == NULL) {
+		NVT_ERR("Failed to allocate memory for nvt flash data\n");
+		return -ENOMEM;
+	}
+
+	rwlock_init(&dev->lock);
+	file->private_data = dev;
+
+	return 0;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen /proc/NVTflash close function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed.
+ *******************************************************/
+static int32_t nvt_flash_close(struct inode *inode, struct file *file)
+{
+	struct nvt_flash_data *dev = file->private_data;
+
+	kfree(dev);
+
+	return 0;
+}
+
+static const struct proc_ops nvt_flash_fops = {
+	.proc_open = nvt_flash_open,
+	.proc_release = nvt_flash_close,
+	.proc_read = nvt_flash_read,
+};
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen /proc/NVTflash initial function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. -12---failed.
+ *******************************************************/
+static int32_t nvt_flash_proc_init(void)
+{
+	NVT_proc_entry = proc_create(DEVICE_NAME, 0444, NULL,&nvt_flash_fops);
+	if (NVT_proc_entry == NULL) {
+		NVT_ERR("Failed!\n");
+		return -ENOMEM;
+	} else {
+		NVT_LOG("Succeeded!\n");
+	}
+
+	NVT_LOG("==========================================================\n");
+	NVT_LOG("Create /proc/%s\n", DEVICE_NAME);
+	NVT_LOG("==========================================================\n");
+
+	return 0;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen /proc/NVTflash deinitial function.
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+static void nvt_flash_proc_deinit(void)
+{
+	if (NVT_proc_entry != NULL) {
+		remove_proc_entry(DEVICE_NAME, NULL);
+		NVT_proc_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", DEVICE_NAME);
+	}
+}
+#endif
+
+#if WAKEUP_GESTURE
+#define GESTURE_WORD_C          12
+#define GESTURE_WORD_W          13
+#define GESTURE_WORD_V          14
+#define GESTURE_DOUBLE_CLICK    15
+#define GESTURE_WORD_Z          16
+#define GESTURE_WORD_M          17
+#define GESTURE_WORD_O          18
+#define GESTURE_WORD_e          19
+#define GESTURE_WORD_S          20
+#define GESTURE_SLIDE_UP        21
+#define GESTURE_SLIDE_DOWN      22
+#define GESTURE_SLIDE_LEFT      23
+#define GESTURE_SLIDE_RIGHT     24
+/* customized gesture id */
+#define DATA_PROTOCOL           30
+
+/* function page definition */
+#define FUNCPAGE_GESTURE         1
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen wake up gesture key report function.
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+void nvt_ts_wakeup_gesture_report(uint8_t gesture_id, uint8_t *data)
+{
+	uint32_t keycode = 0;
+	uint8_t func_type = data[2];
+	uint8_t func_id = data[3];
+
+	/* support fw specifal data protocol */
+	if ((gesture_id == DATA_PROTOCOL) && (func_type == FUNCPAGE_GESTURE)) {
+		gesture_id = func_id;
+	} else if (gesture_id > DATA_PROTOCOL) {
+		NVT_ERR("gesture_id %d is invalid, func_type=%d, func_id=%d\n", gesture_id, func_type, func_id);
+		return;
+	}
+
+	NVT_LOG("gesture_id = %d\n", gesture_id);
+
+	switch (gesture_id) {
+		case GESTURE_WORD_C:
+			NVT_LOG("Gesture : Word-C.\n");
+			keycode = gesture_key_array[0];
+			break;
+		case GESTURE_WORD_W:
+			NVT_LOG("Gesture : Word-W.\n");
+			keycode = gesture_key_array[1];
+			break;
+		case GESTURE_WORD_V:
+			NVT_LOG("Gesture : Word-V.\n");
+			keycode = gesture_key_array[2];
+			break;
+		case GESTURE_DOUBLE_CLICK:
+			NVT_LOG("Gesture : Double Click.\n");
+			keycode = gesture_key_array[3];
+			break;
+		case GESTURE_WORD_Z:
+			NVT_LOG("Gesture : Word-Z.\n");
+			keycode = gesture_key_array[4];
+			break;
+		case GESTURE_WORD_M:
+			NVT_LOG("Gesture : Word-M.\n");
+			keycode = gesture_key_array[5];
+			break;
+		case GESTURE_WORD_O:
+			NVT_LOG("Gesture : Word-O.\n");
+			keycode = gesture_key_array[6];
+			break;
+		case GESTURE_WORD_e:
+			NVT_LOG("Gesture : Word-e.\n");
+			keycode = gesture_key_array[7];
+			break;
+		case GESTURE_WORD_S:
+			NVT_LOG("Gesture : Word-S.\n");
+			keycode = gesture_key_array[8];
+			break;
+		case GESTURE_SLIDE_UP:
+			NVT_LOG("Gesture : Slide UP.\n");
+			keycode = gesture_key_array[9];
+			break;
+		case GESTURE_SLIDE_DOWN:
+			NVT_LOG("Gesture : Slide DOWN.\n");
+			keycode = gesture_key_array[10];
+			break;
+		case GESTURE_SLIDE_LEFT:
+			NVT_LOG("Gesture : Slide LEFT.\n");
+			keycode = gesture_key_array[11];
+			break;
+		case GESTURE_SLIDE_RIGHT:
+			NVT_LOG("Gesture : Slide RIGHT.\n");
+			keycode = gesture_key_array[12];
+			break;
+		default:
+			break;
+	}
+
+	if (keycode > 0) {
+		input_report_key(ts->input_dev, keycode, 1);
+		input_sync(ts->input_dev);
+		input_report_key(ts->input_dev, keycode, 0);
+		input_sync(ts->input_dev);
+	}
+}
+#endif
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen parse device tree function.
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+#ifdef CONFIG_OF
+static void nvt_parse_dt(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+
+#if NVT_TOUCH_SUPPORT_HW_RST
+	ts->reset_gpio = of_get_named_gpio_flags(np, "novatek,reset-gpio", 0, &ts->reset_flags);
+	NVT_LOG("novatek,reset-gpio=%d\n", ts->reset_gpio);
+#endif
+	ts->irq_gpio = of_get_named_gpio_flags(np, "novatek,irq-gpio", 0, &ts->irq_flags);
+	NVT_LOG("novatek,irq-gpio=%d\n", ts->irq_gpio);
+
+}
+#else
+static void nvt_parse_dt(struct device *dev)
+{
+#if NVT_TOUCH_SUPPORT_HW_RST
+	ts->reset_gpio = NVTTOUCH_RST_PIN;
+#endif
+	ts->irq_gpio = NVTTOUCH_INT_PIN;
+}
+#endif
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen config and request gpio
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. not 0---failed.
+ *******************************************************/
+static int nvt_gpio_config(struct nvt_ts_data *ts)
+{
+	int32_t ret = 0;
+
+#if NVT_TOUCH_SUPPORT_HW_RST
+	/* request RST-pin (Output/High) */
+	if (gpio_is_valid(ts->reset_gpio)) {
+		ret = gpio_request_one(ts->reset_gpio, GPIOF_OUT_INIT_HIGH, "NVT-tp-rst");
+		if (ret) {
+			NVT_ERR("Failed to request NVT-tp-rst GPIO\n");
+			goto err_request_reset_gpio;
+		}
+	}
+#endif
+
+	/* request INT-pin (Input) */
+	if (gpio_is_valid(ts->irq_gpio)) {
+		ret = gpio_request_one(ts->irq_gpio, GPIOF_IN, "NVT-int");
+		if (ret) {
+			NVT_ERR("Failed to request NVT-int GPIO\n");
+			goto err_request_irq_gpio;
+		}
+	}
+
+	return ret;
+
+err_request_irq_gpio:
+#if NVT_TOUCH_SUPPORT_HW_RST
+	gpio_free(ts->reset_gpio);
+err_request_reset_gpio:
+#endif
+	return ret;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen deconfig gpio
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+static void nvt_gpio_deconfig(struct nvt_ts_data *ts)
+{
+	if (gpio_is_valid(ts->irq_gpio))
+		gpio_free(ts->irq_gpio);
+#if NVT_TOUCH_SUPPORT_HW_RST
+	if (gpio_is_valid(ts->reset_gpio))
+		gpio_free(ts->reset_gpio);
+#endif
+}
+
+static uint8_t nvt_fw_recovery(uint8_t *point_data)
+{
+	uint8_t i = 0;
+	uint8_t detected = true;
+
+	/* check pattern */
+	for (i=1 ; i<7 ; i++) {
+		if (point_data[i] != 0x77) {
+			detected = false;
+			break;
+		}
+	}
+
+	return detected;
+}
+
+#if NVT_TOUCH_ESD_PROTECT
+void nvt_esd_check_enable(uint8_t enable)
+{
+	/* update interrupt timer */
+	irq_timer = jiffies;
+	/* clear esd_retry counter, if protect function is enabled */
+	esd_retry = enable ? 0 : esd_retry;
+	/* enable/disable esd check flag */
+	esd_check = enable;
+}
+
+static void nvt_esd_check_func(struct work_struct *work)
+{
+	unsigned int timer = jiffies_to_msecs(jiffies - irq_timer);
+
+	//NVT_ERR("esd_check = %d (retry %d)\n", esd_check, esd_retry);	//DEBUG
+
+	if ((timer > NVT_TOUCH_ESD_CHECK_PERIOD) && esd_check) {
+		mutex_lock(&ts->lock);
+		NVT_ERR("do ESD recovery, timer = %d, retry = %d\n", timer, esd_retry);
+		/* do esd recovery, bootloader reset */
+		nvt_bootloader_reset();
+		mutex_unlock(&ts->lock);
+		/* update interrupt timer */
+		irq_timer = jiffies;
+		/* update esd_retry counter */
+		esd_retry++;
+	}
+
+	queue_delayed_work(nvt_esd_check_wq, &nvt_esd_check_work,
+			msecs_to_jiffies(NVT_TOUCH_ESD_CHECK_PERIOD));
+}
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+#define POINT_DATA_LEN 65
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen work function.
+ *
+ * return:
+ *     n.a.
+ *******************************************************/
+static irqreturn_t nvt_ts_work_func(int irq, void *data)
+{
+	int32_t ret = -1;
+	uint8_t point_data[POINT_DATA_LEN + 1] = {0};
+	uint32_t position = 0;
+	uint32_t input_x = 0;
+	uint32_t input_y = 0;
+	uint32_t input_w = 0;
+	uint32_t input_p = 0;
+	uint8_t input_id = 0;
+#if MT_PROTOCOL_B
+	uint8_t press_id[TOUCH_MAX_FINGER_NUM] = {0};
+#endif /* MT_PROTOCOL_B */
+	int32_t i = 0;
+	int32_t finger_cnt = 0;
+
+#if WAKEUP_GESTURE
+	if (bTouchIsAwake == 0) {
+		pm_wakeup_event(&ts->input_dev->dev, 5000);
+	}
+#endif
+
+	mutex_lock(&ts->lock);
+
+	ret = CTP_I2C_READ(ts->client, I2C_FW_Address, point_data, POINT_DATA_LEN + 1);
+	if (ret < 0) {
+		NVT_ERR("CTP_I2C_READ failed.(%d)\n", ret);
+		goto XFER_ERROR;
+	}
+
+	if (nvt_fw_recovery(point_data)) {
+#if NVT_TOUCH_ESD_PROTECT
+		nvt_esd_check_enable(true);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+		goto XFER_ERROR;
+	}
+
+#if WAKEUP_GESTURE
+	if (bTouchIsAwake == 0) {
+		input_id = (uint8_t)(point_data[1] >> 3);
+		nvt_ts_wakeup_gesture_report(input_id, point_data);
+		mutex_unlock(&ts->lock);
+		return IRQ_HANDLED;
+	}
+#endif
+
+	finger_cnt = 0;
+
+	for (i = 0; i < ts->max_touch_num; i++) {
+		position = 1 + 6 * i;
+		input_id = (uint8_t)(point_data[position + 0] >> 3);
+		if ((input_id == 0) || (input_id > ts->max_touch_num))
+			continue;
+
+		if (((point_data[position] & 0x07) == 0x01) || ((point_data[position] & 0x07) == 0x02)) {	//finger down (enter & moving)
+#if NVT_TOUCH_ESD_PROTECT
+			/* update interrupt timer */
+			irq_timer = jiffies;
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+			input_x = (uint32_t)(point_data[position + 1] << 4) + (uint32_t) (point_data[position + 3] >> 4);
+			input_y = (uint32_t)(point_data[position + 2] << 4) + (uint32_t) (point_data[position + 3] & 0x0F);
+			if ((input_x < 0) || (input_y < 0))
+				continue;
+			if ((input_x > ts->abs_x_max) || (input_y > ts->abs_y_max))
+				continue;
+			input_w = (uint32_t)(point_data[position + 4]);
+			if (input_w == 0)
+				input_w = 1;
+			if (i < 2) {
+				input_p = (uint32_t)(point_data[position + 5]) + (uint32_t)(point_data[i + 63] << 8);
+				if (input_p > TOUCH_FORCE_NUM)
+					input_p = TOUCH_FORCE_NUM;
+			} else {
+				input_p = (uint32_t)(point_data[position + 5]);
+			}
+			if (input_p == 0)
+				input_p = 1;
+
+#if MT_PROTOCOL_B
+			press_id[input_id - 1] = 1;
+			input_mt_slot(ts->input_dev, input_id - 1);
+			input_mt_report_slot_state(ts->input_dev, MT_TOOL_FINGER, true);
+#else /* MT_PROTOCOL_B */
+			input_report_abs(ts->input_dev, ABS_MT_TRACKING_ID, input_id - 1);
+			input_report_key(ts->input_dev, BTN_TOUCH, 1);
+#endif /* MT_PROTOCOL_B */
+
+			input_report_abs(ts->input_dev, ABS_MT_POSITION_X, input_x);
+			input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, input_y);
+			input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, input_w);
+			input_report_abs(ts->input_dev, ABS_MT_PRESSURE, input_p);
+
+#if MT_PROTOCOL_B
+#else /* MT_PROTOCOL_B */
+			input_mt_sync(ts->input_dev);
+#endif /* MT_PROTOCOL_B */
+
+			finger_cnt++;
+		}
+	}
+
+#if MT_PROTOCOL_B
+	for (i = 0; i < ts->max_touch_num; i++) {
+		if (press_id[i] != 1) {
+			input_mt_slot(ts->input_dev, i);
+			input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0);
+			input_report_abs(ts->input_dev, ABS_MT_PRESSURE, 0);
+			input_mt_report_slot_state(ts->input_dev, MT_TOOL_FINGER, false);
+		}
+	}
+
+	input_report_key(ts->input_dev, BTN_TOUCH, (finger_cnt > 0));
+#else /* MT_PROTOCOL_B */
+	if (finger_cnt == 0) {
+		input_report_key(ts->input_dev, BTN_TOUCH, 0);
+		input_mt_sync(ts->input_dev);
+	}
+#endif /* MT_PROTOCOL_B */
+
+#if TOUCH_KEY_NUM > 0
+	if (point_data[61] == 0xF8) {
+#if NVT_TOUCH_ESD_PROTECT
+		/* update interrupt timer */
+		irq_timer = jiffies;
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+		for (i = 0; i < ts->max_button_num; i++) {
+			input_report_key(ts->input_dev, touch_key_array[i], ((point_data[62] >> i) & 0x01));
+		}
+	} else {
+		for (i = 0; i < ts->max_button_num; i++) {
+			input_report_key(ts->input_dev, touch_key_array[i], 0);
+		}
+	}
+#endif
+
+	input_sync(ts->input_dev);
+
+XFER_ERROR:
+
+	mutex_unlock(&ts->lock);
+
+	return IRQ_HANDLED;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen check and stop crc reboot loop.
+ *
+ * return:
+ *     n.a.
+*******************************************************/
+void nvt_stop_crc_reboot(void)
+{
+	uint8_t buf[8] = {0};
+	int32_t retry = 0;
+
+	//read dummy buffer to check CRC fail reboot is happening or not
+
+	//---change I2C index to prevent geting 0xFF, but not 0xFC---
+	nvt_set_page(I2C_BLDR_Address, 0x1F64E);
+
+	//---read to check if buf is 0xFC which means IC is in CRC reboot ---
+	buf[0] = 0x4E;
+	CTP_I2C_READ(ts->client, I2C_BLDR_Address, buf, 4);
+
+	if ((buf[1] == 0xFC) ||
+		((buf[1] == 0xFF) && (buf[2] == 0xFF) && (buf[3] == 0xFF))) {
+
+		//IC is in CRC fail reboot loop, needs to be stopped!
+		for (retry = 5; retry > 0; retry--) {
+
+			//---write i2c cmds to reset idle : 1st---
+			buf[0]=0x00;
+			buf[1]=0xA5;
+			CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+
+			//---write i2c cmds to reset idle : 2rd---
+			buf[0]=0x00;
+			buf[1]=0xA5;
+			CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+			msleep(1);
+
+			//---clear CRC_ERR_FLAG---
+			nvt_set_page(I2C_BLDR_Address, 0x3F135);
+
+			buf[0] = 0x35;
+			buf[1] = 0xA5;
+			CTP_I2C_WRITE(ts->client, I2C_BLDR_Address, buf, 2);
+
+			//---check CRC_ERR_FLAG---
+			nvt_set_page(I2C_BLDR_Address, 0x3F135);
+
+			buf[0] = 0x35;
+			buf[1] = 0x00;
+			CTP_I2C_READ(ts->client, I2C_BLDR_Address, buf, 2);
+
+			if (buf[1] == 0xA5)
+				break;
+		}
+		if (retry == 0)
+			NVT_ERR("CRC auto reboot is not able to be stopped!\n");
+	}
+
+	return;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen check chip version trim function.
+ *
+ * return:
+ *     Executive outcomes. 0---NVT IC. -1---not NVT IC.
+*******************************************************/
+static int8_t nvt_ts_check_chip_ver_trim(uint32_t chip_ver_trim_addr)
+{
+	uint8_t buf[8] = {0};
+	int32_t retry = 0;
+	int32_t list = 0;
+	int32_t i = 0;
+	int32_t found_nvt_chip = 0;
+	int32_t ret = -1;
+
+	nvt_bootloader_reset(); // NOT in retry loop
+
+	//---Check for 5 times---
+	for (retry = 5; retry > 0; retry--) {
+		nvt_sw_reset_idle();
+
+		buf[0] = 0x00;
+		buf[1] = 0x35;
+		CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+		msleep(10);
+
+		nvt_set_page(I2C_BLDR_Address, chip_ver_trim_addr);
+
+		buf[0] = chip_ver_trim_addr & 0xFF;
+		buf[1] = 0x00;
+		buf[2] = 0x00;
+		buf[3] = 0x00;
+		buf[4] = 0x00;
+		buf[5] = 0x00;
+		buf[6] = 0x00;
+		CTP_I2C_READ(ts->client, I2C_BLDR_Address, buf, 7);
+		NVT_LOG("buf[1]=0x%02X, buf[2]=0x%02X, buf[3]=0x%02X, buf[4]=0x%02X, buf[5]=0x%02X, buf[6]=0x%02X\n",
+			buf[1], buf[2], buf[3], buf[4], buf[5], buf[6]);
+
+		//---Stop CRC check to prevent IC auto reboot---
+		if ((buf[1] == 0xFC) ||
+			((buf[1] == 0xFF) && (buf[2] == 0xFF) && (buf[3] == 0xFF))) {
+			nvt_stop_crc_reboot();
+			continue;
+		}
+
+		// compare read chip id on supported list
+		for (list = 0; list < (sizeof(trim_id_table) / sizeof(struct nvt_ts_trim_id_table)); list++) {
+			found_nvt_chip = 0;
+
+			// compare each byte
+			for (i = 0; i < NVT_ID_BYTE_MAX; i++) {
+				if (trim_id_table[list].mask[i]) {
+					if (buf[i + 1] != trim_id_table[list].id[i])
+						break;
+				}
+			}
+
+			if (i == NVT_ID_BYTE_MAX) {
+				found_nvt_chip = 1;
+			}
+
+			if (found_nvt_chip) {
+				NVT_LOG("This is NVT touch IC\n");
+				ts->mmap = trim_id_table[list].mmap;
+				ts->carrier_system = trim_id_table[list].hwinfo->carrier_system;
+				ret = 0;
+				goto out;
+			} else {
+				ts->mmap = NULL;
+				ret = -1;
+			}
+		}
+
+		msleep(10);
+	}
+
+out:
+	return ret;
+}
+
+#if defined(CONFIG_DRM)
+static int nvt_ts_check_dt(struct device_node *np)
+{
+	int i;
+	int count;
+	struct device_node *node;
+	struct drm_panel *panel;
+
+	count = of_count_phandle_with_args(np, "panel", NULL);
+	if (count <= 0)
+		return 0;
+
+	for (i = 0; i < count; i++) {
+		node = of_parse_phandle(np, "panel", i);
+		panel = of_drm_find_panel(node);
+		of_node_put(node);
+		if (!IS_ERR(panel)) {
+			active_panel = panel;
+			NVT_LOG(" %s:find\n", __func__);
+			return 0;
+		}
+	}
+
+	NVT_ERR(" %s: not find\n", __func__);
+	return -ENODEV;
+}
+
+static int nvt_ts_check_default_tp(struct device_node *dt, const char *prop)
+{
+	const char **active_tp = NULL;
+	int count, tmp, score = 0;
+	const char *active;
+	int ret, i;
+
+	count = of_property_count_strings(dt->parent, prop);
+	if (count <= 0 || count > 3)
+		return -ENODEV;
+
+	active_tp = kcalloc(count, sizeof(char *),  GFP_KERNEL);
+	if (!active_tp) {
+		NVT_ERR("FTS alloc failed\n");
+		return -ENOMEM;
+	}
+
+	ret = of_property_read_string_array(dt->parent, prop,
+			active_tp, count);
+	if (ret < 0) {
+		NVT_ERR("fail to read %s %d\n", prop, ret);
+		ret = -ENODEV;
+		goto out;
+	}
+
+	for (i = 0; i < count; i++) {
+		active = active_tp[i];
+		if (active != NULL) {
+			tmp = of_device_is_compatible(dt, active);
+			if (tmp > 0)
+				score++;
+		}
+	}
+
+	if (score <= 0) {
+		NVT_ERR("not match this driver\n");
+		ret = -ENODEV;
+		goto out;
+	}
+	ret = 0;
+out:
+	kfree(active_tp);
+	return ret;
+}
+#endif
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen driver probe function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. negative---failed
+ *******************************************************/
+static int32_t nvt_ts_late_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	int32_t ret = 0;
+#if ((TOUCH_KEY_NUM > 0) || WAKEUP_GESTURE)
+	int32_t retry = 0;
+#endif
+	//---request and config GPIOs---
+	ret = nvt_gpio_config(ts);
+	if (ret) {
+		NVT_ERR("gpio config error!\n");
+		goto err_gpio_config_failed;
+	}
+
+	//---check chip version trim---
+	ret = nvt_ts_check_chip_ver_trim(CHIP_VER_TRIM_ADDR);
+	if (ret) {
+		NVT_LOG("try to check from old chip ver trim address\n");
+		ret = nvt_ts_check_chip_ver_trim(CHIP_VER_TRIM_OLD_ADDR);
+		if (ret) {
+			NVT_ERR("chip is not identified\n");
+			ret = -EINVAL;
+			goto err_chipvertrim_failed;
+		}
+	}
+
+	nvt_bootloader_reset();
+	nvt_check_fw_reset_state(RESET_STATE_INIT);
+	nvt_get_fw_info();
+
+	//---allocate input device---
+	ts->input_dev = input_allocate_device();
+	if (ts->input_dev == NULL) {
+		NVT_ERR("allocate input device failed\n");
+		ret = -ENOMEM;
+		goto err_input_dev_alloc_failed;
+	}
+
+	ts->max_touch_num = TOUCH_MAX_FINGER_NUM;
+
+#if TOUCH_KEY_NUM > 0
+	ts->max_button_num = TOUCH_KEY_NUM;
+#endif
+
+	ts->int_trigger_type = INT_TRIGGER_TYPE;
+
+
+	//---set input device info.---
+	ts->input_dev->evbit[0] = BIT_MASK(EV_SYN) | BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS) ;
+	ts->input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+	ts->input_dev->propbit[0] = BIT(INPUT_PROP_DIRECT);
+
+#if MT_PROTOCOL_B
+	input_mt_init_slots(ts->input_dev, ts->max_touch_num, 0);
+#endif
+
+	input_set_abs_params(ts->input_dev, ABS_MT_PRESSURE, 0, TOUCH_FORCE_NUM, 0, 0);    //pressure = TOUCH_FORCE_NUM
+
+#if TOUCH_MAX_FINGER_NUM > 1
+	input_set_abs_params(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0, 255, 0, 0);    //area = 255
+
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_X, 0, ts->abs_x_max, 0, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_Y, 0, ts->abs_y_max, 0, 0);
+#if MT_PROTOCOL_B
+	// no need to set ABS_MT_TRACKING_ID, input_mt_init_slots() already set it
+#else
+	input_set_abs_params(ts->input_dev, ABS_MT_TRACKING_ID, 0, ts->max_touch_num, 0, 0);
+#endif //MT_PROTOCOL_B
+#endif //TOUCH_MAX_FINGER_NUM > 1
+
+#if TOUCH_KEY_NUM > 0
+	for (retry = 0; retry < ts->max_button_num; retry++) {
+		input_set_capability(ts->input_dev, EV_KEY, touch_key_array[retry]);
+	}
+#endif
+
+#if WAKEUP_GESTURE
+	for (retry = 0; retry < ARRAY_SIZE(gesture_key_array); retry++) {
+		input_set_capability(ts->input_dev, EV_KEY, gesture_key_array[retry]);
+	}
+#endif
+
+	snprintf(ts->phys, sizeof(ts->phys), "input/ts");
+	ts->input_dev->name = NVT_TS_NAME;
+	ts->input_dev->phys = ts->phys;
+	ts->input_dev->id.bustype = BUS_I2C;
+
+	//---register input device---
+	ret = input_register_device(ts->input_dev);
+	if (ret) {
+		NVT_ERR("register input device (%s) failed. ret=%d\n", ts->input_dev->name, ret);
+		goto err_input_register_device_failed;
+	}
+
+	//---set int-pin & request irq---
+	client->irq = gpio_to_irq(ts->irq_gpio);
+	if (client->irq) {
+		NVT_LOG("int_trigger_type=%d\n", ts->int_trigger_type);
+		ts->irq_enabled = true;
+		ret = request_threaded_irq(client->irq, NULL, nvt_ts_work_func,
+				ts->int_trigger_type | IRQF_ONESHOT, NVT_I2C_NAME, ts);
+		if (ret != 0) {
+			NVT_ERR("request irq failed. ret=%d\n", ret);
+			goto err_int_request_failed;
+		} else {
+			nvt_irq_enable(false);
+			NVT_LOG("request irq %d succeed\n", client->irq);
+		}
+	}
+
+#if WAKEUP_GESTURE
+	device_init_wakeup(&ts->input_dev->dev, 1);
+#endif
+
+#if BOOT_UPDATE_FIRMWARE
+	nvt_fwu_wq = alloc_workqueue("nvt_fwu_wq", WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	if (!nvt_fwu_wq) {
+		NVT_ERR("nvt_fwu_wq create workqueue failed\n");
+		ret = -ENOMEM;
+		goto err_create_nvt_fwu_wq_failed;
+	}
+	INIT_DELAYED_WORK(&ts->nvt_fwu_work, Boot_Update_Firmware);
+	// please make sure boot update start after display reset(RESX) sequence
+	queue_delayed_work(nvt_fwu_wq, &ts->nvt_fwu_work, msecs_to_jiffies(14000));
+#endif
+
+	NVT_LOG("NVT_TOUCH_ESD_PROTECT is %d\n", NVT_TOUCH_ESD_PROTECT);
+#if NVT_TOUCH_ESD_PROTECT
+	INIT_DELAYED_WORK(&nvt_esd_check_work, nvt_esd_check_func);
+	nvt_esd_check_wq = alloc_workqueue("nvt_esd_check_wq", WQ_MEM_RECLAIM, 1);
+	if (!nvt_esd_check_wq) {
+		NVT_ERR("nvt_esd_check_wq create workqueue failed\n");
+		ret = -ENOMEM;
+		goto err_create_nvt_esd_check_wq_failed;
+	}
+	queue_delayed_work(nvt_esd_check_wq, &nvt_esd_check_work,
+			msecs_to_jiffies(NVT_TOUCH_ESD_CHECK_PERIOD));
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	//---set device node---
+#if NVT_TOUCH_PROC
+	ret = nvt_flash_proc_init();
+	if (ret != 0) {
+		NVT_ERR("nvt flash proc init failed. ret=%d\n", ret);
+		goto err_flash_proc_init_failed;
+	}
+#endif
+
+#if NVT_TOUCH_EXT_PROC
+	ret = nvt_extra_proc_init();
+	if (ret != 0) {
+		NVT_ERR("nvt extra proc init failed. ret=%d\n", ret);
+		goto err_extra_proc_init_failed;
+	}
+#endif
+
+#if NVT_TOUCH_MP
+	ret = nvt_mp_proc_init();
+	if (ret != 0) {
+		NVT_ERR("nvt mp proc init failed. ret=%d\n", ret);
+		goto err_mp_proc_init_failed;
+	}
+#endif
+
+	bTouchIsAwake = 1;
+	NVT_LOG("end\n");
+
+	nvt_irq_enable(true);
+
+	return 0;
+
+#if NVT_TOUCH_MP
+nvt_mp_proc_deinit();
+err_mp_proc_init_failed:
+#endif
+#if NVT_TOUCH_EXT_PROC
+nvt_extra_proc_deinit();
+err_extra_proc_init_failed:
+#endif
+#if NVT_TOUCH_PROC
+nvt_flash_proc_deinit();
+err_flash_proc_init_failed:
+#endif
+#if NVT_TOUCH_ESD_PROTECT
+	if (nvt_esd_check_wq) {
+		cancel_delayed_work_sync(&nvt_esd_check_work);
+		destroy_workqueue(nvt_esd_check_wq);
+		nvt_esd_check_wq = NULL;
+	}
+err_create_nvt_esd_check_wq_failed:
+#endif
+#if BOOT_UPDATE_FIRMWARE
+	if (nvt_fwu_wq) {
+		cancel_delayed_work_sync(&ts->nvt_fwu_work);
+		destroy_workqueue(nvt_fwu_wq);
+		nvt_fwu_wq = NULL;
+	}
+err_create_nvt_fwu_wq_failed:
+#endif
+#if WAKEUP_GESTURE
+	device_init_wakeup(&ts->input_dev->dev, 0);
+#endif
+	free_irq(client->irq, ts);
+err_int_request_failed:
+	input_unregister_device(ts->input_dev);
+	ts->input_dev = NULL;
+err_input_register_device_failed:
+	if (ts->input_dev) {
+		input_free_device(ts->input_dev);
+		ts->input_dev = NULL;
+	}
+err_input_dev_alloc_failed:
+err_chipvertrim_failed:
+	nvt_gpio_deconfig(ts);
+err_gpio_config_failed:
+	NVT_ERR("ret = %d\n", ret);
+	return ret;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen driver probe function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed. negative---failed
+ *******************************************************/
+static int32_t nvt_ts_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	int32_t ret = 0;
+#if defined(CONFIG_DRM)
+	struct device_node *dp = client->dev.of_node;
+#endif
+
+	NVT_LOG("start\n");
+
+#if defined(CONFIG_DRM)
+	if (nvt_ts_check_dt(dp)) {
+		if (!nvt_ts_check_default_tp(dp, "qcom,i2c-touch-active"))
+			ret = -EPROBE_DEFER;
+		else
+			ret = -ENODEV;
+
+		return ret;
+	}
+#endif
+
+	ts = devm_kzalloc(&client->dev, sizeof(struct nvt_ts_data), GFP_KERNEL);
+	if (ts == NULL) {
+		NVT_ERR("failed to allocated memory for nvt ts data\n");
+		return -ENOMEM;
+	}
+
+	ts->client = client;
+	i2c_set_clientdata(client, ts);
+
+	//---parse dts---
+	nvt_parse_dt(&client->dev);
+
+	//---check i2c func.---
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		NVT_ERR("i2c_check_functionality failed. (no I2C_FUNC_I2C)\n");
+		return  -ENODEV;
+	}
+
+	mutex_init(&ts->lock);
+	mutex_init(&ts->xbuf_lock);
+
+	ts->id = id;
+
+#if defined(CONFIG_DRM)
+	nvt_i2c_register_for_panel_events(client->dev.of_node, ts);
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	ts->drm_notif.notifier_call = nvt_drm_notifier_callback;
+	ret = msm_drm_register_client(&ts->drm_notif);
+	if (ret) {
+		NVT_ERR("register drm_notifier failed. ret=%d\n", ret);
+		goto err_register_drm_notif_failed;
+	}
+#else
+	ts->fb_notif.notifier_call = nvt_fb_notifier_callback;
+	ts->fb_notif.notifier_call = nvt_fb_notifier_callback;
+	if (ret) {
+		NVT_ERR("register fb_notifier failed. ret=%d\n", ret);
+		goto err_register_fb_notif_failed;
+	}
+#endif
+	NVT_LOG("end\n");
+	return 0;
+#if defined(CONFIG_DRM)
+
+#elif defined(_MSM_DRM_NOTIFY_H_)
+err_register_drm_notif_failed:
+#else
+err_register_fb_notif_failed:
+#endif
+	return -ENODEV;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen driver release function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed.
+ *******************************************************/
+static int32_t nvt_ts_remove(struct i2c_client *client)
+{
+	NVT_LOG("Removing driver...\n");
+
+#if defined(CONFIG_DRM)
+	if (active_panel && ts->notifier_cookie)
+		panel_event_notifier_unregister(ts->notifier_cookie);
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	if (msm_drm_unregister_client(&ts->drm_notif))
+		NVT_ERR("Error occurred while unregistering drm_notifier.\n");
+#else
+	if (fb_unregister_client(&ts->fb_notif))
+		NVT_ERR("Error occurred while unregistering fb_notifier.\n");
+#endif
+
+#if NVT_TOUCH_MP
+	nvt_mp_proc_deinit();
+#endif
+#if NVT_TOUCH_EXT_PROC
+	nvt_extra_proc_deinit();
+#endif
+#if NVT_TOUCH_PROC
+	nvt_flash_proc_deinit();
+#endif
+
+#if NVT_TOUCH_ESD_PROTECT
+	if (nvt_esd_check_wq) {
+		cancel_delayed_work_sync(&nvt_esd_check_work);
+		nvt_esd_check_enable(false);
+		destroy_workqueue(nvt_esd_check_wq);
+		nvt_esd_check_wq = NULL;
+	}
+#endif
+
+#if BOOT_UPDATE_FIRMWARE
+	if (nvt_fwu_wq) {
+		cancel_delayed_work_sync(&ts->nvt_fwu_work);
+		destroy_workqueue(nvt_fwu_wq);
+		nvt_fwu_wq = NULL;
+	}
+#endif
+
+#if WAKEUP_GESTURE
+	if (ts->input_dev)
+		device_init_wakeup(&ts->input_dev->dev, 0);
+#endif
+
+	nvt_irq_enable(false);
+	free_irq(client->irq, ts);
+
+	mutex_destroy(&ts->xbuf_lock);
+	mutex_destroy(&ts->lock);
+
+	nvt_gpio_deconfig(ts);
+
+	if (ts->input_dev) {
+		input_unregister_device(ts->input_dev);
+		ts->input_dev = NULL;
+	}
+
+	i2c_set_clientdata(client, NULL);
+
+	if (ts) {
+		kfree(ts);
+		ts = NULL;
+	}
+
+	return 0;
+}
+
+static void nvt_ts_shutdown(struct i2c_client *client)
+{
+	NVT_LOG("Shutdown driver...\n");
+
+	nvt_irq_enable(false);
+
+#if defined(CONFIG_DRM)
+	if (active_panel && ts->notifier_cookie)
+		panel_event_notifier_unregister(ts->notifier_cookie);
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	if (msm_drm_unregister_client(&ts->drm_notif))
+		NVT_ERR("Error occurred while unregistering drm_notifier.\n");
+#else
+	if (fb_unregister_client(&ts->fb_notif))
+		NVT_ERR("Error occurred while unregistering fb_notifier.\n");
+#endif
+
+#if NVT_TOUCH_MP
+	nvt_mp_proc_deinit();
+#endif
+#if NVT_TOUCH_EXT_PROC
+	nvt_extra_proc_deinit();
+#endif
+#if NVT_TOUCH_PROC
+	nvt_flash_proc_deinit();
+#endif
+
+#if NVT_TOUCH_ESD_PROTECT
+	if (nvt_esd_check_wq) {
+		cancel_delayed_work_sync(&nvt_esd_check_work);
+		nvt_esd_check_enable(false);
+		destroy_workqueue(nvt_esd_check_wq);
+		nvt_esd_check_wq = NULL;
+	}
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+#if BOOT_UPDATE_FIRMWARE
+	if (nvt_fwu_wq) {
+		cancel_delayed_work_sync(&ts->nvt_fwu_work);
+		destroy_workqueue(nvt_fwu_wq);
+		nvt_fwu_wq = NULL;
+	}
+#endif
+
+#if WAKEUP_GESTURE
+	if (ts->input_dev)
+		device_init_wakeup(&ts->input_dev->dev, 0);
+#endif
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen driver suspend function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed.
+ *******************************************************/
+static int32_t nvt_ts_suspend(struct device *dev)
+{
+	uint8_t buf[4] = {0};
+#if MT_PROTOCOL_B
+	uint32_t i = 0;
+#endif
+
+	if (!bTouchIsAwake) {
+		NVT_LOG("Touch is already suspend\n");
+		return 0;
+	}
+
+#if !WAKEUP_GESTURE
+	nvt_irq_enable(false);
+#endif
+
+#if NVT_TOUCH_ESD_PROTECT
+	NVT_LOG("cancel delayed work sync\n");
+	cancel_delayed_work_sync(&nvt_esd_check_work);
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	mutex_lock(&ts->lock);
+
+	NVT_LOG("start\n");
+
+	bTouchIsAwake = 0;
+
+#if WAKEUP_GESTURE
+	//---write command to enter "wakeup gesture mode"---
+	buf[0] = EVENT_MAP_HOST_CMD;
+	buf[1] = 0x13;
+	CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 2);
+
+	enable_irq_wake(ts->client->irq);
+
+	NVT_LOG("Enabled touch wakeup gesture\n");
+
+#else // WAKEUP_GESTURE
+	//---write command to enter "deep sleep mode"---
+	buf[0] = EVENT_MAP_HOST_CMD;
+	buf[1] = 0x11;
+	CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 2);
+#endif // WAKEUP_GESTURE
+
+	mutex_unlock(&ts->lock);
+
+	/* release all touches */
+#if MT_PROTOCOL_B
+	for (i = 0; i < ts->max_touch_num; i++) {
+		input_mt_slot(ts->input_dev, i);
+		input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0);
+		input_report_abs(ts->input_dev, ABS_MT_PRESSURE, 0);
+		input_mt_report_slot_state(ts->input_dev, MT_TOOL_FINGER, 0);
+	}
+#endif
+	input_report_key(ts->input_dev, BTN_TOUCH, 0);
+#if !MT_PROTOCOL_B
+	input_mt_sync(ts->input_dev);
+#endif
+	input_sync(ts->input_dev);
+
+	msleep(50);
+
+	NVT_LOG("end\n");
+
+	return 0;
+}
+
+/*******************************************************
+ * Description:
+ *     Novatek touchscreen driver resume function.
+ *
+ * return:
+ *     Executive outcomes. 0---succeed.
+ *******************************************************/
+static int32_t nvt_ts_resume(struct device *dev)
+{
+	NVT_LOG("start\n");
+
+	if (bTouchIsAwake || ts->fw_ver == 0) {
+		nvt_ts_late_probe(ts->client, ts->id);
+		NVT_LOG("nvt_ts_late_probe\n");
+		return 0;
+	}
+
+	mutex_lock(&ts->lock);
+
+	// make sure display reset(RESX) sequence and dsi cmds sent before this
+#if NVT_TOUCH_SUPPORT_HW_RST
+	gpio_set_value(ts->reset_gpio, 1);
+#endif
+
+	// NT36772 IC due to no boot-load when RESX/TP_RESX
+	// nvt_bootloader_reset();
+	if (nvt_check_fw_reset_state(RESET_STATE_REK)) {
+		NVT_ERR("FW is not ready! Try to bootloader reset...\n");
+		nvt_bootloader_reset();
+		nvt_check_fw_reset_state(RESET_STATE_REK);
+	}
+
+#if !WAKEUP_GESTURE
+	nvt_irq_enable(true);
+#endif
+
+#if NVT_TOUCH_ESD_PROTECT
+	nvt_esd_check_enable(false);
+	queue_delayed_work(nvt_esd_check_wq, &nvt_esd_check_work,
+			msecs_to_jiffies(NVT_TOUCH_ESD_CHECK_PERIOD));
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	bTouchIsAwake = 1;
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("end\n");
+
+	return 0;
+}
+
+#if defined(CONFIG_DRM)
+
+static void nvt_i2c_panel_notifier_callback(enum panel_event_notifier_tag tag,
+		struct panel_event_notification *notification, void *client_data)
+{
+	struct nvt_ts_data *ts = client_data;
+
+	if (!notification) {
+		pr_err("Invalid notification\n");
+		return;
+	}
+
+	NVT_LOG("Notification type:%d, early_trigger:%d",
+		notification->notif_type,
+		notification->notif_data.early_trigger);
+
+	switch (notification->notif_type) {
+	case DRM_PANEL_EVENT_UNBLANK:
+		if (notification->notif_data.early_trigger)
+			NVT_LOG("resume notification pre commit\n");
+		else
+			nvt_ts_resume(&ts->client->dev);
+		break;
+
+	case DRM_PANEL_EVENT_BLANK:
+		if (notification->notif_data.early_trigger)
+			nvt_ts_suspend(&ts->client->dev);
+		else
+			NVT_LOG("suspend notification post commit\n");
+		break;
+
+	case DRM_PANEL_EVENT_BLANK_LP:
+		NVT_LOG("received lp event\n");
+		break;
+
+	case DRM_PANEL_EVENT_FPS_CHANGE:
+		NVT_LOG("Received fps change old fps:%d new fps:%d\n",
+			notification->notif_data.old_fps,
+			notification->notif_data.new_fps);
+		break;
+	default:
+		NVT_LOG("notification serviced :%d\n",
+			notification->notif_type);
+		break;
+	}
+}
+
+#elif defined(_MSM_DRM_NOTIFY_H_)
+static int nvt_drm_notifier_callback(struct notifier_block *self, unsigned long event, void *data)
+{
+	struct msm_drm_notifier *evdata = data;
+	int *blank;
+	struct nvt_ts_data *ts =
+		container_of(self, struct nvt_ts_data, drm_notif);
+
+	if (!evdata || (evdata->id != 0))
+		return 0;
+
+	if (evdata->data && ts) {
+		blank = evdata->data;
+		if (event == MSM_DRM_EARLY_EVENT_BLANK) {
+			if (*blank == MSM_DRM_BLANK_POWERDOWN) {
+				NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+				nvt_ts_suspend(&ts->client->dev);
+			}
+		} else if (event == MSM_DRM_EVENT_BLANK) {
+			if (*blank == MSM_DRM_BLANK_UNBLANK) {
+				NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+				nvt_ts_resume(&ts->client->dev);
+			}
+		}
+	}
+
+	return 0;
+}
+
+#else
+static int nvt_fb_notifier_callback(struct notifier_block *self, unsigned long event, void *data)
+{
+	struct fb_event *evdata = data;
+	int *blank;
+	struct nvt_ts_data *ts =
+		container_of(self, struct nvt_ts_data, fb_notif);
+
+	if (evdata && evdata->data && event == FB_EARLY_EVENT_BLANK) {
+		blank = evdata->data;
+		if (*blank == FB_BLANK_POWERDOWN) {
+			NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+			nvt_ts_suspend(&ts->client->dev);
+		}
+	} else if (evdata && evdata->data && event == FB_EVENT_BLANK) {
+		blank = evdata->data;
+		if (*blank == FB_BLANK_UNBLANK) {
+			NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+			nvt_ts_resume(&ts->client->dev);
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static const struct i2c_device_id nvt_ts_id[] = {
+	{ NVT_I2C_NAME, 0 },
+	{ }
+};
+
+#ifdef CONFIG_OF
+static struct of_device_id nvt_match_table[] = {
+	{ .compatible = "novatek,NVT-ts",},
+	{ },
+};
+#endif
+
+static struct i2c_driver nvt_i2c_driver = {
+	.probe		= nvt_ts_probe,
+	.remove		= nvt_ts_remove,
+	.shutdown	= nvt_ts_shutdown,
+	.id_table	= nvt_ts_id,
+	.driver = {
+		.name	= NVT_I2C_NAME,
+#ifdef CONFIG_OF
+		.of_match_table = nvt_match_table,
+#endif
+	},
+};
+
+/*******************************************************
+ * Description:
+ *     Driver Install function.
+ *
+ * return:
+ *     Executive Outcomes. 0---succeed. not 0---failed.
+ ********************************************************/
+static int32_t __init nvt_driver_init(void)
+{
+	int32_t ret = 0;
+
+	NVT_LOG("start\n");
+	//---add i2c driver---
+	ret = i2c_add_driver(&nvt_i2c_driver);
+	if (ret) {
+		NVT_ERR("failed to add i2c driver");
+		goto err_driver;
+	}
+
+	NVT_LOG("finished\n");
+
+err_driver:
+	return ret;
+}
+
+/*******************************************************
+ * Description:
+ *     Driver uninstall function.
+ *
+ * return:
+ *     n.a.
+ ********************************************************/
+static void __exit nvt_driver_exit(void)
+{
+	i2c_del_driver(&nvt_i2c_driver);
+}
+
+//late_initcall(nvt_driver_init);
+module_init(nvt_driver_init);
+module_exit(nvt_driver_exit);
+
+MODULE_DESCRIPTION("Novatek Touchscreen Driver");
+MODULE_LICENSE("GPL v2");
+
+#else  /* NT36XXX_SPI */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/proc_fs.h>
+#include <linux/input/mt.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+
+#if defined(CONFIG_DRM)
+#include <linux/soc/qcom/panel_event_notifier.h>
+#endif
+
+#include "nt36xxx.h"
+#if NVT_SPI_TOUCH_ESD_PROTECT
+#include <linux/jiffies.h>
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+static struct delayed_work nvt_spi_esd_check_work;
+static struct workqueue_struct *nvt_spi_esd_check_wq;
+static unsigned long nvt_spi_irq_timer;
+uint8_t nvt_spi_esd_check;
+uint8_t nvt_spi_esd_retry;
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+struct nvt_spi_data_t *nvt_spi_data;
+
+#if NVT_SPI_BOOT_UPDATE_FIRMWARE
+static struct workqueue_struct *nvt_spi_fwu_wq;
+#endif
+
+#if defined(CONFIG_DRM)
+static struct drm_panel *active_spi_panel;
+static void nvt_spi_panel_notifier_callback(enum panel_event_notifier_tag tag,
+			struct panel_event_notification *event, void *client_data);
+
+#elif defined(_MSM_DRM_NOTIFY_H_)
+static int nvt_drm_notifier_callback(
+		struct notifier_block *self, unsigned long event, void *data);
+#else
+static int nvt_fb_notifier_callback(
+		struct notifier_block *self, unsigned long event, void *data);
+#endif
+
+static uint32_t NVT_SPI_ENG_RST_ADDR = 0x7FFF80;
+uint32_t NVT_SPI_SWRST_N8_ADDR; //read from dtsi
+uint32_t NVT_SPI_RD_FAST_ADDR;  //read from dtsi
+
+#if NVT_SPI_TOUCH_KEY_NUM > 0
+const uint16_t nvt_spi_touch_key_array[NVT_SPI_TOUCH_KEY_NUM] = {
+	KEY_BACK,
+	KEY_HOME,
+	KEY_MENU
+};
+#endif
+
+#if NVT_SPI_WAKEUP_GESTURE
+const uint16_t nvt_spi_gesture_key_array[] = {
+	KEY_POWER,  //GESTURE_WORD_C
+	KEY_POWER,  //GESTURE_WORD_W
+	KEY_POWER,  //GESTURE_WORD_V
+	KEY_POWER,  //GESTURE_DOUBLE_CLICK
+	KEY_POWER,  //GESTURE_WORD_Z
+	KEY_POWER,  //GESTURE_WORD_M
+	KEY_POWER,  //GESTURE_WORD_O
+	KEY_POWER,  //GESTURE_WORD_e
+	KEY_POWER,  //GESTURE_WORD_S
+	KEY_POWER,  //GESTURE_SLIDE_UP
+	KEY_POWER,  //GESTURE_SLIDE_DOWN
+	KEY_POWER,  //GESTURE_SLIDE_LEFT
+	KEY_POWER,  //GESTURE_SLIDE_RIGHT
+};
+#endif
+
+static uint8_t bTouchIsAwake;
+
+#if defined(CONFIG_DRM)
+static void nvt_spi_register_for_panel_events(struct device_node *dp,
+					struct nvt_spi_data_t *ts)
+{
+	void *cookie = NULL;
+
+	cookie = panel_event_notifier_register(PANEL_EVENT_NOTIFICATION_PRIMARY,
+			PANEL_EVENT_NOTIFIER_CLIENT_PRIMARY_TOUCH, active_spi_panel,
+			&nvt_spi_panel_notifier_callback, ts);
+	if (!cookie) {
+		pr_err("Failed to register for panel events\n");
+		return;
+	}
+
+	ts->notifier_cookie = cookie;
+}
+#endif
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen irq enable/disable function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void nvt_spi_irq_enable(bool enable)
+{
+	struct irq_desc *desc;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (enable) {
+		if (!ts->irq_enabled) {
+			enable_irq(ts->client->irq);
+			ts->irq_enabled = true;
+		}
+	} else {
+		if (ts->irq_enabled) {
+			disable_irq(ts->client->irq);
+			ts->irq_enabled = false;
+		}
+	}
+
+	desc = irq_to_desc(ts->client->irq);
+	NVT_LOG("enable=%d, desc->depth=%d\n", enable, desc->depth);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen spi read/write core function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static inline int32_t nvt_spi_read_write(
+		struct spi_device *client, uint8_t *buf, size_t len, enum NVT_SPI_RW rw)
+{
+	struct spi_message m;
+	struct spi_transfer t = {
+		.len    = len,
+	};
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	memset(ts->xbuf, 0, len + NVT_SPI_DUMMY_BYTES);
+	memcpy(ts->xbuf, buf, len);
+
+	switch (rw) {
+	case NVT_SPI_READ:
+		t.tx_buf = ts->xbuf;
+		t.rx_buf = ts->rbuf;
+		t.len = (len + NVT_SPI_DUMMY_BYTES);
+		break;
+
+	case NVT_SPI_WRITE:
+		t.tx_buf = ts->xbuf;
+		break;
+	}
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+	return spi_sync(client, &m);
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen spi read function.
+ *
+ * return:
+ *	Executive outcomes. 2---succeed. -5---I/O error
+ ******************************************************
+ */
+int32_t nvt_spi_read(uint8_t *buf, uint16_t len)
+{
+	int32_t ret = -1;
+	int32_t retries = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	mutex_lock(&ts->xbuf_lock);
+
+	buf[0] = NVT_SPI_READ_MASK(buf[0]);
+
+	while (retries < 5) {
+		ret = nvt_spi_read_write(ts->client, buf, len, NVT_SPI_READ);
+		if (ret == 0)
+			break;
+		retries++;
+	}
+
+	if (unlikely(retries == 5)) {
+		NVT_ERR("read error, ret=%d\n", ret);
+		ret = -EIO;
+	} else
+		memcpy((buf+1), (ts->rbuf+2), (len-1));
+
+	mutex_unlock(&ts->xbuf_lock);
+
+	return ret;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen spi write function.
+ *
+ * return:
+ *	Executive outcomes. 1---succeed. -5---I/O error
+ ******************************************************
+ */
+int32_t nvt_spi_write(uint8_t *buf, uint16_t len)
+{
+	int32_t ret = -1;
+	int32_t retries = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	mutex_lock(&ts->xbuf_lock);
+
+	buf[0] = NVT_SPI_WRITE_MASK(buf[0]);
+
+	while (retries < 5) {
+		ret = nvt_spi_read_write(ts->client, buf, len, NVT_SPI_WRITE);
+		if (ret == 0)
+			break;
+		retries++;
+	}
+
+	if (unlikely(retries == 5)) {
+		NVT_ERR("error, ret=%d\n", ret);
+		ret = -EIO;
+	}
+
+	mutex_unlock(&ts->xbuf_lock);
+
+	return ret;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen set index/page/addr address.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -5---access fail.
+ ******************************************************
+ */
+int32_t nvt_spi_set_page(uint32_t addr)
+{
+	uint8_t buf[4] = {0};
+
+	buf[0] = 0xFF;	//set index/page/addr command
+	buf[1] = (addr >> 15) & 0xFF;
+	buf[2] = (addr >> 7) & 0xFF;
+
+	return nvt_spi_write(buf, 3);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen write data to specify address.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -5---access fail.
+ *******************************************************
+ */
+int32_t nvt_spi_write_addr(uint32_t addr, uint8_t data)
+{
+	int32_t ret = 0;
+	uint8_t buf[4] = {0};
+
+	//---set xdata index---
+	buf[0] = 0xFF;	//set index/page/addr command
+	buf[1] = (addr >> 15) & 0xFF;
+	buf[2] = (addr >> 7) & 0xFF;
+	ret = nvt_spi_write(buf, 3);
+	if (ret) {
+		NVT_ERR("set page 0x%06X failed, ret = %d\n", addr, ret);
+		return ret;
+	}
+
+	//---write data to index---
+	buf[0] = addr & (0x7F);
+	buf[1] = data;
+	ret = nvt_spi_write(buf, 2);
+	if (ret) {
+		NVT_ERR("write data to 0x%06X failed, ret = %d\n", addr, ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen enable hw bld crc function.
+ *
+ * return:
+ *	N/A.
+ ******************************************************
+ */
+void nvt_spi_bld_crc_enable(void)
+{
+	uint8_t buf[4] = {0};
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata index to BLD_CRC_EN_ADDR---
+	nvt_spi_set_page(ts->mmap->BLD_CRC_EN_ADDR);
+
+	//---read data from index---
+	buf[0] = ts->mmap->BLD_CRC_EN_ADDR & (0x7F);
+	buf[1] = 0xFF;
+	nvt_spi_read(buf, 2);
+
+	//---write data to index---
+	buf[0] = ts->mmap->BLD_CRC_EN_ADDR & (0x7F);
+	buf[1] = buf[1] | (0x01 << 7);
+	nvt_spi_write(buf, 2);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen clear status & enable fw crc function.
+ *
+ * return:
+ *	N/A.
+ ******************************************************
+ */
+void nvt_spi_fw_crc_enable(void)
+{
+	uint8_t buf[4] = {0};
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR);
+
+	//---clear fw reset status---
+	buf[0] = NVT_SPI_EVENT_MAP_RESET_COMPLETE & (0x7F);
+	buf[1] = 0x00;
+	nvt_spi_write(buf, 2);
+
+	//---enable fw crc---
+	buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD & (0x7F);
+	buf[1] = 0xAE;	//enable fw crc command
+	nvt_spi_write(buf, 2);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen set boot ready function.
+ *
+ * return:
+ *	N/A.
+ ******************************************************
+ */
+void nvt_spi_boot_ready(void)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---write BOOT_RDY status cmds---
+	nvt_spi_write_addr(ts->mmap->BOOT_RDY_ADDR, 1);
+
+	mdelay(5);
+
+	if (!ts->hw_crc) {
+		//---write BOOT_RDY status cmds---
+		nvt_spi_write_addr(ts->mmap->BOOT_RDY_ADDR, 0);
+
+		//---write POR_CD cmds---
+		nvt_spi_write_addr(ts->mmap->POR_CD_ADDR, 0xA0);
+	}
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen enable auto copy mode function.
+ *
+ * return:
+ *	N/A.
+ ******************************************************
+ */
+void nvt_spi_tx_auto_copy_mode(void)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---write TX_AUTO_COPY_EN cmds---
+	nvt_spi_write_addr(ts->mmap->TX_AUTO_COPY_EN, 0x69);
+
+	NVT_ERR("tx auto copy mode enable\n");
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen check spi dma tx info function.
+ *
+ * return:
+ *	N/A.
+ ******************************************************
+ */
+int32_t nvt_spi_check_spi_dma_tx_info(void)
+{
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 200;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	for (i = 0; i < retry; i++) {
+		//---set xdata index to EVENT BUF ADDR---
+		nvt_spi_set_page(ts->mmap->SPI_DMA_TX_INFO);
+
+		//---read fw status---
+		buf[0] = ts->mmap->SPI_DMA_TX_INFO & 0x7F;
+		buf[1] = 0xFF;
+		nvt_spi_read(buf, 2);
+
+		if (buf[1] == 0x00)
+			break;
+
+		usleep_range(1000, 1100);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("failed, i=%d, buf[1]=0x%02X\n", i, buf[1]);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen eng reset cmd function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_eng_reset(void)
+{
+	//---eng reset cmds to ENG_RST_ADDR---
+	nvt_spi_write_addr(NVT_SPI_ENG_RST_ADDR, 0x5A);
+
+	mdelay(1);	//wait tMCU_Idle2TP_REX_Hi after TP_RST
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen reset MCU function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_sw_reset(void)
+{
+	//---software reset cmds to SWRST_N8_ADDR---
+	nvt_spi_write_addr(NVT_SPI_SWRST_N8_ADDR, 0x55);
+
+	msleep(20);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen reset MCU then into idle mode
+ *	function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_sw_reset_idle(void)
+{
+	//---MCU idle cmds to SWRST_N8_ADDR---
+	nvt_spi_write_addr(NVT_SPI_SWRST_N8_ADDR, 0xAA);
+
+	msleep(20);
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen reset MCU (boot) function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_bootloader_reset(void)
+{
+	//---reset cmds to SWRST_N8_ADDR---
+	nvt_spi_write_addr(NVT_SPI_SWRST_N8_ADDR, 0x69);
+
+	mdelay(5);	//wait tBRST2FR after Bootload RST
+
+	if (NVT_SPI_RD_FAST_ADDR) {
+		/* disable SPI_RD_FAST */
+		nvt_spi_write_addr(NVT_SPI_RD_FAST_ADDR, 0x00);
+	}
+
+	NVT_LOG("end\n");
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen clear FW status function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -1---fail.
+ ******************************************************
+ */
+int32_t nvt_spi_clear_fw_status(void)
+{
+	uint32_t addr;
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 20;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	for (i = 0; i < retry; i++) {
+		//---set xdata index to EVENT BUF ADDR---
+		addr = ts->mmap->EVENT_BUF_ADDR;
+		nvt_spi_set_page(addr | NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE);
+
+		//---clear fw status---
+		buf[0] = NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = 0x00;
+		nvt_spi_write(buf, 2);
+
+		//---read fw status---
+		buf[0] = NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = 0xFF;
+		nvt_spi_read(buf, 2);
+
+		if (buf[1] == 0x00)
+			break;
+
+		usleep_range(10000, 11000);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("failed, i=%d, buf[1]=0x%02X\n", i, buf[1]);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen check FW status function.
+
+ * return:
+ *	Executive outcomes. 0---succeed. -1---failed.
+ ******************************************************
+ */
+int32_t nvt_spi_check_fw_status(void)
+{
+	uint32_t addr;
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 50;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	usleep_range(20000, 21000);
+
+	for (i = 0; i < retry; i++) {
+		//---set xdata index to EVENT BUF ADDR---
+		addr = ts->mmap->EVENT_BUF_ADDR;
+		nvt_spi_set_page(addr | NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE);
+
+		//---read fw status---
+		buf[0] = NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = 0x00;
+		nvt_spi_read(buf, 2);
+
+		if ((buf[1] & 0xF0) == 0xA0)
+			break;
+
+		usleep_range(10000, 11000);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("failed, i=%d, buf[1]=0x%02X\n", i, buf[1]);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen check FW reset state function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -1---failed.
+ ******************************************************
+ */
+int32_t nvt_spi_check_fw_reset_state(enum NVT_SPI_RST_COMPLETE_STATE check_reset_state)
+{
+	uint8_t buf[8] = {0};
+	int32_t ret = 0;
+	int32_t retry = 0;
+	int32_t retry_max = (check_reset_state == NVT_SPI_RESET_STATE_INIT) ? 10 : 50;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR | NVT_SPI_EVENT_MAP_RESET_COMPLETE);
+
+	while (1) {
+		//---read reset state---
+		buf[0] = NVT_SPI_EVENT_MAP_RESET_COMPLETE;
+		buf[1] = 0x00;
+		nvt_spi_read(buf, 6);
+
+		if ((buf[1] >= check_reset_state) && (buf[1] <= NVT_SPI_RESET_STATE_MAX)) {
+			ret = 0;
+			break;
+		}
+
+		retry++;
+		if (unlikely(retry > retry_max)) {
+			NVT_ERR("error, retry=%d, buf[1]=0x%02X,0x%02X,0x%02X,0x%02X,0x%02X\n",
+				retry, buf[1], buf[2], buf[3], buf[4], buf[5]);
+			ret = -1;
+			break;
+		}
+
+		usleep_range(10000, 11000);
+	}
+
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen get firmware related information
+ *	function.
+ *
+ * return:
+ *	Executive outcomes. 0---success. -1---fail.
+ ******************************************************
+ */
+int32_t nvt_spi_get_fw_info(void)
+{
+	uint8_t buf[64] = {0};
+	uint32_t retry_count = 0;
+	int32_t ret = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+info_retry:
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR | NVT_SPI_EVENT_MAP_FWINFO);
+
+	//---read fw info---
+	buf[0] = NVT_SPI_EVENT_MAP_FWINFO;
+	nvt_spi_read(buf, 39);
+	if ((buf[1] + buf[2]) != 0xFF) {
+		NVT_ERR("FW info is broken! fw_ver=0x%02X, ~fw_ver=0x%02X\n", buf[1], buf[2]);
+		if (retry_count < 3) {
+			retry_count++;
+			NVT_ERR("retry_count=%d\n", retry_count);
+			goto info_retry;
+		} else {
+			ts->fw_ver = 0;
+			ts->abs_x_max = NVT_SPI_TOUCH_DEFAULT_MAX_WIDTH;
+			ts->abs_y_max = NVT_SPI_TOUCH_DEFAULT_MAX_HEIGHT;
+			ts->max_button_num = NVT_SPI_TOUCH_KEY_NUM;
+			NVT_ERR("Set default fw_ver=%d, abs_x_max=%d, ",
+					ts->fw_ver, ts->abs_x_max);
+			NVT_ERR("abs_y_max=%d, max_button_num=%d!\n",
+					ts->abs_y_max, ts->max_button_num);
+			ret = -1;
+			goto out;
+		}
+	}
+	ts->fw_ver = buf[1];
+	ts->x_num = buf[3];
+	ts->y_num = buf[4];
+	ts->abs_x_max = (uint16_t)((buf[5] << 8) | buf[6]);
+	ts->abs_y_max = (uint16_t)((buf[7] << 8) | buf[8]);
+	ts->max_button_num = buf[11];
+	ts->nvt_pid = (uint16_t)((buf[36] << 8) | buf[35]);
+	if (ts->pen_support) {
+		ts->x_gang_num = buf[37];
+		ts->y_gang_num = buf[38];
+	}
+
+	NVT_ERR("fw_ver=0x%02X, fw_type=0x%02X, PID=0x%04X\n", ts->fw_ver, buf[14], ts->nvt_pid);
+
+	ret = 0;
+out:
+
+	return ret;
+}
+
+/*
+ ******************************************************
+ * Create Device Node (Proc Entry)
+ ******************************************************
+ */
+#if NVT_SPI_TOUCH_PROC
+static struct proc_dir_entry *nvt_spi_proc_entry;
+#define DEVICE_NAME "NVTSPI"
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/NVTSPI read function.
+ *
+ * return:
+ *	Executive outcomes. 2---succeed. -5,-14---failed.
+ ******************************************************
+ */
+static ssize_t nvt_spi_flash_read(
+	struct file *file, char __user *buff, size_t count, loff_t *offp)
+{
+	uint8_t *str = NULL;
+	int32_t ret = 0;
+	int32_t retries = 0;
+	int8_t spi_wr = 0;
+	uint8_t *buf;
+
+	if ((count > NVT_SPI_TRANSFER_LEN + 3) || (count < 3)) {
+		NVT_ERR("invalid transfer len!\n");
+		return -EFAULT;
+	}
+
+	/* allocate buffer for spi transfer */
+	str = kzalloc((count), GFP_KERNEL);
+	if (str == NULL) {
+		NVT_ERR("kzalloc for buf failed!\n");
+		ret = -ENOMEM;
+		goto kzalloc_failed;
+	}
+
+	buf = kzalloc((count), GFP_KERNEL | GFP_DMA);
+	if (buf == NULL) {
+		NVT_ERR("kzalloc for buf failed!\n");
+		ret = -ENOMEM;
+		kfree(str);
+		str = NULL;
+		goto kzalloc_failed;
+	}
+
+	if (copy_from_user(str, buff, count)) {
+		NVT_ERR("copy from user error\n");
+		ret = -EFAULT;
+		goto out;
+	}
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	/*
+	 * stop esd check work to avoid case that 0x77 report righ after here to enable esd
+	 * check again finally lead to trigger esd recovery bootloader reset
+	 */
+	cancel_delayed_work_sync(&nvt_spi_esd_check_work);
+	nvt_spi_esd_check_enable(false);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	spi_wr = str[0] >> 7;
+	memcpy(buf, str+2, ((str[0] & 0x7F) << 8) | str[1]);
+
+	if (spi_wr == NVT_SPI_WRITE) {	//SPI write
+		while (retries < 20) {
+			ret = nvt_spi_write(buf, ((str[0] & 0x7F) << 8) | str[1]);
+			if (!ret)
+				break;
+			NVT_ERR("error, retries=%d, ret=%d\n", retries, ret);
+
+			retries++;
+		}
+
+		if (unlikely(retries == 20)) {
+			NVT_ERR("error, ret = %d\n", ret);
+			ret = -EIO;
+			goto out;
+		}
+	} else if (spi_wr == NVT_SPI_READ) {	//SPI read
+		while (retries < 20) {
+			ret = nvt_spi_read(buf, ((str[0] & 0x7F) << 8) | str[1]);
+			if (!ret)
+				break;
+			NVT_ERR("error, retries=%d, ret=%d\n", retries, ret);
+
+			retries++;
+		}
+
+		memcpy(str+2, buf, ((str[0] & 0x7F) << 8) | str[1]);
+		// copy buff to user if spi transfer
+		if (retries < 20) {
+			if (copy_to_user(buff, str, count)) {
+				ret = -EFAULT;
+				goto out;
+			}
+		}
+
+		if (unlikely(retries == 20)) {
+			NVT_ERR("error, ret = %d\n", ret);
+			ret = -EIO;
+			goto out;
+		}
+	} else {
+		NVT_ERR("Call error, str[0]=%d\n", str[0]);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	kfree(str);
+	kfree(buf);
+kzalloc_failed:
+	return ret;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/NVTSPI open function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -12---failed.
+ ******************************************************
+ */
+static int32_t nvt_spi_flash_open(struct inode *inode, struct file *file)
+{
+	struct nvt_spi_flash_data *dev;
+
+	dev = kmalloc(sizeof(struct nvt_spi_flash_data), GFP_KERNEL);
+	if (dev == NULL) {
+		NVT_ERR("Failed to allocate memory for nvt flash data\n");
+		return -ENOMEM;
+	}
+
+	rwlock_init(&dev->lock);
+	file->private_data = dev;
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/NVTSPI close function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t nvt_spi_flash_close(struct inode *inode, struct file *file)
+{
+	struct nvt_spi_flash_data *dev = file->private_data;
+
+	kfree(dev);
+
+	return 0;
+}
+
+static const struct proc_ops nvt_flash_fops_spi = {
+	.proc_open = nvt_spi_flash_open,
+	.proc_release = nvt_spi_flash_close,
+	.proc_read = nvt_spi_flash_read,
+};
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/NVTSPI initial function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -12---failed.
+ ******************************************************
+ */
+static int32_t nvt_spi_flash_proc_init(void)
+{
+	nvt_spi_proc_entry = proc_create(DEVICE_NAME, 0444, NULL, &nvt_flash_fops_spi);
+	if (nvt_spi_proc_entry == NULL) {
+		NVT_ERR("Failed!\n");
+		return -ENOMEM;
+	}
+
+	NVT_LOG("Succeeded!\n");
+
+	NVT_LOG("============================================================\n");
+	NVT_LOG("Create /proc/%s\n", DEVICE_NAME);
+	NVT_LOG("============================================================\n");
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/NVTSPI deinitial function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void nvt_spi_flash_proc_deinit(void)
+{
+	if (nvt_spi_proc_entry != NULL) {
+		remove_proc_entry(DEVICE_NAME, NULL);
+		nvt_spi_proc_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", DEVICE_NAME);
+	}
+}
+#endif
+
+#if NVT_SPI_WAKEUP_GESTURE
+#define GESTURE_WORD_C          12
+#define GESTURE_WORD_W          13
+#define GESTURE_WORD_V          14
+#define GESTURE_DOUBLE_CLICK    15
+#define GESTURE_WORD_Z          16
+#define GESTURE_WORD_M          17
+#define GESTURE_WORD_O          18
+#define GESTURE_WORD_e          19
+#define GESTURE_WORD_S          20
+#define GESTURE_SLIDE_UP        21
+#define GESTURE_SLIDE_DOWN      22
+#define GESTURE_SLIDE_LEFT      23
+#define GESTURE_SLIDE_RIGHT     24
+/* customized gesture id */
+#define DATA_PROTOCOL           30
+
+/* function page definition */
+#define FUNCPAGE_GESTURE         1
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen wake up gesture key report function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_wakeup_gesture_report(uint8_t gesture_id, uint8_t *data)
+{
+	uint32_t keycode = 0;
+	uint8_t func_type = data[2];
+	uint8_t func_id = data[3];
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	/* support fw specifal data protocol */
+	if ((gesture_id == DATA_PROTOCOL) && (func_type == FUNCPAGE_GESTURE))
+		gesture_id = func_id;
+	else if (gesture_id > DATA_PROTOCOL) {
+		NVT_ERR("gesture_id %d is invalid, func_type=%d, func_id=%d\n",
+				gesture_id, func_type, func_id);
+		return;
+	}
+
+	NVT_LOG("gesture_id = %d\n", gesture_id);
+
+	switch (gesture_id) {
+	case GESTURE_WORD_C:
+		NVT_LOG("Gesture : Word-C.\n");
+		keycode = nvt_spi_gesture_key_array[0];
+		break;
+	case GESTURE_WORD_W:
+		NVT_LOG("Gesture : Word-W.\n");
+		keycode = nvt_spi_gesture_key_array[1];
+		break;
+	case GESTURE_WORD_V:
+		NVT_LOG("Gesture : Word-V.\n");
+		keycode = nvt_spi_gesture_key_array[2];
+		break;
+	case GESTURE_DOUBLE_CLICK:
+		NVT_LOG("Gesture : Double Click.\n");
+		keycode = nvt_spi_gesture_key_array[3];
+		break;
+	case GESTURE_WORD_Z:
+		NVT_LOG("Gesture : Word-Z.\n");
+		keycode = nvt_spi_gesture_key_array[4];
+		break;
+	case GESTURE_WORD_M:
+		NVT_LOG("Gesture : Word-M.\n");
+		keycode = nvt_spi_gesture_key_array[5];
+		break;
+	case GESTURE_WORD_O:
+		NVT_LOG("Gesture : Word-O.\n");
+		keycode = nvt_spi_gesture_key_array[6];
+		break;
+	case GESTURE_WORD_e:
+		NVT_LOG("Gesture : Word-e.\n");
+		keycode = nvt_spi_gesture_key_array[7];
+		break;
+	case GESTURE_WORD_S:
+		NVT_LOG("Gesture : Word-S.\n");
+		keycode = nvt_spi_gesture_key_array[8];
+		break;
+	case GESTURE_SLIDE_UP:
+		NVT_LOG("Gesture : Slide UP.\n");
+		keycode = nvt_spi_gesture_key_array[9];
+		break;
+	case GESTURE_SLIDE_DOWN:
+		NVT_LOG("Gesture : Slide DOWN.\n");
+		keycode = nvt_spi_gesture_key_array[10];
+		break;
+	case GESTURE_SLIDE_LEFT:
+		NVT_LOG("Gesture : Slide LEFT.\n");
+		keycode = nvt_spi_gesture_key_array[11];
+		break;
+	case GESTURE_SLIDE_RIGHT:
+		NVT_LOG("Gesture : Slide RIGHT.\n");
+		keycode = nvt_spi_gesture_key_array[12];
+		break;
+	default:
+		break;
+	}
+
+	if (keycode > 0) {
+		input_report_key(ts->input_dev, keycode, 1);
+		input_sync(ts->input_dev);
+		input_report_key(ts->input_dev, keycode, 0);
+		input_sync(ts->input_dev);
+	}
+}
+#endif
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen parse device tree function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+#ifdef CONFIG_OF
+static int32_t nvt_spi_parse_dt(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	int32_t ret = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+	ts->reset_gpio = of_get_named_gpio_flags(np, "novatek,reset-gpio", 0, &ts->reset_flags);
+	NVT_LOG("novatek,reset-gpio=%d\n", ts->reset_gpio);
+#endif
+	ts->irq_gpio = of_get_named_gpio_flags(np, "novatek,irq-gpio", 0, &ts->irq_flags);
+	NVT_LOG("novatek,irq-gpio=%d\n", ts->irq_gpio);
+
+	ts->pen_support = of_property_read_bool(np, "novatek,pen-support");
+	NVT_LOG("novatek,pen-support=%d\n", ts->pen_support);
+
+	ts->wgp_stylus = of_property_read_bool(np, "novatek,wgp-stylus");
+	NVT_LOG("novatek,wgp-stylus=%d\n", ts->wgp_stylus);
+
+	ret = of_property_read_u32(np, "novatek,swrst-n8-addr", &NVT_SPI_SWRST_N8_ADDR);
+	if (ret) {
+		NVT_ERR("error reading novatek,swrst-n8-addr. ret=%d\n", ret);
+		return ret;
+	}
+	NVT_LOG("SWRST_N8_ADDR=0x%06X\n", NVT_SPI_SWRST_N8_ADDR);
+
+	ret = of_property_read_u32(np, "novatek,spi-rd-fast-addr", &NVT_SPI_RD_FAST_ADDR);
+	if (ret) {
+		NVT_LOG("not support novatek,spi-rd-fast-addr\n");
+		NVT_SPI_RD_FAST_ADDR = 0;
+		ret = 0;
+	} else
+		NVT_LOG("SPI_RD_FAST_ADDR=0x%06X\n", NVT_SPI_RD_FAST_ADDR);
+
+	return ret;
+}
+#else
+static int32_t nvt_spi_parse_dt(struct device *dev)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+	ts->reset_gpio = NVTTOUCH_RST_PIN;
+#endif
+	ts->irq_gpio = NVTTOUCH_INT_PIN;
+	return 0;
+}
+#endif
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen config and request gpio
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. not 0---failed.
+ ******************************************************
+ */
+static int nvt_spi_gpio_config(struct nvt_spi_data_t *ts)
+{
+	int32_t ret = 0;
+
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+	/* request RST-pin (Output/High) */
+	if (gpio_is_valid(ts->reset_gpio)) {
+		ret = gpio_request_one(ts->reset_gpio, GPIOF_OUT_INIT_LOW, "NVT-tp-rst");
+		if (ret) {
+			NVT_ERR("Failed to request NVT-tp-rst GPIO\n");
+			goto err_request_reset_gpio;
+		}
+	}
+#endif
+
+	/* request INT-pin (Input) */
+	if (gpio_is_valid(ts->irq_gpio)) {
+		ret = gpio_request_one(ts->irq_gpio, GPIOF_IN, "NVT-int");
+		if (ret) {
+			NVT_ERR("Failed to request NVT-int GPIO\n");
+			goto err_request_irq_gpio;
+		}
+	}
+
+	return ret;
+
+err_request_irq_gpio:
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+	gpio_free(ts->reset_gpio);
+err_request_reset_gpio:
+#endif
+	return ret;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen deconfig gpio
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void nvt_spi_gpio_deconfig(struct nvt_spi_data_t *ts)
+{
+	if (gpio_is_valid(ts->irq_gpio))
+		gpio_free(ts->irq_gpio);
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+	if (gpio_is_valid(ts->reset_gpio))
+		gpio_free(ts->reset_gpio);
+#endif
+}
+
+static uint8_t nvt_spi_fw_recovery(uint8_t *point_data)
+{
+	uint8_t i = 0;
+	uint8_t detected = true;
+
+	/* check pattern */
+	for (i = 1; i < 7; i++) {
+		if (point_data[i] != 0x77) {
+			detected = false;
+			break;
+		}
+	}
+
+	return detected;
+}
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+void nvt_spi_esd_check_enable(uint8_t enable)
+{
+	/* update interrupt timer */
+	nvt_spi_irq_timer = jiffies;
+	/* clear esd_retry counter, if protect function is enabled */
+	nvt_spi_esd_retry = enable ? 0 : nvt_spi_esd_retry;
+	/* enable/disable esd check flag */
+	nvt_spi_esd_check = enable;
+}
+
+static void nvt_spi_esd_check_func(struct work_struct *work)
+{
+	unsigned int timer = jiffies_to_msecs(jiffies - nvt_spi_irq_timer);
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//NVT_LOG("esd_check = %d (retry %d)\n", esd_check, esd_retry);
+
+	if ((timer > NVT_SPI_TOUCH_ESD_CHECK_PERIOD) && esd_check) {
+		mutex_lock(&ts->lock);
+		NVT_ERR("do ESD recovery, timer = %d, retry = %d\n", timer, nvt_spi_esd_retry);
+		/* do esd recovery, reload fw */
+		nvt_spi_update_firmware(NVT_SPI_BOOT_UPDATE_FIRMWARE_NAME);
+		mutex_unlock(&ts->lock);
+		/* update interrupt timer */
+		nvt_spi_irq_timer = jiffies;
+		/* update esd_retry counter */
+		nvt_spi_esd_retry++;
+	}
+
+	queue_delayed_work(nvt_spi_esd_check_wq, &nvt_spi_esd_check_work,
+			msecs_to_jiffies(NVT_SPI_TOUCH_ESD_CHECK_PERIOD));
+}
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+#define NVT_SPI_PEN_DATA_LEN 14
+#if NVT_SPI_CHECK_PEN_DATA_CHECKSUM
+static int32_t nvt_spi_pen_data_checksum(uint8_t *buf, uint8_t length)
+{
+	uint8_t checksum = 0;
+	int32_t i = 0;
+
+	// Calculate checksum
+	for (i = 0; i < length - 1; i++)
+		checksum += buf[i];
+	checksum = (~checksum + 1);
+
+	// Compare ckecksum and dump fail data
+	if (checksum != buf[length - 1]) {
+		NVT_ERR("pen packet checksum not match. (buf[%d]=0x%02X, checksum=0x%02X)\n",
+			length - 1, buf[length - 1], checksum);
+		//--- dump pen buf ---
+		for (i = 0; i < length; i++)
+			NVT_ERR("%02X ", buf[i]);
+
+		NVT_ERR("\n");
+
+		return -EIO;
+	}
+
+	return 0;
+}
+#endif // #if NVT_SPI_CHECK_PEN_DATA_CHECKSUM
+
+#if NVT_SPI_TOUCH_WDT_RECOVERY
+static uint8_t nvt_spi_recovery_cnt;
+static uint8_t nvt_spi_wdt_fw_recovery(uint8_t *point_data)
+{
+	uint32_t recovery_cnt_max = 10;
+	uint8_t recovery_enable = false;
+	uint8_t i = 0;
+
+	nvt_spi_recovery_cnt++;
+
+	/* check pattern */
+	for (i = 1 ; i < 7 ; i++) {
+		if ((point_data[i] != 0xFD) && (point_data[i] != 0xFE)) {
+			nvt_spi_recovery_cnt = 0;
+			break;
+		}
+	}
+
+	if (nvt_spi_recovery_cnt > recovery_cnt_max) {
+		recovery_enable = true;
+		nvt_spi_recovery_cnt = 0;
+	}
+
+	return recovery_enable;
+}
+#endif	/* #if NVT_SPI_TOUCH_WDT_RECOVERY */
+
+#if NVT_SPI_POINT_DATA_CHECKSUM
+static int32_t nvt_spi_point_data_checksum(uint8_t *buf, uint8_t length)
+{
+	uint8_t checksum = 0;
+	int32_t i = 0;
+
+	// Generate checksum
+	for (i = 0; i < length - 1; i++)
+		checksum += buf[i + 1];
+
+	checksum = (~checksum + 1);
+
+	// Compare ckecksum and dump fail data
+	if (checksum != buf[length]) {
+		NVT_ERR("packet checksum not match. (point_data[%d]=0x%02X, checksum=0x%02X)\n",
+			length, buf[length], checksum);
+
+		for (i = 0; i < 10; i++)
+			NVT_LOG("%02X %02X %02X %02X %02X %02X\n",
+				buf[1 + i * 6], buf[2 + i * 6], buf[3 + i * 6],
+				buf[4 + i * 6], buf[5 + i * 6], buf[6 + i * 6]);
+
+		NVT_LOG("%02X %02X %02X %02X %02X\n",
+			buf[61], buf[62], buf[63], buf[64], buf[65]);
+		return -EIO;
+	}
+
+	return 0;
+}
+#endif /* NVT_SPI_POINT_DATA_CHECKSUM */
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen work function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+#define NVT_SPI_POINT_DATA_LEN 65
+#define NVT_SPI_POINT (NVT_SPI_POINT_DATA_LEN + NVT_SPI_PEN_DATA_LEN + 1 + NVT_SPI_DUMMY_BYTES)
+static irqreturn_t nvt_spi_work_func(int irq, void *data)
+{
+	int32_t ret = -1;
+	uint8_t point_data[NVT_SPI_POINT] = {0};
+	uint32_t position = 0;
+	uint32_t input_x = 0;
+	uint32_t input_y = 0;
+	uint32_t input_w = 0;
+	uint32_t input_p = 0;
+	uint8_t input_id = 0;
+#if NVT_SPI_MT_PROTOCOL_B
+	uint8_t press_id[NVT_SPI_TOUCH_MAX_FINGER_NUM] = {0};
+#endif /* NVT_SPI_MT_PROTOCOL_B */
+	int32_t i = 0;
+	int32_t finger_cnt = 0;
+	uint8_t pen_format_id = 0;
+	uint32_t pen_x = 0;
+	uint32_t pen_y = 0;
+	uint32_t pen_pressure = 0;
+	uint32_t pen_distance = 0;
+	int8_t pen_tilt_x = 0;
+	int8_t pen_tilt_y = 0;
+	uint32_t pen_btn1 = 0;
+	uint32_t pen_btn2 = 0;
+	uint32_t pen_battery = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+#if NVT_SPI_WAKEUP_GESTURE
+	if (bTouchIsAwake == 0)
+		pm_wakeup_event(&ts->input_dev->dev, 5000);
+
+#endif
+
+	mutex_lock(&ts->lock);
+
+	if (ts->pen_support)
+		ret = nvt_spi_read(point_data, NVT_SPI_POINT_DATA_LEN + NVT_SPI_PEN_DATA_LEN + 1);
+	else
+		ret = nvt_spi_read(point_data, NVT_SPI_POINT_DATA_LEN + 1);
+
+	if (ret < 0) {
+		NVT_ERR("nvt_spi_read failed.(%d)\n", ret);
+		goto XFER_ERROR;
+	}
+
+	//--- dump SPI buf ---
+/*
+ *	for (i = 0; i < 10; i++) {
+ *		printk("%02X %02X %02X %02X %02X %02X  ",
+ *			point_data[1+i*6], point_data[2+i*6], point_data[3+i*6],
+ *			point_data[4+i*6], point_data[5+i*6], point_data[6+i*6]);
+ *	}
+ *	printk("\n");
+ */
+
+#if NVT_SPI_TOUCH_WDT_RECOVERY
+	/* ESD protect by WDT */
+	if (nvt_spi_wdt_fw_recovery(point_data)) {
+		NVT_ERR("Recover for fw reset, %02X\n", point_data[1]);
+		nvt_spi_update_firmware(NVT_SPI_BOOT_UPDATE_FIRMWARE_NAME);
+		goto XFER_ERROR;
+	}
+#endif /* #if NVT_SPI_TOUCH_WDT_RECOVERY */
+
+	/* ESD protect by FW handshake */
+	if (nvt_spi_fw_recovery(point_data)) {
+#if NVT_SPI_TOUCH_ESD_PROTECT
+		nvt_spi_esd_check_enable(true);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+		goto XFER_ERROR;
+	}
+
+#if NVT_SPI_POINT_DATA_CHECKSUM
+	if (NVT_SPI_POINT_DATA_LEN >= NVT_SPI_POINT_DATA_CHECKSUM_LEN) {
+		ret = nvt_spi_point_data_checksum(point_data, NVT_SPI_POINT_DATA_CHECKSUM_LEN);
+		if (ret)
+			goto XFER_ERROR;
+	}
+#endif /* NVT_SPI_POINT_DATA_CHECKSUM */
+
+#if NVT_SPI_WAKEUP_GESTURE
+	if (bTouchIsAwake == 0) {
+		input_id = (uint8_t)(point_data[1] >> 3);
+		nvt_spi_wakeup_gesture_report(input_id, point_data);
+		mutex_unlock(&ts->lock);
+		return IRQ_HANDLED;
+	}
+#endif
+
+	finger_cnt = 0;
+
+	for (i = 0; i < ts->max_touch_num; i++) {
+		position = 1 + 6 * i;
+		input_id = (uint8_t)(point_data[position + 0] >> 3);
+		if ((input_id == 0) || (input_id > ts->max_touch_num))
+			continue;
+
+		if (((point_data[position] & 0x07) == 0x01)
+				|| ((point_data[position] & 0x07) == 0x02)) {
+			//finger down (enter & moving)
+#if NVT_SPI_TOUCH_ESD_PROTECT
+			/* update interrupt timer */
+			nvt_spi_irq_timer = jiffies;
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+			input_x = (uint32_t)(point_data[position + 1] << 4);
+			input_x += (uint32_t) (point_data[position + 3] >> 4);
+
+			input_y = (uint32_t)(point_data[position + 2] << 4);
+			input_y += (uint32_t) (point_data[position + 3] & 0x0F);
+			if ((input_x < 0) || (input_y < 0))
+				continue;
+			if ((input_x > ts->abs_x_max) || (input_y > ts->abs_y_max))
+				continue;
+			input_w = (uint32_t)(point_data[position + 4]);
+			if (input_w == 0)
+				input_w = 1;
+			if (i < 2) {
+				input_p = (uint32_t)(point_data[position + 5]);
+				input_p += (uint32_t)(point_data[i + 63] << 8);
+				if (input_p > NVT_SPI_TOUCH_FORCE_NUM)
+					input_p = NVT_SPI_TOUCH_FORCE_NUM;
+			} else
+				input_p = (uint32_t)(point_data[position + 5]);
+
+			if (input_p == 0)
+				input_p = 1;
+
+#if NVT_SPI_MT_PROTOCOL_B
+			press_id[input_id - 1] = 1;
+			input_mt_slot(ts->input_dev, input_id - 1);
+			input_mt_report_slot_state(ts->input_dev, MT_TOOL_FINGER, true);
+#else /* NVT_SPI_MT_PROTOCOL_B */
+			input_report_abs(ts->input_dev, ABS_MT_TRACKING_ID, input_id - 1);
+			input_report_key(ts->input_dev, BTN_TOUCH, 1);
+#endif /* NVT_SPI_MT_PROTOCOL_B */
+
+			input_report_abs(ts->input_dev, ABS_MT_POSITION_X, input_x);
+			input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, input_y);
+			input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, input_w);
+			input_report_abs(ts->input_dev, ABS_MT_PRESSURE, input_p);
+
+#if NVT_SPI_MT_PROTOCOL_B
+#else /* NVT_SPI_MT_PROTOCOL_B */
+			input_mt_sync(ts->input_dev);
+#endif /* NVT_SPI_MT_PROTOCOL_B */
+
+			finger_cnt++;
+		}
+	}
+
+#if NVT_SPI_MT_PROTOCOL_B
+	for (i = 0; i < ts->max_touch_num; i++) {
+		if (press_id[i] != 1) {
+			input_mt_slot(ts->input_dev, i);
+			input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0);
+			input_report_abs(ts->input_dev, ABS_MT_PRESSURE, 0);
+			input_mt_report_slot_state(ts->input_dev, MT_TOOL_FINGER, false);
+		}
+	}
+
+	input_report_key(ts->input_dev, BTN_TOUCH, (finger_cnt > 0));
+#else /* NVT_SPI_MT_PROTOCOL_B */
+	if (finger_cnt == 0) {
+		input_report_key(ts->input_dev, BTN_TOUCH, 0);
+		input_mt_sync(ts->input_dev);
+	}
+#endif /* NVT_SPI_MT_PROTOCOL_B */
+
+#if NVT_SPI_TOUCH_KEY_NUM > 0
+	if (point_data[61] == 0xF8) {
+#if NVT_SPI_TOUCH_ESD_PROTECT
+		/* update interrupt timer */
+		nvt_spi_irq_timer = jiffies;
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+		for (i = 0; i < ts->max_button_num; i++)
+			input_report_key(ts->input_dev, nvt_spi_touch_key_array[i],
+					((point_data[62] >> i) & 0x01));
+
+	} else {
+		for (i = 0; i < ts->max_button_num; i++)
+			input_report_key(ts->input_dev, nvt_spi_touch_key_array[i], 0);
+	}
+#endif
+
+	input_sync(ts->input_dev);
+
+	if (ts->pen_support) {
+
+		//--- dump pen buf ---
+/*
+ *		printk("%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X\n",
+ *			point_data[66], point_data[67], point_data[68], point_data[69],
+ *			point_data[70], point_data[71], point_data[72], point_data[73],
+ *			point_data[74], point_data[75], point_data[76], point_data[77],
+ *			point_data[78], point_data[79]);
+ */
+#if NVT_SPI_CHECK_PEN_DATA_CHECKSUM
+		if (nvt_spi_pen_data_checksum(&point_data[66], NVT_SPI_PEN_DATA_LEN)) {
+			// pen data packet checksum not match, skip it
+			goto XFER_ERROR;
+		}
+#endif // #if NVT_SPI_CHECK_PEN_DATA_CHECKSUM
+
+		// parse and handle pen report
+		pen_format_id = point_data[66];
+		if (pen_format_id != 0xFF) {
+			if (pen_format_id == 0x01) {
+				// report pen data
+				pen_x = (uint32_t)(point_data[67] << 8);
+				pen_x += (uint32_t)(point_data[68]);
+
+				pen_y = (uint32_t)(point_data[69] << 8);
+				pen_y += (uint32_t)(point_data[70]);
+
+				pen_pressure = (uint32_t)(point_data[71] << 8);
+				pen_pressure +=  (uint32_t)(point_data[72]);
+
+				pen_tilt_x = (int32_t)point_data[73];
+				pen_tilt_y = (int32_t)point_data[74];
+
+				pen_distance = (uint32_t)(point_data[75] << 8);
+				pen_distance += (uint32_t)(point_data[76]);
+
+				pen_btn1 = (uint32_t)(point_data[77] & 0x01);
+				pen_btn2 = (uint32_t)((point_data[77] >> 1) & 0x01);
+				pen_battery = (uint32_t)point_data[78];
+//				printk("x=%d,y=%d,p=%d,tx=%d,ty=%d,d=%d,b1=%d,b2=%d,bat=%d\n",
+//					pen_x, pen_y, pen_pressure, pen_tilt_x, pen_tilt_y,
+//					pen_distance, pen_btn1, pen_btn2, pen_battery);
+
+				input_report_abs(ts->pen_input_dev, ABS_X, pen_x);
+				input_report_abs(ts->pen_input_dev, ABS_Y, pen_y);
+				input_report_abs(ts->pen_input_dev, ABS_PRESSURE, pen_pressure);
+				input_report_key(ts->pen_input_dev, BTN_TOUCH, !!pen_pressure);
+				input_report_abs(ts->pen_input_dev, ABS_TILT_X, pen_tilt_x);
+				input_report_abs(ts->pen_input_dev, ABS_TILT_Y, pen_tilt_y);
+				input_report_abs(ts->pen_input_dev, ABS_DISTANCE, pen_distance);
+				input_report_key(ts->pen_input_dev, BTN_TOOL_PEN,
+						!!pen_distance || !!pen_pressure);
+				input_report_key(ts->pen_input_dev, BTN_STYLUS, pen_btn1);
+				input_report_key(ts->pen_input_dev, BTN_STYLUS2, pen_btn2);
+				// TBD: pen battery event report
+				// NVT_LOG("pen_battery=%d\n", pen_battery);
+			} else if (pen_format_id == 0xF0) {
+				// report Pen ID
+			} else {
+				NVT_ERR("Unknown pen format id!\n");
+				goto XFER_ERROR;
+			}
+		} else { // pen_format_id = 0xFF, i.e. no pen present
+			input_report_abs(ts->pen_input_dev, ABS_X, 0);
+			input_report_abs(ts->pen_input_dev, ABS_Y, 0);
+			input_report_abs(ts->pen_input_dev, ABS_PRESSURE, 0);
+			input_report_abs(ts->pen_input_dev, ABS_TILT_X, 0);
+			input_report_abs(ts->pen_input_dev, ABS_TILT_Y, 0);
+			input_report_abs(ts->pen_input_dev, ABS_DISTANCE, 0);
+			input_report_key(ts->pen_input_dev, BTN_TOUCH, 0);
+			input_report_key(ts->pen_input_dev, BTN_TOOL_PEN, 0);
+			input_report_key(ts->pen_input_dev, BTN_STYLUS, 0);
+			input_report_key(ts->pen_input_dev, BTN_STYLUS2, 0);
+		}
+
+		input_sync(ts->pen_input_dev);
+	} /* if (ts->pen_support) */
+
+XFER_ERROR:
+
+	mutex_unlock(&ts->lock);
+
+	return IRQ_HANDLED;
+}
+
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen check chip version trim function.
+ *
+ * return:
+ *	Executive outcomes. 0---NVT IC. -1---not NVT IC.
+ ******************************************************
+ */
+static int8_t nvt_spi_check_chip_ver_trim(uint32_t chip_ver_trim_addr)
+{
+	uint8_t buf[8] = {0};
+	int32_t retry = 0;
+	int32_t list = 0;
+	int32_t i = 0;
+	int32_t found_nvt_chip = 0;
+	int32_t ret = -1;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---Check for 5 times---
+	for (retry = 5; retry > 0; retry--) {
+
+		nvt_spi_bootloader_reset();
+
+		nvt_spi_set_page(chip_ver_trim_addr);
+
+		buf[0] = chip_ver_trim_addr & 0x7F;
+		buf[1] = 0x00;
+		buf[2] = 0x00;
+		buf[3] = 0x00;
+		buf[4] = 0x00;
+		buf[5] = 0x00;
+		buf[6] = 0x00;
+		nvt_spi_read(buf, 7);
+		NVT_LOG("buf[1]=0x%02X,[2]=0x%02X,[3]=0x%02X,[4]=0x%02X,[5]=0x%02X,[6]=0x%02X\n",
+			buf[1], buf[2], buf[3], buf[4], buf[5], buf[6]);
+
+		// compare read chip id on supported list
+		for (list = 0; list < ARRAY_SIZE(nvt_spi_trim_id_table); list++) {
+			found_nvt_chip = 0;
+
+			// compare each byte
+			for (i = 0; i < NVT_SPI_ID_BYTE_MAX; i++) {
+				if (nvt_spi_trim_id_table[list].mask[i]) {
+					if (buf[i + 1] != nvt_spi_trim_id_table[list].id[i])
+						break;
+				}
+			}
+
+			if (i == NVT_SPI_ID_BYTE_MAX)
+				found_nvt_chip = 1;
+
+			if (found_nvt_chip) {
+				NVT_LOG("This is NVT touch IC\n");
+				ts->mmap = nvt_spi_trim_id_table[list].mmap;
+				ts->hw_crc = nvt_spi_trim_id_table[list].hwinfo->hw_crc;
+				ret = 0;
+				goto out;
+			} else {
+				ts->mmap = NULL;
+				ret = -1;
+			}
+		}
+
+		msleep(20);
+	}
+
+out:
+	return ret;
+}
+
+#if defined(CONFIG_DRM)
+static int nvt_spi_check_dt(struct device_node *np)
+{
+	int i;
+	int count;
+	struct device_node *node;
+	struct drm_panel *panel;
+
+	count = of_count_phandle_with_args(np, "panel", NULL);
+	if (count <= 0)
+		return 0;
+
+	for (i = 0; i < count; i++) {
+		node = of_parse_phandle(np, "panel", i);
+		panel = of_drm_find_panel(node);
+		of_node_put(node);
+		if (!IS_ERR(panel)) {
+			active_spi_panel = panel;
+			return 0;
+		}
+	}
+
+	return PTR_ERR(panel);
+}
+#endif
+
+static int32_t nvt_spi_late_probe(struct spi_device *client)
+{
+	int32_t ret = 0;
+
+	//---check chip version trim---
+	ret = nvt_spi_check_chip_ver_trim(NVT_SPI_CHIP_VER_TRIM_ADDR);
+	if (ret) {
+		NVT_LOG("try to check from old chip ver trim address\n");
+		ret = nvt_spi_check_chip_ver_trim(NVT_SPI_CHIP_VER_TRIM_OLD_ADDR);
+		if (ret) {
+			NVT_ERR("chip is not identified\n");
+			ret = -EINVAL;
+			return ret;
+		}
+	}
+
+	if (nvt_spi_update_firmware(NVT_SPI_BOOT_UPDATE_FIRMWARE_NAME))
+		NVT_ERR("download firmware failed, ignore check fw state\n");
+	else
+		nvt_spi_check_fw_reset_state(NVT_SPI_RESET_STATE_REK);
+
+	//---set device node---
+#if NVT_SPI_TOUCH_PROC
+	ret = nvt_spi_flash_proc_init();
+	if (ret != 0) {
+		NVT_ERR("nvt flash proc init failed. ret=%d\n", ret);
+		goto err_flash_proc_init_failed;
+	}
+#endif
+
+#if NVT_SPI_TOUCH_EXT_PROC
+	ret = nvt_spi_extra_proc_init();
+	if (ret != 0) {
+		NVT_ERR("nvt extra proc init failed. ret=%d\n", ret);
+		goto err_extra_proc_init_failed;
+	}
+#endif
+
+#if NVT_SPI_TOUCH_MP
+	ret = nvt_spi_mp_proc_init();
+	if (ret != 0) {
+		NVT_ERR("nvt mp proc init failed. ret=%d\n", ret);
+		goto err_mp_proc_init_failed;
+	}
+#endif
+
+	bTouchIsAwake = 1;
+	NVT_LOG("end\n");
+
+	nvt_spi_irq_enable(true);
+	return 0;
+
+#if NVT_SPI_TOUCH_MP
+	nvt_spi_mp_proc_deinit();
+err_mp_proc_init_failed:
+#endif
+#if NVT_SPI_TOUCH_EXT_PROC
+	nvt_spi_extra_proc_deinit();
+err_extra_proc_init_failed:
+#endif
+#if NVT_SPI_TOUCH_PROC
+	nvt_spi_flash_proc_deinit();
+err_flash_proc_init_failed:
+#endif
+	return 0;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen driver probe function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. negative---failed
+ ******************************************************
+ */
+static int32_t nvt_spi_probe(struct spi_device *client)
+{
+	int32_t ret = 0;
+#if defined(CONFIG_DRM)
+	struct device_node *dp = NULL;
+#endif
+#if ((NVT_SPI_TOUCH_KEY_NUM > 0) || NVT_SPI_WAKEUP_GESTURE)
+	int32_t retry = 0;
+#endif
+	struct nvt_spi_data_t *ts;
+
+#if defined(CONFIG_DRM)
+	dp = client->dev.of_node;
+
+	ret = nvt_spi_check_dt(dp);
+	if (ret == -EPROBE_DEFER)
+		return ret;
+
+	if (ret) {
+		ret = -ENODEV;
+		return ret;
+	}
+#endif
+
+	NVT_LOG("start\n");
+
+	ts = kzalloc(sizeof(struct nvt_spi_data_t), GFP_KERNEL);
+	if (ts == NULL) {
+		NVT_ERR("failed to allocated memory for nvt ts data\n");
+		return -ENOMEM;
+	}
+	nvt_spi_data = ts;
+
+	ts->xbuf = kzalloc(NVT_SPI_XBUF_LEN, GFP_KERNEL);
+	if (ts->xbuf == NULL) {
+		NVT_ERR("kzalloc for xbuf failed!\n");
+		ret = -ENOMEM;
+		goto err_malloc_xbuf;
+	}
+
+	ts->rbuf = kzalloc(NVT_SPI_READ_LEN, GFP_KERNEL);
+	if (ts->rbuf == NULL) {
+		NVT_ERR("kzalloc for rbuf failed!\n");
+		ret = -ENOMEM;
+		goto err_malloc_rbuf;
+	}
+
+	ts->client = client;
+	spi_set_drvdata(client, ts);
+
+	//---prepare for spi parameter---
+	if (ts->client->master->flags & SPI_MASTER_HALF_DUPLEX) {
+		NVT_ERR("Full duplex not supported by master\n");
+		ret = -EIO;
+		goto err_ckeck_full_duplex;
+	}
+	ts->client->bits_per_word = 8;
+	ts->client->mode = SPI_MODE_0;
+
+	ret = spi_setup(ts->client);
+	if (ret < 0) {
+		NVT_ERR("Failed to perform SPI setup\n");
+		goto err_spi_setup;
+	}
+
+	NVT_LOG("mode=%d, max_speed_hz=%d\n", ts->client->mode, ts->client->max_speed_hz);
+
+	//---parse dts---
+	ret = nvt_spi_parse_dt(&client->dev);
+	if (ret) {
+		NVT_ERR("parse dt error\n");
+		goto err_spi_setup;
+	}
+
+	//---request and config GPIOs---
+	ret = nvt_spi_gpio_config(ts);
+	if (ret) {
+		NVT_ERR("gpio config error!\n");
+		goto err_gpio_config_failed;
+	}
+
+	mutex_init(&ts->lock);
+	mutex_init(&ts->xbuf_lock);
+
+	//---eng reset before TP_RESX high
+	nvt_spi_eng_reset();
+
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+	gpio_set_value(ts->reset_gpio, 1);
+#endif
+
+	// need 10ms delay after POR(power on reset)
+	msleep(20);
+
+	ts->abs_x_max = NVT_SPI_TOUCH_DEFAULT_MAX_WIDTH;
+	ts->abs_y_max = NVT_SPI_TOUCH_DEFAULT_MAX_HEIGHT;
+
+	//---allocate input device---
+	ts->input_dev = input_allocate_device();
+	if (ts->input_dev == NULL) {
+		NVT_ERR("allocate input device failed\n");
+		ret = -ENOMEM;
+		goto err_input_dev_alloc_failed;
+	}
+
+	ts->max_touch_num = NVT_SPI_TOUCH_MAX_FINGER_NUM;
+
+#if NVT_SPI_TOUCH_KEY_NUM > 0
+	ts->max_button_num = NVT_SPI_TOUCH_KEY_NUM;
+#endif
+
+	ts->int_trigger_type = NVT_SPI_INT_TRIGGER_TYPE;
+
+
+	//---set input device info.---
+	ts->input_dev->evbit[0] = BIT_MASK(EV_SYN) | BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	ts->input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+	ts->input_dev->propbit[0] = BIT(INPUT_PROP_DIRECT);
+
+#if NVT_SPI_MT_PROTOCOL_B
+	input_mt_init_slots(ts->input_dev, ts->max_touch_num, 0);
+#endif
+
+	// pressure = NVT_SPI_TOUCH_FORCE_NUM
+	input_set_abs_params(ts->input_dev, ABS_MT_PRESSURE, 0, NVT_SPI_TOUCH_FORCE_NUM, 0, 0);
+
+#if NVT_SPI_TOUCH_MAX_FINGER_NUM > 1
+	// area = 255
+	input_set_abs_params(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0, 255, 0, 0);
+
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_X, 0, ts->abs_x_max, 0, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_Y, 0, ts->abs_y_max, 0, 0);
+#if NVT_SPI_MT_PROTOCOL_B
+	// no need to set ABS_MT_TRACKING_ID, input_mt_init_slots() already set it
+#else
+	input_set_abs_params(ts->input_dev, ABS_MT_TRACKING_ID, 0, ts->max_touch_num, 0, 0);
+#endif //NVT_SPI_MT_PROTOCOL_B
+#endif //NVT_SPI_TOUCH_MAX_FINGER_NUM > 1
+
+#if NVT_SPI_TOUCH_KEY_NUM > 0
+	for (retry = 0; retry < ts->max_button_num; retry++)
+		input_set_capability(ts->input_dev, EV_KEY, nvt_spi_touch_key_array[retry]);
+#endif
+
+#if NVT_SPI_WAKEUP_GESTURE
+	for (retry = 0; retry < ARRAY_SIZE(nvt_spi_gesture_key_array); retry++)
+		input_set_capability(ts->input_dev, EV_KEY, nvt_spi_gesture_key_array[retry]);
+#endif
+
+	snprintf(ts->phys, sizeof(ts->phys), "input/ts");
+	ts->input_dev->name = NVT_SPI_TS_NAME;
+	ts->input_dev->phys = ts->phys;
+	ts->input_dev->id.bustype = BUS_SPI;
+
+	//---register input device---
+	ret = input_register_device(ts->input_dev);
+	if (ret) {
+		NVT_ERR("register input device (%s) failed. ret=%d\n", ts->input_dev->name, ret);
+		goto err_input_register_device_failed;
+	}
+
+	if (ts->pen_support) {
+		//---allocate pen input device---
+		ts->pen_input_dev = input_allocate_device();
+		if (ts->pen_input_dev == NULL) {
+			NVT_ERR("allocate pen input device failed\n");
+			ret = -ENOMEM;
+			goto err_pen_input_dev_alloc_failed;
+		}
+
+		//---set pen input device info.---
+		ts->pen_input_dev->evbit[0] =
+				BIT_MASK(EV_SYN) | BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+		ts->pen_input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+		ts->pen_input_dev->keybit[BIT_WORD(BTN_TOOL_PEN)] |= BIT_MASK(BTN_TOOL_PEN);
+		//ts->pen_input_dev->keybit[BIT_WORD(BTN_TOOL_RUBBER)]
+		//		|= BIT_MASK(BTN_TOOL_RUBBER);
+		ts->pen_input_dev->keybit[BIT_WORD(BTN_STYLUS)] |= BIT_MASK(BTN_STYLUS);
+		ts->pen_input_dev->keybit[BIT_WORD(BTN_STYLUS2)] |= BIT_MASK(BTN_STYLUS2);
+		ts->pen_input_dev->propbit[0] = BIT(INPUT_PROP_DIRECT);
+
+		if (ts->wgp_stylus) {
+			input_set_abs_params(ts->pen_input_dev, ABS_X, 0,
+					ts->abs_x_max * 2, 0, 0);
+			input_set_abs_params(ts->pen_input_dev, ABS_Y, 0,
+					ts->abs_y_max * 2, 0, 0);
+		} else {
+			input_set_abs_params(ts->pen_input_dev, ABS_X, 0,
+					ts->abs_x_max, 0, 0);
+
+			input_set_abs_params(ts->pen_input_dev, ABS_Y, 0,
+					ts->abs_y_max, 0, 0);
+		}
+		input_set_abs_params(ts->pen_input_dev, ABS_PRESSURE, 0,
+				NVT_SPI_PEN_PRESSURE_MAX, 0, 0);
+		input_set_abs_params(ts->pen_input_dev, ABS_DISTANCE, 0,
+				NVT_SPI_PEN_DISTANCE_MAX, 0, 0);
+		input_set_abs_params(ts->pen_input_dev, ABS_TILT_X,
+				NVT_SPI_PEN_TILT_MIN, NVT_SPI_PEN_TILT_MAX, 0, 0);
+		input_set_abs_params(ts->pen_input_dev, ABS_TILT_Y,
+				NVT_SPI_PEN_TILT_MIN, NVT_SPI_PEN_TILT_MAX, 0, 0);
+
+		snprintf(ts->pen_phys, sizeof(ts->pen_phys), "input/pen");
+		ts->pen_input_dev->name = NVT_SPI_PEN_NAME;
+		ts->pen_input_dev->phys = ts->pen_phys;
+		ts->pen_input_dev->id.bustype = BUS_SPI;
+
+		//---register pen input device---
+		ret = input_register_device(ts->pen_input_dev);
+		if (ret) {
+			NVT_ERR("register pen input device (%s) failed. ret=%d\n",
+					ts->pen_input_dev->name, ret);
+			goto err_pen_input_register_device_failed;
+		}
+	} /* if (ts->pen_support) */
+
+	//---set int-pin & request irq---
+	client->irq = gpio_to_irq(ts->irq_gpio);
+	if (client->irq) {
+		NVT_LOG("int_trigger_type=%d\n", ts->int_trigger_type);
+		ts->irq_enabled = true;
+		ret = request_threaded_irq(client->irq, NULL, nvt_spi_work_func,
+				ts->int_trigger_type | IRQF_ONESHOT, NVT_SPI_NAME, ts);
+		if (ret != 0) {
+			NVT_ERR("request irq failed. ret=%d\n", ret);
+			goto err_int_request_failed;
+		} else {
+			nvt_spi_irq_enable(false);
+			NVT_LOG("request irq %d succeed\n", client->irq);
+		}
+	}
+
+#if NVT_SPI_WAKEUP_GESTURE
+	device_init_wakeup(&ts->input_dev->dev, 1);
+#endif
+
+#if NVT_SPI_BOOT_UPDATE_FIRMWARE
+	nvt_spi_fwu_wq = alloc_workqueue("nvt_spi_fwu_wq", WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	if (!nvt_spi_fwu_wq) {
+		NVT_ERR("nvt_spi_fwu_wq create workqueue failed\n");
+		ret = -ENOMEM;
+		goto err_create_nvt_spi_fwu_wq_failed;
+	}
+	INIT_DELAYED_WORK(&ts->nvt_fwu_work, nvt_spi_update_firmware_work);
+
+	// please make sure boot update start after display reset(RESX) sequence
+	//queue_delayed_work(nvt_spi_fwu_wq, &ts->nvt_fwu_work, msecs_to_jiffies(14000));
+#endif
+
+	NVT_LOG("NVT_SPI_TOUCH_ESD_PROTECT is %d\n", NVT_SPI_TOUCH_ESD_PROTECT);
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	INIT_DELAYED_WORK(&nvt_spi_esd_check_work, nvt_spi_esd_check_func);
+	nvt_spi_esd_check_wq = alloc_workqueue("nvt_spi_esd_check_wq", WQ_MEM_RECLAIM, 1);
+	if (!nvt_spi_esd_check_wq) {
+		NVT_ERR("nvt_spi_esd_check_wq create workqueue failed\n");
+		ret = -ENOMEM;
+		goto err_create_nvt_spi_esd_check_wq_failed;
+	}
+	queue_delayed_work(nvt_spi_esd_check_wq, &nvt_spi_esd_check_work,
+			msecs_to_jiffies(NVT_SPI_TOUCH_ESD_CHECK_PERIOD));
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+
+#if defined(CONFIG_DRM)
+	//if (!strcmp(ts->touch_environment, "pvm"))
+	nvt_spi_register_for_panel_events(client->dev.of_node, ts);
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	ts->drm_notif.notifier_call = nvt_drm_notifier_callback;
+	ret = msm_drm_register_client(&ts->drm_notif);
+	if (ret) {
+		NVT_ERR("register drm_notifier failed. ret=%d\n", ret);
+		goto err_register_drm_notif_failed;
+	}
+#else
+	ts->fb_notif.notifier_call = nvt_fb_notifier_callback;
+	ret = fb_register_client(&ts->fb_notif);
+	if (ret) {
+		NVT_ERR("register fb_notifier failed. ret=%d\n", ret);
+		goto err_register_fb_notif_failed;
+	}
+#endif
+
+	NVT_LOG("end\n");
+	return 0;
+
+#if defined(CONFIG_DRM)
+
+#elif defined(_MSM_DRM_NOTIFY_H_)
+err_register_drm_notif_failed:
+#else
+err_register_fb_notif_failed:
+#endif
+
+#if NVT_SPI_TOUCH_MP
+	nvt_spi_mp_proc_deinit();
+#endif
+#if NVT_SPI_TOUCH_EXT_PROC
+	nvt_spi_extra_proc_deinit();
+
+#endif
+#if NVT_SPI_TOUCH_PROC
+	nvt_spi_flash_proc_deinit();
+
+#endif
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	if (nvt_spi_esd_check_wq) {
+		cancel_delayed_work_sync(&nvt_spi_esd_check_work);
+		destroy_workqueue(nvt_spi_esd_check_wq);
+		nvt_spi_esd_check_wq = NULL;
+	}
+err_create_nvt_spi_esd_check_wq_failed:
+#endif
+#if NVT_SPI_BOOT_UPDATE_FIRMWARE
+	if (nvt_spi_fwu_wq) {
+		cancel_delayed_work_sync(&ts->nvt_fwu_work);
+		destroy_workqueue(nvt_spi_fwu_wq);
+		nvt_spi_fwu_wq = NULL;
+	}
+err_create_nvt_spi_fwu_wq_failed:
+#endif
+#if NVT_SPI_WAKEUP_GESTURE
+	device_init_wakeup(&ts->input_dev->dev, 0);
+#endif
+	free_irq(client->irq, ts);
+err_int_request_failed:
+	if (ts->pen_support) {
+		input_unregister_device(ts->pen_input_dev);
+		ts->pen_input_dev = NULL;
+	}
+err_pen_input_register_device_failed:
+	if (ts->pen_support) {
+		if (ts->pen_input_dev) {
+			input_free_device(ts->pen_input_dev);
+			ts->pen_input_dev = NULL;
+		}
+	}
+err_pen_input_dev_alloc_failed:
+	input_unregister_device(ts->input_dev);
+	ts->input_dev = NULL;
+err_input_register_device_failed:
+	if (ts->input_dev) {
+		input_free_device(ts->input_dev);
+		ts->input_dev = NULL;
+	}
+err_input_dev_alloc_failed:
+	mutex_destroy(&ts->xbuf_lock);
+	mutex_destroy(&ts->lock);
+	nvt_spi_gpio_deconfig(ts);
+err_gpio_config_failed:
+err_spi_setup:
+err_ckeck_full_duplex:
+	spi_set_drvdata(client, NULL);
+	kfree(ts->rbuf);
+	ts->rbuf = NULL;
+err_malloc_rbuf:
+	kfree(ts->xbuf);
+	ts->xbuf = NULL;
+err_malloc_xbuf:
+	kfree(ts);
+	ts = NULL;
+
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen driver release function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t nvt_spi_remove(struct spi_device *client)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	NVT_LOG("Removing driver...\n");
+
+#if defined(CONFIG_DRM)
+	if (active_spi_panel && ts->notifier_cookie)
+		panel_event_notifier_unregister(ts->notifier_cookie);
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	if (msm_drm_unregister_client(&ts->drm_notif))
+		NVT_ERR("Error occurred while unregistering drm_notifier.\n");
+#else
+	if (fb_unregister_client(&ts->fb_notif))
+		NVT_ERR("Error occurred while unregistering fb_notifier.\n");
+#endif
+
+#if NVT_SPI_TOUCH_MP
+	nvt_spi_mp_proc_deinit();
+#endif
+#if NVT_SPI_TOUCH_EXT_PROC
+	nvt_spi_extra_proc_deinit();
+#endif
+#if NVT_SPI_TOUCH_PROC
+	nvt_spi_flash_proc_deinit();
+#endif
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	if (nvt_spi_esd_check_wq) {
+		cancel_delayed_work_sync(&nvt_spi_esd_check_work);
+		nvt_spi_esd_check_enable(false);
+		destroy_workqueue(nvt_spi_esd_check_wq);
+		nvt_spi_esd_check_wq = NULL;
+	}
+#endif
+
+#if NVT_SPI_BOOT_UPDATE_FIRMWARE
+	if (nvt_spi_fwu_wq) {
+		cancel_delayed_work_sync(&ts->nvt_fwu_work);
+		destroy_workqueue(nvt_spi_fwu_wq);
+		nvt_spi_fwu_wq = NULL;
+	}
+#endif
+
+#if NVT_SPI_WAKEUP_GESTURE
+	device_init_wakeup(&ts->input_dev->dev, 0);
+#endif
+
+	nvt_spi_irq_enable(false);
+	free_irq(client->irq, ts);
+
+	mutex_destroy(&ts->xbuf_lock);
+	mutex_destroy(&ts->lock);
+
+	nvt_spi_gpio_deconfig(ts);
+
+	if (ts->pen_support) {
+		if (ts->pen_input_dev) {
+			input_unregister_device(ts->pen_input_dev);
+			ts->pen_input_dev = NULL;
+		}
+	}
+
+	if (ts->input_dev) {
+		input_unregister_device(ts->input_dev);
+		ts->input_dev = NULL;
+	}
+
+	spi_set_drvdata(client, NULL);
+
+	kfree(ts->xbuf);
+	ts->xbuf = NULL;
+
+	kfree(ts);
+	ts = NULL;
+
+	return 0;
+}
+
+static void nvt_spi_shutdown(struct spi_device *client)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	NVT_LOG("Shutdown driver...\n");
+
+	nvt_spi_irq_enable(false);
+
+#if defined(CONFIG_DRM)
+	if (active_spi_panel && ts->notifier_cookie)
+		panel_event_notifier_unregister(ts->notifier_cookie);
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	if (msm_drm_unregister_client(&ts->drm_notif))
+		NVT_ERR("Error occurred while unregistering drm_notifier.\n");
+#else
+	if (fb_unregister_client(&ts->fb_notif))
+		NVT_ERR("Error occurred while unregistering fb_notifier.\n");
+#endif
+
+#if NVT_SPI_TOUCH_MP
+	nvt_spi_mp_proc_deinit();
+#endif
+#if NVT_SPI_TOUCH_EXT_PROC
+	nvt_spi_extra_proc_deinit();
+#endif
+#if NVT_SPI_TOUCH_PROC
+	nvt_spi_flash_proc_deinit();
+#endif
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	if (nvt_spi_esd_check_wq) {
+		cancel_delayed_work_sync(&nvt_spi_esd_check_work);
+		nvt_spi_esd_check_enable(false);
+		destroy_workqueue(nvt_spi_esd_check_wq);
+		nvt_spi_esd_check_wq = NULL;
+	}
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+#if NVT_SPI_BOOT_UPDATE_FIRMWARE
+	if (nvt_spi_fwu_wq) {
+		cancel_delayed_work_sync(&ts->nvt_fwu_work);
+		destroy_workqueue(nvt_spi_fwu_wq);
+		nvt_spi_fwu_wq = NULL;
+	}
+#endif
+
+#if NVT_SPI_WAKEUP_GESTURE
+	device_init_wakeup(&ts->input_dev->dev, 0);
+#endif
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen driver suspend function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t nvt_spi_suspend(struct device *dev)
+{
+	uint8_t buf[4] = {0};
+#if NVT_SPI_MT_PROTOCOL_B
+	uint32_t i = 0;
+#endif
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (!bTouchIsAwake) {
+		NVT_LOG("Touch is already suspend\n");
+		return 0;
+	}
+
+#if !NVT_SPI_WAKEUP_GESTURE
+	nvt_spi_irq_enable(false);
+#endif
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	NVT_LOG("cancel delayed work sync\n");
+	cancel_delayed_work_sync(&nvt_spi_esd_check_work);
+	nvt_spi_esd_check_enable(false);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	mutex_lock(&ts->lock);
+
+	NVT_LOG("start\n");
+
+	bTouchIsAwake = 0;
+
+#if NVT_SPI_WAKEUP_GESTURE
+	//---write command to enter "wakeup gesture mode"---
+	buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD;
+	buf[1] = 0x13;
+	nvt_spi_write(buf, 2);
+
+	enable_irq_wake(ts->client->irq);
+
+	NVT_LOG("Enabled touch wakeup gesture\n");
+
+#else // NVT_SPI_WAKEUP_GESTURE
+	//---write command to enter "deep sleep mode"---
+	buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD;
+	buf[1] = 0x11;
+	nvt_spi_write(buf, 2);
+#endif // NVT_SPI_WAKEUP_GESTURE
+
+	mutex_unlock(&ts->lock);
+
+	/* release all touches */
+#if NVT_SPI_MT_PROTOCOL_B
+	for (i = 0; i < ts->max_touch_num; i++) {
+		input_mt_slot(ts->input_dev, i);
+		input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0);
+		input_report_abs(ts->input_dev, ABS_MT_PRESSURE, 0);
+		input_mt_report_slot_state(ts->input_dev, MT_TOOL_FINGER, 0);
+	}
+#endif
+	input_report_key(ts->input_dev, BTN_TOUCH, 0);
+#if !NVT_SPI_MT_PROTOCOL_B
+	input_mt_sync(ts->input_dev);
+#endif
+	input_sync(ts->input_dev);
+
+	/* release pen event */
+	if (ts->pen_support) {
+		input_report_abs(ts->pen_input_dev, ABS_X, 0);
+		input_report_abs(ts->pen_input_dev, ABS_Y, 0);
+		input_report_abs(ts->pen_input_dev, ABS_PRESSURE, 0);
+		input_report_abs(ts->pen_input_dev, ABS_TILT_X, 0);
+		input_report_abs(ts->pen_input_dev, ABS_TILT_Y, 0);
+		input_report_abs(ts->pen_input_dev, ABS_DISTANCE, 0);
+		input_report_key(ts->pen_input_dev, BTN_TOUCH, 0);
+		input_report_key(ts->pen_input_dev, BTN_TOOL_PEN, 0);
+		input_report_key(ts->pen_input_dev, BTN_STYLUS, 0);
+		input_report_key(ts->pen_input_dev, BTN_STYLUS2, 0);
+		input_sync(ts->pen_input_dev);
+	}
+
+	msleep(50);
+
+	NVT_LOG("end\n");
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen driver resume function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t nvt_spi_resume(struct device *dev)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (bTouchIsAwake || ts->fw_ver == 0) {
+		nvt_spi_late_probe(ts->client);
+		NVT_LOG("nvt_spi_late_probe\n");
+		return 0;
+	}
+
+	mutex_lock(&ts->lock);
+
+	NVT_LOG("start\n");
+
+	// please make sure display reset(RESX) sequence and mipi dsi cmds sent before this
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+	gpio_set_value(ts->reset_gpio, 1);
+#endif
+
+	if (nvt_spi_update_firmware(NVT_SPI_BOOT_UPDATE_FIRMWARE_NAME))
+		NVT_ERR("download firmware failed, ignore check fw state\n");
+	else
+		nvt_spi_check_fw_reset_state(NVT_SPI_RESET_STATE_REK);
+
+#if !NVT_SPI_WAKEUP_GESTURE
+	nvt_spi_irq_enable(true);
+#endif
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	nvt_spi_esd_check_enable(false);
+	queue_delayed_work(nvt_spi_esd_check_wq, &nvt_spi_esd_check_work,
+			msecs_to_jiffies(NVT_SPI_TOUCH_ESD_CHECK_PERIOD));
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	bTouchIsAwake = 1;
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("end\n");
+
+	return 0;
+}
+
+#if defined(CONFIG_DRM)
+
+static void nvt_spi_panel_notifier_callback(enum panel_event_notifier_tag tag,
+		 struct panel_event_notification *notification, void *client_data)
+{
+	struct nvt_spi_data_t *ts = client_data;
+
+	if (!notification) {
+		pr_err("Invalid notification\n");
+		return;
+	}
+
+	NVT_LOG("Notification type:%d, early_trigger:%d",
+			notification->notif_type,
+			notification->notif_data.early_trigger);
+
+	switch (notification->notif_type) {
+	case DRM_PANEL_EVENT_UNBLANK:
+		if (notification->notif_data.early_trigger)
+			NVT_LOG("resume notification pre commit\n");
+		else
+			nvt_spi_resume(&ts->client->dev);
+		break;
+
+	case DRM_PANEL_EVENT_BLANK:
+		if (notification->notif_data.early_trigger)
+			nvt_spi_suspend(&ts->client->dev);
+		else
+			NVT_LOG("suspend notification post commit\n");
+		break;
+
+	case DRM_PANEL_EVENT_BLANK_LP:
+		NVT_LOG("received lp event\n");
+		break;
+
+	case DRM_PANEL_EVENT_FPS_CHANGE:
+		NVT_LOG("shashank:Received fps change old fps:%d new fps:%d\n",
+				notification->notif_data.old_fps,
+				notification->notif_data.new_fps);
+		break;
+	default:
+		NVT_LOG("notification serviced :%d\n",
+				notification->notif_type);
+		break;
+	}
+}
+
+#elif defined(_MSM_DRM_NOTIFY_H_)
+static int nvt_drm_notifier_callback(struct notifier_block *self, unsigned long event, void *data)
+{
+	struct msm_drm_notifier *evdata = data;
+	int *blank;
+	struct nvt_spi_data_t *ts =
+		container_of(self, struct nvt_spi_data_t, drm_notif);
+
+	if (!evdata || (evdata->id != 0))
+		return 0;
+
+	if (evdata->data && ts) {
+		blank = evdata->data;
+		if (event == MSM_DRM_EARLY_EVENT_BLANK) {
+			if (*blank == MSM_DRM_BLANK_POWERDOWN) {
+				NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+				nvt_spi_suspend(&ts->client->dev);
+			}
+		} else if (event == MSM_DRM_EVENT_BLANK) {
+			if (*blank == MSM_DRM_BLANK_UNBLANK) {
+				NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+				nvt_ts_resume(&ts->client->dev);
+			}
+		}
+	}
+
+	return 0;
+}
+#else
+static int nvt_fb_notifier_callback(struct notifier_block *self, unsigned long event, void *data)
+{
+	struct fb_event *evdata = data;
+	int *blank;
+	struct nvt_spi_data_t *ts =
+		container_of(self, struct nvt_spi_data_t, fb_notif);
+
+	if (evdata && evdata->data && event == FB_EARLY_EVENT_BLANK) {
+		blank = evdata->data;
+		if (*blank == FB_BLANK_POWERDOWN) {
+			NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+			nvt_spi_suspend(&ts->client->dev);
+		}
+	} else if (evdata && evdata->data && event == FB_EVENT_BLANK) {
+		blank = evdata->data;
+		if (*blank == FB_BLANK_UNBLANK) {
+			NVT_LOG("event=%lu, *blank=%d\n", event, *blank);
+			nvt_ts_resume(&ts->client->dev);
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static const struct spi_device_id nvt_spi_id[] = {
+	{ NVT_SPI_NAME, 0 },
+	{ }
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id nvt_spi_match_table[] = {
+	{ .compatible = "novatek,NVT-ts",},
+	{ },
+};
+#endif
+
+static struct spi_driver nvt_spi_driver = {
+	.probe		= nvt_spi_probe,
+	.remove		= nvt_spi_remove,
+	.shutdown	= nvt_spi_shutdown,
+	.id_table	= nvt_spi_id,
+	.driver = {
+		.name	= NVT_SPI_NAME,
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_OF
+		.of_match_table = nvt_spi_match_table,
+#endif
+	},
+};
+
+/*
+ ******************************************************
+ * Description:
+ *	Driver Install function.
+ *
+ * return:
+ *	Executive Outcomes. 0---succeed. not 0---failed.
+ *******************************************************
+ */
+static int32_t __init nvt_spi_driver_init(void)
+{
+	int32_t ret = 0;
+
+	NVT_LOG("start\n");
+
+	//---add spi driver---
+	ret = spi_register_driver(&nvt_spi_driver);
+	if (ret) {
+		NVT_ERR("failed to add spi driver");
+		goto err_driver;
+	}
+
+	NVT_LOG("finished\n");
+
+err_driver:
+	return ret;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Driver uninstall function.
+ *
+ * return:
+ *	n.a.
+ *******************************************************
+ */
+static void __exit nvt_spi_driver_exit(void)
+{
+	spi_unregister_driver(&nvt_spi_driver);
+}
+
+module_init(nvt_spi_driver_init);
+module_exit(nvt_spi_driver_exit);
+
+MODULE_DESCRIPTION("Novatek Touchscreen Driver");
+MODULE_LICENSE("GPL v2");
+
+#endif

nt36xxx/nt36xxx.h

@@ -0,0 +1,386 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2010 - 2018 Novatek, Inc.
+ *
+ * $Revision: 47247 $
+ * $Date: 2019-07-10 10:41:36 +0800 (Wed, 10 Jul 2019) $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+#ifndef _LINUX_NVT_TOUCH_H
+#define _LINUX_NVT_TOUCH_H
+
+#if !defined(NVT_NT36XXX_SPI) /* NT36XXX I2C */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/uaccess.h>
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#include "nt36xxx_mem_map.h"
+
+#define NVT_DEBUG 1
+
+//---GPIO number---
+#define NVTTOUCH_RST_PIN 980
+#define NVTTOUCH_INT_PIN 943
+
+
+//---INT trigger mode---
+//#define IRQ_TYPE_EDGE_RISING 1
+//#define IRQ_TYPE_EDGE_FALLING 2
+#define INT_TRIGGER_TYPE IRQ_TYPE_EDGE_RISING
+
+
+//---I2C driver info.---
+#define NVT_I2C_NAME "NVT-ts"
+#define I2C_BLDR_Address 0x01
+#define I2C_FW_Address 0x01
+#define I2C_HW_Address 0x62
+
+#if NVT_DEBUG
+#define NVT_LOG(fmt, args...)    pr_err("[%s] %s %d: " fmt, NVT_I2C_NAME, __func__, __LINE__, ##args)
+#else
+#define NVT_LOG(fmt, args...)    pr_info("[%s] %s %d: " fmt, NVT_I2C_NAME, __func__, __LINE__, ##args)
+#endif
+#define NVT_ERR(fmt, args...)    pr_err("[%s] %s %d: " fmt, NVT_I2C_NAME, __func__, __LINE__, ##args)
+
+//---Input device info.---
+#define NVT_TS_NAME "NVTCapacitiveTouchScreen"
+
+
+//---Touch info.---
+#define TOUCH_DEFAULT_MAX_WIDTH 1080
+#define TOUCH_DEFAULT_MAX_HEIGHT 2408
+#define TOUCH_MAX_FINGER_NUM 10
+#define TOUCH_KEY_NUM 0
+#if TOUCH_KEY_NUM > 0
+extern const uint16_t touch_key_array[TOUCH_KEY_NUM];
+#endif
+#define TOUCH_FORCE_NUM 1000
+
+/* Enable only when module have tp reset pin and connected to host */
+#define NVT_TOUCH_SUPPORT_HW_RST 1
+
+//---Customerized func.---
+#define NVT_TOUCH_PROC 1
+#define NVT_TOUCH_EXT_PROC 1
+#define NVT_TOUCH_MP 1
+#define MT_PROTOCOL_B 1
+#define WAKEUP_GESTURE 0
+#if WAKEUP_GESTURE
+extern const uint16_t gesture_key_array[];
+#endif
+#define BOOT_UPDATE_FIRMWARE 1
+#define BOOT_UPDATE_FIRMWARE_NAME "novatek_ts_fw.bin"
+
+//---ESD Protect.---
+#define NVT_TOUCH_ESD_PROTECT 0
+#define NVT_TOUCH_ESD_CHECK_PERIOD 1500	/* ms */
+
+struct nvt_ts_data {
+	struct i2c_client *client;
+	struct input_dev *input_dev;
+	struct delayed_work nvt_fwu_work;
+	uint16_t addr;
+	int8_t phys[32];
+	const struct i2c_device_id *id;
+#if defined(CONFIG_DRM)
+	struct notifier_block drm_panel_notif;
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	struct notifier_block drm_notif;
+#else
+	struct notifier_block fb_notif;
+
+#endif
+	uint8_t fw_ver;
+	uint8_t x_num;
+	uint8_t y_num;
+	uint16_t abs_x_max;
+	uint16_t abs_y_max;
+	uint8_t max_touch_num;
+	uint8_t max_button_num;
+	uint32_t int_trigger_type;
+	int32_t irq_gpio;
+	uint32_t irq_flags;
+	int32_t reset_gpio;
+	uint32_t reset_flags;
+	struct mutex lock;
+	const struct nvt_ts_mem_map *mmap;
+	uint8_t carrier_system;
+	uint16_t nvt_pid;
+	uint8_t xbuf[1025];
+	struct mutex xbuf_lock;
+	bool irq_enabled;
+	void *notifier_cookie;
+};
+
+#if NVT_TOUCH_PROC
+struct nvt_flash_data{
+	rwlock_t lock;
+	struct i2c_client *client;
+};
+#endif
+
+typedef enum {
+	RESET_STATE_INIT = 0xA0,// IC reset
+	RESET_STATE_REK,        // ReK baseline
+	RESET_STATE_REK_FINISH, // baseline is ready
+	RESET_STATE_NORMAL_RUN, // normal run
+	RESET_STATE_MAX  = 0xAF
+} RST_COMPLETE_STATE;
+
+typedef enum {
+    EVENT_MAP_HOST_CMD                      = 0x50,
+    EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE   = 0x51,
+    EVENT_MAP_RESET_COMPLETE                = 0x60,
+    EVENT_MAP_FWINFO                        = 0x78,
+    EVENT_MAP_PROJECTID                     = 0x9A,
+} I2C_EVENT_MAP;
+
+//---extern structures---
+extern struct nvt_ts_data *ts;
+
+//---extern functions---
+extern int32_t CTP_I2C_READ(struct i2c_client *client, uint16_t address, uint8_t *buf, uint16_t len);
+extern int32_t CTP_I2C_WRITE(struct i2c_client *client, uint16_t address, uint8_t *buf, uint16_t len);
+extern void nvt_bootloader_reset(void);
+extern void nvt_sw_reset_idle(void);
+extern int32_t nvt_check_fw_reset_state(RST_COMPLETE_STATE check_reset_state);
+extern int32_t nvt_get_fw_info(void);
+extern int32_t nvt_clear_fw_status(void);
+extern int32_t nvt_check_fw_status(void);
+extern int32_t nvt_set_page(uint16_t i2c_addr, uint32_t addr);
+#if NVT_TOUCH_ESD_PROTECT
+extern void nvt_esd_check_enable(uint8_t enable);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+extern void nvt_stop_crc_reboot(void);
+
+#else  /* NT36XXX_SPI */
+
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/of.h>
+#include <linux/spi/spi.h>
+#include <linux/uaccess.h>
+
+#include "nt36xxx_mem_map.h"
+
+#define NVT_SPI_DEBUG 0
+
+//---GPIO number---
+#define NVTTOUCH_RST_PIN 980
+#define NVTTOUCH_INT_PIN 943
+
+//---INT trigger mode---
+//#define NVT_SPI_IRQ_TYPE_EDGE_RISING 1
+//#define NVT_SPI_IRQ_TYPE_EDGE_FALLING 2
+#define NVT_SPI_INT_TRIGGER_TYPE IRQ_TYPE_EDGE_RISING
+
+//---SPI driver info.---
+#define NVT_SPI_NAME "NVT-SPI"
+
+#if NVT_SPI_DEBUG
+#define NVT_LOG(fmt, args...) pr_err("[%s] %s %d: " fmt, "NVT-SPI", __func__, __LINE__, ##args)
+#else
+#define NVT_LOG(fmt, args...) pr_debug("[%s] %s %d: " fmt, "NVT-SPI", __func__, __LINE__, ##args)
+#endif
+#define NVT_ERR(fmt, args...) pr_err("[%s] %s %d: " fmt, "NVT-SPI", __func__, __LINE__, ##args)
+
+//---Input device info.---
+#define NVT_SPI_TS_NAME "NVTCapacitiveTouchScreen"
+#define NVT_SPI_PEN_NAME "NVTCapacitivePen"
+
+//---Touch info.---
+#define NVT_SPI_TOUCH_DEFAULT_MAX_WIDTH 1080
+#define NVT_SPI_TOUCH_DEFAULT_MAX_HEIGHT 2400
+#define NVT_SPI_TOUCH_MAX_FINGER_NUM 10
+#define NVT_SPI_TOUCH_KEY_NUM 0
+#if NVT_SPI_TOUCH_KEY_NUM > 0
+extern const uint16_t nvt_spi_touch_key_array[NVT_SPI_TOUCH_KEY_NUM];
+#endif
+#define NVT_SPI_TOUCH_FORCE_NUM 1000
+//---for Pen---
+#define NVT_SPI_PEN_PRESSURE_MAX (4095)
+#define NVT_SPI_PEN_DISTANCE_MAX (1)
+#define NVT_SPI_PEN_TILT_MIN (-60)
+#define NVT_SPI_PEN_TILT_MAX (60)
+
+/* Enable only when module have tp reset pin and connected to host */
+#define NVT_SPI_TOUCH_SUPPORT_HW_RST 0
+
+//---Customerized func.---
+#define NVT_SPI_TOUCH_PROC 1
+#define NVT_SPI_TOUCH_EXT_PROC 1
+#define NVT_SPI_TOUCH_MP 0
+#define NVT_SPI_MT_PROTOCOL_B 1
+#define NVT_SPI_WAKEUP_GESTURE 0
+
+#if NVT_SPI_WAKEUP_GESTURE
+extern const uint16_t nvt_spi_gesture_key_array[];
+#endif
+#define NVT_SPI_BOOT_UPDATE_FIRMWARE 1
+#define NVT_SPI_BOOT_UPDATE_FIRMWARE_NAME "novatek_spi_fw.bin"
+#define NVT_SPI_MP_UPDATE_FIRMWARE_NAME   "novatek_ts_mp.bin"
+#define NVT_SPI_POINT_DATA_CHECKSUM 1
+#define NVT_SPI_POINT_DATA_CHECKSUM_LEN 65
+
+//---ESD Protect.---
+#define NVT_SPI_TOUCH_ESD_PROTECT 0
+#define NVT_SPI_TOUCH_ESD_CHECK_PERIOD 1500	/* ms */
+#define NVT_SPI_TOUCH_WDT_RECOVERY 1
+
+#define NVT_SPI_CHECK_PEN_DATA_CHECKSUM 0
+
+struct nvt_spi_data_t {
+	struct spi_device *client;
+	struct input_dev *input_dev;
+	struct delayed_work nvt_fwu_work;
+	uint16_t addr;
+	int8_t phys[32];
+
+#if defined(CONFIG_DRM)
+	struct notifier_block drm_panel_notif;
+#elif defined(_MSM_DRM_NOTIFY_H_)
+	struct notifier_block drm_notif;
+#else
+	struct notifier_block fb_notif;
+#endif
+
+	uint8_t fw_ver;
+	uint8_t x_num;
+	uint8_t y_num;
+	uint16_t abs_x_max;
+	uint16_t abs_y_max;
+	uint8_t max_touch_num;
+	uint8_t max_button_num;
+	uint32_t int_trigger_type;
+	int32_t irq_gpio;
+	uint32_t irq_flags;
+	int32_t reset_gpio;
+	uint32_t reset_flags;
+	struct mutex lock;
+	const struct nvt_spi_mem_map *mmap;
+	uint8_t hw_crc;
+	uint16_t nvt_pid;
+	uint8_t *rbuf;
+	uint8_t *xbuf;
+	struct mutex xbuf_lock;
+	bool irq_enabled;
+	bool pen_support;
+	bool wgp_stylus;
+	uint8_t x_gang_num;
+	uint8_t y_gang_num;
+	struct input_dev *pen_input_dev;
+	int8_t pen_phys[32];
+
+	void *notifier_cookie;
+	const char *touch_environment;
+
+#ifdef CONFIG_NOVATEK_SPI_TRUSTED_TOUCH
+	struct trusted_touch_vm_info *vm_info;
+	struct mutex fts_clk_io_ctrl_mutex;
+	struct completion trusted_touch_powerdown;
+	struct clk *core_clk;
+	struct clk *iface_clk;
+	atomic_t trusted_touch_initialized;
+	atomic_t trusted_touch_enabled;
+	atomic_t trusted_touch_underway;
+	atomic_t trusted_touch_event;
+	atomic_t trusted_touch_abort_status;
+	atomic_t delayed_vm_probe_pending;
+	atomic_t trusted_touch_mode;
+#endif
+};
+
+#if NVT_SPI_TOUCH_PROC
+struct nvt_spi_flash_data {
+	rwlock_t lock;
+};
+#endif
+
+enum NVT_SPI_RST_COMPLETE_STATE {
+	NVT_SPI_RESET_STATE_INIT = 0xA0,// IC reset
+	NVT_SPI_RESET_STATE_REK,        // ReK baseline
+	NVT_SPI_RESET_STATE_REK_FINISH, // baseline is ready
+	NVT_SPI_RESET_STATE_NORMAL_RUN, // normal run
+	NVT_SPI_RESET_STATE_MAX  = 0xAF
+};
+
+enum NVT_SPI_EVENT_MAP {
+	NVT_SPI_EVENT_MAP_HOST_CMD                    = 0x50,
+	NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE = 0x51,
+	NVT_SPI_EVENT_MAP_RESET_COMPLETE              = 0x60,
+	NVT_SPI_EVENT_MAP_FWINFO                      = 0x78,
+	NVT_SPI_EVENT_MAP_PROJECTID                   = 0x9A,
+};
+
+//---SPI READ/WRITE---
+#define NVT_SPI_WRITE_MASK(a) (a | 0x80)
+#define NVT_SPI_READ_MASK(a)  (a & 0x7F)
+
+#define NVT_SPI_DUMMY_BYTES  (1)
+#define NVT_SPI_TRANSFER_LEN (63*1024)
+#define NVT_SPI_READ_LEN     (2*1024)
+#define NVT_SPI_XBUF_LEN     (NVT_SPI_TRANSFER_LEN+1+NVT_SPI_DUMMY_BYTES)
+
+enum NVT_SPI_RW {
+	NVT_SPI_WRITE = 0,
+	NVT_SPI_READ  = 1
+};
+
+//---extern structures---
+extern struct nvt_spi_data_t *nvt_spi_data;
+
+//---extern functions---
+int32_t nvt_spi_read(uint8_t *buf, uint16_t len);
+int32_t nvt_spi_write(uint8_t *buf, uint16_t len);
+void nvt_spi_bootloader_reset(void);
+void nvt_spi_eng_reset(void);
+void nvt_spi_sw_reset(void);
+void nvt_spi_sw_reset_idle(void);
+void nvt_spi_boot_ready(void);
+void nvt_spi_bld_crc_enable(void);
+void nvt_spi_fw_crc_enable(void);
+void nvt_spi_tx_auto_copy_mode(void);
+int32_t nvt_spi_update_firmware(char *firmware_name);
+void nvt_spi_update_firmware_work(struct work_struct *work);
+int32_t nvt_spi_check_fw_reset_state(enum NVT_SPI_RST_COMPLETE_STATE reset_state);
+int32_t nvt_spi_get_fw_info(void);
+int32_t nvt_spi_clear_fw_status(void);
+int32_t nvt_spi_check_fw_status(void);
+int32_t nvt_spi_check_spi_dma_tx_info(void);
+int32_t nvt_spi_set_page(uint32_t addr);
+int32_t nvt_spi_write_addr(uint32_t addr, uint8_t data);
+
+#if NVT_SPI_TOUCH_EXT_PROC
+int32_t nvt_spi_extra_proc_init(void);
+void nvt_spi_extra_proc_deinit(void);
+#endif
+
+#if NVT_SPI_TOUCH_MP
+extern int32_t nvt_spi_mp_proc_init(void);
+extern void nvt_spi_mp_proc_deinit(void);
+#endif
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+extern void nvt_spi_esd_check_enable(uint8_t enable);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+
+#endif
+
+#endif /* _LINUX_NVT_TOUCH_H */

nt36xxx/nt36xxx_ext_proc.c

@@ -0,0 +1,1537 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2010 - 2018 Novatek, Inc.
+ *
+ * $Revision: 47247 $
+ * $Date: 2019-07-10 10:41:36 +0800 (Wed, 10 Jul 2019) $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+
+#if !defined(NVT_NT36XXX_SPI) /* TOUCHSCREEN_NT36XXX I2C */
+
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+
+#include "nt36xxx.h"
+
+#if NVT_TOUCH_EXT_PROC
+#define NVT_FW_VERSION "nvt_fw_version"
+#define NVT_BASELINE "nvt_baseline"
+#define NVT_RAW "nvt_raw"
+#define NVT_DIFF "nvt_diff"
+
+#define BUS_TRANSFER_LENGTH  64
+
+#define NORMAL_MODE 0x00
+#define TEST_MODE_1 0x21
+#define TEST_MODE_2 0x22
+#define HANDSHAKING_HOST_READY 0xBB
+
+#define XDATA_SECTOR_SIZE   256
+
+static uint8_t xdata_tmp[2048] = {0};
+static int32_t xdata[2048] = {0};
+
+static struct proc_dir_entry *NVT_proc_fw_version_entry;
+static struct proc_dir_entry *NVT_proc_baseline_entry;
+static struct proc_dir_entry *NVT_proc_raw_entry;
+static struct proc_dir_entry *NVT_proc_diff_entry;
+
+/*******************************************************
+Description:
+	Novatek touchscreen change mode function.
+
+return:
+	n.a.
+*******************************************************/
+void nvt_change_mode(uint8_t mode)
+{
+	uint8_t buf[8] = {0};
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HOST_CMD);
+
+	//---set mode---
+	buf[0] = EVENT_MAP_HOST_CMD;
+	buf[1] = mode;
+	CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 2);
+
+	if (mode == NORMAL_MODE) {
+		buf[0] = EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = HANDSHAKING_HOST_READY;
+		CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 2);
+		msleep(20);
+	}
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen get firmware pipe function.
+
+return:
+	Executive outcomes. 0---pipe 0. 1---pipe 1.
+*******************************************************/
+uint8_t nvt_get_fw_pipe(void)
+{
+	uint8_t buf[8]= {0};
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE);
+
+	//---read fw status---
+	buf[0] = EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+	buf[1] = 0x00;
+	CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 2);
+
+	//NVT_LOG("FW pipe=%d, buf[1]=0x%02X\n", (buf[1]&0x01), buf[1]);
+
+	return (buf[1] & 0x01);
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen read meta data function.
+
+return:
+	n.a.
+*******************************************************/
+void nvt_read_mdata(uint32_t xdata_addr, uint32_t xdata_btn_addr)
+{
+	int32_t i = 0;
+	int32_t j = 0;
+	int32_t k = 0;
+	uint8_t buf[BUS_TRANSFER_LENGTH + 1] = {0};
+	uint32_t head_addr = 0;
+	int32_t dummy_len = 0;
+	int32_t data_len = 0;
+	int32_t residual_len = 0;
+
+	//---set xdata sector address & length---
+	head_addr = xdata_addr - (xdata_addr % XDATA_SECTOR_SIZE);
+	dummy_len = xdata_addr - head_addr;
+	data_len = ts->x_num * ts->y_num * 2;
+	residual_len = (head_addr + dummy_len + data_len) % XDATA_SECTOR_SIZE;
+
+	//printk("head_addr=0x%05X, dummy_len=0x%05X, data_len=0x%05X, residual_len=0x%05X\n", head_addr, dummy_len, data_len, residual_len);
+
+	//read xdata : step 1
+	for (i = 0; i < ((dummy_len + data_len) / XDATA_SECTOR_SIZE); i++) {
+		//---change xdata index---
+		nvt_set_page(I2C_FW_Address, head_addr + XDATA_SECTOR_SIZE * i);
+
+		//---read xdata by BUS_TRANSFER_LENGTH
+		for (j = 0; j < (XDATA_SECTOR_SIZE / BUS_TRANSFER_LENGTH); j++) {
+			//---read data---
+			buf[0] = BUS_TRANSFER_LENGTH * j;
+			CTP_I2C_READ(ts->client, I2C_FW_Address, buf, BUS_TRANSFER_LENGTH + 1);
+
+			//---copy buf to xdata_tmp---
+			for (k = 0; k < BUS_TRANSFER_LENGTH; k++) {
+				xdata_tmp[XDATA_SECTOR_SIZE * i + BUS_TRANSFER_LENGTH * j + k] = buf[k + 1];
+				//printk("0x%02X, 0x%04X\n", buf[k+1], (XDATA_SECTOR_SIZE*i + BUS_TRANSFER_LENGTH*j + k));
+			}
+		}
+		//printk("addr=0x%05X\n", (head_addr+XDATA_SECTOR_SIZE*i));
+	}
+
+	//read xdata : step2
+	if (residual_len != 0) {
+		//---change xdata index---
+		nvt_set_page(I2C_FW_Address, xdata_addr + data_len - residual_len);
+
+		//---read xdata by BUS_TRANSFER_LENGTH
+		for (j = 0; j < (residual_len / BUS_TRANSFER_LENGTH + 1); j++) {
+			//---read data---
+			buf[0] = BUS_TRANSFER_LENGTH * j;
+			CTP_I2C_READ(ts->client, I2C_FW_Address, buf, BUS_TRANSFER_LENGTH + 1);
+
+			//---copy buf to xdata_tmp---
+			for (k = 0; k < BUS_TRANSFER_LENGTH; k++) {
+				xdata_tmp[(dummy_len + data_len - residual_len) + BUS_TRANSFER_LENGTH * j + k] = buf[k + 1];
+				//printk("0x%02X, 0x%04x\n", buf[k+1], ((dummy_len+data_len-residual_len) + BUS_TRANSFER_LENGTH*j + k));
+			}
+		}
+		//printk("addr=0x%05X\n", (xdata_addr+data_len-residual_len));
+	}
+
+	//---remove dummy data and 2bytes-to-1data---
+	for (i = 0; i < (data_len / 2); i++) {
+		xdata[i] = (int16_t)(xdata_tmp[dummy_len + i * 2] + 256 * xdata_tmp[dummy_len + i * 2 + 1]);
+	}
+
+#if TOUCH_KEY_NUM > 0
+	//read button xdata : step3
+	//---change xdata index---
+	nvt_set_page(I2C_FW_Address, xdata_btn_addr);
+
+	//---read data---
+	buf[0] = (xdata_btn_addr & 0xFF);
+	CTP_I2C_READ(ts->client, I2C_FW_Address, buf, (TOUCH_KEY_NUM * 2 + 1));
+
+	//---2bytes-to-1data---
+	for (i = 0; i < TOUCH_KEY_NUM; i++) {
+		xdata[ts->x_num * ts->y_num + i] = (int16_t)(buf[1 + i * 2] + 256 * buf[1 + i * 2 + 1]);
+	}
+#endif
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR);
+}
+
+/*******************************************************
+Description:
+    Novatek touchscreen get meta data function.
+
+return:
+    n.a.
+*******************************************************/
+void nvt_get_mdata(int32_t *buf, uint8_t *m_x_num, uint8_t *m_y_num)
+{
+    *m_x_num = ts->x_num;
+    *m_y_num = ts->y_num;
+    memcpy(buf, xdata, ((ts->x_num * ts->y_num + TOUCH_KEY_NUM) * sizeof(int32_t)));
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen firmware version show function.
+
+return:
+	Executive outcomes. 0---succeed.
+*******************************************************/
+static int32_t c_fw_version_show(struct seq_file *m, void *v)
+{
+	seq_printf(m, "fw_ver=%d, x_num=%d, y_num=%d, button_num=%d\n", ts->fw_ver, ts->x_num, ts->y_num, ts->max_button_num);
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen xdata sequence print show
+	function.
+
+return:
+	Executive outcomes. 0---succeed.
+*******************************************************/
+static int32_t c_show(struct seq_file *m, void *v)
+{
+	int32_t i = 0;
+	int32_t j = 0;
+
+	for (i = 0; i < ts->y_num; i++) {
+		for (j = 0; j < ts->x_num; j++) {
+			seq_printf(m, "%5d, ", xdata[i * ts->x_num + j]);
+		}
+		seq_puts(m, "\n");
+	}
+
+#if TOUCH_KEY_NUM > 0
+	for (i = 0; i < TOUCH_KEY_NUM; i++) {
+		seq_printf(m, "%5d, ", xdata[ts->x_num * ts->y_num + i]);
+	}
+	seq_puts(m, "\n");
+#endif
+
+	seq_printf(m, "\n\n");
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen xdata sequence print start
+	function.
+
+return:
+	Executive outcomes. 1---call next function.
+	NULL---not call next function and sequence loop
+	stop.
+*******************************************************/
+static void *c_start(struct seq_file *m, loff_t *pos)
+{
+	return *pos < 1 ? (void *)1 : NULL;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen xdata sequence print next
+	function.
+
+return:
+	Executive outcomes. NULL---no next and call sequence
+	stop function.
+*******************************************************/
+static void *c_next(struct seq_file *m, void *v, loff_t *pos)
+{
+	++*pos;
+	return NULL;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen xdata sequence print stop
+	function.
+
+return:
+	n.a.
+*******************************************************/
+static void c_stop(struct seq_file *m, void *v)
+{
+	return;
+}
+
+const struct seq_operations nvt_fw_version_seq_ops = {
+	.start  = c_start,
+	.next   = c_next,
+	.stop   = c_stop,
+	.show   = c_fw_version_show
+};
+
+const struct seq_operations nvt_seq_ops = {
+	.start  = c_start,
+	.next   = c_next,
+	.stop   = c_stop,
+	.show   = c_show
+};
+
+/*******************************************************
+Description:
+	Novatek touchscreen /proc/nvt_fw_version open
+	function.
+
+return:
+	n.a.
+*******************************************************/
+static int32_t nvt_fw_version_open(struct inode *inode, struct file *file)
+{
+	if (mutex_lock_interruptible(&ts->lock)) {
+		return -ERESTARTSYS;
+	}
+
+	NVT_LOG("++\n");
+
+#if NVT_TOUCH_ESD_PROTECT
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	if (nvt_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_fw_version_seq_ops);
+}
+
+static const struct proc_ops nvt_fw_version_fops = {
+	.proc_open = nvt_fw_version_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*******************************************************
+Description:
+	Novatek touchscreen /proc/nvt_baseline open function.
+
+return:
+	Executive outcomes. 0---succeed.
+*******************************************************/
+static int32_t nvt_baseline_open(struct inode *inode, struct file *file)
+{
+	if (mutex_lock_interruptible(&ts->lock)) {
+		return -ERESTARTSYS;
+	}
+
+	NVT_LOG("++\n");
+
+#if NVT_TOUCH_ESD_PROTECT
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	if (nvt_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_change_mode(TEST_MODE_2);
+
+	if (nvt_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_read_mdata(ts->mmap->BASELINE_ADDR, ts->mmap->BASELINE_BTN_ADDR);
+
+	nvt_change_mode(NORMAL_MODE);
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_seq_ops);
+}
+
+static const struct proc_ops nvt_baseline_fops = {
+	.proc_open = nvt_baseline_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*******************************************************
+Description:
+	Novatek touchscreen /proc/nvt_raw open function.
+
+return:
+	Executive outcomes. 0---succeed.
+*******************************************************/
+static int32_t nvt_raw_open(struct inode *inode, struct file *file)
+{
+	if (mutex_lock_interruptible(&ts->lock)) {
+		return -ERESTARTSYS;
+	}
+
+	NVT_LOG("++\n");
+
+#if NVT_TOUCH_ESD_PROTECT
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	if (nvt_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_change_mode(TEST_MODE_2);
+
+	if (nvt_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_get_fw_pipe() == 0)
+		nvt_read_mdata(ts->mmap->RAW_PIPE0_ADDR, ts->mmap->RAW_BTN_PIPE0_ADDR);
+	else
+		nvt_read_mdata(ts->mmap->RAW_PIPE1_ADDR, ts->mmap->RAW_BTN_PIPE1_ADDR);
+
+	nvt_change_mode(NORMAL_MODE);
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_seq_ops);
+}
+
+static const struct proc_ops nvt_raw_fops = {
+	.proc_open = nvt_raw_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*******************************************************
+Description:
+	Novatek touchscreen /proc/nvt_diff open function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+static int32_t nvt_diff_open(struct inode *inode, struct file *file)
+{
+	if (mutex_lock_interruptible(&ts->lock)) {
+		return -ERESTARTSYS;
+	}
+
+	NVT_LOG("++\n");
+
+#if NVT_TOUCH_ESD_PROTECT
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	if (nvt_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_change_mode(TEST_MODE_2);
+
+	if (nvt_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_get_fw_pipe() == 0)
+		nvt_read_mdata(ts->mmap->DIFF_PIPE0_ADDR, ts->mmap->DIFF_BTN_PIPE0_ADDR);
+	else
+		nvt_read_mdata(ts->mmap->DIFF_PIPE1_ADDR, ts->mmap->DIFF_BTN_PIPE1_ADDR);
+
+	nvt_change_mode(NORMAL_MODE);
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_seq_ops);
+}
+
+static const struct proc_ops nvt_diff_fops = {
+	.proc_open = nvt_diff_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*******************************************************
+Description:
+	Novatek touchscreen extra function proc. file node
+	initial function.
+
+return:
+	Executive outcomes. 0---succeed. -12---failed.
+*******************************************************/
+int32_t nvt_extra_proc_init(void)
+{
+	NVT_proc_fw_version_entry = proc_create(NVT_FW_VERSION, 0444, NULL,&nvt_fw_version_fops);
+	if (NVT_proc_fw_version_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_FW_VERSION);
+		return -ENOMEM;
+	} else {
+		NVT_LOG("create proc/%s Succeeded!\n", NVT_FW_VERSION);
+	}
+
+	NVT_proc_baseline_entry = proc_create(NVT_BASELINE, 0444, NULL,&nvt_baseline_fops);
+	if (NVT_proc_baseline_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_BASELINE);
+		return -ENOMEM;
+	} else {
+		NVT_LOG("create proc/%s Succeeded!\n", NVT_BASELINE);
+	}
+
+	NVT_proc_raw_entry = proc_create(NVT_RAW, 0444, NULL,&nvt_raw_fops);
+	if (NVT_proc_raw_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_RAW);
+		return -ENOMEM;
+	} else {
+		NVT_LOG("create proc/%s Succeeded!\n", NVT_RAW);
+	}
+
+	NVT_proc_diff_entry = proc_create(NVT_DIFF, 0444, NULL,&nvt_diff_fops);
+	if (NVT_proc_diff_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_DIFF);
+		return -ENOMEM;
+	} else {
+		NVT_LOG("create proc/%s Succeeded!\n", NVT_DIFF);
+	}
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen extra function proc. file node
+	deinitial function.
+
+return:
+	n.a.
+*******************************************************/
+void nvt_extra_proc_deinit(void)
+{
+	if (NVT_proc_fw_version_entry != NULL) {
+		remove_proc_entry(NVT_FW_VERSION, NULL);
+		NVT_proc_fw_version_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_FW_VERSION);
+	}
+
+	if (NVT_proc_baseline_entry != NULL) {
+		remove_proc_entry(NVT_BASELINE, NULL);
+		NVT_proc_baseline_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_BASELINE);
+	}
+
+	if (NVT_proc_raw_entry != NULL) {
+		remove_proc_entry(NVT_RAW, NULL);
+		NVT_proc_raw_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_RAW);
+	}
+
+	if (NVT_proc_diff_entry != NULL) {
+		remove_proc_entry(NVT_DIFF, NULL);
+		NVT_proc_diff_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_DIFF);
+	}
+}
+#endif
+
+#else  /* NT36XXX_SPI */
+
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+
+#include "nt36xxx.h"
+
+#if NVT_SPI_TOUCH_EXT_PROC
+#define NVT_SPI_FW_VERSION "nvt_fw_version"
+#define NVT_SPI_BASELINE "nvt_baseline"
+#define NVT_SPI_RAW "nvt_raw"
+#define NVT_SPI_DIFF "nvt_diff"
+#define NVT_SPI_PEN_DIFF "nvt_pen_diff"
+
+#define NVT_SPI_BUS_TRANSFER_LENGTH 256
+
+#define NVT_SPI_NORMAL_MODE 0x00
+#define NVT_SPI_TEST_MODE_2 0x22
+#define NVT_SPI_HANDSHAKING_HOST_READY 0xBB
+
+#define NVT_SPI_XDATA_SECTOR_SIZE 256
+
+static uint8_t nvt_spi_xdata_tmp[5000] = {0};
+static int32_t nvt_spi_xdata[2500] = {0};
+static int32_t nvt_spi_xdata_pen_tip_x[256] = {0};
+static int32_t nvt_spi_xdata_pen_tip_y[256] = {0};
+static int32_t nvt_spi_xdata_pen_ring_x[256] = {0};
+static int32_t nvt_spi_xdata_pen_ring_y[256] = {0};
+
+static struct proc_dir_entry *nvt_spi_proc_fw_version_entry;
+static struct proc_dir_entry *nvt_spi_proc_baseline_entry;
+static struct proc_dir_entry *nvt_spi_proc_raw_entry;
+static struct proc_dir_entry *nvt_spi_proc_diff_entry;
+static struct proc_dir_entry *nvt_spi_proc_pen_diff_entry;
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen change mode function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_change_mode(uint8_t mode)
+{
+	uint8_t buf[8] = {0};
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR | NVT_SPI_EVENT_MAP_HOST_CMD);
+
+	//---set mode---
+	buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD;
+	buf[1] = mode;
+	nvt_spi_write(buf, 2);
+
+	if (mode == NVT_SPI_NORMAL_MODE) {
+		usleep_range(20000, 21000);
+		buf[0] = NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = NVT_SPI_HANDSHAKING_HOST_READY;
+		nvt_spi_write(buf, 2);
+		usleep_range(20000, 21000);
+	}
+}
+
+static int32_t nvt_set_pen_inband_mode_1_spi(uint8_t freq_idx, uint8_t x_term)
+{
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 5;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR | NVT_SPI_EVENT_MAP_HOST_CMD);
+
+	//---set mode---
+	buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD;
+	buf[1] = 0xC1;
+	buf[2] = 0x02;
+	buf[3] = freq_idx;
+	buf[4] = x_term;
+	nvt_spi_write(buf, 5);
+
+	for (i = 0; i < retry; i++) {
+		buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD;
+		buf[1] = 0xFF;
+		nvt_spi_read(buf, 2);
+
+		if (buf[1] == 0x00)
+			break;
+
+		usleep_range(10000, 11000);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("failed, i=%d, buf[1]=0x%02X\n", i, buf[1]);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int32_t nvt_spi_set_pen_normal_mode(void)
+{
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 5;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR | NVT_SPI_EVENT_MAP_HOST_CMD);
+
+	//---set mode---
+	buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD;
+	buf[1] = 0xC1;
+	buf[2] = 0x04;
+	nvt_spi_write(buf, 3);
+
+	for (i = 0; i < retry; i++) {
+		buf[0] = NVT_SPI_EVENT_MAP_HOST_CMD;
+		buf[1] = 0xFF;
+		nvt_spi_read(buf, 2);
+
+		if (buf[1] == 0x00)
+			break;
+
+		usleep_range(10000, 11000);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("failed, i=%d, buf[1]=0x%02X\n", i, buf[1]);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen get firmware pipe function.
+ *
+ * return:
+ *	Executive outcomes. 0---pipe 0. 1---pipe 1.
+ ******************************************************
+ */
+uint8_t nvt_spi_get_fw_pipe(void)
+{
+	uint32_t addr;
+	uint8_t buf[8] = {0};
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata index to EVENT BUF ADDR---
+	addr = ts->mmap->EVENT_BUF_ADDR;
+	nvt_spi_set_page(addr | NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE);
+
+	//---read fw status---
+	buf[0] = NVT_SPI_EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+	buf[1] = 0x00;
+	nvt_spi_read(buf, 2);
+
+	//NVT_LOG("FW pipe=%d, buf[1]=0x%02X\n", (buf[1]&0x01), buf[1]);
+
+	return (buf[1] & 0x01);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen read meta data function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void nvt_spi_read_mdata(uint32_t xdata_addr, uint32_t xdata_btn_addr)
+{
+	int32_t i = 0;
+	int32_t j = 0;
+	int32_t k = 0;
+	uint8_t buf[NVT_SPI_BUS_TRANSFER_LENGTH + 2] = {0};
+	uint32_t head_addr = 0;
+	int32_t dummy_len = 0;
+	int32_t data_len = 0;
+	int32_t residual_len = 0;
+	int32_t index, data;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata sector address & length---
+	head_addr = xdata_addr - (xdata_addr % NVT_SPI_XDATA_SECTOR_SIZE);
+	dummy_len = xdata_addr - head_addr;
+	data_len = ts->x_num * ts->y_num * 2;
+	residual_len = (head_addr + dummy_len + data_len) % NVT_SPI_XDATA_SECTOR_SIZE;
+
+	//printk("head_addr=0x%05X, dummy_len=0x%05X, data_len=0x%05X, residual_len=0x%05X\n",
+	//		head_addr, dummy_len, data_len, residual_len);
+
+	//read xdata : step 1
+	for (i = 0; i < ((dummy_len + data_len) / NVT_SPI_XDATA_SECTOR_SIZE); i++) {
+		//---change xdata index---
+		nvt_spi_set_page(head_addr + NVT_SPI_XDATA_SECTOR_SIZE * i);
+
+		//---read xdata by NVT_SPI_BUS_TRANSFER_LENGTH
+		for (j = 0; j < (NVT_SPI_XDATA_SECTOR_SIZE / NVT_SPI_BUS_TRANSFER_LENGTH); j++) {
+			//---read data---
+			buf[0] = NVT_SPI_BUS_TRANSFER_LENGTH * j;
+			nvt_spi_read(buf, NVT_SPI_BUS_TRANSFER_LENGTH + 1);
+
+			//---copy buf to nvt_spi_xdata_tmp---
+			for (k = 0; k < NVT_SPI_BUS_TRANSFER_LENGTH; k++) {
+				index = NVT_SPI_XDATA_SECTOR_SIZE * i;
+				index += NVT_SPI_BUS_TRANSFER_LENGTH * j + k;
+				nvt_spi_xdata_tmp[index] = buf[k + 1];
+				//printk("0x%02X, 0x%04X\n", buf[k+1], index);
+			}
+		}
+		//printk("addr=0x%05X\n", (head_addr+NVT_SPI_XDATA_SECTOR_SIZE*i));
+	}
+
+	//read xdata : step2
+	if (residual_len != 0) {
+		//---change xdata index---
+		nvt_spi_set_page(xdata_addr + data_len - residual_len);
+
+		//---read xdata by NVT_SPI_BUS_TRANSFER_LENGTH
+		for (j = 0; j < (residual_len / NVT_SPI_BUS_TRANSFER_LENGTH + 1); j++) {
+			//---read data---
+			buf[0] = NVT_SPI_BUS_TRANSFER_LENGTH * j;
+			nvt_spi_read(buf, NVT_SPI_BUS_TRANSFER_LENGTH + 1);
+
+			//---copy buf to nvt_spi_xdata_tmp---
+			for (k = 0; k < NVT_SPI_BUS_TRANSFER_LENGTH; k++) {
+				index = (dummy_len + data_len - residual_len);
+				index += NVT_SPI_BUS_TRANSFER_LENGTH * j + k;
+				nvt_spi_xdata_tmp[index] = buf[k + 1];
+				//printk("0x%02X, 0x%04x\n", buf[k+1], index);
+
+			}
+		}
+		//printk("addr=0x%05X\n", (xdata_addr+data_len-residual_len));
+	}
+
+	//---remove dummy data and 2bytes-to-1data---
+	for (i = 0; i < (data_len / 2); i++) {
+		data = nvt_spi_xdata_tmp[dummy_len + i * 2];
+		data += 256 * nvt_spi_xdata_tmp[dummy_len + i * 2 + 1];
+		nvt_spi_xdata[i] = (int16_t)data;
+	}
+
+#if NVT_SPI_TOUCH_KEY_NUM > 0
+	//read button xdata : step3
+	//---change xdata index---
+	nvt_spi_set_page(xdata_btn_addr);
+
+	//---read data---
+	buf[0] = (xdata_btn_addr & 0xFF);
+	nvt_spi_read(buf, (NVT_SPI_TOUCH_KEY_NUM * 2 + 1));
+
+	//---2bytes-to-1data---
+	for (i = 0; i < NVT_SPI_TOUCH_KEY_NUM; i++)
+		nvt_spi_xdata[ts->x_num * ts->y_num + i] =
+				(int16_t)(buf[1 + i * 2] + 256 * buf[1 + i * 2 + 1]);
+#endif
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen get meta data function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_get_mdata(int32_t *buf, uint8_t *m_x_num, uint8_t *m_y_num)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	*m_x_num = ts->x_num;
+	*m_y_num = ts->y_num;
+	memcpy(buf, nvt_spi_xdata,
+		((ts->x_num * ts->y_num + NVT_SPI_TOUCH_KEY_NUM) * sizeof(int32_t)));
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen read and get number of meta data function.
+ *
+ * return:
+ *	n.a.
+ *******************************************************
+ */
+void nvt_spi_read_get_num_mdata(uint32_t xdata_addr, int32_t *buffer, uint32_t num)
+{
+	int32_t i = 0;
+	int32_t j = 0;
+	int32_t k = 0;
+	uint8_t buf[NVT_SPI_BUS_TRANSFER_LENGTH + 2] = {0};
+	uint32_t head_addr = 0;
+	int32_t dummy_len = 0;
+	int32_t data_len = 0;
+	int32_t residual_len = 0;
+	int32_t index, data;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	//---set xdata sector address & length---
+	head_addr = xdata_addr - (xdata_addr % NVT_SPI_XDATA_SECTOR_SIZE);
+	dummy_len = xdata_addr - head_addr;
+	data_len = num * 2;
+	residual_len = (head_addr + dummy_len + data_len) % NVT_SPI_XDATA_SECTOR_SIZE;
+
+	//printk("head_addr=0x%05X, dummy_len=0x%05X, data_len=0x%05X, residual_len=0x%05X\n",
+	//	head_addr, dummy_len, data_len, residual_len);
+
+	//read xdata : step 1
+	for (i = 0; i < ((dummy_len + data_len) / NVT_SPI_XDATA_SECTOR_SIZE); i++) {
+		//---change xdata index---
+		nvt_spi_set_page(head_addr + NVT_SPI_XDATA_SECTOR_SIZE * i);
+
+		//---read xdata by NVT_SPI_BUS_TRANSFER_LENGTH
+		for (j = 0; j < (NVT_SPI_XDATA_SECTOR_SIZE / NVT_SPI_BUS_TRANSFER_LENGTH); j++) {
+			//---read data---
+			buf[0] = NVT_SPI_BUS_TRANSFER_LENGTH * j;
+			nvt_spi_read(buf, NVT_SPI_BUS_TRANSFER_LENGTH + 1);
+
+			//---copy buf to nvt_spi_xdata_tmp---
+			for (k = 0; k < NVT_SPI_BUS_TRANSFER_LENGTH; k++) {
+				index = NVT_SPI_XDATA_SECTOR_SIZE * i;
+				index += NVT_SPI_BUS_TRANSFER_LENGTH * j + k;
+				nvt_spi_xdata_tmp[index] = buf[k + 1];
+			}
+		}
+		//printk("addr=0x%05X\n", (head_addr+NVT_SPI_XDATA_SECTOR_SIZE*i));
+	}
+
+	//read xdata : step2
+	if (residual_len != 0) {
+		//---change xdata index---
+		nvt_spi_set_page(xdata_addr + data_len - residual_len);
+
+		//---read xdata by NVT_SPI_BUS_TRANSFER_LENGTH
+		for (j = 0; j < (residual_len / NVT_SPI_BUS_TRANSFER_LENGTH + 1); j++) {
+			//---read data---
+			buf[0] = NVT_SPI_BUS_TRANSFER_LENGTH * j;
+			nvt_spi_read(buf, NVT_SPI_BUS_TRANSFER_LENGTH + 1);
+
+			//---copy buf to nvt_spi_xdata_tmp---
+			for (k = 0; k < NVT_SPI_BUS_TRANSFER_LENGTH; k++) {
+				index = (dummy_len + data_len - residual_len);
+				index += NVT_SPI_BUS_TRANSFER_LENGTH * j + k;
+				nvt_spi_xdata_tmp[index] = buf[k + 1];
+				//printk("0x%02X, 0x%04x\n", buf[k+1], index));
+
+			}
+		}
+		//printk("addr=0x%05X\n", (xdata_addr+data_len-residual_len));
+	}
+
+	//---remove dummy data and 2bytes-to-1data---
+	for (i = 0; i < (data_len / 2); i++) {
+		data = nvt_spi_xdata_tmp[dummy_len + i * 2];
+		data += 256 * nvt_spi_xdata_tmp[dummy_len + i * 2 + 1];
+		buffer[i] = (int16_t)data;
+	}
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_spi_set_page(ts->mmap->EVENT_BUF_ADDR);
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen firmware version show function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t c_fw_version_show(struct seq_file *m, void *v)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	seq_printf(m, "fw_ver=%d, x_num=%d, y_num=%d, button_num=%d\n",
+		ts->fw_ver, ts->x_num, ts->y_num, ts->max_button_num);
+	return 0;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen xdata sequence print show
+ *	function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t c_show(struct seq_file *m, void *v)
+{
+	int32_t i = 0;
+	int32_t j = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	for (i = 0; i < ts->y_num; i++) {
+		for (j = 0; j < ts->x_num; j++)
+			seq_printf(m, "%5d, ", nvt_spi_xdata[i * ts->x_num + j]);
+
+		seq_puts(m, "\n");
+	}
+
+#if NVT_SPI_TOUCH_KEY_NUM > 0
+	for (i = 0; i < NVT_SPI_TOUCH_KEY_NUM; i++)
+		seq_printf(m, "%5d, ", nvt_spi_xdata[ts->x_num * ts->y_num + i]);
+
+	seq_puts(m, "\n");
+#endif
+
+	seq_puts(m, "\n\n");
+	return 0;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek pen 1D diff xdata sequence print show
+ *	function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t c_pen_1d_diff_show(struct seq_file *m, void *v)
+{
+	int32_t i = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	seq_puts(m, "Tip X:\n");
+	for (i = 0; i < ts->x_num; i++)
+		seq_printf(m, "%5d, ", nvt_spi_xdata_pen_tip_x[i]);
+
+	seq_puts(m, "\n");
+	seq_puts(m, "Tip Y:\n");
+	for (i = 0; i < ts->y_num; i++)
+		seq_printf(m, "%5d, ", nvt_spi_xdata_pen_tip_y[i]);
+
+	seq_puts(m, "\n");
+	seq_puts(m, "Ring X:\n");
+	for (i = 0; i < ts->x_num; i++)
+		seq_printf(m, "%5d, ", nvt_spi_xdata_pen_ring_x[i]);
+
+	seq_puts(m, "\n");
+	seq_puts(m, "Ring Y:\n");
+	for (i = 0; i < ts->y_num; i++)
+		seq_printf(m, "%5d, ", nvt_spi_xdata_pen_ring_y[i]);
+
+	seq_puts(m, "\n");
+
+	seq_puts(m, "\n\n");
+	return 0;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen xdata sequence print start
+ *	function.
+ *
+ * return:
+ *	Executive outcomes. 1---call next function.
+ *	NULL---not call next function and sequence loop
+ *	stop.
+ *******************************************************
+ */
+static void *c_start(struct seq_file *m, loff_t *pos)
+{
+	return *pos < 1 ? (void *)1 : NULL;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen xdata sequence print next
+ *	function.
+ *
+ * return:
+ *	Executive outcomes. NULL---no next and call sequence
+ *	stop function.
+ ******************************************************
+ */
+static void *c_next(struct seq_file *m, void *v, loff_t *pos)
+{
+	++*pos;
+	return NULL;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen xdata sequence print stop
+ *	function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void c_stop(struct seq_file *m, void *v)
+{
+}
+
+const struct seq_operations nvt_spi_fw_version_seq_ops = {
+	.start = c_start,
+	.next  = c_next,
+	.stop  = c_stop,
+	.show  = c_fw_version_show
+};
+
+const struct seq_operations nvt_spi_seq_ops = {
+	.start = c_start,
+	.next  = c_next,
+	.stop  = c_stop,
+	.show  = c_show
+};
+
+const struct seq_operations nvt_spi_pen_diff_seq_ops = {
+	.start = c_start,
+	.next  = c_next,
+	.stop  = c_stop,
+	.show  = c_pen_1d_diff_show
+};
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/nvt_fw_version open
+ *	function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static int32_t nvt_spi_fw_version_open(struct inode *inode, struct file *file)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (mutex_lock_interruptible(&ts->lock))
+		return -ERESTARTSYS;
+
+	NVT_LOG("++\n");
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	nvt_spi_esd_check_enable(false);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	if (nvt_spi_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_spi_fw_version_seq_ops);
+}
+
+static const struct proc_ops nvt_spi_fw_version_fops = {
+	.proc_open = nvt_spi_fw_version_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/nvt_baseline open function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t nvt_spi_baseline_open(struct inode *inode, struct file *file)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (mutex_lock_interruptible(&ts->lock))
+		return -ERESTARTSYS;
+
+	NVT_LOG("++\n");
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	nvt_spi_esd_check_enable(false);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	if (nvt_spi_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_spi_change_mode(NVT_SPI_TEST_MODE_2);
+
+	if (nvt_spi_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_spi_read_mdata(ts->mmap->BASELINE_ADDR, ts->mmap->BASELINE_BTN_ADDR);
+
+	nvt_spi_change_mode(NVT_SPI_NORMAL_MODE);
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_spi_seq_ops);
+}
+
+static const struct proc_ops nvt_spi_baseline_fops = {
+	.proc_open = nvt_spi_baseline_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/nvt_raw open function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed.
+ ******************************************************
+ */
+static int32_t nvt_spi_raw_open(struct inode *inode, struct file *file)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (mutex_lock_interruptible(&ts->lock))
+		return -ERESTARTSYS;
+
+	NVT_LOG("++\n");
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	nvt_spi_esd_check_enable(false);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	if (nvt_spi_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_spi_change_mode(NVT_SPI_TEST_MODE_2);
+
+	if (nvt_spi_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_get_fw_pipe() == 0)
+		nvt_spi_read_mdata(ts->mmap->RAW_PIPE0_ADDR, ts->mmap->RAW_BTN_PIPE0_ADDR);
+	else
+		nvt_spi_read_mdata(ts->mmap->RAW_PIPE1_ADDR, ts->mmap->RAW_BTN_PIPE1_ADDR);
+
+	nvt_spi_change_mode(NVT_SPI_NORMAL_MODE);
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_spi_seq_ops);
+}
+
+static const struct proc_ops nvt_spi_raw_fops = {
+	.proc_open = nvt_spi_raw_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/nvt_diff open function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. negative---failed.
+ ******************************************************
+ */
+static int32_t nvt_spi_diff_open(struct inode *inode, struct file *file)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (mutex_lock_interruptible(&ts->lock))
+		return -ERESTARTSYS;
+
+	NVT_LOG("++\n");
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	nvt_spi_esd_check_enable(false);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	if (nvt_spi_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_spi_change_mode(NVT_SPI_TEST_MODE_2);
+
+	if (nvt_spi_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_get_fw_pipe() == 0)
+		nvt_spi_read_mdata(ts->mmap->DIFF_PIPE0_ADDR, ts->mmap->DIFF_BTN_PIPE0_ADDR);
+	else
+		nvt_spi_read_mdata(ts->mmap->DIFF_PIPE1_ADDR, ts->mmap->DIFF_BTN_PIPE1_ADDR);
+
+	nvt_spi_change_mode(NVT_SPI_NORMAL_MODE);
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_spi_seq_ops);
+}
+
+static const struct proc_ops nvt_spi_diff_fops = {
+	.proc_open = nvt_spi_diff_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen /proc/nvt_pen_diff open function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. negative---failed.
+ ******************************************************
+ */
+static int32_t nvt_spi_pen_diff_open(struct inode *inode, struct file *file)
+{
+	uint32_t addr;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (mutex_lock_interruptible(&ts->lock))
+		return -ERESTARTSYS;
+
+	NVT_LOG("++\n");
+
+#if NVT_SPI_TOUCH_ESD_PROTECT
+	nvt_spi_esd_check_enable(false);
+#endif /* #if NVT_SPI_TOUCH_ESD_PROTECT */
+
+	if (nvt_set_pen_inband_mode_1_spi(0xFF, 0x00)) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_check_fw_reset_state(NVT_SPI_RESET_STATE_NORMAL_RUN)) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	nvt_spi_change_mode(NVT_SPI_TEST_MODE_2);
+
+	if (nvt_spi_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	if (nvt_spi_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		return -EAGAIN;
+	}
+
+	addr = ts->mmap->PEN_1D_DIFF_TIP_X_ADDR;
+	nvt_spi_read_get_num_mdata(addr, nvt_spi_xdata_pen_tip_x, ts->x_num);
+
+	addr = ts->mmap->PEN_1D_DIFF_TIP_Y_ADDR;
+	nvt_spi_read_get_num_mdata(addr, nvt_spi_xdata_pen_tip_y, ts->y_num);
+
+	addr = ts->mmap->PEN_1D_DIFF_RING_X_ADDR;
+	nvt_spi_read_get_num_mdata(addr, nvt_spi_xdata_pen_ring_x, ts->x_num);
+
+	addr = ts->mmap->PEN_1D_DIFF_RING_Y_ADDR;
+	nvt_spi_read_get_num_mdata(addr, nvt_spi_xdata_pen_ring_y, ts->y_num);
+
+	nvt_spi_change_mode(NVT_SPI_NORMAL_MODE);
+
+	nvt_spi_set_pen_normal_mode();
+
+	nvt_spi_check_fw_reset_state(NVT_SPI_RESET_STATE_NORMAL_RUN);
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	return seq_open(file, &nvt_spi_pen_diff_seq_ops);
+}
+
+static const struct proc_ops nvt_spi_pen_diff_fops = {
+	.proc_open = nvt_spi_pen_diff_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen extra function proc. file node
+ *	initial function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -12---failed.
+ ******************************************************
+ */
+int32_t nvt_spi_extra_proc_init(void)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	nvt_spi_proc_fw_version_entry = proc_create(NVT_SPI_FW_VERSION, 0444, NULL,
+			&nvt_spi_fw_version_fops);
+	if (nvt_spi_proc_fw_version_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_SPI_FW_VERSION);
+		return -ENOMEM;
+	}
+	NVT_LOG("create proc/%s Succeeded!\n", NVT_SPI_FW_VERSION);
+
+	nvt_spi_proc_baseline_entry = proc_create(NVT_SPI_BASELINE, 0444, NULL,
+			&nvt_spi_baseline_fops);
+	if (nvt_spi_proc_baseline_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_SPI_BASELINE);
+		return -ENOMEM;
+	}
+	NVT_LOG("create proc/%s Succeeded!\n", NVT_SPI_BASELINE);
+
+	nvt_spi_proc_raw_entry = proc_create(NVT_SPI_RAW, 0444, NULL, &nvt_spi_raw_fops);
+	if (nvt_spi_proc_raw_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_SPI_RAW);
+		return -ENOMEM;
+	}
+	NVT_LOG("create proc/%s Succeeded!\n", NVT_SPI_RAW);
+
+	nvt_spi_proc_diff_entry = proc_create(NVT_SPI_DIFF, 0444, NULL, &nvt_spi_diff_fops);
+	if (nvt_spi_proc_diff_entry == NULL) {
+		NVT_ERR("create proc/%s Failed!\n", NVT_SPI_DIFF);
+		return -ENOMEM;
+	}
+	NVT_LOG("create proc/%s Succeeded!\n", NVT_SPI_DIFF);
+
+	if (ts->pen_support) {
+		nvt_spi_proc_pen_diff_entry = proc_create(NVT_SPI_PEN_DIFF, 0444, NULL,
+				&nvt_spi_pen_diff_fops);
+		if (nvt_spi_proc_pen_diff_entry == NULL) {
+			NVT_ERR("create proc/%s Failed!\n", NVT_SPI_PEN_DIFF);
+			return -ENOMEM;
+		}
+		NVT_LOG("create proc/%s Succeeded!\n", NVT_SPI_PEN_DIFF);
+	}
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen extra function proc. file node
+ *	deinitial function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_extra_proc_deinit(void)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	if (nvt_spi_proc_fw_version_entry != NULL) {
+		remove_proc_entry(NVT_SPI_FW_VERSION, NULL);
+		nvt_spi_proc_fw_version_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_SPI_FW_VERSION);
+	}
+
+	if (nvt_spi_proc_baseline_entry != NULL) {
+		remove_proc_entry(NVT_SPI_BASELINE, NULL);
+		nvt_spi_proc_baseline_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_SPI_BASELINE);
+	}
+
+	if (nvt_spi_proc_raw_entry != NULL) {
+		remove_proc_entry(NVT_SPI_RAW, NULL);
+		nvt_spi_proc_raw_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_SPI_RAW);
+	}
+
+	if (nvt_spi_proc_diff_entry != NULL) {
+		remove_proc_entry(NVT_SPI_DIFF, NULL);
+		nvt_spi_proc_diff_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", NVT_SPI_DIFF);
+	}
+
+	if (ts->pen_support) {
+		if (nvt_spi_proc_pen_diff_entry != NULL) {
+			remove_proc_entry(NVT_SPI_PEN_DIFF, NULL);
+			nvt_spi_proc_pen_diff_entry = NULL;
+			NVT_LOG("Removed /proc/%s\n", NVT_SPI_PEN_DIFF);
+		}
+	}
+}
+#endif
+
+
+
+#endif

nt36xxx/nt36xxx_fw_update.c

@@ -0,0 +1,2002 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2010 - 2018 Novatek, Inc.
+ *
+ * $Revision: 47247 $
+ * $Date: 2019-07-10 10:41:36 +0800 (Wed, 10 Jul 2019) $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+
+#if !defined(NVT_NT36XXX_SPI) /* NT36XXX I2C */
+
+#include <linux/firmware.h>
+
+#include "nt36xxx.h"
+
+#if BOOT_UPDATE_FIRMWARE
+
+#define SIZE_4KB 4096
+#define FLASH_SECTOR_SIZE SIZE_4KB
+#define SIZE_64KB 65536
+#define BLOCK_64KB_NUM 4
+#define FW_BIN_VER_OFFSET (fw_need_write_size - SIZE_4KB)
+#define FW_BIN_VER_BAR_OFFSET (FW_BIN_VER_OFFSET + 1)
+
+#define NVT_FLASH_END_FLAG_LEN 3
+#define NVT_FLASH_END_FLAG_ADDR (fw_need_write_size - NVT_FLASH_END_FLAG_LEN)
+
+const struct firmware *fw_entry = NULL;
+static size_t fw_need_write_size = 0;
+
+static int32_t nvt_get_fw_need_write_size(const struct firmware *fw_entry)
+{
+	int32_t i = 0;
+	int32_t total_sectors_to_check = 0;
+
+	total_sectors_to_check = fw_entry->size / FLASH_SECTOR_SIZE;
+	/* printk("total_sectors_to_check = %d\n", total_sectors_to_check); */
+
+	for (i = total_sectors_to_check; i > 0; i--) {
+		/* printk("current end flag address checked = 0x%X\n", i * FLASH_SECTOR_SIZE - NVT_FLASH_END_FLAG_LEN); */
+		/* check if there is end flag "NVT" at the end of this sector */
+		if ((memcmp((const char *)&fw_entry->data[i * FLASH_SECTOR_SIZE -
+				NVT_FLASH_END_FLAG_LEN], "NVT", NVT_FLASH_END_FLAG_LEN) == 0) ||
+			(memcmp((const char *)&fw_entry->data[i * FLASH_SECTOR_SIZE -
+				NVT_FLASH_END_FLAG_LEN], "MOD", NVT_FLASH_END_FLAG_LEN) == 0)) {
+			fw_need_write_size = i * FLASH_SECTOR_SIZE;
+			NVT_LOG("fw_need_write_size = %zu(0x%zx)\n", fw_need_write_size, fw_need_write_size);
+			return 0;
+		}
+	}
+
+	NVT_ERR("end flag \"NVT\" not found!\n");
+	return -1;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen request update firmware function.
+
+return:
+	Executive outcomes. 0---succeed. -1,-22---failed.
+*******************************************************/
+int32_t update_firmware_request(char *filename)
+{
+	int32_t ret = 0;
+
+	if (NULL == filename) {
+		return -1;
+	}
+
+	NVT_LOG("filename is %s\n", filename);
+
+	ret = request_firmware(&fw_entry, filename, &ts->client->dev);
+	if (ret) {
+		NVT_ERR("firmware load failed, ret=%d\n", ret);
+		return ret;
+	}
+
+	// check FW need to write size
+	if (nvt_get_fw_need_write_size(fw_entry)) {
+		NVT_ERR("get fw need to write size fail!\n");
+		return -EINVAL;
+	}
+
+	// check if FW version add FW version bar equals 0xFF
+	if (*(fw_entry->data + FW_BIN_VER_OFFSET) + *(fw_entry->data + FW_BIN_VER_BAR_OFFSET) != 0xFF) {
+		NVT_ERR("bin file FW_VER + FW_VER_BAR should be 0xFF!\n");
+		NVT_ERR("FW_VER=0x%02X, FW_VER_BAR=0x%02X\n", *(fw_entry->data+FW_BIN_VER_OFFSET), *(fw_entry->data+FW_BIN_VER_BAR_OFFSET));
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen release update firmware function.
+
+return:
+	n.a.
+*******************************************************/
+void update_firmware_release(void)
+{
+	if (fw_entry) {
+		release_firmware(fw_entry);
+	}
+	fw_entry=NULL;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen check firmware version function.
+
+return:
+	Executive outcomes. 0---need update. 1---need not
+	update.
+*******************************************************/
+int32_t Check_FW_Ver(void)
+{
+	uint8_t buf[16] = {0};
+	int32_t ret = 0;
+
+	//write i2c index to EVENT BUF ADDR
+	ret = nvt_set_page(I2C_BLDR_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_FWINFO);
+	if (ret < 0) {
+		NVT_ERR("i2c write error!(%d)\n", ret);
+		return ret;
+	}
+
+	//read Firmware Version
+	buf[0] = EVENT_MAP_FWINFO;
+	buf[1] = 0x00;
+	buf[2] = 0x00;
+	ret = CTP_I2C_READ(ts->client, I2C_BLDR_Address, buf, 3);
+	if (ret < 0) {
+		NVT_ERR("i2c read error!(%d)\n", ret);
+		return ret;
+	}
+
+	NVT_LOG("IC FW Ver = 0x%02X, FW Ver Bar = 0x%02X\n", buf[1], buf[2]);
+	NVT_LOG("Bin FW Ver = 0x%02X, FW ver Bar = 0x%02X\n",
+			fw_entry->data[FW_BIN_VER_OFFSET], fw_entry->data[FW_BIN_VER_BAR_OFFSET]);
+
+	// check IC FW_VER + FW_VER_BAR equals 0xFF or not, need to update if not
+	if ((buf[1] + buf[2]) != 0xFF) {
+		NVT_ERR("IC FW_VER + FW_VER_BAR not equals to 0xFF!\n");
+		return 0;
+	}
+
+	// compare IC and binary FW version
+	if (buf[1] > fw_entry->data[FW_BIN_VER_OFFSET])
+		return 1;
+	else
+		return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen resume from deep power down function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+int32_t Resume_PD(void)
+{
+	uint8_t buf[8] = {0};
+	int32_t ret = 0;
+	int32_t retry = 0;
+
+	// Resume Command
+	buf[0] = 0x00;
+	buf[1] = 0xAB;
+	ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+	if (ret < 0) {
+		NVT_ERR("Write Enable error!!(%d)\n", ret);
+		return ret;
+	}
+
+	// Check 0xAA (Resume Command)
+	retry = 0;
+	while(1) {
+		msleep(1);
+		buf[0] = 0x00;
+		buf[1] = 0x00;
+		ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+		if (ret < 0) {
+			NVT_ERR("Check 0xAA (Resume Command) error!!(%d)\n", ret);
+			return ret;
+		}
+		if (buf[1] == 0xAA) {
+			break;
+		}
+		retry++;
+		if (unlikely(retry > 20)) {
+			NVT_ERR("Check 0xAA (Resume Command) error!! status=0x%02X\n", buf[1]);
+			return -1;
+		}
+	}
+	msleep(10);
+
+	NVT_LOG("Resume PD OK\n");
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen check firmware checksum function.
+
+return:
+	Executive outcomes. 0---checksum not match.
+	1---checksum match. -1--- checksum read failed.
+*******************************************************/
+int32_t Check_CheckSum(void)
+{
+	uint8_t buf[64] = {0};
+	uint32_t XDATA_Addr = ts->mmap->READ_FLASH_CHECKSUM_ADDR;
+	int32_t ret = 0;
+	int32_t i = 0;
+	int32_t k = 0;
+	uint16_t WR_Filechksum[BLOCK_64KB_NUM] = {0};
+	uint16_t RD_Filechksum[BLOCK_64KB_NUM] = {0};
+	size_t len_in_blk = 0;
+	int32_t retry = 0;
+
+	if (Resume_PD()) {
+		NVT_ERR("Resume PD error!!\n");
+		return -1;
+	}
+
+	for (i = 0; i < BLOCK_64KB_NUM; i++) {
+		if (fw_need_write_size > (i * SIZE_64KB)) {
+			// Calculate WR_Filechksum of each 64KB block
+			len_in_blk = min(fw_need_write_size - i * SIZE_64KB, (size_t)SIZE_64KB);
+			WR_Filechksum[i] = i + 0x00 + 0x00 + (((len_in_blk - 1) >> 8) & 0xFF) + ((len_in_blk - 1) & 0xFF);
+			for (k = 0; k < len_in_blk; k++) {
+				WR_Filechksum[i] += fw_entry->data[k + i * SIZE_64KB];
+			}
+			WR_Filechksum[i] = 65535 - WR_Filechksum[i] + 1;
+
+			// Fast Read Command
+			buf[0] = 0x00;
+			buf[1] = 0x07;
+			buf[2] = i;
+			buf[3] = 0x00;
+			buf[4] = 0x00;
+			buf[5] = ((len_in_blk - 1) >> 8) & 0xFF;
+			buf[6] = (len_in_blk - 1) & 0xFF;
+			ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 7);
+			if (ret < 0) {
+				NVT_ERR("Fast Read Command error!!(%d)\n", ret);
+				return ret;
+			}
+			// Check 0xAA (Fast Read Command)
+			retry = 0;
+			while (1) {
+				msleep(80);
+				buf[0] = 0x00;
+				buf[1] = 0x00;
+				ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+				if (ret < 0) {
+					NVT_ERR("Check 0xAA (Fast Read Command) error!!(%d)\n", ret);
+					return ret;
+				}
+				if (buf[1] == 0xAA) {
+					break;
+				}
+				retry++;
+				if (unlikely(retry > 5)) {
+					NVT_ERR("Check 0xAA (Fast Read Command) failed, buf[1]=0x%02X, retry=%d\n", buf[1], retry);
+					return -1;
+				}
+			}
+			// Read Checksum (write addr high byte & middle byte)
+			ret = nvt_set_page(I2C_BLDR_Address, XDATA_Addr);
+			if (ret < 0) {
+				NVT_ERR("Read Checksum (write addr high byte & middle byte) error!!(%d)\n", ret);
+				return ret;
+			}
+			// Read Checksum
+			buf[0] = (XDATA_Addr) & 0xFF;
+			buf[1] = 0x00;
+			buf[2] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_BLDR_Address, buf, 3);
+			if (ret < 0) {
+				NVT_ERR("Read Checksum error!!(%d)\n", ret);
+				return ret;
+			}
+
+			RD_Filechksum[i] = (uint16_t)((buf[2] << 8) | buf[1]);
+			if (WR_Filechksum[i] != RD_Filechksum[i]) {
+				NVT_ERR("RD_Filechksum[%d]=0x%04X, WR_Filechksum[%d]=0x%04X\n", i, RD_Filechksum[i], i, WR_Filechksum[i]);
+				NVT_ERR("firmware checksum not match!!\n");
+				return 0;
+			}
+		}
+	}
+
+	NVT_LOG("firmware checksum match\n");
+	return 1;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen initial bootloader and flash
+	block function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+int32_t Init_BootLoader(void)
+{
+	uint8_t buf[64] = {0};
+	int32_t ret = 0;
+	int32_t retry = 0;
+
+	// SW Reset & Idle
+	nvt_sw_reset_idle();
+
+	// Initiate Flash Block
+	buf[0] = 0x00;
+	buf[1] = 0x00;
+	buf[2] = I2C_FW_Address;
+	ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 3);
+	if (ret < 0) {
+		NVT_ERR("Inittial Flash Block error!!(%d)\n", ret);
+		return ret;
+	}
+
+	// Check 0xAA (Initiate Flash Block)
+	retry = 0;
+	while(1) {
+		msleep(1);
+		buf[0] = 0x00;
+		buf[1] = 0x00;
+		ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+		if (ret < 0) {
+			NVT_ERR("Check 0xAA (Inittial Flash Block) error!!(%d)\n", ret);
+			return ret;
+		}
+		if (buf[1] == 0xAA) {
+			break;
+		}
+		retry++;
+		if (unlikely(retry > 20)) {
+			NVT_ERR("Check 0xAA (Inittial Flash Block) error!! status=0x%02X\n", buf[1]);
+			return -1;
+		}
+	}
+
+	NVT_LOG("Init OK \n");
+	msleep(20);
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen erase flash sectors function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+int32_t Erase_Flash(void)
+{
+	uint8_t buf[64] = {0};
+	int32_t ret = 0;
+	int32_t count = 0;
+	int32_t i = 0;
+	int32_t Flash_Address = 0;
+	int32_t retry = 0;
+
+	// Write Enable
+	buf[0] = 0x00;
+	buf[1] = 0x06;
+	ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+	if (ret < 0) {
+		NVT_ERR("Write Enable (for Write Status Register) error!!(%d)\n", ret);
+		return ret;
+	}
+	// Check 0xAA (Write Enable)
+	retry = 0;
+	while (1) {
+		msleep(1);
+		buf[0] = 0x00;
+		buf[1] = 0x00;
+		ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+		if (ret < 0) {
+			NVT_ERR("Check 0xAA (Write Enable for Write Status Register) error!!(%d)\n", ret);
+			return ret;
+		}
+		if (buf[1] == 0xAA) {
+			break;
+		}
+		retry++;
+		if (unlikely(retry > 20)) {
+			NVT_ERR("Check 0xAA (Write Enable for Write Status Register) error!! status=0x%02X\n", buf[1]);
+			return -1;
+		}
+	}
+
+	// Write Status Register
+	buf[0] = 0x00;
+	buf[1] = 0x01;
+	buf[2] = 0x00;
+	ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 3);
+	if (ret < 0) {
+		NVT_ERR("Write Status Register error!!(%d)\n", ret);
+		return ret;
+	}
+	// Check 0xAA (Write Status Register)
+	retry = 0;
+	while (1) {
+		msleep(1);
+		buf[0] = 0x00;
+		buf[1] = 0x00;
+		ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+		if (ret < 0) {
+			NVT_ERR("Check 0xAA (Write Status Register) error!!(%d)\n", ret);
+			return ret;
+		}
+		if (buf[1] == 0xAA) {
+			break;
+		}
+		retry++;
+		if (unlikely(retry > 20)) {
+			NVT_ERR("Check 0xAA (Write Status Register) error!! status=0x%02X\n", buf[1]);
+			return -1;
+		}
+	}
+
+	// Read Status
+	retry = 0;
+	while (1) {
+		msleep(5);
+		buf[0] = 0x00;
+		buf[1] = 0x05;
+		ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+		if (ret < 0) {
+			NVT_ERR("Read Status (for Write Status Register) error!!(%d)\n", ret);
+			return ret;
+		}
+
+		// Check 0xAA (Read Status)
+		buf[0] = 0x00;
+		buf[1] = 0x00;
+		buf[2] = 0x00;
+		ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 3);
+		if (ret < 0) {
+			NVT_ERR("Check 0xAA (Read Status for Write Status Register) error!!(%d)\n", ret);
+			return ret;
+		}
+		if ((buf[1] == 0xAA) && (buf[2] == 0x00)) {
+			break;
+		}
+		retry++;
+		if (unlikely(retry > 100)) {
+			NVT_ERR("Check 0xAA (Read Status for Write Status Register) failed, buf[1]=0x%02X, buf[2]=0x%02X, retry=%d\n", buf[1], buf[2], retry);
+			return -1;
+		}
+	}
+
+	if (fw_need_write_size % FLASH_SECTOR_SIZE)
+		count = fw_need_write_size / FLASH_SECTOR_SIZE + 1;
+	else
+		count = fw_need_write_size / FLASH_SECTOR_SIZE;
+
+	for(i = 0; i < count; i++) {
+		// Write Enable
+		buf[0] = 0x00;
+		buf[1] = 0x06;
+		ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+		if (ret < 0) {
+			NVT_ERR("Write Enable error!!(%d,%d)\n", ret, i);
+			return ret;
+		}
+		// Check 0xAA (Write Enable)
+		retry = 0;
+		while (1) {
+			msleep(1);
+			buf[0] = 0x00;
+			buf[1] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+			if (ret < 0) {
+				NVT_ERR("Check 0xAA (Write Enable) error!!(%d,%d)\n", ret, i);
+				return ret;
+			}
+			if (buf[1] == 0xAA) {
+				break;
+			}
+			retry++;
+			if (unlikely(retry > 20)) {
+				NVT_ERR("Check 0xAA (Write Enable) error!! status=0x%02X\n", buf[1]);
+				return -1;
+			}
+		}
+
+		Flash_Address = i * FLASH_SECTOR_SIZE;
+
+		// Sector Erase
+		buf[0] = 0x00;
+		buf[1] = 0x20;    // Command : Sector Erase
+		buf[2] = ((Flash_Address >> 16) & 0xFF);
+		buf[3] = ((Flash_Address >> 8) & 0xFF);
+		buf[4] = (Flash_Address & 0xFF);
+		ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 5);
+		if (ret < 0) {
+			NVT_ERR("Sector Erase error!!(%d,%d)\n", ret, i);
+			return ret;
+		}
+		// Check 0xAA (Sector Erase)
+		retry = 0;
+		while (1) {
+			msleep(1);
+			buf[0] = 0x00;
+			buf[1] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+			if (ret < 0) {
+				NVT_ERR("Check 0xAA (Sector Erase) error!!(%d,%d)\n", ret, i);
+				return ret;
+			}
+			if (buf[1] == 0xAA) {
+				break;
+			}
+			retry++;
+			if (unlikely(retry > 20)) {
+				NVT_ERR("Check 0xAA (Sector Erase) failed, buf[1]=0x%02X, retry=%d\n", buf[1], retry);
+				return -1;
+			}
+		}
+
+		// Read Status
+		retry = 0;
+		while (1) {
+			msleep(5);
+			buf[0] = 0x00;
+			buf[1] = 0x05;
+			ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+			if (ret < 0) {
+				NVT_ERR("Read Status error!!(%d,%d)\n", ret, i);
+				return ret;
+			}
+
+			// Check 0xAA (Read Status)
+			buf[0] = 0x00;
+			buf[1] = 0x00;
+			buf[2] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 3);
+			if (ret < 0) {
+				NVT_ERR("Check 0xAA (Read Status) error!!(%d,%d)\n", ret, i);
+				return ret;
+			}
+			if ((buf[1] == 0xAA) && (buf[2] == 0x00)) {
+				break;
+			}
+			retry++;
+			if (unlikely(retry > 100)) {
+				NVT_ERR("Check 0xAA (Read Status) failed, buf[1]=0x%02X, buf[2]=0x%02X, retry=%d\n", buf[1], buf[2], retry);
+				return -1;
+			}
+		}
+	}
+
+	NVT_LOG("Erase OK \n");
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen write flash sectors function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+int32_t Write_Flash(void)
+{
+	uint8_t buf[64] = {0};
+	uint32_t XDATA_Addr = ts->mmap->RW_FLASH_DATA_ADDR;
+	uint32_t Flash_Address = 0;
+	int32_t i = 0, j = 0, k = 0;
+	uint8_t tmpvalue = 0;
+	int32_t count = 0;
+	int32_t ret = 0;
+	int32_t retry = 0;
+	int32_t percent = 0;
+	int32_t previous_percent = -1;
+
+	// change I2C buffer index
+	ret = nvt_set_page(I2C_BLDR_Address, XDATA_Addr);
+	if (ret < 0) {
+		NVT_ERR("change I2C buffer index error!!(%d)\n", ret);
+		return ret;
+	}
+
+	if (fw_need_write_size % 256)
+		count = fw_need_write_size / 256 + 1;
+	else
+		count = fw_need_write_size / 256;
+
+	for (i = 0; i < count; i++) {
+		Flash_Address = i * 256;
+
+		// Write Enable
+		buf[0] = 0x00;
+		buf[1] = 0x06;
+		ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+		if (ret < 0) {
+			NVT_ERR("Write Enable error!!(%d)\n", ret);
+			return ret;
+		}
+		// Check 0xAA (Write Enable)
+		retry = 0;
+		while (1) {
+			udelay(100);
+			buf[0] = 0x00;
+			buf[1] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+			if (ret < 0) {
+				NVT_ERR("Check 0xAA (Write Enable) error!!(%d,%d)\n", ret, i);
+				return ret;
+			}
+			if (buf[1] == 0xAA) {
+				break;
+			}
+			retry++;
+			if (unlikely(retry > 20)) {
+				NVT_ERR("Check 0xAA (Write Enable) error!! status=0x%02X\n", buf[1]);
+				return -1;
+			}
+		}
+
+		// Write Page : 256 bytes
+		for (j = 0; j < min(fw_need_write_size - i * 256, (size_t)256); j += 32) {
+			buf[0] = (XDATA_Addr + j) & 0xFF;
+			for (k = 0; k < 32; k++) {
+				buf[1 + k] = fw_entry->data[Flash_Address + j + k];
+			}
+			ret = CTP_I2C_WRITE(ts->client, I2C_BLDR_Address, buf, 33);
+			if (ret < 0) {
+				NVT_ERR("Write Page error!!(%d), j=%d\n", ret, j);
+				return ret;
+			}
+		}
+		if (fw_need_write_size - Flash_Address >= 256)
+			tmpvalue=(Flash_Address >> 16) + ((Flash_Address >> 8) & 0xFF) + (Flash_Address & 0xFF) + 0x00 + (255);
+		else
+			tmpvalue=(Flash_Address >> 16) + ((Flash_Address >> 8) & 0xFF) + (Flash_Address & 0xFF) + 0x00 + (fw_need_write_size - Flash_Address - 1);
+
+		for (k = 0; k < min(fw_need_write_size - Flash_Address, (size_t)256); k++)
+			tmpvalue += fw_entry->data[Flash_Address + k];
+
+		tmpvalue = 255 - tmpvalue + 1;
+
+		// Page Program
+		buf[0] = 0x00;
+		buf[1] = 0x02;
+		buf[2] = ((Flash_Address >> 16) & 0xFF);
+		buf[3] = ((Flash_Address >> 8) & 0xFF);
+		buf[4] = (Flash_Address & 0xFF);
+		buf[5] = 0x00;
+		buf[6] = min(fw_need_write_size - Flash_Address, (size_t)256) - 1;
+		buf[7] = tmpvalue;
+		ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 8);
+		if (ret < 0) {
+			NVT_ERR("Page Program error!!(%d), i=%d\n", ret, i);
+			return ret;
+		}
+		// Check 0xAA (Page Program)
+		retry = 0;
+		while (1) {
+			msleep(1);
+			buf[0] = 0x00;
+			buf[1] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+			if (ret < 0) {
+				NVT_ERR("Page Program error!!(%d)\n", ret);
+				return ret;
+			}
+			if (buf[1] == 0xAA || buf[1] == 0xEA) {
+				break;
+			}
+			retry++;
+			if (unlikely(retry > 20)) {
+				NVT_ERR("Check 0xAA (Page Program) failed, buf[1]=0x%02X, retry=%d\n", buf[1], retry);
+				return -1;
+			}
+		}
+		if (buf[1] == 0xEA) {
+			NVT_ERR("Page Program error!! i=%d\n", i);
+			return -3;
+		}
+
+		// Read Status
+		retry = 0;
+		while (1) {
+			msleep(5);
+			buf[0] = 0x00;
+			buf[1] = 0x05;
+			ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+			if (ret < 0) {
+				NVT_ERR("Read Status error!!(%d)\n", ret);
+				return ret;
+			}
+
+			// Check 0xAA (Read Status)
+			buf[0] = 0x00;
+			buf[1] = 0x00;
+			buf[2] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 3);
+			if (ret < 0) {
+				NVT_ERR("Check 0xAA (Read Status) error!!(%d)\n", ret);
+				return ret;
+			}
+			if (((buf[1] == 0xAA) && (buf[2] == 0x00)) || (buf[1] == 0xEA)) {
+				break;
+			}
+			retry++;
+			if (unlikely(retry > 100)) {
+				NVT_ERR("Check 0xAA (Read Status) failed, buf[1]=0x%02X, buf[2]=0x%02X, retry=%d\n", buf[1], buf[2], retry);
+				return -1;
+			}
+		}
+		if (buf[1] == 0xEA) {
+			NVT_ERR("Page Program error!! i=%d\n", i);
+			return -4;
+		}
+
+		percent = ((i + 1) * 100) / count;
+		if (((percent % 10) == 0) && (percent != previous_percent)) {
+			NVT_LOG("Programming...%2d%%\n", percent);
+			previous_percent = percent;
+		}
+	}
+
+	NVT_LOG("Program OK         \n");
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen verify checksum of written
+	flash function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+int32_t Verify_Flash(void)
+{
+	uint8_t buf[64] = {0};
+	uint32_t XDATA_Addr = ts->mmap->READ_FLASH_CHECKSUM_ADDR;
+	int32_t ret = 0;
+	int32_t i = 0;
+	int32_t k = 0;
+	uint16_t WR_Filechksum[BLOCK_64KB_NUM] = {0};
+	uint16_t RD_Filechksum[BLOCK_64KB_NUM] = {0};
+	size_t len_in_blk = 0;
+	int32_t retry = 0;
+
+	for (i = 0; i < BLOCK_64KB_NUM; i++) {
+		if (fw_need_write_size > (i * SIZE_64KB)) {
+			// Calculate WR_Filechksum of each 64KB block
+			len_in_blk = min(fw_need_write_size - i * SIZE_64KB, (size_t)SIZE_64KB);
+			WR_Filechksum[i] = i + 0x00 + 0x00 + (((len_in_blk - 1) >> 8) & 0xFF) + ((len_in_blk - 1) & 0xFF);
+			for (k = 0; k < len_in_blk; k++) {
+				WR_Filechksum[i] += fw_entry->data[k + i * SIZE_64KB];
+			}
+			WR_Filechksum[i] = 65535 - WR_Filechksum[i] + 1;
+
+			// Fast Read Command
+			buf[0] = 0x00;
+			buf[1] = 0x07;
+			buf[2] = i;
+			buf[3] = 0x00;
+			buf[4] = 0x00;
+			buf[5] = ((len_in_blk - 1) >> 8) & 0xFF;
+			buf[6] = (len_in_blk - 1) & 0xFF;
+			ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 7);
+			if (ret < 0) {
+				NVT_ERR("Fast Read Command error!!(%d)\n", ret);
+				return ret;
+			}
+			// Check 0xAA (Fast Read Command)
+			retry = 0;
+			while (1) {
+				msleep(80);
+				buf[0] = 0x00;
+				buf[1] = 0x00;
+				ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+				if (ret < 0) {
+					NVT_ERR("Check 0xAA (Fast Read Command) error!!(%d)\n", ret);
+					return ret;
+				}
+				if (buf[1] == 0xAA) {
+					break;
+				}
+				retry++;
+				if (unlikely(retry > 5)) {
+					NVT_ERR("Check 0xAA (Fast Read Command) failed, buf[1]=0x%02X, retry=%d\n", buf[1], retry);
+					return -1;
+				}
+			}
+			// Read Checksum (write addr high byte & middle byte)
+			ret = nvt_set_page(I2C_BLDR_Address, XDATA_Addr);
+			if (ret < 0) {
+				NVT_ERR("Read Checksum (write addr high byte & middle byte) error!!(%d)\n", ret);
+				return ret;
+			}
+			// Read Checksum
+			buf[0] = (XDATA_Addr) & 0xFF;
+			buf[1] = 0x00;
+			buf[2] = 0x00;
+			ret = CTP_I2C_READ(ts->client, I2C_BLDR_Address, buf, 3);
+			if (ret < 0) {
+				NVT_ERR("Read Checksum error!!(%d)\n", ret);
+				return ret;
+			}
+
+			RD_Filechksum[i] = (uint16_t)((buf[2] << 8) | buf[1]);
+			if (WR_Filechksum[i] != RD_Filechksum[i]) {
+				NVT_ERR("Verify Fail%d!!\n", i);
+				NVT_ERR("RD_Filechksum[%d]=0x%04X, WR_Filechksum[%d]=0x%04X\n", i, RD_Filechksum[i], i, WR_Filechksum[i]);
+				return -1;
+			}
+		}
+	}
+
+	NVT_LOG("Verify OK \n");
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen update firmware function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+int32_t Update_Firmware(void)
+{
+	int32_t ret = 0;
+
+	//---Stop CRC check to prevent IC auto reboot---
+	nvt_stop_crc_reboot();
+
+	// Step 1 : initial bootloader
+	ret = Init_BootLoader();
+	if (ret) {
+		return ret;
+	}
+
+	// Step 2 : Resume PD
+	ret = Resume_PD();
+	if (ret) {
+		return ret;
+	}
+
+	// Step 3 : Erase
+	ret = Erase_Flash();
+	if (ret) {
+		return ret;
+	}
+
+	// Step 4 : Program
+	ret = Write_Flash();
+	if (ret) {
+		return ret;
+	}
+
+	// Step 5 : Verify
+	ret = Verify_Flash();
+	if (ret) {
+		return ret;
+	}
+
+	//Step 6 : Bootloader Reset
+	nvt_bootloader_reset();
+	nvt_check_fw_reset_state(RESET_STATE_INIT);
+	nvt_get_fw_info();
+
+	return ret;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen check flash end flag function.
+
+return:
+	Executive outcomes. 0---succeed. 1,negative---failed.
+*******************************************************/
+int32_t nvt_check_flash_end_flag(void)
+{
+	uint8_t buf[8] = {0};
+	uint8_t nvt_end_flag[NVT_FLASH_END_FLAG_LEN + 1] = {0};
+	int32_t ret = 0;
+
+	// Step 1 : initial bootloader
+	ret = Init_BootLoader();
+	if (ret) {
+		return ret;
+	}
+
+	// Step 2 : Resume PD
+	ret = Resume_PD();
+	if (ret) {
+		return ret;
+	}
+
+	// Step 3 : unlock
+	buf[0] = 0x00;
+	buf[1] = 0x35;
+	ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 2);
+	if (ret < 0) {
+		NVT_ERR("write unlock error!!(%d)\n", ret);
+		return ret;
+	}
+	msleep(10);
+
+	//Step 4 : Flash Read Command
+	buf[0] = 0x00;
+	buf[1] = 0x03;
+	buf[2] = (NVT_FLASH_END_FLAG_ADDR >> 16) & 0xFF; //Addr_H
+	buf[3] = (NVT_FLASH_END_FLAG_ADDR >> 8) & 0xFF; //Addr_M
+	buf[4] = NVT_FLASH_END_FLAG_ADDR & 0xFF; //Addr_L
+	buf[5] = (NVT_FLASH_END_FLAG_LEN >> 8) & 0xFF; //Len_H
+	buf[6] = NVT_FLASH_END_FLAG_LEN & 0xFF; //Len_L
+	ret = CTP_I2C_WRITE(ts->client, I2C_HW_Address, buf, 7);
+	if (ret < 0) {
+		NVT_ERR("write Read Command error!!(%d)\n", ret);
+		return ret;
+	}
+	msleep(10);
+
+	// Check 0xAA (Read Command)
+	buf[0] = 0x00;
+	buf[1] = 0x00;
+	ret = CTP_I2C_READ(ts->client, I2C_HW_Address, buf, 2);
+	if (ret < 0) {
+		NVT_ERR("Check 0xAA (Read Command) error!!(%d)\n", ret);
+		return ret;
+	}
+	if (buf[1] != 0xAA) {
+		NVT_ERR("Check 0xAA (Read Command) error!! status=0x%02X\n", buf[1]);
+		return -1;
+	}
+
+	msleep(10);
+
+	//Step 5 : Read Flash Data
+	ret = nvt_set_page(I2C_BLDR_Address, ts->mmap->READ_FLASH_CHECKSUM_ADDR);
+	if (ret < 0) {
+		NVT_ERR("change index error!! (%d)\n", ret);
+		return ret;
+	}
+	msleep(10);
+
+	// Read Back
+	buf[0] = ts->mmap->READ_FLASH_CHECKSUM_ADDR & 0xFF;
+	ret = CTP_I2C_READ(ts->client, I2C_BLDR_Address, buf, 6);
+	if (ret < 0) {
+		NVT_ERR("Read Back error!! (%d)\n", ret);
+		return ret;
+	}
+
+	//buf[3:5] => NVT End Flag
+	strlcpy(nvt_end_flag, &buf[3], sizeof(nvt_end_flag));
+	NVT_LOG("nvt_end_flag=%s (%02X %02X %02X)\n", nvt_end_flag, buf[3], buf[4], buf[5]);
+
+	if ((memcmp(nvt_end_flag, "NVT", NVT_FLASH_END_FLAG_LEN) == 0) ||
+		(memcmp(nvt_end_flag, "MOD", NVT_FLASH_END_FLAG_LEN) == 0)) {
+		return 0;
+	} else {
+		NVT_ERR("\"NVT\" end flag not found!\n");
+		return 1;
+	}
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen update firmware when booting
+	function.
+
+return:
+	n.a.
+*******************************************************/
+void Boot_Update_Firmware(struct work_struct *work)
+{
+	int32_t ret = 0;
+
+	char firmware_name[256] = "";
+
+	snprintf(firmware_name, sizeof(firmware_name),
+			BOOT_UPDATE_FIRMWARE_NAME);
+
+	// request bin file in "/etc/firmware"
+	ret = update_firmware_request(firmware_name);
+	if (ret) {
+		NVT_ERR("update_firmware_request failed. (%d)\n", ret);
+		return;
+	}
+
+	mutex_lock(&ts->lock);
+
+#if NVT_TOUCH_ESD_PROTECT
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	nvt_sw_reset_idle();
+
+	ret = Check_CheckSum();
+
+	if (ret < 0) {	// read firmware checksum failed
+		NVT_ERR("read firmware checksum failed\n");
+		Update_Firmware();
+	} else if ((ret == 0) && (Check_FW_Ver() == 0)) { // (fw checksum not match) && (bin fw version >= ic fw version)
+		NVT_LOG("firmware version not match\n");
+		Update_Firmware();
+	} else if (nvt_check_flash_end_flag()) {
+		NVT_LOG("check flash end flag failed\n");
+		Update_Firmware();
+	} else {
+		// Bootloader Reset
+		nvt_bootloader_reset();
+		ret = nvt_check_fw_reset_state(RESET_STATE_INIT);
+		if (ret) {
+			NVT_LOG("check fw reset state failed\n");
+			Update_Firmware();
+		}
+	}
+
+	mutex_unlock(&ts->lock);
+
+	update_firmware_release();
+}
+#endif /* BOOT_UPDATE_FIRMWARE */
+
+#else  /* NT36XXX_SPI */
+
+#include <linux/firmware.h>
+#include <linux/gpio.h>
+
+#include "nt36xxx.h"
+
+#if NVT_SPI_BOOT_UPDATE_FIRMWARE
+
+#define SIZE_4KB 4096
+#define FLASH_SECTOR_SIZE SIZE_4KB
+#define FW_BIN_VER_OFFSET (nvt_spi_fw_need_write_size - SIZE_4KB)
+#define FW_BIN_VER_BAR_OFFSET (FW_BIN_VER_OFFSET + 1)
+#define NVT_FLASH_END_FLAG_LEN 3
+#define NVT_FLASH_END_FLAG_ADDR (nvt_spi_fw_need_write_size - NVT_FLASH_END_FLAG_LEN)
+
+#define NVT_DUMP_PARTITION      (0)
+#define NVT_DUMP_PARTITION_LEN  (1024)
+#define NVT_DUMP_PARTITION_PATH "/data/local/tmp"
+
+static ktime_t nvt_spi_start, nvt_spi_end;
+static const struct firmware *nvt_spi_fw_entry;
+static size_t nvt_spi_fw_need_write_size;
+static uint8_t *nvt_spi_fwbuf;
+
+struct nvt_spi_bin_map_t {
+	char name[12];
+	uint32_t BIN_addr;
+	uint32_t SRAM_addr;
+	uint32_t size;
+	uint32_t crc;
+};
+
+static struct nvt_spi_bin_map_t *nvt_spi_bin_map;
+
+static int32_t nvt_spi_get_fw_need_write_size(const struct firmware *fw_entry)
+{
+	int32_t i = 0;
+	int32_t total_sectors_to_check = 0;
+
+	total_sectors_to_check = fw_entry->size / FLASH_SECTOR_SIZE;
+	/* printk("total_sectors_to_check = %d\n", total_sectors_to_check); */
+
+	for (i = total_sectors_to_check; i > 0; i--) {
+		// printk("current end flag address checked = 0x%X\n",
+		//			i * FLASH_SECTOR_SIZE - NVT_FLASH_END_FLAG_LEN);
+		/* check if there is end flag "NVT" at the end of this sector */
+		if (memcmp(&fw_entry->data[i * FLASH_SECTOR_SIZE - NVT_FLASH_END_FLAG_LEN],
+			"NVT", NVT_FLASH_END_FLAG_LEN) == 0) {
+			nvt_spi_fw_need_write_size = i * FLASH_SECTOR_SIZE;
+			NVT_LOG("fw_need_write_size = %zu(0x%zx), NVT end flag\n",
+				nvt_spi_fw_need_write_size, nvt_spi_fw_need_write_size);
+			return 0;
+		}
+
+		/* check if there is end flag "MOD" at the end of this sector */
+		if (memcmp(&fw_entry->data[i * FLASH_SECTOR_SIZE - NVT_FLASH_END_FLAG_LEN],
+			"MOD", NVT_FLASH_END_FLAG_LEN) == 0) {
+			nvt_spi_fw_need_write_size = i * FLASH_SECTOR_SIZE;
+			NVT_LOG("fw_need_write_size = %zu(0x%zx), MOD end flag\n",
+				nvt_spi_fw_need_write_size, nvt_spi_fw_need_write_size);
+			return 0;
+		}
+	}
+
+	NVT_ERR("end flag \"NVT\" \"MOD\" not found!\n");
+	return -EINVAL;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen init variable and allocate buffer
+ * for download firmware function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static int32_t nvt_spi_download_init(void)
+{
+	uint8_t *buf;
+	/* allocate buffer for transfer firmware */
+	//NVT_LOG("NVT_TRANSFER_LEN = 0x%06X\n", NVT_SPI_TRANSFER_LEN);
+
+	if (nvt_spi_fwbuf == NULL) {
+		buf = kzalloc((NVT_SPI_TRANSFER_LEN + 1 + NVT_SPI_DUMMY_BYTES), GFP_KERNEL);
+		if (buf == NULL) {
+			NVT_ERR("kzalloc for fwbuf failed!\n");
+			return -ENOMEM;
+		}
+		nvt_spi_fwbuf = buf;
+	}
+
+	return 0;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen checksum function. Calculate bin
+ * file checksum for comparison.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static uint32_t CheckSum(const u8 *data, size_t len)
+{
+	uint32_t i = 0;
+	uint32_t checksum = 0;
+
+	for (i = 0 ; i < len + 1; i++)
+		checksum += data[i];
+
+	checksum += len;
+	checksum = ~checksum + 1;
+
+	return checksum;
+}
+
+static uint32_t byte_to_word(const uint8_t *data)
+{
+	return data[0] + (data[1] << 8) + (data[2] << 16) + (data[3] << 24);
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen parsing bin header function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static uint32_t nvt_spi_partition;
+static uint8_t nvt_spi_ilm_dlm_num = 2;
+static uint8_t nvt_spi_cascade_2nd_header_info;
+static int32_t nvt_spi_bin_header_parser(const u8 *fwdata, size_t fwsize)
+{
+	uint32_t list = 0;
+	uint32_t pos = 0x00;
+	uint32_t end = 0x00;
+	uint8_t info_sec_num = 0;
+	uint8_t ovly_sec_num = 0;
+	uint8_t ovly_info = 0;
+	uint8_t find_bin_header = 0;
+	struct nvt_spi_bin_map_t *bin_map = NULL;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	/* Find the header size */
+	end = fwdata[0] + (fwdata[1] << 8) + (fwdata[2] << 16) + (fwdata[3] << 24);
+
+	/* check cascade next header */
+	nvt_spi_cascade_2nd_header_info = (fwdata[0x20] & 0x02) >> 1;
+	NVT_LOG("cascade_2nd_header_info = %d\n", nvt_spi_cascade_2nd_header_info);
+
+	if (nvt_spi_cascade_2nd_header_info) {
+		pos = 0x30;	// info section start at 0x30 offset
+		while (pos < (end / 2)) {
+			info_sec_num++;
+			pos += 0x10;	/* each header info is 16 bytes */
+		}
+
+		info_sec_num = info_sec_num + 1; //next header section
+	} else {
+		pos = 0x30;	// info section start at 0x30 offset
+		while (pos < end) {
+			info_sec_num++;
+			pos += 0x10;	/* each header info is 16 bytes */
+		}
+	}
+
+	/*
+	 * Find the DLM OVLY section
+	 * [0:3] Overlay Section Number
+	 * [4]   Overlay Info
+	 */
+	ovly_info = (fwdata[0x28] & 0x10) >> 4;
+	ovly_sec_num = (ovly_info) ? (fwdata[0x28] & 0x0F) : 0;
+
+	/*
+	 * calculate all partition number
+	 * ilm_dlm_num (ILM & DLM) + ovly_sec_num + info_sec_num
+	 */
+	nvt_spi_partition = nvt_spi_ilm_dlm_num + ovly_sec_num + info_sec_num;
+	NVT_LOG("ovly_info=%d, ilm_dlm_num=%d, ovly_sec_num=%d, info_sec_num=%d, partition=%d\n",
+		ovly_info, nvt_spi_ilm_dlm_num, ovly_sec_num, info_sec_num, nvt_spi_partition);
+
+
+	/* allocated memory for header info */
+	bin_map = kzalloc((nvt_spi_partition+1) * sizeof(struct nvt_spi_bin_map_t), GFP_KERNEL);
+	if (bin_map == NULL) {
+		NVT_ERR("kzalloc for bin_map failed!\n");
+		return -ENOMEM;
+	}
+	nvt_spi_bin_map = bin_map;
+
+	for (list = 0; list < nvt_spi_partition; list++) {
+		/*
+		 * [1] parsing ILM & DLM header info
+		 * BIN_addr : SRAM_addr : size (12-bytes)
+		 * crc located at 0x18 & 0x1C
+		 */
+		if (list < nvt_spi_ilm_dlm_num) {
+			bin_map[list].BIN_addr = byte_to_word(&fwdata[0 + list * 12]);
+			bin_map[list].SRAM_addr = byte_to_word(&fwdata[4 + list * 12]);
+			bin_map[list].size = byte_to_word(&fwdata[8 + list * 12]);
+			if (ts->hw_crc)
+				bin_map[list].crc = byte_to_word(&fwdata[0x18 + list * 4]);
+			else { //ts->hw_crc
+				if ((bin_map[list].BIN_addr + bin_map[list].size) < fwsize)
+					bin_map[list].crc = CheckSum(
+							&fwdata[bin_map[list].BIN_addr],
+							bin_map[list].size);
+				else {
+					NVT_ERR("access range (0x%08X to 0x%08X) is too large!\n",
+						bin_map[list].BIN_addr,
+						bin_map[list].BIN_addr + bin_map[list].size);
+					return -EINVAL;
+				}
+			} //ts->hw_crc
+			if (list == 0)
+				snprintf(bin_map[list].name, sizeof(bin_map[list].name), "ILM");
+			else if (list == 1)
+				snprintf(bin_map[list].name, sizeof(bin_map[list].name), "DLM");
+		}
+
+		/*
+		 * [2] parsing others header info
+		 * SRAM_addr : size : BIN_addr : crc (16-bytes)
+		 */
+		if ((list >= nvt_spi_ilm_dlm_num)
+			&& (list < (nvt_spi_ilm_dlm_num + info_sec_num))) {
+
+			if (find_bin_header == 0) {
+				/* others partition located at 0x30 offset */
+				pos = 0x30 + (0x10 * (list - nvt_spi_ilm_dlm_num));
+			} else if (find_bin_header && nvt_spi_cascade_2nd_header_info) {
+				/* cascade 2nd header info */
+				pos = end - 0x10;
+			}
+
+			bin_map[list].SRAM_addr = byte_to_word(&fwdata[pos]);
+			bin_map[list].size = byte_to_word(&fwdata[pos + 4]);
+			bin_map[list].BIN_addr = byte_to_word(&fwdata[pos + 8]);
+			if (ts->hw_crc)
+				bin_map[list].crc = byte_to_word(&fwdata[pos + 12]);
+			else { //ts->hw_crc
+				if ((bin_map[list].BIN_addr + bin_map[list].size) < fwsize)
+					bin_map[list].crc = CheckSum(
+							&fwdata[bin_map[list].BIN_addr],
+							bin_map[list].size);
+				else {
+					NVT_ERR("access range (0x%08X to 0x%08X) is too large!\n",
+						bin_map[list].BIN_addr,
+						bin_map[list].BIN_addr + bin_map[list].size);
+					return -EINVAL;
+				}
+			} //ts->hw_crc
+			/* detect header end to protect parser function */
+			if ((bin_map[list].BIN_addr < end) && (bin_map[list].size != 0)) {
+				snprintf(bin_map[list].name, sizeof(bin_map[list].name),
+					"Header");
+				find_bin_header = 1;
+			} else
+				snprintf(bin_map[list].name, sizeof(bin_map[list].name),
+					"Info-%d", (list - nvt_spi_ilm_dlm_num));
+		}
+
+		/*
+		 * [3] parsing overlay section header info
+		 * SRAM_addr : size : BIN_addr : crc (16-bytes)
+		 */
+		if (list >= (nvt_spi_ilm_dlm_num + info_sec_num)) {
+			/* overlay info located at DLM (list = 1) start addr */
+			pos = bin_map[1].BIN_addr;
+			pos += (0x10 * (list - nvt_spi_ilm_dlm_num - info_sec_num));
+
+			bin_map[list].SRAM_addr = byte_to_word(&fwdata[pos]);
+			bin_map[list].size = byte_to_word(&fwdata[pos + 4]);
+			bin_map[list].BIN_addr = byte_to_word(&fwdata[pos + 8]);
+			if (ts->hw_crc)
+				bin_map[list].crc = byte_to_word(&fwdata[pos + 12]);
+			else { //ts->hw_crc
+				if ((bin_map[list].BIN_addr + bin_map[list].size) < fwsize)
+					bin_map[list].crc = CheckSum(
+								&fwdata[bin_map[list].BIN_addr],
+								bin_map[list].size);
+				else {
+					NVT_ERR("access range (0x%08X to 0x%08X) is too large!\n",
+						bin_map[list].BIN_addr,
+						bin_map[list].BIN_addr + bin_map[list].size);
+					return -EINVAL;
+				}
+			} //ts->hw_crc
+			snprintf(bin_map[list].name, sizeof(bin_map[list].name),
+					"Overlay-%d",
+					(list - nvt_spi_ilm_dlm_num - info_sec_num));
+		}
+
+		/* BIN size error detect */
+		if ((bin_map[list].BIN_addr + bin_map[list].size) > fwsize) {
+			NVT_ERR("access range (0x%08X to 0x%08X) is larger than bin size!\n",
+					bin_map[list].BIN_addr,
+					bin_map[list].BIN_addr + bin_map[list].size);
+			return -EINVAL;
+		}
+
+//		NVT_LOG("[%d][%s] SRAM (0x%08X), SIZE (0x%08X), BIN (0x%08X), CRC (0x%08X)\n",
+//				list, bin_map[list].name,
+//				bin_map[list].SRAM_addr, bin_map[list].size,
+//				bin_map[list].BIN_addr, bin_map[list].crc);
+	}
+
+	return 0;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen release update firmware function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void nvt_spi_update_firmware_release(void)
+{
+	if (nvt_spi_fw_entry)
+		release_firmware(nvt_spi_fw_entry);
+
+	nvt_spi_fw_entry = NULL;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen request update firmware function.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. -1,-22---failed.
+ ******************************************************
+ */
+static int32_t nvt_spi_update_firmware_request(char *filename)
+{
+	uint8_t retry = 0;
+	int32_t ret = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+	uint8_t ver;
+
+	if (filename == NULL)
+		return -ENOENT;
+
+	while (1) {
+		NVT_LOG("filename is %s\n", filename);
+
+		ret = request_firmware(&nvt_spi_fw_entry, filename, &ts->client->dev);
+		if (ret) {
+			NVT_ERR("firmware load failed, ret=%d\n", ret);
+			goto request_fail;
+		}
+
+		// check FW need to write size
+		if (nvt_spi_get_fw_need_write_size(nvt_spi_fw_entry)) {
+			NVT_ERR("get fw need to write size fail!\n");
+			ret = -EINVAL;
+			goto invalid;
+		}
+
+		// check if FW version add FW version bar equals 0xFF
+		ver = *(nvt_spi_fw_entry->data + FW_BIN_VER_OFFSET);
+		if (ver + *(nvt_spi_fw_entry->data + FW_BIN_VER_BAR_OFFSET) != 0xFF) {
+			NVT_ERR("bin file FW_VER + FW_VER_BAR should be 0xFF!\n");
+			NVT_ERR("FW_VER=0x%02X, FW_VER_BAR=0x%02X\n",
+					*(nvt_spi_fw_entry->data+FW_BIN_VER_OFFSET),
+					*(nvt_spi_fw_entry->data+FW_BIN_VER_BAR_OFFSET));
+			ret = -ENOEXEC;
+			goto invalid;
+		}
+
+		/* BIN Header Parser */
+		ret = nvt_spi_bin_header_parser(nvt_spi_fw_entry->data, nvt_spi_fw_entry->size);
+		if (ret) {
+			NVT_ERR("bin header parser failed\n");
+			goto invalid;
+		} else
+			break;
+
+invalid:
+		nvt_spi_update_firmware_release();
+		if (!IS_ERR_OR_NULL(nvt_spi_bin_map)) {
+			kfree(nvt_spi_bin_map);
+			nvt_spi_bin_map = NULL;
+		}
+
+request_fail:
+		retry++;
+		if (unlikely(retry > 2)) {
+			NVT_ERR("error, retry=%d\n", retry);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen write data to sram function.
+ *
+ * - fwdata   : The buffer is written
+ * - SRAM_addr: The sram destination address
+ * - size     : Number of data bytes in @fwdata being written
+ * - BIN_addr : The transferred data offset of @fwdata
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. else---fail.
+ *******************************************************
+ */
+static int32_t nvt_spi_write_sram(const u8 *fwdata,
+		uint32_t SRAM_addr, uint32_t size, uint32_t BIN_addr)
+{
+	int32_t ret = 0;
+	uint32_t i = 0;
+	uint16_t len = 0;
+	int32_t count = 0;
+
+	if (size % NVT_SPI_TRANSFER_LEN)
+		count = (size / NVT_SPI_TRANSFER_LEN) + 1;
+	else
+		count = (size / NVT_SPI_TRANSFER_LEN);
+
+	for (i = 0 ; i < count ; i++) {
+		len = (size < NVT_SPI_TRANSFER_LEN) ? size : NVT_SPI_TRANSFER_LEN;
+
+		//---set xdata index to start address of SRAM---
+		ret = nvt_spi_set_page(SRAM_addr);
+		if (ret) {
+			NVT_ERR("set page failed, ret = %d\n", ret);
+			return ret;
+		}
+
+		//---write data into SRAM---
+		nvt_spi_fwbuf[0] = SRAM_addr & 0x7F;	//offset
+		memcpy(nvt_spi_fwbuf+1, &fwdata[BIN_addr], len);	//payload
+		ret = nvt_spi_write(nvt_spi_fwbuf, len+1);
+		if (ret) {
+			NVT_ERR("write to sram failed, ret = %d\n", ret);
+			return ret;
+		}
+
+		SRAM_addr += NVT_SPI_TRANSFER_LEN;
+		BIN_addr += NVT_SPI_TRANSFER_LEN;
+		size -= NVT_SPI_TRANSFER_LEN;
+	}
+
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen nvt_spi_write_firmware function to write
+ *	firmware into each partition.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static int32_t nvt_spi_write_firmware(const u8 *fwdata, size_t fwsize)
+{
+	uint32_t list = 0;
+	char *name;
+	uint32_t BIN_addr, SRAM_addr, size;
+	int32_t ret = 0;
+
+	memset(nvt_spi_fwbuf, 0, (NVT_SPI_TRANSFER_LEN+1));
+
+	for (list = 0; list < nvt_spi_partition; list++) {
+		/* initialize variable */
+		SRAM_addr = nvt_spi_bin_map[list].SRAM_addr;
+		size = nvt_spi_bin_map[list].size;
+		BIN_addr = nvt_spi_bin_map[list].BIN_addr;
+		name = nvt_spi_bin_map[list].name;
+
+//		NVT_LOG("[%d][%s] SRAM (0x%08X), SIZE (0x%08X), BIN (0x%08X)\n",
+//				list, name, SRAM_addr, size, BIN_addr);
+
+		/* Check data size */
+		if ((BIN_addr + size) > fwsize) {
+			NVT_ERR("access range (0x%08X to 0x%08X) is larger than bin size!\n",
+					BIN_addr, BIN_addr + size);
+			ret = -EINVAL;
+			goto out;
+		}
+
+		/* ignore reserved partition (Reserved Partition size is zero) */
+		if (!size)
+			continue;
+		else
+			size = size + 1;
+
+		/* write data to SRAM */
+		ret = nvt_spi_write_sram(fwdata, SRAM_addr, size, BIN_addr);
+		if (ret) {
+			NVT_ERR("sram program failed, ret = %d\n", ret);
+			goto out;
+		}
+	}
+
+out:
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen check checksum function.
+ * This function will compare file checksum and fw checksum.
+ *
+ * return:
+ *	n.a.
+ *******************************************************
+ */
+static int32_t nvt_spi_check_fw_checksum(void)
+{
+	uint32_t fw_checksum = 0;
+	uint32_t len = nvt_spi_partition * 4;
+	uint32_t list = 0;
+	int32_t ret = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	memset(nvt_spi_fwbuf, 0, (len+1));
+
+	//---set xdata index to checksum---
+	nvt_spi_set_page(ts->mmap->R_ILM_CHECKSUM_ADDR);
+
+	/* read checksum */
+	nvt_spi_fwbuf[0] = (ts->mmap->R_ILM_CHECKSUM_ADDR) & 0x7F;
+	ret = nvt_spi_read(nvt_spi_fwbuf, len+1);
+	if (ret) {
+		NVT_ERR("Read fw checksum failed\n");
+		return ret;
+	}
+
+	/*
+	 * Compare each checksum from fw
+	 * ILM + DLM + Overlay + Info
+	 * nvt_spi_ilm_dlm_num (ILM & DLM) + ovly_sec_num + info_sec_num
+	 */
+	for (list = 0; list < nvt_spi_partition; list++) {
+		fw_checksum = byte_to_word(&nvt_spi_fwbuf[1+list*4]);
+
+		/* ignore reserved partition (Reserved Partition size is zero) */
+		if (!nvt_spi_bin_map[list].size)
+			continue;
+
+		if (nvt_spi_bin_map[list].crc != fw_checksum) {
+			NVT_ERR("[%d] BIN_checksum=0x%08X, FW_checksum=0x%08X\n",
+				list, nvt_spi_bin_map[list].crc, fw_checksum);
+			ret = -EIO;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen set bootload crc reg bank function.
+ * This function will set hw crc reg before enable crc function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void nvt_spi_set_bld_crc_bank(uint32_t DES_ADDR, uint32_t SRAM_ADDR,
+		uint32_t LENGTH_ADDR, uint32_t size,
+		uint32_t G_CHECKSUM_ADDR, uint32_t crc)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	/* write destination address */
+	nvt_spi_set_page(DES_ADDR);
+	nvt_spi_fwbuf[0] = DES_ADDR & 0x7F;
+	nvt_spi_fwbuf[1] = (SRAM_ADDR) & 0xFF;
+	nvt_spi_fwbuf[2] = (SRAM_ADDR >> 8) & 0xFF;
+	nvt_spi_fwbuf[3] = (SRAM_ADDR >> 16) & 0xFF;
+	nvt_spi_write(nvt_spi_fwbuf, 4);
+
+	/* write length */
+	//nvt_spi_set_page(LENGTH_ADDR);
+	nvt_spi_fwbuf[0] = LENGTH_ADDR & 0x7F;
+	nvt_spi_fwbuf[1] = (size) & 0xFF;
+	nvt_spi_fwbuf[2] = (size >> 8) & 0xFF;
+	nvt_spi_fwbuf[3] = (size >> 16) & 0x01;
+	if (ts->hw_crc == 1)
+		nvt_spi_write(nvt_spi_fwbuf, 3);
+	else if (ts->hw_crc > 1)
+		nvt_spi_write(nvt_spi_fwbuf, 4);
+
+	/* write golden dlm checksum */
+	//nvt_spi_set_page(G_CHECKSUM_ADDR);
+	nvt_spi_fwbuf[0] = G_CHECKSUM_ADDR & 0x7F;
+	nvt_spi_fwbuf[1] = (crc) & 0xFF;
+	nvt_spi_fwbuf[2] = (crc >> 8) & 0xFF;
+	nvt_spi_fwbuf[3] = (crc >> 16) & 0xFF;
+	nvt_spi_fwbuf[4] = (crc >> 24) & 0xFF;
+	nvt_spi_write(nvt_spi_fwbuf, 5);
+}
+
+/*
+ *********************************************************
+ * Description:
+ *	Novatek touchscreen set BLD hw crc function.
+ * This function will set ILM and DLM crc information to register.
+ *
+ * return:
+ *	n.a.
+ *********************************************************
+ */
+static void nvt_spi_set_bld_hw_crc(void)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	/* [0] ILM */
+	/* write register bank */
+	nvt_spi_set_bld_crc_bank(ts->mmap->ILM_DES_ADDR, nvt_spi_bin_map[0].SRAM_addr,
+			ts->mmap->ILM_LENGTH_ADDR, nvt_spi_bin_map[0].size,
+			ts->mmap->G_ILM_CHECKSUM_ADDR, nvt_spi_bin_map[0].crc);
+
+	/* [1] DLM */
+	/* write register bank */
+	nvt_spi_set_bld_crc_bank(ts->mmap->DLM_DES_ADDR, nvt_spi_bin_map[1].SRAM_addr,
+			ts->mmap->DLM_LENGTH_ADDR, nvt_spi_bin_map[1].size,
+			ts->mmap->G_DLM_CHECKSUM_ADDR, nvt_spi_bin_map[1].crc);
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen read BLD hw crc info function.
+ * This function will check crc results from register.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static void nvt_spi_read_bld_hw_crc(void)
+{
+	uint8_t buf[8] = {0};
+	uint32_t g_crc = 0, r_crc = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	/* CRC Flag */
+	nvt_spi_set_page(ts->mmap->BLD_ILM_DLM_CRC_ADDR);
+	buf[0] = ts->mmap->BLD_ILM_DLM_CRC_ADDR & 0x7F;
+	buf[1] = 0x00;
+	nvt_spi_read(buf, 2);
+	NVT_ERR("crc_done = %d, ilm_crc_flag = %d, dlm_crc_flag = %d\n",
+			(buf[1] >> 2) & 0x01, (buf[1] >> 0) & 0x01, (buf[1] >> 1) & 0x01);
+
+	/* ILM CRC */
+	nvt_spi_set_page(ts->mmap->G_ILM_CHECKSUM_ADDR);
+	buf[0] = ts->mmap->G_ILM_CHECKSUM_ADDR & 0x7F;
+	buf[1] = 0x00;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+	buf[4] = 0x00;
+	nvt_spi_read(buf, 5);
+	g_crc = buf[1] | (buf[2] << 8) | (buf[3] << 16) | (buf[4] << 24);
+
+	nvt_spi_set_page(ts->mmap->R_ILM_CHECKSUM_ADDR);
+	buf[0] = ts->mmap->R_ILM_CHECKSUM_ADDR & 0x7F;
+	buf[1] = 0x00;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+	buf[4] = 0x00;
+	nvt_spi_read(buf, 5);
+	r_crc = buf[1] | (buf[2] << 8) | (buf[3] << 16) | (buf[4] << 24);
+
+	NVT_ERR("ilm: bin crc = 0x%08X, golden = 0x%08X, result = 0x%08X\n",
+			nvt_spi_bin_map[0].crc, g_crc, r_crc);
+
+	/* DLM CRC */
+	nvt_spi_set_page(ts->mmap->G_DLM_CHECKSUM_ADDR);
+	buf[0] = ts->mmap->G_DLM_CHECKSUM_ADDR & 0x7F;
+	buf[1] = 0x00;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+	buf[4] = 0x00;
+	nvt_spi_read(buf, 5);
+	g_crc = buf[1] | (buf[2] << 8) | (buf[3] << 16) | (buf[4] << 24);
+
+	nvt_spi_set_page(ts->mmap->R_DLM_CHECKSUM_ADDR);
+	buf[0] = ts->mmap->R_DLM_CHECKSUM_ADDR & 0x7F;
+	buf[1] = 0x00;
+	buf[2] = 0x00;
+	buf[3] = 0x00;
+	buf[4] = 0x00;
+	nvt_spi_read(buf, 5);
+	r_crc = buf[1] | (buf[2] << 8) | (buf[3] << 16) | (buf[4] << 24);
+
+	NVT_ERR("dlm: bin crc = 0x%08X, golden = 0x%08X, result = 0x%08X\n",
+			nvt_spi_bin_map[1].crc, g_crc, r_crc);
+
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen Download_Firmware with HW CRC
+ * function. It's complete download firmware flow.
+ *
+ * return:
+ *	Executive outcomes. 0---succeed. else---fail.
+ ******************************************************
+ */
+static int32_t nvt_spi_download_firmware_hw_crc(void)
+{
+	uint8_t retry = 0;
+	int32_t ret = 0;
+	const struct firmware *fw = nvt_spi_fw_entry;
+
+	nvt_spi_start = ktime_get();
+
+	while (1) {
+		/* bootloader reset to reset MCU */
+		nvt_spi_bootloader_reset();
+
+		/* set ilm & dlm reg bank */
+		nvt_spi_set_bld_hw_crc();
+
+		/* Start to write firmware process */
+		if (nvt_spi_cascade_2nd_header_info) {
+			/* for cascade */
+			nvt_spi_tx_auto_copy_mode();
+
+			ret = nvt_spi_write_firmware(fw->data, fw->size);
+			if (ret) {
+				NVT_ERR("Write_Firmware failed. (%d)\n", ret);
+				goto fail;
+			}
+
+			ret = nvt_spi_check_spi_dma_tx_info();
+			if (ret) {
+				NVT_ERR("spi dma tx info failed. (%d)\n", ret);
+				goto fail;
+			}
+		} else {
+			ret = nvt_spi_write_firmware(fw->data, fw->size);
+			if (ret) {
+				NVT_ERR("Write_Firmware failed. (%d)\n", ret);
+				goto fail;
+			}
+		}
+
+#if NVT_DUMP_PARTITION
+		ret = nvt_dump_partition();
+		if (ret)
+			NVT_ERR("nvt_dump_partition failed, ret = %d\n", ret);
+#endif
+
+		/* enable hw bld crc function */
+		nvt_spi_bld_crc_enable();
+
+		/* clear fw reset status & enable fw crc check */
+		nvt_spi_fw_crc_enable();
+
+		/* Set Boot Ready Bit */
+		nvt_spi_boot_ready();
+
+		ret = nvt_spi_check_fw_reset_state(NVT_SPI_RESET_STATE_INIT);
+		if (ret) {
+			NVT_ERR("nvt_check_fw_reset_state failed. (%d)\n", ret);
+			goto fail;
+		} else {
+			break;
+		}
+
+fail:
+		retry++;
+		if (unlikely(retry > 2)) {
+			NVT_ERR("error, retry=%d\n", retry);
+			nvt_spi_read_bld_hw_crc();
+			break;
+		}
+	}
+
+	nvt_spi_end = ktime_get();
+
+	return ret;
+}
+
+/*
+ ********************************************************
+ * Description:
+ *	Novatek touchscreen Download_Firmware function. It's
+ * complete download firmware flow.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+static int32_t nvt_spi_download_firmware(void)
+{
+	uint32_t addr;
+	uint8_t retry = 0;
+	int32_t ret = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	nvt_spi_start = ktime_get();
+
+	while (1) {
+		/*
+		 * Send eng reset cmd before download FW
+		 * Keep TP_RESX low when send eng reset cmd
+		 */
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+		gpio_set_value(ts->reset_gpio, 0);
+		mdelay(1);	//wait 1ms
+#endif
+		nvt_spi_eng_reset();
+#if NVT_SPI_TOUCH_SUPPORT_HW_RST
+		gpio_set_value(ts->reset_gpio, 1);
+		mdelay(10);	//wait tRT2BRST after TP_RST
+#endif
+		nvt_spi_bootloader_reset();
+
+		addr = ts->mmap->EVENT_BUF_ADDR;
+		/* clear fw reset status */
+		nvt_spi_write_addr(addr | NVT_SPI_EVENT_MAP_RESET_COMPLETE, 0x00);
+
+		/* Start to write firmware process */
+		ret = nvt_spi_write_firmware(nvt_spi_fw_entry->data, nvt_spi_fw_entry->size);
+		if (ret) {
+			NVT_ERR("Write_Firmware failed. (%d)\n", ret);
+			goto fail;
+		}
+
+#if NVT_DUMP_PARTITION
+		ret = nvt_dump_partition();
+		if (ret)
+			NVT_ERR("nvt_dump_partition failed, ret = %d\n", ret);
+#endif
+
+		/* Set Boot Ready Bit */
+		nvt_spi_boot_ready();
+
+		ret = nvt_spi_check_fw_reset_state(NVT_SPI_RESET_STATE_INIT);
+		if (ret) {
+			NVT_ERR("nvt_check_fw_reset_state failed. (%d)\n", ret);
+			goto fail;
+		}
+
+		/* check fw checksum result */
+		ret = nvt_spi_check_fw_checksum();
+		if (ret) {
+			NVT_ERR("firmware checksum not match, retry=%d\n", retry);
+			goto fail;
+		} else
+			break;
+
+fail:
+		retry++;
+		if (unlikely(retry > 2)) {
+			NVT_ERR("error, retry=%d\n", retry);
+			break;
+		}
+	}
+
+	nvt_spi_end = ktime_get();
+
+	return ret;
+}
+
+/*
+ ******************************************************
+ * Description:
+ *	Novatek touchscreen update firmware main function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+int32_t nvt_spi_update_firmware(char *firmware_name)
+{
+	int32_t ret = 0;
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	// request bin file in "/etc/firmware"
+	ret = nvt_spi_update_firmware_request(firmware_name);
+	if (ret) {
+		NVT_ERR("update_firmware_request failed. (%d)\n", ret);
+		goto request_firmware_fail;
+	}
+
+	/* initial buffer and variable */
+	ret = nvt_spi_download_init();
+	if (ret) {
+		NVT_ERR("Download Init failed. (%d)\n", ret);
+		goto download_fail;
+	}
+
+	/* download firmware process */
+	if (ts->hw_crc)
+		ret = nvt_spi_download_firmware_hw_crc();
+	else
+		ret = nvt_spi_download_firmware();
+	if (ret) {
+		NVT_ERR("Download Firmware failed. (%d)\n", ret);
+		goto download_fail;
+	}
+
+	NVT_LOG("Update firmware success! <%ld us>\n",
+			(long) ktime_us_delta(nvt_spi_end, nvt_spi_start));
+
+	/* Get FW Info */
+	ret = nvt_spi_get_fw_info();
+	if (ret)
+		NVT_ERR("nvt_get_fw_info failed. (%d)\n", ret);
+
+download_fail:
+	if (!IS_ERR_OR_NULL(nvt_spi_bin_map)) {
+		kfree(nvt_spi_bin_map);
+		nvt_spi_bin_map = NULL;
+	}
+
+	nvt_spi_update_firmware_release();
+request_firmware_fail:
+
+	return ret;
+}
+
+/*
+ *******************************************************
+ * Description:
+ *	Novatek touchscreen update firmware when booting
+ *	function.
+ *
+ * return:
+ *	n.a.
+ ******************************************************
+ */
+void nvt_spi_update_firmware_work(struct work_struct *work)
+{
+	struct nvt_spi_data_t *ts = nvt_spi_data;
+
+	mutex_lock(&ts->lock);
+	nvt_spi_update_firmware(NVT_SPI_BOOT_UPDATE_FIRMWARE_NAME);
+	mutex_unlock(&ts->lock);
+}
+#endif
+
+#endif

nt36xxx/nt36xxx_mem_map.h

@@ -0,0 +1,607 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2010 - 2018 Novatek, Inc.
+ *
+ * $Revision: 48764 $
+ * $Date: 2019-08-08 14:52:12 +0800 (Thu, 08 Aug 2019) $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+#define CHIP_VER_TRIM_ADDR 0x3F004
+#define CHIP_VER_TRIM_OLD_ADDR 0x1F64E
+
+#if !defined(NVT_NT36XXX_SPI) /* NT36XXX I2C */
+
+struct nvt_ts_mem_map {
+	uint32_t EVENT_BUF_ADDR;
+	uint32_t RAW_PIPE0_ADDR;
+	uint32_t RAW_PIPE1_ADDR;
+	uint32_t BASELINE_ADDR;
+	uint32_t BASELINE_BTN_ADDR;
+	uint32_t DIFF_PIPE0_ADDR;
+	uint32_t DIFF_PIPE1_ADDR;
+	uint32_t RAW_BTN_PIPE0_ADDR;
+	uint32_t RAW_BTN_PIPE1_ADDR;
+	uint32_t DIFF_BTN_PIPE0_ADDR;
+	uint32_t DIFF_BTN_PIPE1_ADDR;
+	uint32_t READ_FLASH_CHECKSUM_ADDR;
+	uint32_t RW_FLASH_DATA_ADDR;
+};
+
+struct nvt_ts_hw_info {
+	uint8_t carrier_system;
+	uint8_t hw_crc;
+};
+
+static const struct nvt_ts_mem_map NT36526_memory_map = {
+	.EVENT_BUF_ADDR           = 0x22D00,
+	.RAW_PIPE0_ADDR           = 0x24000,
+	.RAW_PIPE1_ADDR           = 0x24000,
+	.BASELINE_ADDR            = 0x21758,
+	.BASELINE_BTN_ADDR        = 0,
+	.DIFF_PIPE0_ADDR          = 0x20AB0,
+	.DIFF_PIPE1_ADDR          = 0x24AB0,
+	.RAW_BTN_PIPE0_ADDR       = 0,
+	.RAW_BTN_PIPE1_ADDR       = 0,
+	.DIFF_BTN_PIPE0_ADDR      = 0,
+	.DIFF_BTN_PIPE1_ADDR      = 0,
+	.READ_FLASH_CHECKSUM_ADDR = 0x24000,
+	.RW_FLASH_DATA_ADDR       = 0x24002,
+};
+
+static const struct nvt_ts_mem_map NT36675_memory_map = {
+	.EVENT_BUF_ADDR           = 0x22D00,
+	.RAW_PIPE0_ADDR           = 0x24000,
+	.RAW_PIPE1_ADDR           = 0x24000,
+	.BASELINE_ADDR            = 0x21B90,
+	.BASELINE_BTN_ADDR        = 0,
+	.DIFF_PIPE0_ADDR          = 0x20C60,
+	.DIFF_PIPE1_ADDR          = 0x24C60,
+	.RAW_BTN_PIPE0_ADDR       = 0,
+	.RAW_BTN_PIPE1_ADDR       = 0,
+	.DIFF_BTN_PIPE0_ADDR      = 0,
+	.DIFF_BTN_PIPE1_ADDR      = 0,
+	.READ_FLASH_CHECKSUM_ADDR = 0x24000,
+	.RW_FLASH_DATA_ADDR       = 0x24002,
+};
+
+static const struct nvt_ts_mem_map NT36672A_memory_map = {
+	.EVENT_BUF_ADDR           = 0x21C00,
+	.RAW_PIPE0_ADDR           = 0x20000,
+	.RAW_PIPE1_ADDR           = 0x23000,
+	.BASELINE_ADDR            = 0x20BFC,
+	.BASELINE_BTN_ADDR        = 0x23BFC,
+	.DIFF_PIPE0_ADDR          = 0x206DC,
+	.DIFF_PIPE1_ADDR          = 0x236DC,
+	.RAW_BTN_PIPE0_ADDR       = 0x20510,
+	.RAW_BTN_PIPE1_ADDR       = 0x23510,
+	.DIFF_BTN_PIPE0_ADDR      = 0x20BF0,
+	.DIFF_BTN_PIPE1_ADDR      = 0x23BF0,
+	.READ_FLASH_CHECKSUM_ADDR = 0x24000,
+	.RW_FLASH_DATA_ADDR       = 0x24002,
+};
+
+static const struct nvt_ts_mem_map NT36772_memory_map = {
+	.EVENT_BUF_ADDR           = 0x11E00,
+	.RAW_PIPE0_ADDR           = 0x10000,
+	.RAW_PIPE1_ADDR           = 0x12000,
+	.BASELINE_ADDR            = 0x10E70,
+	.BASELINE_BTN_ADDR        = 0x12E70,
+	.DIFF_PIPE0_ADDR          = 0x10830,
+	.DIFF_PIPE1_ADDR          = 0x12830,
+	.RAW_BTN_PIPE0_ADDR       = 0x10E60,
+	.RAW_BTN_PIPE1_ADDR       = 0x12E60,
+	.DIFF_BTN_PIPE0_ADDR      = 0x10E68,
+	.DIFF_BTN_PIPE1_ADDR      = 0x12E68,
+	.READ_FLASH_CHECKSUM_ADDR = 0x14000,
+	.RW_FLASH_DATA_ADDR       = 0x14002,
+};
+
+static const struct nvt_ts_mem_map NT36525_memory_map = {
+	.EVENT_BUF_ADDR           = 0x11A00,
+	.RAW_PIPE0_ADDR           = 0x10000,
+	.RAW_PIPE1_ADDR           = 0x12000,
+	.BASELINE_ADDR            = 0x10B08,
+	.BASELINE_BTN_ADDR        = 0x12B08,
+	.DIFF_PIPE0_ADDR          = 0x1064C,
+	.DIFF_PIPE1_ADDR          = 0x1264C,
+	.RAW_BTN_PIPE0_ADDR       = 0x10634,
+	.RAW_BTN_PIPE1_ADDR       = 0x12634,
+	.DIFF_BTN_PIPE0_ADDR      = 0x10AFC,
+	.DIFF_BTN_PIPE1_ADDR      = 0x12AFC,
+	.READ_FLASH_CHECKSUM_ADDR = 0x14000,
+	.RW_FLASH_DATA_ADDR       = 0x14002,
+};
+
+static const struct nvt_ts_mem_map NT36676F_memory_map = {
+	.EVENT_BUF_ADDR           = 0x11A00,
+	.RAW_PIPE0_ADDR           = 0x10000,
+	.RAW_PIPE1_ADDR           = 0x12000,
+	.BASELINE_ADDR            = 0x10B08,
+	.BASELINE_BTN_ADDR        = 0x12B08,
+	.DIFF_PIPE0_ADDR          = 0x1064C,
+	.DIFF_PIPE1_ADDR          = 0x1264C,
+	.RAW_BTN_PIPE0_ADDR       = 0x10634,
+	.RAW_BTN_PIPE1_ADDR       = 0x12634,
+	.DIFF_BTN_PIPE0_ADDR      = 0x10AFC,
+	.DIFF_BTN_PIPE1_ADDR      = 0x12AFC,
+	.READ_FLASH_CHECKSUM_ADDR = 0x14000,
+	.RW_FLASH_DATA_ADDR       = 0x14002,
+};
+
+static struct nvt_ts_hw_info NT36526_hw_info = {
+	.carrier_system = 2,
+	.hw_crc         = 2,
+};
+
+static struct nvt_ts_hw_info NT36675_hw_info = {
+	.carrier_system = 2,
+	.hw_crc         = 2,
+};
+
+static struct nvt_ts_hw_info NT36672A_hw_info = {
+	.carrier_system = 0,
+	.hw_crc         = 1,
+};
+
+static struct nvt_ts_hw_info NT36772_hw_info = {
+	.carrier_system = 0,
+	.hw_crc         = 0,
+};
+
+static struct nvt_ts_hw_info NT36525_hw_info = {
+	.carrier_system = 0,
+	.hw_crc         = 0,
+};
+
+static struct nvt_ts_hw_info NT36676F_hw_info = {
+	.carrier_system = 0,
+	.hw_crc         = 0,
+};
+
+#define NVT_ID_BYTE_MAX 6
+struct nvt_ts_trim_id_table {
+	uint8_t id[NVT_ID_BYTE_MAX];
+	uint8_t mask[NVT_ID_BYTE_MAX];
+	const struct nvt_ts_mem_map *mmap;
+	const struct nvt_ts_hw_info *hwinfo;
+};
+
+static const struct nvt_ts_trim_id_table trim_id_table[] = {
+	{.id = {0x20, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x00, 0xFF, 0xFF, 0x80, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x0C, 0xFF, 0xFF, 0x25, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0E, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x0C, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x26, 0x65, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36526_memory_map,  .hwinfo = &NT36526_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x75, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x82, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x25, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x72, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x82, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x70, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x70, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x72, 0x67, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x55, 0x00, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0x55, 0x72, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xAA, 0x00, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xAA, 0x72, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x72, 0x67, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x70, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x70, 0x67, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x25, 0x65, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36525_memory_map,  .hwinfo = &NT36525_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x76, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36676F_memory_map, .hwinfo = &NT36676F_hw_info}
+};
+
+#else  /* NT36XXX_SPI */
+
+#define NVT_SPI_CHIP_VER_TRIM_ADDR 0x3F004
+#define NVT_SPI_CHIP_VER_TRIM_OLD_ADDR 0x1F64E
+
+struct nvt_spi_mem_map {
+	uint32_t EVENT_BUF_ADDR;
+	uint32_t RAW_PIPE0_ADDR;
+	uint32_t RAW_PIPE1_ADDR;
+	uint32_t BASELINE_ADDR;
+	uint32_t BASELINE_BTN_ADDR;
+	uint32_t DIFF_PIPE0_ADDR;
+	uint32_t DIFF_PIPE1_ADDR;
+	uint32_t RAW_BTN_PIPE0_ADDR;
+	uint32_t RAW_BTN_PIPE1_ADDR;
+	uint32_t DIFF_BTN_PIPE0_ADDR;
+	uint32_t DIFF_BTN_PIPE1_ADDR;
+	uint32_t PEN_2D_BL_TIP_X_ADDR;
+	uint32_t PEN_2D_BL_TIP_Y_ADDR;
+	uint32_t PEN_2D_BL_RING_X_ADDR;
+	uint32_t PEN_2D_BL_RING_Y_ADDR;
+	uint32_t PEN_2D_DIFF_TIP_X_ADDR;
+	uint32_t PEN_2D_DIFF_TIP_Y_ADDR;
+	uint32_t PEN_2D_DIFF_RING_X_ADDR;
+	uint32_t PEN_2D_DIFF_RING_Y_ADDR;
+	uint32_t PEN_2D_RAW_TIP_X_ADDR;
+	uint32_t PEN_2D_RAW_TIP_Y_ADDR;
+	uint32_t PEN_2D_RAW_RING_X_ADDR;
+	uint32_t PEN_2D_RAW_RING_Y_ADDR;
+	uint32_t PEN_1D_DIFF_TIP_X_ADDR;
+	uint32_t PEN_1D_DIFF_TIP_Y_ADDR;
+	uint32_t PEN_1D_DIFF_RING_X_ADDR;
+	uint32_t PEN_1D_DIFF_RING_Y_ADDR;
+	uint32_t READ_FLASH_CHECKSUM_ADDR;
+	uint32_t RW_FLASH_DATA_ADDR;
+	/* Phase 2 Host Download */
+	uint32_t BOOT_RDY_ADDR;
+	uint32_t POR_CD_ADDR;
+	uint32_t TX_AUTO_COPY_EN;
+	uint32_t SPI_DMA_TX_INFO;
+	/* BLD CRC */
+	uint32_t BLD_LENGTH_ADDR;
+	uint32_t ILM_LENGTH_ADDR;
+	uint32_t DLM_LENGTH_ADDR;
+	uint32_t BLD_DES_ADDR;
+	uint32_t ILM_DES_ADDR;
+	uint32_t DLM_DES_ADDR;
+	uint32_t G_ILM_CHECKSUM_ADDR;
+	uint32_t G_DLM_CHECKSUM_ADDR;
+	uint32_t R_ILM_CHECKSUM_ADDR;
+	uint32_t R_DLM_CHECKSUM_ADDR;
+	uint32_t BLD_CRC_EN_ADDR;
+	uint32_t DMA_CRC_EN_ADDR;
+	uint32_t BLD_ILM_DLM_CRC_ADDR;
+	uint32_t DMA_CRC_FLAG_ADDR;
+};
+
+struct nvt_spi_hw_info {
+	uint8_t hw_crc;
+};
+
+static const struct nvt_spi_mem_map NT36523_memory_map = {
+	.EVENT_BUF_ADDR           = 0x2FE00,
+	.RAW_PIPE0_ADDR           = 0x30FA0,
+	.RAW_PIPE1_ADDR           = 0x30FA0,
+	.BASELINE_ADDR            = 0x36510,
+	.BASELINE_BTN_ADDR        = 0,
+	.DIFF_PIPE0_ADDR          = 0x373E8,
+	.DIFF_PIPE1_ADDR          = 0x38068,
+	.RAW_BTN_PIPE0_ADDR       = 0,
+	.RAW_BTN_PIPE1_ADDR       = 0,
+	.DIFF_BTN_PIPE0_ADDR      = 0,
+	.DIFF_BTN_PIPE1_ADDR      = 0,
+	.PEN_2D_BL_TIP_X_ADDR     = 0x2988A,
+	.PEN_2D_BL_TIP_Y_ADDR     = 0x29A1A,
+	.PEN_2D_BL_RING_X_ADDR    = 0x29BAA,
+	.PEN_2D_BL_RING_Y_ADDR    = 0x29D3A,
+	.PEN_2D_DIFF_TIP_X_ADDR   = 0x29ECA,
+	.PEN_2D_DIFF_TIP_Y_ADDR   = 0x2A05A,
+	.PEN_2D_DIFF_RING_X_ADDR  = 0x2A1EA,
+	.PEN_2D_DIFF_RING_Y_ADDR  = 0x2A37A,
+	.PEN_2D_RAW_TIP_X_ADDR    = 0x2A50A,
+	.PEN_2D_RAW_TIP_Y_ADDR    = 0x2A69A,
+	.PEN_2D_RAW_RING_X_ADDR   = 0x2A82A,
+	.PEN_2D_RAW_RING_Y_ADDR   = 0x2A9BA,
+	.PEN_1D_DIFF_TIP_X_ADDR   = 0x2AB4A,
+	.PEN_1D_DIFF_TIP_Y_ADDR   = 0x2ABAE,
+	.PEN_1D_DIFF_RING_X_ADDR  = 0x2AC12,
+	.PEN_1D_DIFF_RING_Y_ADDR  = 0x2AC76,
+	.READ_FLASH_CHECKSUM_ADDR = 0x24000,
+	.RW_FLASH_DATA_ADDR       = 0x24002,
+	/* Phase 2 Host Download */
+	.BOOT_RDY_ADDR            = 0x3F10D,
+	.TX_AUTO_COPY_EN          = 0x3F7E8,
+	.SPI_DMA_TX_INFO          = 0x3F7F1,
+	/* BLD CRC */
+	.BLD_LENGTH_ADDR          = 0x3F138,	//0x3F138 ~ 0x3F13A	(3 bytes)
+	.ILM_LENGTH_ADDR          = 0x3F118,	//0x3F118 ~ 0x3F11A	(3 bytes)
+	.DLM_LENGTH_ADDR          = 0x3F130,	//0x3F130 ~ 0x3F132	(3 bytes)
+	.BLD_DES_ADDR             = 0x3F114,	//0x3F114 ~ 0x3F116	(3 bytes)
+	.ILM_DES_ADDR             = 0x3F128,	//0x3F128 ~ 0x3F12A	(3 bytes)
+	.DLM_DES_ADDR             = 0x3F12C,	//0x3F12C ~ 0x3F12E	(3 bytes)
+	.G_ILM_CHECKSUM_ADDR      = 0x3F100,	//0x3F100 ~ 0x3F103	(4 bytes)
+	.G_DLM_CHECKSUM_ADDR      = 0x3F104,	//0x3F104 ~ 0x3F107	(4 bytes)
+	.R_ILM_CHECKSUM_ADDR      = 0x3F120,	//0x3F120 ~ 0x3F123 (4 bytes)
+	.R_DLM_CHECKSUM_ADDR      = 0x3F124,	//0x3F124 ~ 0x3F127 (4 bytes)
+	.BLD_CRC_EN_ADDR          = 0x3F30E,
+	.DMA_CRC_EN_ADDR          = 0x3F136,
+	.BLD_ILM_DLM_CRC_ADDR     = 0x3F133,
+	.DMA_CRC_FLAG_ADDR        = 0x3F134,
+};
+
+static const struct nvt_spi_mem_map NT36526_memory_map = {
+	.EVENT_BUF_ADDR           = 0x22D00,
+	.RAW_PIPE0_ADDR           = 0x24000,
+	.RAW_PIPE1_ADDR           = 0x24000,
+	.BASELINE_ADDR            = 0x21758,
+	.BASELINE_BTN_ADDR        = 0,
+	.DIFF_PIPE0_ADDR          = 0x20AB0,
+	.DIFF_PIPE1_ADDR          = 0x24AB0,
+	.RAW_BTN_PIPE0_ADDR       = 0,
+	.RAW_BTN_PIPE1_ADDR       = 0,
+	.DIFF_BTN_PIPE0_ADDR      = 0,
+	.DIFF_BTN_PIPE1_ADDR      = 0,
+	.READ_FLASH_CHECKSUM_ADDR = 0x24000,
+	.RW_FLASH_DATA_ADDR       = 0x24002,
+	/* Phase 2 Host Download */
+	.BOOT_RDY_ADDR            = 0x3F10D,
+	/* BLD CRC */
+	.BLD_LENGTH_ADDR          = 0x3F138,	//0x3F138 ~ 0x3F13A	(3 bytes)
+	.ILM_LENGTH_ADDR          = 0x3F118,	//0x3F118 ~ 0x3F11A	(3 bytes)
+	.DLM_LENGTH_ADDR          = 0x3F130,	//0x3F130 ~ 0x3F132	(3 bytes)
+	.BLD_DES_ADDR             = 0x3F114,	//0x3F114 ~ 0x3F116	(3 bytes)
+	.ILM_DES_ADDR             = 0x3F128,	//0x3F128 ~ 0x3F12A	(3 bytes)
+	.DLM_DES_ADDR             = 0x3F12C,	//0x3F12C ~ 0x3F12E	(3 bytes)
+	.G_ILM_CHECKSUM_ADDR      = 0x3F100,	//0x3F100 ~ 0x3F103	(4 bytes)
+	.G_DLM_CHECKSUM_ADDR      = 0x3F104,	//0x3F104 ~ 0x3F107	(4 bytes)
+	.R_ILM_CHECKSUM_ADDR      = 0x3F120,	//0x3F120 ~ 0x3F123 (4 bytes)
+	.R_DLM_CHECKSUM_ADDR      = 0x3F124,	//0x3F124 ~ 0x3F127 (4 bytes)
+	.BLD_CRC_EN_ADDR          = 0x3F30E,
+	.DMA_CRC_EN_ADDR          = 0x3F136,
+	.BLD_ILM_DLM_CRC_ADDR     = 0x3F133,
+	.DMA_CRC_FLAG_ADDR        = 0x3F134,
+};
+
+
+static const struct nvt_spi_mem_map NT36675_memory_map = {
+	.EVENT_BUF_ADDR           = 0x22D00,
+	.RAW_PIPE0_ADDR           = 0x24000,
+	.RAW_PIPE1_ADDR           = 0x24000,
+	.BASELINE_ADDR            = 0x21B90,
+	.BASELINE_BTN_ADDR        = 0,
+	.DIFF_PIPE0_ADDR          = 0x20C60,
+	.DIFF_PIPE1_ADDR          = 0x24C60,
+	.RAW_BTN_PIPE0_ADDR       = 0,
+	.RAW_BTN_PIPE1_ADDR       = 0,
+	.DIFF_BTN_PIPE0_ADDR      = 0,
+	.DIFF_BTN_PIPE1_ADDR      = 0,
+	.READ_FLASH_CHECKSUM_ADDR = 0x24000,
+	.RW_FLASH_DATA_ADDR       = 0x24002,
+	/* Phase 2 Host Download */
+	.BOOT_RDY_ADDR            = 0x3F10D,
+	/* BLD CRC */
+	.BLD_LENGTH_ADDR          = 0x3F138,	//0x3F138 ~ 0x3F13A	(3 bytes)
+	.ILM_LENGTH_ADDR          = 0x3F118,	//0x3F118 ~ 0x3F11A	(3 bytes)
+	.DLM_LENGTH_ADDR          = 0x3F130,	//0x3F130 ~ 0x3F132	(3 bytes)
+	.BLD_DES_ADDR             = 0x3F114,	//0x3F114 ~ 0x3F116	(3 bytes)
+	.ILM_DES_ADDR             = 0x3F128,	//0x3F128 ~ 0x3F12A	(3 bytes)
+	.DLM_DES_ADDR             = 0x3F12C,	//0x3F12C ~ 0x3F12E	(3 bytes)
+	.G_ILM_CHECKSUM_ADDR      = 0x3F100,	//0x3F100 ~ 0x3F103	(4 bytes)
+	.G_DLM_CHECKSUM_ADDR      = 0x3F104,	//0x3F104 ~ 0x3F107	(4 bytes)
+	.R_ILM_CHECKSUM_ADDR      = 0x3F120,	//0x3F120 ~ 0x3F123 (4 bytes)
+	.R_DLM_CHECKSUM_ADDR      = 0x3F124,	//0x3F124 ~ 0x3F127 (4 bytes)
+	.BLD_CRC_EN_ADDR          = 0x3F30E,
+	.DMA_CRC_EN_ADDR          = 0x3F136,
+	.BLD_ILM_DLM_CRC_ADDR     = 0x3F133,
+	.DMA_CRC_FLAG_ADDR        = 0x3F134,
+};
+
+static const struct nvt_spi_mem_map NT36672A_memory_map = {
+	.EVENT_BUF_ADDR           = 0x21C00,
+	.RAW_PIPE0_ADDR           = 0x20000,
+	.RAW_PIPE1_ADDR           = 0x23000,
+	.BASELINE_ADDR            = 0x20BFC,
+	.BASELINE_BTN_ADDR        = 0x23BFC,
+	.DIFF_PIPE0_ADDR          = 0x206DC,
+	.DIFF_PIPE1_ADDR          = 0x236DC,
+	.RAW_BTN_PIPE0_ADDR       = 0x20510,
+	.RAW_BTN_PIPE1_ADDR       = 0x23510,
+	.DIFF_BTN_PIPE0_ADDR      = 0x20BF0,
+	.DIFF_BTN_PIPE1_ADDR      = 0x23BF0,
+	.READ_FLASH_CHECKSUM_ADDR = 0x24000,
+	.RW_FLASH_DATA_ADDR       = 0x24002,
+	/* Phase 2 Host Download */
+	.BOOT_RDY_ADDR            = 0x3F10D,
+	/* BLD CRC */
+	.BLD_LENGTH_ADDR          = 0x3F10E,	//0x3F10E ~ 0x3F10F	(2 bytes)
+	.ILM_LENGTH_ADDR          = 0x3F118,	//0x3F118 ~ 0x3F119	(2 bytes)
+	.DLM_LENGTH_ADDR          = 0x3F130,	//0x3F130 ~ 0x3F131	(2 bytes)
+	.BLD_DES_ADDR             = 0x3F114,	//0x3F114 ~ 0x3F116	(3 bytes)
+	.ILM_DES_ADDR             = 0x3F128,	//0x3F128 ~ 0x3F12A	(3 bytes)
+	.DLM_DES_ADDR             = 0x3F12C,	//0x3F12C ~ 0x3F12E	(3 bytes)
+	.G_ILM_CHECKSUM_ADDR      = 0x3F100,	//0x3F100 ~ 0x3F103	(4 bytes)
+	.G_DLM_CHECKSUM_ADDR      = 0x3F104,	//0x3F104 ~ 0x3F107	(4 bytes)
+	.R_ILM_CHECKSUM_ADDR      = 0x3F120,	//0x3F120 ~ 0x3F123 (4 bytes)
+	.R_DLM_CHECKSUM_ADDR      = 0x3F124,	//0x3F124 ~ 0x3F127 (4 bytes)
+	.BLD_CRC_EN_ADDR          = 0x3F30E,
+	.DMA_CRC_EN_ADDR          = 0x3F132,
+	.BLD_ILM_DLM_CRC_ADDR     = 0x3F133,
+	.DMA_CRC_FLAG_ADDR        = 0x3F134,
+};
+
+static const struct nvt_spi_mem_map NT36772_memory_map = {
+	.EVENT_BUF_ADDR           = 0x11E00,
+	.RAW_PIPE0_ADDR           = 0x10000,
+	.RAW_PIPE1_ADDR           = 0x12000,
+	.BASELINE_ADDR            = 0x10E70,
+	.BASELINE_BTN_ADDR        = 0x12E70,
+	.DIFF_PIPE0_ADDR          = 0x10830,
+	.DIFF_PIPE1_ADDR          = 0x12830,
+	.RAW_BTN_PIPE0_ADDR       = 0x10E60,
+	.RAW_BTN_PIPE1_ADDR       = 0x12E60,
+	.DIFF_BTN_PIPE0_ADDR      = 0x10E68,
+	.DIFF_BTN_PIPE1_ADDR      = 0x12E68,
+	.READ_FLASH_CHECKSUM_ADDR = 0x14000,
+	.RW_FLASH_DATA_ADDR       = 0x14002,
+	/* Phase 2 Host Download */
+	.BOOT_RDY_ADDR            = 0x1F141,
+	.POR_CD_ADDR              = 0x1F61C,
+	/* BLD CRC */
+	.R_ILM_CHECKSUM_ADDR      = 0x1BF00,
+};
+
+static const struct nvt_spi_mem_map NT36525_memory_map = {
+	.EVENT_BUF_ADDR           = 0x11A00,
+	.RAW_PIPE0_ADDR           = 0x10000,
+	.RAW_PIPE1_ADDR           = 0x12000,
+	.BASELINE_ADDR            = 0x10B08,
+	.BASELINE_BTN_ADDR        = 0x12B08,
+	.DIFF_PIPE0_ADDR          = 0x1064C,
+	.DIFF_PIPE1_ADDR          = 0x1264C,
+	.RAW_BTN_PIPE0_ADDR       = 0x10634,
+	.RAW_BTN_PIPE1_ADDR       = 0x12634,
+	.DIFF_BTN_PIPE0_ADDR      = 0x10AFC,
+	.DIFF_BTN_PIPE1_ADDR      = 0x12AFC,
+	.READ_FLASH_CHECKSUM_ADDR = 0x14000,
+	.RW_FLASH_DATA_ADDR       = 0x14002,
+	/* Phase 2 Host Download */
+	.BOOT_RDY_ADDR            = 0x1F141,
+	.POR_CD_ADDR              = 0x1F61C,
+	/* BLD CRC */
+	.R_ILM_CHECKSUM_ADDR      = 0x1BF00,
+};
+
+static const struct nvt_spi_mem_map NT36676F_memory_map = {
+	.EVENT_BUF_ADDR           = 0x11A00,
+	.RAW_PIPE0_ADDR           = 0x10000,
+	.RAW_PIPE1_ADDR           = 0x12000,
+	.BASELINE_ADDR            = 0x10B08,
+	.BASELINE_BTN_ADDR        = 0x12B08,
+	.DIFF_PIPE0_ADDR          = 0x1064C,
+	.DIFF_PIPE1_ADDR          = 0x1264C,
+	.RAW_BTN_PIPE0_ADDR       = 0x10634,
+	.RAW_BTN_PIPE1_ADDR       = 0x12634,
+	.DIFF_BTN_PIPE0_ADDR      = 0x10AFC,
+	.DIFF_BTN_PIPE1_ADDR      = 0x12AFC,
+	.READ_FLASH_CHECKSUM_ADDR = 0x14000,
+	.RW_FLASH_DATA_ADDR       = 0x14002,
+};
+
+static struct nvt_spi_hw_info NT36523_hw_info = {
+	.hw_crc         = 2,
+};
+
+static struct nvt_spi_hw_info NT36526_hw_info = {
+	.hw_crc         = 2,
+};
+
+static struct nvt_spi_hw_info NT36675_hw_info = {
+	.hw_crc         = 2,
+};
+
+static struct nvt_spi_hw_info NT36672A_hw_info = {
+	.hw_crc         = 1,
+};
+
+static struct nvt_spi_hw_info NT36772_hw_info = {
+	.hw_crc         = 0,
+};
+
+static struct nvt_spi_hw_info NT36525_hw_info = {
+	.hw_crc         = 0,
+};
+
+static struct nvt_spi_hw_info NT36676F_hw_info = {
+	.hw_crc         = 0,
+};
+
+#define NVT_SPI_ID_BYTE_MAX 6
+struct nvt_spi_trim_id_table_t {
+	uint8_t id[NVT_SPI_ID_BYTE_MAX];
+	uint8_t mask[NVT_SPI_ID_BYTE_MAX];
+	const struct nvt_spi_mem_map *mmap;
+	const struct nvt_spi_hw_info *hwinfo;
+};
+
+static const struct nvt_spi_trim_id_table_t nvt_spi_trim_id_table[] = {
+	{.id = {0x0D, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x20, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x00, 0xFF, 0xFF, 0x80, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x0C, 0xFF, 0xFF, 0x25, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0E, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x20, 0xFF, 0xFF, 0x23, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36523_memory_map,  .hwinfo = &NT36523_hw_info},
+	{.id = {0x0C, 0xFF, 0xFF, 0x23, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36523_memory_map,  .hwinfo = &NT36523_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x23, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36523_memory_map,  .hwinfo = &NT36523_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x23, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36523_memory_map,  .hwinfo = &NT36523_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x23, 0x65, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36523_memory_map,  .hwinfo = &NT36523_hw_info},
+	{.id = {0x0C, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x26, 0x65, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36526_memory_map,  .hwinfo = &NT36526_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x75, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36675_memory_map,  .hwinfo = &NT36675_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x82, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x25, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x72, 0x65, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x82, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x70, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0B, 0xFF, 0xFF, 0x70, 0x66, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x0A, 0xFF, 0xFF, 0x72, 0x67, 0x03}, .mask = {1, 0, 0, 1, 1, 1},
+		.mmap = &NT36672A_memory_map, .hwinfo = &NT36672A_hw_info},
+	{.id = {0x55, 0x00, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0x55, 0x72, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xAA, 0x00, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xAA, 0x72, 0xFF, 0x00, 0x00, 0x00}, .mask = {1, 1, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x72, 0x67, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x70, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x70, 0x67, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x72, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36772_memory_map,  .hwinfo = &NT36772_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x25, 0x65, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36525_memory_map,  .hwinfo = &NT36525_hw_info},
+	{.id = {0xFF, 0xFF, 0xFF, 0x76, 0x66, 0x03}, .mask = {0, 0, 0, 1, 1, 1},
+		.mmap = &NT36676F_memory_map, .hwinfo = &NT36676F_hw_info}
+};
+
+#endif

nt36xxx/nt36xxx_mp_ctrlram.c

@@ -0,0 +1,1480 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2010 - 2018 Novatek, Inc.
+ *
+ * $Revision: 47247 $
+ * $Date: 2019-07-10 10:41:36 +0800 (Wed, 10 Jul 2019) $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+
+#include "nt36xxx.h"
+#include "nt36xxx_mp_ctrlram.h"
+
+#if NVT_TOUCH_MP
+
+#define NORMAL_MODE 0x00
+#define TEST_MODE_1 0x21
+#define TEST_MODE_2 0x22
+#define MP_MODE_CC 0x41
+#define FREQ_HOP_DISABLE 0x66
+#define FREQ_HOP_ENABLE 0x65
+
+#define SHORT_TEST_CSV_FILE "/data/local/tmp/ShortTest.csv"
+#define OPEN_TEST_CSV_FILE "/data/local/tmp/OpenTest.csv"
+#define FW_RAWDATA_CSV_FILE "/data/local/tmp/FWMutualTest.csv"
+#define FW_CC_CSV_FILE "/data/local/tmp/FWCCTest.csv"
+#define NOISE_TEST_CSV_FILE "/data/local/tmp/NoiseTest.csv"
+
+#define nvt_mp_seq_printf(m, fmt, args...) do {	\
+	seq_printf(m, fmt, ##args);	\
+	if (!nvt_mp_test_result_printed)	\
+		printk(fmt, ##args);	\
+} while (0)
+
+static uint8_t *RecordResult_Short = NULL;
+static uint8_t *RecordResult_Open = NULL;
+static uint8_t *RecordResult_FWMutual = NULL;
+static uint8_t *RecordResult_FW_CC = NULL;
+static uint8_t *RecordResult_FW_DiffMax = NULL;
+static uint8_t *RecordResult_FW_DiffMin = NULL;
+
+static int32_t TestResult_Short = 0;
+static int32_t TestResult_Open = 0;
+static int32_t TestResult_FW_Rawdata = 0;
+static int32_t TestResult_FWMutual = 0;
+static int32_t TestResult_FW_CC = 0;
+static int32_t TestResult_Noise = 0;
+static int32_t TestResult_FW_DiffMax = 0;
+static int32_t TestResult_FW_DiffMin = 0;
+
+static int32_t *RawData_Short = NULL;
+static int32_t *RawData_Open = NULL;
+static int32_t *RawData_Diff = NULL;
+static int32_t *RawData_Diff_Min = NULL;
+static int32_t *RawData_Diff_Max = NULL;
+static int32_t *RawData_FWMutual = NULL;
+static int32_t *RawData_FW_CC = NULL;
+
+static struct proc_dir_entry *NVT_proc_selftest_entry = NULL;
+static int8_t nvt_mp_test_result_printed = 0;
+static uint8_t fw_ver = 0;
+
+extern void nvt_change_mode(uint8_t mode);
+extern uint8_t nvt_get_fw_pipe(void);
+extern void nvt_read_mdata(uint32_t xdata_addr, uint32_t xdata_btn_addr);
+extern void nvt_get_mdata(int32_t *buf, uint8_t *m_x_num, uint8_t *m_y_num);
+int32_t nvt_mp_parse_dt(struct device_node *root, const char *node_compatible);
+
+/*******************************************************
+Description:
+	Novatek touchscreen allocate buffer for mp selftest.
+
+return:
+	Executive outcomes. 0---succeed. -12---Out of memory
+*******************************************************/
+static int nvt_mp_buffer_init(void)
+{
+	size_t RecordResult_BufSize = IC_X_CFG_SIZE * IC_Y_CFG_SIZE + IC_KEY_CFG_SIZE;
+	size_t RawData_BufSize = (IC_X_CFG_SIZE * IC_Y_CFG_SIZE + IC_KEY_CFG_SIZE) * sizeof(int32_t);
+
+	RecordResult_Short = kzalloc(RecordResult_BufSize, GFP_KERNEL);
+	if (!RecordResult_Short) {
+		NVT_ERR("kzalloc for RecordResult_Short failed!\n");
+		return -ENOMEM;
+	}
+
+	RecordResult_Open = kzalloc(RecordResult_BufSize, GFP_KERNEL);
+	if (!RecordResult_Open) {
+		NVT_ERR("kzalloc for RecordResult_Open failed!\n");
+		return -ENOMEM;
+	}
+
+	RecordResult_FWMutual = kzalloc(RecordResult_BufSize, GFP_KERNEL);
+	if (!RecordResult_FWMutual) {
+		NVT_ERR("kzalloc for RecordResult_FWMutual failed!\n");
+		return -ENOMEM;
+	}
+
+	RecordResult_FW_CC = kzalloc(RecordResult_BufSize, GFP_KERNEL);
+	if (!RecordResult_FW_CC) {
+		NVT_ERR("kzalloc for RecordResult_FW_CC failed!\n");
+		return -ENOMEM;
+	}
+
+	RecordResult_FW_DiffMax = kzalloc(RecordResult_BufSize, GFP_KERNEL);
+	if (!RecordResult_FW_DiffMax) {
+		NVT_ERR("kzalloc for RecordResult_FW_DiffMax failed!\n");
+		return -ENOMEM;
+	}
+
+	RecordResult_FW_DiffMin = kzalloc(RecordResult_BufSize, GFP_KERNEL);
+	if (!RecordResult_FW_DiffMin) {
+		NVT_ERR("kzalloc for RecordResult_FW_DiffMin failed!\n");
+		return -ENOMEM;
+	}
+
+	RawData_Short = kzalloc(RawData_BufSize, GFP_KERNEL);
+	if (!RawData_Short) {
+		NVT_ERR("kzalloc for RawData_Short failed!\n");
+		return -ENOMEM;
+	}
+
+	RawData_Open = kzalloc(RawData_BufSize, GFP_KERNEL);
+	if (!RawData_Open) {
+		NVT_ERR("kzalloc for RawData_Open failed!\n");
+		return -ENOMEM;
+	}
+
+	RawData_Diff = kzalloc(RawData_BufSize, GFP_KERNEL);
+	if (!RawData_Diff) {
+		NVT_ERR("kzalloc for RawData_Diff failed!\n");
+		return -ENOMEM;
+	}
+
+	RawData_Diff_Min = kzalloc(RawData_BufSize, GFP_KERNEL);
+	if (!RawData_Diff_Min) {
+		NVT_ERR("kzalloc for RawData_Diff_Min failed!\n");
+		return -ENOMEM;
+	}
+
+	RawData_Diff_Max = kzalloc(RawData_BufSize, GFP_KERNEL);
+	if (!RawData_Diff_Max) {
+		NVT_ERR("kzalloc for RawData_Diff_Max failed!\n");
+		return -ENOMEM;
+	}
+
+	RawData_FWMutual = kzalloc(RawData_BufSize, GFP_KERNEL);
+	if (!RawData_FWMutual) {
+		NVT_ERR("kzalloc for RawData_FWMutual failed!\n");
+		return -ENOMEM;
+	}
+
+	RawData_FW_CC = kzalloc(RawData_BufSize, GFP_KERNEL);
+	if (!RawData_FW_CC) {
+		NVT_ERR("kzalloc for RawData_FW_CC failed!\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen free buffer for mp selftest.
+
+return:
+	n.a.
+*******************************************************/
+static void nvt_mp_buffer_deinit(void)
+{
+	if (RecordResult_Short) {
+		kfree(RecordResult_Short);
+		RecordResult_Short = NULL;
+	}
+
+	if (RecordResult_Open) {
+		kfree(RecordResult_Open);
+		RecordResult_Open = NULL;
+	}
+
+	if (RecordResult_FWMutual) {
+		kfree(RecordResult_FWMutual);
+		RecordResult_FWMutual = NULL;
+	}
+
+	if (RecordResult_FW_CC) {
+		kfree(RecordResult_FW_CC);
+		RecordResult_FW_CC = NULL;
+	}
+
+	if (RecordResult_FW_DiffMax) {
+		kfree(RecordResult_FW_DiffMax);
+		RecordResult_FW_DiffMax = NULL;
+	}
+
+	if (RecordResult_FW_DiffMin) {
+		kfree(RecordResult_FW_DiffMin);
+		RecordResult_FW_DiffMin = NULL;
+	}
+
+	if (RawData_Short) {
+		kfree(RawData_Short);
+		RawData_Short = NULL;
+	}
+
+	if (RawData_Open) {
+		kfree(RawData_Open);
+		RawData_Open = NULL;
+	}
+
+	if (RawData_Diff) {
+		kfree(RawData_Diff);
+		RawData_Diff = NULL;
+	}
+
+	if (RawData_Diff_Min) {
+		kfree(RawData_Diff_Min);
+		RawData_Diff_Min = NULL;
+	}
+
+	if (RawData_Diff_Max) {
+		kfree(RawData_Diff_Max);
+		RawData_Diff_Max = NULL;
+	}
+
+	if (RawData_FWMutual) {
+		kfree(RawData_FWMutual);
+		RawData_FWMutual = NULL;
+	}
+
+	if (RawData_FW_CC) {
+		kfree(RawData_FW_CC);
+		RawData_FW_CC = NULL;
+	}
+}
+
+static void nvt_print_data_log_in_one_line(int32_t *data, int32_t data_num)
+{
+	char *tmp_log = NULL;
+	int32_t i = 0;
+	int32_t count = data_num * 7 + 1;
+
+	tmp_log = kzalloc(count, GFP_KERNEL);
+	if (!tmp_log) {
+		NVT_ERR("kzalloc for tmp_log failed!\n ");
+		return;
+	}
+
+	for (i = 0; i < data_num; i++) {
+		snprintf(tmp_log + i * 7, count - i * 7, "%5d, ", data[i]);
+	}
+	tmp_log[data_num * 7] = '\0';
+	printk("%s", tmp_log);
+	if (tmp_log) {
+		kfree(tmp_log);
+		tmp_log = NULL;
+	}
+
+	return;
+}
+
+static void nvt_print_result_log_in_one_line(uint8_t *result, int32_t result_num)
+{
+	char *tmp_log = NULL;
+	int32_t i = 0;
+	int32_t count = 0;
+
+	count = result_num * 6 + 1;
+	tmp_log = kzalloc(count, GFP_KERNEL);
+	if (!tmp_log) {
+		NVT_ERR("kzalloc for tmp_log failed!\n ");
+		return;
+	}
+
+	for (i = 0; i < result_num; i++) {
+		snprintf(tmp_log + i * 6, count - i * 6, "0x%02X, ", result[i]);
+	}
+	tmp_log[result_num * 6] = '\0';
+	printk("%s", tmp_log);
+	if (tmp_log) {
+		kfree(tmp_log);
+		tmp_log = NULL;
+	}
+
+	return;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen self-test criteria print function.
+
+return:
+	n.a.
+*******************************************************/
+static void nvt_print_lmt_array(int32_t *array, int32_t x_ch, int32_t y_ch)
+{
+	int32_t j = 0;
+
+	for (j = 0; j < y_ch; j++) {
+		nvt_print_data_log_in_one_line(array + j * x_ch, x_ch);
+		printk("\n");
+	}
+#if TOUCH_KEY_NUM > 0
+	nvt_print_data_log_in_one_line(array + y_ch * x_ch, Key_Channel);
+	printk("\n");
+#endif /* #if TOUCH_KEY_NUM > 0 */
+}
+
+static void nvt_print_criteria(void)
+{
+	NVT_LOG("++\n");
+
+	//---PS_Config_Lmt_Short_Rawdata---
+	printk("PS_Config_Lmt_Short_Rawdata_P:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_Short_Rawdata_P, X_Channel, Y_Channel);
+	printk("PS_Config_Lmt_Short_Rawdata_N:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_Short_Rawdata_N, X_Channel, Y_Channel);
+
+	//---PS_Config_Lmt_Open_Rawdata---
+	printk("PS_Config_Lmt_Open_Rawdata_P:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_Open_Rawdata_P, X_Channel, Y_Channel);
+	printk("PS_Config_Lmt_Open_Rawdata_N:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_Open_Rawdata_N, X_Channel, Y_Channel);
+
+	//---PS_Config_Lmt_FW_Rawdata---
+	printk("PS_Config_Lmt_FW_Rawdata_P:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_FW_Rawdata_P, X_Channel, Y_Channel);
+	printk("PS_Config_Lmt_FW_Rawdata_N:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_FW_Rawdata_N, X_Channel, Y_Channel);
+
+	//---PS_Config_Lmt_FW_CC---
+	printk("PS_Config_Lmt_FW_CC_P:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_FW_CC_P, X_Channel, Y_Channel);
+	printk("PS_Config_Lmt_FW_CC_N:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_FW_CC_N, X_Channel, Y_Channel);
+
+	//---PS_Config_Lmt_FW_Diff---
+	printk("PS_Config_Lmt_FW_Diff_P:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_FW_Diff_P, X_Channel, Y_Channel);
+	printk("PS_Config_Lmt_FW_Diff_N:\n");
+	nvt_print_lmt_array(PS_Config_Lmt_FW_Diff_N, X_Channel, Y_Channel);
+
+	NVT_LOG("--\n");
+}
+
+static int32_t nvt_polling_hand_shake_status(void)
+{
+	uint8_t buf[8] = {0};
+	int32_t i = 0;
+	const int32_t retry = 70;
+
+	for (i = 0; i < retry; i++) {
+		//---set xdata index to EVENT BUF ADDR---
+		nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE);
+
+		//---read fw status---
+		buf[0] = EVENT_MAP_HANDSHAKING_or_SUB_CMD_BYTE;
+		buf[1] = 0x00;
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 2);
+
+		if ((buf[1] == 0xA0) || (buf[1] == 0xA1))
+			break;
+
+		usleep_range(10000, 10000);
+	}
+
+	if (i >= retry) {
+		NVT_ERR("polling hand shake status failed, buf[1]=0x%02X\n", buf[1]);
+
+		// Read back 5 bytes from offset EVENT_MAP_HOST_CMD for debug check
+		nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HOST_CMD);
+
+		buf[0] = EVENT_MAP_HOST_CMD;
+		buf[1] = 0x00;
+		buf[2] = 0x00;
+		buf[3] = 0x00;
+		buf[4] = 0x00;
+		buf[5] = 0x00;
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 6);
+		NVT_ERR("Read back 5 bytes from offset EVENT_MAP_HOST_CMD: 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X\n", buf[1], buf[2], buf[3], buf[4], buf[5]);
+
+		return -1;
+	} else {
+		return 0;
+	}
+}
+
+static int8_t nvt_switch_FreqHopEnDis(uint8_t FreqHopEnDis)
+{
+	uint8_t buf[8] = {0};
+	uint8_t retry = 0;
+	int8_t ret = 0;
+
+	NVT_LOG("++\n");
+
+	for (retry = 0; retry < 20; retry++) {
+		//---set xdata index to EVENT BUF ADDR---
+		nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HOST_CMD);
+
+		//---switch FreqHopEnDis---
+		buf[0] = EVENT_MAP_HOST_CMD;
+		buf[1] = FreqHopEnDis;
+		CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 2);
+
+		msleep(35);
+
+		buf[0] = EVENT_MAP_HOST_CMD;
+		buf[1] = 0xFF;
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, 2);
+
+		if (buf[1] == 0x00)
+			break;
+	}
+
+	if (unlikely(retry == 20)) {
+		NVT_ERR("switch FreqHopEnDis 0x%02X failed, buf[1]=0x%02X\n", FreqHopEnDis, buf[1]);
+		ret = -1;
+	}
+
+	NVT_LOG("--\n");
+
+	return ret;
+}
+
+static int32_t nvt_read_baseline(int32_t *xdata)
+{
+	uint8_t x_num = 0;
+	uint8_t y_num = 0;
+	uint32_t x = 0;
+	uint32_t y = 0;
+	int32_t iArrayIndex = 0;
+#if TOUCH_KEY_NUM > 0
+	int32_t k = 0;
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	NVT_LOG("++\n");
+
+	nvt_read_mdata(ts->mmap->BASELINE_ADDR, ts->mmap->BASELINE_BTN_ADDR);
+
+	nvt_get_mdata(xdata, &x_num, &y_num);
+
+	for (y = 0; y < y_num; y++) {
+		for (x = 0; x < x_num; x++) {
+			iArrayIndex = y * x_num + x;
+			xdata[iArrayIndex] = (int16_t)xdata[iArrayIndex];
+		}
+	}
+#if TOUCH_KEY_NUM > 0
+	for (k = 0; k < Key_Channel; k++) {
+		iArrayIndex = Y_Channel * X_Channel + k;
+		xdata[iArrayIndex] = (int16_t)xdata[iArrayIndex];
+	}
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	NVT_LOG("--\n");
+
+	return 0;
+}
+
+static int32_t nvt_read_CC(int32_t *xdata)
+{
+	uint8_t x_num = 0;
+	uint8_t y_num = 0;
+	uint32_t x = 0;
+	uint32_t y = 0;
+	int32_t iArrayIndex = 0;
+#if TOUCH_KEY_NUM > 0
+	int32_t k = 0;
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	NVT_LOG("++\n");
+
+	if (nvt_get_fw_pipe() == 0)
+		nvt_read_mdata(ts->mmap->DIFF_PIPE1_ADDR, ts->mmap->DIFF_BTN_PIPE1_ADDR);
+	else
+		nvt_read_mdata(ts->mmap->DIFF_PIPE0_ADDR, ts->mmap->DIFF_BTN_PIPE0_ADDR);
+
+	nvt_get_mdata(xdata, &x_num, &y_num);
+
+	for (y = 0; y < y_num; y++) {
+		for (x = 0; x < x_num; x++) {
+			iArrayIndex = y * x_num + x;
+			xdata[iArrayIndex] = (int16_t)xdata[iArrayIndex];
+		}
+	}
+#if TOUCH_KEY_NUM > 0
+	for (k = 0; k < Key_Channel; k++) {
+		iArrayIndex = Y_Channel * X_Channel + k;
+		xdata[iArrayIndex] = (int16_t)xdata[iArrayIndex];
+	}
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	NVT_LOG("--\n");
+
+	return 0;
+}
+
+static void nvt_enable_noise_collect(int32_t frame_num)
+{
+	uint8_t buf[8] = {0};
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HOST_CMD);
+
+	//---enable noise collect---
+	buf[0] = EVENT_MAP_HOST_CMD;
+	buf[1] = 0x47;
+	buf[2] = 0xAA;
+	buf[3] = frame_num;
+	buf[4] = 0x00;
+	CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 5);
+}
+
+static int32_t nvt_read_fw_noise(int32_t *xdata)
+{
+	uint8_t x_num = 0;
+	uint8_t y_num = 0;
+	uint32_t x = 0;
+	uint32_t y = 0;
+	int32_t iArrayIndex = 0;
+	int32_t frame_num = 0;
+#if TOUCH_KEY_NUM > 0
+	int32_t k = 0;
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	NVT_LOG("++\n");
+
+	//---Enter Test Mode---
+	if (nvt_clear_fw_status()) {
+		return -EAGAIN;
+	}
+
+	frame_num = PS_Config_Diff_Test_Frame / 10;
+	if (frame_num <= 0)
+		frame_num = 1;
+	printk("%s: frame_num=%d\n", __func__, frame_num);
+	nvt_enable_noise_collect(frame_num);
+	// need wait PS_Config_Diff_Test_Frame * 8.3ms
+	msleep(frame_num * 83);
+
+	if (nvt_polling_hand_shake_status()) {
+		return -EAGAIN;
+	}
+
+	if (nvt_get_fw_info()) {
+		return -EAGAIN;
+	}
+
+	if (nvt_get_fw_pipe() == 0)
+		nvt_read_mdata(ts->mmap->DIFF_PIPE0_ADDR, ts->mmap->DIFF_BTN_PIPE0_ADDR);
+	else
+		nvt_read_mdata(ts->mmap->DIFF_PIPE1_ADDR, ts->mmap->DIFF_BTN_PIPE1_ADDR);
+
+	nvt_get_mdata(xdata, &x_num, &y_num);
+
+	for (y = 0; y < y_num; y++) {
+		for (x = 0; x < x_num; x++) {
+			iArrayIndex = y * x_num + x;
+			RawData_Diff_Max[iArrayIndex] = (int8_t)((xdata[iArrayIndex] >> 8) & 0xFF);
+			RawData_Diff_Min[iArrayIndex] = (int8_t)(xdata[iArrayIndex] & 0xFF);
+		}
+	}
+#if TOUCH_KEY_NUM > 0
+	for (k = 0; k < Key_Channel; k++) {
+		iArrayIndex = Y_Channel * X_Channel + k;
+		RawData_Diff_Max[iArrayIndex] = (int8_t)((xdata[iArrayIndex] >> 8) & 0xFF);
+		RawData_Diff_Min[iArrayIndex] = (int8_t)(xdata[iArrayIndex] & 0xFF);
+	}
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	//---Leave Test Mode---
+	nvt_change_mode(NORMAL_MODE);
+
+	NVT_LOG("--\n");
+
+	return 0;
+}
+
+static void nvt_enable_open_test(void)
+{
+	uint8_t buf[8] = {0};
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HOST_CMD);
+
+	//---enable open test---
+	buf[0] = EVENT_MAP_HOST_CMD;
+	buf[1] = 0x45;
+	buf[2] = 0xAA;
+	buf[3] = 0x02;
+	buf[4] = 0x00;
+	CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 5);
+}
+
+static void nvt_enable_short_test(void)
+{
+	uint8_t buf[8] = {0};
+
+	//---set xdata index to EVENT BUF ADDR---
+	nvt_set_page(I2C_FW_Address, ts->mmap->EVENT_BUF_ADDR | EVENT_MAP_HOST_CMD);
+
+	//---enable short test---
+	buf[0] = EVENT_MAP_HOST_CMD;
+	buf[1] = 0x43;
+	buf[2] = 0xAA;
+	buf[3] = 0x02;
+	buf[4] = 0x00;
+	CTP_I2C_WRITE(ts->client, I2C_FW_Address, buf, 5);
+}
+
+static int32_t nvt_read_fw_open(int32_t *xdata)
+{
+	uint32_t raw_pipe_addr = 0;
+	uint8_t *rawdata_buf = NULL;
+	uint32_t x = 0;
+	uint32_t y = 0;
+	uint8_t buf[128] = {0};
+#if TOUCH_KEY_NUM > 0
+	uint32_t raw_btn_pipe_addr = 0;
+	int32_t k = 0;
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	NVT_LOG("++\n");
+
+	//---Enter Test Mode---
+	if (nvt_clear_fw_status()) {
+		return -EAGAIN;
+	}
+
+	nvt_enable_open_test();
+
+	if (nvt_polling_hand_shake_status()) {
+		return -EAGAIN;
+	}
+
+#if TOUCH_KEY_NUM > 0
+	rawdata_buf = kzalloc((IC_X_CFG_SIZE * IC_Y_CFG_SIZE + IC_KEY_CFG_SIZE) * 2, GFP_KERNEL);
+#else
+	rawdata_buf = kzalloc(IC_X_CFG_SIZE * IC_Y_CFG_SIZE * 2, GFP_KERNEL);
+#endif /* #if TOUCH_KEY_NUM > 0 */
+	if (!rawdata_buf) {
+		NVT_ERR("kzalloc for rawdata_buf failed!\n");
+		return -ENOMEM;
+	}
+
+	if (nvt_get_fw_pipe() == 0)
+		raw_pipe_addr = ts->mmap->RAW_PIPE0_ADDR;
+	else
+		raw_pipe_addr = ts->mmap->RAW_PIPE1_ADDR;
+
+	for (y = 0; y < IC_Y_CFG_SIZE; y++) {
+		//---change xdata index---
+		nvt_set_page(I2C_FW_Address, raw_pipe_addr + y * IC_X_CFG_SIZE * 2);
+		buf[0] = (uint8_t)((raw_pipe_addr + y * IC_X_CFG_SIZE * 2) & 0xFF);
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, IC_X_CFG_SIZE * 2 + 1);
+		memcpy(rawdata_buf + y * IC_X_CFG_SIZE * 2, buf + 1, IC_X_CFG_SIZE * 2);
+	}
+#if TOUCH_KEY_NUM > 0
+	if (nvt_get_fw_pipe() == 0)
+		raw_btn_pipe_addr = ts->mmap->RAW_BTN_PIPE0_ADDR;
+	else
+		raw_btn_pipe_addr = ts->mmap->RAW_BTN_PIPE1_ADDR;
+
+	//---change xdata index---
+	nvt_set_page(I2C_FW_Address, raw_btn_pipe_addr);
+	buf[0] = (uint8_t)(raw_btn_pipe_addr & 0xFF);
+	CTP_I2C_READ(ts->client, I2C_FW_Address, buf, IC_KEY_CFG_SIZE * 2 + 1);
+	memcpy(rawdata_buf + IC_Y_CFG_SIZE * IC_X_CFG_SIZE * 2, buf + 1, IC_KEY_CFG_SIZE * 2);
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	for (y = 0; y < IC_Y_CFG_SIZE; y++) {
+		for (x = 0; x < IC_X_CFG_SIZE; x++) {
+			if ((AIN_Y[y] != 0xFF) && (AIN_X[x] != 0xFF)) {
+				xdata[AIN_Y[y] * X_Channel + AIN_X[x]] = (int16_t)((rawdata_buf[(y * IC_X_CFG_SIZE + x) * 2] + 256 * rawdata_buf[(y * IC_X_CFG_SIZE + x) * 2 + 1]));
+			}
+		}
+	}
+#if TOUCH_KEY_NUM > 0
+	for (k = 0; k < IC_KEY_CFG_SIZE; k++) {
+		if (AIN_KEY[k] != 0xFF)
+			xdata[Y_Channel * X_Channel + AIN_KEY[k]] = (int16_t)(rawdata_buf[(IC_Y_CFG_SIZE * IC_X_CFG_SIZE + k) * 2] + 256 * rawdata_buf[(IC_Y_CFG_SIZE * IC_X_CFG_SIZE + k) * 2 + 1]);
+	}
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	if (rawdata_buf) {
+		kfree(rawdata_buf);
+		rawdata_buf = NULL;
+	}
+
+	//---Leave Test Mode---
+	nvt_change_mode(NORMAL_MODE);
+
+	NVT_LOG("--\n");
+
+	return 0;
+}
+
+static int32_t nvt_read_fw_short(int32_t *xdata)
+{
+	uint32_t raw_pipe_addr = 0;
+	uint8_t *rawdata_buf = NULL;
+	uint32_t x = 0;
+	uint32_t y = 0;
+	uint8_t buf[128] = {0};
+	int32_t iArrayIndex = 0;
+#if TOUCH_KEY_NUM > 0
+	uint32_t raw_btn_pipe_addr = 0;
+	int32_t k = 0;
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	NVT_LOG("++\n");
+
+	//---Enter Test Mode---
+	if (nvt_clear_fw_status()) {
+		return -EAGAIN;
+	}
+
+	nvt_enable_short_test();
+
+	if (nvt_polling_hand_shake_status()) {
+		return -EAGAIN;
+	}
+
+#if TOUCH_KEY_NUM > 0
+    rawdata_buf = kzalloc((X_Channel * Y_Channel + Key_Channel) * 2, GFP_KERNEL);
+#else
+    rawdata_buf = kzalloc(X_Channel * Y_Channel * 2, GFP_KERNEL);
+#endif /* #if TOUCH_KEY_NUM > 0 */
+	if (!rawdata_buf) {
+		NVT_ERR("kzalloc for rawdata_buf failed!\n");
+		return -ENOMEM;
+	}
+
+	if (nvt_get_fw_pipe() == 0)
+		raw_pipe_addr = ts->mmap->RAW_PIPE0_ADDR;
+	else
+		raw_pipe_addr = ts->mmap->RAW_PIPE1_ADDR;
+
+	for (y = 0; y < Y_Channel; y++) {
+		//---change xdata index---
+		nvt_set_page(I2C_FW_Address, raw_pipe_addr + y * X_Channel * 2);
+		buf[0] = (uint8_t)((raw_pipe_addr + y * X_Channel * 2) & 0xFF);
+		CTP_I2C_READ(ts->client, I2C_FW_Address, buf, X_Channel * 2 + 1);
+		memcpy(rawdata_buf + y * X_Channel * 2, buf + 1, X_Channel * 2);
+	}
+#if TOUCH_KEY_NUM > 0
+	if (nvt_get_fw_pipe() == 0)
+		raw_btn_pipe_addr = ts->mmap->RAW_BTN_PIPE0_ADDR;
+	else
+		raw_btn_pipe_addr = ts->mmap->RAW_BTN_PIPE1_ADDR;
+
+    //---change xdata index---
+	nvt_set_page(I2C_FW_Address, raw_btn_pipe_addr);
+	buf[0] = (uint8_t)(raw_btn_pipe_addr & 0xFF);
+	CTP_I2C_READ(ts->client, I2C_FW_Address, buf, Key_Channel * 2 + 1);
+	memcpy(rawdata_buf + Y_Channel * X_Channel * 2, buf + 1, Key_Channel * 2);
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	for (y = 0; y < Y_Channel; y++) {
+		for (x = 0; x < X_Channel; x++) {
+			iArrayIndex = y * X_Channel + x;
+			xdata[iArrayIndex] = (int16_t)(rawdata_buf[iArrayIndex * 2] + 256 * rawdata_buf[iArrayIndex * 2 + 1]);
+		}
+	}
+#if TOUCH_KEY_NUM > 0
+	for (k = 0; k < Key_Channel; k++) {
+		iArrayIndex = Y_Channel * X_Channel + k;
+		xdata[iArrayIndex] = (int16_t)(rawdata_buf[iArrayIndex * 2] + 256 * rawdata_buf[iArrayIndex * 2 + 1]);
+	}
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	if (rawdata_buf) {
+		kfree(rawdata_buf);
+		rawdata_buf = NULL;
+	}
+
+	//---Leave Test Mode---
+	nvt_change_mode(NORMAL_MODE);
+
+	NVT_LOG("--\n");
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen raw data test for each single point function.
+
+return:
+	Executive outcomes. 0---passed. negative---failed.
+*******************************************************/
+static int32_t RawDataTest_SinglePoint_Sub(int32_t rawdata[], uint8_t RecordResult[], uint8_t x_ch, uint8_t y_ch, int32_t Rawdata_Limit_Postive[], int32_t Rawdata_Limit_Negative[])
+{
+	int32_t i = 0;
+	int32_t j = 0;
+#if TOUCH_KEY_NUM > 0
+    int32_t k = 0;
+#endif /* #if TOUCH_KEY_NUM > 0 */
+	int32_t iArrayIndex = 0;
+	bool isPass = true;
+
+	for (j = 0; j < y_ch; j++) {
+		for (i = 0; i < x_ch; i++) {
+			iArrayIndex = j * x_ch + i;
+
+			RecordResult[iArrayIndex] = 0x00; // default value for PASS
+
+			if(rawdata[iArrayIndex] > Rawdata_Limit_Postive[iArrayIndex])
+				RecordResult[iArrayIndex] |= 0x01;
+
+			if(rawdata[iArrayIndex] < Rawdata_Limit_Negative[iArrayIndex])
+				RecordResult[iArrayIndex] |= 0x02;
+		}
+	}
+#if TOUCH_KEY_NUM > 0
+	for (k = 0; k < Key_Channel; k++) {
+		iArrayIndex = y_ch * x_ch + k;
+
+		RecordResult[iArrayIndex] = 0x00; // default value for PASS
+
+		if(rawdata[iArrayIndex] > Rawdata_Limit_Postive[iArrayIndex])
+			RecordResult[iArrayIndex] |= 0x01;
+
+		if(rawdata[iArrayIndex] < Rawdata_Limit_Negative[iArrayIndex])
+			RecordResult[iArrayIndex] |= 0x02;
+	}
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	//---Check RecordResult---
+	for (j = 0; j < y_ch; j++) {
+		for (i = 0; i < x_ch; i++) {
+			if (RecordResult[j * x_ch + i] != 0) {
+				isPass = false;
+				break;
+			}
+		}
+	}
+#if TOUCH_KEY_NUM > 0
+	for (k = 0; k < Key_Channel; k++) {
+		iArrayIndex = y_ch * x_ch + k;
+		if (RecordResult[iArrayIndex] != 0) {
+			isPass = false;
+			break;
+		}
+	}
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	if (!isPass) {
+		return -1; // FAIL
+	} else {
+		return 0; // PASS
+	}
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen print self-test result function.
+
+return:
+	n.a.
+*******************************************************/
+void print_selftest_result(struct seq_file *m, int32_t TestResult, uint8_t RecordResult[], int32_t rawdata[], uint8_t x_len, uint8_t y_len)
+{
+	int32_t i = 0;
+	int32_t j = 0;
+	int32_t iArrayIndex = 0;
+#if TOUCH_KEY_NUM > 0
+	int32_t k = 0;
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+	switch (TestResult) {
+		case 0:
+			nvt_mp_seq_printf(m, " PASS!\n");
+			break;
+
+		case 1:
+			nvt_mp_seq_printf(m, " ERROR! Read Data FAIL!\n");
+			break;
+
+		case -1:
+			nvt_mp_seq_printf(m, " FAIL!\n");
+			nvt_mp_seq_printf(m, "RecordResult:\n");
+			for (i = 0; i < y_len; i++) {
+				for (j = 0; j < x_len; j++) {
+					iArrayIndex = i * x_len + j;
+					seq_printf(m, "0x%02X, ", RecordResult[iArrayIndex]);
+				}
+				if (!nvt_mp_test_result_printed)
+					nvt_print_result_log_in_one_line(RecordResult + i * x_len, x_len);
+				nvt_mp_seq_printf(m, "\n");
+			}
+#if TOUCH_KEY_NUM > 0
+			for (k = 0; k < Key_Channel; k++) {
+				iArrayIndex = y_len * x_len + k;
+				seq_printf(m, "0x%02X, ", RecordResult[iArrayIndex]);
+			}
+			if (!nvt_mp_test_result_printed)
+				nvt_print_result_log_in_one_line(RecordResult + y_len * x_len, Key_Channel);
+			nvt_mp_seq_printf(m, "\n");
+#endif /* #if TOUCH_KEY_NUM > 0 */
+			nvt_mp_seq_printf(m, "ReadData:\n");
+			for (i = 0; i < y_len; i++) {
+				for (j = 0; j < x_len; j++) {
+					iArrayIndex = i * x_len + j;
+					seq_printf(m, "%5d, ", rawdata[iArrayIndex]);
+				}
+				if (!nvt_mp_test_result_printed)
+					nvt_print_data_log_in_one_line(rawdata + i * x_len, x_len);
+				nvt_mp_seq_printf(m, "\n");
+			}
+#if TOUCH_KEY_NUM > 0
+			for (k = 0; k < Key_Channel; k++) {
+				iArrayIndex = y_len * x_len + k;
+				seq_printf(m, "%5d, ", rawdata[iArrayIndex]);
+			}
+			if (!nvt_mp_test_result_printed)
+				nvt_print_data_log_in_one_line(rawdata + y_len * x_len, Key_Channel);
+			nvt_mp_seq_printf(m, "\n");
+#endif /* #if TOUCH_KEY_NUM > 0 */
+			break;
+	}
+	nvt_mp_seq_printf(m, "\n");
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen self-test sequence print show
+	function.
+
+return:
+	Executive outcomes. 0---succeed.
+*******************************************************/
+static int32_t c_show_selftest(struct seq_file *m, void *v)
+{
+	NVT_LOG("++\n");
+
+	nvt_mp_seq_printf(m, "FW Version: %d\n\n", fw_ver);
+
+	nvt_mp_seq_printf(m, "Short Test");
+
+	print_selftest_result(m, TestResult_Short, RecordResult_Short, RawData_Short, X_Channel, Y_Channel);
+
+	nvt_mp_seq_printf(m, "Open Test");
+	print_selftest_result(m, TestResult_Open, RecordResult_Open, RawData_Open, X_Channel, Y_Channel);
+
+	nvt_mp_seq_printf(m, "FW Rawdata Test");
+	if ((TestResult_FW_Rawdata == 0) || (TestResult_FW_Rawdata == 1)) {
+		 print_selftest_result(m, TestResult_FWMutual, RecordResult_FWMutual, RawData_FWMutual, X_Channel, Y_Channel);
+	} else { // TestResult_FW_Rawdata is -1
+		nvt_mp_seq_printf(m, " FAIL!\n");
+		if (TestResult_FWMutual == -1) {
+			nvt_mp_seq_printf(m, "FW Mutual");
+			print_selftest_result(m, TestResult_FWMutual, RecordResult_FWMutual, RawData_FWMutual, X_Channel, Y_Channel);
+		}
+		if (TestResult_FW_CC == -1) {
+			nvt_mp_seq_printf(m, "FW CC");
+			print_selftest_result(m, TestResult_FW_CC, RecordResult_FW_CC, RawData_FW_CC, X_Channel, Y_Channel);
+		}
+	}
+
+	nvt_mp_seq_printf(m, "Noise Test");
+	if ((TestResult_Noise == 0) || (TestResult_Noise == 1)) {
+		print_selftest_result(m, TestResult_FW_DiffMax, RecordResult_FW_DiffMax, RawData_Diff_Max, X_Channel, Y_Channel);
+	} else { // TestResult_Noise is -1
+		nvt_mp_seq_printf(m, " FAIL!\n");
+
+		if (TestResult_FW_DiffMax == -1) {
+			nvt_mp_seq_printf(m, "FW Diff Max");
+			print_selftest_result(m, TestResult_FW_DiffMax, RecordResult_FW_DiffMax, RawData_Diff_Max, X_Channel, Y_Channel);
+		}
+		if (TestResult_FW_DiffMin == -1) {
+			nvt_mp_seq_printf(m, "FW Diff Min");
+			print_selftest_result(m, TestResult_FW_DiffMin, RecordResult_FW_DiffMin, RawData_Diff_Min, X_Channel, Y_Channel);
+		}
+	}
+
+	nvt_mp_test_result_printed = 1;
+
+	NVT_LOG("--\n");
+
+    return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen self-test sequence print start
+	function.
+
+return:
+	Executive outcomes. 1---call next function.
+	NULL---not call next function and sequence loop
+	stop.
+*******************************************************/
+static void *c_start(struct seq_file *m, loff_t *pos)
+{
+	return *pos < 1 ? (void *)1 : NULL;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen self-test sequence print next
+	function.
+
+return:
+	Executive outcomes. NULL---no next and call sequence
+	stop function.
+*******************************************************/
+static void *c_next(struct seq_file *m, void *v, loff_t *pos)
+{
+	++*pos;
+	return NULL;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen self-test sequence print stop
+	function.
+
+return:
+	n.a.
+*******************************************************/
+static void c_stop(struct seq_file *m, void *v)
+{
+	return;
+}
+
+const struct seq_operations nvt_selftest_seq_ops = {
+	.start  = c_start,
+	.next   = c_next,
+	.stop   = c_stop,
+	.show   = c_show_selftest
+};
+
+/*******************************************************
+Description:
+	Novatek touchscreen /proc/nvt_selftest open function.
+
+return:
+	Executive outcomes. 0---succeed. negative---failed.
+*******************************************************/
+static int32_t nvt_selftest_open(struct inode *inode, struct file *file)
+{
+	struct device_node *np = ts->client->dev.of_node;
+	unsigned char mpcriteria[PAGE_SIZE] = {0};	//novatek-mp-criteria-default
+
+	TestResult_Short = 0;
+	TestResult_Open = 0;
+	TestResult_FW_Rawdata = 0;
+	TestResult_FWMutual = 0;
+	TestResult_FW_CC = 0;
+	TestResult_Noise = 0;
+	TestResult_FW_DiffMax = 0;
+	TestResult_FW_DiffMin = 0;
+
+	NVT_LOG("++\n");
+
+	if (mutex_lock_interruptible(&ts->lock)) {
+		return -ERESTARTSYS;
+	}
+
+#if NVT_TOUCH_ESD_PROTECT
+	nvt_esd_check_enable(false);
+#endif /* #if NVT_TOUCH_ESD_PROTECT */
+
+	if (nvt_get_fw_info()) {
+		mutex_unlock(&ts->lock);
+		NVT_ERR("get fw info failed!\n");
+		return -EAGAIN;
+	}
+
+	fw_ver = ts->fw_ver;
+
+	/* Parsing criteria from dts */
+	if(of_property_read_bool(np, "novatek,mp-support-dt")) {
+		/*
+		 * Parsing Criteria by Novatek PID
+		 * The string rule is "novatek-mp-criteria-<nvt_pid>"
+		 * nvt_pid is 2 bytes (show hex).
+		 *
+		 * Ex. nvt_pid = 500A
+		 *     mpcriteria = "novatek-mp-criteria-500A"
+		 */
+		snprintf(mpcriteria, PAGE_SIZE, "novatek-mp-criteria-%04X", ts->nvt_pid);
+
+		if (nvt_mp_parse_dt(np, mpcriteria)) {
+			mutex_unlock(&ts->lock);
+			NVT_ERR("mp parse device tree failed!\n");
+			return -EINVAL;
+		}
+	} else {
+		NVT_LOG("Not found novatek,mp-support-dt, use default setting\n");
+		//---Print Test Criteria---
+		nvt_print_criteria();
+	}
+
+	if (nvt_switch_FreqHopEnDis(FREQ_HOP_DISABLE)) {
+		mutex_unlock(&ts->lock);
+		NVT_ERR("switch frequency hopping disable failed!\n");
+		return -EAGAIN;
+	}
+
+	if (nvt_check_fw_reset_state(RESET_STATE_NORMAL_RUN)) {
+		mutex_unlock(&ts->lock);
+		NVT_ERR("check fw reset state failed!\n");
+		return -EAGAIN;
+	}
+
+	msleep(100);
+
+	//---Enter Test Mode---
+	if (nvt_clear_fw_status()) {
+		mutex_unlock(&ts->lock);
+		NVT_ERR("clear fw status failed!\n");
+		return -EAGAIN;
+	}
+
+	nvt_change_mode(MP_MODE_CC);
+
+	if (nvt_check_fw_status()) {
+		mutex_unlock(&ts->lock);
+		NVT_ERR("check fw status failed!\n");
+		return -EAGAIN;
+	}
+
+	//---FW Rawdata Test---
+	if (nvt_read_baseline(RawData_FWMutual) != 0) {
+		TestResult_FWMutual = 1;
+	} else {
+		TestResult_FWMutual = RawDataTest_SinglePoint_Sub(RawData_FWMutual, RecordResult_FWMutual, X_Channel, Y_Channel,
+						PS_Config_Lmt_FW_Rawdata_P, PS_Config_Lmt_FW_Rawdata_N);
+	}
+	if (nvt_read_CC(RawData_FW_CC) != 0) {
+		TestResult_FW_CC = 1;
+	} else {
+		TestResult_FW_CC = RawDataTest_SinglePoint_Sub(RawData_FW_CC, RecordResult_FW_CC, X_Channel, Y_Channel,
+						PS_Config_Lmt_FW_CC_P, PS_Config_Lmt_FW_CC_N);
+	}
+
+	if ((TestResult_FWMutual == 1) || (TestResult_FW_CC == 1)) {
+		TestResult_FW_Rawdata = 1;
+	} else {
+		if ((TestResult_FWMutual == -1) || (TestResult_FW_CC == -1))
+			TestResult_FW_Rawdata = -1;
+		else
+			TestResult_FW_Rawdata = 0;
+	}
+
+	//---Leave Test Mode---
+	nvt_change_mode(NORMAL_MODE);
+
+	//---Noise Test---
+	if (nvt_read_fw_noise(RawData_Diff) != 0) {
+		TestResult_Noise = 1;	// 1: ERROR
+		TestResult_FW_DiffMax = 1;
+		TestResult_FW_DiffMin = 1;
+	} else {
+		TestResult_FW_DiffMax = RawDataTest_SinglePoint_Sub(RawData_Diff_Max, RecordResult_FW_DiffMax, X_Channel, Y_Channel,
+						PS_Config_Lmt_FW_Diff_P, PS_Config_Lmt_FW_Diff_N);
+
+		TestResult_FW_DiffMin = RawDataTest_SinglePoint_Sub(RawData_Diff_Min, RecordResult_FW_DiffMin, X_Channel, Y_Channel,
+						PS_Config_Lmt_FW_Diff_P, PS_Config_Lmt_FW_Diff_N);
+
+		if ((TestResult_FW_DiffMax == -1) || (TestResult_FW_DiffMin == -1))
+			TestResult_Noise = -1;
+		else
+			TestResult_Noise = 0;
+	}
+
+	//--Short Test---
+	if (nvt_read_fw_short(RawData_Short) != 0) {
+		TestResult_Short = 1; // 1:ERROR
+	} else {
+		//---Self Test Check --- // 0:PASS, -1:FAIL
+		TestResult_Short = RawDataTest_SinglePoint_Sub(RawData_Short, RecordResult_Short, X_Channel, Y_Channel,
+						PS_Config_Lmt_Short_Rawdata_P, PS_Config_Lmt_Short_Rawdata_N);
+	}
+
+	//---Open Test---
+	if (nvt_read_fw_open(RawData_Open) != 0) {
+		TestResult_Open = 1;    // 1:ERROR
+	} else {
+		//---Self Test Check --- // 0:PASS, -1:FAIL
+		TestResult_Open = RawDataTest_SinglePoint_Sub(RawData_Open, RecordResult_Open, X_Channel, Y_Channel,
+						PS_Config_Lmt_Open_Rawdata_P, PS_Config_Lmt_Open_Rawdata_N);
+	}
+
+	//---Reset IC---
+	nvt_bootloader_reset();
+
+	mutex_unlock(&ts->lock);
+
+	NVT_LOG("--\n");
+
+	nvt_mp_test_result_printed = 0;
+
+	return seq_open(file, &nvt_selftest_seq_ops);
+}
+
+static const struct proc_ops nvt_selftest_fops = {
+	.proc_open = nvt_selftest_open,
+	.proc_read = seq_read,
+	.proc_lseek = seq_lseek,
+	.proc_release = seq_release,
+};
+
+#ifdef CONFIG_OF
+/*******************************************************
+Description:
+	Novatek touchscreen parse AIN setting for array type.
+
+return:
+	n.a.
+*******************************************************/
+int32_t nvt_mp_parse_ain(struct device_node *np, const char *name, uint8_t *array, int32_t size)
+{
+	struct property *data;
+	int32_t len, ret;
+	int32_t tmp[40];
+	int32_t i;
+
+	data = of_find_property(np, name, &len);
+	len /= sizeof(u32);
+	if ((!data) || (!len) || (len != size)) {
+		NVT_ERR("error find %s. len=%d\n", name, len);
+		return -1;
+	} else {
+		NVT_LOG("%s. len=%d\n", name, len);
+		ret = of_property_read_u32_array(np, name, tmp, len);
+		if (ret) {
+			NVT_ERR("error reading %s. ret=%d\n", name, ret);
+			return -1;
+		}
+
+		for (i = 0; i < len; i++)
+			array[i] = tmp[i];
+
+#if NVT_DEBUG
+		printk("[NVT-ts] %s = ", name);
+		nvt_print_result_log_in_one_line(array, len);
+		printk("\n");
+#endif
+	}
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen parse criterion for u32 type.
+
+return:
+	n.a.
+*******************************************************/
+int32_t nvt_mp_parse_u32(struct device_node *np, const char *name, int32_t *para)
+{
+	int32_t ret;
+
+	ret = of_property_read_u32(np, name, para);
+	if (ret) {
+		NVT_ERR("error reading %s. ret=%d\n", name, ret);
+		return -1;
+	} else {
+#if NVT_DEBUG
+		NVT_LOG("%s=%d\n", name, *para);
+#endif
+	}
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen parse criterion for array type.
+
+return:
+	n.a.
+*******************************************************/
+int32_t nvt_mp_parse_array(struct device_node *np, const char *name, int32_t *array,
+		int32_t size)
+{
+	struct property *data;
+	int32_t len, ret;
+#if NVT_DEBUG
+	int32_t j = 0;
+#endif
+
+	data = of_find_property(np, name, &len);
+	len /= sizeof(u32);
+	if ((!data) || (!len) || (len < size)) {
+		NVT_ERR("error find %s. len=%d\n", name, len);
+		return -1;
+	} else {
+		NVT_LOG("%s. len=%d\n", name, len);
+		ret = of_property_read_u32_array(np, name, array, len);
+		if (ret) {
+			NVT_ERR("error reading %s. ret=%d\n", name, ret);
+			return -1;
+		}
+
+#if NVT_DEBUG
+		NVT_LOG("%s =\n", name);
+		for (j = 0; j < Y_Channel; j++) {
+			nvt_print_data_log_in_one_line(array + j * X_Channel, X_Channel);
+			printk("\n");
+		}
+#if TOUCH_KEY_NUM > 0
+		nvt_print_data_log_in_one_line(array + Y_Channel * X_Channel, Key_Channel);
+		printk("\n");
+#endif
+#endif
+	}
+
+	return 0;
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen parse device tree mp function.
+
+return:
+	n.a.
+*******************************************************/
+int32_t nvt_mp_parse_dt(struct device_node *root, const char *node_compatible)
+{
+	struct device_node *np = root;
+	struct device_node *child = NULL;
+
+	NVT_LOG("Parse mp criteria for node %s\n", node_compatible);
+
+	/* find each MP sub-nodes */
+	for_each_child_of_node(root, child) {
+		/* find the specified node */
+		if (of_device_is_compatible(child, node_compatible)) {
+			NVT_LOG("found child node %s\n", node_compatible);
+			np = child;
+			break;
+		}
+	}
+	if (child == NULL) {
+		NVT_ERR("Not found compatible node %s!\n", node_compatible);
+		return -1;
+	}
+
+	/* MP Config*/
+	if (nvt_mp_parse_u32(np, "IC_X_CFG_SIZE", &IC_X_CFG_SIZE))
+		return -1;
+
+	if (nvt_mp_parse_u32(np, "IC_Y_CFG_SIZE", &IC_Y_CFG_SIZE))
+		return -1;
+
+#if TOUCH_KEY_NUM > 0
+	if (nvt_mp_parse_u32(np, "IC_KEY_CFG_SIZE", &IC_KEY_CFG_SIZE))
+		return -1;
+#endif
+
+	if (nvt_mp_parse_u32(np, "X_Channel", &X_Channel))
+		return -1;
+
+	if (nvt_mp_parse_u32(np, "Y_Channel", &Y_Channel))
+		return -1;
+
+	if (nvt_mp_parse_ain(np, "AIN_X", AIN_X, IC_X_CFG_SIZE))
+		return -1;
+
+	if (nvt_mp_parse_ain(np, "AIN_Y", AIN_Y, IC_Y_CFG_SIZE))
+		return -1;
+
+#if TOUCH_KEY_NUM > 0
+	if (nvt_mp_parse_ain(np, "AIN_KEY", AIN_KEY, IC_KEY_CFG_SIZE))
+		return -1;
+#endif
+
+	/* MP Criteria */
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_Short_Rawdata_P", PS_Config_Lmt_Short_Rawdata_P,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_Short_Rawdata_N", PS_Config_Lmt_Short_Rawdata_N,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_Open_Rawdata_P", PS_Config_Lmt_Open_Rawdata_P,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_Open_Rawdata_N", PS_Config_Lmt_Open_Rawdata_N,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_FW_Rawdata_P", PS_Config_Lmt_FW_Rawdata_P,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_FW_Rawdata_N", PS_Config_Lmt_FW_Rawdata_N,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_FW_CC_P", PS_Config_Lmt_FW_CC_P,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_FW_CC_N", PS_Config_Lmt_FW_CC_N,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_FW_Diff_P", PS_Config_Lmt_FW_Diff_P,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_array(np, "PS_Config_Lmt_FW_Diff_N", PS_Config_Lmt_FW_Diff_N,
+			X_Channel * Y_Channel + Key_Channel))
+		return -1;
+
+	if (nvt_mp_parse_u32(np, "PS_Config_Diff_Test_Frame", &PS_Config_Diff_Test_Frame))
+		return -1;
+
+	NVT_LOG("Parse mp criteria done!\n");
+
+	return 0;
+}
+#endif /* #ifdef CONFIG_OF */
+
+/*******************************************************
+Description:
+	Novatek touchscreen MP function proc. file node
+	initial function.
+
+return:
+	Executive outcomes. 0---succeed. -1---failed.
+*******************************************************/
+int32_t nvt_mp_proc_init(void)
+{
+	NVT_proc_selftest_entry = proc_create("nvt_selftest", 0444, NULL, &nvt_selftest_fops);
+	if (NVT_proc_selftest_entry == NULL) {
+		NVT_ERR("create /proc/nvt_selftest Failed!\n");
+		return -1;
+	} else {
+		if(nvt_mp_buffer_init()) {
+			NVT_ERR("Allocate mp memory failed\n");
+			return -1;
+		}
+		else {
+			NVT_LOG("create /proc/nvt_selftest Succeeded!\n");
+		}
+		return 0;
+	}
+}
+
+/*******************************************************
+Description:
+	Novatek touchscreen MP function proc. file node
+	deinitial function.
+
+return:
+	n.a.
+*******************************************************/
+void nvt_mp_proc_deinit(void)
+{
+	nvt_mp_buffer_deinit();
+
+	if (NVT_proc_selftest_entry != NULL) {
+		remove_proc_entry("nvt_selftest", NULL);
+		NVT_proc_selftest_entry = NULL;
+		NVT_LOG("Removed /proc/%s\n", "nvt_selftest");
+	}
+}
+#endif /* #if NVT_TOUCH_MP */

nt36xxx/nt36xxx_mp_ctrlram.h

@@ -0,0 +1,460 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2010 - 2018 Novatek, Inc.
+ *
+ * $Revision: 46179 $
+ * $Date: 2019-06-14 13:47:17 +0800 (Fri, 14 Jun 2019) $
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+
+#if NVT_TOUCH_MP
+
+static uint32_t IC_X_CFG_SIZE = 18;
+static uint32_t IC_Y_CFG_SIZE = 36;
+static uint32_t IC_KEY_CFG_SIZE = 4;
+static uint32_t X_Channel = 18;
+static uint32_t Y_Channel = 36;
+static uint32_t Key_Channel = TOUCH_KEY_NUM;
+static uint8_t AIN_X[40] =
+	{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17};
+static uint8_t AIN_Y[40] =
+	{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17,
+	18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35};
+
+#if TOUCH_KEY_NUM > 0
+static uint8_t AIN_KEY[8] = {0, 1, 2, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+#endif /* #if TOUCH_KEY_NUM > 0 */
+
+static int32_t PS_Config_Lmt_Short_Rawdata_P[40 * 40] = {
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+	14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,14008,
+#if TOUCH_KEY_NUM > 0
+	14008,14008,14008,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_Short_Rawdata_N[40 * 40] = {
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+	10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,10000,
+#if TOUCH_KEY_NUM > 0
+	10000,10000,10000,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_Open_Rawdata_P[40 * 40] = {
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+	5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,5120,
+#if TOUCH_KEY_NUM > 0
+	5120,5120,5120,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_Open_Rawdata_N[40 * 40] = {
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+	50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,
+#if TOUCH_KEY_NUM > 0
+	50,50,50,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_FW_Rawdata_P[40 * 40] = {
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+	2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,2560,
+#if TOUCH_KEY_NUM > 0
+	2560,2560,2560,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_FW_Rawdata_N[40 * 40] = {
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+	240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,240,
+#if TOUCH_KEY_NUM > 0
+	240,240,240,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_FW_CC_P[40 * 40] = {
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+	314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,314,
+#if TOUCH_KEY_NUM > 0
+	314,314,314,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_FW_CC_N[40 * 40] = {
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+#if TOUCH_KEY_NUM > 0
+	0,0,0,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_FW_Diff_P[40 * 40] = {
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+	75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,
+#if TOUCH_KEY_NUM > 0
+	75,75,75,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Lmt_FW_Diff_N[40 *40] = {
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+	-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,-75,
+#if TOUCH_KEY_NUM > 0
+	-75,-75,-75,
+#endif /* #if TOUCH_KEY_NUM > 0 */
+};
+
+static int32_t PS_Config_Diff_Test_Frame = 50;
+
+#endif /* #if NVT_TOUCH_MP */

nt36xxx/nt36xxx_spi.c

@@ -0,0 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2021 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#define NVT_NT36XXX_SPI
+
+#include "nt36xxx.c"

nt36xxx/nt36xxx_spi_ext_proc.c

@@ -0,0 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2021 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#define NVT_NT36XXX_SPI
+
+#include "nt36xxx_ext_proc.c"

nt36xxx/nt36xxx_spi_fw_update.c

@@ -0,0 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2021 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#define NVT_NT36XXX_SPI
+
+#include "nt36xxx_fw_update.c"

st/fts.c

@@ -0,0 +1,6066 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group)
+ *
+ *		[email protected]
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/irq.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/completion.h>
+/*#include <linux/wakelock.h>*/
+#include <linux/pm_wakeup.h>
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+#include <linux/of_irq.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/consumer.h>
+
+#if defined(CONFIG_FB_MSM)
+#include <linux/notifier.h>
+#include <linux/fb.h>
+#else
+#include <drm/drm_panel.h>
+#endif
+
+#ifdef KERNEL_ABOVE_2_6_38
+#include <linux/input/mt.h>
+#endif
+
+#include "fts.h"
+#include "fts_lib/ftsCompensation.h"
+#include "fts_lib/ftsIO.h"
+#include "fts_lib/ftsError.h"
+#include "fts_lib/ftsFlash.h"
+#include "fts_lib/ftsFrame.h"
+#include "fts_lib/ftsGesture.h"
+#include "fts_lib/ftsTest.h"
+#include "fts_lib/ftsTime.h"
+#include "fts_lib/ftsTool.h"
+#include "linux/moduleparam.h"
+
+#if defined(CONFIG_ST_TRUSTED_TOUCH)
+
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+
+#include <linux/debugfs.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/kobject.h>
+#include <linux/sysfs.h>
+#include "linux/gunyah/gh_irq_lend.h"
+#include "linux/gunyah/gh_msgq.h"
+#include "linux/gunyah/gh_mem_notifier.h"
+#include "linux/gunyah/gh_rm_drv.h"
+#include <linux/sort.h>
+#endif
+
+#define LINK_KOBJ_NAME "tp"
+
+#define FTS_DVDD_VOL_MIN 1800000
+#define FTS_DVDD_VOL_MAX 1800000
+#define FTS_DVDD_LOAD 20000
+#define FTS_AVDD_VOL_MIN 3000000
+#define FTS_AVDD_VOL_MAX 3300000
+#define FTS_AVDD_LOAD 20000
+
+/*
+ * Uncomment to use polling mode instead of interrupt mode.
+ *
+ */
+// #define FTS_USE_POLLING_MODE
+
+/*
+ * Event installer helpers
+ */
+#define event_id(_e) EVENTID_##_e
+#define handler_name(_h) fts_##_h##_event_handler
+
+#define install_handler(_i, _evt, _hnd) \
+	(_i->event_dispatch_table[event_id(_evt)].handler = handler_name(_hnd))
+
+/*
+ * Asyncronouns command helper
+ */
+#define WAIT_WITH_TIMEOUT(_info, _timeout, _command) \
+do { \
+	if (wait_for_completion_timeout(&_info->cmd_done, _timeout) == 0) { \
+		dev_warn(_info->dev, "Waiting for %s command: timeout\n", \
+		#_command); \
+	} \
+} while (0)
+
+#ifdef KERNEL_ABOVE_2_6_38
+#define TYPE_B_PROTOCOL
+#endif
+
+#if defined(SCRIPTLESS) || defined(DRIVER_TEST)
+static struct class *fts_cmd_class;
+#endif
+
+static void fts_interrupt_disable(struct fts_ts_info *info);
+static void fts_interrupt_enable(struct fts_ts_info *info);
+static irqreturn_t fts_interrupt_handler(int irq, void *handle);
+static int fts_probe_delayed(struct fts_ts_info *info);
+
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+
+static struct gh_acl_desc *fts_vm_get_acl(enum gh_vm_names vm_name)
+{
+	struct gh_acl_desc *acl_desc;
+	gh_vmid_t vmid;
+
+	gh_rm_get_vmid(vm_name, &vmid);
+
+	acl_desc = kzalloc(offsetof(struct gh_acl_desc, acl_entries[1]),
+			GFP_KERNEL);
+	if (!acl_desc)
+		return ERR_PTR(ENOMEM);
+
+	acl_desc->n_acl_entries = 1;
+	acl_desc->acl_entries[0].vmid = vmid;
+	acl_desc->acl_entries[0].perms = GH_RM_ACL_R | GH_RM_ACL_W;
+
+	return acl_desc;
+}
+
+static struct gh_sgl_desc *fts_vm_get_sgl(struct trusted_touch_vm_info *vm_info)
+{
+	struct gh_sgl_desc *sgl_desc;
+	int i;
+
+	sgl_desc = kzalloc(offsetof(struct gh_sgl_desc,
+			sgl_entries[vm_info->iomem_list_size]), GFP_KERNEL);
+	if (!sgl_desc)
+		return ERR_PTR(ENOMEM);
+
+	sgl_desc->n_sgl_entries = vm_info->iomem_list_size;
+
+	for (i = 0; i < vm_info->iomem_list_size; i++) {
+		sgl_desc->sgl_entries[i].ipa_base = vm_info->iomem_bases[i];
+		sgl_desc->sgl_entries[i].size = vm_info->iomem_sizes[i];
+	}
+
+	return sgl_desc;
+}
+
+static int fts_populate_vm_info(struct fts_ts_info *info)
+{
+	int rc = 0;
+	struct trusted_touch_vm_info *vm_info;
+	struct device_node *np = info->client->dev.of_node;
+	int num_regs, num_sizes = 0;
+
+	vm_info = kzalloc(sizeof(struct trusted_touch_vm_info), GFP_KERNEL);
+	if (!vm_info) {
+		rc = -ENOMEM;
+		goto error;
+	}
+
+	info->vm_info = vm_info;
+	vm_info->irq_label = GH_IRQ_LABEL_TRUSTED_TOUCH;
+	vm_info->vm_name = GH_TRUSTED_VM;
+	rc = of_property_read_u32(np, "st,trusted-touch-spi-irq",
+			&vm_info->hw_irq);
+	if (rc) {
+		pr_err("Failed to read trusted touch SPI irq:%d\n", rc);
+		goto vm_error;
+	}
+	num_regs = of_property_count_u32_elems(np,
+			"st,trusted-touch-io-bases");
+	if (num_regs < 0) {
+		pr_err("Invalid number of IO regions specified\n");
+		rc = -EINVAL;
+		goto vm_error;
+	}
+
+	num_sizes = of_property_count_u32_elems(np,
+			"st,trusted-touch-io-sizes");
+	if (num_sizes < 0) {
+		pr_err("Invalid number of IO regions specified\n");
+		rc = -EINVAL;
+		goto vm_error;
+	}
+
+	if (num_regs != num_sizes) {
+		pr_err("IO bases and sizes doe not match\n");
+		rc = -EINVAL;
+		goto vm_error;
+	}
+
+	vm_info->iomem_list_size = num_regs;
+
+	vm_info->iomem_bases = kcalloc(num_regs, sizeof(*vm_info->iomem_bases),
+								GFP_KERNEL);
+	if (!vm_info->iomem_bases) {
+		rc = -ENOMEM;
+		goto vm_error;
+	}
+
+	rc = of_property_read_u32_array(np, "st,trusted-touch-io-bases",
+			vm_info->iomem_bases, vm_info->iomem_list_size);
+	if (rc) {
+		pr_err("Failed to read trusted touch io bases:%d\n", rc);
+		goto io_bases_error;
+	}
+
+	vm_info->iomem_sizes = kzalloc(
+			sizeof(*vm_info->iomem_sizes) * num_sizes, GFP_KERNEL);
+	if (!vm_info->iomem_sizes) {
+		rc = -ENOMEM;
+		goto io_bases_error;
+	}
+
+	rc = of_property_read_u32_array(np, "st,trusted-touch-io-sizes",
+			vm_info->iomem_sizes, vm_info->iomem_list_size);
+	if (rc) {
+		pr_err("Failed to read trusted touch io sizes:%d\n", rc);
+		goto io_sizes_error;
+	}
+	return rc;
+
+io_sizes_error:
+	kfree(vm_info->iomem_sizes);
+io_bases_error:
+	kfree(vm_info->iomem_bases);
+vm_error:
+	kfree(vm_info);
+error:
+	return rc;
+}
+
+static void fts_destroy_vm_info(struct fts_ts_info *info)
+{
+	kfree(info->vm_info->iomem_sizes);
+	kfree(info->vm_info->iomem_bases);
+	kfree(info->vm_info);
+}
+
+static void fts_vm_deinit(struct fts_ts_info *info)
+{
+	if (info->vm_info->mem_cookie)
+		gh_mem_notifier_unregister(info->vm_info->mem_cookie);
+	fts_destroy_vm_info(info);
+}
+
+#ifdef CONFIG_ARCH_QTI_VM
+static int fts_vm_mem_release(struct fts_ts_info *info);
+static void fts_trusted_touch_vm_mode_disable(struct fts_ts_info *info);
+
+static int fts_sgl_cmp(const void *a, const void *b)
+{
+	struct gh_sgl_entry *left = (struct gh_sgl_entry *)a;
+	struct gh_sgl_entry *right = (struct gh_sgl_entry *)b;
+
+	return (left->ipa_base - right->ipa_base);
+}
+
+static int fts_vm_compare_sgl_desc(struct gh_sgl_desc *expected,
+		struct gh_sgl_desc *received)
+{
+	int idx;
+
+	if (expected->n_sgl_entries != received->n_sgl_entries)
+		return -E2BIG;
+	sort(received->sgl_entries, received->n_sgl_entries,
+			sizeof(received->sgl_entries[0]), fts_sgl_cmp, NULL);
+	sort(expected->sgl_entries, expected->n_sgl_entries,
+			sizeof(expected->sgl_entries[0]), fts_sgl_cmp, NULL);
+
+	for (idx = 0; idx < expected->n_sgl_entries; idx++) {
+		struct gh_sgl_entry *left = &expected->sgl_entries[idx];
+		struct gh_sgl_entry *right = &received->sgl_entries[idx];
+
+		if ((left->ipa_base != right->ipa_base) ||
+				(left->size != right->size)) {
+			pr_err("sgl mismatch: left_base:%d right base:%d left size:%d right size:%d\n",
+					left->ipa_base, right->ipa_base,
+					left->size, right->size);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int fts_vm_handle_vm_hardware(struct fts_ts_info *info)
+{
+	int rc = 0;
+
+	if (atomic_read(&info->delayed_vm_probe_pending)) {
+		rc = fts_probe_delayed(info);
+		if (rc) {
+			pr_err(" Delayed probe failure on VM!\n");
+			return rc;
+		}
+		atomic_set(&info->delayed_vm_probe_pending, 0);
+		return rc;
+	}
+
+	queue_delayed_work(info->fwu_workqueue, &info->fwu_work,
+			msecs_to_jiffies(EXP_FN_WORK_DELAY_MS));
+	fts_interrupt_enable(info);
+	return rc;
+}
+
+static void fts_vm_irq_on_lend_callback(void *data,
+					unsigned long notif_type,
+					enum gh_irq_label label)
+{
+	struct fts_ts_info *info = data;
+	struct irq_data *irq_data;
+	int irq = 0;
+	int const resource_timeout = msecs_to_jiffies(2000);
+	int rc = 0;
+
+	irq = gh_irq_accept(info->vm_info->irq_label, -1, IRQ_TYPE_LEVEL_HIGH);
+	if (irq < 0) {
+		pr_err("failed to accept irq\n");
+		goto irq_fail;
+	}
+	atomic_set(&info->vm_info->tvm_owns_irq, 1);
+	irq_data = irq_get_irq_data(irq);
+	if (!irq_data) {
+		pr_err("Invalid irq data for trusted touch\n");
+		goto irq_fail;
+	}
+	if (!irq_data->hwirq) {
+		pr_err("Invalid irq in irq data\n");
+		goto irq_fail;
+	}
+	if (irq_data->hwirq != info->vm_info->hw_irq) {
+		pr_err("Invalid irq lent\n");
+		goto irq_fail;
+	}
+
+	pr_debug("irq:returned from accept:%d\n", irq);
+	info->client->irq = irq;
+	if (!wait_for_completion_timeout(&info->resource_checkpoint,
+				resource_timeout)) {
+		pr_err("Resources not acquired in TVM\n");
+		goto irq_fail;
+	}
+
+	rc = fts_vm_handle_vm_hardware(info);
+	if (rc) {
+		pr_err(" Delayed probe failure on VM!\n");
+		goto irq_fail;
+	}
+
+	atomic_set(&info->trusted_touch_enabled, 1);
+	return;
+irq_fail:
+	fts_trusted_touch_vm_mode_disable(info);
+}
+
+static void fts_vm_mem_on_lend_handler(enum gh_mem_notifier_tag tag,
+		unsigned long notif_type, void *entry_data, void *notif_msg)
+{
+	struct gh_rm_notif_mem_shared_payload *payload;
+	struct gh_sgl_desc *sgl_desc, *expected_sgl_desc;
+	struct gh_acl_desc *acl_desc;
+	struct trusted_touch_vm_info *vm_info;
+	struct fts_ts_info *info;
+	int rc = 0;
+
+	if (notif_type != GH_RM_NOTIF_MEM_SHARED ||
+			tag != GH_MEM_NOTIFIER_TAG_TOUCH) {
+		pr_err("Invalid command passed from rm\n");
+		return;
+	}
+
+	if (!entry_data || !notif_msg) {
+		pr_err("Invalid entry data passed from rm\n");
+		return;
+	}
+
+	info = (struct fts_ts_info *)entry_data;
+	vm_info = info->vm_info;
+	if (!vm_info) {
+		pr_err("Invalid vm_info\n");
+		return;
+	}
+
+	payload = (struct gh_rm_notif_mem_shared_payload  *)notif_msg;
+	if (payload->trans_type != GH_RM_TRANS_TYPE_LEND ||
+			payload->label != TRUSTED_TOUCH_MEM_LABEL) {
+		pr_err("Invalid label or transaction type\n");
+		goto onlend_fail;
+	}
+
+	acl_desc = fts_vm_get_acl(GH_TRUSTED_VM);
+	if (IS_ERR(acl_desc)) {
+		pr_err("failed to populated acl data:rc=%d\n",
+				PTR_ERR(acl_desc));
+		goto onlend_fail;
+	}
+
+	sgl_desc = gh_rm_mem_accept(payload->mem_handle, GH_RM_MEM_TYPE_IO,
+			GH_RM_TRANS_TYPE_LEND,
+			GH_RM_MEM_ACCEPT_VALIDATE_ACL_ATTRS |
+			GH_RM_MEM_ACCEPT_VALIDATE_LABEL |
+			GH_RM_MEM_ACCEPT_DONE,  payload->label, acl_desc,
+			NULL, NULL, 0);
+	if (IS_ERR_OR_NULL(sgl_desc)) {
+		pr_err("failed to do mem accept :rc=%d\n",
+				PTR_ERR(sgl_desc));
+		goto acl_fail;
+	}
+	atomic_set(&vm_info->tvm_owns_iomem, 1);
+
+	/* Initiate i2c session on tvm */
+	rc = pm_runtime_get_sync(info->client->adapter->dev.parent);
+	if (rc < 0) {
+		pr_err("failed to get sync rc:%d\n", rc);
+		(void)fts_vm_mem_release(info);
+		atomic_set(&info->vm_info->tvm_owns_iomem, 0);
+		goto acl_fail;
+	}
+	complete(&info->resource_checkpoint);
+
+	expected_sgl_desc = fts_vm_get_sgl(vm_info);
+	if (fts_vm_compare_sgl_desc(expected_sgl_desc, sgl_desc)) {
+		pr_err("IO sg list does not match\n");
+		goto sgl_cmp_fail;
+	}
+
+	vm_info->vm_mem_handle = payload->mem_handle;
+	kfree(expected_sgl_desc);
+	kfree(acl_desc);
+	return;
+
+sgl_cmp_fail:
+	kfree(expected_sgl_desc);
+acl_fail:
+	kfree(acl_desc);
+onlend_fail:
+	fts_trusted_touch_vm_mode_disable(info);
+}
+
+static int fts_vm_mem_release(struct fts_ts_info *info)
+{
+	int rc = 0;
+
+	rc = gh_rm_mem_release(info->vm_info->vm_mem_handle, 0);
+	if (rc)
+		pr_err("VM mem release failed: rc=%d\n", rc);
+
+	rc = gh_rm_mem_notify(info->vm_info->vm_mem_handle,
+				GH_RM_MEM_NOTIFY_OWNER_RELEASED,
+				GH_MEM_NOTIFIER_TAG_TOUCH, 0);
+	if (rc)
+		pr_err("Failed to notify mem release to PVM: rc=%d\n");
+
+	info->vm_info->vm_mem_handle = 0;
+	return rc;
+}
+
+static void fts_trusted_touch_vm_mode_disable(struct fts_ts_info *info)
+{
+	int rc = 0;
+
+
+	if (atomic_read(&info->vm_info->tvm_owns_iomem) &&
+			atomic_read(&info->vm_info->tvm_owns_irq))
+		fts_interrupt_disable(info);
+
+	if (atomic_read(&info->vm_info->tvm_owns_iomem)) {
+		flushFIFO();
+		release_all_touches(info);
+		rc = fts_vm_mem_release(info);
+		if (rc)
+			pr_err("Failed to release mem rc:%d\n", rc);
+		else
+			atomic_set(&info->vm_info->tvm_owns_iomem, 0);
+		pm_runtime_put_sync(info->client->adapter->dev.parent);
+	}
+
+	if (atomic_read(&info->vm_info->tvm_owns_irq)) {
+		rc = gh_irq_release(info->vm_info->irq_label);
+		if (rc)
+			pr_err("Failed to release irq rc:%d\n", rc);
+		else
+			atomic_set(&info->vm_info->tvm_owns_irq, 0);
+
+		rc = gh_irq_release_notify(info->vm_info->irq_label);
+		if (rc)
+			pr_err("Failed to notify release irq rc:%d\n", rc);
+	}
+	atomic_set(&info->trusted_touch_enabled, 0);
+	reinit_completion(&info->resource_checkpoint);
+	pr_debug("trusted touch disabled\n");
+}
+
+static int fts_handle_trusted_touch_tvm(struct fts_ts_info *info, int value)
+{
+	int err = 0;
+
+	switch (value) {
+	case 0:
+		if (atomic_read(&info->trusted_touch_enabled) == 0) {
+			pr_err("Trusted touch is already disabled\n");
+			break;
+		}
+		if (atomic_read(&info->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			fts_trusted_touch_vm_mode_disable(info);
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	case 1:
+		if (atomic_read(&info->trusted_touch_enabled)) {
+			pr_err("Trusted touch usecase underway\n");
+			err = -EBUSY;
+			break;
+		}
+		if (atomic_read(&info->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			pr_err("Cannot turnon trusted touch(vm mode) in VM\n");
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	default:
+		dev_err(&info->client->dev, "unsupported value: %lu\n", value);
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+#else
+
+static int fts_clk_prepare_enable(struct fts_ts_info *info)
+{
+	int ret;
+
+	ret = clk_prepare_enable(info->iface_clk);
+	if (ret) {
+		dev_err(&info->client->dev,
+			"error on clk_prepare_enable(iface_clk):%d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(info->core_clk);
+	if (ret) {
+		clk_disable_unprepare(info->iface_clk);
+		dev_err(&info->client->dev,
+			"error clk_prepare_enable(core_clk):%d\n", ret);
+	}
+	return ret;
+}
+
+static void fts_clk_disable_unprepare(struct fts_ts_info *info)
+{
+	clk_disable_unprepare(info->core_clk);
+	clk_disable_unprepare(info->iface_clk);
+}
+
+static int fts_bus_get(struct fts_ts_info *info)
+{
+	int rc = 0;
+
+	mutex_lock(&info->fts_clk_io_ctrl_mutex);
+	rc = pm_runtime_get_sync(info->client->adapter->dev.parent);
+	if (rc >= 0 &&  info->core_clk != NULL && info->iface_clk != NULL) {
+		rc = fts_clk_prepare_enable(info);
+		if (rc)
+			pm_runtime_put_sync(info->client->adapter->dev.parent);
+	}
+	mutex_unlock(&info->fts_clk_io_ctrl_mutex);
+	return rc;
+}
+
+static void fts_bus_put(struct fts_ts_info *info)
+{
+	mutex_lock(&info->fts_clk_io_ctrl_mutex);
+	if (info->core_clk != NULL && info->iface_clk != NULL)
+		fts_clk_disable_unprepare(info);
+	pm_runtime_put_sync(info->client->adapter->dev.parent);
+	mutex_unlock(&info->fts_clk_io_ctrl_mutex);
+}
+
+static struct gh_notify_vmid_desc *fts_vm_get_vmid(gh_vmid_t vmid)
+{
+	struct gh_notify_vmid_desc *vmid_desc;
+
+	vmid_desc = kzalloc(offsetof(struct gh_notify_vmid_desc,
+				vmid_entries[1]), GFP_KERNEL);
+	if (!vmid_desc)
+		return ERR_PTR(ENOMEM);
+
+	vmid_desc->n_vmid_entries = 1;
+	vmid_desc->vmid_entries[0].vmid = vmid;
+	return vmid_desc;
+}
+
+static void fts_trusted_touch_complete(struct fts_ts_info *info)
+{
+	if (atomic_read(&info->vm_info->pvm_owns_iomem) &&
+			atomic_read(&info->vm_info->pvm_owns_irq)) {
+		fts_interrupt_enable(info);
+		fts_bus_put(info);
+		complete(&info->trusted_touch_powerdown);
+		atomic_set(&info->trusted_touch_enabled, 0);
+		pr_debug("reenabled interrupts on PVM\n");
+	} else {
+		pr_err("PVM does not own irq and IOMEM\n");
+	}
+}
+
+static void fts_vm_irq_on_release_callback(void *data,
+					unsigned long notif_type,
+					enum gh_irq_label label)
+{
+	struct fts_ts_info *info = data;
+	int rc = 0;
+
+	rc = gh_irq_reclaim(info->vm_info->irq_label);
+	if (rc)
+		pr_err("failed to reclaim irq on pvm rc:%d\n", rc);
+	else
+		atomic_set(&info->vm_info->pvm_owns_irq, 1);
+}
+
+static void fts_vm_mem_on_release_handler(enum gh_mem_notifier_tag tag,
+		unsigned long notif_type, void *entry_data, void *notif_msg)
+{
+	struct gh_rm_notif_mem_released_payload *payload;
+	struct trusted_touch_vm_info *vm_info;
+	struct fts_ts_info *info;
+	int rc = 0;
+
+	if (notif_type != GH_RM_NOTIF_MEM_RELEASED ||
+			tag != GH_MEM_NOTIFIER_TAG_TOUCH) {
+		pr_err(" Invalid tag or command passed\n");
+		return;
+	}
+
+	if (!entry_data || !notif_msg) {
+		pr_err(" Invalid data or notification message\n");
+		return;
+	}
+
+	payload = (struct gh_rm_notif_mem_released_payload  *)notif_msg;
+	info = (struct fts_ts_info *)entry_data;
+	vm_info = info->vm_info;
+	if (!vm_info) {
+		pr_err(" Invalid vm_info\n");
+		return;
+	}
+
+	if (payload->mem_handle != vm_info->vm_mem_handle) {
+		pr_err("Invalid mem handle detected\n");
+		return;
+	}
+
+	rc = gh_rm_mem_reclaim(payload->mem_handle, 0);
+	if (rc) {
+		pr_err("Trusted touch VM mem release failed rc:%d\n", rc);
+		return;
+	}
+	atomic_set(&vm_info->pvm_owns_iomem, 1);
+	vm_info->vm_mem_handle = 0;
+}
+
+static int fts_vm_mem_lend(struct fts_ts_info *info)
+{
+	struct gh_acl_desc *acl_desc;
+	struct gh_sgl_desc *sgl_desc;
+	struct gh_notify_vmid_desc *vmid_desc;
+	gh_memparcel_handle_t mem_handle;
+	gh_vmid_t trusted_vmid;
+	int rc = 0;
+
+	acl_desc = fts_vm_get_acl(GH_TRUSTED_VM);
+	if (IS_ERR(acl_desc)) {
+		pr_err("Failed to get acl of IO memories for Trusted touch\n");
+		PTR_ERR(acl_desc);
+		return -EINVAL;
+	}
+
+	sgl_desc = fts_vm_get_sgl(info->vm_info);
+	if (IS_ERR(sgl_desc)) {
+		pr_err("Failed to get sgl of IO memories for Trusted touch\n");
+		PTR_ERR(sgl_desc);
+		rc = -EINVAL;
+		goto sgl_error;
+	}
+
+	rc = gh_rm_mem_lend(GH_RM_MEM_TYPE_IO, 0, TRUSTED_TOUCH_MEM_LABEL,
+			acl_desc, sgl_desc, NULL, &mem_handle);
+	if (rc) {
+		pr_err("Failed to lend IO memories for Trusted touch rc:%d\n",
+							rc);
+		goto error;
+	}
+
+	gh_rm_get_vmid(GH_TRUSTED_VM, &trusted_vmid);
+
+	vmid_desc = fts_vm_get_vmid(trusted_vmid);
+
+	rc = gh_rm_mem_notify(mem_handle, GH_RM_MEM_NOTIFY_RECIPIENT_SHARED,
+			GH_MEM_NOTIFIER_TAG_TOUCH, vmid_desc);
+	if (rc) {
+		pr_err("Failed to notify mem lend to hypervisor rc:%d\n", rc);
+		goto vmid_error;
+	}
+
+	info->vm_info->vm_mem_handle = mem_handle;
+vmid_error:
+	kfree(vmid_desc);
+error:
+	kfree(sgl_desc);
+sgl_error:
+	kfree(acl_desc);
+
+	return rc;
+}
+
+static int fts_trusted_touch_vm_mode_enable(struct fts_ts_info *info)
+{
+	int rc = 0;
+	struct trusted_touch_vm_info *vm_info = info->vm_info;
+
+	/* i2c session start and resource acquire */
+	if (fts_bus_get(info) < 0) {
+		dev_err(&info->client->dev, "fts_bus_get failed\n");
+		rc = -EIO;
+		return rc;
+	}
+
+	/* flush pending interurpts from FIFO */
+	fts_interrupt_disable(info);
+	flushFIFO();
+	release_all_touches(info);
+
+	rc = fts_vm_mem_lend(info);
+	if (rc) {
+		pr_err("Failed to lend memory\n");
+		return -EINVAL;
+	}
+	atomic_set(&vm_info->pvm_owns_iomem, 0);
+
+	rc = gh_irq_lend_v2(vm_info->irq_label, vm_info->vm_name,
+		info->client->irq, &fts_vm_irq_on_release_callback, info);
+	if (rc) {
+		pr_err("Failed to lend irq\n");
+		return -EINVAL;
+	}
+	atomic_set(&vm_info->pvm_owns_irq, 0);
+
+	rc = gh_irq_lend_notify(vm_info->irq_label);
+	if (rc) {
+		pr_err("Failed to notify irq\n");
+		return -EINVAL;
+	}
+
+	reinit_completion(&info->trusted_touch_powerdown);
+	atomic_set(&info->trusted_touch_enabled, 1);
+	pr_debug("trusted touch enabled\n");
+	return rc;
+}
+
+static int fts_handle_trusted_touch_pvm(struct fts_ts_info *info, int value)
+{
+	int err = 0;
+
+	switch (value) {
+	case 0:
+		if (atomic_read(&info->trusted_touch_enabled) == 0) {
+			pr_err("Trusted touch is already disabled\n");
+			break;
+		}
+		if (atomic_read(&info->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			fts_trusted_touch_complete(info);
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	case 1:
+		if (atomic_read(&info->trusted_touch_enabled)) {
+			pr_err("Trusted touch usecase underway\n");
+			err = -EBUSY;
+			break;
+		}
+		if (atomic_read(&info->trusted_touch_mode) ==
+				TRUSTED_TOUCH_VM_MODE) {
+			err = fts_trusted_touch_vm_mode_enable(info);
+		} else {
+			pr_err("Unsupported trusted touch mode\n");
+		}
+		break;
+
+	default:
+		dev_err(&info->client->dev, "unsupported value: %lu\n", value);
+		err = -EINVAL;
+		break;
+	}
+	return err;
+}
+#endif
+
+static int fts_vm_init(struct fts_ts_info *info)
+{
+	int rc = 0;
+	struct trusted_touch_vm_info *vm_info;
+	void *mem_cookie;
+
+	rc = fts_populate_vm_info(info);
+	if (rc) {
+		pr_err("Cannot setup vm pipeline\n");
+		rc = -EINVAL;
+		goto fail;
+	}
+
+	vm_info = info->vm_info;
+#ifdef CONFIG_ARCH_QTI_VM
+	mem_cookie = gh_mem_notifier_register(GH_MEM_NOTIFIER_TAG_TOUCH,
+			fts_vm_mem_on_lend_handler, info);
+	if (!mem_cookie) {
+		pr_err("Failed to register on lend mem notifier\n");
+		rc = -EINVAL;
+		goto init_fail;
+	}
+	vm_info->mem_cookie = mem_cookie;
+	rc = gh_irq_wait_for_lend_v2(vm_info->irq_label, GH_PRIMARY_VM,
+			&fts_vm_irq_on_lend_callback, info);
+	atomic_set(&vm_info->tvm_owns_irq, 0);
+	atomic_set(&vm_info->tvm_owns_iomem, 0);
+	init_completion(&info->resource_checkpoint);
+#else
+	mem_cookie = gh_mem_notifier_register(GH_MEM_NOTIFIER_TAG_TOUCH,
+			fts_vm_mem_on_release_handler, info);
+	if (!mem_cookie) {
+		pr_err("Failed to register on release mem notifier\n");
+		rc = -EINVAL;
+		goto init_fail;
+	}
+	vm_info->mem_cookie = mem_cookie;
+	atomic_set(&vm_info->pvm_owns_irq, 1);
+	atomic_set(&vm_info->pvm_owns_iomem, 1);
+#endif
+	return rc;
+init_fail:
+	fts_vm_deinit(info);
+fail:
+	return rc;
+}
+
+static void fts_dt_parse_trusted_touch_info(struct fts_ts_info *info)
+{
+	struct device_node *np = info->client->dev.of_node;
+	int rc = 0;
+	const char *selection;
+	const char *environment;
+
+	rc = of_property_read_string(np, "st,trusted-touch-mode",
+								&selection);
+	if (rc) {
+		dev_warn(&info->client->dev,
+			"%s: No trusted touch mode selection made\n", __func__);
+	}
+
+	if (!strcmp(selection, "vm_mode")) {
+		atomic_set(&info->trusted_touch_mode, TRUSTED_TOUCH_VM_MODE);
+		pr_err("Selected trusted touch mode to VM mode\n");
+	} else {
+		atomic_set(&info->trusted_touch_mode, TRUSTED_TOUCH_MODE_NONE);
+		pr_err("Invalid trusted_touch mode\n");
+	}
+
+	rc = of_property_read_string(np, "st,touch-environment",
+								&environment);
+	if (rc) {
+		dev_warn(&info->client->dev,
+			"%s: No trusted touch mode environment\n", __func__);
+	}
+	info->touch_environment = environment;
+	pr_err("Trusted touch environment:%s\n",
+			info->touch_environment);
+}
+
+static void fts_trusted_touch_init(struct fts_ts_info *info)
+{
+	int rc = 0;
+
+	atomic_set(&info->trusted_touch_initialized, 0);
+	init_completion(&info->trusted_touch_powerdown);
+	fts_dt_parse_trusted_touch_info(info);
+
+	/* Get clocks */
+		info->core_clk = devm_clk_get(info->client->dev.parent,
+							"m-ahb");
+		if (IS_ERR(info->core_clk)) {
+			info->core_clk = NULL;
+			dev_warn(&info->client->dev,
+				"%s: core_clk is not defined\n", __func__);
+		}
+
+		info->iface_clk = devm_clk_get(info->client->dev.parent,
+							"se-clk");
+		if (IS_ERR(info->iface_clk)) {
+			info->iface_clk = NULL;
+			dev_warn(&info->client->dev,
+				"%s: iface_clk is not defined\n", __func__);
+		}
+	if (atomic_read(&info->trusted_touch_mode) == TRUSTED_TOUCH_VM_MODE) {
+		rc = fts_vm_init(info);
+		if (rc)
+			pr_err("Failed to init VM\n");
+	}
+	atomic_set(&info->trusted_touch_initialized, 1);
+}
+
+static ssize_t fts_trusted_touch_enable_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info;
+
+	if (!client)
+		return scnprintf(buf, PAGE_SIZE, "client is null\n");
+
+	info = i2c_get_clientdata(client);
+	if (!info) {
+		logError(0, "info is null\n");
+		return scnprintf(buf, PAGE_SIZE, "info is null\n");
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d",
+			atomic_read(&info->trusted_touch_enabled));
+}
+
+static ssize_t fts_trusted_touch_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info;
+	unsigned long value;
+	int err = 0;
+
+	if (!client)
+		return -EIO;
+	info = i2c_get_clientdata(client);
+	if (!info) {
+		logError(0, "info is null\n");
+		return -EIO;
+	}
+	if (count > 2)
+		return -EINVAL;
+	err = kstrtoul(buf, 10, &value);
+	if (err != 0)
+		return err;
+
+	if (!atomic_read(&info->trusted_touch_initialized))
+		return -EIO;
+
+#ifdef CONFIG_ARCH_QTI_VM
+	err = fts_handle_trusted_touch_tvm(info, value);
+	if (err) {
+		pr_err("Failed to handle trusted touch in tvm\n");
+		return -EINVAL;
+	}
+#else
+	err = fts_handle_trusted_touch_pvm(info, value);
+	if (err) {
+		pr_err("Failed to handle trusted touch in pvm\n");
+		return -EINVAL;
+	}
+#endif
+	err = count;
+	return err;
+}
+
+#endif
+
+//struct chipInfo ftsInfo;
+
+/**
+ * #ifdef PHONE_GESTURE
+ * extern struct mutex gestureMask_mutex;
+ * #endif
+ */
+
+static char tag[8] = "[ FTS ]\0";
+
+static char fts_ts_phys[64];
+static u32 typeOfComand[CMD_STR_LEN] = {0};
+static int numberParameters;
+#ifdef USE_ONE_FILE_NODE
+static int feature_feasibility = ERROR_OP_NOT_ALLOW;
+#endif
+#ifdef PHONE_GESTURE
+static u8 mask[GESTURE_MASK_SIZE + 2];
+//extern u16 gesture_coordinates_x[GESTURE_COORDS_REPORT_MAX];
+//extern u16 gesture_coordinates_y[GESTURE_COORDS_REPORT_MAX];
+//extern int gesture_coords_reported;
+//extern struct mutex gestureMask_mutex;
+#ifdef USE_CUSTOM_GESTURES
+static int custom_gesture_res;
+#endif
+#endif
+#ifdef USE_NOISE_PARAM
+static u8 noise_params[NOISE_PARAMETERS_SIZE] = {0};
+#endif
+static void fts_interrupt_enable(struct fts_ts_info *info);
+static int fts_init_afterProbe(struct fts_ts_info *info);
+static int fts_mode_handler(struct fts_ts_info *info, int force);
+static int fts_command(struct fts_ts_info *info, unsigned char cmd);
+static int fts_chip_initialization(struct fts_ts_info *info);
+static int fts_enable_reg(struct fts_ts_info *info, bool enable);
+
+static struct drm_panel *active_panel;
+
+void touch_callback(unsigned int status)
+{
+	/* Empty */
+}
+
+unsigned int le_to_uint(const unsigned char *ptr)
+{
+	return (unsigned int) ptr[0] + (unsigned int) ptr[1] * 0x100;
+}
+
+unsigned int be_to_uint(const unsigned char *ptr)
+{
+	return (unsigned int) ptr[1] + (unsigned int) ptr[0] * 0x100;
+}
+
+void release_all_touches(struct fts_ts_info *info)
+{
+	unsigned int type = MT_TOOL_FINGER;
+	int i;
+
+	for (i = 0; i < TOUCH_ID_MAX; i++) {
+#ifdef STYLUS_MODE
+		if (test_bit(i, &info->stylus_id))
+			type = MT_TOOL_PEN;
+#endif
+		input_mt_slot(info->input_dev, i);
+		input_mt_report_slot_state(info->input_dev, type, 0);
+		input_report_abs(info->input_dev, ABS_MT_TRACKING_ID, -1);
+	}
+	input_sync(info->input_dev);
+	info->touch_id = 0;
+#ifdef STYLUS_MODE
+	info->stylus_id = 0;
+#endif
+}
+
+/************************* FW UPGGRADE *********************************/
+/* update firmware*/
+/**
+ * echo 01/00 > fwupdate     perform a fw update taking the FW to burn always
+ * from a bin file stored in /system/etc/firmware, 01= force the FW update
+ * whicheve fw_version and config_id; 00=perform a fw update only if the fw
+ * in the file has a greater fw_version or config_id
+ */
+
+/**
+ * cat fwupdate to show the result of the burning procedure
+ * (example output in the terminal = "AA00000001BB" if the switch is enabled)
+ */
+
+/**
+ * echo 01/00 > fwupdate; cat fwupdate to perform both operation stated before
+ * in just one call
+ */
+static ssize_t fts_fwupdate_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret, mode;
+	/*const struct firmware *fw = NULL;*/
+	/*char *firmware_name = "st_fts.bin";*/
+	struct Firmware fwD;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	int orig_size;
+	u8 *orig_data;
+
+	/* reading out firmware upgrade mode */
+	ret = kstrtoint(buf, 10, &mode);
+	if (ret != 0) {
+		pr_err("%s: ret = %d\n", __func__, ret);
+		return -EINVAL;
+	}
+
+	fwD.data = NULL;
+	ret = getFWdata_nocheck(PATH_FILE_FW, &orig_data, &orig_size, 0);
+	if (ret < OK) {
+		logError(1, "%s %s: impossible retrieve FW... ERROR %08X\n",
+			tag, __func__, ERROR_MEMH_READ);
+		ret = (ret | ERROR_MEMH_READ);
+		goto END;
+	}
+
+	ret = parseBinFile(orig_data, orig_size, &fwD, !mode);
+	if (ret < OK) {
+		logError(1, "%s %s: impossible parse ERROR %08X\n",
+			tag, __func__, ERROR_MEMH_READ);
+		ret = (ret | ERROR_MEMH_READ);
+		goto END;
+	}
+
+	logError(0, "%s Starting flashing procedure...\n", tag);
+	ret = flash_burn(&fwD, mode, !mode);
+
+	if (ret < OK && ret != (ERROR_FW_NO_UPDATE | ERROR_FLASH_BURN_FAILED))
+		logError(0, "%s flashProcedure: ERROR %02X\n",
+			tag, ERROR_FLASH_PROCEDURE);
+	logError(0, "%s flashing procedure Finished!\n", tag);
+
+END:
+	kfree(fwD.data);
+	info->fwupdate_stat = ret;
+
+	if (ret < OK)
+		logError(1, "%s  %s Unable to upgrade firmware! ERROR %08X\n",
+			tag, __func__, ret);
+
+	return count;
+}
+
+static ssize_t fts_fwupdate_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	//fwupdate_stat: ERROR code Returned by flashProcedure.
+	return snprintf(buf, PAGE_SIZE, "AA%08XBB\n", info->fwupdate_stat);
+}
+
+/****UTILITIES (current fw_ver/conf_id, active mode, file fw_ver/conf_id)****/
+/**
+ * cat appid show on the terminal fw_version.config_id of
+ * the FW running in the IC
+ */
+static ssize_t fts_appid_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	int error;
+
+	error = snprintf(buf, PAGE_SIZE, "%x.%x\n", ftsInfo.u16_fwVer,
+		ftsInfo.u16_cfgId);
+	return error;
+}
+
+/**
+ * cat mode_active to show the bitmask of which indicate the modes/features
+ * which are running on the IC in a specific istant oftime (example output in
+ * the terminal = "AA10000000BB" only senseOn performed)
+ */
+static ssize_t fts_mode_active_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(1, "%s Current mode active = %08X\n", tag, info->mode);
+	//return sprintf(buf, "AA%08XBB\n", info->mode);
+	return snprintf(buf, PAGE_SIZE, "AA%08XBB\n", info->mode);
+}
+
+/**
+ * cat fw_file_test show on the terminal fw_version and config_id of the FW
+ * stored in the fw file/header file
+ */
+static ssize_t fts_fw_test_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct Firmware fw;
+	int ret;
+
+	fw.data = NULL;
+	ret = readFwFile(PATH_FILE_FW, &fw, 0);
+
+	if (ret < OK)
+		logError(1, "%s Error during reading FW file! ERROR %08X\n",
+			tag, ret);
+	else {
+		logError(1, "%s fw_version = %04X, config_version = %04X, ",
+			tag, fw.fw_ver, fw.config_id);
+		logError(1, "size = %dbytes\n", fw.data_size);
+	}
+
+	kfree(fw.data);
+	return 0;
+}
+
+/**
+ * cat lockdown_info to show the lockdown info on the terminal
+ * (example output in the terminal = "AA00000000X1X2..X10BB" )
+ * where first 4 bytes correspond t
+ */
+static ssize_t fts_lockdown_info_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	u8 data[LOCKDOWN_CODE_SIZE] = {0};
+	int ret, size = 100;
+	char buff[CMD_STR_LEN] = {0};
+	char all_strbuff[100] = {0};
+
+	ret = fts_disableInterrupt();
+	if (ret < OK)
+		goto END;
+
+	ret = lockDownInfo((u8 *)data, LOCKDOWN_CODE_SIZE);
+	if (ret < OK)
+		goto END;
+
+END:
+	ret |= fts_enableInterrupt();
+
+	snprintf(buff, sizeof(buff), "%02X", 0xAA);
+	strlcat(all_strbuff, buff, size);
+	snprintf(buff, sizeof(buff), "%08X", ret);
+	strlcat(all_strbuff, buff, size);
+	if (ret >= OK) {
+		for (ret = 0; ret < LOCKDOWN_CODE_SIZE; ret++) {
+			snprintf(buff, sizeof(buff), "%02X", data[ret]);
+			strlcat(all_strbuff, buff, size);
+		}
+	} else {
+		logError(1, "%s Error while reading lockdown info = %08X\n",
+			tag, ret);
+	}
+	snprintf(buff, sizeof(buff), "%02X", 0xBB);
+	strlcat(all_strbuff, buff, size);
+	return snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+}
+
+/**
+ * cat strength_frame	to obtain strength data
+ * the string returned in the shell is made up as follow:
+ * AA = start byte
+ * X1X2X3X4 = 4 bytes in HEX format which represent an
+ * error code (00000000 no error)
+ *
+ * if error code is all 0s
+ * FF = 1 byte in HEX format number of rows
+ * SS = 1 byte in HEX format number of columns
+ * N1, ... = the decimal value of each node separated by a coma
+ *
+ * BB = end byte
+ */
+static ssize_t fts_strength_frame_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	/*unsigned int temp;*/
+	/*int res;*/
+	/*struct i2c_client *client = to_i2c_client(dev); */
+	/*struct fts_ts_info *info = i2c_get_clientdata(client); */
+
+	if (sscanf(p, "%x ", &typeOfComand[0]) != 1)
+		return -EINVAL;
+
+	logError(1, "%s %s: Type of Strength Frame selected: %d\n", tag,
+		__func__, typeOfComand[0]);
+	return count;
+}
+
+static ssize_t fts_strength_frame_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct MutualSenseFrame frame;
+	int res = ERROR_OP_NOT_ALLOW, j, size = 6*2;
+	int count = 0;
+	u16 type = 0;
+	char *all_strbuff = NULL;
+	char buff[CMD_STR_LEN] = {0};
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	frame.node_data = NULL;
+
+	res = fts_disableInterrupt();
+	if (res < OK)
+		goto END;
+
+	res = senseOn();
+#ifdef PHONE_KEY
+	res = keyOn();
+#endif
+	if (res < OK) {
+		logError(1, "%s %s: could not start scanning! ERROR %08X\n",
+			tag, __func__, res);
+		goto END;
+	}
+	msleep(WAIT_FOR_FRESH_FRAMES);
+
+	res = senseOff();
+#ifdef PHONE_KEY
+	res = keyOff();
+#endif
+	if (res < OK) {
+		logError(1, "%s %s: could not finish scanning! ERROR %08X\n",
+			tag, __func__, res);
+		goto END;
+	}
+	/* mdelay(WAIT_AFTER_SENSEOFF); */
+	msleep(WAIT_AFTER_SENSEOFF);
+	flushFIFO();
+
+	switch (typeOfComand[0]) {
+	case 1:
+		type = ADDR_NORM_TOUCH;
+		break;
+#ifdef PHONE_KEY
+	case 2:
+		type = ADDR_NORM_MS_KEY;
+		break;
+#endif
+	default:
+		logError(1, "%s %s: Strength type %d not valid! ERROR %08X\n",
+			tag, __func__, typeOfComand[0], ERROR_OP_NOT_ALLOW);
+		res = ERROR_OP_NOT_ALLOW;
+		goto END;
+	}
+
+	res = getMSFrame(type, &frame, 0);
+	if (res < OK) {
+		logError(1, "%s %s: could not get the frame! ERROR %08X\n",
+			tag, __func__, res);
+		goto END;
+	} else {
+		size += (res * 6);
+		logError(0, "%s The frame size is %d words\n", tag, res);
+		res = OK;
+		print_frame_short("MS Strength frame =",
+			array1dTo2d_short(frame.node_data,
+				frame.node_data_size,
+				frame.header.sense_node),
+			frame.header.force_node,
+			frame.header.sense_node);
+	}
+
+END:
+	flushFIFO();
+	release_all_touches(info);
+	fts_mode_handler(info, 1);
+
+	all_strbuff = kmalloc_array(size, sizeof(char), GFP_KERNEL);
+
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", res);
+		strlcat(all_strbuff, buff, size);
+
+		if (res >= OK) {
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frame.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frame.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			for (j = 0; j < frame.node_data_size; j++) {
+				snprintf(buff, sizeof(buff), "%d,",
+					frame.node_data[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			kfree(frame.node_data);
+		}
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1, "%s %s: Unable toallocate all_strbuff!ERROR %08X\n",
+			tag, ERROR_ALLOC);
+	}
+
+	fts_enableInterrupt();
+	return count;
+}
+
+/********** FEATURES *********************/
+
+/**
+ * TODO: edit this function according to the features policy to
+ * allow during the screen on/off, following is shown an example
+ * but check always with ST for more details
+ */
+int check_feature_feasibility(struct fts_ts_info *info, unsigned int feature)
+{
+	int res = OK;
+
+	/**
+	 * Example based on the status of the screen and
+	 * on the feature that is trying to enable
+	 */
+
+	/*Example based only on the feature that is going to be activated*/
+	switch (feature) {
+	case FEAT_GESTURE:
+		if (info->cover_enabled == 1) {
+			res = ERROR_OP_NOT_ALLOW;
+
+			logError(1, "%s %s:Feature not allowed when in Cover ",
+				tag, __func__);
+			logError(1, "mode %08X\n", res);
+			/**
+			 * for example here can be place a code for
+			 * disabling the cover mode when gesture is
+			 * activated
+			 */
+		}
+		break;
+
+	case FEAT_COVER:
+		if (info->gesture_enabled == 1) {
+			res = ERROR_OP_NOT_ALLOW;
+			/*logError(1,"Feature not allowed*/
+			/*when Gestures enabled!");*/
+			logError(1, "s %s: Feature not allowed when Gestures ",
+				tag, __func__);
+			logError(1, "enabled%08X\n", res);
+			/**
+			 * for example here can be place a code for
+			 * disabling the gesture mode when cover is
+			 * activated (that means that cover mode has
+			 * an higher priority on gesture mode)
+			 */
+		}
+		break;
+
+	default:
+		logError(1, "%s %s: Feature Allowed!\n", tag, __func__);
+	}
+	return res;
+}
+
+#ifdef USE_ONE_FILE_NODE
+/**
+ * echo XXXX 00/01 > feature_enable
+ * set the feature to disable/enable.
+ * XXXX = 4 bytes which identify the feature
+ *
+ * cat feature_enable
+ * set the enabled mode/features in the IC
+ * and return an error code
+ *
+ * echo XXXX 00/01 > feature_enable;
+ * cat feature_enable to perform both action stated
+ * before in just one call
+ */
+static ssize_t fts_feature_enable_store(struct device *dev,
+	struct device_attribute *attr,
+	const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res = OK;
+
+	if ((count - 8 + 1) / 3 != 1) {
+		logError(1, "%s fts_feature_enable: ", tag);
+		logError(1, "Number of parameter wrong! %d > %d\n",
+			(count - 8 + 1) / 3, 1);
+		return -EINVAL;
+	}
+
+	if (sscanf(p, "%08X ", &temp) != 1)
+		return -EINVAL;
+	p += 9;
+	res = check_feature_feasibility(info, temp);
+	if (res < OK)
+		return -EINVAL;
+
+	switch (temp) {
+#ifdef PHONE_GESTURE
+	case FEAT_GESTURE:
+		if (sscanf(p, "%02X ", &info->gesture_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s fts_feature_enable: Gesture Enabled = %d\n",
+			tag, info->gesture_enabled);
+
+		break;
+#endif
+
+#ifdef GLOVE_MODE
+	case FEAT_GLOVE:
+		if (sscanf(p, "%02X ", &info->glove_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s fts_feature_enable: Glove Enabled = %d\n",
+			tag, info->glove_enabled);
+
+		break;
+#endif
+
+#ifdef STYLUS_MODE
+	case FEAT_STYLUS:
+		if (sscanf(p, "%02X ", &info->stylus_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s fts_feature_enable: Stylus Enabled = %d\n",
+			tag, info->stylus_enabled);
+
+		break;
+#endif
+
+#ifdef COVER_MODE
+	case FEAT_COVER:
+		if (sscanf(p, "%02X ", &info->cover_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s fts_feature_enable: Cover Enabled = %d\n",
+			tag, info->cover_enabled);
+
+	break;
+#endif
+
+#ifdef CHARGER_MODE
+	case FEAT_CHARGER:
+		if (sscanf(p, "%02X ", &info->charger_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s fts_feature_enable: Charger Enabled= %d\n",
+			tag, info->charger_enabled);
+
+		break;
+#endif
+
+#ifdef VR_MODE
+	case FEAT_VR:
+		if (sscanf(p, "%02X ", &info->vr_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s fts_feature_enable: VR Enabled = %d\n",
+			tag, info->vr_enabled);
+
+		break;
+#endif
+
+#ifdef EDGE_REJ
+	case FEAT_EDGE_REJECTION:
+		if (sscanf(p, "%02X ", &info->edge_rej_enabled) != 1)
+			return -EINVAL;
+		logError(1, "%s %s: Edge Rejection Enabled= %d\n",
+			tag, __func__, info->edge_rej_enabled);
+
+		break;
+#endif
+
+#ifdef CORNER_REJ
+	case FEAT_CORNER_REJECTION:
+		if (sscanf(p, "%02X ", &info->corner_rej_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s %s: Corner Rejection Enabled= %d\n",
+			tag, __func__, info->corner_rej_enabled);
+
+		break;
+#endif
+
+#ifdef EDGE_PALM_REJ
+	case FEAT_EDGE_PALM_REJECTION:
+		if (sscanf(p, "%02X", &info->edge_palm_rej_enabled) != 1)
+			return -EINVAL;
+
+		logError(1, "%s %s:Edge Palm RejectionEnabled= %d\n",
+			tag, __func__, info->edge_palm_rej_enabled);
+
+		break;
+#endif
+	default:
+		logError(1, "%s %s: Feature %08X not valid! ERROR %08X\n",
+			tag, __func__, temp, ERROR_OP_NOT_ALLOW);
+		res = ERROR_OP_NOT_ALLOW;
+	}
+	feature_feasibility = res;
+
+	if (feature_feasibility >= OK)
+		feature_feasibility = fts_mode_handler(info, 1);
+	else {
+		logError(1, "%s %s: Call echo XXXX 00/01 > feature_enable ",
+			tag, __func__);
+		logError(1, "with a correct feature! ERROR %08X\n", res);
+	}
+	return count;
+}
+
+static ssize_t fts_feature_enable_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+
+	if (feature_feasibility < OK) {
+		logError(1,
+			"%s %s:Call before echo 00/01 > feature_enable %08X\n",
+			tag, __func__, feature_feasibility);
+	}
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", feature_feasibility);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s: Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	feature_feasibility = ERROR_OP_NOT_ALLOW;
+	return count;
+}
+
+#else
+
+#ifdef EDGE_REJ
+/**
+ * echo 01/00 > edge_rej    to enable/disable edge rejection
+ * cat edge_rej	 to show the status of the edge_rej_enabled
+ * switch (example output in the terminal = "AA00000001BB"
+ * if the switch is enabled)
+ *
+ * echo 01/00 > edge_rej; cat edge_rej	to enable/disable
+ * edge rejection and see the switch status in just one call
+ */
+static ssize_t fts_edge_rej_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s: edge_rej_enabled = %d\n",
+		tag, __func__, info->edge_rej_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->edge_rej_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else
+		logError(1,
+			"%s %s: Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+
+	return count;
+}
+
+static ssize_t fts_edge_rej_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1,
+			"%s %s:Number bytes of parameter wrong!%d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+		return -EINVAL;
+	}
+
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+
+	p += 3;
+
+	/**
+	 * this is a standard code that should be always
+	 * used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature can be enabled
+	 *
+	 * second step: call fts_mode_handler to actually enable it
+	 * NOTE: Disabling a feature is always allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_EDGE_REJECTION);
+	if (res < OK && temp != FEAT_DISABLE)
+		return -EINVAL;
+
+	info->edge_rej_enabled = temp;
+	res = fts_mode_handler(info, 1);
+	if (res < OK) {
+		logError(1,
+			"%s %s: Error during fts_mode_handler! ERROR %08X\n",
+			tag, __func__, res);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#ifdef CORNER_REJ
+/**
+ * echo 01/00 > corner_rej	to enable/disable corner rejection
+ * cat corner_rej	to show the status of the
+ * corner_rej_enabled switch (example output in the terminal
+ * = "AA00000001BB" if the switch is enabled)
+ *
+ * echo 01/00 > corner_rej; cat corner_rej	to enable/disable
+ * corner rejection and see the switch status in just one call
+ */
+static ssize_t fts_corner_rej_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s: corner_rej_enabled = %d\n",
+		tag, __func__, info->corner_rej_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->corner_rej_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1, "%s%s:Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_corner_rej_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code according
+	 * to customer needs
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1,
+			"%s %s:Number bytes of parameter wrong!%d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+		return -EINVAL;
+	}
+
+	/*sscanf(p, "%02X ", &temp);*/
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+
+	p += 3;
+
+	/**
+	 * this is a standard code that should be always
+	 * used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature
+	 * can be enabled
+	 *
+	 * second step: call fts_mode_handler to
+	 * actually enable it
+	 *
+	 * NOTE: Disabling a feature is always
+	 * allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_CORNER_REJECTION);
+	if (res >= OK || temp == FEAT_DISABLE) {
+		info->corner_rej_enabled = temp;
+		res = fts_mode_handler(info, 1);
+		if (res < OK) {
+			logError(1,
+				"%s %s: During fts_mode_handler!ERROR %08X\n",
+				tag, __func__, res);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#ifdef EDGE_PALM_REJ
+/**
+ * echo 01/00 > edge_palm_rej
+ * to enable/disable edge palm rejection
+ *
+ * cat edge_palm_rej to show the status of the
+ * edge_palm_rej_enabled switch (example output
+ * in the terminal = "AA00000001BB" if the switch is enabled)
+ *
+ * echo 01/00 > edge_palm_rej; cat edge_palm_rej
+ * to enable/disable edge palm rejection and see
+ * the switch status in just one call
+ */
+static ssize_t fts_edge_palm_rej_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s: edge_palm_rej_enabled = %d\n",
+		tag, __func__, info->edge_palm_rej_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X",
+				info->edge_palm_rej_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n",
+				all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1, "%s%s:Unable to allocate all_strbuff! %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_edge_palm_rej_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code according
+	 * to customer needs
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1,
+			"%s%s:Number bytes of parameter wrong! %d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+		return -EINVAL;
+	}
+	/*sscanf(p, "%02X ", &temp);*/
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+
+	p += 3;
+
+	/**
+	 * this is a standard code that should be
+	 * always used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature can be enabled
+	 *
+	 * second step: call fts_mode_handler to actually enable it
+	 *
+	 * NOTE: Disabling a feature is always allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_EDGE_PALM_REJECTION);
+	if (res >= OK || temp == FEAT_DISABLE) {
+		info->edge_palm_rej_enabled = temp;
+		res = fts_mode_handler(info, 1);
+		if (res < OK) {
+			logError(1, "%s%s:Error in fts_mode_handler!%08X\n",
+				tag, __func__, res);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#ifdef CHARGER_MODE
+/**
+ * echo 01/00 > charger_mode    to enable/disable charger mode
+ *
+ * cat charger_mode	 to show the status of
+ * the charger_enabled switch (example output in the terminal
+ * = "AA00000001BB" if the switch is enabled)
+ *
+ * echo 01/00 > charger_mode; cat charger_mode
+ * to enable/disable charger mode and see the
+ * switch status in just one call
+ */
+static ssize_t fts_charger_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s:charger_enabled = %d\n",
+		tag, __func__, info->charger_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->charger_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1, "%s %s:Unable to allocate all_strbuff! %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_charger_mode_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code
+	 * according to customer needs
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1, "%s %s:Size of parameter wrong! %d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+		return -EINVAL;
+	}
+	/*sscanf(p, "%02X ", &temp);*/
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+
+	p += 3;
+
+	/**
+	 * this is a standard code that should be always
+	 * used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature
+	 * can be enabled
+	 * second step: call fts_mode_handler to
+	 * actually enable it
+	 *
+	 * NOTE: Disabling a feature is always
+	 * allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_CHARGER);
+	if (res >= OK || temp == FEAT_DISABLE) {
+		info->charger_enabled = temp;
+		res = fts_mode_handler(info, 1);
+		if (res < OK) {
+			logError(1, "%s %s: Error during fts_mode_handler! ",
+				tag, __func__);
+			logError(1, "ERROR %08X\n", res);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#ifdef GLOVE_MODE
+/**
+ * echo 01/00 > glove_mode
+ * to enable/disable glove mode
+ *
+ * cat glove_mode	to show the status of
+ * the glove_enabled switch (example output in the
+ * terminal = "AA00000001BB" if the switch is enabled)
+ *
+ * echo 01/00 > glove_mode; cat glove_mode
+ * to enable/disable glove mode and see the
+ * switch status in just one call
+ */
+static ssize_t fts_glove_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s:glove_enabled = %d\n",
+		tag, __func__, info->glove_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->glove_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1, "%s %s:Unable to allocate all_strbuff! %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_glove_mode_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code
+	 * according to customer needs
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1, "%s %s:Size of parameter wrong! %d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+		return -EINVAL;
+	}
+
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+
+	p += 3;
+
+	/**
+	 * this is a standard code that should be
+	 * always used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature can be enabled
+	 *
+	 * second step: call fts_mode_handler to actually enable it
+	 *
+	 * NOTE: Disabling a feature is always allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_GLOVE);
+	if (res >= OK || temp == FEAT_DISABLE) {
+		info->glove_enabled = temp;
+		res = fts_mode_handler(info, 1);
+		if (res < OK) {
+			logError(1, "%s %s: Error during fts_mode_handler! ",
+				tag, __func__);
+			logError(1, "ERROR %08X\n", res);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#ifdef VR_MODE
+/**
+ * echo 01/00 > vr_mode   to enable/disable vr mode
+ *
+ * cat vr_mode		 to show the status of
+ * the vr_enabled switch (example output in the
+ * terminal = "AA00000001BB" if the switch is enabled)
+ *
+ * echo 01/00 > vr_mode; cat vr_mode  to enable/disable
+ * vr mode and see the switch status in just one call
+ */
+static ssize_t fts_vr_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s: vr_enabled = %d\n",
+		tag, __func__, info->vr_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->vr_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s: Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_vr_mode_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code
+	 * according to customer needs
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1,
+			"%s %s:Number bytes of parameter wrong!%d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+		return -EINVAL;
+	}
+
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+
+	p += 3;
+
+	/**
+	 * this is a standard code that should be always
+	 * used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature can be enabled
+	 * second step: call fts_mode_handler to actually enable it
+	 *
+	 * NOTE: Disabling a feature is always allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_VR);
+	if (res >= OK || temp == FEAT_DISABLE) {
+		info->vr_enabled = temp;
+		res = fts_mode_handler(info, 1);
+		if (res < OK) {
+			logError(1, "%s %s: Error in fts_mode_handler!%08X\n",
+				tag, __func__, res);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#ifdef COVER_MODE
+/**
+ * echo 01/00 > cover_mode	to enable/disable cover mode
+ * cat cover_mode  to show the status of the
+ * cover_enabled switch (example output in the
+ * terminal = "AA00000001BB" if the switch is enabled)
+ *
+ * echo 01/00 > cover_mode; cat cover_mode	to
+ * enable/disable cover mode and see the switch
+ * status in just one call
+ *
+ * NOTE: the cover can be handled also using a notifier,
+ * in this case the body of these functions
+ * should be copied in the notifier callback
+ */
+static ssize_t fts_cover_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s: cover_enabled = %d\n",
+		tag, __func__, info->cover_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->cover_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s:Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_cover_mode_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code according
+	 * to customer needs
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1,
+			"%s %s:Number bytes of parameter wrong!%d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+			return -EINVAL;
+
+	}
+
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+	p += 3;
+
+	/**
+	 * this is a standard code that should be
+	 * always used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature can be enabled
+	 * second step: call fts_mode_handler to actually enable it
+	 * NOTE: Disabling a feature is always allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_COVER);
+	if (res >= OK || temp == FEAT_DISABLE) {
+		info->cover_enabled = temp;
+		res = fts_mode_handler(info, 1);
+		if (res < OK) {
+			logError(1, "%s%s:Error in fts_mode_handler!%08X\n",
+				tag, __func__, res);
+		}
+	}
+
+
+	return count;
+}
+#endif
+
+#ifdef STYLUS_MODE
+/**
+ * echo 01/00 > stylus_mode   to enable/disable stylus mode
+ * cat stylus_mode		    to show the status of
+ * the stylus_enabled switch (example output in the
+ * terminal = "AA00000001BB" if the switch is enabled)
+ *
+ * echo 01/00 > stylus_mode; cat stylus_mode  to
+ * enable/disable stylus mode and see the
+ * switch status in just one call
+ */
+static ssize_t fts_stylus_mode_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s: stylus_enabled = %d\n",
+		tag, __func__, info->stylus_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->stylus_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s: Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_stylus_mode_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	/**
+	 * in case of a different elaboration of the input,
+	 * just modify this initial part of the code
+	 * according to customer needs
+	 */
+	if ((count + 1) / 3 != 1) {
+		logError(1, "%s %s:Size of parameter wrong! %d != %d byte\n",
+			tag, __func__, (count + 1) / 3, 1);
+		return -EINVAL;
+	}
+
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+	p += 3;
+
+	/**
+	 * this is a standard code that should be
+	 * always used when a feature is enabled!
+	 *
+	 * first step : check if the wanted feature can be enabled
+	 * second step: call fts_mode_handler to actually enable it
+	 * NOTE: Disabling a feature is always allowed by default
+	 */
+	res = check_feature_feasibility(info, FEAT_STYLUS);
+	if (res >= OK || temp == FEAT_DISABLE) {
+		info->stylus_enabled = temp;
+		res = fts_mode_handler(info, 1);
+		if (res < OK) {
+			logError(1,
+				"%s %s:Error during fts_mode_handler! %08X\n",
+				tag, __func__, res);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#endif
+
+/************** GESTURES *************/
+#ifdef PHONE_GESTURE
+#ifdef USE_GESTURE_MASK
+
+/**
+ * if this define is used, a gesture bit mask
+ * is used as method to select the gestures
+ * to enable/disable
+ */
+
+/**
+ * echo EE X1 X2 ... X8 > gesture_mask   set
+ * the gesture mask to disable/enable;
+ * EE = 00(disable) or 01(enable);
+ * X1 ... X8 = gesture mask (example 06 00 ... 00
+ * this gesture mask represent the gestures with ID = 1 and 2)
+ * can be specified from 1 to 8 bytes, if less than 8 bytes
+ * are specified the remaining bytes are kept as previous settings
+ *
+ * cat gesture_mask		    enable/disable the given mask,
+ * if one or more gestures is enabled the driver will
+ * automatically enable the gesture mode.
+ * If all the gestures are disabled the driver
+ * automatically will disable the gesture mode.
+ * At the end an error code will be printed
+ * (example output in the terminal = "AA00000000BB"
+ * if there are no errors)
+ *
+ * echo EE X1 X2 ... X8 > gesture_mask;
+ * cat gesture_mask        perform in one
+ * command both actions stated before
+ */
+static ssize_t fts_gesture_mask_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0, res, temp;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	if (mask[0] == 0) {
+		res = ERROR_OP_NOT_ALLOW;
+		logError(1, "%s %s:Call before echo enable/disable xx xx >",
+			tag, __func__);
+		logError(1, "%s %s: gesture_mask with a correct number of ",
+			tag, __func__);
+		logError(1, "parameters! ERROR %08X\n", res);
+		return -EINVAL;
+	}
+
+	if (mask[1] == FEAT_ENABLE || mask[1] == FEAT_DISABLE)
+		res = updateGestureMask(&mask[2], mask[0], mask[1]);
+	else
+		res = ERROR_OP_NOT_ALLOW;
+
+	if (res < OK) {
+		logError(1, "%s fts_gesture_mask_store: ERROR %08X\n",
+			tag, res);
+	}
+
+	res |= check_feature_feasibility(info, FEAT_GESTURE);
+	temp = isAnyGestureActive();
+	if (res >= OK || temp == FEAT_DISABLE)
+		info->gesture_enabled = temp;
+
+	logError(1, "%s fts_gesture_mask_store:Gesture Enabled = %d\n",
+		tag, info->gesture_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", res);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s:Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	mask[0] = 0;
+	return count;
+}
+
+
+static ssize_t fts_gesture_mask_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	int n;
+	unsigned int temp;
+
+	if ((count + 1) / 3 > GESTURE_MASK_SIZE + 1) {
+		logError(1, "%s %s: Number of bytes of parameter wrong! ", tag,
+			__func__);
+		logError(1, "%d > (enable/disable + %d )\n", (count + 1) / 3,
+			GESTURE_MASK_SIZE);
+		mask[0] = 0;
+		return -EINVAL;
+	}
+	mask[0] = ((count + 1) / 3) - 1;
+	for (n = 1; n <= (count + 1) / 3; n++) {
+		if (sscanf(p, "%02X ", &temp) != 1)
+			return -EINVAL;
+		p += 3;
+		mask[n] = (u8)temp;
+		logError(1, "%s mask[%d] = %02X\n", tag, n, mask[n]);
+	}
+
+	return count;
+}
+
+#else
+
+/**
+ * if this define is not used,
+ * to select the gestures to enable/disable
+ * are used the IDs of the gestures
+ *
+ * echo EE X1 X2 ... > gesture_mask    set
+ * the gesture to disable/enable; EE = 00(disable)
+ * or 01(enable); X1 ... = gesture IDs
+ * (example 01 02 05... represent the gestures with
+ * ID = 1, 2 and 5) there is no limit of the parameters
+ * that can be passed, but of course the gesture IDs
+ * should be valid (all the valid IDs are listed
+ * in ftsGesture.h)
+ *
+ * cat gesture_mask        enable/disable the
+ * given gestures, if one or more gestures is enabled
+ * the driver will automatically enable the gesture mode.
+ * If all the gestures are disabled the driver automatically
+ * will disable the gesture mode. At the end an error code
+ * will be printed (example output in the terminal =
+ * "AA00000000BB" if there are no errors)
+ *
+ * echo EE X1 X2 ... > gesture_mask; cat gesture_mask
+ * perform in one command both actions stated before
+ */
+static ssize_t fts_gesture_mask_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	logError(0, "%s %s: gesture_enabled = %d\n", tag, __func__,
+		info->gesture_enabled);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", info->gesture_enabled);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s: Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_gesture_mask_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	int n;
+	unsigned int temp;
+	int res;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	if ((count + 1) / 3 < 2 || (count + 1) / 3 > GESTURE_MASK_SIZE + 1) {
+		logError(1,
+			"%s %s:Number bytes of parameter wrong! %d %d bytes)\n",
+			tag, __func__, (count + 1) / 3, GESTURE_MASK_SIZE);
+		mask[0] = 0;
+		return -EINVAL;
+	}
+
+	memset(mask, 0, GESTURE_MASK_SIZE + 2);
+	mask[0] = ((count + 1) / 3) - 1;
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+
+	p += 3;
+	mask[1] = (u8)temp;
+	for (n = 1; n < (count + 1) / 3; n++) {
+		/*sscanf(p, "%02X ", &temp);*/
+		if (sscanf(p, "%02X ", &temp) != 1)
+			return -EINVAL;
+
+		p += 3;
+		gestureIDtoGestureMask((u8)temp, &mask[2]);
+	}
+
+	for (n = 0; n < GESTURE_MASK_SIZE + 2; n++)
+		logError(1, "%s mask[%d] = %02X\n", tag, n, mask[n]);
+
+	if (mask[0] == 0) {
+		res = ERROR_OP_NOT_ALLOW;
+		logError(1, "%s %s: Call before echo enable/disable xx xx ....",
+			tag, __func__);
+		logError(1, " > gesture_mask with parameters! ERROR %08X\n",
+			res);
+
+	} else {
+
+		if (mask[1] == FEAT_ENABLE || mask[1] == FEAT_DISABLE)
+			res = updateGestureMask(&mask[2], mask[0], mask[1]);
+		else
+			res = ERROR_OP_NOT_ALLOW;
+
+		if (res < OK)
+			logError(1, "%s %s: ERROR %08X\n", tag, __func__, res);
+
+	}
+
+	res = check_feature_feasibility(info, FEAT_GESTURE);
+	temp = isAnyGestureActive();
+	if (res >= OK || temp == FEAT_DISABLE)
+		info->gesture_enabled = temp;
+	res = fts_mode_handler(info, 0);
+
+	return count;
+}
+#endif
+
+#ifdef USE_CUSTOM_GESTURES
+/**
+ * allow to use user defined gestures
+ *
+ * echo ID X1 Y1 X2 Y2 ... X30 Y30 >
+ * add_custom_gesture     add a custom gesture;
+ * ID = 1 byte that represent the gesture ID of
+ * the custom gesture (can be chosen only between
+ * the custom IDs defined in ftsGesture.h);
+ * X1 Y1 ... = a series of 30 points (x,y) which
+ * represent the gesture template.
+ * The loaded gesture is enabled automatically
+ *
+ * cat add_custom_gesture/remove_custom_gesture
+ * Print the error code of the last operation
+ * performed with the custom gestures
+ * (example output in the terminal = "AA00000000BB"
+ * if there are no errors)
+ *
+ * echo ID X1 Y1 X2 Y2 ... X30 Y30 >
+ * add_custom_gesture; cat add_custom_gesture
+ * perform in one command both actions stated before
+ */
+static ssize_t fts_add_custom_gesture_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+
+	logError(0, "%s %s:Last Operation Result = %08X\n",
+		tag, __func__, custom_gesture_res);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", custom_gesture_res);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s:Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_add_custom_gesture_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	int n;
+	unsigned int temp;
+	u8 gestureID;
+	u8 gestMask[GESTURE_MASK_SIZE] = {0};
+	u8 template[GESTURE_CUSTOM_POINTS];
+	int res;
+	/*struct i2c_client *client = to_i2c_client(dev);*/
+	/*struct fts_ts_info *info = i2c_get_clientdata(client);*/
+
+	if ((count + 1) / 3 != GESTURE_CUSTOM_POINTS + 1) {
+		logError(1,
+			"%s %s: Number bytes of parameter wrong! %d != %d\n",
+			tag, __func__,  (count + 1) / 3,
+			GESTURE_CUSTOM_POINTS + 1);
+		res = ERROR_OP_NOT_ALLOW;
+		return -EINVAL;
+	}
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+	p += 3;
+	gestureID = (u8)temp;
+
+	for (n = 1; n < (count + 1) / 3; n++) {
+		/*sscanf(p, "%02X ", &temp);*/
+		if (sscanf(p, "%02X ", &temp) != 1)
+			return -EINVAL;
+
+		p += 3;
+		template[n-1] = (u8)temp;
+		logError(1, "%s template[%d] = %02X\n",
+			tag, n-1, template[n-1]);
+	}
+
+	res = fts_disableInterrupt();
+	if (res >= OK) {
+		logError(1, "%s %s: Adding custom gesture ID = %02X\n",
+			tag, __func__, gestureID);
+		res = addCustomGesture(template,
+			GESTURE_CUSTOM_POINTS, gestureID);
+		if (res < OK) {
+			logError(1,
+				"%s %s:error during add custom gesture ",
+				tag, __func__);
+			logError(1, "ERROR %08X\n", res);
+		} else {
+			logError(1,
+				"%s %s:Enabling in the gesture mask...\n",
+				tag, __func__);
+			gestureIDtoGestureMask(gestureID, gestMask);
+			res = enableGesture(gestMask, GESTURE_MASK_SIZE);
+			if (res < OK) {
+				logError(1, "%s %s:error during enable gesture",
+					tag, __func__);
+				logError(1, " mask: ERROR %08X\n", res);
+			} else {
+				/*if (check_feature_feasibility(info,*/
+				/*FEAT_GESTURE)==OK)*/
+				/*info->gesture_enabled =*/
+				/*isAnyGestureActive();*/
+				/*uncomment if you want to activate*/
+				/* automatically*/
+				/*the gesture mode when a custom gesture*/
+				/*is loaded*/
+				logError(1, "%s %s:Custom Gesture enabled!\n",
+					tag, __func__, res);
+			}
+		}
+	}
+	res |= fts_enableInterrupt();
+
+	custom_gesture_res = res;
+
+	return count;
+}
+
+
+/**
+ * echo ID > remove_custom_gesture
+ * remove a custom gesture;
+ * ID = 1 byte that represent the gesture ID
+ * of the custom gesture (can be chosen only
+ * between the custom IDs defined in ftsGesture.h);
+ * the same gesture is disabled automatically
+ */
+static ssize_t fts_remove_custom_gesture_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	int count = 0;
+
+	logError(0, "%s %s:Last Operation Result = %08X\n",
+		tag, __func__, custom_gesture_res);
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", custom_gesture_res);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s:Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, __func__, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_remove_custom_gesture_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	unsigned int temp;
+	int res;
+	u8 gestureID;
+	u8 gestMask[GESTURE_MASK_SIZE] = {0};
+	/*struct i2c_client *client = to_i2c_client(dev);*/
+	/*struct fts_ts_info *info = i2c_get_clientdata(client);*/
+
+	if ((count + 1) / 3 < 1) {
+		logError(1,
+			"%s %s:Number bytes of parameter wrong! %d != %d\n",
+			tag, __func__, (count + 1) / 3, 1);
+		res = ERROR_OP_NOT_ALLOW;
+		return -EINVAL;
+	}
+
+	if (sscanf(p, "%02X ", &temp) != 1)
+		return -EINVAL;
+	p += 3;
+	gestureID = (u8)temp;
+	res = fts_disableInterrupt();
+	if (res >= OK) {
+		logError(1,
+			"%s %s: Removing custom gesture ID = %02X\n",
+			tag, __func__, gestureID);
+		res = removeCustomGesture(gestureID);
+		if (res < OK) {
+			logError(1,
+				"%s %s:error in custom gesture:%08X\n",
+				tag, __func__, res);
+		} else {
+			logError(1, "%s %s: Enabling in the gesture mask...\n",
+				tag, __func__);
+			gestureIDtoGestureMask(gestureID, gestMask);
+			res = disableGesture(gestMask, GESTURE_MASK_SIZE);
+			if (res < OK) {
+				logError(1,
+				    "%s %s:error in enable gesture mask:%08X\n",
+				    tag, __func__, res);
+			} else {
+				/*if (check_feature_feasibility*/
+				/*(info,FEAT_GESTURE)==OK)*/
+				/*info->gesture_enabled = */
+				/*isAnyGestureActive();*/
+				/**
+				 * uncomment if you want to disable
+				 * automatically
+				 * the gesture mode when a custom gesture is
+				 * removed and no other gestures were enabled
+				 */
+				logError(1, "%s %s: Custom Gesture disabled!\n",
+					tag, __func__, res);
+			}
+
+		}
+	}
+
+	res |= fts_enableInterrupt();
+
+	custom_gesture_res = res;
+	return count;
+}
+#endif
+
+
+/**
+ * cat gesture_coordinates	to obtain the gesture coordinates
+ * the string returned in the shell follow this up as follow:
+ * AA = start byte
+ * X1X2X3X4 = 4 bytes in HEX format
+ * which represent an error code (00000000 no error)
+ */
+ /**** if error code is all 0s ****/
+/**
+ * CC = 1 byte in HEX format number of coords
+ * (pair of x,y) returned
+ *
+ * X1X2 Y1Y2 ... = X1X2 2 bytes in HEX format for
+ * x[i] and Y1Y2 2 bytes in HEX format for y[i] (MSB first)
+ */
+/********************************/
+/* BB = end byte*/
+static ssize_t fts_gesture_coordinates_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int size = 6 * 2;
+	//u8 coords_num;
+	u8 *all_strbuff = NULL;
+	int count = 0, res, i = 0;
+
+	logError(0, "%s %s: Getting gestures coordinates...\n", tag, __func__);
+
+	if (gesture_coords_reported < OK) {
+		logError(1, "%s %s:invalid coordinates! ERROR %08X\n",
+			tag, __func__, gesture_coords_reported);
+		res = gesture_coords_reported;
+	} else {
+		/*coords are pairs of x,y (*2) where each coord*/
+		/*is a short(2bytes=4char)(*4) + 1 byte(2char) num*/
+		/*of coords (+2)*/
+		size += gesture_coords_reported * 2 * 4 + 2;
+		/*coords_num = res;*/
+		res = OK;
+		/*set error code to OK*/
+	}
+
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (all_strbuff != NULL) {
+		memset(all_strbuff, 0, size);
+
+		snprintf(buff, sizeof(buff), "%02X", 0xAA);
+		strlcat(all_strbuff, buff, size);
+
+		snprintf(buff, sizeof(buff), "%08X", res);
+		strlcat(all_strbuff, buff, size);
+
+		if (res >= OK) {
+			snprintf(buff, sizeof(buff), "%02X",
+				gesture_coords_reported);
+			strlcat(all_strbuff, buff, size);
+
+			for (i = 0; i < gesture_coords_reported; i++) {
+				snprintf(buff, sizeof(buff), "%04X",
+					gesture_coordinates_x[i]);
+				strlcat(all_strbuff, buff, size);
+
+				snprintf(buff, sizeof(buff), "%04X",
+					gesture_coordinates_y[i]);
+				strlcat(all_strbuff, buff, size);
+			}
+		}
+
+		snprintf(buff, sizeof(buff), "%02X", 0xBB);
+		strlcat(all_strbuff, buff, size);
+
+		count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+		kfree(all_strbuff);
+	} else {
+		logError(1,
+			"%s %s:Unable to allocate all_strbuff! ERROR %08X\n",
+			tag, ERROR_ALLOC);
+	}
+
+	return count;
+}
+
+static ssize_t fts_gesture_coordinates_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	return 0;
+}
+#endif
+
+/***************** PRODUCTION TEST ****************/
+static ssize_t fts_stm_cmd_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int n;
+	char *p = (char *) buf;
+
+	memset(typeOfComand, 0, CMD_STR_LEN * sizeof(u32));
+
+	logError(1, "%s\n", tag);
+	for (n = 0; n < (count + 1) / 3; n++) {
+
+		if (sscanf(p, "%02X ", &typeOfComand[n]) != 1)
+			return -EINVAL;
+		p += 3;
+		logError(1, "%s typeOfComand[%d] = %02X\n",
+			tag, n, typeOfComand[n]);
+
+	}
+
+	numberParameters = n;
+	logError(1, "%s Number of Parameters = %d\n", tag, numberParameters);
+	return count;
+}
+
+static ssize_t fts_stm_cmd_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int res, j, doClean = 0, count;
+
+	int size = 6 * 2;
+	u8 *all_strbuff = NULL;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	struct MutualSenseData compData = {0};
+	struct SelfSenseData comData = {0};
+	struct MutualSenseFrame frameMS = {0};
+	struct SelfSenseFrame frameSS = {0};
+
+	/**
+	 * struct used for defining which test
+	 * perform during the production test
+	 */
+	struct TestToDo todoDefault;
+
+	todoDefault.MutualRaw = 1;
+	todoDefault.MutualRawGap = 1;
+	todoDefault.MutualCx1 = 0;
+	todoDefault.MutualCx2 = 1;
+	todoDefault.MutualCx2Adj = 1;
+	todoDefault.MutualCxTotal = 0;
+	todoDefault.MutualCxTotalAdj = 0;
+
+	todoDefault.MutualKeyRaw = 0;
+	todoDefault.MutualKeyCx1 = 0;
+	todoDefault.MutualKeyCx2 = 0;
+	todoDefault.MutualKeyCxTotal = 0;
+
+	todoDefault.SelfForceRaw = 1;
+	todoDefault.SelfForceRawGap = 0;
+	todoDefault.SelfForceIx1 = 0;
+	todoDefault.SelfForceIx2 = 0;
+	todoDefault.SelfForceIx2Adj = 0;
+	todoDefault.SelfForceIxTotal = 1;
+	todoDefault.SelfForceIxTotalAdj = 0;
+	todoDefault.SelfForceCx1 = 0;
+	todoDefault.SelfForceCx2 = 0;
+	todoDefault.SelfForceCx2Adj = 0;
+	todoDefault.SelfForceCxTotal = 0;
+	todoDefault.SelfForceCxTotalAdj = 0;
+
+	todoDefault.SelfSenseRaw = 1;
+	todoDefault.SelfSenseRawGap = 0;
+	todoDefault.SelfSenseIx1 = 0;
+	todoDefault.SelfSenseIx2 = 0;
+	todoDefault.SelfSenseIx2Adj = 0;
+	todoDefault.SelfSenseIxTotal = 1;
+	todoDefault.SelfSenseIxTotalAdj = 0;
+	todoDefault.SelfSenseCx1 = 0;
+	todoDefault.SelfSenseCx2 = 0;
+	todoDefault.SelfSenseCx2Adj = 0;
+	todoDefault.SelfSenseCxTotal = 0;
+	todoDefault.SelfSenseCxTotalAdj = 0;
+
+
+	if (numberParameters >= 1) {
+		res = fts_disableInterrupt();
+		if (res < 0) {
+			logError(0, "%s fts_disableInterrupt: ERROR %08X\n",
+				tag, res);
+			res = (res | ERROR_DISABLE_INTER);
+			goto END;
+		}
+
+#if defined(CONFIG_FB_MSM)
+		res = fb_unregister_client(&info->notifier);
+#else
+		if (active_panel)
+			res = drm_panel_notifier_unregister(active_panel,
+				&info->notifier);
+#endif
+		if (res < 0) {
+			logError(1, "%s ERROR: unregister notifier failed!\n",
+				tag);
+			goto END;
+		}
+
+		switch (typeOfComand[0]) {
+		/*ITO TEST*/
+		case 0x01:
+			res = production_test_ito();
+			break;
+		/*PRODUCTION TEST*/
+		case 0x00:
+			if (ftsInfo.u32_mpPassFlag != INIT_MP) {
+				logError(0, "%s MP Flag not set!\n", tag, res);
+				res = production_test_main(LIMITS_FILE, 1, 1,
+						&todoDefault, INIT_MP);
+			} else {
+				logError(0, "%s MP Flag set!\n", tag, res);
+				res = production_test_main(LIMITS_FILE, 1, 0,
+					&todoDefault, INIT_MP);
+			}
+			break;
+		/*read mutual raw*/
+		case 0x13:
+			logError(0, "%s Get 1 MS Frame\n", tag);
+			//res = getMSFrame(ADDR_RAW_TOUCH, &frame, 0);
+			res = getMSFrame2(MS_TOUCH_ACTIVE, &frameMS);
+			if (res < 0) {
+				logError(0,
+					"%s Error in taking MS frame.%02X\n",
+					tag, res);
+
+			} else {
+				logError(0, "%s The frame size is %d words\n",
+					tag, res);
+				size = (res * sizeof(short) + 8) * 2;
+				/* set res to OK because if getMSFrame is*/
+				/* successful res = number of words read*/
+				res = OK;
+			print_frame_short("MS frame =",
+					array1dTo2d_short(frameMS.node_data,
+						frameMS.node_data_size,
+						frameMS.header.sense_node),
+					frameMS.header.force_node,
+					frameMS.header.sense_node);
+			}
+			break;
+		/*read self raw*/
+		case 0x15:
+			logError(0, "%s Get 1 SS Frame\n", tag);
+			res = getSSFrame2(SS_TOUCH, &frameSS);
+
+			if (res < OK) {
+				logError(0,
+				"%s Error while taking the SS frame%02X\n",
+					tag, res);
+
+			} else {
+				logError(0, "%s The frame size is %d words\n",
+						tag, res);
+				size = (res * sizeof(short) + 8) * 2 + 1;
+				/**
+				 * set res to OK because if getMSFrame is
+				 * successful res = number of words read
+				 */
+				res = OK;
+				print_frame_short("SS force frame =",
+					array1dTo2d_short(frameSS.force_data,
+					frameSS.header.force_node, 1),
+					frameSS.header.force_node, 1);
+				print_frame_short("SS sense frame =",
+					array1dTo2d_short(frameSS.sense_data,
+						frameSS.header.sense_node,
+						frameSS.header.sense_node),
+					1,
+					frameSS.header.sense_node);
+			}
+			break;
+
+		/*read mutual comp data*/
+		case 0x14:
+			logError(0, "%s Get MS Compensation Data\n", tag);
+			res = readMutualSenseCompensationData(MS_TOUCH_ACTIVE,
+				&compData);
+
+			if (res < 0) {
+				logError(0,
+					"%s Error MS compensation data%02X\n",
+					tag, res);
+			} else {
+				logError(0,
+					"%s MS Data Reading Finished!\n",
+					tag);
+				size = ((compData.node_data_size + 9) *
+						sizeof(u8)) * 2;
+				print_frame_u8("MS Data (Cx2) =",
+					array1dTo2d_u8(compData.node_data,
+						compData.node_data_size,
+						compData.header.sense_node),
+					compData.header.force_node,
+					compData.header.sense_node);
+			}
+			break;
+
+		/*read self comp data*/
+		case 0x16:
+			logError(0, "%s Get SS Compensation Data...\n", tag);
+			res = readSelfSenseCompensationData(SS_TOUCH, &comData);
+			if (res < 0) {
+				logError(0, "%s Error reading SS data%02X\n",
+					tag, res);
+			} else {
+				logError(0, "%s SS Data Reading Finished!\n",
+					tag);
+				size = ((comData.header.force_node
+					+ comData.header.sense_node) * 2 + 12);
+				size *= sizeof(u8) * 2;
+				print_frame_u8("SS Data Ix2_fm = ",
+					array1dTo2d_u8(comData.ix2_fm,
+						comData.header.force_node, 1),
+					comData.header.force_node,
+					1);
+				print_frame_u8("SS Data Cx2_fm = ",
+					array1dTo2d_u8(comData.cx2_fm,
+						comData.header.force_node, 1),
+					comData.header.force_node,
+					1);
+				print_frame_u8("SS Data Ix2_sn = ",
+					array1dTo2d_u8(comData.ix2_sn,
+						comData.header.sense_node,
+						comData.header.sense_node),
+					1,
+					comData.header.sense_node);
+				print_frame_u8("SS Data Cx2_sn = ",
+					array1dTo2d_u8(comData.cx2_sn,
+						comData.header.sense_node,
+						comData.header.sense_node),
+					1,
+					comData.header.sense_node);
+			}
+			break;
+
+		/* MS Raw DATA TEST */
+		case 0x03:
+			res = fts_system_reset();
+			if (res >= OK)
+				res = production_test_ms_raw(LIMITS_FILE,
+					1, &todoDefault);
+			break;
+			/* MS CX DATA TEST */
+		case 0x04:
+			res = fts_system_reset();
+			if (res >= OK)
+				res = production_test_ms_cx(LIMITS_FILE,
+					1, &todoDefault);
+			break;
+		/* SS RAW DATA TEST */
+		case 0x05:
+			res = fts_system_reset();
+			if (res >= OK)
+				res = production_test_ss_raw(LIMITS_FILE,
+					1, &todoDefault);
+			break;
+		 /* SS IX CX DATA TEST */
+		case 0x06:
+			res = fts_system_reset();
+			if (res >= OK)
+				res = production_test_ss_ix_cx(LIMITS_FILE,
+					1, &todoDefault);
+			break;
+
+		case 0xF0:
+		/* TOUCH ENABLE/DISABLE */
+		case 0xF1:
+			doClean = (int) (typeOfComand[0] & 0x01);
+			res = cleanUp(doClean);
+			break;
+
+		default:
+			logError(1,
+				"%s COMMAND NOT VALID!! Insert a proper value\n",
+				tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+
+		doClean = fts_enableInterrupt();
+		if (doClean < 0) {
+			logError(0, "%s fts_enableInterrupt: ERROR %08X\n",
+				tag, (doClean|ERROR_ENABLE_INTER));
+		}
+	} else {
+		logError(1, "%s NO COMMAND SPECIFIED!!!\n", tag);
+		res = ERROR_OP_NOT_ALLOW;
+	}
+
+#if defined(CONFIG_FB_MSM)
+	if (fb_register_client(&info->notifier) < 0)
+		logError(1, "%s ERROR: register notifier failed!\n", tag);
+#else
+	if (active_panel &&
+		drm_panel_notifier_register(active_panel, &info->notifier) < 0)
+		logError(1, "%s ERROR: register notifier failed!\n", tag);
+#endif
+
+END:
+	/*here start the reporting phase,*/
+	/* assembling the data to send in the file node */
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	if (!all_strbuff)
+		return 0;
+	memset(all_strbuff, 0, size);
+
+	snprintf(buff, sizeof(buff), "%02X", 0xAA);
+	strlcat(all_strbuff, buff, size);
+
+	snprintf(buff, sizeof(buff), "%08X", res);
+	strlcat(all_strbuff, buff, size);
+
+	if (res >= OK) {
+		/*all the other cases are already fine printing only the res.*/
+		switch (typeOfComand[0]) {
+		case 0x13:
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frameMS.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frameMS.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			for (j = 0; j < frameMS.node_data_size; j++) {
+				snprintf(buff, sizeof(buff), "%04X",
+					frameMS.node_data[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			kfree(frameMS.node_data);
+			break;
+
+		case 0x15:
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frameSS.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frameSS.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			/* Copying self raw data Force */
+			for (j = 0; j < frameSS.header.force_node; j++) {
+				snprintf(buff, sizeof(buff), "%04X",
+					frameSS.force_data[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			/* Copying self raw data Sense */
+			for (j = 0; j < frameSS.header.sense_node; j++) {
+				snprintf(buff, sizeof(buff), "%04X",
+					frameSS.sense_data[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			kfree(frameSS.force_data);
+			kfree(frameSS.sense_data);
+			break;
+
+		case 0x14:
+			snprintf(buff, sizeof(buff), "%02X",
+					(u8) compData.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+					(u8) compData.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			/* Cpying CX1 value */
+			snprintf(buff, sizeof(buff), "%02X", compData.cx1);
+			strlcat(all_strbuff, buff, size);
+
+			/* Copying CX2 values */
+			for (j = 0; j < compData.node_data_size; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+						*(compData.node_data + j));
+				strlcat(all_strbuff, buff, size);
+			}
+
+			kfree(compData.node_data);
+			break;
+
+		case 0x16:
+			snprintf(buff, sizeof(buff), "%02X",
+				comData.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				comData.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.f_ix1);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.s_ix1);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.f_cx1);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.s_cx1);
+			strlcat(all_strbuff, buff, size);
+
+			/* Copying IX2 Force */
+			for (j = 0; j < comData.header.force_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.ix2_fm[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			/* Copying IX2 Sense */
+			for (j = 0; j < comData.header.sense_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.ix2_sn[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			/* Copying CX2 Force */
+			for (j = 0; j < comData.header.force_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.cx2_fm[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			/* Copying CX2 Sense */
+			for (j = 0; j < comData.header.sense_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.cx2_sn[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			kfree(comData.ix2_fm);
+			kfree(comData.ix2_sn);
+			kfree(comData.cx2_fm);
+			kfree(comData.cx2_sn);
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	snprintf(buff, sizeof(buff), "%02X", 0xBB);
+	strlcat(all_strbuff, buff, size);
+
+	count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+	/**
+	 * need to reset the number of parameters
+	 * in order to wait the next command,
+	 * comment if you want to repeat
+	 * the last command sent just doing a cat
+	 */
+	numberParameters = 0;
+	/* logError(0,"%s numberParameters = %d\n",tag, numberParameters);*/
+	kfree(all_strbuff);
+
+	return count;
+}
+
+static DEVICE_ATTR_RW(fts_fwupdate);
+static DEVICE_ATTR_RO(fts_appid);
+static DEVICE_ATTR_RO(fts_mode_active);
+static DEVICE_ATTR_RO(fts_lockdown_info);
+static DEVICE_ATTR_RW(fts_strength_frame);
+static DEVICE_ATTR_RO(fts_fw_test);
+static DEVICE_ATTR_RW(fts_stm_cmd);
+#ifdef USE_ONE_FILE_NODE
+static DEVICE_ATTR_RW(fts_feature_enable);
+#else
+
+#ifdef EDGE_REJ
+static DEVICE_ATTR_RW(fts_edge_rej);
+#endif
+
+#ifdef CORNER_REJ
+static DEVICE_ATTR_RW(fts_corner_rej);
+#endif
+
+#ifdef EDGE_PALM_REJ
+static DEVICE_ATTR_RW(fts_edge_palm_rej);
+#endif
+
+#ifdef CHARGER_MODE
+static DEVICE_ATTR_RW(fts_charger_mode);
+#endif
+
+#ifdef GLOVE_MODE
+static DEVICE_ATTR_RW(fts_glove_mode);
+#endif
+
+#ifdef VR_MODE
+static DEVICE_ATTR_RW(fts_vr_mode);
+#endif
+
+#ifdef COVER_MODE
+static DEVICE_ATTR_RW(fts_cover_mode);
+#endif
+
+#ifdef STYLUS_MODE
+static DEVICE_ATTR_RW(fts_stylus_mode);
+#endif
+
+#endif
+
+#ifdef PHONE_GESTURE
+static DEVICE_ATTR_RW(fts_gesture_mask);
+static DEVICE_ATTR_RW(fts_gesture_coordinates);
+#ifdef USE_CUSTOM_GESTURES
+static DEVICE_ATTR_RW(fts_add_custom_gesture);
+static DEVICE_ATTR_RW(fts_remove_custom_gesture);
+#endif
+#endif
+
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+static DEVICE_ATTR(trusted_touch_enable,
+	0664,
+	fts_trusted_touch_enable_show,
+	fts_trusted_touch_enable_store);
+#endif
+
+/*  /sys/devices/soc.0/f9928000.i2c/i2c-6/6-0049  */
+static struct attribute *fts_attr_group[] = {
+	&dev_attr_fts_fwupdate.attr,
+	&dev_attr_fts_appid.attr,
+	&dev_attr_fts_mode_active.attr,
+	&dev_attr_fts_lockdown_info.attr,
+	&dev_attr_fts_strength_frame.attr,
+	&dev_attr_fts_fw_test.attr,
+	&dev_attr_fts_stm_cmd.attr,
+#ifdef USE_ONE_FILE_NODE
+	&dev_attr_fts_feature_enable.attr,
+#else
+
+#ifdef EDGE_REJ
+	&dev_attr_fts_edge_rej.attr,
+#endif
+#ifdef CORNER_REJ
+	&dev_attr_fts_corner_rej.attr,
+#endif
+#ifdef EDGE_PALM_REJ
+	&dev_attr_fts_edge_palm_rej.attr,
+#endif
+#ifdef CHARGER_MODE
+	&dev_attr_fts_charger_mode.attr,
+#endif
+#ifdef GLOVE_MODE
+	&dev_attr_fts_glove_mode.attr,
+#endif
+#ifdef VR_MODE
+	&dev_attr_fts_vr_mode.attr,
+#endif
+#ifdef COVER_MODE
+	&dev_attr_fts_cover_mode.attr,
+#endif
+#ifdef STYLUS_MODE
+	&dev_attr_fts_stylus_mode.attr,
+#endif
+
+#endif
+
+#ifdef PHONE_GESTURE
+	&dev_attr_fts_gesture_mask.attr,
+	&dev_attr_fts_gesture_coordinates.attr,
+#ifdef USE_CUSTOM_GESTURES
+	&dev_attr_fts_add_custom_gesture.attr,
+	&dev_attr_fts_remove_custom_gesture.attr,
+#endif
+
+#endif
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+	&dev_attr_trusted_touch_enable.attr,
+#endif
+	NULL,
+};
+
+static int fts_command(struct fts_ts_info *info, unsigned char cmd)
+{
+	unsigned char regAdd;
+	int ret;
+
+	regAdd = cmd;
+
+	ret = fts_writeCmd(&regAdd, sizeof(regAdd)); /* 0 = ok */
+
+	logError(0, "%s Issued command 0x%02x, return value %08X\n", cmd, ret);
+
+	return ret;
+}
+
+void fts_input_report_key(struct fts_ts_info *info, int key_code)
+{
+	mutex_lock(&info->input_report_mutex);
+	input_report_key(info->input_dev, key_code, 1);
+	input_sync(info->input_dev);
+	input_report_key(info->input_dev, key_code, 0);
+	input_sync(info->input_dev);
+	mutex_unlock(&info->input_report_mutex);
+}
+
+/*
+ * New Interrupt handle implementation
+ */
+static inline unsigned char *fts_next_event(unsigned char *evt)
+{
+	/* Nothing to do with this event, moving to the next one */
+	evt += FIFO_EVENT_SIZE;
+
+	/* the previous one was the last event ?  */
+	return (evt[-1] & 0x1F) ? evt : NULL;
+}
+
+/* EventId : 0x00 */
+static void fts_nop_event_handler(struct fts_ts_info *info,
+	unsigned char *event)
+{
+	/**
+	 * logError(1,
+	 * "%s %s Doing nothing for event =
+	 * %02X %02X %02X %02X %02X %02X %02X %02X\n",
+	 * tag, __func__, event[0], event[1], event[2],
+	 * event[3], event[4], event[5], event[6], event[7]);
+	 */
+	/* return fts_next_event(event); */
+}
+
+/* EventId : 0x03 */
+static void fts_enter_pointer_event_handler(struct fts_ts_info *info,
+			unsigned char *event)
+{
+	unsigned char touchId, touchcount;
+	int x, y;
+	int minor;
+	int major, distance = 0;
+	u8 touchsize;
+
+	if (!info->resume_bit && !info->aoi_notify_enabled)
+		return;
+
+	touchId = event[1] & 0x0F;
+	touchcount = (event[1] & 0xF0) >> 4;
+	touchsize = (event[5] & 0xC0) >> 6;
+	major = (event[5] & 0x1F); // bit0-bit4: major
+	minor = event[6]; // event6:minor
+
+	__set_bit(touchId, &info->touch_id);
+
+	x = (event[2] << 4) | (event[4] & 0xF0) >> 4;
+	y = (event[3] << 4) | (event[4] & 0x0F);
+
+	if (info->bdata->x_flip)
+		x = X_AXIS_MAX - x;
+	if (info->bdata->y_flip)
+		y = Y_AXIS_MAX - y;
+
+	if (x == X_AXIS_MAX)
+		x--;
+
+	if (y == Y_AXIS_MAX)
+		y--;
+
+	if (info->sensor_sleep && info->aoi_notify_enabled)
+		if ((x < info->aoi_left || x > info->aoi_right)
+			|| (y < info->aoi_top || y > info->aoi_bottom)) {
+			x = -x;
+			y = -y;
+		}
+
+	input_mt_slot(info->input_dev, touchId);
+	input_mt_report_slot_state(info->input_dev, MT_TOOL_FINGER, 1);
+
+	if (touchcount == 1) {
+		input_report_key(info->input_dev, BTN_TOUCH, 1);
+		input_report_key(info->input_dev, BTN_TOOL_FINGER, 1);
+	}
+	input_report_abs(info->input_dev, ABS_MT_POSITION_X, x);
+	input_report_abs(info->input_dev, ABS_MT_POSITION_Y, y);
+	input_report_abs(info->input_dev, ABS_MT_TOUCH_MAJOR, major);
+	input_report_abs(info->input_dev, ABS_MT_TOUCH_MINOR, minor);
+	input_report_abs(info->input_dev, ABS_MT_DISTANCE, distance);
+
+	return;
+}
+
+/* EventId : 0x04 */
+static void fts_leave_pointer_event_handler(struct fts_ts_info *info,
+			unsigned char *event)
+{
+	unsigned char touchId, touchcount;
+	u8 touchsize;
+
+	touchId = event[1] & 0x0F;
+	touchcount = (event[1] & 0xF0) >> 4;
+	touchsize = (event[5] & 0xC0) >> 6;
+
+	input_mt_slot(info->input_dev, touchId);
+
+	__clear_bit(touchId, &info->touch_id);
+	input_mt_report_slot_state(info->input_dev, MT_TOOL_FINGER, 0);
+
+	if (touchcount == 0) {
+		input_report_key(info->input_dev, BTN_TOUCH, 0);
+		input_report_key(info->input_dev, BTN_TOOL_FINGER, 0);
+	}
+
+	input_report_abs(info->input_dev, ABS_MT_TRACKING_ID, -1);
+
+}
+
+/* EventId : 0x05 */
+#define fts_motion_pointer_event_handler fts_enter_pointer_event_handler
+
+#ifdef PHONE_KEY
+/* EventId : 0x0E */
+static void fts_key_status_event_handler(struct fts_ts_info *info,
+			unsigned char *event)
+{
+	int value;
+
+	logError(0,
+		"%s %sReceived event %02X %02X %02X %02X %02X %02X %02X %02X\n",
+		tag, __func__, event[0], event[1], event[2], event[3],
+		event[4], event[5], event[6], event[7]);
+	/*
+	 * TODO: the customer should handle the events coming
+	 * from the keys according his needs (this is an example
+	 * that report only the single pressure of one key at time)
+	 */
+	/* event[2] contain the bitmask of the keys that are actually pressed */
+	if (event[2] != 0) {
+		switch (event[2]) {
+		case KEY1:
+			value = KEY_HOMEPAGE;
+			logError(0, "%s %s: Button HOME!\n", tag, __func__);
+		break;
+
+		case KEY2:
+			value = KEY_BACK;
+			logError(0, "%s %s: Button Back !\n", tag, __func__);
+			break;
+
+		case KEY3:
+			value = KEY_MENU;
+			logError(0, "%s %s: Button Menu !\n", tag, __func__);
+			break;
+
+		default:
+			logError(0,
+				"%s %s:No valid Button ID or more than one key pressed!\n",
+				tag, __func__);
+			return;
+		}
+
+		fts_input_report_key(info, value);
+	} else {
+		logError(0, "%s %s: All buttons released!\n", tag, __func__);
+	}
+}
+#endif
+
+/* EventId : 0x0F */
+static void fts_error_event_handler(struct fts_ts_info *info,
+		unsigned char *event)
+{
+	int error = 0;
+
+	logError(0,
+		"%s %sReceived event:%02X %02X %02X %02X %02X %02X %02X %02X\n",
+		tag, __func__, event[0], event[1], event[2], event[3],
+		event[4], event[5], event[6], event[7]);
+
+	switch (event[1]) {
+	case EVENT_TYPE_ESD_ERROR: /* esd */
+		/* before reset clear all slot */
+		release_all_touches(info);
+
+		fts_chip_powercycle(info);
+
+		error = fts_system_reset();
+		error |= fts_mode_handler(info, 0);
+		error |= fts_enableInterrupt();
+		if (error < OK) {
+			logError(1,
+				"%s %s Cannot restore the device ERROR %08X\n",
+				tag, __func__, error);
+		}
+		break;
+	case EVENT_TYPE_WATCHDOG_ERROR: /* watch dog timer */
+		/* if (event[2] == 0) { */
+		dumpErrorInfo();
+		/* before reset clear all slot */
+		release_all_touches(info);
+		error = fts_system_reset();
+		error |= fts_mode_handler(info, 0);
+		error |= fts_enableInterrupt();
+		if (error < OK) {
+			logError(1,
+				"%s %s Cannot reset the device ERROR %08X\n",
+				tag, __func__, error);
+		}
+		/* } */
+		break;
+}
+    /* return fts_next_event(event); */
+}
+
+/* EventId : 0x10 */
+static void fts_controller_ready_event_handler(struct fts_ts_info *info,
+	unsigned char *event)
+{
+	int error;
+
+	logError(0, "%s %s Received event 0x%02x\n", tag, __func__, event[0]);
+	release_all_touches(info);
+	setSystemResettedUp(1);
+	setSystemResettedDown(1);
+	error = fts_mode_handler(info, 0);
+	if (error < OK) {
+		logError(1,
+			"%s %s Cannot restore the device status ERROR %08X\n",
+			tag, __func__, error);
+	}
+	/* return fts_next_event(event); */
+}
+
+/* EventId : 0x16 */
+static void fts_status_event_handler(struct fts_ts_info *info,
+		unsigned char *event)
+{
+	/* logError(1, "%s Received event 0x%02x\n", tag, event[0]); */
+
+	switch (event[1]) {
+	case EVENT_TYPE_MS_TUNING_CMPL:
+	case EVENT_TYPE_SS_TUNING_CMPL:
+	case FTS_FORCE_CAL_SELF_MUTUAL:
+	case FTS_FLASH_WRITE_CONFIG:
+	case FTS_FLASH_WRITE_COMP_MEMORY:
+	case FTS_FORCE_CAL_SELF:
+	case FTS_WATER_MODE_ON:
+	case FTS_WATER_MODE_OFF:
+	default:
+		logError(0, "%s %s Received unhandled status event = ",
+			tag, __func__);
+		logError(0, "%02X %02X %02X %02X %02X %02X %02X %02X\n",
+			event[0], event[1], event[2], event[3], event[4],
+			event[5], event[6], event[7]);
+		break;
+	}
+
+	/* return fts_next_event(event); */
+}
+
+#ifdef PHONE_GESTURE
+/**
+ * TODO: Customer should implement their own action
+ * in respons of a gesture event.
+ * This is an example that simply print the gesture received
+ */
+static void fts_gesture_event_handler(struct fts_ts_info *info,
+			unsigned char *event)
+{
+	unsigned char touchId;
+	int value;
+	int needCoords = 0;
+
+	logError(0,
+		"%s gesture event: %02X %02X %02X %02X %02X %02X %02X %02X\n",
+		tag, event[0], event[1], event[2], event[3],
+		event[4], event[5], event[6], event[7]);
+
+	if (event[1] == 0x03) {
+		logError(1, "%s %s: Gesture ID %02X enable_status = %02X\n",
+			tag, __func__, event[2], event[3]);
+	}
+
+	if (event[1] == EVENT_TYPE_ENB && event[2] == 0x00) {
+		switch (event[3]) {
+		case GESTURE_ENABLE:
+			logError(1, "%s %s: Gesture Enabled! res = %02X\n",
+				tag, __func__, event[4]);
+			break;
+
+		case GESTURE_DISABLE:
+			logError(1, "%s %s: Gesture Disabled! res = %02X\n",
+				tag, __func__, event[4]);
+			break;
+
+		default:
+			logError(1, "%s %s: Event not Valid!\n", tag, __func__);
+		}
+	}
+
+	if (event[0] == EVENTID_GESTURE && (event[1] == EVENT_TYPE_GESTURE_DTC1
+			|| event[1] == EVENT_TYPE_GESTURE_DTC2)) {
+		/* always use touchId zero */
+		touchId = 0;
+		__set_bit(touchId, &info->touch_id);
+
+		/* by default read the coordinates*/
+		/* for all gestures excluding double tap */
+		needCoords = 1;
+
+		switch (event[2]) {
+		case GES_ID_DBLTAP:
+			value = KEY_WAKEUP;
+			logError(0, "%s %s: double tap!\n", tag, __func__);
+			needCoords = 0;
+		break;
+
+		case GES_ID_AT:
+			value = KEY_WWW;
+			logError(0, "%s %s: @!\n", tag, __func__);
+			break;
+
+		case GES_ID_C:
+			value = KEY_C;
+			logError(0, "%s %s: C !\n", tag, __func__);
+			break;
+
+		case GES_ID_E:
+			value = KEY_E;
+			logError(0, "%s %s: e !\n", tag, __func__);
+		break;
+
+		case GES_ID_F:
+			value = KEY_F;
+			logError(0, "%s %s: F !\n", tag, __func__);
+			break;
+
+		case GES_ID_L:
+			value = KEY_L;
+			logError(0, "%s %s: L !\n", tag, __func__);
+			break;
+
+		case GES_ID_M:
+			value = KEY_M;
+			logError(0, "%s %s: M !\n", tag, __func__);
+			break;
+
+		case GES_ID_O:
+			value = KEY_O;
+			logError(0, "%s %s: O !\n", tag, __func__);
+			break;
+
+		case GES_ID_S:
+			value = KEY_S;
+			logError(0, "%s %s: S !\n", tag, __func__);
+			break;
+
+		case GES_ID_V:
+			value = KEY_V;
+			logError(0, "%s %s:  V !\n", tag, __func__);
+			break;
+
+		case GES_ID_W:
+			value = KEY_W;
+			logError(0, "%s %s:  W !\n", tag, __func__);
+			break;
+
+		case GES_ID_Z:
+			value = KEY_Z;
+			logError(0, "%s %s:  Z !\n", tag, __func__);
+			break;
+
+		case GES_ID_HFLIP_L2R:
+			value = KEY_RIGHT;
+			logError(0, "%s %s:  -> !\n", tag, __func__);
+			break;
+
+		case GES_ID_HFLIP_R2L:
+			value = KEY_LEFT;
+			logError(0, "%s %s:  <- !\n", tag, __func__);
+			break;
+
+		case GES_ID_VFLIP_D2T:
+			value = KEY_UP;
+			logError(0, "%s %s:  UP !\n", tag, __func__);
+			break;
+
+		case GES_ID_VFLIP_T2D:
+			value = KEY_DOWN;
+			logError(0, "%s %s:  DOWN !\n", tag, __func__);
+			break;
+
+		case GES_ID_CUST1:
+			value = KEY_F1;
+			logError(0, "%s %s:  F1 !\n", tag, __func__);
+			break;
+
+		case GES_ID_CUST2:
+			value = KEY_F1;
+			logError(0, "%s %s:  F2 !\n", tag, __func__);
+			break;
+
+		case GES_ID_CUST3:
+			value = KEY_F3;
+			logError(0, "%s %s:  F3 !\n", tag, __func__);
+			break;
+
+		case GES_ID_CUST4:
+			value = KEY_F1;
+			logError(0, "%s %s:  F4 !\n", tag, __func__);
+			break;
+
+		case GES_ID_CUST5:
+			value = KEY_F1;
+			logError(0, "%s %s:  F5 !\n", tag, __func__);
+			break;
+
+		case GES_ID_LEFTBRACE:
+			value = KEY_LEFTBRACE;
+			logError(0, "%s %s:  < !\n", tag, __func__);
+			break;
+
+		case GES_ID_RIGHTBRACE:
+			value = KEY_RIGHTBRACE;
+			logError(0, "%s %s:  > !\n", tag, __func__);
+			break;
+		default:
+			logError(0, "%s %s:  No valid GestureID!\n",
+				tag, __func__);
+			goto gesture_done;
+		}
+
+		/* no coordinates for gestures reported by FW */
+		if (event[1] == EVENT_TYPE_GESTURE_DTC1)
+			needCoords = 0;
+
+		if (needCoords == 1)
+			readGestureCoords(event);
+
+		fts_input_report_key(info, value);
+
+gesture_done:
+		/* Done with gesture event, clear bit. */
+		__clear_bit(touchId, &info->touch_id);
+	}
+	/* return fts_next_event(event); */
+}
+#endif
+
+/* EventId : 0x05 */
+#define fts_motion_pointer_event_handler fts_enter_pointer_event_handler
+
+/*
+ * This handler is called each time there is at least
+ * one new event in the FIFO
+ */
+static void fts_event_handler(struct work_struct *work)
+{
+	struct fts_ts_info *info;
+	int error = 0, count = 0;
+	unsigned char regAdd;
+	unsigned char data[FIFO_EVENT_SIZE] = {0};
+	unsigned char eventId;
+
+	struct event_dispatch_handler_t event_handler;
+
+	info = container_of(work, struct fts_ts_info, work);
+	/*
+	 * read all the FIFO and parsing events
+	 */
+
+	__pm_wakeup_event(info->wakeup_source, HZ);
+	regAdd = FIFO_CMD_READONE;
+
+	for (count = 0; count < FIFO_DEPTH; count++) {
+		error = fts_readCmd(&regAdd, sizeof(regAdd), data,
+				FIFO_EVENT_SIZE);
+		if (error == OK && data[0] != EVENTID_NO_EVENT)
+			eventId = data[0];
+		else
+			break;
+
+		if (eventId < EVENTID_LAST) {
+			event_handler = info->event_dispatch_table[eventId];
+			event_handler.handler(info, (data));
+		}
+	}
+	input_sync(info->input_dev);
+
+	fts_interrupt_enable(info);
+}
+
+static void fts_fw_update_auto(struct work_struct *work)
+{
+	u8 cmd[4] = { FTS_CMD_HW_REG_W, 0x00, 0x00, SYSTEM_RESET_VALUE };
+	int event_to_search[2] = {(int)EVENTID_ERROR_EVENT,
+					(int)EVENT_TYPE_CHECKSUM_ERROR};
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	int flag_init = 0;
+	int retval = 0;
+	int retval1 = 0;
+	int ret;
+	struct fts_ts_info *info;
+	struct delayed_work *fwu_work = container_of(work,
+	struct delayed_work, work);
+	int crc_status = 0;
+	int error = 0;
+	struct Firmware fwD;
+	int orig_size;
+	u8 *orig_data;
+
+	info = container_of(fwu_work, struct fts_ts_info, fwu_work);
+	logError(0, "%s Fw Auto Update is starting...\n", tag);
+
+	ret = getFWdata(PATH_FILE_FW, &orig_data, &orig_size, 0);
+	if (ret < OK) {
+		logError(0, "%s %s: impossible retrieve FW... ERROR %08X\n",
+			tag, __func__, ERROR_MEMH_READ);
+		ret = (ret | ERROR_MEMH_READ);
+		goto NO_FIRMWARE_UPDATE;
+	}
+
+	ret = parseBinFile(orig_data, orig_size, &fwD, 1);
+	if (ret < OK) {
+		logError(1, "%s %s: impossible parse ERROR %08X\n",
+			tag, __func__, ERROR_MEMH_READ);
+		ret = (ret | ERROR_MEMH_READ);
+		kfree(fwD.data);
+		goto NO_FIRMWARE_UPDATE;
+	}
+
+	fts_chip_powercycle(info);
+	retval = flash_burn(&fwD, crc_status, 1);
+
+	if ((retval & 0xFF000000) == ERROR_FLASH_PROCEDURE) {
+		logError(1, "%s %s:firmware update retry! ERROR %08X\n",
+			tag, __func__, retval);
+		fts_chip_powercycle(info);
+
+		retval1 = flash_burn(&fwD, crc_status, 1);
+
+		if ((retval1 & 0xFF000000) == ERROR_FLASH_PROCEDURE) {
+			logError(1, "%s %s: update failed again! ERROR %08X\n",
+				tag, __func__, retval1);
+			logError(1, "%s Fw Auto Update Failed!\n", tag);
+		}
+	}
+
+	kfree(fwD.data);
+	u16ToU8_be(SYSTEM_RESET_ADDRESS, &cmd[1]);
+	ret = fts_writeCmd(cmd, 4);
+	if (ret < OK) {
+		logError(1, "%s %s Can't send reset command! ERROR %08X\n",
+			tag, __func__, ret);
+	} else {
+		setSystemResettedDown(1);
+		setSystemResettedUp(1);
+		ret = pollForEvent(event_to_search, 2, readData,
+				GENERAL_TIMEOUT);
+		if (ret < OK) {
+			logError(0, "%s %s: No CX CRC Found!\n", tag, __func__);
+		} else {
+			if (readData[2] == CRC_CX_MEMORY) {
+				logError(1, "%s %s: CRC Error! ERROR:%02X\n\n",
+					tag, __func__, readData[2]);
+
+				flag_init = 1;
+			}
+		}
+	}
+
+	if (ftsInfo.u8_msScrConfigTuneVer != ftsInfo.u8_msScrCxmemTuneVer ||
+		ftsInfo.u8_ssTchConfigTuneVer != ftsInfo.u8_ssTchCxmemTuneVer)
+		ret = ERROR_GET_INIT_STATUS;
+	else if (((ftsInfo.u32_mpPassFlag != INIT_MP)
+		&& (ftsInfo.u32_mpPassFlag != INIT_FIELD)) || flag_init == 1)
+		ret = ERROR_GET_INIT_STATUS;
+	else
+		ret = OK;
+
+	if (ret == ERROR_GET_INIT_STATUS) {
+		error = fts_chip_initialization(info);
+		if (error < OK)
+			logError(1, "%s %s Can't initialize chip! ERROR %08X",
+				tag, __func__, error);
+	}
+
+NO_FIRMWARE_UPDATE:
+	error = fts_init_afterProbe(info);
+	if (error < OK)
+		logError(1, "%s Can't initialize hardware device ERROR %08X\n",
+			tag, error);
+
+	logError(0, "%s Fw Auto Update Finished!\n", tag);
+}
+
+static int fts_chip_initialization(struct fts_ts_info *info)
+{
+	int ret2 = 0;
+	int retry;
+	int initretrycnt = 0;
+	struct TestToDo todoDefault;
+
+	todoDefault.MutualRaw = 1;
+	todoDefault.MutualRawGap = 1;
+	todoDefault.MutualCx1 = 0;
+	todoDefault.MutualCx2 = 0;
+	todoDefault.MutualCx2Adj = 0;
+	todoDefault.MutualCxTotal = 0;
+	todoDefault.MutualCxTotalAdj = 0;
+
+	todoDefault.MutualKeyRaw = 0;
+	todoDefault.MutualKeyCx1 = 0;
+	todoDefault.MutualKeyCx2 = 0;
+	todoDefault.MutualKeyCxTotal = 0;
+
+	todoDefault.SelfForceRaw = 0;
+	todoDefault.SelfForceRawGap = 0;
+	todoDefault.SelfForceIx1 = 0;
+	todoDefault.SelfForceIx2 = 0;
+	todoDefault.SelfForceIx2Adj = 0;
+	todoDefault.SelfForceIxTotal = 0;
+	todoDefault.SelfForceIxTotalAdj = 0;
+	todoDefault.SelfForceCx1 = 0;
+	todoDefault.SelfForceCx2 = 0;
+	todoDefault.SelfForceCx2Adj = 0;
+	todoDefault.SelfForceCxTotal = 0;
+	todoDefault.SelfForceCxTotalAdj = 0;
+
+	todoDefault.SelfSenseRaw = 1;
+	todoDefault.SelfSenseRawGap = 0;
+	todoDefault.SelfSenseIx1 = 0;
+	todoDefault.SelfSenseIx2 = 0;
+	todoDefault.SelfSenseIx2Adj = 0;
+	todoDefault.SelfSenseIxTotal = 0;
+	todoDefault.SelfSenseIxTotalAdj = 0;
+	todoDefault.SelfSenseCx1 = 0;
+	todoDefault.SelfSenseCx2 = 0;
+	todoDefault.SelfSenseCx2Adj = 0;
+	todoDefault.SelfSenseCxTotal = 0;
+	todoDefault.SelfSenseCxTotalAdj = 0;
+
+	for (retry = 0; retry <= INIT_FLAG_CNT; retry++) {
+		ret2 = production_test_main(LIMITS_FILE, 1, 1, &todoDefault,
+					INIT_FIELD);
+		if (ret2 == OK)
+			break;
+		initretrycnt++;
+		logError(1, "%s %s: cycle count = %04d - ERROR %08X\n",
+			tag, __func__, initretrycnt, ret2);
+		fts_chip_powercycle(info);
+	}
+
+	if (ret2 < OK)
+		logError(1, "%s failed to initializate 3 times\n", tag);
+
+	return ret2;
+}
+
+#ifdef FTS_USE_POLLING_MODE
+
+static enum hrtimer_restart fts_timer_func(struct hrtimer *timer)
+{
+	struct fts_ts_info *info =
+		container_of(timer, struct fts_ts_info, timer);
+
+	queue_work(info->event_wq, &info->work);
+	return HRTIMER_NORESTART;
+}
+#else
+
+static irqreturn_t fts_interrupt_handler(int irq, void *handle)
+{
+	struct fts_ts_info *info = handle;
+
+	if (!info) {
+		pr_err("%s: Invalid info\n", __func__);
+		return IRQ_HANDLED;
+	}
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+#ifndef CONFIG_ARCH_QTI_VM
+	if (atomic_read(&info->vm_info->pvm_owns_iomem) &&
+			atomic_read(&info->vm_info->pvm_owns_irq) &&
+			atomic_read(&info->trusted_touch_enabled)) {
+		pr_err("%s: Cannot service interrupts in PVM while trusted touch is enabled\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+#endif
+#endif
+	disable_irq_nosync(info->client->irq);
+
+	queue_work(info->event_wq, &info->work);
+
+	return IRQ_HANDLED;
+}
+#endif
+
+static int fts_interrupt_install(struct fts_ts_info *info)
+{
+	int i, error = 0;
+	size_t len;
+
+	len = sizeof(struct event_dispatch_handler_t) * EVENTID_LAST;
+	info->event_dispatch_table = kzalloc(len, GFP_KERNEL);
+
+	if (!info->event_dispatch_table) {
+		logError(1, "%s OOM allocating event dispatch table\n", tag);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < EVENTID_LAST; i++)
+		info->event_dispatch_table[i].handler = fts_nop_event_handler;
+	install_handler(info, ENTER_POINTER, enter_pointer);
+	install_handler(info, LEAVE_POINTER, leave_pointer);
+	install_handler(info, MOTION_POINTER, motion_pointer);
+	install_handler(info, ERROR_EVENT, error);
+	install_handler(info, CONTROL_READY, controller_ready);
+	install_handler(info, STATUS_UPDATE, status);
+#ifdef PHONE_GESTURE
+	install_handler(info, GESTURE, gesture);
+#endif
+#ifdef PHONE_KEY
+	install_handler(info, KEY_STATUS, key_status);
+#endif
+	/* disable interrupts in any case */
+	error = fts_disableInterrupt();
+
+#ifdef FTS_USE_POLLING_MODE
+	logError(0, "%s Polling Mode\n");
+	hrtimer_init(&info->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	info->timer.function = fts_timer_func;
+	hrtimer_start(&info->timer, ktime_set(1, 0), HRTIMER_MODE_REL);
+#else
+#ifdef CONFIG_ARCH_QTI_VM
+	logError(0, "%s Interrupt Mode\n", tag);
+	if (request_threaded_irq(info->client->irq, NULL, fts_interrupt_handler,
+		IRQF_TRIGGER_HIGH | IRQF_ONESHOT, info->client->name, info)) {
+		logError(1, "%s Request irq failed\n", tag);
+		kfree(info->event_dispatch_table);
+		error = -EBUSY;
+	}
+#else
+	logError(0, "%s Interrupt Mode\n", tag);
+	if (request_threaded_irq(info->client->irq, NULL, fts_interrupt_handler,
+		IRQF_TRIGGER_LOW | IRQF_ONESHOT, info->client->name, info)) {
+		logError(1, "%s Request irq failed\n", tag);
+		kfree(info->event_dispatch_table);
+		error = -EBUSY;
+	}
+#endif
+#endif
+	return error;
+}
+
+static void fts_interrupt_uninstall(struct fts_ts_info *info)
+{
+	fts_disableInterrupt();
+
+	kfree(info->event_dispatch_table);
+#ifdef FTS_USE_POLLING_MODE
+	hrtimer_cancel(&info->timer);
+#else
+	free_irq(info->client->irq, info);
+#endif
+}
+
+static void fts_interrupt_enable(struct fts_ts_info *info)
+{
+#ifdef FTS_USE_POLLING_MODE
+	hrtimer_start(&info->timer, ktime_set(0, 10000000), HRTIMER_MODE_REL);
+#else
+	enable_irq(info->client->irq);
+#endif
+	/* enable the touch IC irq */
+	fts_enableInterrupt();
+}
+
+static void fts_interrupt_disable(struct fts_ts_info *info)
+{
+	/* disable the touch IC irq */
+	fts_disableInterrupt();
+
+#ifdef FTS_USE_POLLING_MODE
+	hrtimer_cancel(&info->timer);
+#else
+	disable_irq(info->client->irq);
+#endif
+
+}
+
+static int fts_init(struct fts_ts_info *info)
+{
+	int error;
+
+	error = fts_system_reset();
+	if (error < OK && error != (ERROR_TIMEOUT | ERROR_SYSTEM_RESET_FAIL)) {
+		logError(1, "%s Cannot reset the device! ERROR %08X\n",
+			tag, error);
+		return error;
+	}
+	if (error == (ERROR_TIMEOUT | ERROR_SYSTEM_RESET_FAIL)) {
+		logError(1, "%s Setting default Chip INFO!\n", tag);
+		defaultChipInfo(0);
+	} else {
+		error = readChipInfo(0);
+		if (error < OK) {
+			logError(1, "%s Cannot read Chip Info!ERROR:%08X\n",
+				tag, error);
+		}
+	}
+
+	error = fts_interrupt_install(info);
+
+	if (error != OK)
+		logError(1, "%s Init (1) error (ERROR  = %08X)\n", tag, error);
+
+	return error;
+}
+
+int fts_chip_powercycle(struct fts_ts_info *info)
+{
+	int error = 0;
+
+	logError(0, "%s %s: Power Cycle Starting...\n", tag, __func__);
+
+	/*
+	 * if IRQ pin is short with DVDD a call to
+	 * the ISR will triggered when the regulator is turned off
+	 */
+
+	logError(0, "%s %s: Disabling IRQ...\n", tag, __func__);
+	disable_irq_nosync(info->client->irq);
+	if (info->pwr_reg) {
+		error = regulator_disable(info->pwr_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to disable DVDD regulator\n",
+				tag, __func__);
+		}
+	}
+
+	if (info->bus_reg) {
+		error = regulator_disable(info->bus_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to disable AVDD regulator\n",
+				tag, __func__);
+		}
+	}
+
+	if (info->bdata->reset_gpio != GPIO_NOT_DEFINED)
+		gpio_set_value(info->bdata->reset_gpio, 0);
+	else
+		msleep(300);
+
+	if (info->pwr_reg) {
+		error = regulator_enable(info->bus_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to enable AVDD regulator\n",
+				tag, __func__);
+		}
+	}
+
+	if (info->bus_reg) {
+		error = regulator_enable(info->pwr_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to enable DVDD regulator\n",
+				tag, __func__);
+		}
+	}
+	/* time needed by the regulators for reaching the regime values */
+	msleep(20);
+
+	if (info->bdata->reset_gpio != GPIO_NOT_DEFINED) {
+		/* time to wait before bring up the reset */
+		/* gpio after the power up of the regulators */
+		msleep(20);
+		gpio_set_value(info->bdata->reset_gpio, 1);
+		/* mdelay(300); */
+	}
+
+	release_all_touches(info);
+
+	logError(0, "%s %s: Enabling IRQ...\n", tag, __func__);
+	enable_irq(info->client->irq);
+	logError(0, "%s %s: Power Cycle Finished! ERROR CODE = %08x\n",
+		tag, __func__, error);
+	setSystemResettedUp(1);
+	setSystemResettedDown(1);
+	return error;
+}
+
+int fts_chip_powercycle2(struct fts_ts_info *info, unsigned long sleep)
+{
+	int error = 0;
+
+	logError(0, "%s %s: Power Cycle Starting...\n", tag, __func__);
+
+	if (info->pwr_reg) {
+		error = regulator_disable(info->pwr_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to disable DVDD regulator\n",
+				tag, __func__);
+		}
+	}
+
+	if (info->bus_reg) {
+		error = regulator_disable(info->bus_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to disable AVDD regulator\n",
+				tag, __func__);
+		}
+	}
+
+	if (info->bdata->reset_gpio != GPIO_NOT_DEFINED)
+		gpio_set_value(info->bdata->reset_gpio, 0);
+
+	msleep(sleep);
+	if (info->pwr_reg) {
+		error = regulator_enable(info->bus_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to enable AVDD regulator\n",
+				tag, __func__);
+		}
+	}
+
+	if (info->bus_reg) {
+		error = regulator_enable(info->pwr_reg);
+		if (error < 0) {
+			logError(1, "%s %s: Failed to enable DVDD regulator\n",
+				tag, __func__);
+		}
+	}
+	/* time needed by the regulators for reaching the regime values */
+	msleep(500);
+
+	if (info->bdata->reset_gpio != GPIO_NOT_DEFINED) {
+		/*
+		 * time to wait before bring up the reset
+		 * gpio after the power up of the regulators
+		 */
+		msleep(20);
+		gpio_set_value(info->bdata->reset_gpio, 1);
+		/* msleep(300); */
+	}
+
+	/* before reset clear all slot */
+	release_all_touches(info);
+
+	logError(0, "%s %s: Power Cycle Finished! ERROR CODE = %08x\n",
+		tag, __func__, error);
+	setSystemResettedUp(1);
+	setSystemResettedDown(1);
+	return error;
+}
+
+static int fts_init_afterProbe(struct fts_ts_info *info)
+{
+	int error = 0;
+
+	/* system reset */
+	error = cleanUp(0);
+
+	/* enable the features and the sensing */
+	error |= fts_mode_handler(info, 0);
+
+	/* enable the interrupt */
+	error |= fts_enableInterrupt();
+
+#if defined(CONFIG_FB_MSM)
+	error |= fb_register_client(&info->notifier);
+#else
+	if (active_panel)
+		error |= drm_panel_notifier_register(active_panel,
+			&info->notifier);
+#endif
+
+	if (error < OK)
+		logError(1, "%s %s Init after Probe error (ERROR = %08X)\n",
+			tag, __func__, error);
+
+	return error;
+}
+
+/*
+ * TODO: change this function according with the needs
+ * of customer in terms of feature to enable/disable
+ */
+static int fts_mode_handler(struct fts_ts_info *info, int force)
+{
+	int res = OK;
+	int ret = OK;
+
+	/* initialize the mode to Nothing in order */
+	/* to be updated depending on the features enabled */
+	info->mode = MODE_NOTHING;
+
+	logError(0, "%s %s: Mode Handler starting...\n", tag, __func__);
+	switch (info->resume_bit) {
+	case 0:
+		/* screen down */
+		logError(0, "%s %s: Screen OFF...\n", tag, __func__);
+		/*
+		 * do sense off in order to avoid the flooding
+		 * of the fifo with touch events if someone is
+		 * touching the panel during suspend
+		 */
+		logError(0, "%s %s: Sense OFF!\n", tag, __func__);
+		/*
+		 *we need to use fts_command for speed reason
+		 * (no need to check echo in this case and interrupt
+		 * can be enabled)
+		 */
+		res |= fts_command(info, FTS_CMD_MS_MT_SENSE_OFF);
+#ifdef PHONE_KEY
+		logError(0, "%s %s: Key OFF!\n", tag, __func__);
+		res |= fts_command(info, FTS_CMD_MS_KEY_OFF);
+#endif
+
+#ifdef PHONE_GESTURE
+		if (info->gesture_enabled == 1) {
+			logError(0, "%s %s: enter in gesture mode!\n",
+				tag, __func__);
+			ret = enterGestureMode(isSystemResettedDown());
+			if (ret >= OK) {
+				info->mode |= FEAT_GESTURE;
+			} else {
+				logError(1,
+					"%s %s:enterGestureMode failed!%08X recovery in senseOff\n",
+					tag, __func__, ret);
+			}
+			res |= ret;
+		}
+#endif
+		if (info->mode != (FEAT_GESTURE|MODE_NOTHING)
+			|| info->gesture_enabled == 0)
+			info->mode |= MODE_SENSEOFF;
+		setSystemResettedDown(0);
+		break;
+
+	case 1:
+		/* screen up */
+		logError(0, "%s %s: Screen ON...\n", tag, __func__);
+
+#ifdef FEAT_GLOVE
+		if ((info->glove_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s: Glove Mode setting...\n",
+				tag, __func__);
+			ret = featureEnableDisable(info->glove_enabled,
+					FEAT_GLOVE);
+			if (ret < OK) {
+				logError(1,
+				     "%s %s:error in setting GLOVE_MODE!%08X\n",
+				     tag, __func__, ret);
+			}
+			res |= ret;
+
+			if (ret >= OK && info->glove_enabled == FEAT_ENABLE) {
+				info->mode |= FEAT_GLOVE;
+				logError(1, "%s %s: GLOVE_MODE Enabled!\n",
+					tag, __func__);
+			} else {
+				logError(1, "%s %s: GLOVE_MODE Disabled!\n",
+					tag, __func__);
+			}
+		}
+#endif
+#ifdef FEAT_STYLUS
+		if ((info->stylus_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s: Stylus Mode setting...\n",
+						tag, __func__);
+			ret = featureEnableDisable(info->stylus_enabled,
+					FEAT_STYLUS);
+			if (ret < OK) {
+				logError(1,
+					"%s %s:error in set STYLUS_MODE!%08X\n",
+					tag, __func__, ret);
+			}
+			res |= ret;
+
+			if (ret >= OK && info->stylus_enabled == FEAT_ENABLE) {
+				info->mode |= FEAT_STYLUS;
+				logError(1, "%s %s: STYLUS_MODE Enabled!\n",
+					tag, __func__);
+			} else {
+				logError(1, "%s %s: STYLUS_MODE Disabled!\n",
+					tag, __func__);
+			}
+		}
+#endif
+#ifdef FEAT_COVER
+		if ((info->cover_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s: Cover Mode setting...\n",
+				tag, __func__);
+			ret = featureEnableDisable(info->cover_enabled,
+					FEAT_COVER);
+			if (ret < OK) {
+				logError(1,
+					"%s %s:error setting COVER_MODE!%08X\n",
+					tag, __func__, ret);
+			}
+			res |= ret;
+
+			if (ret >= OK && info->cover_enabled == FEAT_ENABLE) {
+				info->mode |= FEAT_COVER;
+				logError(1, "%s %s: COVER_MODE Enabled!\n",
+					tag, __func__);
+			} else {
+				logError(1, "%s %s: COVER_MODE Disabled!\n",
+					tag, __func__);
+			}
+		}
+#endif
+#ifdef FEAT_CHARGER
+		if ((info->charger_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s: Charger Mode setting...\n",
+					tag, __func__);
+			ret = featureEnableDisable(info->charger_enabled,
+				FEAT_CHARGER);
+			if (ret < OK) {
+				logError(1,
+					"%s %s:error set CHARGER_MODE!%08X\n",
+					tag, __func__, ret);
+			}
+			res |= ret;
+
+			if (ret >= OK && info->charger_enabled == FEAT_ENABLE) {
+				info->mode |= FEAT_CHARGER;
+				logError(1, "%s %s: CHARGER_MODE Enabled!\n",
+					tag, __func__);
+			} else {
+				logError(1, "%s %s: CHARGER_MODE Disabled!\n",
+					tag, __func__);
+			}
+		}
+#endif
+#ifdef FEAT_VR
+		if ((info->vr_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s: Vr Mode setting\n", tag, __func__);
+			ret = featureEnableDisable(info->vr_enabled, FEAT_VR);
+			if (ret < OK) {
+				logError(1,
+					"%s %s:error setting VR_MODE!:%08X\n",
+					tag, __func__, ret);
+				}
+			res |= ret;
+
+			if (ret >= OK && info->vr_enabled == FEAT_ENABLE) {
+				info->mode |= FEAT_VR;
+				logError(1, "%s %s: VR_MODE Enabled!\n",
+					tag, __func__);
+			} else {
+				logError(1, "%s %s: VR_MODE Disabled!\n",
+					tag, __func__);
+			}
+		}
+#endif
+#ifdef FEAT_EDGE_REJECTION
+		if ((info->edge_rej_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s: Edge Rejection Mode setting\n",
+				tag, __func__);
+			ret = featureEnableDisable(info->edge_rej_enabled,
+				FEAT_EDGE_REJECTION);
+			if (ret < OK) {
+				logError(1,
+				"%s %s:err set EDGE_REJECTION_MODE!%08X\n",
+					tag, __func__, ret);
+			}
+			res |= ret;
+
+			if (ret >= OK && info->edge_rej_enabled ==
+					FEAT_ENABLE) {
+				info->mode |= FEAT_EDGE_REJECTION;
+				logError(1,
+					"%s %s:EDGE_REJECTION_MODE Enabled!\n",
+					tag, __func__);
+			} else {
+				logError(1,
+					"%s %s:EDGE_REJECTION_MODE Disabled!\n",
+					tag, __func__);
+			}
+		}
+#endif
+#ifdef FEAT_CORNER_REJECTION
+		if ((info->corner_rej_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s: Corner rejection Mode setting\n",
+				tag, __func__);
+			ret = featureEnableDisable(info->corner_rej_enabled,
+				FEAT_CORNER_REJECTION);
+			if (ret < OK) {
+				logError(1,
+					"%s%s:err CORNER_REJECTION_MODE!%08X\n",
+					tag, __func__, ret);
+			}
+			res |= ret;
+
+			if (ret >= OK && info->corner_rej_enabled ==
+				FEAT_ENABLE) {
+				info->mode |= FEAT_CORNER_REJECTION;
+				logError(1,
+					"%s%s:CORNER_REJECTION_MODE Enabled!\n",
+					tag, __func__);
+			} else {
+				logError(1,
+					"%s%s:CORNER_REJECTION_MODE Disabled\n",
+					tag, __func__);
+			}
+		}
+#endif
+#ifdef FEAT_EDGE_PALM_REJECTION
+		if ((info->edge_palm_rej_enabled == FEAT_ENABLE &&
+			isSystemResettedUp()) || force == 1) {
+			logError(0, "%s %s:Edge Palm rejection Mode setting\n",
+				tag, __func__);
+			ret = featureEnableDisable(info->edge_palm_rej_enabled,
+				FEAT_EDGE_PALM_REJECTION);
+			if (ret < OK) {
+				logError(1,
+				    "%s %s:err EDGE_PALM_REJECTION_MODE!%08X\n",
+				    tag, __func__, ret);
+			}
+			res |= ret;
+
+			if (ret >= OK && info->edge_palm_rej_enabled ==
+				FEAT_ENABLE) {
+				info->mode |= FEAT_EDGE_PALM_REJECTION;
+				logError(1,
+				    "%s %s:EDGE_PALM_REJECTION_MODE Enabled!\n",
+				    tag, __func__);
+			} else {
+				logError(1,
+				   "%s %s:EDGE_PALM_REJECTION_MODE Disabled!\n",
+				    tag, __func__);
+			}
+		}
+#endif
+		logError(0, "%s %s: Sense ON!\n", tag, __func__);
+		res |= fts_command(info, FTS_CMD_MS_MT_SENSE_ON);
+		info->mode |= MODE_SENSEON;
+#ifdef PHONE_KEY
+		logError(0, "%s %s: Key ON!\n", tag, __func__);
+		res |= fts_command(info, FTS_CMD_MS_KEY_ON);
+#endif
+		setSystemResettedUp(0);
+		break;
+
+	default:
+		logError(1,
+			"%s %s: invalid resume_bit value = %d! ERROR %08X\n",
+			tag, __func__, info->resume_bit, ERROR_OP_NOT_ALLOW);
+		res = ERROR_OP_NOT_ALLOW;
+	}
+	logError(0, "%s %s: Mode Handler finished! res = %08X\n", tag, __func__,
+		res);
+	return res;
+}
+
+static int fts_chip_power_switch(struct fts_ts_info *info, bool on)
+{
+	int error = -1;
+
+	if (info->bdata->pwr_on_suspend) {
+		if (!info->ts_pinctrl)
+			return 0;
+
+		if (on) {
+			error = pinctrl_select_state(info->ts_pinctrl,
+				info->pinctrl_state_active);
+			if (error < 0)
+				logError(1, "%s: Failed to select %s\n",
+					__func__, PINCTRL_STATE_ACTIVE);
+		} else {
+			error = pinctrl_select_state(info->ts_pinctrl,
+				info->pinctrl_state_suspend);
+			if (error < 0)
+				logError(1, "%s: Failed to select %s\n",
+					__func__, PINCTRL_STATE_SUSPEND);
+		}
+
+		return 0;
+	}
+
+	if (on) {
+		if (info->bus_reg) {
+			error = regulator_enable(info->bus_reg);
+			if (error < 0)
+				logError(1, "%s %s: Failed to enable AVDD\n",
+					tag, __func__);
+		}
+
+		if (info->pwr_reg) {
+			error = regulator_enable(info->pwr_reg);
+			if (error < 0)
+				logError(1, "%s %s: Failed to enable DVDD\n",
+					tag, __func__);
+		}
+
+		if (info->ts_pinctrl) {
+			if (pinctrl_select_state(info->ts_pinctrl,
+				info->pinctrl_state_active) < 0) {
+				logError(1, "%s: Failed to select %s\n",
+					__func__, PINCTRL_STATE_ACTIVE);
+			}
+		}
+	} else {
+		if (info->bdata->reset_gpio != GPIO_NOT_DEFINED)
+			gpio_set_value(info->bdata->reset_gpio, 0);
+		else
+			msleep(300);
+
+		if (info->ts_pinctrl) {
+			if (pinctrl_select_state(info->ts_pinctrl,
+				info->pinctrl_state_suspend) < 0) {
+				logError(1, "%s: Failed to select %s\n",
+					__func__, PINCTRL_STATE_SUSPEND);
+			}
+		}
+
+		if (info->pwr_reg) {
+			error = regulator_disable(info->pwr_reg);
+			if (error < 0)
+				logError(1, "%s %s: Failed to disable DVDD\n",
+					tag, __func__);
+		}
+
+		if (info->bus_reg) {
+			error = regulator_disable(info->bus_reg);
+			if (error < 0)
+				logError(1, "%s %s: Failed to disable AVDD\n",
+					tag, __func__);
+		}
+	}
+	return error;
+}
+
+
+static void fts_resume_work(struct work_struct *work)
+{
+	struct fts_ts_info *info;
+
+	info = container_of(work, struct fts_ts_info, resume_work);
+
+	__pm_wakeup_event(info->wakeup_source, HZ);
+
+	fts_chip_power_switch(info, true);
+
+	info->resume_bit = 1;
+
+	fts_system_reset();
+#ifdef USE_NOISE_PARAM
+	readNoiseParameters(noise_params);
+#endif
+
+#ifdef USE_NOISE_PARAM
+	writeNoiseParameters(noise_params);
+#endif
+
+	release_all_touches(info);
+
+	fts_mode_handler(info, 0);
+
+	info->sensor_sleep = false;
+
+	fts_interrupt_enable(info);
+}
+
+static void fts_suspend_work(struct work_struct *work)
+{
+	struct fts_ts_info *info;
+
+	info = container_of(work, struct fts_ts_info, suspend_work);
+
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+	if (atomic_read(&info->trusted_touch_enabled))
+		wait_for_completion_interruptible(
+				&info->trusted_touch_powerdown);
+#endif
+
+	__pm_wakeup_event(info->wakeup_source, HZ);
+
+	info->resume_bit = 0;
+
+	fts_mode_handler(info, 0);
+
+	fts_interrupt_disable(info);
+	release_all_touches(info);
+	info->sensor_sleep = true;
+
+	fts_chip_power_switch(info, false);
+}
+
+#if defined(CONFIG_FB_MSM)
+static int fts_fb_state_chg_callback(struct notifier_block *nb,
+			unsigned long val, void *data)
+{
+	struct fts_ts_info *info = container_of(nb,
+			struct fts_ts_info, notifier);
+	struct fb_event *evdata = data;
+	unsigned int blank;
+
+	if (!evdata || (evdata->id != 0))
+		return 0;
+
+	if (val != FB_EVENT_BLANK)
+		return 0;
+
+	logError(0, "%s %s: fts notifier begin!\n", tag, __func__);
+
+	if (evdata->data && val == FB_EVENT_BLANK && info) {
+
+		blank = *(int *) (evdata->data);
+
+		switch (blank) {
+		case FB_BLANK_POWERDOWN:
+			if (info->sensor_sleep)
+				break;
+
+			logError(0, "%s %s: FB_BLANK_POWERDOWN\n",
+				tag, __func__);
+
+			queue_work(info->event_wq, &info->suspend_work);
+
+			break;
+
+		case FB_BLANK_UNBLANK:
+			if (!info->sensor_sleep)
+				break;
+
+			logError(0, "%s %s: FB_BLANK_UNBLANK\n",
+				tag, __func__);
+
+			queue_work(info->event_wq, &info->resume_work);
+			break;
+		default:
+			break;
+		}
+	}
+	return NOTIFY_OK;
+}
+
+#else
+static int fts_fb_state_chg_callback(struct notifier_block *nb,
+		unsigned long val, void *data)
+{
+	struct fts_ts_info *info = container_of(nb, struct fts_ts_info,
+				notifier);
+	struct drm_panel_notifier *evdata = data;
+	unsigned int blank;
+
+	if (!evdata)
+		return 0;
+
+	if (val != DRM_PANEL_EVENT_BLANK)
+		return 0;
+
+	logError(0, "%s %s: fts notifier begin!\n", tag, __func__);
+	if (evdata->data && val == DRM_PANEL_EVENT_BLANK && info) {
+		blank = *(int *) (evdata->data);
+
+		switch (blank) {
+		case DRM_PANEL_BLANK_POWERDOWN:
+			if (info->sensor_sleep && info->aoi_notify_enabled)
+				break;
+
+			if (info->aoi_notify_enabled)
+				info->aoi_wake_on_suspend = true;
+			else
+				info->aoi_wake_on_suspend = false;
+
+			if (info->aoi_wake_on_suspend) {
+				info->sensor_sleep = true;
+				__pm_stay_awake(info->wakeup_source);
+			} else {
+				queue_work(info->event_wq, &info->suspend_work);
+			}
+			break;
+
+		case DRM_PANEL_BLANK_UNBLANK:
+			if (info->aoi_wake_on_suspend)
+				__pm_relax(info->wakeup_source);
+
+			if (!info->sensor_sleep)
+				break;
+
+			if (!info->resume_bit)
+				queue_work(info->event_wq, &info->resume_work);
+
+			if (info->aoi_wake_on_suspend)
+				info->sensor_sleep = false;
+
+			break;
+		default:
+			break;
+		}
+	}
+	return NOTIFY_OK;
+}
+#endif
+
+static struct notifier_block fts_noti_block = {
+	.notifier_call = fts_fb_state_chg_callback,
+};
+
+static int fts_pinctrl_init(struct fts_ts_info *info)
+{
+	int retval;
+
+	/* Get pinctrl if target uses pinctrl */
+	info->ts_pinctrl = devm_pinctrl_get(info->dev);
+	if (IS_ERR_OR_NULL(info->ts_pinctrl)) {
+		retval = PTR_ERR(info->ts_pinctrl);
+		logError(1, "Target does not use pinctrl %d\n", retval);
+		goto err_pinctrl_get;
+	}
+
+	info->pinctrl_state_active
+		= pinctrl_lookup_state(info->ts_pinctrl, PINCTRL_STATE_ACTIVE);
+	if (IS_ERR_OR_NULL(info->pinctrl_state_active)) {
+		retval = PTR_ERR(info->pinctrl_state_active);
+		logError(1, "Can not lookup %s pinstate %d\n",
+					PINCTRL_STATE_ACTIVE, retval);
+		goto err_pinctrl_lookup;
+	}
+
+	info->pinctrl_state_suspend
+		= pinctrl_lookup_state(info->ts_pinctrl, PINCTRL_STATE_SUSPEND);
+	if (IS_ERR_OR_NULL(info->pinctrl_state_suspend)) {
+		retval = PTR_ERR(info->pinctrl_state_suspend);
+		logError(1, "Can not lookup %s pinstate %d\n",
+					PINCTRL_STATE_SUSPEND, retval);
+		goto err_pinctrl_lookup;
+	}
+
+	info->pinctrl_state_release
+		= pinctrl_lookup_state(info->ts_pinctrl, PINCTRL_STATE_RELEASE);
+	if (IS_ERR_OR_NULL(info->pinctrl_state_release)) {
+		retval = PTR_ERR(info->pinctrl_state_release);
+		logError(1, "Can not lookup %s pinstate %d\n",
+					PINCTRL_STATE_RELEASE, retval);
+	}
+
+	return 0;
+
+err_pinctrl_lookup:
+	devm_pinctrl_put(info->ts_pinctrl);
+err_pinctrl_get:
+	info->ts_pinctrl = NULL;
+	return retval;
+}
+
+static int fts_get_reg(struct fts_ts_info *info, bool get)
+{
+	int retval;
+	const struct fts_i2c_platform_data *bdata = info->bdata;
+
+	if (!get) {
+		retval = 0;
+		goto regulator_put;
+	}
+
+	if ((bdata->pwr_reg_name != NULL) && (*bdata->pwr_reg_name != 0)) {
+		info->pwr_reg = regulator_get(info->dev,
+					bdata->pwr_reg_name);
+		if (IS_ERR(info->pwr_reg)) {
+			logError(1, "%s %s: Failed to get power regulator\n",
+				tag, __func__);
+			retval = PTR_ERR(info->pwr_reg);
+			goto regulator_put;
+		}
+
+		retval = regulator_set_load(info->pwr_reg, FTS_DVDD_LOAD);
+		if (retval < 0) {
+			logError(1, "%s %s: Failed to set power load\n",
+				tag, __func__);
+			goto regulator_put;
+		}
+
+		retval = regulator_set_voltage(info->pwr_reg,
+				FTS_DVDD_VOL_MIN, FTS_DVDD_VOL_MAX);
+		if (retval < 0) {
+			logError(1, "%s %s: Failed to set power voltage\n",
+				tag, __func__);
+			goto regulator_put;
+		}
+	}
+
+	if ((bdata->bus_reg_name != NULL) && (*bdata->bus_reg_name != 0)) {
+		info->bus_reg = regulator_get(info->dev,
+					bdata->bus_reg_name);
+		if (IS_ERR(info->bus_reg)) {
+			logError(1,
+				"%s %s:Failed to get bus pullup regulator\n",
+				tag, __func__);
+			retval = PTR_ERR(info->bus_reg);
+			goto regulator_put;
+		}
+
+		retval = regulator_set_load(info->bus_reg, FTS_AVDD_LOAD);
+		if (retval < 0) {
+			logError(1, "%s %s: Failed to set power load\n",
+				tag, __func__);
+			goto regulator_put;
+		}
+
+		retval = regulator_set_voltage(info->bus_reg,
+				FTS_AVDD_VOL_MIN, FTS_AVDD_VOL_MAX);
+
+		if (retval < 0) {
+			logError(1, "%s %s: Failed to set power voltage\n",
+				tag, __func__);
+			goto regulator_put;
+		}
+	}
+
+	return 0;
+
+regulator_put:
+	if (info->pwr_reg) {
+		regulator_put(info->pwr_reg);
+		info->pwr_reg = NULL;
+	}
+
+	if (info->bus_reg) {
+		regulator_put(info->bus_reg);
+		info->bus_reg = NULL;
+	}
+
+	return retval;
+}
+
+static int fts_enable_reg(struct fts_ts_info *info,
+		bool enable)
+{
+	int retval;
+
+	if (!enable) {
+		retval = 0;
+		goto disable_pwr_reg;
+	}
+
+	if (info->bus_reg) {
+		retval = regulator_enable(info->bus_reg);
+		if (retval < 0) {
+			logError(1, "%s %s: Failed to enable bus regulator\n",
+				tag, __func__);
+			goto exit;
+		}
+	}
+
+	if (info->pwr_reg) {
+		retval = regulator_enable(info->pwr_reg);
+		if (retval < 0) {
+			logError(1, "%s %s: Failed to enable power regulator\n",
+				tag, __func__);
+			goto disable_bus_reg;
+		}
+	}
+
+	return OK;
+
+disable_pwr_reg:
+	if (info->pwr_reg)
+		regulator_disable(info->pwr_reg);
+
+disable_bus_reg:
+	if (info->bus_reg)
+		regulator_disable(info->bus_reg);
+
+exit:
+	return retval;
+}
+
+static int fts_gpio_setup(int gpio, bool config, int dir, int state)
+{
+	int retval = 0;
+	unsigned char buf[16];
+
+	if (config) {
+		snprintf(buf, 16, "fts_gpio_%u\n", gpio);
+
+		retval = gpio_request(gpio, buf);
+		if (retval) {
+			logError(1, "%s %s: Failed to get gpio %d (code: %d)",
+				tag, __func__, gpio, retval);
+			return retval;
+		}
+
+		if (dir == 0)
+			retval = gpio_direction_input(gpio);
+		else
+			retval = gpio_direction_output(gpio, state);
+		if (retval) {
+			logError(1, "%s %s: Failed to set gpio %d direction",
+				tag, __func__, gpio);
+			return retval;
+		}
+	} else {
+		gpio_free(gpio);
+	}
+
+	return retval;
+}
+
+static int fts_set_gpio(struct fts_ts_info *info)
+{
+	int retval;
+	const struct fts_i2c_platform_data *bdata =
+			info->bdata;
+
+	retval = fts_gpio_setup(bdata->irq_gpio, true, 0, 0);
+	if (retval < 0) {
+		logError(1, "%s %s: Failed to configure irq GPIO\n",
+			tag, __func__);
+		goto err_gpio_irq;
+	}
+
+	if (bdata->reset_gpio >= 0) {
+		retval = fts_gpio_setup(bdata->reset_gpio, true, 1, 0);
+		if (retval < 0) {
+			logError(1, "%s %s: Failed to configure reset GPIO\n",
+				tag, __func__);
+			goto err_gpio_reset;
+		}
+	}
+	if (bdata->reset_gpio >= 0) {
+		gpio_set_value(bdata->reset_gpio, 0);
+		msleep(20);
+		gpio_set_value(bdata->reset_gpio, 1);
+	}
+
+	setResetGpio(bdata->reset_gpio);
+	return OK;
+
+err_gpio_reset:
+	fts_gpio_setup(bdata->irq_gpio, false, 0, 0);
+	setResetGpio(GPIO_NOT_DEFINED);
+err_gpio_irq:
+	return retval;
+}
+
+static int parse_dt(struct device *dev,
+		struct fts_i2c_platform_data *bdata)
+{
+	int retval;
+	const char *name;
+	struct device_node *np = dev->of_node;
+
+	bdata->irq_gpio = of_get_named_gpio_flags(np,
+		"st,irq-gpio", 0, NULL);
+
+	logError(0, "%s irq_gpio = %d\n", tag, bdata->irq_gpio);
+
+	bdata->pwr_on_suspend =
+		of_property_read_bool(np, "st,power_on_suspend");
+
+	retval = of_property_read_string(np, "st,regulator_dvdd", &name);
+	if (retval == -EINVAL)
+		bdata->pwr_reg_name = NULL;
+	else if (retval < 0)
+		return retval;
+
+	bdata->pwr_reg_name = name;
+	logError(0, "%s pwr_reg_name = %s\n", tag, name);
+
+	retval = of_property_read_string(np, "st,regulator_avdd", &name);
+	if (retval == -EINVAL)
+		bdata->bus_reg_name = NULL;
+	else if (retval < 0)
+		return retval;
+
+	bdata->bus_reg_name = name;
+	logError(0, "%s bus_reg_name = %s\n", tag, name);
+
+	if (of_property_read_bool(np, "st,reset-gpio")) {
+		bdata->reset_gpio = of_get_named_gpio_flags(np,
+				"st,reset-gpio", 0, NULL);
+		logError(0, "%s reset_gpio =%d\n", tag, bdata->reset_gpio);
+	} else {
+		bdata->reset_gpio = GPIO_NOT_DEFINED;
+	}
+
+	bdata->x_flip = of_property_read_bool(np, "st,x-flip");
+	bdata->y_flip = of_property_read_bool(np, "st,y-flip");
+
+	return OK;
+}
+
+static int check_dt(struct device_node *np)
+{
+	int i;
+	int count;
+	struct device_node *node;
+	struct drm_panel *panel;
+
+	count = of_count_phandle_with_args(np, "panel", NULL);
+	if (count <= 0)
+		return OK;
+
+	for (i = 0; i < count; i++) {
+		node = of_parse_phandle(np, "panel", i);
+		panel = of_drm_find_panel(node);
+		of_node_put(node);
+		if (!IS_ERR(panel)) {
+			active_panel = panel;
+			return OK;
+		}
+	}
+
+	return PTR_ERR(panel);
+}
+
+static int check_default_tp(struct device_node *dt, const char *prop)
+{
+	const char *active_tp;
+	const char *compatible;
+	char *start;
+	int ret;
+
+	ret = of_property_read_string(dt->parent, prop, &active_tp);
+	if (ret) {
+		pr_err(" %s:fail to read %s %d\n", __func__, prop, ret);
+		return -ENODEV;
+	}
+
+	ret = of_property_read_string(dt, "compatible", &compatible);
+	if (ret < 0) {
+		pr_err(" %s:fail to read %s %d\n", __func__, "compatible", ret);
+		return -ENODEV;
+	}
+
+	start = strnstr(active_tp, compatible, strlen(active_tp));
+	if (start == NULL) {
+		pr_err(" %s:no match compatible, %s, %s\n",
+			__func__, compatible, active_tp);
+		ret = -ENODEV;
+	}
+
+	return ret;
+}
+
+static int fts_probe_delayed(struct fts_ts_info *info)
+{
+	int error = 0;
+	int retval = 0;
+
+/* Avoid setting up hardware for TVM during probe */
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+#ifdef CONFIG_ARCH_QTI_VM
+	if (!atomic_read(&info->delayed_vm_probe_pending)) {
+		atomic_set(&info->delayed_vm_probe_pending, 1);
+		return 0;
+	}
+	goto tvm_setup;
+#endif
+#endif
+	logError(0, "%s SET Regulators:\n", tag);
+	retval = fts_get_reg(info, true);
+	if (retval < 0) {
+		logError(1, "%s ERROR: %s: Failed to get regulators\n",
+				tag, __func__);
+		goto Exit_1;
+	}
+	retval = fts_enable_reg(info, true);
+	if (retval < 0) {
+		logError(1,
+			"%s %s: ERROR Failed to enable regulators\n",
+				tag, __func__);
+		goto Exit_2;
+	}
+
+	logError(0, "%s SET GPIOS:\n", tag);
+	retval = fts_set_gpio(info);
+	if (retval < 0) {
+		logError(1, "%s %s: ERROR Failed to set up GPIO's\n",
+			tag, __func__);
+		goto Exit_2;
+	}
+
+	info->client->irq = gpio_to_irq(info->bdata->irq_gpio);
+	retval = fts_pinctrl_init(info);
+	if (!retval && info->ts_pinctrl) {
+		/*
+		 * Pinctrl handle is optional. If pinctrl handle is
+		 * found let pins to be configured in active state.
+		 * If not found continue further without error.
+		 */
+		retval = pinctrl_select_state(info->ts_pinctrl,
+						info->pinctrl_state_active);
+		if (retval < 0) {
+			logError(1,
+				"%s: Failed to select %s pinstate %d\n",
+			__func__, PINCTRL_STATE_ACTIVE, retval);
+		}
+	}
+
+#ifdef CONFIG_ARCH_QTI_VM
+tvm_setup:
+#endif
+	/* init hardware device */
+	logError(0, "%s Device Initialization:\n", tag);
+	error = fts_init(info);
+	if (error < OK) {
+		logError(1, "%s Cannot initialize the device ERROR %08X\n",
+			tag, error);
+		error = -ENODEV;
+#ifdef CONFIG_ARCH_QTI_VM
+		return error;
+#endif
+		goto Exit_3;
+	}
+
+	queue_delayed_work(info->fwu_workqueue, &info->fwu_work,
+			msecs_to_jiffies(EXP_FN_WORK_DELAY_MS));
+	return error;
+
+Exit_3:
+	if (info->ts_pinctrl) {
+		if (IS_ERR_OR_NULL(info->pinctrl_state_release)) {
+			devm_pinctrl_put(info->ts_pinctrl);
+			info->ts_pinctrl = NULL;
+		} else {
+			if (pinctrl_select_state(info->ts_pinctrl,
+						info->pinctrl_state_release))
+				logError(1, "%s:Failed to select %s pinstate\n",
+					__func__, PINCTRL_STATE_RELEASE);
+		}
+	}
+	fts_enable_reg(info, false);
+	fts_gpio_setup(info->bdata->irq_gpio, false, 0, 0);
+	fts_gpio_setup(info->bdata->reset_gpio, false, 0, 0);
+
+Exit_2:
+	fts_get_reg(info, false);
+Exit_1:
+	return error;
+}
+
+static int fts_probe_internal(struct i2c_client *client,
+		const struct i2c_device_id *idp)
+{
+	struct fts_ts_info *info = NULL;
+	int error = 0;
+	struct device_node *dp = client->dev.of_node;
+	int skip_5_1 = 0;
+
+	logError(0, "%s %s: driver probe begin!\n", tag, __func__);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		logError(1, "%s Unsupported I2C functionality\n", tag);
+		error = -EIO;
+		goto ProbeErrorExit_0;
+	}
+
+	openChannel(client);
+
+	info = kzalloc(sizeof(struct fts_ts_info), GFP_KERNEL);
+	if (!info) {
+		logError(1,
+			"%s can't allocate struct info!\n",
+			tag);
+		error = -ENOMEM;
+		goto ProbeErrorExit_0;
+	}
+
+	info->client = client;
+
+	i2c_set_clientdata(client, info);
+
+	info->i2c_data = kmalloc(I2C_DATA_MAX_LEN, GFP_KERNEL);
+	if (info->i2c_data == NULL) {
+		error = -ENOMEM;
+		goto ProbeErrorExit_0P1;
+	}
+	info->i2c_data_len = I2C_DATA_MAX_LEN;
+
+	logError(0, "%s i2c address: %x\n", tag, client->addr);
+	info->dev = &info->client->dev;
+	if (dp) {
+		info->bdata = devm_kzalloc(&client->dev,
+				sizeof(struct fts_i2c_platform_data),
+				GFP_KERNEL);
+		if (!info->bdata) {
+			logError(1, "%s ERROR:info.bdata kzalloc failed\n",
+				tag);
+			goto ProbeErrorExit_1;
+		}
+		parse_dt(&client->dev, info->bdata);
+	}
+
+	logError(0, "%s SET Auto Fw Update:\n", tag);
+	info->fwu_workqueue = alloc_workqueue("fts-fwu-queue",
+				WQ_UNBOUND|WQ_HIGHPRI|WQ_CPU_INTENSIVE, 1);
+	if (!info->fwu_workqueue) {
+		logError(1, "%s ERROR: Cannot create fwu work thread\n", tag);
+		goto ProbeErrorExit_1;
+	}
+
+	INIT_DELAYED_WORK(&info->fwu_work, fts_fw_update_auto);
+
+	logError(0, "%s SET Event Handler:\n", tag);
+	info->wakeup_source = wakeup_source_register(&client->dev,
+			dev_name(&client->dev));
+
+	info->event_wq = alloc_workqueue("fts-event-queue",
+				WQ_UNBOUND|WQ_HIGHPRI|WQ_CPU_INTENSIVE, 1);
+	if (!info->event_wq) {
+		logError(1, "%s ERROR: Cannot create work thread\n", tag);
+		error = -ENOMEM;
+		goto ProbeErrorExit_4;
+	}
+
+	INIT_WORK(&info->work, fts_event_handler);
+
+	INIT_WORK(&info->resume_work, fts_resume_work);
+	INIT_WORK(&info->suspend_work, fts_suspend_work);
+
+	logError(0, "%s SET Input Device Property:\n", tag);
+	/* info->dev = &info->client->dev; */
+	info->input_dev = input_allocate_device();
+	if (!info->input_dev) {
+		logError(1, "%s ERROR: No such input device defined!\n",
+			tag);
+		error = -ENODEV;
+		goto ProbeErrorExit_5;
+	}
+	info->input_dev->dev.parent = &client->dev;
+	info->input_dev->name = FTS_TS_DRV_NAME;
+	snprintf(fts_ts_phys, sizeof(fts_ts_phys), "%s/input0",
+		info->input_dev->name);
+	info->input_dev->phys = fts_ts_phys;
+	info->input_dev->id.bustype = BUS_I2C;
+	info->input_dev->id.vendor = 0x0001;
+	info->input_dev->id.product = 0x0002;
+	info->input_dev->id.version = 0x0100;
+
+	__set_bit(EV_SYN, info->input_dev->evbit);
+	__set_bit(EV_KEY, info->input_dev->evbit);
+	__set_bit(EV_ABS, info->input_dev->evbit);
+	__set_bit(BTN_TOUCH, info->input_dev->keybit);
+	__set_bit(BTN_TOOL_FINGER, info->input_dev->keybit);
+
+	input_mt_init_slots(info->input_dev, TOUCH_ID_MAX, INPUT_MT_DIRECT);
+
+	input_set_abs_params(info->input_dev, ABS_MT_POSITION_X,
+			X_AXIS_MIN, X_AXIS_MAX, 0, 0);
+	input_set_abs_params(info->input_dev, ABS_MT_POSITION_Y,
+			Y_AXIS_MIN, Y_AXIS_MAX, 0, 0);
+	input_set_abs_params(info->input_dev, ABS_MT_TOUCH_MAJOR,
+			AREA_MIN, AREA_MAX, 0, 0);
+	input_set_abs_params(info->input_dev, ABS_MT_TOUCH_MINOR,
+			AREA_MIN, AREA_MAX, 0, 0);
+
+#ifdef PHONE_GESTURE
+	input_set_capability(info->input_dev, EV_KEY, KEY_WAKEUP);
+
+	input_set_capability(info->input_dev, EV_KEY, KEY_M);
+	input_set_capability(info->input_dev, EV_KEY, KEY_O);
+	input_set_capability(info->input_dev, EV_KEY, KEY_E);
+	input_set_capability(info->input_dev, EV_KEY, KEY_W);
+	input_set_capability(info->input_dev, EV_KEY, KEY_C);
+	input_set_capability(info->input_dev, EV_KEY, KEY_L);
+	input_set_capability(info->input_dev, EV_KEY, KEY_F);
+	input_set_capability(info->input_dev, EV_KEY, KEY_V);
+	input_set_capability(info->input_dev, EV_KEY, KEY_S);
+	input_set_capability(info->input_dev, EV_KEY, KEY_Z);
+	input_set_capability(info->input_dev, EV_KEY, KEY_WWW);
+
+	input_set_capability(info->input_dev, EV_KEY, KEY_LEFT);
+	input_set_capability(info->input_dev, EV_KEY, KEY_RIGHT);
+	input_set_capability(info->input_dev, EV_KEY, KEY_UP);
+	input_set_capability(info->input_dev, EV_KEY, KEY_DOWN);
+
+	input_set_capability(info->input_dev, EV_KEY, KEY_F1);
+	input_set_capability(info->input_dev, EV_KEY, KEY_F2);
+	input_set_capability(info->input_dev, EV_KEY, KEY_F3);
+	input_set_capability(info->input_dev, EV_KEY, KEY_F4);
+	input_set_capability(info->input_dev, EV_KEY, KEY_F5);
+
+	input_set_capability(info->input_dev, EV_KEY, KEY_LEFTBRACE);
+	input_set_capability(info->input_dev, EV_KEY, KEY_RIGHTBRACE);
+#endif
+
+#ifdef PHONE_KEY
+	/* KEY associated to the touch screen buttons */
+	input_set_capability(info->input_dev, EV_KEY, KEY_HOMEPAGE);
+	input_set_capability(info->input_dev, EV_KEY, KEY_BACK);
+	input_set_capability(info->input_dev, EV_KEY, KEY_MENU);
+#endif
+
+	mutex_init(&(info->input_report_mutex));
+
+#ifdef PHONE_GESTURE
+	mutex_init(&gestureMask_mutex);
+#endif
+
+	/* register the multi-touch input device */
+	error = input_register_device(info->input_dev);
+	if (error) {
+		logError(1, "%s ERROR: No such input device\n", tag);
+		error = -ENODEV;
+		goto ProbeErrorExit_5_1;
+	}
+
+	skip_5_1 = 1;
+	/* track slots */
+	info->touch_id = 0;
+#ifdef STYLUS_MODE
+	info->stylus_id = 0;
+#endif
+
+	/*
+	 * init feature switches (by default all the features
+	 * are disable, if one feature want to be enabled from
+	 * the start, set the corresponding value to 1)
+	 */
+	info->gesture_enabled = 0;
+	info->glove_enabled = 0;
+	info->charger_enabled = 0;
+	info->stylus_enabled = 0;
+	info->vr_enabled = 0;
+	info->cover_enabled = 0;
+	info->edge_rej_enabled = 0;
+	info->corner_rej_enabled = 0;
+	info->edge_palm_rej_enabled = 0;
+
+	info->resume_bit = 1;
+	info->notifier = fts_noti_block;
+
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+	fts_trusted_touch_init(info);
+	mutex_init(&(info->fts_clk_io_ctrl_mutex));
+#endif
+
+	logError(0, "%s SET Device File Nodes:\n", tag);
+	/* sysfs stuff */
+	info->attrs.attrs = fts_attr_group;
+	error = sysfs_create_group(&client->dev.kobj, &info->attrs);
+	if (error) {
+		logError(1, "%s ERROR: Cannot create sysfs structure!\n", tag);
+		error = -ENODEV;
+		goto ProbeErrorExit_7;
+	}
+
+	/* I2C cmd */
+	fts_cmd_class = class_create(THIS_MODULE, FTS_TS_DRV_NAME);
+
+#ifdef SCRIPTLESS
+	info->i2c_cmd_dev = device_create(fts_cmd_class,
+			NULL, DCHIP_ID_0, info, "fts_i2c");
+	if (IS_ERR(info->i2c_cmd_dev)) {
+		logError(1,
+			"%s ERROR: Failed to create device for the sysfs!\n",
+			tag);
+		goto ProbeErrorExit_8;
+	}
+
+	dev_set_drvdata(info->i2c_cmd_dev, info);
+
+	error = sysfs_create_group(&info->i2c_cmd_dev->kobj,
+			&i2c_cmd_attr_group);
+	if (error) {
+		logError(1, "%s ERROR: Failed to create sysfs group!\n", tag);
+		goto ProbeErrorExit_9;
+	}
+#endif
+
+#ifdef DRIVER_TEST
+	info->test_cmd_dev = device_create(fts_cmd_class,
+			NULL, DCHIP_ID_0, info, "fts_driver_test");
+	if (IS_ERR(info->test_cmd_dev)) {
+		logError(1,
+			"%s ERROR: Failed to create device for the sysfs!\n",
+			tag);
+		goto ProbeErrorExit_10;
+	}
+
+	dev_set_drvdata(info->test_cmd_dev, info);
+
+	error = sysfs_create_group(&info->test_cmd_dev->kobj,
+			&test_cmd_attr_group);
+	if (error) {
+		logError(1, "%s ERROR: Failed to create sysfs group!\n", tag);
+		goto ProbeErrorExit_11;
+	}
+#endif
+
+	info->aoi_cmd_dev = device_create(fts_cmd_class,
+			NULL, DCHIP_ID_0, info, "touch_aoi");
+	if (IS_ERR(info->aoi_cmd_dev)) {
+		logError(1,
+			"%s ERROR: Failed to create device for the sysfs\n",
+			tag);
+		goto ProbeErrorExit_10;
+	}
+
+	dev_set_drvdata(info->aoi_cmd_dev, info);
+
+	error = sysfs_create_group(&info->aoi_cmd_dev->kobj,
+			&aoi_cmd_attr_group);
+	if (error) {
+		logError(1, "%s ERROR: Failed to create sysfs group\n", tag);
+		goto ProbeErrorExit_11;
+	}
+
+	info->aoi_class = class_create(THIS_MODULE, "android_touch");
+	if (info->aoi_class) {
+		info->aoi_dev = device_create(info->aoi_class,
+			NULL, DCHIP_ID_0, info, "touch");
+		if (!IS_ERR(info->aoi_dev)) {
+			dev_set_drvdata(info->aoi_dev, info);
+
+			error = sysfs_create_group(&info->aoi_dev->kobj,
+				&aoi_enable_attr_group);
+		}
+	}
+
+	error = fts_probe_delayed(info);
+	if (error) {
+		logError(1, "%s ERROR: Failed to enable resources\n",
+						tag);
+		goto ProbeErrorExit_11;
+	}
+	logError(1, "%s Probe Finished!\n", tag);
+	return OK;
+
+	/* error exit path */
+#ifdef DRIVER_TEST
+ProbeErrorExit_11:
+#ifndef SCRIPTLESS
+	device_destroy(fts_cmd_class, DCHIP_ID_0);
+#endif
+
+ProbeErrorExit_10:
+#ifndef SCRIPTLESS
+	sysfs_remove_group(&client->dev.kobj, &info->attrs);
+#endif
+#endif
+
+#ifdef SCRIPTLESS
+ProbeErrorExit_9:
+	device_destroy(fts_cmd_class, DCHIP_ID_0);
+
+ProbeErrorExit_8:
+	sysfs_remove_group(&client->dev.kobj, &info->attrs);
+#endif
+
+ProbeErrorExit_7:
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+	fts_vm_deinit(info);
+#endif
+	/* fb_unregister_client(&info->notifier); */
+	input_unregister_device(info->input_dev);
+
+ProbeErrorExit_5_1:
+	if (skip_5_1 != 1)
+		input_free_device(info->input_dev);
+
+ProbeErrorExit_5:
+	destroy_workqueue(info->event_wq);
+
+ProbeErrorExit_4:
+	destroy_workqueue(info->fwu_workqueue);
+	wakeup_source_unregister(info->wakeup_source);
+
+ProbeErrorExit_1:
+	kfree(info->i2c_data);
+ProbeErrorExit_0P1:
+	kfree(info);
+
+ProbeErrorExit_0:
+	logError(1, "%s Probe Failed!\n", tag);
+
+	return error;
+}
+
+static int fts_probe(struct i2c_client *client, const struct i2c_device_id *idp)
+{
+	int error = 0;
+	struct device_node *dp = client->dev.of_node;
+
+	error = check_dt(dp);
+	if (error == -EPROBE_DEFER)
+		return error;
+
+	if (error) {
+		if (!check_default_tp(dp, "qcom,i2c-touch-active"))
+			error = -EPROBE_DEFER;
+		else
+			error = -ENODEV;
+
+		return error;
+	}
+
+	device_init_wakeup(&client->dev, true);
+	return fts_probe_internal(client, idp);
+}
+
+static int fts_remove(struct i2c_client *client)
+{
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	if (info->aoi_dev) {
+		sysfs_remove_group(&info->aoi_dev->kobj,
+			&aoi_enable_attr_group);
+		info->aoi_dev = NULL;
+	}
+
+	if (info->aoi_class) {
+		device_destroy(info->aoi_class, DCHIP_ID_0);
+		info->aoi_class = NULL;
+	}
+
+#ifdef DRIVER_TEST
+	sysfs_remove_group(&info->test_cmd_dev->kobj,
+			&test_cmd_attr_group);
+#endif
+
+#ifdef SCRIPTLESS
+	/* I2C cmd */
+	sysfs_remove_group(&info->i2c_cmd_dev->kobj, &i2c_cmd_attr_group);
+#endif
+
+#if defined(SCRIPTLESS) || defined(DRIVER_TEST)
+	device_destroy(fts_cmd_class, DCHIP_ID_0);
+#endif
+
+	/* sysfs stuff */
+	sysfs_remove_group(&client->dev.kobj, &info->attrs);
+
+	/* remove interrupt and event handlers */
+	fts_interrupt_uninstall(info);
+
+#if defined(CONFIG_FB_MSM)
+	fb_unregister_client(&info->notifier);
+#else
+	if (active_panel)
+		drm_panel_notifier_register(active_panel, &info->notifier);
+#endif
+
+	/* unregister the device */
+	input_unregister_device(info->input_dev);
+
+	/* input_free_device(info->input_dev ); */
+
+	/* Empty the FIFO buffer */
+	fts_command(info, FIFO_CMD_FLUSH);
+	/* flushFIFO(); */
+
+	/* Remove the work thread */
+	destroy_workqueue(info->event_wq);
+	/* wake_lock_destroy(&info->wakelock); */
+	wakeup_source_unregister(info->wakeup_source);
+	destroy_workqueue(info->fwu_workqueue);
+
+	if (info->ts_pinctrl) {
+		if (IS_ERR_OR_NULL(info->pinctrl_state_release)) {
+			devm_pinctrl_put(info->ts_pinctrl);
+			info->ts_pinctrl = NULL;
+		} else {
+			pinctrl_select_state(info->ts_pinctrl,
+						info->pinctrl_state_release);
+		}
+	}
+	fts_enable_reg(info, false);
+	fts_gpio_setup(info->bdata->irq_gpio, false, 0, 0);
+	fts_gpio_setup(info->bdata->reset_gpio, false, 0, 0);
+
+	fts_get_reg(info, false);
+
+	/* free all */
+	kfree(info->i2c_data);
+	kfree(info);
+
+	device_init_wakeup(&client->dev, false);
+	return OK;
+}
+
+static const struct of_device_id fts_of_match_table[] = {
+	{
+		.compatible = "st,fts",
+	},
+	{},
+};
+static const struct i2c_device_id fts_device_id[] = {
+	{FTS_TS_DRV_NAME, 0},
+	{}
+};
+
+static struct i2c_driver fts_i2c_driver = {
+	.driver = {
+		.name = FTS_TS_DRV_NAME,
+		.of_match_table = fts_of_match_table,
+	},
+	.probe = fts_probe,
+	.remove = fts_remove,
+	.id_table = fts_device_id,
+};
+
+static int __init fts_driver_init(void)
+{
+	return i2c_add_driver(&fts_i2c_driver);
+}
+
+static void __exit fts_driver_exit(void)
+{
+	i2c_del_driver(&fts_i2c_driver);
+}
+
+module_init(fts_driver_init);
+module_exit(fts_driver_exit);
+
+MODULE_DESCRIPTION("STMicroelectronics MultiTouch IC Driver");
+MODULE_LICENSE("GPL v2");

st/fts.h

@@ -0,0 +1,396 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _LINUX_FTS_I2C_H_
+#define _LINUX_FTS_I2C_H_
+
+/*#include <linux/wakelock.h>*/
+#include <linux/pm_wakeup.h>
+#include <linux/timekeeping.h>
+#include <linux/gunyah/gh_irq_lend.h>
+
+#include "fts_lib/ftsSoftware.h"
+#include "fts_lib/ftsHardware.h"
+#include "fts_lib/ftsGesture.h"
+
+#define FTS_POWER_ON     1
+#define FTS_POWER_OFF    0
+
+/****************** CONFIGURATION SECTION ******************/
+
+/**** CODE CONFIGURATION ****/
+
+#define FTS_TS_DRV_NAME      "fts"
+#define FTS_TS_DRV_VERSION   "4.2.14" /* version */
+
+/*#define SCRIPTLESS*/ /*allow to work in scriptless mode with the GUI*/
+#ifdef SCRIPTLESS
+#define SCRIPTLESS_DEBUG
+/**
+ * uncomment this macro definition to print debug
+ * message for script less  support
+ */
+#endif
+
+#define DRIVER_TEST
+
+/* #define FW_H_FILE */ /*include the FW as header file*/
+#ifdef FW_H_FILE
+	#define FW_SIZE_NAME myArray_size
+	#define FW_ARRAY_NAME myArray
+#endif
+
+/*#define LIMITS_H_FILE*/ /*include the limits file as header file*/
+#ifdef LIMITS_H_FILE
+	#define LIMITS_SIZE_NAME myArray2_size
+	#define LIMITS_ARRAY_NAME myArray2
+#endif
+
+/**** END ****/
+
+
+/**** FEATURES USED IN THE IC ***/
+/* #define PHONE_KEY enable the keys */
+
+#define PHONE_GESTURE /*allow to use the gestures*/
+#ifdef PHONE_GESTURE
+	#define USE_GESTURE_MASK
+	#define USE_CUSTOM_GESTURES
+#endif
+
+#define USE_ONE_FILE_NODE
+/*allow to enable/disable all the features just using one file node*/
+
+#define EDGE_REJ
+/*allow edge rej feature (comment to disable)*/
+
+#define CORNER_REJ
+/*allow corn rej feature (comment to disable)*/
+
+#define EDGE_PALM_REJ
+/*allow edge palm rej feature (comment to disable)*/
+
+#define CHARGER_MODE
+/*allow charger mode feature (comment to disable)*/
+
+#define GLOVE_MODE
+/*allow glove mode feature (comment to disable)*/
+
+#define VR_MODE
+/*allow vr mode feature (comment to disable)*/
+
+#define COVER_MODE
+/*allow cover mode feature (comment to disable)*/
+
+#define STYLUS_MODE
+/*allow stylus mode feature (comment to disable)*/
+
+#define USE_NOISE_PARAM
+/*set noise params during resume (comment to disable)*/
+
+/**** END ****/
+
+
+/**** PANEL SPECIFICATION ****/
+#define X_AXIS_MAX           1440
+#define X_AXIS_MIN           0
+#define Y_AXIS_MAX           2880
+#define Y_AXIS_MIN           0
+
+#define PRESSURE_MIN         0
+#define PRESSURE_MAX         127
+
+#define TOUCH_ID_MAX         10
+
+#define AREA_MIN             PRESSURE_MIN
+#define AREA_MAX             PRESSURE_MAX
+/**** END ****/
+
+/*********************************************************/
+
+/* Flash programming */
+
+#define INIT_FLAG_CNT        3
+
+/* KEYS */
+#define KEY1                 0x02
+#define KEY2                 0x01
+#define KEY3                 0x04
+
+/*
+ * Configuration mode
+ */
+/**
+ * bitmask which can assume the value defined as
+ * features in ftsSoftware.h or the following values
+ */
+
+#define MODE_NOTHING         0x00000000
+#define MODE_SENSEON         0x10000000
+#define MODE_SENSEOFF        0x20000000
+#define FEAT_GESTURE         0x40000000
+
+
+/*
+ * Status Event Field:
+ * id of command that triggered the event
+ */
+
+#define FTS_FLASH_WRITE_CONFIG      0x03
+#define FTS_FLASH_WRITE_COMP_MEMORY 0x04
+#define FTS_FORCE_CAL_SELF_MUTUAL   0x05
+#define FTS_FORCE_CAL_SELF          0x06
+#define FTS_WATER_MODE_ON           0x07
+#define FTS_WATER_MODE_OFF          0x08
+
+
+#define EXP_FN_WORK_DELAY_MS 1000
+
+#define CMD_STR_LEN		32
+#define I2C_DATA_MAX_LEN	32
+
+#ifdef SCRIPTLESS
+/*
+ * I2C Command Read/Write Function
+ */
+
+#define CMD_RESULT_STR_LEN   2048
+#endif
+
+#define TSP_BUF_SIZE         4096
+
+#define PINCTRL_STATE_ACTIVE    "pmx_ts_active"
+#define PINCTRL_STATE_SUSPEND   "pmx_ts_suspend"
+#define PINCTRL_STATE_RELEASE   "pmx_ts_release"
+
+/*add by guchong*/
+#ifdef PHONE_GESTURE
+extern u16 gesture_coordinates_x[GESTURE_COORDS_REPORT_MAX];
+extern u16 gesture_coordinates_y[GESTURE_COORDS_REPORT_MAX];
+extern int gesture_coords_reported;
+extern struct mutex gestureMask_mutex;
+#endif
+
+struct fts_i2c_platform_data {
+	bool x_flip;
+	bool y_flip;
+	int (*power)(bool on);
+	int irq_gpio;
+	int reset_gpio;
+	const char *pwr_reg_name;
+	const char *bus_reg_name;
+	bool pwr_on_suspend;
+};
+
+/*
+ * Forward declaration
+ */
+struct fts_ts_info;
+
+/*
+ * Dispatch event handler
+ */
+struct event_dispatch_handler_t {
+	void (*handler)(struct fts_ts_info *info, unsigned char *data);
+};
+
+enum trusted_touch_mode_config {
+	TRUSTED_TOUCH_VM_MODE,
+	TRUSTED_TOUCH_MODE_NONE
+};
+
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+#define TRUSTED_TOUCH_MEM_LABEL 0x7
+
+struct trusted_touch_vm_info {
+	enum gh_irq_label irq_label;
+	enum gh_vm_names vm_name;
+	u32 hw_irq;
+	gh_memparcel_handle_t vm_mem_handle;
+	u32 *iomem_bases;
+	u32 *iomem_sizes;
+	u32 iomem_list_size;
+	void *mem_cookie;
+#ifdef CONFIG_ARCH_QTI_VM
+	atomic_t tvm_owns_iomem;
+	atomic_t tvm_owns_irq;
+#else
+	atomic_t pvm_owns_iomem;
+	atomic_t pvm_owns_irq;
+#endif
+};
+#endif
+
+/*
+ * struct fts_ts_info - FTS capacitive touch screen device information
+ * @dev:                  Pointer to the structure device
+ * @client:               I2C client structure
+ * @input_dev             Input device structure
+ * @work                  Work thread
+ * @event_wq              Event queue for work thread
+ * @event_dispatch_table  Event dispatch table handlers
+ * @attrs                 SysFS attributes
+ * @mode                  Device operating mode (bitmask)
+ * @touch_id              Bitmask for touch id (mapped to input slots)
+ * @stylus_id             Bitmask for tracking the stylus touches
+ * (mapped using the touchId)
+ * @timer                 Timer when operating in polling mode
+ * @power                 Power on/off routine
+ * @bdata                 HW info retrived from device tree
+ * @pwr_reg               DVDD power regulator
+ * @bus_reg               AVDD power regulator
+ * @resume_bit            Indicate if screen off/on
+ * @fwupdate_stat         Store the result of a fw update triggered by the host
+ * @notifier              Used for be notified from a suspend/resume event
+ * @sensor_sleep          true susped was called, false resume was called
+ * @wakelock              Wake Lock struct
+ * @input_report_mutex    mutex for handling the pressure of keys
+ * @series of switches    to store the enabling status of a particular
+ * feature from the host
+ */
+struct fts_ts_info {
+	struct device            *dev;
+	struct i2c_client        *client;
+	struct input_dev         *input_dev;
+
+	struct work_struct       work;
+	struct work_struct       suspend_work;
+	struct work_struct       resume_work;
+	struct workqueue_struct  *event_wq;
+
+	struct delayed_work      fwu_work;
+	struct workqueue_struct  *fwu_workqueue;
+	struct completion        cmd_done;
+
+	struct pinctrl *ts_pinctrl;
+	struct pinctrl_state *pinctrl_state_active;
+	struct pinctrl_state *pinctrl_state_suspend;
+	struct pinctrl_state *pinctrl_state_release;
+
+	struct event_dispatch_handler_t *event_dispatch_table;
+
+	struct attribute_group    attrs;
+
+	unsigned int              mode;
+	unsigned long             touch_id;
+#ifdef STYLUS_MODE
+	unsigned long             stylus_id;
+#endif
+
+
+#ifdef FTS_USE_POLLING_MODE
+	struct hrtimer            timer;
+#endif
+
+
+#ifdef SCRIPTLESS
+	/*I2C cmd*/
+	struct device             *i2c_cmd_dev;
+	char cmd_read_result[CMD_RESULT_STR_LEN];
+	char cmd_wr_result[CMD_RESULT_STR_LEN];
+	char cmd_write_result[20];
+#endif
+
+#ifdef DRIVER_TEST
+	struct device             *test_cmd_dev;
+#endif
+	int (*power)(bool on);
+
+	struct fts_i2c_platform_data *bdata;
+	struct regulator          *pwr_reg;
+	struct regulator          *bus_reg;
+
+	int resume_bit;
+	int fwupdate_stat;
+
+	struct notifier_block notifier;
+	bool sensor_sleep;
+	struct wakeup_source *wakeup_source;
+
+	/* input lock */
+	struct mutex input_report_mutex;
+
+	/*switches for features*/
+	unsigned int gesture_enabled;
+	unsigned int glove_enabled;
+	unsigned int charger_enabled;
+	unsigned int stylus_enabled;
+	unsigned int vr_enabled;
+	unsigned int cover_enabled;
+	unsigned int edge_rej_enabled;
+	unsigned int corner_rej_enabled;
+	unsigned int edge_palm_rej_enabled;
+
+	uint8_t *i2c_data;
+	uint8_t i2c_data_len;
+
+	struct device *aoi_cmd_dev;
+	bool aoi_notify_enabled;
+	bool aoi_wake_on_suspend;
+
+	/* aoi region */
+	struct class *aoi_class;
+	struct device *aoi_dev;
+	int aoi_left;
+	int aoi_top;
+	int aoi_bottom;
+	int aoi_right;
+
+#ifdef CONFIG_ST_TRUSTED_TOUCH
+	struct trusted_touch_vm_info *vm_info;
+	struct mutex fts_clk_io_ctrl_mutex;
+	const char *touch_environment;
+	struct completion trusted_touch_powerdown;
+	struct completion resource_checkpoint;
+	struct clk *core_clk;
+	struct clk *iface_clk;
+	atomic_t trusted_touch_initialized;
+	atomic_t trusted_touch_enabled;
+	atomic_t delayed_vm_probe_pending;
+	atomic_t trusted_touch_mode;
+#endif
+};
+
+extern struct chipInfo ftsInfo;
+
+int fts_chip_powercycle(struct fts_ts_info *info);
+int fts_chip_powercycle2(struct fts_ts_info *info, unsigned long sleep);
+void release_all_touches(struct fts_ts_info *info);
+/*int fts_get_fw_version(struct fts_ts_info *info);*/
+/*extern unsigned int le_to_uint(const unsigned char *ptr);*/
+/*extern unsigned int be_to_uint(const unsigned char *ptr);*/
+extern int input_register_notifier_client(struct notifier_block *nb);
+extern int input_unregister_notifier_client(struct notifier_block *nb);
+
+extern struct attribute_group aoi_cmd_attr_group;
+extern struct attribute_group aoi_enable_attr_group;
+
+#ifdef SCRIPTLESS
+extern struct attribute_group i2c_cmd_attr_group;
+#endif
+
+#ifdef DRIVER_TEST
+extern struct attribute_group test_cmd_attr_group;
+#endif
+
+
+#endif
+

st/fts_aoi_event.c

@@ -0,0 +1,153 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2019-2020, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include "fts.h"
+
+static ssize_t touch_event_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+
+	return 0;
+}
+
+ssize_t aoi_set_store(struct device *dev, struct device_attribute *attr,
+	const char *buf, size_t count)
+{
+	int ret;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	int left, top, right, bottom;
+
+	ret = sscanf(buf, "%d %d %d %d", &left, &top, &right, &bottom);
+	if (ret != 4)
+		return -EINVAL;
+
+	if (right > X_AXIS_MAX)
+		right = X_AXIS_MAX;
+	if (bottom > Y_AXIS_MAX)
+		bottom = Y_AXIS_MAX;
+
+	if (left < 0 || left > X_AXIS_MAX || right < 0 ||
+		top > Y_AXIS_MAX || bottom < 0)
+		return -EINVAL;
+
+	if (left >= right || top >= bottom) {
+		info->aoi_left = 0;
+		info->aoi_top = 0;
+		info->aoi_right = 0;
+		info->aoi_bottom = 0;
+		info->aoi_notify_enabled = false;
+		return count;
+	}
+
+	info->aoi_left = left;
+	info->aoi_top = top;
+	info->aoi_right = right;
+	info->aoi_bottom = bottom;
+
+	info->aoi_notify_enabled = true;
+	return count;
+}
+
+static ssize_t aoi_set_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	size_t len = 0;
+
+	len = scnprintf(buf + len, PAGE_SIZE,
+				"%d %d %d %d",
+				info->aoi_left,
+				info->aoi_top,
+				info->aoi_right,
+				info->aoi_bottom);
+
+	return len;
+}
+
+static ssize_t power_set_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int enable;
+
+	if (kstrtoint(buf, 10, &enable))
+		return -EINVAL;
+
+	return count;
+}
+
+static DEVICE_ATTR_RO(touch_event);
+static DEVICE_ATTR_RW(aoi_set);
+static DEVICE_ATTR_WO(power_set);
+
+static struct attribute *aoi_cmd_attributes[] = {
+	&dev_attr_touch_event.attr,
+	&dev_attr_aoi_set.attr,
+	&dev_attr_power_set.attr,
+	NULL,
+};
+
+struct attribute_group aoi_cmd_attr_group = {
+	.attrs = aoi_cmd_attributes,
+};
+
+static ssize_t enable_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fts_ts_info *info = dev_get_drvdata(dev);
+	int enable;
+
+	if (kstrtoint(buf, 10, &enable))
+		return -EINVAL;
+
+	if (!enable && info->aoi_notify_enabled) {
+		info->aoi_left = 0;
+		info->aoi_top = 0;
+		info->aoi_right = 0;
+		info->aoi_bottom = 0;
+		info->aoi_notify_enabled = false;
+	} else {
+		info->aoi_left = 0;
+		info->aoi_top = 0;
+		info->aoi_right = X_AXIS_MAX;
+		info->aoi_bottom = Y_AXIS_MAX;
+		info->aoi_notify_enabled = true;
+	}
+
+	return count;
+}
+
+static ssize_t enable_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct fts_ts_info *info = dev_get_drvdata(dev);
+	size_t len = 0;
+
+	len = scnprintf(buf, PAGE_SIZE,
+				"%d",
+				info->aoi_notify_enabled);
+
+	return len;
+}
+
+static DEVICE_ATTR_RW(enable);
+
+static struct attribute *aoi_enable_attributes[] = {
+	&dev_attr_aoi_set.attr,
+	&dev_attr_enable.attr,
+	NULL,
+};
+
+struct attribute_group aoi_enable_attr_group = {
+	.attrs = aoi_enable_attributes,
+};

st/fts_driver_test.c

@@ -0,0 +1,1107 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/completion.h>
+/*#include <linux/wakelock.h>*/
+#include <linux/pm_wakeup.h>
+
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/consumer.h>
+#include "fts.h"
+#include "fts_lib/ftsCompensation.h"
+#include "fts_lib/ftsIO.h"
+#include "fts_lib/ftsError.h"
+#include "fts_lib/ftsFrame.h"
+#include "fts_lib/ftsFlash.h"
+#include "fts_lib/ftsTest.h"
+#include "fts_lib/ftsTime.h"
+#include "fts_lib/ftsTool.h"
+
+#ifdef DRIVER_TEST
+
+#define MAX_PARAMS 50
+
+/*DEFINE COMMANDS TO TEST*/
+#define CMD_READ               0x00
+#define CMD_WRITE              0x01
+#define CMD_READU16            0x02
+#define CMD_READB2             0x03
+#define CMD_READB2U16          0x04
+#define CMD_POLLFOREVENT       0x05
+#define CMD_SYSTEMRESET        0x06
+#define CMD_CLEANUP            0x07
+#define CMD_GETFORCELEN        0x08
+#define CMD_GETSENSELEN        0x09
+#define CMD_GETMSFRAME         0x0A
+/*#define CMD_GETMSKEYFRAME        0x0B*/
+#define CMD_GETSSFRAME         0x0C
+#define CMD_REQCOMPDATA        0x0D
+#define CMD_READCOMPDATAHEAD   0x0E
+#define CMD_READMSCOMPDATA     0x0F
+#define CMD_READSSCOMPDATA     0x10
+#define CMD_READGNCOMPDATA     0x11
+#define CMD_GETFWVER           0x12
+#define CMD_FLASHSTATUS        0x13
+#define CMD_FLASHUNLOCK        0x14
+#define CMD_READFWFILE         0x15
+#define CMD_FLASHPROCEDURE     0x16
+#define CMD_ITOTEST            0x17
+#define CMD_INITTEST           0x18
+#define CMD_MSRAWTEST          0x19
+#define CMD_MSINITDATATEST     0x1A
+#define CMD_SSRAWTEST          0x1B
+#define CMD_SSINITDATATEST     0x1C
+#define CMD_MAINTEST           0x1D
+#define CMD_POWERCYCLE         0x1E
+#define CMD_FWWRITE            0x1F
+#define CMD_READCHIPINFO       0x20
+#define CMD_REQFRAME           0x21
+
+static char tag[8] = "[ FTS ]\0";
+static u32 functionToTest[MAX_PARAMS];
+static int numberParam;
+
+static ssize_t stm_driver_test_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int n;
+	char *p = (char *)buf;
+	int ret;
+
+	memset(functionToTest, 0, MAX_PARAMS * sizeof(u32));
+
+	for (n = 0; n < (count + 1) / 3 && n < MAX_PARAMS; n++) {
+		ret = sscanf(p, "%02X ", &functionToTest[n]);
+		if (ret != 1)
+			return -EINVAL;
+		p += 3;
+		logError(1, "%s functionToTest[%d] = %02X\n", tag, n,
+			functionToTest[n]);
+	}
+
+	numberParam = n;
+	logError(1, "%s Number of Parameters = %d\n", tag, numberParam);
+	return count;
+}
+
+static ssize_t stm_driver_test_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	char buff[CMD_STR_LEN] = {0};
+	int res = -1, j, count;
+	int size = 6 * 2;
+	int temp = 0;
+	int i;
+	int byteToRead = 0;
+	u8 *readData = NULL;
+	u8 *all_strbuff = NULL;
+	u8 *cmd = NULL;
+
+	struct MutualSenseFrame frameMS = {0};
+	struct SelfSenseFrame frameSS = {0};
+
+	struct DataHeader dataHead = {0};
+	struct MutualSenseData compData = {0};
+	struct SelfSenseData comData = {0};
+	struct GeneralData gnData = {0};
+
+	u16 address = 0;
+	u16 fw_version = 0;
+	u16 config_id = 0;
+
+	struct Firmware fw;
+
+	/*struct used for defining which test*/
+	/*perform during the  MP test*/
+
+	struct TestToDo todoDefault;
+
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	fw.data = NULL;
+
+	todoDefault.MutualRaw = 1;
+	todoDefault.MutualRawGap = 1;
+	todoDefault.MutualCx1 = 0;
+	todoDefault.MutualCx2 = 1;
+	todoDefault.MutualCx2Adj = 1;
+	todoDefault.MutualCxTotal = 0;
+	todoDefault.MutualCxTotalAdj = 0;
+
+	todoDefault.MutualKeyRaw = 0;
+	todoDefault.MutualKeyCx1 = 0;
+	todoDefault.MutualKeyCx2 = 0;
+	todoDefault.MutualKeyCxTotal = 0;
+
+	todoDefault.SelfForceRaw = 1;
+	todoDefault.SelfForceRawGap = 0;
+	todoDefault.SelfForceIx1 = 0;
+	todoDefault.SelfForceIx2 = 0;
+	todoDefault.SelfForceIx2Adj = 0;
+	todoDefault.SelfForceIxTotal = 1;
+	todoDefault.SelfForceIxTotalAdj = 0;
+	todoDefault.SelfForceCx1 = 0;
+	todoDefault.SelfForceCx2 = 0;
+	todoDefault.SelfForceCx2Adj = 0;
+	todoDefault.SelfForceCxTotal = 0;
+	todoDefault.SelfForceCxTotalAdj = 0;
+
+	todoDefault.SelfSenseRaw = 1;
+	todoDefault.SelfSenseRawGap = 0;
+	todoDefault.SelfSenseIx1 = 0;
+	todoDefault.SelfSenseIx2 = 0;
+	todoDefault.SelfSenseIx2Adj = 0;
+	todoDefault.SelfSenseIxTotal = 1;
+	todoDefault.SelfSenseIxTotalAdj = 0;
+	todoDefault.SelfSenseCx1 = 0;
+	todoDefault.SelfSenseCx2 = 0;
+	todoDefault.SelfSenseCx2Adj = 0;
+	todoDefault.SelfSenseCxTotal = 0;
+	todoDefault.SelfSenseCxTotalAdj = 0;
+
+	if (numberParam < 1) {
+		logError(1, "%s NO COMMAND SPECIFIED!!! ", tag);
+		logError(1, "do: 'echo [cmd_code] [args] > stm_fts_cmd' ");
+		logError(1, "before looking for result!\n");
+		res = ERROR_OP_NOT_ALLOW;
+		goto END;
+	}
+
+	res = fts_disableInterrupt();
+	if (res < 0) {
+		logError(0, "%s %s: ERROR %08X\n", tag, __func__, res);
+		res = (res | ERROR_DISABLE_INTER);
+		goto END;
+	}
+	switch (functionToTest[0]) {
+	case CMD_READ:
+		if (numberParam != 4) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/**
+		 * need to pass:cmdLength
+		 * cmd[0]cmd[1]…cmd[cmdLength-1]
+		 * byteToRead
+		 */
+		temp = (int)functionToTest[1];
+		if (numberParam == 4 + (temp - 1) && temp != 0) {
+			cmd = (u8 *)kmalloc_array(temp, sizeof(u8), GFP_KERNEL);
+			if (!cmd) {
+				res = ERROR_OP_NOT_ALLOW;
+				break;
+			}
+			for (i = 0; i < temp; i++)
+				cmd[i] = functionToTest[i + 2];
+			byteToRead = functionToTest[i + 2];
+			readData = (u8 *)kmalloc_array(byteToRead, sizeof(u8),
+					GFP_KERNEL);
+			if (!readData) {
+				kfree(cmd);
+				res = ERROR_OP_NOT_ALLOW;
+				break;
+			}
+			res = fts_readCmd(cmd, temp, readData, byteToRead);
+			size += (byteToRead * sizeof(u8)) * 2;
+			kfree(cmd);
+		} else {
+			logError(1, "%s Wrong parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+		}
+		break;
+
+	case CMD_WRITE:
+		if (numberParam != 3)  {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/**
+		 * need to pass:cmdLength
+		 * cmd[0] cmd[1]…cmd[cmdLength-1]
+		 */
+		temp = (int)functionToTest[1];
+		if (numberParam == 3 + (temp - 1) && temp != 0) {
+			cmd = (u8 *)kmalloc_array(temp, sizeof(u8), GFP_KERNEL);
+			if (!cmd) {
+				res = ERROR_OP_NOT_ALLOW;
+				break;
+			}
+			for (i = 0; i < temp; i++)
+				cmd[i] = functionToTest[i + 2];
+			res = fts_writeCmd(cmd, temp);
+			kfree(cmd);
+		} else {
+			logError(1, "%s Wrong parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+		}
+		break;
+
+	case CMD_FWWRITE:
+		if (numberParam != 3)  {
+			logError(1, "%s Wrong number  parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/**
+		 * need to pass:cmdLength
+		 * cmd[0] cmd[1]…cmd[cmdLength-1]
+		 */
+		temp = (int)functionToTest[1];
+		if (numberParam == 3 + (temp - 1) && temp != 0) {
+			cmd = (u8 *)kmalloc_array(temp, sizeof(u8), GFP_KERNEL);
+			if (!cmd) {
+				res = ERROR_OP_NOT_ALLOW;
+				break;
+			}
+			for (i = 0; i < temp; i++)
+				cmd[i] = functionToTest[i + 2];
+			res = fts_writeFwCmd(cmd, temp);
+			kfree(cmd);
+		} else {
+			logError(1, "%s Wrong parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+		}
+		break;
+
+	case CMD_READU16:
+		if (numberParam != 6) {
+			logError(1, "%s Wrong number  parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/**
+		 * need to pass: cmd addr[0]  addr[1]
+		 * byteToRead hasDummyByte
+		 */
+		byteToRead = functionToTest[4];
+		readData = kmalloc_array(byteToRead,
+			sizeof(u8), GFP_KERNEL);
+		if (!readData) {
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		res = readCmdU16((u8)functionToTest[1],
+			(u16)((((u8) functionToTest[2]
+				& 0x00FF) << 8) + ((u8) functionToTest[3]
+				& 0x00FF)),
+			readData,
+			byteToRead,
+			functionToTest[5]);
+		size += (byteToRead * sizeof(u8)) * 2;
+		break;
+
+	case CMD_READB2:
+		if (numberParam != 4) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to pass: addr[0]  addr[1] byteToRead*/
+		byteToRead = functionToTest[3];
+		readData = kmalloc_array(byteToRead,
+				sizeof(u8), GFP_KERNEL);
+		if (!readData) {
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		res = readB2((u16)(
+			(((u8)functionToTest[1] & 0x00FF) << 8)
+				+ ((u8) functionToTest[2] & 0x00FF)),
+			readData,
+			byteToRead);
+		size += (byteToRead * sizeof(u8)) * 2;
+		break;
+
+	case CMD_READB2U16:
+		if (numberParam != 4) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to pass: addr[0]  addr[1] byteToRead*/
+		byteToRead = functionToTest[3];
+		readData = (u8 *)kmalloc_array(byteToRead,
+			sizeof(u8), GFP_KERNEL);
+		if (!readData) {
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		res = readB2U16((u16)((((u8)functionToTest[1]
+			& 0x00FF) << 8) + ((u8)functionToTest[2]
+			& 0x00FF)), readData, byteToRead);
+		size += (byteToRead * sizeof(u8)) * 2;
+		break;
+
+	case CMD_POLLFOREVENT:
+		if (numberParam < 5) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+
+		/**
+		 * need to pass: eventLength event[0] event[1]
+		 * … event[eventLength-1] timeTowait
+		 */
+		temp = (int)functionToTest[1];
+		if (numberParam == 5 + (temp - 1) && temp != 0) {
+			readData = (u8 *)kmalloc_array(FIFO_EVENT_SIZE,
+					sizeof(u8), GFP_KERNEL);
+			if (!readData) {
+				res = ERROR_OP_NOT_ALLOW;
+				break;
+			}
+			res = pollForEvent((int *)&functionToTest[2],
+				temp,
+				readData,
+				((functionToTest[temp + 2] & 0x00FF) << 8)
+					+ (functionToTest[temp + 3] & 0x00FF));
+			//pollForEvent return the number of error found
+			if (res >= OK)
+				res = OK;
+			size += (FIFO_EVENT_SIZE * sizeof(u8)) * 2;
+			byteToRead = FIFO_EVENT_SIZE;
+		} else {
+			logError(1, "%s Wrong parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+		}
+		break;
+
+	case CMD_SYSTEMRESET:
+		res = fts_system_reset();
+		break;
+
+	case CMD_READCHIPINFO:
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to pass: doRequest */
+		res = readChipInfo(functionToTest[1]);
+		break;
+
+	/* TOUCH ENABLE/DISABLE */
+	case CMD_CLEANUP:
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/* need to pass: enableTouch*/
+		res = cleanUp(functionToTest[1]);
+		break;
+
+	case CMD_GETFORCELEN:
+		/*read number Tx channels */
+		temp = getForceLen();
+		if (temp < OK)
+			res = temp;
+		else {
+			size += (1 * sizeof(u8)) * 2;
+			res = OK;
+		}
+		break;
+
+	case CMD_GETSENSELEN:
+		/* read number Rx channels */
+		temp = getSenseLen();
+		if (temp < OK)
+			res = temp;
+		else {
+			size += (1 * sizeof(u8)) * 2;
+			res = OK;
+		}
+		break;
+
+	case CMD_REQFRAME:
+		/* request a frame */
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		logError(0, "%s Requesting Frame\n", tag);
+		res = requestFrame((u16)((((u8)functionToTest[1] & 0x00FF) << 8)
+			+ ((u8)functionToTest[2] & 0x00FF)));
+
+		if (res < OK) {
+			logError(0, "%s Err requesting frame ERROR:%02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s Requesting Frame Finished!\n", tag);
+		}
+		break;
+
+	case CMD_GETMSFRAME:
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of param!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		logError(0, "%s Get 1 MS Frame\n", tag);
+		flushFIFO();
+		/**
+		 * delete the events related to some
+		 * touch (allow to call this function
+		 * while touching the sreen without
+		 * having a flooding of the FIFO)
+		 */
+		res = getMSFrame2((u16)((((u8)functionToTest[1] & 0x00FF) << 8)
+				+ ((u8)functionToTest[2] & 0x00FF)), &frameMS);
+		if (res < 0) {
+			logError(0, "%s Err while taking  MS frame:%02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s:frame size is %d words\n", tag, res);
+			size = (res * sizeof(short) + 8) * 2;
+			/*set res to OK because if getMSFrame is*/
+			/*successful res = number of words read*/
+			res = OK;
+			print_frame_short("MS frame =",
+				array1dTo2d_short(frameMS.node_data,
+					frameMS.node_data_size,
+					frameMS.header.sense_node),
+				frameMS.header.force_node,
+				frameMS.header.sense_node);
+		}
+		break;
+
+	/*read self raw*/
+	case CMD_GETSSFRAME:
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+
+		logError(0, "%s Get 1 SS Frame\n", tag);
+		flushFIFO();
+		/**
+		 * delete the events related to some
+		 * touch (allow to call this function
+		 * while touching the sreen without
+		 * having a flooding of the FIFO)
+		 */
+		res = getSSFrame2((u16)((((u8)functionToTest[1] & 0x00FF) << 8)
+			+ ((u8)functionToTest[2] & 0x00FF)), &frameSS);
+
+		if (res < OK) {
+			logError(0,
+			  "%s Error while taking the SS frame... ERROR %02X\n",
+			  tag, res);
+		} else {
+			logError(0, "%s The frame size is %d words\n",
+				tag, res);
+			size = (res * sizeof(short) + 8) * 2 + 1;
+
+			/*set res to OK because if getMSFrame is*/
+			/*successful res = number of words read*/
+			res = OK;
+			print_frame_short("SS force frame =",
+				array1dTo2d_short(frameSS.force_data,
+					frameSS.header.force_node, 1),
+				frameSS.header.force_node,
+				1);
+			print_frame_short("SS sense frame =",
+				array1dTo2d_short(frameSS.sense_data,
+					frameSS.header.sense_node,
+					frameSS.header.sense_node),
+				1,
+				frameSS.header.sense_node);
+		}
+		break;
+
+	case CMD_REQCOMPDATA:
+		/*request comp data*/
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+
+		logError(0, "%s Requesting Compensation Data\n", tag);
+		res = requestCompensationData((u16)
+			((((u8)functionToTest[1] & 0x00FF) << 8)
+			+ ((u8)functionToTest[2] & 0x00FF)));
+
+		if (res < OK) {
+			logError(0,
+			  "%s Error requesting compensation data ERROR %02X\n",
+			  tag, res);
+		} else {
+			logError(0,
+				"%s Requesting Compensation Data Finished!\n",
+				tag);
+		}
+		break;
+
+	case CMD_READCOMPDATAHEAD:
+		/*read comp data header*/
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+
+		logError(0, "%s Requesting Compensation Data\n", tag);
+		res = requestCompensationData(
+			(u16) ((((u8)functionToTest[1] & 0x00FF) << 8)
+				+ ((u8)functionToTest[2] & 0x00FF)));
+		if (res < OK) {
+			logError(0, "%s Error requesting:%02X\n", tag, res);
+		} else {
+			logError(0,
+			  "%s Requesting Compensation Data Finished!\n", tag);
+			res = readCompensationDataHeader(
+				(u16)((((u8)functionToTest[1] & 0x00FF) << 8)
+					+((u8)functionToTest[2] & 0x00FF)),
+				&dataHead,
+				&address);
+			if (res < OK) {
+				logError(0, "%s Read Header ERROR:%02X\n",
+					tag, res);
+			} else {
+				logError(0, "%s Read Header OK!\n", tag);
+				size += (2 * sizeof(u8)) * 2;
+			}
+		}
+		break;
+	case CMD_READMSCOMPDATA:
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*read mutual comp data */
+		logError(0, "%s Get MS Compensation Data\n", tag);
+		res = readMutualSenseCompensationData(
+			(u16)((((u8)functionToTest[1] & 0x00FF) << 8)
+				+ ((u8)functionToTest[2] & 0x00FF)),
+			&compData);
+
+		if (res < OK) {
+			logError(0, "%s Error reading MS compe data:%02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s MS Compensa Reading Finished!\n",
+				tag);
+
+			size = ((compData.node_data_size + 9) * sizeof(u8)) * 2;
+			print_frame_u8("MS Data (Cx2) = ",
+				array1dTo2d_u8(compData.node_data,
+					compData.node_data_size,
+					compData.header.sense_node),
+				compData.header.force_node,
+				compData.header.sense_node);
+		}
+		break;
+	case CMD_READSSCOMPDATA:
+		if (numberParam != 3) {
+			logError(1, "%sWrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+
+		/*read self comp data*/
+		logError(0, "%s Get SS Compensation Data...\n", tag);
+		res = readSelfSenseCompensationData((u16)
+			((((u8)functionToTest[1] & 0x00FF) << 8)
+				+ ((u8)functionToTest[2] & 0x00FF)),
+			&comData);
+		if (res < OK) {
+			logError(0, "%s Error reading SS Compensa data %02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s SS Compensa Reading Finished!\n", tag);
+			size = comData.header.force_node
+				+ comData.header.sense_node;
+			size = (size * 2 + 12) * sizeof(u8) * 2;
+			print_frame_u8("SS Data Ix2_fm = ",
+				array1dTo2d_u8(comData.ix2_fm,
+					comData.header.force_node,
+					comData.header.force_node),
+				1,
+				comData.header.force_node);
+			print_frame_u8("SS Data Cx2_fm = ",
+				array1dTo2d_u8(comData.cx2_fm,
+					comData.header.force_node,
+					comData.header.force_node),
+				1,
+				comData.header.force_node);
+			print_frame_u8("SS Data Ix2_sn = ",
+				array1dTo2d_u8(comData.ix2_sn,
+					comData.header.sense_node,
+					comData.header.sense_node),
+				1,
+				comData.header.sense_node);
+			print_frame_u8("SS Data Cx2_sn = ",
+				array1dTo2d_u8(comData.cx2_sn,
+					comData.header.sense_node,
+					comData.header.sense_node),
+				1,
+				comData.header.sense_node);
+		}
+		break;
+
+	case CMD_READGNCOMPDATA:
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*read self comp data */
+		logError(0, "%s Get General Compensation Data...\n", tag);
+		res = readGeneralCompensationData((u16)
+			((((u8)functionToTest[1]
+			& 0x00FF) << 8) + ((u8)functionToTest[2]
+			& 0x00FF)), &gnData);
+		if (res < OK) {
+			logError(0,
+				"%s Reading General compensa data ERROR %02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s:General compensa Reading Finished!\n",
+				tag);
+			size = (14) * sizeof(u8) * 2;
+		}
+		break;
+	case CMD_GETFWVER:
+		res = getFirmwareVersion(&fw_version, &config_id);
+		if (res < OK) {
+			logError(1, "%s Reading firmware version ERROR %02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s getFirmware Version Finished!\n", tag);
+			size += (4) * sizeof(u8) * 2;
+		}
+		break;
+#ifdef FTM3_CHIP
+	case CMD_FLASHSTATUS:
+		res = flash_status();
+		/*return 0 = flash ready, 1 = flash busy, <0 error*/
+		if (res < OK) {
+			logError(1, "%s Reading flash status ERROR %02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s Flash Status: %d\n", tag, res);
+			size += (1 * sizeof(u8)) * 2;
+			/*need to store the value for further display */
+			temp = res;
+
+			/*set res =ok for returning code*/
+			res = OK;
+		}
+		break;
+#endif
+	case CMD_FLASHUNLOCK:
+		res = flash_unlock();
+		if (res < OK) {
+			logError(1, "%s:Impossible Unlock Flash ERROR %02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s Flash Unlock OK!\n", tag);
+		}
+		break;
+	case CMD_READFWFILE:
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*read fw file */
+		logError(0, "%s Reading FW File...\n", tag);
+		res = readFwFile(PATH_FILE_FW, &fw, functionToTest[1]);
+		if (res < OK) {
+			logError(0, "%s Error reading FW File:%02X\n",
+				tag, res);
+		} else {
+			logError(0, "%s Read FW File Finished!\n", tag);
+		}
+		kfree(fw.data);
+		break;
+	case CMD_FLASHPROCEDURE:
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*flashing procedure*/
+		logError(0, "%s Starting Flashing Procedure\n", tag);
+		res = flashProcedure(PATH_FILE_FW,
+				functionToTest[1], functionToTest[2]);
+		if (res < OK) {
+			logError(0, "%s During flash procedure ERROR %02X",
+				tag, res);
+		} else {
+			logError(0, "%s Flash Procedure Finished!\n", tag);
+		}
+		break;
+
+	/*ITO TEST*/
+	case CMD_ITOTEST:
+		res = production_test_ito();
+		break;
+
+	/*Initialization*/
+	case CMD_INITTEST:
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to specify if if save value on Flash*/
+		if (functionToTest[1] == 0x01)
+			res = production_test_initialization();
+		else
+			res = production_test_split_initialization(false);
+		break;
+
+	case CMD_MSRAWTEST:
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/* MS Raw DATA TEST*/
+		/* need to specify if stopOnFail */
+		res = production_test_ms_raw(LIMITS_FILE, functionToTest[1],
+			&todoDefault);
+		break;
+
+	case CMD_MSINITDATATEST:
+		/*MS CX DATA TEST*/
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to specify if stopOnFail*/
+		res = production_test_ms_cx(LIMITS_FILE, functionToTest[1],
+			&todoDefault);
+		break;
+
+	case CMD_SSRAWTEST:
+		/*SS RAW DATA TEST*/
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to specify if stopOnFail*/
+		res = production_test_ss_raw(LIMITS_FILE, functionToTest[1],
+			&todoDefault);
+		break;
+
+	case CMD_SSINITDATATEST:
+		/*SS IX CX DATA TEST*/
+		if (numberParam != 2) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to specify if stopOnFail*/
+		res = production_test_ss_ix_cx(LIMITS_FILE, functionToTest[1],
+			&todoDefault);
+		break;
+
+	case CMD_MAINTEST:
+		/*PRODUCTION TEST*/
+		if (numberParam != 3) {
+			logError(1, "%s Wrong number of parameters!\n", tag);
+			res = ERROR_OP_NOT_ALLOW;
+			break;
+		}
+		/*need to specify if stopOnFail and saveInit*/
+		res = production_test_main(LIMITS_FILE, functionToTest[1],
+			functionToTest[2], &todoDefault, INIT_FIELD);
+		break;
+
+	case CMD_POWERCYCLE:
+		res = fts_chip_powercycle(info);
+		break;
+
+	default:
+		logError(1, "%s COMMAND ID NOT VALID!!\n", tag);
+		logError(1, "%s Inset a value between 00 and 1E.\n", tag);
+		res = ERROR_OP_NOT_ALLOW;
+		break;
+	}
+
+END:
+	/**
+	 * here start the reporting phase,
+	 * assembling the data to send in the file node
+	 */
+	all_strbuff = kmalloc(size, GFP_KERNEL);
+	memset(all_strbuff, 0, size);
+
+	snprintf(buff, sizeof(buff), "%02X", 0xAA);
+	strlcat(all_strbuff, buff, size);
+
+	snprintf(buff, sizeof(buff), "%08X", res);
+	strlcat(all_strbuff, buff, size);
+
+	if (res >= OK) {
+		/*all the other cases are already*/
+		/*fine printing only the res.*/
+		switch (functionToTest[0]) {
+		case CMD_READ:
+		case CMD_READU16:
+		case CMD_READB2:
+		case CMD_READB2U16:
+		case CMD_POLLFOREVENT:
+			for (j = 0; j < byteToRead; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					readData[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+			break;
+
+		case CMD_GETFORCELEN:
+		case CMD_GETSENSELEN:
+		case CMD_FLASHSTATUS:
+			snprintf(buff, sizeof(buff), "%02X", (u8)temp);
+			strlcat(all_strbuff, buff, size);
+			break;
+
+		case CMD_GETMSFRAME:
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frameMS.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8)frameMS.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			for (j = 0; j < frameMS.node_data_size; j++) {
+				snprintf(buff, sizeof(buff), "%04X",
+					frameMS.node_data[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+			kfree(frameMS.node_data);
+			break;
+
+		case CMD_GETSSFRAME:
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8) frameSS.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8)frameSS.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			/* Copying self raw data Force */
+			for (j = 0; j < frameSS.header.force_node; j++) {
+				snprintf(buff, sizeof(buff), "%04X",
+					frameSS.force_data[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+
+			/* Copying self raw data Sense */
+			for (j = 0; j < frameSS.header.sense_node; j++) {
+				snprintf(buff, sizeof(buff), "%04X",
+					frameSS.sense_data[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+			kfree(frameSS.force_data);
+			kfree(frameSS.sense_data);
+			break;
+
+		case CMD_READMSCOMPDATA:
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8)compData.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				(u8)compData.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			/* Cpying CX1 value */
+			snprintf(buff, sizeof(buff), "%02X",
+				compData.cx1);
+			strlcat(all_strbuff, buff, size);
+
+			/* Copying CX2 values */
+			for (j = 0; j < compData.node_data_size; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					*(compData.node_data + j));
+				strlcat(all_strbuff, buff, size);
+			}
+			kfree(compData.node_data);
+			break;
+
+		case CMD_READSSCOMPDATA:
+			snprintf(buff, sizeof(buff), "%02X",
+				comData.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				comData.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.f_ix1);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.s_ix1);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.f_cx1);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X", comData.s_cx1);
+			strlcat(all_strbuff, buff, size);
+
+			/* Copying IX2 Force */
+			for (j = 0; j < comData.header.force_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.ix2_fm[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+			/* Copying IX2 Sense*/
+			for (j = 0; j < comData.header.sense_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.ix2_sn[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+			/* Copying CX2 Force */
+			for (j = 0; j < comData.header.force_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.cx2_fm[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			/* Copying CX2 Sense */
+			for (j = 0; j < comData.header.sense_node; j++) {
+				snprintf(buff, sizeof(buff), "%02X",
+					comData.cx2_sn[j]);
+				strlcat(all_strbuff, buff, size);
+			}
+
+			kfree(comData.ix2_fm);
+			kfree(comData.ix2_sn);
+			kfree(comData.cx2_fm);
+			kfree(comData.cx2_sn);
+			break;
+
+		case CMD_READGNCOMPDATA:
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.header.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.header.sense_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.ftsd_lp_timer_cal0);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.ftsd_lp_timer_cal1);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.ftsd_lp_timer_cal2);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.ftsd_lp_timer_cal3);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.ftsa_lp_timer_cal0);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				gnData.ftsa_lp_timer_cal1);
+			strlcat(all_strbuff, buff, size);
+			break;
+
+		case CMD_GETFWVER:
+			snprintf(buff, sizeof(buff), "%04X", fw_version);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%04X", config_id);
+			strlcat(all_strbuff, buff, size);
+			break;
+
+		case CMD_READCOMPDATAHEAD:
+			snprintf(buff, sizeof(buff), "%02X",
+				dataHead.force_node);
+			strlcat(all_strbuff, buff, size);
+
+			snprintf(buff, sizeof(buff), "%02X",
+				dataHead.sense_node);
+			strlcat(all_strbuff, buff, size);
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	snprintf(buff, sizeof(buff), "%02X", 0xBB);
+	strlcat(all_strbuff, buff, size);
+
+	count = snprintf(buf, TSP_BUF_SIZE, "%s\n", all_strbuff);
+	numberParam = 0;
+	/**
+	 * need to reset the number of parameters
+	 * in order to wait the next command,
+	 * comment if you want to repeat
+	 * the last command sent just doing a cat
+	 */
+
+	kfree(readData);
+	kfree(all_strbuff);
+	return count;
+}
+
+static DEVICE_ATTR_RW(stm_driver_test);
+
+static struct attribute *test_cmd_attributes[] = {
+	&dev_attr_stm_driver_test.attr,
+	NULL,
+};
+
+struct attribute_group test_cmd_attr_group = {
+	.attrs = test_cmd_attributes,
+};
+#endif

st/fts_gui.c

@@ -0,0 +1,412 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/completion.h>
+//#include <linux/wakelock.h>
+#include <linux/pm_wakeup.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/consumer.h>
+#include "fts.h"
+#include "fts_lib/ftsIO.h"
+
+#ifdef SCRIPTLESS
+
+static unsigned int fts_data[CMD_RESULT_STR_LEN] = {0};
+static unsigned char fts_pAddress_i2c[CMD_RESULT_STR_LEN] = {0};
+static int byte_count_read;
+static char Out_buff[TSP_BUF_SIZE];
+
+
+/*I2C CMd functions: functions to interface with GUI without script */
+
+ssize_t fts_i2c_wr_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	int i;
+	char buff[16];
+
+	memset(Out_buff, 0x00, sizeof(Out_buff));
+	if (byte_count_read == 0) {
+		snprintf(Out_buff, sizeof(Out_buff), "{FAILED}\n");
+		return snprintf(buf, TSP_BUF_SIZE, "%s\n", Out_buff);
+	}
+#ifdef SCRIPTLESS_DEBUG
+	pr_err("%s:DATA READ {\n", __func__);
+	for (i = 0; i < byte_count_read; i++) {
+		pr_err(" %02X\n", (unsigned int)info->cmd_wr_result[i]);
+		if (i < (byte_count_read - 1))
+			pr_err("\n");
+	}
+	pr_err("}\n");
+#endif
+	snprintf(buff, sizeof(buff), "{");
+	strlcat(Out_buff, buff, sizeof(Out_buff));
+	for (i = 0; i < (byte_count_read + 2); i++) {
+		char temp_byte_count_read;
+
+		if (i == 0) {
+			temp_byte_count_read = (byte_count_read >> 8) & 0xFF;
+
+			snprintf(buff, sizeof(buff), "%02X",
+				temp_byte_count_read);
+		} else if (i == 1) {
+			temp_byte_count_read = (byte_count_read) & 0xFF;
+
+			snprintf(buff, sizeof(buff), "%02X",
+				temp_byte_count_read);
+
+		} else {
+			snprintf(buff, sizeof(buff), "%02X",
+				info->cmd_wr_result[i-2]);
+		}
+		//snprintf(buff, sizeof(buff), "%02X", info->cmd_wr_result[i]);
+		strlcat(Out_buff, buff,  sizeof(Out_buff));
+		if (i < (byte_count_read + 1)) {
+			snprintf(buff, sizeof(buff), " ");
+			strlcat(Out_buff, buff, sizeof(Out_buff));
+		}
+	}
+	snprintf(buff, sizeof(buff), "}");
+	strlcat(Out_buff, buff, sizeof(Out_buff));
+	return snprintf(buf, TSP_BUF_SIZE, "%s\n", Out_buff);
+}
+
+ssize_t fts_i2c_wr_store(struct device *dev, struct device_attribute *attr,
+	const char *buf, size_t count)
+{
+	int ret;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	unsigned char pAddress[9];
+	unsigned int byte_count = 0;
+	int i;
+	unsigned int data[9];
+
+	memset(data, 0x00, sizeof(data));
+	memset(pAddress, 0x00, sizeof(pAddress));
+	memset(info->cmd_wr_result, 0x00, CMD_RESULT_STR_LEN);
+	ret = sscanf(buf, "%x %x %x %x %x %x %x %x %x ",
+		(data + 8), (data), (data + 1), (data + 2), (data + 3),
+		(data + 4), (data + 5), (data + 6), (data + 7));
+
+	byte_count = data[8];
+
+	/**
+	 * if(sizeof(buf) != byte_count )
+	 * {
+	 * printk("%s : Byte count is wrong\n",__func__);
+	 * return count;
+	 * }
+	 */
+
+	if (byte_count > sizeof(pAddress))
+		return -EINVAL;
+#ifdef SCRIPTLESS_DEBUG
+
+	pr_err("%s: Input Data 1:\n", __func__);
+
+	for (i = 0 ; i < byte_count; i++) {
+		pr_err(" %02X\n", data[i]);
+		pAddress[i] = (unsigned char)data[i];
+	}
+	pr_err("\n");
+#else
+	for (i = 0 ; i < byte_count; i++)
+		pAddress[i] = (unsigned char)data[i];
+#endif
+	byte_count_read = (((unsigned int)data[byte_count - 2]) << 8)
+				| data[byte_count - 1];
+	ret = fts_writeCmd(pAddress, 3);
+	msleep(20);
+	ret = fts_readCmd(&pAddress[3], (byte_count - 5),
+			info->cmd_wr_result, byte_count_read);
+#ifdef SCRIPTLESS_DEBUG
+	 pr_err("%s:DATA READ {\n", __func__);
+	for (i = 0; i < (2 + byte_count_read); i++) {
+		char temp_byte_count_read;
+
+		if (i == 0) {
+			temp_byte_count_read = (byte_count_read >> 8) & 0xFF;
+			pr_err("%02X\n", (unsigned int)temp_byte_count_read);
+		} else if (i == 1) {
+			temp_byte_count_read = (byte_count_read) & 0xFF;
+			pr_err("%02X\n", (unsigned int)temp_byte_count_read);
+
+		} else {
+			pr_err("%02X\n",
+				(unsigned int)info->cmd_read_result[i - 2]);
+		}
+
+		if (i < (byte_count_read + 1))
+			pr_err("\n");
+
+	}
+	pr_err("}\n");
+#endif
+	if (ret)
+		dev_err(dev, "Unable to read register\n");
+	return count;
+}
+
+ssize_t fts_i2c_read_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	int i;
+	char buff[16];
+
+	memset(Out_buff, 0x00, sizeof(Out_buff));
+	if (byte_count_read == 0) {
+		snprintf(Out_buff, sizeof(Out_buff), "{FAILED}");
+		return snprintf(buf, TSP_BUF_SIZE, "%s\n", Out_buff);
+	}
+#ifdef SCRIPTLESS_DEBUG
+	pr_err("%s:DATA READ {\n", __func__);
+	for (i = 0; i < byte_count_read; i++) {
+		pr_err("%02X\n", (unsigned int)info->cmd_read_result[i]);
+		if (i < (byte_count_read - 1))
+			pr_err("\n");
+	}
+	pr_err("}\n");
+#endif
+	snprintf(buff, sizeof(buff), "{");
+	strlcat(Out_buff, buff, sizeof(Out_buff));
+	for (i = 0; i < (byte_count_read + 2); i++) {
+		char temp_byte_count_read;
+
+		if (i == 0) {
+			temp_byte_count_read = (byte_count_read >> 8) & 0xFF;
+			snprintf(buff, sizeof(buff), "%02X",
+				temp_byte_count_read);
+		} else if (i == 1) {
+			temp_byte_count_read = (byte_count_read) & 0xFF;
+			snprintf(buff, sizeof(buff), "%02X",
+				temp_byte_count_read);
+		} else {
+			snprintf(buff, sizeof(buff), "%02X",
+				info->cmd_read_result[i - 2]);
+		}
+		strlcat(Out_buff, buff, sizeof(Out_buff));
+		if (i < (byte_count_read + 1)) {
+			snprintf(buff, sizeof(buff), " ");
+			strlcat(Out_buff, buff, sizeof(Out_buff));
+		}
+	}
+	snprintf(buff, sizeof(buff), "}");
+	strlcat(Out_buff, buff, sizeof(Out_buff));
+
+	return snprintf(buf, TSP_BUF_SIZE, "%s\n", Out_buff);
+}
+
+ssize_t fts_i2c_read_store(struct device *dev, struct device_attribute *attr,
+	const char *buf, size_t count)
+{
+	int ret;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	unsigned char pAddress[9];
+	unsigned int byte_count = 0;
+	int i;
+	unsigned int data[9];
+
+	byte_count_read = 0;
+	memset(data, 0x00, sizeof(data));
+	memset(pAddress, 0x00, sizeof(pAddress));
+	memset(info->cmd_read_result, 0x00, CMD_RESULT_STR_LEN);
+	ret = sscanf(buf, "%x %x %x %x %x %x %x %x %x ",
+		(data + 8), (data), (data + 1), (data + 2), (data + 3),
+		(data + 4), (data + 5), (data + 6), (data + 7));
+
+	byte_count = data[8];
+
+	if (byte_count > 8) {
+#ifdef SCRIPTLESS_DEBUG
+		pr_err("%s:Byte count is more than 8\n", __func__);
+#endif
+		return count;
+	}
+	/*if(sizeof(buf) != byte_count )*/
+	/*{*/
+	/*	printk("%s : Byte count is wrong\n",__func__);*/
+	/*	return count;*/
+	/*}*/
+#ifdef SCRIPTLESS_DEBUG
+	pr_err("%s: Input Data 1:\n", __func__);
+	for (i = 0 ; i < byte_count; i++) {
+		pr_err("%02X\n", data[i]);
+		pAddress[i] = (unsigned char)data[i];
+	}
+	pr_err("\n");
+#else
+	for (i = 0 ; i < byte_count; i++)
+		pAddress[i] = (unsigned char)data[i];
+#endif
+	byte_count_read = (((unsigned int)data[byte_count - 2]) << 8)
+				| data[byte_count - 1];
+	ret = fts_readCmd(pAddress, (byte_count - 2), info->cmd_read_result,
+		byte_count_read);
+#ifdef SCRIPTLESS_DEBUG
+	pr_err("%s:DATA READ\n{\n", __func__);
+	for (i = 0; i < (byte_count_read + 2); i++) {
+		char temp_byte_count_read;
+
+		if (i == 0) {
+			temp_byte_count_read = (byte_count_read >> 8) & 0xFF;
+
+			pr_err("%02X\n", (unsigned int)temp_byte_count_read);
+		} else if (i == 1) {
+			temp_byte_count_read = (byte_count_read) & 0xFF;
+
+			pr_err("%02X\n", (unsigned int)temp_byte_count_read);
+
+		} else {
+			pr_err("%02X\n",
+				(unsigned int)info->cmd_read_result[i - 2]);
+		}
+		if (i < (byte_count_read + 1))
+			pr_err("\n");
+	}
+	pr_err("}\n");
+#endif
+	if (ret)
+		dev_err(dev, "Unable to read register\n");
+	return count;
+}
+
+
+ssize_t fts_i2c_write_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+
+	return snprintf(buf, TSP_BUF_SIZE, "%s", info->cmd_write_result);
+
+}
+
+ssize_t fts_i2c_write_store(struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	int ret;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	unsigned int byte_count = 0;
+	int i;
+	unsigned int *data = &fts_data[0];
+
+	memset(fts_data, 0x00, sizeof(fts_data));
+	memset(fts_pAddress_i2c, 0x00, sizeof(fts_pAddress_i2c));
+	memset(info->cmd_write_result, 0x00, sizeof(info->cmd_write_result));
+	ret = sscanf(buf, "%x %x", data, (data + 1));
+	if (ret != 2)
+		return -EINVAL;
+	byte_count = data[0] << 8 | data[1];
+
+	if (byte_count <= sizeof(fts_pAddress_i2c)) {
+		for (i = 0; i < (byte_count); i++) {
+			ret = sscanf(&buf[3 * (i + 2)], "%x ", (data + i));
+			if (ret != 1)
+				return -EINVAL;
+		}
+	} else {
+#ifdef SCRIPTLESS_DEBUG
+		pr_err("%s:message size is > allowed limit of 512 bytes\n",
+			__func__);
+#endif
+		snprintf(info->cmd_write_result, sizeof(info->cmd_write_result),
+			"{Write NOT OK}\n");
+		return -EINVAL;
+	}
+#ifdef SCRIPTLESS_DEBUG
+	pr_err("\n");
+	pr_err("%s:Byte_count = %02d|Count = %02d |size of buf:%02d\n",
+		__func__, byte_count, (int)count, (int)sizeof(buf));
+	pr_err("%s: Input Data 1:\n", __func__);
+	for (i = 0 ; i < byte_count; i++) {
+		pr_err(" %02X\n", data[i]);
+		fts_pAddress_i2c[i] = (unsigned char)data[i];
+	}
+	pr_err("\n");
+#else
+	for (i = 0 ; i < byte_count; i++)
+		fts_pAddress_i2c[i] = (unsigned char)data[i];
+#endif
+	if ((fts_pAddress_i2c[0] == 0xb3) && (fts_pAddress_i2c[3] == 0xb1)) {
+		ret = fts_writeCmd(fts_pAddress_i2c, 3);
+		msleep(20);
+		ret = fts_writeCmd(&fts_pAddress_i2c[3], byte_count-3);
+	} else
+		ret = fts_writeCmd(fts_pAddress_i2c, byte_count);
+
+#ifdef SCRIPTLESS_DEBUG
+	pr_err("%s:DATA :\n", __func__);
+	for (i = 0; i < byte_count; i++)
+		pr_err(" %02X\n", (unsigned int)fts_pAddress_i2c[i]);
+	pr_err(" byte_count: %02X\n", byte_count);
+#endif
+	if (ret < 0) {
+		dev_err(dev, "{Write NOT OK}\n");
+		snprintf(info->cmd_write_result, sizeof(info->cmd_write_result),
+			"{Write NOT OK}\n");
+	} else {
+		snprintf(info->cmd_write_result, sizeof(info->cmd_write_result),
+			"{Write OK}\n");
+#ifdef SCRIPTLESS_DEBUG
+		pr_err("%s : {Write OK}\n", __func__);
+#endif
+	}
+	return count;
+}
+
+static DEVICE_ATTR_RW(fts_i2c_read);
+static DEVICE_ATTR_RW(fts_i2c_wr);
+static DEVICE_ATTR_RW(fts_i2c_write);
+
+static struct attribute *i2c_cmd_attributes[] = {
+	&dev_attr_fts_i2c_read.attr,
+	&dev_attr_fts_i2c_wr.attr,
+	&dev_attr_fts_i2c_write.attr,
+	NULL,
+};
+
+struct attribute_group i2c_cmd_attr_group = {
+	.attrs = i2c_cmd_attributes,
+};
+
+ #endif

st/fts_lib/ftsCompensation.c

@@ -0,0 +1,744 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *               FTS functions for getting Initialization Data            *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <stdarg.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/time.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+//#include <linux/sec_sysfs.h>
+
+#include "ftsCrossCompile.h"
+#include "ftsCompensation.h"
+#include "ftsError.h"
+#include "ftsFrame.h"
+#include "ftsHardware.h"
+#include "ftsIO.h"
+#include "ftsSoftware.h"
+#include "ftsTool.h"
+
+
+static char tag[8] = "[ FTS ]\0";
+struct chipInfo ftsInfo;
+
+int requestCompensationData(u16 type)
+{
+	int retry = 0;
+	int ret;
+	char *temp = NULL;
+	u16 answer;
+
+	int event_to_search[3];
+	u8 readEvent[FIFO_EVENT_SIZE];
+
+	u8 cmd[3] = { FTS_CMD_REQU_COMP_DATA, 0x00, 0x00};
+	/* B8 is the command for asking compensation data*/
+	u16ToU8(type, &cmd[1]);
+
+	event_to_search[0] = (int)EVENTID_COMP_DATA_READ;
+	event_to_search[1] = cmd[1];
+	event_to_search[2] = cmd[2];
+
+	while (retry < COMP_DATA_READ_RETRY) {
+		temp =  printHex("Command = ", cmd, 3);
+		if (temp != NULL)
+			logError(0, "%s %s", tag, temp);
+		kfree(temp);
+		ret = fts_writeFwCmd(cmd, 3);
+		/*send the request to the chip to load*/
+		/*in memory the Compensation Data*/
+		if (ret < OK) {
+			logError(1, "%s %s:ERROR %02X\n",
+				tag, __func__, ERROR_I2C_W);
+			return ERROR_I2C_W;
+		}
+		ret = pollForEvent(event_to_search, 3, readEvent,
+				TIMEOUT_REQU_COMP_DATA);
+		if (ret < OK) {
+			logError(0, "%s Event did not Found at %d attemp!\n",
+				tag, retry + 1);
+			retry += 1;
+		} else {
+			retry = 0;
+			break;
+		}
+	}
+
+	if (retry == COMP_DATA_READ_RETRY) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_TIMEOUT);
+		return ERROR_TIMEOUT;
+	}
+
+	u8ToU16_le(&readEvent[1], &answer);
+
+	if (answer == type)
+		return OK;
+
+	logError(1, "%sThe event found has a different type of ", tag);
+	logError(1, "Compensation data %02X\n", ERROR_DIFF_COMP_TYPE);
+	return ERROR_DIFF_COMP_TYPE;
+
+}
+
+int readCompensationDataHeader(u16 type, struct DataHeader *header,
+	u16 *address)
+{
+	u16 offset = ADDR_FRAMEBUFFER_DATA;
+	u16 answer;
+	u8 data[COMP_DATA_HEADER];
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, offset, data, COMP_DATA_HEADER,
+		DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+	logError(0, "%s Read Data Header done!\n", tag);
+
+	if (data[0] != HEADER_SIGNATURE) {
+		logError(1, "%s %s:%02X The Header Signature was wrong!",
+			tag, __func__, ERROR_WRONG_COMP_SIGN);
+		logError(1, "%02X != %02X\n", data[0], HEADER_SIGNATURE);
+
+		return ERROR_WRONG_COMP_SIGN;
+	}
+
+
+	u8ToU16_le(&data[1], &answer);
+
+	if (answer != type) {
+		logError(1, "%s %s:ERROR %02X\n",
+			tag, __func__, ERROR_DIFF_COMP_TYPE);
+
+		return ERROR_DIFF_COMP_TYPE;
+	}
+
+	logError(0, "%s Type of Compensation data OK!\n", tag);
+
+	header->type = type;
+	header->force_node = (int)data[4];
+	header->sense_node = (int)data[5];
+
+	*address = offset + COMP_DATA_HEADER;
+
+	return OK;
+
+}
+
+int readMutualSenseGlobalData(u16 *address, struct MutualSenseData *global)
+{
+
+	u8 data[COMP_DATA_GLOBAL];
+
+	logError(0, "%s Address for Global data= %02X\n", tag, *address);
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, *address, data,
+		COMP_DATA_GLOBAL, DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+
+		return ERROR_I2C_R;
+	}
+	logError(0, "%s Global data Read!\n", tag);
+
+	global->tuning_ver = data[0];
+	global->cx1 = data[1];
+
+	logError(0, "%s tuning_ver = %d  CX1 = %d\n",
+		tag, global->tuning_ver, global->cx1);
+
+	*address += COMP_DATA_GLOBAL;
+	return OK;
+}
+
+
+
+int readMutualSenseNodeData(u16 address, struct MutualSenseData *node)
+{
+	int size = node->header.force_node*node->header.sense_node;
+
+	logError(0, "%s Address for Node data = %02X\n", tag, address);
+
+	node->node_data = (u8 *)kmalloc_array(size, (sizeof(u8)), GFP_KERNEL);
+
+	if (node->node_data == NULL) {
+		logError(1, "%s %s: ERROR %02X", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	logError(0, "%s Node Data to read %d bytes\n", tag, size);
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, address, node->node_data,
+		size, DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %s:ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+		kfree(node->node_data);
+		return ERROR_I2C_R;
+	}
+	node->node_data_size = size;
+
+	logError(0, "%s Read node data ok!\n", tag);
+
+	return size;
+}
+
+
+int readMutualSenseCompensationData(u16 type, struct MutualSenseData *data)
+{
+	int ret;
+	u16 address;
+
+	data->node_data = NULL;
+
+	if (!(type == MS_TOUCH_ACTIVE || type == MS_TOUCH_LOW_POWER
+		|| type == MS_TOUCH_ULTRA_LOW_POWER || type == MS_KEY)) {
+		logError(1, "%s %s: Choose a MS type of compensation data ",
+			tag, __func__);
+		logError(1, "ERROR %02X\n", ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	ret = requestCompensationData(type);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_REQU_COMP_DATA);
+		return (ret|ERROR_REQU_COMP_DATA);
+	}
+
+	ret = readCompensationDataHeader(type, &(data->header), &address);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_HEADER);
+		return (ret | ERROR_COMP_DATA_HEADER);
+	}
+
+	ret = readMutualSenseGlobalData(&address, data);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_GLOBAL);
+		return (ret|ERROR_COMP_DATA_GLOBAL);
+	}
+
+	ret = readMutualSenseNodeData(address, data);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+		tag, __func__, ERROR_COMP_DATA_NODE);
+		return (ret | ERROR_COMP_DATA_NODE);
+	}
+
+	return OK;
+}
+
+
+int readSelfSenseGlobalData(u16 *address, struct SelfSenseData *global)
+{
+	u8 data[COMP_DATA_GLOBAL];
+
+	logError(0, "%s Address for Global data= %02X\n", tag, *address);
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, *address, data,
+		COMP_DATA_GLOBAL, DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+		tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+
+	logError(0, "%s Global data Read!\n", tag);
+
+	global->tuning_ver = data[0];
+	global->f_ix1 = data[1];
+	global->s_ix1 = data[2];
+	global->f_cx1 = data[3];
+	global->s_cx1 = data[4];
+	global->f_max_n = data[5];
+	global->s_max_n = data[6];
+
+	logError(0,
+		"%stuning_ver = %df_ix1 = %ds_ix1 = %df_cx1 = %d s_cx1 = %d\n",
+		tag, global->tuning_ver, global->f_ix1,
+		global->s_ix1, global->f_cx1, global->s_cx1);
+	logError(0, "%s max_n = %d   s_max_n = %d\n",
+		tag, global->f_max_n, global->s_max_n);
+
+	*address += COMP_DATA_GLOBAL;
+	return OK;
+}
+
+int readSelfSenseNodeData(u16 address, struct SelfSenseData *node)
+{
+	int size = node->header.force_node * 2 + node->header.sense_node * 2;
+	u8 *data;
+
+	node->ix2_fm = (u8 *)kmalloc_array(node->header.force_node,
+				sizeof(u8), GFP_KERNEL);
+	if (node->ix2_fm == NULL) {
+		logError(1, "%s %s: ERROR %02X", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	node->cx2_fm = (u8 *)kmalloc_array(node->header.force_node,
+				sizeof(u8), GFP_KERNEL);
+	if (node->cx2_fm == NULL) {
+		logError(1, "%s %s: ERROR %02X", tag, __func__, ERROR_ALLOC);
+		kfree(node->ix2_fm);
+		return ERROR_ALLOC;
+	}
+	node->ix2_sn = (u8 *)kmalloc_array(node->header.sense_node,
+				sizeof(u8), GFP_KERNEL);
+	if (node->ix2_sn == NULL) {
+		logError(1, "%s %s: ERROR %02X", tag, __func__, ERROR_ALLOC);
+		kfree(node->ix2_fm);
+		kfree(node->cx2_fm);
+		return ERROR_ALLOC;
+	}
+	node->cx2_sn = (u8 *)kmalloc_array(node->header.sense_node,
+				sizeof(u8), GFP_KERNEL);
+	if (node->cx2_sn == NULL) {
+		logError(1, "%s %s: ERROR %02X", tag, __func__, ERROR_ALLOC);
+		kfree(node->ix2_fm);
+		kfree(node->cx2_fm);
+		kfree(node->ix2_sn);
+		return ERROR_ALLOC;
+	}
+
+	logError(0, "%s Address for Node data = %02X\n", tag, address);
+
+	logError(0, "%s Node Data to read %d bytes\n", tag, size);
+
+	data = (u8 *)kmalloc_array(size, sizeof(u8), GFP_KERNEL);
+	if (data == NULL) {
+		logError(1, "%s %s: ERROR %02X", tag, __func__, ERROR_ALLOC);
+		kfree(node->ix2_fm);
+		kfree(node->cx2_fm);
+		kfree(node->ix2_sn);
+		kfree(node->cx2_sn);
+		return ERROR_ALLOC;
+	}
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, address, data, size,
+		DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+		kfree(node->ix2_fm);
+		kfree(node->cx2_fm);
+		kfree(node->ix2_sn);
+		kfree(node->cx2_sn);
+		kfree(data);
+		return ERROR_I2C_R;
+	}
+
+	logError(0, "%s Read node data ok!\n", tag);
+
+	memcpy(node->ix2_fm, data, node->header.force_node);
+	memcpy(node->ix2_sn, &data[node->header.force_node],
+		node->header.sense_node);
+	memcpy(node->cx2_fm,
+		&data[node->header.force_node + node->header.sense_node],
+		node->header.force_node);
+	memcpy(node->cx2_sn,
+		&data[node->header.force_node * 2 + node->header.sense_node],
+		node->header.sense_node);
+
+	kfree(data);
+
+	return OK;
+}
+
+int readSelfSenseCompensationData(u16 type, struct SelfSenseData *data)
+{
+
+	int ret;
+	u16 address;
+
+	data->ix2_fm = NULL;
+	data->cx2_fm = NULL;
+	data->ix2_sn = NULL;
+	data->cx2_sn = NULL;
+
+	if (!(type == SS_TOUCH || type == SS_KEY || type == SS_HOVER
+		|| type == SS_PROXIMITY)) {
+		logError(1, "%s %s:Choose a SS type of compensation data ",
+			tag, __func__);
+		logError(1, "ERROR %02X\n", ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	ret = requestCompensationData(type);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_REQU_COMP_DATA);
+		return (ret | ERROR_REQU_COMP_DATA);
+	}
+
+	ret = readCompensationDataHeader(type, &(data->header), &address);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_HEADER);
+		return (ret|ERROR_COMP_DATA_HEADER);
+	}
+
+	ret = readSelfSenseGlobalData(&address, data);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_GLOBAL);
+		return (ret | ERROR_COMP_DATA_GLOBAL);
+	}
+
+	ret = readSelfSenseNodeData(address, data);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_NODE);
+		return (ret | ERROR_COMP_DATA_NODE);
+	}
+
+	return OK;
+}
+
+
+int readGeneralGlobalData(u16 address, struct GeneralData *global)
+{
+	u8 data[COMP_DATA_GLOBAL];
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, address, data, COMP_DATA_GLOBAL,
+		DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+
+	global->ftsd_lp_timer_cal0 = data[0];
+	global->ftsd_lp_timer_cal1 = data[1];
+	global->ftsd_lp_timer_cal2 = data[2];
+	global->ftsd_lp_timer_cal3 = data[3];
+	global->ftsa_lp_timer_cal0 = data[4];
+	global->ftsa_lp_timer_cal1 = data[5];
+
+	return OK;
+}
+
+
+int readGeneralCompensationData(u16 type, struct GeneralData *data)
+{
+	int ret;
+	u16 address;
+
+	if (!(type == GENERAL_TUNING)) {
+		logError(1, "%s %s:Choose a GENERAL type of compensation data ",
+			tag);
+		logError(1, "ERROR %02X\n", ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	ret = requestCompensationData(type);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_REQU_COMP_DATA);
+		return ERROR_REQU_COMP_DATA;
+	}
+
+	ret = readCompensationDataHeader(type, &(data->header), &address);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_HEADER);
+		return ERROR_COMP_DATA_HEADER;
+	}
+
+	ret = readGeneralGlobalData(address, data);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_GLOBAL);
+		return ERROR_COMP_DATA_GLOBAL;
+	}
+
+	return OK;
+
+}
+
+
+int defaultChipInfo(int i2cError)
+{
+	int i;
+
+	logError(0, "%s Setting default Chip Info...\n", tag);
+	ftsInfo.u32_echoEn = 0x00000000;
+	ftsInfo.u8_msScrConfigTuneVer = 0;
+	ftsInfo.u8_ssTchConfigTuneVer = 0;
+	ftsInfo.u8_msScrCxmemTuneVer = 0;
+	ftsInfo.u8_ssTchCxmemTuneVer = 0;
+	if (i2cError == 1) {
+		ftsInfo.u16_fwVer = 0xFFFF;
+		ftsInfo.u16_cfgId = 0xFFFF;
+		for (i = 0; i < EXTERNAL_RELEASE_INFO_SIZE; i++)
+			ftsInfo.u8_extReleaseInfo[i] = 0xFF;
+	} else {
+		ftsInfo.u16_fwVer = 0x0000;
+		ftsInfo.u16_cfgId = 0x0000;
+		for (i = 0; i < EXTERNAL_RELEASE_INFO_SIZE; i++)
+			ftsInfo.u8_extReleaseInfo[i] = 0x00;
+	}
+	ftsInfo.u32_mpPassFlag = INIT_FIELD;
+	ftsInfo.u16_errOffset = INVALID_ERROR_OFFS;
+	logError(0, "%s default Chip Info DONE!\n", tag);
+	return OK;
+}
+
+int readChipInfo(int doRequest)
+{
+	int ret, i;
+	u16 answer;
+	u8 data[CHIP_INFO_SIZE + 3];
+	/*+3 because need to read all the field of*/
+	/*the struct plus the signature and 2 address bytes*/
+	int index = 0;
+
+	logError(0, "%s Starting Read Chip Info...\n", tag);
+	if (doRequest == 1) {
+		ret = requestCompensationData(CHIP_INFO);
+		if (ret < 0) {
+			logError(1, "%s %s: ERROR %02X\n",
+				tag, __func__, ERROR_REQU_COMP_DATA);
+			ret = (ret | ERROR_REQU_COMP_DATA);
+			goto FAIL;
+		}
+	}
+
+	logError(0, "%s Byte to read = %d bytes\n", tag, CHIP_INFO_SIZE + 3);
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, ADDR_FRAMEBUFFER_DATA, data,
+		CHIP_INFO_SIZE + 3, DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+		ret = ERROR_I2C_R;
+		goto FAIL;
+	}
+
+	logError(0, "%s Read data ok!\n", tag);
+
+	logError(0, "%s Starting parsing of data...\n", tag);
+
+	if (data[0] != HEADER_SIGNATURE) {
+		logError(1, "%s %s:ERROR ", tag, __func__);
+		logError(1, "%02X The Header Signature is wrong!%02X != %02X\n",
+			ERROR_WRONG_COMP_SIGN, data[0], HEADER_SIGNATURE);
+		ret = ERROR_WRONG_COMP_SIGN;
+		goto FAIL;
+	}
+
+	u8ToU16_le(&data[1], &answer);
+
+	if (answer != CHIP_INFO) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_DIFF_COMP_TYPE);
+		ret = ERROR_DIFF_COMP_TYPE;
+		goto FAIL;
+	}
+
+	index += 3;
+	ftsInfo.u8_loadCnt = data[index++];
+	ftsInfo.u8_infoVer = data[index++];
+	u8ToU16(&data[index], &ftsInfo.u16_ftsdId);
+	index += 2;
+	ftsInfo.u8_ftsdVer = data[index++];
+	ftsInfo.u8_ftsaId = data[index++];
+	ftsInfo.u8_ftsaVer = data[index++];
+	ftsInfo.u8_tchRptVer = data[index++];
+
+	logError(0, "%s External Release =  ", tag);
+	for (i = 0; i < EXTERNAL_RELEASE_INFO_SIZE; i++) {
+		ftsInfo.u8_extReleaseInfo[i] = data[index++];
+		logError(0, "%02X ", ftsInfo.u8_extReleaseInfo[i]);
+	}
+	logError(0, "\n");
+
+	for (i = 0; i < sizeof(ftsInfo.u8_custInfo); i++)
+		ftsInfo.u8_custInfo[i] = data[index++];
+
+	u8ToU16(&data[index], &ftsInfo.u16_fwVer);
+	index += 2;
+	logError(1, "%s FW VERSION = %04X\n", tag, ftsInfo.u16_fwVer);
+
+	u8ToU16(&data[index], &ftsInfo.u16_cfgId);
+	index += 2;
+	logError(1, "%s CONFIG ID = %04X\n", tag, ftsInfo.u16_cfgId);
+
+	ftsInfo.u32_projId = ((data[index + 3] & 0x000000FF) << 24) +
+				((data[index + 2] & 0x000000FF) << 16) +
+				((data[index + 1] & 0x000000FF) << 8) +
+				(data[index] & 0x000000FF);
+	index += 4;
+
+	u8ToU16(&data[index], &ftsInfo.u16_scrXRes);
+	index += 2;
+
+	u8ToU16(&data[index], &ftsInfo.u16_scrYRes);
+	index += 2;
+
+	ftsInfo.u8_scrForceLen = data[index++];
+	logError(0, "%s Force Len = %d\n", tag, ftsInfo.u8_scrForceLen);
+
+	ftsInfo.u8_scrSenseLen = data[index++];
+	logError(0, "%s Sense Len = %d\n", tag, ftsInfo.u8_scrSenseLen);
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_scrForceEn[i] = data[index++];
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_scrSenseEn[i] = data[index++];
+
+	ftsInfo.u8_msKeyLen = data[index++];
+	logError(0, "%s MS Key Len = %d\n", tag, ftsInfo.u8_msKeyLen);
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_msKeyForceEn[i] = data[index++];
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_msKeySenseEn[i] = data[index++];
+
+	ftsInfo.u8_ssKeyLen = data[index++];
+	logError(0, "%s SS Key Len = %d\n", tag, ftsInfo.u8_ssKeyLen);
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_ssKeyForceEn[i] = data[index++];
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_ssKeySenseEn[i] = data[index++];
+
+	ftsInfo.u8_frcTchXLen = data[index++];
+
+	ftsInfo.u8_frcTchYLen = data[index++];
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_frcTchForceEn[i] = data[index++];
+
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_frcTchSenseEn[i] = data[index++];
+
+
+	ftsInfo.u8_msScrConfigTuneVer = data[index++];
+	logError(0, "%s CFG MS TUNING VERSION = %02X\n",
+		tag, ftsInfo.u8_msScrConfigTuneVer);
+	ftsInfo.u8_msScrLpConfigTuneVer = data[index++];
+	ftsInfo.u8_msScrHwulpConfigTuneVer = data[index++];
+	ftsInfo.u8_msKeyConfigTuneVer = data[index++];
+	ftsInfo.u8_ssTchConfigTuneVer = data[index++];
+	logError(0, "%s CFG SS TUNING VERSION = %02X\n",
+		tag, ftsInfo.u8_ssTchConfigTuneVer);
+	ftsInfo.u8_ssKeyConfigTuneVer = data[index++];
+	ftsInfo.u8_ssHvrConfigTuneVer = data[index++];
+	ftsInfo.u8_frcTchConfigTuneVer = data[index++];
+	ftsInfo.u8_msScrCxmemTuneVer = data[index++];
+	logError(0, "%s CX MS TUNING VERSION = %02X\n",
+		tag, ftsInfo.u8_msScrCxmemTuneVer);
+	ftsInfo.u8_msScrLpCxmemTuneVer = data[index++];
+	ftsInfo.u8_msScrHwulpCxmemTuneVer = data[index++];
+	ftsInfo.u8_msKeyCxmemTuneVer = data[index++];
+	ftsInfo.u8_ssTchCxmemTuneVer = data[index++];
+	logError(0, "%s CX SS TUNING VERSION = %02X\n",
+		tag, ftsInfo.u8_ssTchCxmemTuneVer);
+	ftsInfo.u8_ssKeyCxmemTuneVer = data[index++];
+	ftsInfo.u8_ssHvrCxmemTuneVer = data[index++];
+	ftsInfo.u8_frcTchCxmemTuneVer = data[index++];
+	ftsInfo.u32_mpPassFlag = ((data[index + 3] & 0x000000FF) << 24)
+				+ ((data[index + 2] & 0x000000FF) << 16) +
+				((data[index + 1] & 0x000000FF) << 8) +
+				(data[index] & 0x000000FF);
+	index += 4;
+	logError(0, "%s MP SIGNATURE = %08X\n", tag, ftsInfo.u32_mpPassFlag);
+	ftsInfo.u32_featEn = ((data[index + 3] & 0x000000FF) << 24) +
+				((data[index + 2] & 0x000000FF) << 16) +
+				((data[index + 1] & 0x000000FF) << 8) +
+				(data[index] & 0x000000FF);
+	index += 4;
+	ftsInfo.u32_echoEn = ((data[index + 3] & 0x000000FF) << 24) +
+				((data[index + 2] & 0x000000FF) << 16) +
+				((data[index + 1] & 0x000000FF) << 8) +
+				(data[index] & 0x000000FF);
+	index += 4;
+	logError(0, "%s FEATURES = %08X\n", tag, ftsInfo.u32_echoEn);
+	ftsInfo.u8_sideTchConfigTuneVer = data[index++];
+	ftsInfo.u8_sideTchCxmemTuneVer = data[index++];
+	ftsInfo.u8_sideTchForceLen = data[index++];
+	logError(0, "%s Side Touch Force Len = %d\n",
+		tag, ftsInfo.u8_sideTchForceLen);
+	ftsInfo.u8_sideTchSenseLen = data[index++];
+	logError(0, "%s Side Touch Sense Len = %d\n",
+		tag, ftsInfo.u8_sideTchSenseLen);
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_sideTchForceEn[i] = data[index++];
+	for (i = 0; i < 8; i++)
+		ftsInfo.u64_sideTchSenseEn[i] = data[index++];
+	ftsInfo.u8_errSign = data[index++];
+	logError(0, "%s ERROR SIGNATURE = %02X\n", tag, ftsInfo.u8_errSign);
+	if (ftsInfo.u8_errSign == ERROR_SIGN_HEAD) {
+		logError(0, "%s Correct Error Signature found!\n", tag);
+		u8ToU16(&data[index], &ftsInfo.u16_errOffset);
+	} else {
+		logError(1, "%s Error Signature NOT FOUND!\n", tag);
+		ftsInfo.u16_errOffset = INVALID_ERROR_OFFS;
+	}
+	logError(0, "%s ERROR OFFSET = %04X\n", tag, ftsInfo.u16_errOffset);
+	index += 2;
+	logError(0, "%s Parsed %d bytes!\n", tag, index);
+
+
+	if (index != CHIP_INFO_SIZE + 3) {
+		logError(1, "%s %s: index = %d different from %d ERROR %02X\n",
+			tag, __func__, index, CHIP_INFO_SIZE + 3,
+			ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	logError(0, "%s Chip Info Read DONE!\n", tag);
+	return OK;
+
+FAIL:
+	defaultChipInfo(isI2cError(ret));
+	return ret;
+}
+

st/fts_lib/ftsCompensation.h

@@ -0,0 +1,207 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        FTS API for Flashing the IC                     *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#ifndef __FTS_COMPENSATION_H
+#define __FTS_COMPENSATION_H
+
+
+#include "ftsCrossCompile.h"
+#include "ftsSoftware.h"
+
+
+#define COMP_DATA_READ_RETRY           2
+
+//Bytes dimension of Compensation Data Format
+
+#define COMP_DATA_HEADER               8
+#define COMP_DATA_GLOBAL               8
+
+
+#define HEADER_SIGNATURE               0xA5
+#define INVALID_ERROR_OFFS             0xFFFF
+
+//Possible Compensation/Frame Data Type
+#define GENERAL_TUNING                 0x0100
+#define MS_TOUCH_ACTIVE                0x0200
+#define MS_TOUCH_LOW_POWER             0x0400
+#define MS_TOUCH_ULTRA_LOW_POWER       0x0800
+#define MS_KEY                         0x1000
+#define SS_TOUCH                       0x2000
+#define SS_KEY                         0x4000
+#define SS_HOVER                       0x8000
+#define SS_PROXIMITY                   0x0001
+#define CHIP_INFO                      0xFFFF
+
+
+#define TIMEOUT_REQU_COMP_DATA         1000 //ms
+
+//CHIP INFO
+#define CHIP_INFO_SIZE                 161
+/*bytes to read from framebuffer (exclude the signature and the type*/
+/*because already checked during the reading)*/
+#define EXTERNAL_RELEASE_INFO_SIZE     8 //bytes
+
+struct DataHeader {
+	int force_node, sense_node;
+	u16 type;
+};
+
+
+struct  MutualSenseData {
+	struct DataHeader header;
+	u8 tuning_ver;
+	u8 cx1;
+	u8 *node_data;
+	int node_data_size;
+};
+
+
+struct SelfSenseData {
+	struct DataHeader header;
+	u8 tuning_ver;
+	u8 f_ix1, s_ix1;
+	u8 f_cx1, s_cx1;
+	u8 f_max_n, s_max_n;
+
+	u8 *ix2_fm;
+	u8 *ix2_sn;
+	u8 *cx2_fm;
+	u8 *cx2_sn;
+};
+
+
+struct GeneralData {
+	struct DataHeader header;
+	u8 ftsd_lp_timer_cal0;
+	u8 ftsd_lp_timer_cal1;
+	u8 ftsd_lp_timer_cal2;
+
+	u8 ftsd_lp_timer_cal3;
+	u8 ftsa_lp_timer_cal0;
+	u8 ftsa_lp_timer_cal1;
+};
+
+struct chipInfo {
+	u8 u8_loadCnt;          ///< 03 - Load Counter
+	u8 u8_infoVer;          ///< 04 - New chip info version
+	u16 u16_ftsdId;         ///< 05 - FTSD ID
+	u8 u8_ftsdVer;          ///< 07 - FTSD version
+	u8 u8_ftsaId;           ///< 08 - FTSA ID
+	u8 u8_ftsaVer;          ///< 09 - FTSA version
+	u8 u8_tchRptVer;  ///< 0A - Touch report version (e.g. ST, Samsung etc)
+
+	///< 0B - External release information
+	u8 u8_extReleaseInfo[EXTERNAL_RELEASE_INFO_SIZE];
+	u8 u8_custInfo[12];     ///< 13 - Customer information
+	u16 u16_fwVer;          ///< 1F - Firmware version
+	u16 u16_cfgId;          ///< 21 - Configuration ID
+	u32 u32_projId;         ///< 23 - Project ID
+	u16 u16_scrXRes;        ///< 27 - X resolution on main screen
+	u16 u16_scrYRes;        ///< 29 - Y resolution on main screen
+	u8 u8_scrForceLen;      ///< 2B - Number of force channel on main screen
+	u8 u8_scrSenseLen;      ///< 2C - Number of sense channel on main screen
+	u8 u64_scrForceEn[8];   ///< 2D - Force channel enabled on main screen
+	u8 u64_scrSenseEn[8];   ///< 35 - Sense channel enabled on main screen
+	u8 u8_msKeyLen;         ///< 3D - Number of MS Key channel
+	u8 u64_msKeyForceEn[8]; ///< 3E - MS Key force channel enable
+	u8 u64_msKeySenseEn[8]; ///< 46 - MS Key sense channel enable
+	u8 u8_ssKeyLen;         ///< 4E - Number of SS Key channel
+	u8 u64_ssKeyForceEn[8]; ///< 4F - SS Key force channel enable
+	u8 u64_ssKeySenseEn[8]; ///< 57 - SS Key sense channel enable
+	u8 u8_frcTchXLen;       ///< 5F - Number of force touch force channel
+	u8 u8_frcTchYLen;       ///< 60 - Number of force touch sense channel
+	u8 u64_frcTchForceEn[8];///< 61 - Force touch force channel enable
+	u8 u64_frcTchSenseEn[8];///< 69 - Force touch sense channel enable
+	u8 u8_msScrConfigTuneVer; ///< 71 - MS screen tuning version in config
+
+	///< 72 - MS screen LP mode tuning version in config
+	u8 u8_msScrLpConfigTuneVer;
+
+	///< 73 - MS screen ultra low power mode tuning version in config
+	u8 u8_msScrHwulpConfigTuneVer;
+	u8 u8_msKeyConfigTuneVer; ///< 74 - MS Key tuning version in config
+	u8 u8_ssTchConfigTuneVer; ///< 75 - SS touch tuning version in config
+	u8 u8_ssKeyConfigTuneVer; ///< 76 - SS Key tuning version in config
+	u8 u8_ssHvrConfigTuneVer; ///< 77 - SS hover tuning version in config
+
+	///< 78 - Force touch tuning version in config
+	u8 u8_frcTchConfigTuneVer;
+	u8 u8_msScrCxmemTuneVer; ///< 79 - MS screen tuning version in cxmem
+
+	///< 7A - MS screen LP mode tuning version in cxmem
+	u8 u8_msScrLpCxmemTuneVer;
+
+	///< 7B - MS screen ultra low power mode tuning version in cxmem
+	u8 u8_msScrHwulpCxmemTuneVer;
+	u8 u8_msKeyCxmemTuneVer; ///< 7C - MS Key tuning version in cxmem
+	u8 u8_ssTchCxmemTuneVer; ///< 7D - SS touch tuning version in cxmem
+	u8 u8_ssKeyCxmemTuneVer; ///< 7E - SS Key tuning version in cxmem
+	u8 u8_ssHvrCxmemTuneVer; ///< 7F - SS hover tuning version in cxmem
+
+	///< 80 - Force touch tuning version in cxmem
+	u8 u8_frcTchCxmemTuneVer;
+	u32 u32_mpPassFlag;      ///< 81 - Mass production pass flag
+	u32 u32_featEn;          ///< 85 - Supported features
+
+	///< 89 - enable of particular features: first bit is Echo Enables
+	u32 u32_echoEn;
+
+	///< 8D - Side Touch tuning version in config
+	u8 u8_sideTchConfigTuneVer;
+	u8 u8_sideTchCxmemTuneVer; ///< 8E - Side Touch tuning version in cxmem
+	u8 u8_sideTchForceLen;   ///< 8F - Number of force channel on side touch
+	u8 u8_sideTchSenseLen;   ///< 90 - Number of sense channel on side touch
+	u8 u64_sideTchForceEn[8];///< 91 - Side touch force channel enable
+	u8 u64_sideTchSenseEn[8];///< 99 - Side touch sense channel enable
+	u8 u8_errSign;           ///< A1 - Signature for error field
+	u16 u16_errOffset;       ///< A2 - Error Offset
+};
+
+int requestCompensationData(u16 type);
+int readCompensationDataHeader(u16 type, struct DataHeader *header,
+	u16 *address);
+int readMutualSenseGlobalData(u16 *address, struct MutualSenseData *global);
+int readMutualSenseNodeData(u16 address, struct MutualSenseData *node);
+int readMutualSenseCompensationData(u16 type, struct MutualSenseData *data);
+int readSelfSenseGlobalData(u16 *address, struct SelfSenseData *global);
+int readSelfSenseNodeData(u16 address, struct SelfSenseData *node);
+int readSelfSenseCompensationData(u16 type, struct SelfSenseData *data);
+int readGeneralGlobalData(u16 address, struct GeneralData *global);
+int readGeneralCompensationData(u16 type, struct GeneralData *data);
+int defaultChipInfo(int i2cError);
+int readChipInfo(int doRequest);
+
+#endif

st/fts_lib/ftsCrossCompile.c

@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                         FTS Cross Compile                              *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/init.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+//#include <linux/sec_sysfs.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/spi/spidev.h>
+#include <linux/fcntl.h>
+#include <linux/syscalls.h>
+
+#include "ftsCrossCompile.h"
+#include "ftsError.h"
+
+void *stmalloc(size_t size)
+{
+	return kmalloc(size, GFP_KERNEL);
+}
+
+void stfree(void *ptr)
+{
+	kfree(ptr);
+}

st/fts_lib/ftsCrossCompile.h

@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2018, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        FTS cross compile                               *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#ifndef __FTS_CROSS_COMPILE_H
+#define __FTS_CROSS_COMPILE_H
+
+//#define NDK
+//#define DEBUG
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/init.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+//#include <linux/sec_sysfs.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/spi/spidev.h>
+#include <linux/fcntl.h>
+#include <linux/syscalls.h>
+
+void *stmalloc(size_t size);
+void stfree(void *ptr);
+
+#endif

st/fts_lib/ftsError.c

@@ -0,0 +1,262 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                  FTS error/info kernel log reporting                   *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/completion.h>
+//#include <linux/wakelock.h>
+#include <linux/pm_wakeup.h>
+
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/consumer.h>
+
+#include "../fts.h"
+#include "ftsCrossCompile.h"
+#include "ftsError.h"
+#include "ftsIO.h"
+#include "ftsTool.h"
+#include "ftsCompensation.h"
+
+static char tag[8] = "[ FTS ]\0";
+
+
+void logError(int force, const char *msg, ...)
+{
+
+	if (force == 1
+#ifdef DEBUG
+		|| 1
+#endif
+	) {
+		va_list args;
+
+		va_start(args, msg);
+		vprintk(msg, args);
+		va_end(args);
+	}
+}
+
+int isI2cError(int error)
+{
+	if (((error & 0x000000FF) >= (ERROR_I2C_R & 0x000000FF))
+		&& ((error & 0x000000FF) <= (ERROR_I2C_O & 0x000000FF)))
+		return 1;
+	else
+		return 0;
+}
+
+int dumpErrorInfo(void)
+{
+	int ret, i;
+	u8 data[ERROR_INFO_SIZE] = {0};
+	u32 sign = 0;
+
+	logError(0, "%s %s: Starting dump of error info...\n", tag, __func__);
+	if (ftsInfo.u16_errOffset == INVALID_ERROR_OFFS)  {
+		logError(1, "%s %s: Invalid error offset ERROR %02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	ret = readCmdU16(FTS_CMD_FRAMEBUFFER_R, ftsInfo.u16_errOffset,
+		data, ERROR_INFO_SIZE, DUMMY_FRAMEBUFFER);
+	if (ret < OK) {
+		logError(1, "%s %s: reading data ERROR %02X\n",
+			tag,  __func__, ret);
+		return ret;
+	}
+	logError(0, "%s %s: Error Info =\n", tag, __func__);
+	u8ToU32(data, &sign);
+	if (sign != ERROR_SIGNATURE)
+		logError(1, "%s %s:Wrong Signature! Data may be invalid!\n",
+			tag, __func__);
+	else
+		logError(1, "%s %s: Error Signature OK! Data are valid!\n",
+			tag, __func__);
+
+	for (i = 0; i < ERROR_INFO_SIZE; i++) {
+		if (i % 4 == 0)
+			logError(1, KERN_ERR "\n%s %s: %d) ",
+				tag, __func__, i / 4);
+		logError(1, "%02X ", data[i]);
+	}
+	logError(1, "\n");
+
+	logError(0, "%s %s: dump of error info FINISHED!\n", tag, __func__);
+	return OK;
+
+}
+
+int errorHandler(u8 *event, int size)
+{
+	int res = OK;
+	struct fts_ts_info *info = NULL;
+
+	if (getClient() != NULL)
+		info = i2c_get_clientdata(getClient());
+
+	if (info == NULL || event == NULL || size <= 1 || event[0] !=
+		EVENTID_ERROR_EVENT) {
+		logError(1, "%s %s: event Null or not correct size! ",
+			tag, __func__,  ERROR_OP_NOT_ALLOW);
+		logError(1, "ERROR %08X\n", ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	logError(0, "%s %s: Starting handling...\n", tag, __func__);
+	//TODO: write an error log for undefinied command subtype 0xBA
+	switch (event[1]) {
+	case EVENT_TYPE_ESD_ERROR:	//esd
+		res = fts_chip_powercycle(info);
+		if (res < OK) {
+			logError(1, "%s %s: ", tag, res);
+			logError(1, "Error performing powercycle ERROR %08X\n");
+		}
+
+		res = fts_system_reset();
+		if (res < OK)
+			logError(1, "%s %s:Cannot reset device ERROR%08X\n",
+				tag, __func__, res);
+		res = (ERROR_HANDLER_STOP_PROC | res);
+		break;
+
+	case EVENT_TYPE_WATCHDOG_ERROR:	//watchdog
+		dumpErrorInfo();
+		res = fts_system_reset();
+		if (res < OK)
+			logError(1, "%s %s:Cannot reset device:ERROR%08X\n",
+				tag, __func__, res);
+		res = (ERROR_HANDLER_STOP_PROC | res);
+		break;
+	case EVENT_TYPE_CHECKSUM_ERROR: //CRC ERRORS
+		switch (event[2]) {
+		case CRC_CONFIG_SIGNATURE:
+			logError(1, "%s %s: Config Signature ERROR!\n",
+				tag, __func__);
+			break;
+		case CRC_CONFIG:
+			logError(1, "%s %s:Config CRC ERROR!\n", tag, __func__);
+			break;
+		case CRC_CX_MEMORY:
+			logError(1, "%s %s: CX CRC ERROR!\n", tag, __func__);
+			break;
+		}
+		break;
+	case EVENT_TYPE_LOCKDOWN_ERROR:
+		//res = (ERROR_HANDLER_STOP_PROC|res);
+		//stop lockdown code routines in order to retry
+		switch (event[2]) {
+		case 0x01:
+			logError(1, "%s %s:Lockdown code alredy ",
+				tag, __func__);
+			logError(1, "written into the IC!\n");
+			break;
+		case 0x02:
+			logError(1, "%s %s:Lockdown CRC ", tag, __func__);
+			logError(1, "check fail during a WRITE!\n");
+			break;
+
+		case 0x03:
+			logError(1,
+				"%s %s:Lockdown WRITE command format wrong!\n",
+				tag, __func__);
+			break;
+		case 0x04:
+			pr_err("Lockdown Memory Corrupted!\n");
+			logError(1, "%s %s:Please contact ST for support!\n",
+				tag, __func__);
+			break;
+		case 0x11:
+			logError(1,
+				"%s %s:NO Lockdown code to READ into the IC!\n",
+				tag, __func__);
+			break;
+		case 0x12:
+			logError(1,
+				"%s %s:Lockdown code data corrupted\n",
+				tag, __func__);
+			break;
+		case 0x13:
+			logError(1,
+				"%s %s:Lockdown READ command format wrong!\n",
+				tag, __func__);
+			break;
+		case 0x21:
+			pr_err("Exceeded maximum number of\n");
+			logError(1,
+				"%s %s:Lockdown code REWRITE into IC!\n",
+				tag, __func__);
+			break;
+		case 0x22:
+			logError(1, "%s %s:Lockdown CRC check", tag, __func__);
+			logError(1, " fail during a REWRITE!\n");
+			break;
+		case 0x23:
+			logError(1, "%s %s:", tag, __func__);
+			logError(1, "Lockdown REWRITE command format wrong!\n");
+			break;
+		case 0x24:
+			pr_err("Lockdown Memory Corrupted!\n");
+			logError(1, "%s %s:Please contact ST for support!\n",
+				tag, __func__);
+			break;
+		default:
+			logError(1, "%s %s:No valid error type for LOCKDOWN!\n",
+				tag, __func__);
+		}
+		break;
+
+	default:
+		logError(0,  "%s %s: No Action taken!\n", tag, __func__);
+	break;
+	}
+	logError(0, "%s %s: handling Finished! res = %08X\n",
+		tag, __func__, res);
+	return res;
+}
+

st/fts_lib/ftsError.h

@@ -0,0 +1,181 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                  FTS error/info kernel log reporting                   *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#ifndef __FTS_ERROR_H
+#define __FTS_ERROR_H
+
+
+//FIRST LEVEL ERROR CODE
+#define OK                             (int)(0x00000000)/* No ERROR*/
+/*allocation of memory failed*/
+#define ERROR_ALLOC                    (int)(0x80000001)
+#define ERROR_I2C_R                    (int)(0x80000002)//i2c read failed
+#define ERROR_I2C_W                    (int)(0x80000003)//i2c write failed
+#define ERROR_I2C_WR                   (int)(0x80000004)//i2c write/read failed
+
+//error during opening i2c device
+#define ERROR_I2C_O                    (int)(0x80000005)
+#define ERROR_OP_NOT_ALLOW             (int)(0x80000006)//operation not allowed
+
+//timeout expired! exceed the max number of
+//retries or the max waiting time
+#define ERROR_TIMEOUT                  (int)(0x80000007)
+
+//the file that i want to open is not found
+#define ERROR_FILE_NOT_FOUND           (int)(0x80000008)
+//error during parsing the file
+#define ERROR_FILE_PARSE               (int)(0x80000009)
+//error during reading the file
+#define ERROR_FILE_READ                (int)(0x8000000A)
+#define ERROR_LABEL_NOT_FOUND          (int)(0x8000000B)//label not found
+
+//fw in the chip newer than the one in the memmh
+#define ERROR_FW_NO_UPDATE             (int)(0x8000000C)
+//flash status busy or unknown
+#define ERROR_FLASH_UNKNOWN            (int)(0x8000000D)
+
+//SECOND LEVEL ERROR CODE
+//unable to disable the interrupt
+#define ERROR_DISABLE_INTER            (int)(0x80000200)
+
+//unable to activate the interrupt
+#define ERROR_ENABLE_INTER             (int)(0x80000300)
+
+#define ERROR_READ_B2                  (int)(0x80000400)//B2 command failed
+
+//unable to read an offset from memory
+#define ERROR_GET_OFFSET               (int)(0x80000500)
+
+//unable to retrieve the data of a required frame
+#define ERROR_GET_FRAME_DATA           (int)(0x80000600)
+
+//FW answers with an event that has a
+//different address respect the request done
+#define ERROR_DIFF_COMP_TYPE           (int)(0x80000700)
+
+//the signature of the compensation data is not A5
+#define ERROR_WRONG_COMP_SIGN          (int)(0x80000800)
+//the command Sense On failed
+#define ERROR_SENSE_ON_FAIL            (int)(0x80000900)
+//the command Sense Off failed
+#define ERROR_SENSE_OFF_FAIL           (int)(0x80000A00)
+
+//the command SYSTEM RESET failed
+#define ERROR_SYSTEM_RESET_FAIL        (int)(0x80000B00)
+
+//flash status not ready within a timeout
+#define ERROR_FLASH_NOT_READY          (int)(0x80000C00)
+
+//unable to retrieve fw_vers or the config_id
+#define ERROR_FW_VER_READ              (int)(0x80000D00)
+
+//unable to enable/disable the gesture
+#define ERROR_GESTURE_ENABLE_FAIL      (int)(0x80000E00)
+
+//unable to start to add custom gesture
+#define ERROR_GESTURE_START_ADD        (int)(0x80000F00)
+
+//unable to finish to add custom gesture
+#define ERROR_GESTURE_FINISH_ADD       (int)(0x80001000)
+
+//unable to add custom gesture data
+#define ERROR_GESTURE_DATA_ADD         (int)(0x80001100)
+
+//unable to remove custom gesture data
+#define ERROR_GESTURE_REMOVE           (int)(0x80001200)
+
+//unable to enable/disable a feature mode in the IC
+#define ERROR_FEATURE_ENABLE_DISABLE   (int)(0x80001300)
+
+//unable to set/read noise parameter in the IC
+#define ERROR_NOISE_PARAMETERS         (int)(0x80001400)
+
+//unable to write/rewrite/read lockdown code in the IC
+#define ERROR_LOCKDOWN_CODE            (int)(0x80001500)
+
+//THIRD LEVEL ERROR CODE
+//unable to retrieve the force and/or sense length
+#define ERROR_CH_LEN                   (int)(0x80010000)
+
+//compensation data request failed
+#define ERROR_REQU_COMP_DATA           (int)(0x80020000)
+
+//unable to retrieve the compensation data header
+#define ERROR_COMP_DATA_HEADER         (int)(0x80030000)
+
+//unable to retrieve the global compensation data
+#define ERROR_COMP_DATA_GLOBAL         (int)(0x80040000)
+
+//unable to retrieve the compensation data for each node
+#define ERROR_COMP_DATA_NODE           (int)(0x80050000)
+
+//check of production limits or of fw answers failed
+#define ERROR_TEST_CHECK_FAIL          (int)(0x80060000)
+#define ERROR_MEMH_READ                (int)(0x80070000)//memh reading failed
+#define ERROR_FLASH_BURN_FAILED        (int)(0x80080000)//flash burn failed
+#define ERROR_MS_TUNING                (int)(0x80090000)//ms tuning failed
+#define ERROR_SS_TUNING                (int)(0x800A0000)//ss tuning failed
+//lp timer calibration failed
+#define ERROR_LP_TIMER_TUNING          (int)(0x800B0000)
+//save cx data to flash failed
+#define ERROR_SAVE_CX_TUNING           (int)(0x800C0000)
+
+//stop the poll of the FIFO if particular errors are found
+#define ERROR_HANDLER_STOP_PROC        (int)(0x800D0000)
+//unable to retrieve echo event
+#define ERROR_CHECK_ECHO_FAIL          (int)(0x800E0000)
+
+//FOURTH LEVEL ERROR CODE
+//production data test failed
+#define ERROR_PROD_TEST_DATA           (int)(0x81000000)
+
+//complete flash procedure failed
+#define ERROR_FLASH_PROCEDURE          (int)(0x82000000)
+//production ito test failed
+#define ERROR_PROD_TEST_ITO            (int)(0x83000000)
+
+//production initialization test failed
+#define ERROR_PROD_TEST_INITIALIZATION (int)(0x84000000)
+
+//mismatch of the MS or SS tuning_version
+#define ERROR_GET_INIT_STATUS          (int)(0x85000000)
+
+
+void logError(int force, const char *msg, ...);
+int isI2cError(int error);
+int dumpErrorInfo(void);
+int errorHandler(u8 *event, int size);
+
+#endif

st/fts_lib/ftsFlash.c

@@ -0,0 +1,1178 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        FTS API for Flashing the IC                     *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <stdarg.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/time.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+
+#include "ftsCrossCompile.h"
+#include "ftsCompensation.h"
+#include "ftsError.h"
+#include "ftsFlash.h"
+#include "ftsFrame.h"
+#include "ftsIO.h"
+#include "ftsSoftware.h"
+#include "ftsTest.h"
+#include "ftsTime.h"
+#include "ftsTool.h"
+#include "../fts.h"//needed for including the define FW_H_FILE
+
+#ifdef FW_H_FILE
+#include <../fts_fw.h>
+#define LOAD_FW_FROM 1
+#else
+#define LOAD_FW_FROM 0
+#endif
+
+#define FTS_LATEST_VERSION 0x1101
+
+static char tag[8] = "[ FTS ]\0";
+
+int getFirmwareVersion(u16 *fw_vers, u16 *config_id)
+{
+	u8 fwvers[DCHIP_FW_VER_BYTE];
+	u8 confid[CONFIG_ID_BYTE];
+	int res;
+
+	res = readCmdU16(FTS_CMD_HW_REG_R, DCHIP_FW_VER_ADDR,
+			fwvers, DCHIP_FW_VER_BYTE, DUMMY_HW_REG);
+	if (res < OK) {
+		logError(1,
+		"%s %s:unable to read fw_version ERROR %02X\n",
+		tag, __func__, ERROR_FW_VER_READ);
+		return (res | ERROR_FW_VER_READ);
+	}
+
+	u8ToU16(fwvers, fw_vers); //fw version use big endian
+	if (*fw_vers != 0) {
+	// if fw_version is 00 00 means that there is
+	//no firmware running in the chip therefore will be
+	//impossible find the config_id
+		res = readB2(CONFIG_ID_ADDR, confid, CONFIG_ID_BYTE);
+		if (res < OK) {
+			logError(1, "%s %s:unable to read config_id ",
+				tag, __func__);
+			logError(1, "ERROR %02X\n", ERROR_FW_VER_READ);
+			return (res | ERROR_FW_VER_READ);
+		}
+		u8ToU16(confid, config_id); //config id use little endian
+	} else {
+		*config_id = 0x0000;
+	}
+
+	logError(0, "%s FW VERS = %04X\n", tag, *fw_vers);
+	logError(0, "%s CONFIG ID = %04X\n", tag, *config_id);
+	return OK;
+}
+
+int getFWdata_nocheck(const char *pathToFile, u8 **data, int *size, int from)
+{
+	const struct firmware *fw = NULL;
+	struct device *dev = getDev();
+	int res;
+
+	if (dev == NULL)
+		return ERROR_OP_NOT_ALLOW;
+
+	logError(0, "%s Read FW from BIN file!\n", tag);
+
+	res = firmware_request_nowarn(&fw, pathToFile, dev);
+	if (res) {
+		logError(1, "%s %s:No File found! ERROR %08X\n",
+			tag, __func__, ERROR_FILE_NOT_FOUND);
+		return ERROR_FILE_NOT_FOUND;
+	}
+
+	*size = fw->size;
+	*data = (u8 *)kmalloc_array((*size), sizeof(u8), GFP_KERNEL);
+	if (*data == NULL) {
+		logError(1, "%s %s:Impossible to allocate! %08X\n", __func__);
+		release_firmware(fw);
+		return ERROR_ALLOC;
+	}
+	memcpy(*data, (u8 *)fw->data, (*size));
+	release_firmware(fw);
+
+	logError(0, "%s %s:Finshed!\n", tag, __func__);
+	return OK;
+}
+
+int getFWdata(const char *pathToFile, u8 **data, int *size, int from)
+{
+	const struct firmware *fw = NULL;
+	struct device *dev = NULL;
+	int res;
+
+	logError(0, "%s %s starting...\n", tag, __func__);
+	switch (from) {
+#ifdef FW_H_FILE
+	case 1:
+		logError(1, "%s Read FW from .h file!\n", tag);
+		*size = FW_SIZE_NAME;
+		*data = (u8 *)kmalloc_array((*size), sizeof(u8), GFP_KERNEL);
+		if (*data == NULL) {
+			logError(1, "%s %s:Impossible to allocate memory! ",
+				tag, __func__);
+			logError(1, "ERROR %08X\n", ERROR_ALLOC);
+
+			return ERROR_ALLOC;
+		}
+		memcpy(*data, (u8 *)FW_ARRAY_NAME, (*size));
+		break;
+#endif
+	default:
+		logError(0, "%s Read FW from BIN file!\n", tag);
+
+		if (ftsInfo.u16_fwVer >= FTS_LATEST_VERSION)
+			return ERROR_FW_NO_UPDATE;
+
+		dev = getDev();
+
+		if (dev != NULL) {
+			res = firmware_request_nowarn(&fw, pathToFile, dev);
+			if (res == 0) {
+				*size = fw->size;
+				*data = (u8 *)kmalloc_array((*size), sizeof(u8),
+					GFP_KERNEL);
+				if (*data == NULL) {
+					logError(1, "%s %s:Impossible to ",
+						tag, __func__);
+					logError(1, "%allocate! %08X\n",
+						ERROR_ALLOC);
+					release_firmware(fw);
+					return ERROR_ALLOC;
+				}
+				memcpy(*data, (u8 *)fw->data, (*size));
+				release_firmware(fw);
+			} else {
+				logError(0, "%s %s:No File found! ERROR %08X\n",
+					tag, __func__, ERROR_FILE_NOT_FOUND);
+				return ERROR_FILE_NOT_FOUND;
+			}
+		} else {
+			logError(1, "%s %s:No device found! ERROR %08X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+			return ERROR_OP_NOT_ALLOW;
+		}
+		/* break; */
+	}
+
+	logError(0, "%s %s:Finshed!\n", tag, __func__);
+	return OK;
+}
+
+int readFwFile(const char *path, struct Firmware *fw, int keep_cx)
+{
+	int res;
+	int orig_size;
+	u8 *orig_data = NULL;
+
+
+	res = getFWdata(path, &orig_data, &orig_size, LOAD_FW_FROM);
+	if (res < OK) {
+		logError(0, "%s %s:impossible retrieve FW... ERROR %08X\n",
+			tag, __func__, ERROR_MEMH_READ);
+
+		return (res | ERROR_MEMH_READ);
+	}
+	res = parseBinFile(orig_data, orig_size, fw, keep_cx);
+
+	if (res < OK) {
+		logError(1, "%s %s:impossible parse ERROR %08X\n",
+			tag, __func__, ERROR_MEMH_READ);
+		return (res | ERROR_MEMH_READ);
+	}
+
+	return OK;
+}
+
+int flashProcedure(const char *path, int force, int keep_cx)
+{
+	struct Firmware fw;
+	int res;
+
+	fw.data = NULL;
+	logError(0, "%s Reading Fw file...\n", tag);
+	res = readFwFile(path, &fw, keep_cx);
+	if (res < OK) {
+		logError(0, "%s %s: ERROR %02X\n",
+			tag, __func__, (res | ERROR_FLASH_PROCEDURE));
+		kfree(fw.data);
+		return (res | ERROR_FLASH_PROCEDURE);
+	}
+	logError(0, "%s Fw file read COMPLETED!\n", tag);
+
+	logError(0, "%s Starting flashing procedure...\n", tag);
+	res = flash_burn(&fw, force, keep_cx);
+	if (res < OK && res != (ERROR_FW_NO_UPDATE | ERROR_FLASH_BURN_FAILED)) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_FLASH_PROCEDURE);
+		kfree(fw.data);
+		return (res | ERROR_FLASH_PROCEDURE);
+	}
+	logError(0, "%s flashing procedure Finished!\n", tag);
+	kfree(fw.data);
+
+	return res;
+}
+
+#ifdef FTM3_CHIP
+int flash_status(void)
+{
+	u8 cmd[2] = {FLASH_CMD_READSTATUS, 0x00};
+	u8 readData = 0;
+
+	logError(0, "%s %s:Reading ...\n", tag, __func__);
+	if (fts_readCmd(cmd, 2, &readData, FLASH_STATUS_BYTES) < 0) {
+		logError(1, "%s %s: ERROR % 02X\n", tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+
+	readData &= 0x01;
+	logError(0, "%s %s = %d\n", tag, __func__, readData);
+	return (int) readData;
+}
+
+int flash_status_ready(void)
+{
+
+	int status = flash_status();
+
+	if (status == ERROR_I2C_R) {
+		logError(1, "%s %s: ERROR % 02X\n", tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+
+	if (status != FLASH_READY) {
+		//logError(1,
+		//"%s %s:flash busy or unknown STATUS = % 02X\n",
+		//tag, status);
+		return ERROR_FLASH_UNKNOWN;
+	}
+
+	return FLASH_READY;
+}
+
+int wait_for_flash_ready(void)
+{
+	int status;
+	int (*code)(void);
+
+	code = flash_status_ready;
+
+	logError(0, "%s Waiting for flash ready...\n", tag);
+	status = attempt_function(code, FLASH_WAIT_BEFORE_RETRY,
+			FLASH_RETRY_COUNT);
+
+	if (status != FLASH_READY) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_FLASH_NOT_READY);
+		return (status | ERROR_FLASH_NOT_READY);
+	}
+
+	logError(0, "%s Flash ready!\n", tag);
+	return OK;
+}
+
+int flash_unlock(void)
+{
+	int status;
+	//write the command to perform the unlock
+	u8 cmd[3] = {FLASH_CMD_UNLOCK, FLASH_UNLOCK_CODE0,
+		FLASH_UNLOCK_CODE1};
+
+	logError(0, "%s Try to unlock flash...\n", tag);
+	status = wait_for_flash_ready();
+
+	if (status != OK) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_FLASH_NOT_READY);
+		//Flash not ready within the chosen time, better exit!
+		return (status | ERROR_FLASH_NOT_READY);
+	}
+
+	logError(0, "%s Command unlock ...\n", tag);
+	if (fts_writeCmd(cmd, sizeof(cmd)) < 0) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+
+	status = wait_for_flash_ready();
+
+	if (status != OK) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_FLASH_NOT_READY);
+		//Flash not ready within the chosen time,
+		//better exit!
+		return (status | ERROR_FLASH_NOT_READY);
+	}
+	logError(0, "%s Unlock flash DONE!\n", tag);
+
+	return OK;
+}
+
+int parseBinFile(u8 *fw_data, int fw_size, Firmware *fwData, int keep_cx)
+{
+	int dimension;
+
+	if (keep_cx) {
+		dimension = FW_SIZE - FW_CX_SIZE;
+		logError(1, "%s %s: Selected 124k Configuration!\n",
+			tag, __func__);
+	} else {
+		dimension = FW_SIZE;
+		logError(1, "%s %s: Selected 128k Configuration!\n",
+			tag, __func__);
+	}
+
+	if (fw_size - FW_HEADER_SIZE != FW_SIZE || fw_data == NULL) {
+		logError(1, "%s %s:Read only %d instead of %d... ERROR %02X\n",
+			tag, __func__,
+			fw_size - FW_HEADER_SIZE,
+			FW_SIZE, ERROR_FILE_PARSE);
+		kfree(fw_data);
+		return ERROR_FILE_PARSE;
+	}
+
+	fwData->data = (u8 *)kmalloc_array(dimension, sizeof(u8), GFP_KERNEL);
+	if (fwData->data == NULL) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+
+		kfree(fw_data);
+		return ERROR_ALLOC;
+	}
+
+	memcpy(fwData->data, ((u8 *)(fw_data) + FW_HEADER_SIZE),
+				dimension);
+	fwData->data_size = dimension;
+
+	fwData->fw_ver = (u16)(((fwData->data[FW_VER_MEMH_BYTE1] & 0x00FF) << 8)
+				+ (fwData->data[FW_VER_MEMH_BYTE0] & 0x00FF));
+
+	fwData->config_id = (u16)(((fwData->data[(FW_CODE_SIZE)
+				+ FW_OFF_CONFID_MEMH_BYTE1] & 0x00FF) << 8)
+			+ (fwData->data[(FW_CODE_SIZE) +
+				FW_OFF_CONFID_MEMH_BYTE0] & 0x00FF));
+
+	logError(0, "%s %s: FW VERS File = %04X\n",
+			tag, __func__, fwData->fw_ver);
+	logError(0, "%s %s: CONFIG ID File = %04X\n",
+			tag, __func__, fwData->config_id);
+
+	logError(0, "%s READ FW DONE %d bytes!\n", tag, fwData->data_size);
+
+	kfree(fw_data);
+		return OK;
+}
+
+int fillMemory(u32 address, u8 *data, int size)
+{
+	int remaining = size;
+	int toWrite = 0;
+	int delta;
+
+	u8 *buff = (u8 *)kmalloc_array((MEMORY_CHUNK + 3), sizeof(u8),
+				GFP_KERNEL);
+	if (buff == NULL) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	while (remaining > 0) {
+		if (remaining >= MEMORY_CHUNK) {
+			if ((address + MEMORY_CHUNK) < FLASH_ADDR_SWITCH_CMD) {
+				buff[0] = FLASH_CMD_WRITE_LOWER_64;
+				toWrite = MEMORY_CHUNK;
+				remaining -= MEMORY_CHUNK;
+			} else {
+				if (address < FLASH_ADDR_SWITCH_CMD) {
+					delta = FLASH_ADDR_SWITCH_CMD - address;
+
+					buff[0] = FLASH_CMD_WRITE_LOWER_64;
+					toWrite = delta;
+					remaining -= delta;
+				} else {
+					buff[0] = FLASH_CMD_WRITE_UPPER_64;
+					toWrite = MEMORY_CHUNK;
+					remaining -= MEMORY_CHUNK;
+				}
+			}
+		} else {
+			if ((address + remaining) < FLASH_ADDR_SWITCH_CMD) {
+				buff[0] = FLASH_CMD_WRITE_LOWER_64;
+				toWrite = remaining;
+				remaining = 0;
+			} else {
+				if (address < FLASH_ADDR_SWITCH_CMD) {
+					delta = FLASH_ADDR_SWITCH_CMD - address;
+
+					buff[0] = FLASH_CMD_WRITE_LOWER_64;
+					toWrite = delta;
+					remaining -= delta;
+				} else {
+					buff[0] = FLASH_CMD_WRITE_UPPER_64;
+					toWrite = remaining;
+					remaining = 0;
+				}
+			}
+		}
+
+		buff[1] = (u8) ((address & 0x0000FF00) >> 8);
+		buff[2] = (u8) (address & 0x000000FF);
+		memcpy(buff + 3, data, toWrite);
+		//logError(0,
+		//"%s Command = %02X , address = %02X %02X, bytes = %d\n",
+		//tag, buff[0], buff[1], buff[2], toWrite);
+		if (fts_writeCmd(buff, 3 + toWrite) < 0) {
+			logError(1, "%s %s: ERROR %02X\n",
+				tag, __func__, ERROR_I2C_W);
+			kfree(buff);
+			return ERROR_I2C_W;
+		}
+		address += toWrite;
+		data += toWrite;
+	}
+	kfree(buff);
+	return OK;
+}
+
+int flash_burn(Firmware *fw, int force_burn, int keep_cx)
+{
+	u8 cmd;
+	int res;
+
+	if (!force_burn && (ftsInfo.u16_fwVer >= fw->fw_ver)
+		&& (ftsInfo.u16_cfgId >= fw->config_id)) {
+		logError(0, "Firmware in the chip newer");
+		logError(0, " or equal to the one to burn! ");
+		logError(0, "%s %s:NO UPDATE ERROR %02X\n",
+			tag, __func__, ERROR_FW_NO_UPDATE);
+		return (ERROR_FW_NO_UPDATE | ERROR_FLASH_BURN_FAILED);
+	}
+
+	//programming procedure start
+
+	logError(0, "%s Programming Procedure for flashing started:\n", tag);
+
+	logError(0, "%s 1) SYSTEM RESET:\n", tag);
+	res = fts_system_reset();
+	if (res < 0) {
+		logError(1, "%s system reset FAILED!\n", tag);
+		//if there is no firmware i will not
+		//get the controller ready event and
+		//there will be a timeout but i can
+		//keep going, but if there is
+		//an I2C error i have to exit
+		if (res != (ERROR_SYSTEM_RESET_FAIL | ERROR_TIMEOUT))
+			return (res | ERROR_FLASH_BURN_FAILED);
+	} else
+		logError(0, "%s system reset COMPLETED!\n\n", tag);
+
+	logError(0, "%s 2) FLASH UNLOCK:\n", tag);
+	res = flash_unlock();
+	if (res < 0) {
+		logError(1, "%s flash unlock FAILED! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s flash unlock COMPLETED!\n\n", tag);
+
+
+	//Write the lower part of the Program RAM
+	logError(0, "%s 3) PREPARING DATA FOR FLASH BURN:\n", tag);
+
+	res = fillMemory(FLASH_ADDR_CODE, fw->data, fw->data_size);
+	if (res < 0) {
+		logError(1, "%s Error During filling the memory!%02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s Data copy COMPLETED!\n\n", tag);
+
+	logError(0, "%s 4) ERASE FLASH:\n", tag);
+	res = wait_for_flash_ready();
+	if (res < 0) {
+		logError(1, "%s Flash not ready! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+
+	logError(0, "%s Command erase ...\n", tag);
+	cmd = FLASH_CMD_ERASE;
+	if (fts_writeCmd(&cmd, 1) < 0) {
+		logError(1, "%s Error during erasing flash! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (ERROR_I2C_W | ERROR_FLASH_BURN_FAILED);
+	}
+
+	res = wait_for_flash_ready();
+	if (res < 0) {
+		logError(1, "%s Flash not ready 2! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+
+	logError(0, "%s Flash erase COMPLETED!\n\n", tag);
+
+	logError(0, "%s 5) BURN FLASH:\n", tag);
+	logError(0, "%s Command burn ...\n", tag);
+	cmd = FLASH_CMD_BURN;
+	if (fts_writeCmd(&cmd, 1) < 0) {
+		logError(1, "%s Error during burning data! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (ERROR_I2C_W | ERROR_FLASH_BURN_FAILED);
+	}
+
+	res = wait_for_flash_ready();
+	if (res < 0) {
+		logError(1, "%s Flash not ready! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+
+	logError(0, "%s Flash burn COMPLETED!\n\n", tag);
+
+	logError(0, "%s 6) SYSTEM RESET:\n", tag);
+	res = fts_system_reset();
+	if (res < 0) {
+		logError(1, "%s system reset FAILED! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s system reset COMPLETED!\n\n", tag);
+
+
+	logError(0, "%s 7) FINAL CHECK:\n", tag);
+	res = readChipInfo(0);
+	if (res < 0) {
+		logError(1, "%s %s:Unable to retrieve Chip INFO!%02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+
+	if ((ftsInfo.u16_fwVer != fw->fw_ver)
+		&& (ftsInfo.u16_cfgId != fw->config_id)) {
+		logError(1, "Firmware in the chip different");
+		logError(1, " from the one that was burn!");
+		logError(1, "%s fw: %x != %x , conf: %x != %x\n",
+			tag, ftsInfo.u16_fwVer,
+			fw->fw_ver,
+			ftsInfo.u16_cfgId,
+			fw->config_id);
+		return ERROR_FLASH_BURN_FAILED;
+	}
+
+	logError(0, "%s Final check OK! fw: %02X, conf: %02X\n",
+		tag, ftsInfo.u16_fwVer, ftsInfo.u16_cfgId);
+
+	return OK;
+}
+
+#else
+
+int wait_for_flash_ready(u8 type)
+{
+	u8 cmd[2] = {FLASH_CMD_READ_REGISTER, type};
+	u8 readData = 0;
+	int i, res = -1;
+
+	logError(0, "%s Waiting for flash ready ...\n", tag);
+	for (i = 0; i < FLASH_RETRY_COUNT && res != 0; i++) {
+		if (fts_readCmd(cmd, sizeof(cmd), &readData, 1) < 0) {
+			logError(1, "%s %s: ERROR % 02X\n",
+				tag, __func__, ERROR_I2C_R);
+		} else {
+			res = readData & 0x80;
+			//logError(0, "%s flash status = %d\n", tag, res);
+		}
+		msleep(FLASH_WAIT_BEFORE_RETRY);
+	}
+
+	if (i == FLASH_RETRY_COUNT && res != 0) {
+		logError(1, "%s Wait for flash TIMEOUT! ERROR %02X\n",
+			tag, ERROR_TIMEOUT);
+		return ERROR_TIMEOUT;
+	}
+
+	logError(0, "%s Flash READY!\n", tag);
+	return OK;
+}
+
+int fts_warm_boot(void)
+{
+	//write the command to perform the warm boot
+	u8 cmd[4] = {FTS_CMD_HW_REG_W, 0x00, 0x00, WARM_BOOT_VALUE};
+
+	u16ToU8_be(ADDR_WARM_BOOT, &cmd[1]);
+
+	logError(0, "%s Command warm boot ...\n", tag);
+	if (fts_writeCmd(cmd, sizeof(cmd)) < 0) {
+		logError(1, "%s flash_unlock: ERROR % 02X\n", tag, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+
+	logError(0, "%s Warm boot DONE!\n", tag);
+
+	return OK;
+}
+
+int parseBinFile(u8 *data, int fw_size,
+		struct Firmware *fwData, int keep_cx)
+{
+	int dimension, index = 0;
+	u32 temp;
+	int res, i;
+
+	//the file should contain at least the header plus the content_crc
+	if (fw_size < FW_HEADER_SIZE+FW_BYTES_ALIGN || data == NULL) {
+		logError(1, "%s %s:Read only %d instead of %d...ERROR %02X\n",
+			tag, __func__, fw_size,
+			FW_HEADER_SIZE + FW_BYTES_ALIGN,
+			ERROR_FILE_PARSE);
+		res = ERROR_FILE_PARSE;
+		goto END;
+	} else {
+		//start parsing of bytes
+		u8ToU32(&data[index], &temp);
+		if (temp != FW_HEADER_SIGNATURE) {
+			logError(1, "%s %s:Wrong Signature %08X...ERROR %02X\n",
+				tag, __func__, temp, ERROR_FILE_PARSE);
+			res = ERROR_FILE_PARSE;
+			goto END;
+		}
+
+		logError(0, "%s %s: Fw Signature OK!\n", tag, __func__);
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		if (temp != FW_FTB_VER) {
+			logError(1, "%s %s:Wrong ftb_version %08X.ERROR %02X\n",
+			tag, __func__, temp, ERROR_FILE_PARSE);
+			res = ERROR_FILE_PARSE;
+			goto END;
+		}
+		logError(0, "%s %s:ftb_version OK!\n", __func__, tag);
+		index += FW_BYTES_ALIGN;
+		if (data[index] != DCHIP_ID_0 || data[index+1] != DCHIP_ID_1) {
+			logError(1, "%s %s:Wrong target %02X != %02X ",
+				tag, __func__, data[index]);
+			logError(1, "%%02X != %02X:%08X\n",
+				DCHIP_ID_0, data[index+1],
+				DCHIP_ID_1, ERROR_FILE_PARSE);
+			res = ERROR_FILE_PARSE;
+			goto END;
+		}
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		logError(0, "%s %s: Fw ID = %08X\n", tag, __func__, temp);
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		fwData->fw_ver = temp;
+		logError(0, "%s %s:FILE Fw Version = %04X\n",
+			tag, __func__, fwData->fw_ver);
+
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		fwData->config_id = temp;
+		logError(0, "%s %s:FILE Config ID = %04X\n",
+			tag, __func__, fwData->config_id);
+
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		logError(0, "%s %s:Config Version = %08X\n",
+			tag, __func__, temp);
+		//skip reserved data
+		index += FW_BYTES_ALIGN * 2;
+		index += FW_BYTES_ALIGN;
+		logError(0, "%s %s:File External Release =  ",
+			tag, __func__);
+		for (i = 0; i < EXTERNAL_RELEASE_INFO_SIZE; i++) {
+			fwData->externalRelease[i] = data[index++];
+			logError(0, "%02X", fwData->externalRelease[i]);
+		}
+		logError(0, "\n");
+
+		//index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		fwData->sec0_size = temp;
+		logError(0, "%s %s:sec0_size = %08X (%d bytes)\n",
+			tag, __func__, fwData->sec0_size, fwData->sec0_size);
+
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		fwData->sec1_size = temp;
+		logError(0, "%s %s:sec1_size = %08X (%d bytes)\n",
+			tag, __func__, fwData->sec1_size, fwData->sec1_size);
+
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		fwData->sec2_size = temp;
+		logError(0, "%s %s:sec2_size = %08X (%d bytes)\n",
+			tag, __func__, fwData->sec2_size, fwData->sec2_size);
+
+		index += FW_BYTES_ALIGN;
+		u8ToU32(&data[index], &temp);
+		fwData->sec3_size = temp;
+		logError(0, "%s %s:sec3_size = %08X (%d bytes)\n",
+			tag, __func__, fwData->sec3_size, fwData->sec3_size);
+
+		//skip header crc
+		index += FW_BYTES_ALIGN;
+		if (!keep_cx) {
+			dimension = fwData->sec0_size + fwData->sec1_size
+				+ fwData->sec2_size + fwData->sec3_size;
+			temp = fw_size;
+		} else {
+			//sec2 may contain cx data (future implementation)
+			//sec3 atm not used
+			dimension = fwData->sec0_size + fwData->sec1_size;
+			temp = fw_size - fwData->sec2_size - fwData->sec3_size;
+			fwData->sec2_size = 0;
+			fwData->sec3_size = 0;
+		}
+		if (dimension + FW_HEADER_SIZE + FW_BYTES_ALIGN != temp) {
+			logError(1, "%s %s:Read only %d instead of %d...",
+				tag, __func__, fw_size,
+				dimension + FW_HEADER_SIZE + FW_BYTES_ALIGN);
+			logError(1, "ERROR %02X\n", ERROR_FILE_PARSE);
+			res = ERROR_FILE_PARSE;
+			goto END;
+		}
+		fwData->data = (u8 *)kmalloc_array(dimension, sizeof(u8),
+					GFP_KERNEL);
+		if (fwData->data == NULL) {
+			logError(1, "%s %s: ERROR %02X\n",
+				tag, __func__, ERROR_ALLOC);
+			res = ERROR_ALLOC;
+			goto END;
+		}
+		index += FW_BYTES_ALIGN;
+		memcpy(fwData->data, &data[index], dimension);
+		fwData->data_size = dimension;
+		logError(0, "%s READ FW DONE %d bytes!\n",
+			tag, fwData->data_size);
+		res = OK;
+		goto END;
+	}
+END:
+	kfree(data);
+	return res;
+}
+
+int flash_unlock(void)
+{
+	//write the command to perform the unlock
+	u8 cmd[3] = {FLASH_CMD_UNLOCK,
+		FLASH_UNLOCK_CODE0, FLASH_UNLOCK_CODE1};
+	logError(0, "%s Command unlock ...\n", tag);
+	if (fts_writeCmd(cmd, sizeof(cmd)) < 0) {
+		logError(1, "%s %s: ERROR % 02X\n", tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+	//mdelay(FLASH_WAIT_TIME);
+	logError(0, "%s Unlock flash DONE!\n", tag);
+
+	return OK;
+}
+
+int flash_erase_unlock(void)
+{
+	//write the command to perform
+	//the unlock for erasing the flash
+	u8 cmd[3] = {FLASH_CMD_WRITE_REGISTER, FLASH_ERASE_UNLOCK_CODE0,
+		FLASH_ERASE_UNLOCK_CODE1};
+
+	logError(0, "%s Try to erase unlock flash...\n", tag);
+
+	logError(0, "%s Command erase unlock ...\n", tag);
+	if (fts_writeCmd(cmd, sizeof(cmd)) < 0) {
+		logError(1, "%s %s:ERROR % 02X\n", tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+
+	logError(0, "%s Erase Unlock flash DONE!\n", tag);
+
+	return OK;
+}
+
+int flash_full_erase(void)
+{
+	int status;
+	//write the command to erase the flash
+	u8 cmd[3] = {FLASH_CMD_WRITE_REGISTER, FLASH_ERASE_CODE0,
+			FLASH_ERASE_CODE1};
+
+	logError(0, "%s Command full erase sent...\n",
+		tag);
+	if (fts_writeCmd(cmd, sizeof(cmd)) < 0) {
+		logError(1, "%s %s:ERROR % 02X\n", tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+
+	status = wait_for_flash_ready(FLASH_ERASE_CODE0);
+
+	if (status != OK) {
+		logError(1, "%s %s:ERROR % 02X\n",
+			tag, __func__, ERROR_FLASH_NOT_READY);
+		//Flash not ready within the chosen time,
+		//better exit!
+		return (status | ERROR_FLASH_NOT_READY);
+	}
+
+	logError(0, "%s Full Erase flash DONE!\n", tag);
+
+	return OK;
+}
+
+int flash_erase_page_by_page(int keep_cx)
+{
+	u8 status, i = 0;
+	//write the command to erase the flash
+	u8 cmd[4] = {FLASH_CMD_WRITE_REGISTER, FLASH_ERASE_CODE0, 0x00, 0x00};
+
+	for (i = 0; i < FLASH_NUM_PAGE; i++) {
+		if (i >= FLASH_CX_PAGE_START && i <= FLASH_CX_PAGE_END
+			&& keep_cx == 1) {
+			logError(0, "%s Skipping erase page %d!\n", tag, i);
+			continue;
+		}
+		cmd[2] = (0x3F & i) | FLASH_ERASE_START;
+		logError(0, "Command erase page %d sent", i);
+		logError(0, "%s:%02X %02X %02X %02X\n",
+			tag, i, cmd[0], cmd[1], cmd[2], cmd[3]);
+		if (fts_writeCmd(cmd, sizeof(cmd)) < 0) {
+			logError(1,
+			"%s %s:ERROR % 08X\n",
+			tag, __func__, ERROR_I2C_W);
+			return ERROR_I2C_W;
+		}
+
+		status = wait_for_flash_ready(FLASH_ERASE_CODE0);
+		if (status != OK) {
+			logError(1, "%s %s:ERROR % 08X\n",
+				tag, __func__, ERROR_FLASH_NOT_READY);
+			//Flash not ready within the chosen time,
+			//better exit!
+			return (status | ERROR_FLASH_NOT_READY);
+		}
+	}
+
+	logError(0, "%s Erase flash page by page DONE!\n", tag);
+
+	return OK;
+}
+
+int start_flash_dma(void)
+{
+	int status;
+	//write the command to erase the flash
+	u8 cmd[3] = {FLASH_CMD_WRITE_REGISTER, FLASH_DMA_CODE0,
+			FLASH_DMA_CODE1};
+
+	logError(0, "%s Command flash DMA ...\n", tag);
+	if (fts_writeCmd(cmd, sizeof(cmd)) < 0) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+
+	status = wait_for_flash_ready(FLASH_DMA_CODE0);
+
+	if (status != OK) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_FLASH_NOT_READY);
+		//Flash not ready within the chosen time, better exit!
+		return (status | ERROR_FLASH_NOT_READY);
+	}
+
+	logError(0, "%s flash DMA DONE!\n", tag);
+
+	return OK;
+}
+
+int fillFlash(u32 address, u8 *data, int size)
+{
+	int remaining = size;
+	int toWrite = 0;
+	int byteBlock = 0;
+	int wheel = 0;
+	u32 addr = 0;
+	int res;
+	int delta;
+	u8 *buff = NULL;
+	u8 buff2[9] = {0};
+
+
+	buff = (u8 *)kmalloc_array((DMA_CHUNK + 3), sizeof(u8), GFP_KERNEL);
+	if (buff == NULL) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	while (remaining > 0) {
+		byteBlock = 0;
+		addr = 0;
+		while (byteBlock < FLASH_CHUNK && remaining > 0) {
+			buff[0] = FLASH_CMD_WRITE_64K;
+			if (remaining >= DMA_CHUNK) {
+				if ((byteBlock + DMA_CHUNK) <= FLASH_CHUNK) {
+					//logError(1, "%s fillFlash:1\n", tag);
+					toWrite = DMA_CHUNK;
+					remaining -= DMA_CHUNK;
+					byteBlock += DMA_CHUNK;
+				} else {
+					//logError(1, "%s fillFlash:2\n", tag);
+					delta = FLASH_CHUNK - byteBlock;
+					toWrite = delta;
+					remaining -= delta;
+					byteBlock += delta;
+				}
+			} else {
+				if ((byteBlock + remaining) <= FLASH_CHUNK) {
+					//logError(1, "%s fillFlash:3\n", tag);
+					toWrite = remaining;
+					byteBlock += remaining;
+					remaining = 0;
+				} else {
+					//logError(1, "%s fillFlash:4\n", tag);
+					delta = FLASH_CHUNK - byteBlock;
+					toWrite = delta;
+					remaining -= delta;
+					byteBlock += delta;
+				}
+			}
+
+			buff[1] = (u8) ((addr & 0x0000FF00) >> 8);
+			buff[2] = (u8) (addr & 0x000000FF);
+			memcpy(&buff[3], data, toWrite);
+			//logError(0,
+			//"%s Command = %02X, address = %02X %02X,
+			//bytes = %d, data = %02X %02X, %02X %02X\n",
+			//tag, buff[0], buff[1], buff[2], toWrite,
+			//buff[3], buff[4], buff[3 + toWrite-2],
+			//buff[3 + toWrite-1]);
+			if (fts_writeCmd(buff, 3 + toWrite) < 0) {
+				logError(1, "%s %s: ERROR %02X\n",
+					tag, __func__, ERROR_I2C_W);
+				kfree(buff);
+				return ERROR_I2C_W;
+			}
+
+			addr += toWrite;
+			data += toWrite;
+		}
+		//configuring the DMA
+		byteBlock = byteBlock / 4 - 1;
+
+		buff2[0] = FLASH_CMD_WRITE_REGISTER;
+		buff2[1] = FLASH_DMA_CONFIG;
+		buff2[2] = 0x00;
+		buff2[3] = 0x00;
+
+		addr = address + ((wheel * FLASH_CHUNK)/4);
+		buff2[4] = (u8) ((addr & 0x000000FF));
+		buff2[5] = (u8) ((addr & 0x0000FF00) >> 8);
+		buff2[6] = (u8) (byteBlock & 0x000000FF);
+		buff2[7] = (u8) ((byteBlock & 0x0000FF00) >> 8);
+		buff2[8] = 0x00;
+
+		logError(0, "%s:Command:%02X, address:%02X %02X, ",
+			tag, buff2[0], buff2[5], buff2[4]);
+		logError(0, "words:%02X %02X\n", buff2[7], buff2[6]);
+		if (fts_writeCmd(buff2, 9) < OK) {
+			logError(1, "%s Error during filling Flash!:%02X\n",
+				tag, ERROR_I2C_W);
+			kfree(buff);
+			return ERROR_I2C_W;
+		}
+		//mdelay(FLASH_WAIT_TIME);
+		res = start_flash_dma();
+		if (res < OK) {
+			logError(1, "%s Error during flashing DMA!:%02X\n",
+				tag, res);
+			kfree(buff);
+			return res;
+		}
+		wheel++;
+	}
+	kfree(buff);
+	return OK;
+}
+
+int flash_burn(struct Firmware *fw, int force_burn, int keep_cx)
+{
+	int res;
+
+	if (!force_burn && (ftsInfo.u16_fwVer >= fw->fw_ver)
+		&& (ftsInfo.u16_cfgId >= fw->config_id)) {
+		for (res = EXTERNAL_RELEASE_INFO_SIZE-1; res >= 0; res--) {
+			if (fw->externalRelease[res] >
+				ftsInfo.u8_extReleaseInfo[res])
+				goto start;
+		}
+
+		logError(0, "Firmware in the chip newer or ");
+		logError(0, "equal to the one to burn!");
+		logError(0, "%s %s:NO UPDATE ERROR %02X\n",
+			tag, __func__, ERROR_FW_NO_UPDATE);
+		return (ERROR_FW_NO_UPDATE | ERROR_FLASH_BURN_FAILED);
+	}
+
+	//programming procedure start
+start:
+	logError(0, "%s Programming Procedure for flashing started:\n\n", tag);
+
+	logError(0, "%s 1) SYSTEM RESET:\n", tag);
+
+	logError(0, "%s 2) WARM BOOT:\n", tag);
+	res = fts_warm_boot();
+	if (res < OK) {
+		logError(1, "%s warm boot FAILED!\n", tag);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s warm boot COMPLETED!\n\n", tag);
+
+	//mdelay(FLASH_WAIT_TIME);
+	logError(0, "%s 3) FLASH UNLOCK:\n", tag);
+	res = flash_unlock();
+	if (res < OK) {
+		logError(1, "%s flash unlock FAILED! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s flash unlock COMPLETED!\n\n", tag);
+
+	//mdelay(200);
+	logError(0, "%s 4) FLASH ERASE UNLOCK:\n", tag);
+	res = flash_erase_unlock();
+	if (res < 0) {
+		logError(1, "%s flash unlock FAILED! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s flash unlock COMPLETED!\n\n", tag);
+
+	//mdelay(FLASH_WAIT_TIME);
+	logError(0, "%s 5) FLASH ERASE:\n", tag);
+	if (keep_cx == 1)
+		res = flash_erase_page_by_page(keep_cx);
+	else
+		res = flash_full_erase();
+	if (res < 0) {
+		logError(1, "%s flash erase FAILED! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s flash erase COMPLETED!\n\n", tag);
+
+
+	//mdelay(FLASH_WAIT_TIME);
+	logError(0, "%s 6) LOAD PROGRAM:\n", tag);
+	res = fillFlash(FLASH_ADDR_CODE, (u8 *)(&fw->data[0]),
+					fw->sec0_size);
+	if (res < OK) {
+		logError(1, "%s   load program ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s   load program DONE!\n", tag);
+	logError(0, "%s 7) LOAD CONFIG:\n", tag);
+	res = fillFlash(FLASH_ADDR_CONFIG,
+		&(fw->data[fw->sec0_size]), fw->sec1_size);
+	if (res < OK) {
+		logError(1, "%s   load config ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s   load config DONE!\n", tag);
+
+	logError(0, "%s   Flash burn COMPLETED!\n\n", tag);
+
+	logError(0, "%s 8) SYSTEM RESET:\n", tag);
+	res = fts_system_reset();
+	if (res < 0) {
+		logError(1, "%s system reset FAILED! ERROR %02X\n",
+			tag, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+	logError(0, "%s   system reset COMPLETED!\n\n", tag);
+
+
+	logError(0, "%s 9) FINAL CHECK:\n", tag);
+	res = readChipInfo(0);
+	if (res < 0) {
+		logError(1, "%s %s:Unable to retrieve Chip INFO!:%02X\n",
+			tag, __func__, ERROR_FLASH_BURN_FAILED);
+		return (res | ERROR_FLASH_BURN_FAILED);
+	}
+
+	if ((ftsInfo.u16_fwVer != fw->fw_ver)
+		&& (ftsInfo.u16_cfgId != fw->config_id)) {
+		pr_err("Firmware is different from the old!\n");
+		logError(1, "%s fw: %x != %x, conf: %x != %x\n",
+			tag, ftsInfo.u16_fwVer, fw->fw_ver,
+			ftsInfo.u16_cfgId, fw->config_id);
+		return ERROR_FLASH_BURN_FAILED;
+	}
+
+	logError(0, "%s Final check OK! fw: %02X , conf: %02X\n",
+		tag, ftsInfo.u16_fwVer, ftsInfo.u16_cfgId);
+
+	return OK;
+}
+
+#endif

st/fts_lib/ftsFlash.h

@@ -0,0 +1,109 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        FTS API for Flashing the IC                     *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#ifndef __FTS_FLASH_H
+#define __FTS_FLASH_H
+
+
+#include "ftsSoftware.h"
+
+//Flash possible status
+#define FLASH_READY              0
+#define FLASH_BUSY               1
+#define FLASH_UNKNOWN           -1
+#define FLASH_STATUS_BYTES       1
+
+
+//Flash timing parameters
+#define FLASH_RETRY_COUNT        1000
+#define FLASH_WAIT_BEFORE_RETRY  50   //ms
+#define FLASH_WAIT_TIME          200  //ms
+
+
+//PATHS FW FILES
+//#define PATH_FILE_FW           "fw.memh"
+#ifdef FTM3_CHIP
+#define PATH_FILE_FW             "st_fts.bin"
+#else
+#define PATH_FILE_FW             "st_fts.ftb"//new bin file structure
+#endif
+
+#ifndef FTM3_CHIP
+#define FLASH_CHUNK              (64 * 1024)
+#define DMA_CHUNK                (2 * 1024)
+#endif
+
+
+struct Firmware {
+	u8 *data;
+	u16 fw_ver;
+	u16 config_id;
+	u8 externalRelease[EXTERNAL_RELEASE_INFO_SIZE];
+	int data_size;
+#ifndef FTM3_CHIP
+	u32 sec0_size;
+	u32 sec1_size;
+	u32 sec2_size;
+	u32 sec3_size;
+#endif
+};
+
+#ifdef FTM3_CHIP
+int flash_status(void);
+int flash_status_ready(void);
+int wait_for_flash_ready(void);
+#else
+int wait_for_flash_ready(u8 type);
+int fts_warm_boot(void);
+int flash_erase_unlock(void);
+int flash_full_erase(void);
+int flash_erase_page_by_page(int keep_cx);
+//int flash_erase_page_by_page_info(int page);
+int start_flash_dma(void);
+int fillFlash(u32 address, u8 *data, int size);
+#endif
+
+int flash_unlock(void);
+int fillMemory(u32 address, u8 *data, int size);
+int getFirmwareVersion(u16 *fw_vers, u16 *config_id);
+int getFWdata(const char *pathToFile, u8 **data, int *size, int from);
+int getFWdata_nocheck(const char *pathToFile, u8 **data, int *size, int from);
+int parseBinFile(u8 *fw_data, int fw_size, struct Firmware *fw, int keep_cx);
+int readFwFile(const char *path, struct Firmware *fw, int keep_cx);
+int flash_burn(struct Firmware *fw, int force_burn, int keep_cx);
+int flashProcedure(const char *path, int force, int keep_cx);
+
+#endif

st/fts_lib/ftsFrame.c

@@ -0,0 +1,519 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                  FTS functions for getting frames                      *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <stdarg.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/time.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+//#include <linux/sec_sysfs.h>
+
+#include "ftsCrossCompile.h"
+#include "ftsCompensation.h"
+#include "ftsError.h"
+#include "ftsFrame.h"
+#include "ftsHardware.h"
+#include "ftsIO.h"
+#include "ftsSoftware.h"
+#include "ftsTool.h"
+#include "ftsTime.h"
+#include "../fts.h"
+
+static char tag[8] = "[ FTS ]\0";
+static int sense_len, force_len;
+
+int getOffsetFrame(u16 address, u16 *offset)
+{
+	u8 data[2];
+	u8 cmd = { FTS_CMD_FRAMEBUFFER_R };
+	char *temp = NULL;
+
+	if (readCmdU16(cmd, address, data, OFFSET_LENGTH,
+		DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s %S: ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+
+	u8ToU16(data, offset);
+	temp = printHex("Offest = ", data, OFFSET_LENGTH);
+	if (temp != NULL)
+		logError(0, "%s %s", tag, temp);
+	kfree(temp);
+	return OK;
+}
+
+int getChannelsLength(void)
+{
+	int ret;
+	u8 *data = (u8 *)kmalloc_array(2, sizeof(u8), GFP_KERNEL);
+
+	if (data == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	ret = readB2(ADDR_SENSE_LEN, data, 2);
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_READ_B2);
+		kfree(data);
+		return (ret|ERROR_READ_B2);
+	}
+
+	sense_len = (int)data[0];
+	force_len = (int)data[1];
+
+	logError(0, "%s Force_len = %d Sense_Len = %d\n",
+		tag, force_len, sense_len);
+	kfree(data);
+
+	return OK;
+}
+
+
+int getFrameData(u16 address, int size, short **frame)
+{
+	int i, j, ret;
+	u8 *data = (u8 *)kmalloc_array(size, sizeof(u8), GFP_KERNEL);
+
+	if (data == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+	ret = readCmdU16(FTS_CMD_FRAMEBUFFER_R, address,
+			data, size, DUMMY_FRAMEBUFFER);
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_I2C_R);
+		kfree(data);
+		return ERROR_I2C_R;
+	}
+	j = 0;
+	for (i = 0; i < size; i += 2) {
+		(*frame)[j] = (short)((data[i + 1] << 8) + data[i]);
+		j++;
+	}
+	kfree(data);
+	return OK;
+}
+
+
+int getMSFrame(u16 type, struct MutualSenseFrame *frame, int keep_first_row)
+{
+	u16 offset;
+	int ret;
+
+	if (getSenseLen() == 0 || getForceLen() == 0) {
+		ret = getChannelsLength();
+		if (ret < OK) {
+			logError(1, "%s %s: ERROR %02X\n",
+				tag, __func__, ERROR_CH_LEN);
+			return (ret|ERROR_CH_LEN);
+		}
+	}
+
+	ret = getOffsetFrame(type, &offset);
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_GET_OFFSET);
+		return (ret | ERROR_GET_OFFSET);
+	}
+
+	switch (type) {
+	case ADDR_RAW_TOUCH:
+	case ADDR_FILTER_TOUCH:
+	case ADDR_NORM_TOUCH:
+	case ADDR_CALIB_TOUCH:
+		if (keep_first_row == 1) {
+			frame->node_data_size = ((force_len + 1) * sense_len);
+			frame->header.force_node = force_len + 1;
+		} else {
+			frame->node_data_size = ((force_len) * sense_len);
+			offset += (sense_len * BYTES_PER_NODE);
+			frame->header.force_node = force_len;
+		}
+		frame->header.sense_node = sense_len;
+		break;
+	case ADDR_NORM_MS_KEY:
+	case ADDR_RAW_MS_KEY:
+		frame->header.force_node = 1;
+		frame->header.sense_node = ftsInfo.u8_msKeyLen;
+		frame->node_data_size = ftsInfo.u8_msKeyLen;
+		break;
+	default:
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+	frame->node_data = (short *)kmalloc_array(frame->node_data_size,
+			sizeof(short), GFP_KERNEL);
+	if (frame->node_data == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	ret = getFrameData(offset,
+		frame->node_data_size * BYTES_PER_NODE,
+		&(frame->node_data));
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_GET_FRAME_DATA);
+		kfree(frame->node_data);
+		return (ret | ERROR_GET_FRAME_DATA);
+	}
+	// if you want to access one node i,j,
+	//you should compute the offset like:
+	//offset = i * columns + j => frame[i, j]
+	logError(0, "%s Frame acquired!\n", tag);
+	//return the number of data put inside frame
+	return frame->node_data_size;
+}
+
+int getSenseLen(void)
+{
+	int ret;
+
+	if (sense_len != 0)
+		return sense_len;
+
+	if (ftsInfo.u8_scrSenseLen != 0) {
+		sense_len = ftsInfo.u8_scrSenseLen;
+	} else {
+		ret = getChannelsLength();
+		if (ret < OK)
+			return ret;
+	}
+	return sense_len;
+}
+
+int getForceLen(void)
+{
+	int ret;
+
+	if (force_len != 0)
+		return force_len;
+
+	if (ftsInfo.u8_scrForceLen != 0) {
+		force_len = ftsInfo.u8_scrForceLen;
+	} else {
+		ret = getChannelsLength();
+		if (ret < OK)
+			return ret;
+	}
+	return force_len;
+}
+
+int requestFrame(u16 type)
+{
+	int retry = 0;
+	int ret;
+	u16 answer;
+	char *temp = NULL;
+
+	int event_to_search[1];
+	u8 readEvent[FIFO_EVENT_SIZE];
+
+	u8 cmd[3] = { FTS_CMD_REQU_FRAME_DATA, 0x00, 0x00};
+	// B7 is the command for asking frame data
+	event_to_search[0] = (int)EVENTID_FRAME_DATA_READ;
+
+
+	u16ToU8(type, &cmd[1]);
+
+	while (retry < FRAME_DATA_READ_RETRY) {
+		temp = printHex("Command = ", cmd, 3);
+		if (temp != NULL)
+			logError(0, "%s %s", tag, temp);
+		kfree(temp);
+
+		//send the request to the chip to load in memory the Frame Data
+		ret = fts_writeFwCmd(cmd, 3);
+		if (ret < OK) {
+			logError(1, "%s %s:  ERROR %02X\n",
+				tag, __func__, ERROR_I2C_W);
+			return ERROR_I2C_W;
+		}
+		ret = pollForEvent(event_to_search,
+			1,
+			readEvent,
+			TIMEOUT_REQU_COMP_DATA);
+		if (ret < OK) {
+			logError(0, "%s Event did not Found at %d attemp!\n",
+				tag, retry + 1);
+			retry += 1;
+		} else {
+			retry = 0;
+			break;
+		}
+	}
+	if (retry == FRAME_DATA_READ_RETRY) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag,  __func__, ERROR_TIMEOUT);
+		return ERROR_TIMEOUT;
+	}
+	u8ToU16_le(&readEvent[1], &answer);
+
+	if (answer == type)
+		return OK;
+
+	logError(1, "%s The event found has a different type of ", tag);
+	logError(1, "Frame data:%02X\n", ERROR_DIFF_COMP_TYPE);
+	return ERROR_DIFF_COMP_TYPE;
+
+}
+
+
+int readFrameDataHeader(u16 type, struct DataHeader *header)
+{
+	u16 offset = ADDR_FRAMEBUFFER_DATA;
+	u16 answer;
+	u8 data[FRAME_DATA_HEADER];
+
+	if (readCmdU16(FTS_CMD_FRAMEBUFFER_R, offset, data,
+		FRAME_DATA_HEADER, DUMMY_FRAMEBUFFER) < 0) {
+		logError(1, "%s  %s: ERROR %02X\n",
+			tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+	logError(0, "%s Read Data Header done!\n", tag);
+
+	if (data[0] != FRAME_HEADER_SIGNATURE) {
+		logError(1, "%s %s %02X Wrong Header Signature !%02X != %02X\n",
+			tag, __func__, ERROR_WRONG_COMP_SIGN, data[0],
+			HEADER_SIGNATURE);
+		return ERROR_WRONG_COMP_SIGN;
+	}
+
+	u8ToU16_le(&data[1], &answer);
+
+	if (answer != type) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_DIFF_COMP_TYPE);
+		return ERROR_DIFF_COMP_TYPE;
+	}
+
+	logError(0, "%s Type of Frame data OK!\n", tag);
+
+	header->type = type;
+	header->force_node = (int)data[4];
+	header->sense_node = (int)data[5];
+
+	return OK;
+}
+
+int getMSFrame2(u16 type, struct MutualSenseFrame *frame)
+{
+	u16 offset = ADDR_FRAMEBUFFER_DATA+FRAME_DATA_HEADER;
+	int size, ret;
+
+	frame->node_data = NULL;
+
+	if (!(type == MS_TOUCH_ACTIVE || type == MS_TOUCH_LOW_POWER
+		|| type == MS_TOUCH_ULTRA_LOW_POWER
+		|| type == MS_KEY)) {
+		logError(1, "%s %s:Choose a MS type of frame data ERROR %02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	ret = requestFrame(type);
+	if (ret < 0) {
+		logError(1, "%s readMutualSenseCompensation:ERROR %02X\n",
+			tag, ERROR_REQU_COMP_DATA);
+		return (ret | ERROR_REQU_COMP_DATA);
+	}
+
+	ret = readFrameDataHeader(type, &(frame->header));
+	if (ret < 0) {
+		logError(1, "%s readMutualSenseCompensationData:ERROR %02X\n",
+			tag, ERROR_COMP_DATA_HEADER);
+		return (ret | ERROR_COMP_DATA_HEADER);
+	}
+
+	switch (type) {
+	case MS_TOUCH_ACTIVE:
+	case MS_TOUCH_LOW_POWER:
+	case MS_TOUCH_ULTRA_LOW_POWER:
+		size = frame->header.force_node * frame->header.sense_node;
+		break;
+	case MS_KEY:
+		//or use directly the number in the ftsChip
+		if (frame->header.force_node > frame->header.sense_node)
+			size = frame->header.force_node;
+		else
+			size = frame->header.sense_node;
+		frame->header.force_node = 1;
+		frame->header.sense_node = size;
+		break;
+
+	default:
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	frame->node_data = (short *)kmalloc_array(size,
+				sizeof(short), GFP_KERNEL);
+	if (frame->node_data == NULL) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	ret = getFrameData(offset, size*BYTES_PER_NODE, &(frame->node_data));
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_GET_FRAME_DATA);
+		kfree(frame->node_data);
+		return (ret | ERROR_GET_FRAME_DATA);
+	}
+	// if you want to access one node i,j,
+	//you should compute the offset like:
+	//offset = i * columns + j = > frame[i, j]
+	logError(0, "%s Frame acquired!\n", tag);
+	frame->node_data_size = size;
+	return size;//return the number of data put inside frame
+
+}
+
+int getSSFrame2(u16 type, struct SelfSenseFrame *frame)
+{
+	u16 offset = ADDR_FRAMEBUFFER_DATA + FRAME_DATA_HEADER;
+	int size, ret;
+	short *temp = NULL;
+
+	frame->force_data = NULL;
+	frame->sense_data = NULL;
+
+	if (!(type == SS_TOUCH || type == SS_KEY || type == SS_HOVER
+		|| type == SS_PROXIMITY)) {
+		logError(1, "%s %s:Choose a SS type of frame data ERROR %02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	ret = requestFrame(type);
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_REQU_COMP_DATA);
+		return (ret | ERROR_REQU_COMP_DATA);
+	}
+
+	ret = readFrameDataHeader(type, &(frame->header));
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_COMP_DATA_HEADER);
+		return (ret | ERROR_COMP_DATA_HEADER);
+	}
+
+	switch (type) {
+	case SS_TOUCH:
+	case SS_HOVER:
+	case SS_PROXIMITY:
+		size = frame->header.force_node + frame->header.sense_node;
+		break;
+
+	default:
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+
+	temp = (short *)kmalloc_array(size, sizeof(short), GFP_KERNEL);
+	if (temp == NULL) {
+		logError(1, "%s %s: temp ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	ret = getFrameData(offset, size*BYTES_PER_NODE, &temp);
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_GET_FRAME_DATA);
+		kfree(temp);
+		return (ret | ERROR_GET_FRAME_DATA);
+	}
+
+	frame->force_data = (short *)kmalloc_array(frame->header.force_node,
+			sizeof(short), GFP_KERNEL);
+	if (frame->force_data == NULL) {
+		logError(1, "%s %s: frame->force_data ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		kfree(temp);
+		return ERROR_ALLOC;
+	}
+
+	memcpy(frame->force_data, temp,
+		frame->header.force_node * sizeof(short));
+
+	frame->sense_data = (short *)kmalloc_array(frame->header.sense_node,
+			sizeof(short), GFP_KERNEL);
+	if (frame->sense_data == NULL) {
+		logError(1, "%s %s: frame->sense_data ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		kfree(temp);
+		kfree(frame->force_data);
+		return ERROR_ALLOC;
+	}
+
+	memcpy(frame->sense_data, &temp[frame->header.force_node],
+		frame->header.sense_node * sizeof(short));
+
+	logError(0, "%s Frame acquired!\n", tag);
+	kfree(temp);
+	return size; //return the number of data put inside frame
+}

st/fts_lib/ftsFrame.h

@@ -0,0 +1,76 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                  FTS functions for getting frames                      *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#ifndef __FTS_FRAME_H
+#define __FTS_FRAME_H
+
+#include "ftsSoftware.h"
+
+//Number of data bytes for each node
+#define BYTES_PER_NODE                 2
+#define OFFSET_LENGTH                  2
+#define FRAME_DATA_HEADER              8
+#define FRAME_HEADER_SIGNATURE         0xB5
+#define FRAME_DATA_READ_RETRY          2
+
+struct MutualSenseFrame {
+	struct DataHeader header;
+	short *node_data;
+	int node_data_size;
+};
+
+struct SelfSenseFrame {
+	struct DataHeader header;
+	short *force_data;
+	short *sense_data;
+};
+
+int getOffsetFrame(u16 address, u16 *offset);
+int getChannelsLength(void);
+int getFrameData(u16 address, int size, short **frame);
+int getMSFrame(u16 type, struct MutualSenseFrame *frame, int keep_first_row);
+//int getMSKeyFrame(u16 type, short **frame);
+//int getSSFrame(u16 type, short **frame);
+//int getNmsFrame(u16 type, short ***frames, int * sizes,
+//int keep_first_row, int fs, int n);
+int getSenseLen(void);
+int getForceLen(void);
+int requestFrame(u16 type);
+int readFrameDataHeader(u16 type, struct DataHeader *header);
+int getMSFrame2(u16 type, struct MutualSenseFrame *frame);
+int getSSFrame2(u16 type, struct SelfSenseFrame *frame);
+
+#endif
+

st/fts_lib/ftsGesture.c

@@ -0,0 +1,651 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                     FTS Gesture Utilities                              *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#include "ftsSoftware.h"
+#include "ftsError.h"
+#include "ftsGesture.h"
+#include "ftsIO.h"
+#include "ftsTool.h"
+
+
+static char tag[8] = "[ FTS ]\0";
+
+static u8 gesture_mask[GESTURE_MASK_SIZE] = { 0 };
+static u8 custom_gestures[GESTURE_CUSTOM_NUMBER][GESTURE_CUSTOM_POINTS];
+static u8 custom_gesture_index[GESTURE_CUSTOM_NUMBER] = { 0 };
+static int refreshGestureMask;
+
+u16 gesture_coordinates_x[GESTURE_COORDS_REPORT_MAX] = {0};
+u16 gesture_coordinates_y[GESTURE_COORDS_REPORT_MAX] = {0};
+int gesture_coords_reported = ERROR_OP_NOT_ALLOW;
+struct mutex gestureMask_mutex;
+
+
+int updateGestureMask(u8 *mask, int size, int en)
+{
+	u8 temp;
+	int i;
+
+	if (mask == NULL) {
+		logError(1, "%s %s: Mask NULL! ERROR %08X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	if (size > GESTURE_MASK_SIZE) {
+		logError(1, "%s %s:Size not valid! %d > %d ERROR %08X\n",
+			tag, __func__, size, GESTURE_MASK_SIZE);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	if (en == FEAT_ENABLE) {
+		mutex_lock(&gestureMask_mutex);
+		logError(0, "%s %s:setting gesture mask to enable..\n",
+			tag, __func__);
+		if (mask != NULL) {
+			for (i = 0; i < size; i++) {
+				//back up the gesture enabled
+				gesture_mask[i] = gesture_mask[i] | mask[i];
+			}
+		}
+		refreshGestureMask = 1;
+		logError(0, "%s %s:gesture mask to enable SET!\n",
+			tag, __func__);
+		mutex_unlock(&gestureMask_mutex);
+		return OK;
+	}
+	if (en == FEAT_DISABLE) {
+		mutex_lock(&gestureMask_mutex);
+		logError(0, "%s %s:setting gesture ", tag, __func__);
+		logError(0, "mask to disable...\n");
+		for (i = 0; i < size; i++) {
+			// enabled XOR disabled
+			temp = gesture_mask[i] ^ mask[i];
+			gesture_mask[i] = temp & gesture_mask[i];
+		}
+		logError(0, "%s %s:gesture mask to disable SET!\n",
+			tag, __func__);
+		refreshGestureMask = 1;
+		mutex_unlock(&gestureMask_mutex);
+		return OK;
+	}
+	logError(1, "%s%s:Enable parameter Invalid%d!=%d or%d%:08X",
+		tag, __func__, FEAT_DISABLE,
+	FEAT_ENABLE, ERROR_OP_NOT_ALLOW);
+	return ERROR_OP_NOT_ALLOW;
+
+}
+
+int enableGesture(u8 *mask, int size)
+{
+	u8 cmd[GESTURE_MASK_SIZE + 2];
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	int i, res;
+	int event_to_search[4] = { EVENTID_GESTURE,
+		EVENT_TYPE_ENB, 0x00, GESTURE_ENABLE };
+
+	logError(0, "%s Trying to enable gesture...\n", tag);
+	cmd[0] = FTS_CMD_GESTURE_CMD;
+	cmd[1] = GESTURE_ENABLE;
+
+
+	if (size > GESTURE_MASK_SIZE) {
+		logError(1, "%s %s: Size not valid! %d > %d ERROR %08X\n",
+			tag, __func__, size, GESTURE_MASK_SIZE);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	mutex_lock(&gestureMask_mutex);
+	if (mask != NULL) {
+		for (i = 0; i < size; i++) {
+			cmd[i + 2] = mask[i];
+			//back up of the gesture enabled
+			gesture_mask[i] = gesture_mask[i] | mask[i];
+		}
+		while (i < GESTURE_MASK_SIZE) {
+			cmd[i + 2] = gesture_mask[i];
+			i++;
+		}
+	} else {
+		for (i = 0; i < GESTURE_MASK_SIZE; i++)
+			cmd[i + 2] = gesture_mask[i];
+	}
+
+	res = fts_writeFwCmd(cmd, GESTURE_MASK_SIZE + 2);
+	if (res < OK) {
+		logError(1, "%s %s: ERROR %08X\n", tag, __func__, res);
+		goto END;
+	}
+
+	res = pollForEvent(event_to_search, 4,
+			readData, GENERAL_TIMEOUT);
+	if (res < OK) {
+		logError(1, "%s %s: pollForEvent ERROR %08X\n",
+			tag, __func__, res);
+		goto END;
+	}
+
+	if (readData[4] != 0x00) {
+		logError(1, "%s %s: ERROR %08X\n",
+			tag, __func__, ERROR_GESTURE_ENABLE_FAIL);
+		res = ERROR_GESTURE_ENABLE_FAIL;
+		goto END;
+	}
+
+	logError(0, "%s %s: DONE!\n", tag, __func__);
+	res = OK;
+
+END:
+	mutex_unlock(&gestureMask_mutex);
+	return res;
+}
+
+
+int disableGesture(u8 *mask, int size)
+{
+	u8 cmd[2 + GESTURE_MASK_SIZE];
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	u8 temp;
+	int i, res;
+	int event_to_search[4] = { EVENTID_GESTURE,
+			EVENT_TYPE_ENB, 0x00, GESTURE_DISABLE };
+
+	logError(0, "%s Trying to disable gesture...\n", tag);
+	cmd[0] = FTS_CMD_GESTURE_CMD;
+	cmd[1] = GESTURE_DISABLE;
+
+	if (size > GESTURE_MASK_SIZE) {
+		logError(1, "%s %s: Size not valid! %d > %d ERROR %08X\n",
+			tag, __func__, size, GESTURE_MASK_SIZE);
+		return ERROR_OP_NOT_ALLOW;
+	}
+	mutex_lock(&gestureMask_mutex);
+	if (mask != NULL) {
+		for (i = 0; i < size; i++) {
+			cmd[i + 2] = mask[i];
+			// enabled XOR disabled
+			temp = gesture_mask[i] ^ mask[i];
+			gesture_mask[i] = temp & gesture_mask[i];
+		}
+		while (i < GESTURE_MASK_SIZE) {
+			//cmd[i + 2] = gesture_mask[i];
+			//gesture_mask[i] = 0x00;
+
+			cmd[i + 2] = 0x00;
+			//leave untouched the gestures not specified
+
+			i++;
+		}
+	} else {
+		for (i = 0; i < GESTURE_MASK_SIZE; i++) {
+			//cmd[i + 2] = gesture_mask[i];
+			cmd[i + 2] = 0xFF;
+		}
+	}
+
+	res = fts_writeFwCmd(cmd, 2 + GESTURE_MASK_SIZE);
+	if (res < OK) {
+		logError(1, "%s %s:ERROR %08X\n", tag, __func__, res);
+		goto END;
+	}
+
+	res = pollForEvent(event_to_search, 4, readData, GENERAL_TIMEOUT);
+	if (res < OK) {
+		logError(1, "%s %s: pollForEvent ERROR %08X\n",
+			tag, __func__, res);
+		goto END;
+	}
+
+	if (readData[4] != 0x00) {
+		logError(1, "%s %s:ERROR %08X\n",
+			tag, __func__, ERROR_GESTURE_ENABLE_FAIL);
+		res = ERROR_GESTURE_ENABLE_FAIL;
+		goto END;
+	}
+
+	logError(0, "%s %s: DONE!\n", tag, __func__);
+	res = OK;
+END:
+	mutex_unlock(&gestureMask_mutex);
+	return res;
+
+}
+
+int startAddCustomGesture(u8 gestureID)
+{
+	u8 cmd[3] = { FTS_CMD_GESTURE_CMD, GESTURE_START_ADD, gestureID };
+	int res;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	int event_to_search[4] = { EVENTID_GESTURE, EVENT_TYPE_ENB,
+				gestureID, GESTURE_START_ADD };
+
+	res = fts_writeFwCmd(cmd, 3);
+	if (res < OK) {
+		logError(1,
+		"%s%s:Impossible to start adding custom gesture ID:%02X %08X\n",
+			tag, __func__, gestureID, res);
+		return res;
+	}
+
+	res = pollForEvent(event_to_search, 4, readData, GENERAL_TIMEOUT);
+	if (res < OK) {
+		logError(1, "%s %s:start add event not found! ERROR %08X\n",
+			tag, __func__, res);
+		return res;
+	}
+	//check of gestureID is redundant
+	if (readData[2] != gestureID || readData[4] != 0x00) {
+		logError(1, "%s %s:start add event status not OK! ERROR %08X\n",
+			tag, __func__, readData[4]);
+		return ERROR_GESTURE_START_ADD;
+	}
+
+	return OK;
+}
+
+int finishAddCustomGesture(u8 gestureID)
+{
+	u8 cmd[3] = { FTS_CMD_GESTURE_CMD,
+			GESTURE_FINISH_ADD,  gestureID };
+	int res;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	int event_to_search[4] = { EVENTID_GESTURE, EVENT_TYPE_ENB,
+				gestureID, GESTURE_FINISH_ADD };
+
+	res = fts_writeFwCmd(cmd, 3);
+	if (res < OK) {
+		logError(1,
+		"%s%s:Impossible to finish adding custom gestureID:%02X %08X\n",
+			tag, __func__, gestureID, res);
+		return res;
+	}
+
+	res = pollForEvent(event_to_search, 4, readData, GENERAL_TIMEOUT);
+	if (res < OK) {
+		logError(1, "%s %s: finish add event not found! ERROR %08X\n",
+			tag, __func__, res);
+		return res;
+	}
+	//check of gestureID is redundant
+	if (readData[2] != gestureID || readData[4] != 0x00) {
+		logError(1,
+			"%s %s:finish add event status not OK! ERROR %08X\n",
+			tag, __func__, readData[4]);
+		return ERROR_GESTURE_FINISH_ADD;
+	}
+
+	return OK;
+}
+
+int loadCustomGesture(u8 *template, u8 gestureID)
+{
+	int res, i, wheel;
+	int remaining = GESTURE_CUSTOM_POINTS;
+	int toWrite, offset = 0;
+	u8 cmd[TEMPLATE_CHUNK + 5];
+	int event_to_search[4] = { EVENTID_GESTURE, EVENT_TYPE_ENB,
+				gestureID, GESTURE_DATA_ADD };
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+
+	logError(0, "%s Starting adding custom gesture procedure...\n", tag);
+
+	res = startAddCustomGesture(gestureID);
+	if (res < OK) {
+		logError(1, "%s %s:unable to start adding procedure %08X\n",
+			tag, __func__, res);
+		return res;
+	}
+
+	cmd[0] = FTS_CMD_GESTURE_CMD;
+	cmd[1] = GESTURE_DATA_ADD;
+	cmd[2] = gestureID;
+	wheel = 0;
+	while (remaining > 0) {
+		if (remaining > TEMPLATE_CHUNK)
+			toWrite = TEMPLATE_CHUNK;
+		else
+			toWrite = remaining;
+
+		cmd[3] = toWrite;
+		cmd[4] = offset;
+		for (i = 0; i < toWrite; i++)
+			cmd[i + 5] = template[wheel++];
+
+		res = fts_writeFwCmd(cmd, toWrite + 5);
+		if (res < OK) {
+			logError(1, "%s %s:unable to start ", tag, __func__);
+			logError(1, "adding procedure %08X\n", res);
+			return res;
+		}
+
+		res = pollForEvent(event_to_search,
+			4,
+			readData,
+			GENERAL_TIMEOUT);
+		if (res < OK) {
+			logError(1, "%s %s: add event not found! ERROR %08X\n",
+				tag, __func__, res);
+			return res;
+		}
+		//check of gestureID is redundant
+		if (readData[2] != gestureID || readData[4] != 0x00) {
+			logError(1, "%s %s:add event status not OK! ",
+				tag, __func__);
+			logError(1, "ERROR %08X\n", readData[4]);
+			return ERROR_GESTURE_DATA_ADD;
+		}
+
+		remaining -= toWrite;
+		offset += toWrite / 2;
+	}
+
+	res = finishAddCustomGesture(gestureID);
+	if (res < OK) {
+		logError(1, "%s %s:unable to finish adding procedure! ",
+			tag, __func__);
+		logError(1, "ERROR %08X\n", res);
+		return res;
+	}
+
+	logError(0, "%s Adding custom gesture procedure DONE!\n", tag);
+	return OK;
+
+}
+
+
+int reloadCustomGesture(void)
+{
+	int res, i;
+
+	logError(0, "%s Starting reload Gesture Template...\n", tag);
+
+	for (i = 0; i < GESTURE_CUSTOM_NUMBER; i++) {
+		if (custom_gesture_index[i] == 1) {
+			res = loadCustomGesture(custom_gestures[i],
+				GESTURE_CUSTOM_OFFSET + i);
+			if (res < OK) {
+				logError(1, "%s %s:Impossible load gesture ",
+					tag, __func__);
+				logError(1, "D:%02X %08X\n",
+					GESTURE_CUSTOM_OFFSET + i, res);
+				return res;
+			}
+		}
+	}
+
+	logError(0, "%s Reload Gesture Template DONE!\n", tag);
+	return OK;
+
+}
+
+int enterGestureMode(int reload)
+{
+	u8 cmd = FTS_CMD_GESTURE_MODE;
+	int res, ret;
+
+	res = fts_disableInterrupt();
+	if (res < OK) {
+		logError(1, "%s %s: ERROR %08X\n",
+			tag, __func__, res | ERROR_DISABLE_INTER);
+		return res | ERROR_DISABLE_INTER;
+	}
+
+	if (reload == 1 || refreshGestureMask == 1) {
+		if (reload == 1) {
+			res = reloadCustomGesture();
+			if (res < OK) {
+				logError(1, "%s %s:impossible reload ",
+					tag, __func__);
+				logError(1, "custom gesture %08X\n", res);
+				goto END;
+			}
+		}
+
+		/**
+		 * mandatory steps to set the correct gesture
+		 * mask defined by the user
+		 */
+		res = disableGesture(NULL, 0);
+		if (res < OK) {
+			logError(1, "%s %s:disableGesture ERROR %08X\n",
+				tag, res);
+			goto END;
+		}
+
+		res = enableGesture(NULL, 0);
+		if (res < OK) {
+			logError(1, "%s %s:enableGesture ERROR %08X\n",
+				tag, __func__, res);
+			goto END;
+		}
+
+		refreshGestureMask = 0;
+		/**************************************************/
+	}
+
+	res = fts_writeFwCmd(&cmd, 1);
+	if (res < OK) {
+		logError(1, "%s %s:enter gesture mode ERROR %08X\n",
+			tag, __func__, res);
+		goto END;
+	}
+
+	res = OK;
+END:
+	ret = fts_enableInterrupt();
+	if (ret < OK) {
+		logError(1, "%s %s:fts_enableInterrupt ERROR %08X\n",
+			tag, __func__, res | ERROR_ENABLE_INTER);
+		res |= ret | ERROR_ENABLE_INTER;
+	}
+
+	return res;
+}
+
+int addCustomGesture(u8 *data, int size, u8 gestureID)
+{
+	int index, res, i;
+
+	index = gestureID - GESTURE_CUSTOM_OFFSET;
+
+	logError(0, "%s Starting Custom Gesture Adding procedure...\n", tag);
+	if (size != GESTURE_CUSTOM_POINTS || (gestureID != GES_ID_CUST1
+		&& gestureID != GES_ID_CUST2 && gestureID != GES_ID_CUST3
+		&& gestureID != GES_ID_CUST4 && gestureID != GES_ID_CUST5)) {
+		logError(1, "%s %s:Invalid size(%d) or Custom GestureID ",
+			tag, __func__, size);
+		logError(1, "(%02X)!ERROR%08X\n",
+			size, gestureID, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	for (i = 0; i < GESTURE_CUSTOM_POINTS; i++)
+		custom_gestures[index][i] = data[i];
+
+	res = loadCustomGesture(custom_gestures[index], gestureID);
+	if (res < OK) {
+		logError(1, "%s %s:impossible to load the custom gesture! ",
+			tag, __func__);
+		logError(1, "ERROR %08X\n", res);
+		return res;
+	}
+
+	custom_gesture_index[index] = 1;
+	logError(0, "%s Custom Gesture Adding procedure DONE!\n", tag);
+	return OK;
+}
+
+int removeCustomGesture(u8 gestureID)
+{
+	int res, index;
+	u8 cmd[3] = { FTS_CMD_GESTURE_CMD, GETURE_REMOVE_CUSTOM, gestureID };
+	int event_to_search[4] = { EVENTID_GESTURE, EVENT_TYPE_ENB,
+				gestureID, GETURE_REMOVE_CUSTOM };
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+
+	index = gestureID - GESTURE_CUSTOM_OFFSET;
+
+	logError(0, "%s Starting Custom Gesture Removing procedure...\n", tag);
+	if (gestureID != GES_ID_CUST1 && gestureID != GES_ID_CUST2 &&
+		gestureID != GES_ID_CUST3 && gestureID !=
+		GES_ID_CUST4 && gestureID != GES_ID_CUST5) {
+		logError(1, "%s %s:Invalid Custom GestureID (%02X)! ",
+			tag, __func__, gestureID);
+		logError(1, "ERROR %08X\n", ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+	//when a gesture is removed, it is also disabled automatically
+	res = fts_writeFwCmd(cmd, 3);
+	if (res < OK) {
+		logError(1, "%s %s:Impossible to remove custom ",
+			tag, __func__);
+		logError(1, "%gesture ID:%02X %08X\n", gestureID, res);
+		return res;
+	}
+
+	res = pollForEvent(event_to_search, 4, readData, GENERAL_TIMEOUT);
+	if (res < OK) {
+		logError(1, "%s %s:remove event not found! ERROR %08X\n",
+			tag, __func__, res);
+		return res;
+	}
+	//check of gestureID is redundant
+	if (readData[2] != gestureID || readData[4] != 0x00) {
+		logError(1, "%s %s:remove event status not OK! ERROR %08X\n",
+			tag, __func__, readData[4]);
+		return ERROR_GESTURE_REMOVE;
+	}
+
+	custom_gesture_index[index] = 0;
+	logError(0, "%s Custom Gesture Remove procedure DONE!\n", tag);
+	return OK;
+
+}
+
+int isAnyGestureActive(void)
+{
+	int res = 0;
+	/*-1 because in any case the last gesture mask byte will*/
+	/*be evaluated with the following if*/
+	while (res < (GESTURE_MASK_SIZE - 1) && gesture_mask[res] == 0)
+		res++;
+
+	if (gesture_mask[res] == 0) {
+		logError(0, "%s %s: All Gestures Disabled!\n", tag, __func__);
+		return FEAT_DISABLE;
+	}
+
+	logError(0, "%s %s:Active Gestures Found! gesture_mask[%d] = %02X!\n",
+		tag, __func__, res, gesture_mask[res]);
+	return FEAT_ENABLE;
+}
+
+int gestureIDtoGestureMask(u8 id, u8 *mask)
+{
+	logError(0, "%s %s: Index = %d Position = %d!\n",
+		tag, __func__, ((int)((id) / 8)), (id % 8));
+	mask[((int)((id) / 8))] |= 0x01 << (id % 8);
+	return OK;
+}
+
+
+int readGestureCoords(u8 *event)
+{
+	int i = 0;
+	u8 rCmd[3] = {FTS_CMD_FRAMEBUFFER_R, 0x00, 0x00 };
+	int res;
+	unsigned char val[GESTURE_COORDS_REPORT_MAX * 4 + 1];
+
+	//the max coordinates to read are GESTURE_COORDS_REPORT_MAX*4
+	//(because each coordinate is a short(*2) and we have x and y)
+	//+ dummy byte
+	if (event[0] != EVENTID_GESTURE
+		|| event[1] != EVENT_TYPE_GESTURE_DTC2) {
+
+		logError(1, "%s %s:The event passsed as argument is invalid! ",
+			tag, __func__);
+		logError(1, "ERROR %08X\n", ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	rCmd[1] = event[4];	// Offset address L
+	rCmd[2] = event[3];	// Offset address H
+	//number of coords reported L
+	gesture_coords_reported = event[6];
+	//number of coords reported H
+	gesture_coords_reported = (gesture_coords_reported << 8) | event[5];
+	if (gesture_coords_reported > GESTURE_COORDS_REPORT_MAX) {
+		logError(1, "%s%s:FW reported more than:%d points ",
+			tag, __func__, gesture_coords_reported);
+		logError(1, "for gestures!\n");
+		logError(1, " Decreasing to %d\n", GESTURE_COORDS_REPORT_MAX);
+		gesture_coords_reported = GESTURE_COORDS_REPORT_MAX;
+	}
+
+	logError(1, "%s %s: Offset: %02X %02X points = %d\n", tag,
+		__func__, rCmd[1], rCmd[2], gesture_coords_reported);
+	res = fts_readCmd(rCmd, 3, (unsigned char *)val,
+		1 + (gesture_coords_reported * 2));
+	if (res < OK) {
+		logError(1, "%s %s: Cannot read the coordinates! ERROR %08X\n",
+			tag, __func__, res);
+		gesture_coords_reported = ERROR_OP_NOT_ALLOW;
+		return res;
+	}
+	//all the points of the gesture are stored in val
+	for (i = 0; i < gesture_coords_reported; i++) {
+		//ignore first byte data because it is a dummy byte
+		gesture_coordinates_x[i] = (((u16) val[i * 2 + 1 + 1])
+			& 0x0F) << 8 | (((u16) val[i * 2 + 1]) & 0xFF);
+		gesture_coordinates_y[i] =
+			(((u16)val[gesture_coords_reported * 2
+				+ i * 2 + 1 + 1]) & 0x0F) << 8
+			| (((u16)val[gesture_coords_reported * 2
+				+ i * 2 + 1]) & 0xFF);
+	}
+
+	logError(1, "%s %s: Reading Gesture Coordinates DONE!\n",
+		tag, __func__);
+	return OK;
+}
+
+int getGestureCoords(u16 *x, u16 *y)
+{
+	x = gesture_coordinates_x;
+	y = gesture_coordinates_y;
+	logError(1, "%s %s:Number of gesture coordinates returned = %d\n",
+		tag, __func__, gesture_coords_reported);
+	return gesture_coords_reported;
+}

st/fts_lib/ftsGesture.h

@@ -0,0 +1,123 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                     FTS Gesture Utilities                              *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#ifndef _FTS_GESTURE_H_
+#define _FTS_GESTURE_H_
+
+#include "ftsHardware.h"
+
+#define GESTURE_MASK_SIZE              8
+
+//max number of gestures coordinates reported
+#define GESTURE_COORDS_REPORT_MAX      32
+
+// for each custom gesture should be provided 30
+//points (each point is a couple of x,y)
+#define GESTURE_CUSTOM_POINTS          (30 * 2)
+
+//fw support up to 5 custom gestures
+#define GESTURE_CUSTOM_NUMBER          5
+
+#ifdef FTM3
+
+//number of points to transfer with each I2C transaction
+#define TEMPLATE_CHUNK                 (10 * 2)
+#else
+//number of points to transfer with each I2C transaction
+#define TEMPLATE_CHUNK                 (5 * 2)
+#endif
+
+
+//Gesture IDs
+#define GES_ID_UNKNOWN                 0x00 //no meaningful gesture
+#define GES_ID_DBLTAP                  0x01 //Double Tap
+#define GES_ID_O                       0x02 //'O'
+#define GES_ID_C                       0x03 //'C'
+#define GES_ID_M                       0x04 //'M'
+#define GES_ID_W                       0x05 //'W'
+#define GES_ID_E                       0x06 //'e'
+#define GES_ID_HFLIP_L2R               0x07 //Left to right line
+#define GES_ID_HFLIP_R2L               0x08 //Right to left line
+#define GES_ID_VFLIP_T2D               0x09 //Top to bottom line
+#define GES_ID_VFLIP_D2T               0x0A //Bottom to Top line
+#define GES_ID_L                       0x0B //'L'
+#define GES_ID_F                       0x0C //'F'
+#define GES_ID_V                       0x0D //'V'
+#define GES_ID_AT                      0x0E //'@'
+#define GES_ID_S                       0x0F //'S'
+#define GES_ID_Z                       0x10 //'Z'
+#define GES_ID_CUST1                   0x11 //Custom gesture 1
+#define GES_ID_CUST2                   0x12 //Custom gesture 2
+#define GES_ID_CUST3                   0x13 //Custom gesture 3
+#define GES_ID_CUST4                   0x14 //Custom gesture 4
+#define GES_ID_CUST5                   0x15 //Custom gesture 5
+#define GES_ID_LEFTBRACE               0x20 //'<'
+#define GES_ID_RIGHTBRACE              0x21 //'>'
+
+#define GESTURE_CUSTOM_OFFSET          GES_ID_CUST1
+
+//Command sub-type
+#define GESTURE_ENABLE                 0x01
+#define GESTURE_DISABLE                0x02
+#define GESTURE_ENB_CHECK              0x03
+#define GESTURE_START_ADD              0x10
+#define GESTURE_DATA_ADD               0x11
+#define        GESTURE_FINISH_ADD      0x12
+#define GETURE_REMOVE_CUSTOM           0x13
+#define GESTURE_CHECK_CUSTOM           0x14
+
+
+//Event sub-type
+#define EVENT_TYPE_ENB                 0x04
+#define EVENT_TYPE_CHECK_ENB           0x03
+#define EVENT_TYPE_GESTURE_DTC1        0x01
+#define EVENT_TYPE_GESTURE_DTC2        0x02
+
+int updateGestureMask(u8 *mask, int size, int en);
+int disableGesture(u8 *mask, int size);
+int enableGesture(u8 *mask, int size);
+int startAddCustomGesture(u8 gestureID);
+int finishAddCustomGesture(u8 gestureID);
+int loadCustomGesture(u8 *template, u8 gestureID);
+int reloadCustomGesture(void);
+int enterGestureMode(int reload);
+int addCustomGesture(u8 *data, int size, u8 gestureID);
+int removeCustomGesture(u8 gestureID);
+int isAnyGestureActive(void);
+int gestureIDtoGestureMask(u8 id, u8 *mask);
+int readGestureCoords(u8 *event);
+int getGestureCoords(u16 *x, u16 *y);
+
+#endif

st/fts_lib/ftsHardware.h

@@ -0,0 +1,213 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        HW related data                                **
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#ifndef __FTS_HARDWARE_H
+#define __FTS_HARDWARE_H
+
+//DUMMY BYTES DATA
+#define DUMMY_HW_REG                   1
+#define DUMMY_FRAMEBUFFER              1
+#define DUMMY_MEMORY                   1
+
+//DIGITAL CHIP INFO
+#ifdef FTM3_CHIP
+#define DCHIP_ID_0                     0x39
+#define DCHIP_ID_1                     0x6C
+#else
+#define DCHIP_ID_0                     0x36
+#define DCHIP_ID_1                     0x70
+#endif
+
+#ifdef FTM3_CHIP
+#define DCHIP_ID_ADDR                  0x0007
+#define DCHIP_FW_VER_ADDR              0x000A
+#else
+#define DCHIP_ID_ADDR                  0x0004
+#define DCHIP_FW_VER_ADDR              0x0008
+#endif
+
+#define DCHIP_FW_VER_BYTE              2
+
+//CHUNKS
+#define READ_CHUNK                     (2 * 1024)
+#define WRITE_CHUNK                    (2 * 1024)
+#define MEMORY_CHUNK                   (2 * 1024)
+
+//PROTOCOL INFO
+#ifdef FTM3_CHIP
+#define I2C_SAD                        0x49
+#else
+#define I2C_SAD                        0x49
+#endif
+
+#define I2C_INTERFACE                  //comment if the chip use SPI
+#define ICR_ADDR                       0x0024
+#define ICR_SPI_VALUE                  0x02
+
+//SYSTEM RESET INFO
+#ifdef FTM3_CHIP
+#define SYSTEM_RESET_ADDRESS           0x0023
+#define SYSTEM_RESET_VALUE             0x01
+#else
+#define SYSTEM_RESET_ADDRESS           0x0028
+#define SYSTEM_RESET_VALUE             0x80
+#endif
+
+//INTERRUPT INFO
+#ifdef FTM3_CHIP
+#define IER_ADDR                       0x001C
+#else
+#define IER_ADDR                       0x002C
+#endif
+
+#define IER_ENABLE                     0x41
+#define IER_DISABLE                    0x00
+
+//FLASH COMMAND
+
+#define FLASH_CMD_UNLOCK               0xF7
+
+
+#ifdef FTM3_CHIP
+#define FLASH_CMD_WRITE_LOWER_64       0xF0
+#define FLASH_CMD_WRITE_UPPER_64       0xF1
+#define FLASH_CMD_BURN                 0xF2
+#define FLASH_CMD_ERASE                0xF3
+#define FLASH_CMD_READSTATUS           0xF4
+#else
+#define FLASH_CMD_WRITE_64K            0xF8
+#define FLASH_CMD_READ_REGISTER        0xF9
+#define FLASH_CMD_WRITE_REGISTER       0xFA
+#endif
+
+//FLASH UNLOCK PARAMETER
+#define FLASH_UNLOCK_CODE0             0x74
+#define FLASH_UNLOCK_CODE1             0x45
+
+#ifndef FTM3_CHIP
+//FLASH ERASE and DMA PARAMETER
+#define FLASH_ERASE_UNLOCK_CODE0       0x72
+#define FLASH_ERASE_UNLOCK_CODE1       0x03
+#define FLASH_ERASE_UNLOCK_CODE2       0x02
+#define FLASH_ERASE_CODE0              0x02
+#define FLASH_ERASE_CODE1              0xC0
+#define FLASH_DMA_CODE0                0x05
+#define FLASH_DMA_CODE1                0xC0
+#define FLASH_DMA_CONFIG               0x06
+#define FLASH_ERASE_START              0x80
+#define FLASH_NUM_PAGE                 64//number of pages
+#define FLASH_CX_PAGE_START            61
+#define FLASH_CX_PAGE_END              62
+#endif
+
+
+//FLASH ADDRESS
+#ifdef FTM3_CHIP
+#define FLASH_ADDR_SWITCH_CMD          0x00010000
+#define FLASH_ADDR_CODE                0x00000000
+#define FLASH_ADDR_CONFIG              0x0001E800
+#define FLASH_ADDR_CX                  0x0001F000
+#else
+#define ADDR_WARM_BOOT                 0x001E
+#define WARM_BOOT_VALUE                0x38
+#define FLASH_ADDR_CODE                0x00000000
+#define FLASH_ADDR_CONFIG              0x0000FC00
+#endif
+
+
+//CRC ADDR
+#ifdef FTM3_CHIP
+#define ADDR_CRC_BYTE0                 0x00
+#define ADDR_CRC_BYTE1                 0x86
+#define CRC_MASK                       0x02
+#else
+#define ADDR_CRC_BYTE0                 0x00
+#define ADDR_CRC_BYTE1                 0x74
+#define CRC_MASK                       0x03
+#endif
+
+//SIZES FW, CODE, CONFIG, MEMH
+#ifdef FTM3_CHIP
+#define FW_HEADER_SIZE                 32
+#define FW_SIZE                        (int)(128*1024)
+#define FW_CODE_SIZE                   (int)(122*1024)
+#define FW_CONFIG_SIZE                 (int)(2*1024)
+#define FW_CX_SIZE		(int)(FW_SIZE-FW_CODE_SIZE-FW_CONFIG_SIZE)
+#define FW_VER_MEMH_BYTE1              193
+#define FW_VER_MEMH_BYTE0              192
+#define FW_OFF_CONFID_MEMH_BYTE1       2
+#define FW_OFF_CONFID_MEMH_BYTE0       1
+#define FW_BIN_VER_OFFSET              4
+#define FW_BIN_CONFIG_VER_OFFSET       (FW_HEADER_SIZE+FW_CODE_SIZE+1)
+#else
+#define FW_HEADER_SIZE                 64
+#define FW_HEADER_SIGNATURE            0xAA55AA55
+#define FW_FTB_VER                     0x00000001
+#define FW_BYTES_ALIGN                 4
+#define FW_BIN_VER_OFFSET              16
+#define FW_BIN_CONFIG_VER_OFFSET       20
+#endif
+
+//FIFO
+#define FIFO_EVENT_SIZE                8
+
+#ifdef FTM3_CHIP
+#define FIFO_DEPTH                     32
+#else
+#define FIFO_DEPTH                     64
+#endif
+
+#define FIFO_CMD_READONE               0x85
+#define FIFO_CMD_READALL               0x86
+#define FIFO_CMD_LAST                  0x87
+#define FIFO_CMD_FLUSH                 0xA1
+
+
+//CONSTANT TOTAL CX
+#ifdef FTM3_CHIP
+#define CX1_WEIGHT                     4
+#define CX2_WEIGHT                     1
+#else
+#define CX1_WEIGHT                     8
+#define CX2_WEIGHT                     1
+#endif
+
+//OP CODES FOR MEMORY (based on protocol)
+
+#define FTS_CMD_HW_REG_R               0xB6
+#define FTS_CMD_HW_REG_W               0xB6
+#define FTS_CMD_FRAMEBUFFER_R          0xD0
+#define FTS_CMD_FRAMEBUFFER_W          0xD0
+
+#endif

st/fts_lib/ftsIO.c

@@ -0,0 +1,526 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        I2C/SPI Communication                          **
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <stdarg.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/init.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/spi/spidev.h>
+#include <linux/timekeeping.h>
+
+#include "ftsSoftware.h"
+#include "ftsCrossCompile.h"
+#include "ftsError.h"
+#include "ftsHardware.h"
+#include "ftsIO.h"
+#include "ftsTool.h"
+#include "../fts.h"
+
+static char tag[8] = "[ FTS ]\0";
+static struct i2c_client *client;
+static u16 I2CSAD;
+
+int openChannel(struct i2c_client *clt)
+{
+	client = clt;
+	I2CSAD = clt->addr;
+	logError(0, "%s %s: SAD: %02X\n", tag, __func__, I2CSAD);
+	return OK;
+}
+
+struct device *getDev()
+{
+	if (client != NULL)
+		return &(client->dev);
+	else
+		return NULL;
+}
+
+struct i2c_client *getClient()
+{
+	if (client != NULL)
+		return client;
+	else
+		return NULL;
+}
+
+int fts_readCmd(u8 *cmd, int cmdLength, u8 *outBuf, int byteToRead)
+{
+	int ret = -1;
+	int retry = 0;
+	struct i2c_msg I2CMsg[2];
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	uint8_t *buf = info->i2c_data;
+
+	/*
+	 * Reassign memory for i2c_data in case length is greater than 32 bytes
+	 */
+	if (info->i2c_data_len < cmdLength + byteToRead + 1) {
+		kfree(info->i2c_data);
+		info->i2c_data = kmalloc(cmdLength + byteToRead + 1,
+							GFP_KERNEL);
+		if (!info->i2c_data) {
+			info->i2c_data_len = 0;
+			return -ENOMEM;
+		}
+		info->i2c_data_len = cmdLength + byteToRead + 1;
+		buf = info->i2c_data;
+	}
+
+	//write msg
+	I2CMsg[0].addr = (__u16)I2CSAD;
+	I2CMsg[0].flags = (__u16)0;
+	I2CMsg[0].len = (__u16)cmdLength;
+	I2CMsg[0].buf = buf;
+
+	//read msg
+	I2CMsg[1].addr = (__u16)I2CSAD;
+	I2CMsg[1].flags = I2C_M_RD;
+	I2CMsg[1].len = byteToRead;
+	I2CMsg[1].buf = buf + cmdLength;
+
+	memcpy(buf, cmd, cmdLength);
+
+	if (client == NULL)
+		return ERROR_I2C_O;
+	while (retry < I2C_RETRY && ret < OK) {
+		ret = i2c_transfer(client->adapter, I2CMsg, 2);
+		retry++;
+		if (ret < OK)
+			msleep(I2C_WAIT_BEFORE_RETRY);
+	}
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_I2C_R);
+		return ERROR_I2C_R;
+	}
+
+	memcpy(outBuf, buf + cmdLength, byteToRead);
+
+	return OK;
+}
+
+int fts_writeCmd(u8 *cmd, int cmdLength)
+{
+	int ret = -1;
+	int retry = 0;
+	struct i2c_msg I2CMsg[1];
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	uint8_t *buf = info->i2c_data;
+
+	/*
+	 * Reassign memory for i2c_data in case length is greater than 32 bytes
+	 */
+	if (info->i2c_data_len < cmdLength + 1) {
+		kfree(info->i2c_data);
+		info->i2c_data = kmalloc(cmdLength + 1, GFP_KERNEL);
+		if (!info->i2c_data) {
+			info->i2c_data_len = 0;
+			return -ENOMEM;
+		}
+		info->i2c_data_len = cmdLength + 1;
+		buf = info->i2c_data;
+	}
+
+	I2CMsg[0].addr = (__u16)I2CSAD;
+	I2CMsg[0].flags = (__u16)0;
+	I2CMsg[0].len = (__u16)cmdLength;
+	I2CMsg[0].buf = buf;
+
+	memcpy(buf, cmd, cmdLength);
+
+	if (client == NULL)
+		return ERROR_I2C_O;
+	while (retry < I2C_RETRY && ret < OK) {
+		ret = i2c_transfer(client->adapter, I2CMsg, 1);
+		retry++;
+		if (ret < OK)
+			msleep(I2C_WAIT_BEFORE_RETRY);
+		//logError(1,"%s fts_writeCmd: attempt %d\n", tag, retry);
+	}
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+	return OK;
+}
+
+int fts_writeFwCmd(u8 *cmd, int cmdLength)
+{
+	int ret = -1;
+	int ret2 = -1;
+	int retry = 0;
+	struct i2c_msg I2CMsg[1];
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	uint8_t *buf = info->i2c_data;
+
+	/*
+	 * Reassign memory for i2c_data in case length is greater than 32 bytes
+	 */
+	if (info->i2c_data_len < cmdLength + 1) {
+		kfree(info->i2c_data);
+		info->i2c_data = kmalloc(cmdLength + 1, GFP_KERNEL);
+		if (!info->i2c_data) {
+			info->i2c_data_len = 0;
+			return -ENOMEM;
+		}
+		info->i2c_data_len = cmdLength + 1;
+		buf = info->i2c_data;
+	}
+
+	I2CMsg[0].addr = (__u16)I2CSAD;
+	I2CMsg[0].flags = (__u16)0;
+	I2CMsg[0].len = (__u16)cmdLength;
+	I2CMsg[0].buf = buf;
+
+	memcpy(buf, cmd, cmdLength);
+
+	if (client == NULL)
+		return ERROR_I2C_O;
+	while (retry < I2C_RETRY && (ret < OK || ret2 < OK)) {
+		ret = i2c_transfer(client->adapter, I2CMsg, 1);
+		retry++;
+		if (ret >= 0)
+			ret2 = checkEcho(cmd, cmdLength);
+		if (ret < OK || ret2 < OK)
+			msleep(I2C_WAIT_BEFORE_RETRY);
+		//logError(1,"%s fts_writeCmd: attempt %d\n", tag, retry);
+	}
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+	if (ret2 < OK) {
+		logError(1, "%s %s: check echo ERROR %02X\n",
+			tag, __func__, ret2);
+		return (ret | ERROR_I2C_W);
+	}
+	return OK;
+}
+
+
+int writeReadCmd(u8 *writeCmd1, int writeCmdLength, u8 *readCmd1,
+	int readCmdLength, u8 *outBuf, int byteToRead)
+{
+	int ret = -1;
+	int retry = 0;
+	struct i2c_msg I2CMsg[3];
+	struct fts_ts_info *info = i2c_get_clientdata(client);
+	uint8_t *buf = info->i2c_data;
+	uint8_t wr_len = writeCmdLength + readCmdLength + byteToRead;
+
+	/*
+	 * Reassign memory for i2c_data in case length is greater than 32 bytes
+	 */
+	if (info->i2c_data_len < wr_len + 1) {
+		kfree(info->i2c_data);
+		info->i2c_data = kmalloc(wr_len + 1, GFP_KERNEL);
+		if (!info->i2c_data) {
+			info->i2c_data_len = 0;
+			return -ENOMEM;
+		}
+		info->i2c_data_len = wr_len + 1;
+		buf = info->i2c_data;
+	}
+
+	//write msg
+	I2CMsg[0].addr = (__u16)I2CSAD;
+	I2CMsg[0].flags = (__u16)0;
+	I2CMsg[0].len = (__u16)writeCmdLength;
+	I2CMsg[0].buf = buf;
+
+	//write msg
+	I2CMsg[1].addr = (__u16)I2CSAD;
+	I2CMsg[1].flags = (__u16)0;
+	I2CMsg[1].len = (__u16)readCmdLength;
+	I2CMsg[1].buf = buf + writeCmdLength;
+
+	//read msg
+	I2CMsg[2].addr = (__u16)I2CSAD;
+	I2CMsg[2].flags = I2C_M_RD;
+	I2CMsg[2].len = byteToRead;
+	I2CMsg[2].buf = buf + writeCmdLength + readCmdLength;
+
+	memcpy(buf, writeCmd1, writeCmdLength);
+	memcpy(buf + writeCmdLength, readCmd1, readCmdLength);
+
+	if (client == NULL)
+		return ERROR_I2C_O;
+	while (retry < I2C_RETRY && ret < OK) {
+		ret = i2c_transfer(client->adapter, I2CMsg, 3);
+		retry++;
+		if (ret < OK)
+			msleep(I2C_WAIT_BEFORE_RETRY);
+	}
+
+	if (ret < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_I2C_WR);
+		return ERROR_I2C_WR;
+	}
+
+	memcpy(outBuf, buf + writeCmdLength + readCmdLength, byteToRead);
+
+	return OK;
+}
+
+
+int readCmdU16(u8 cmd, u16 address, u8 *outBuf, int byteToRead,
+	int hasDummyByte)
+{
+	int remaining = byteToRead;
+	int toRead = 0;
+	u8 rCmd[3] = { cmd, 0x00, 0x00 };
+
+	u8 *buff = (u8 *)kmalloc_array(READ_CHUNK + 1, sizeof(u8), GFP_KERNEL);
+
+	if (buff == NULL) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	while (remaining > 0) {
+		if (remaining >= READ_CHUNK) {
+			toRead = READ_CHUNK;
+			remaining -= READ_CHUNK;
+		} else {
+			toRead = remaining;
+			remaining = 0;
+		}
+
+		rCmd[1] = (u8)((address & 0xFF00) >> 8);
+		rCmd[2] = (u8)(address & 0xFF);
+
+		if (hasDummyByte) {
+			if (fts_readCmd(rCmd, 3, buff, toRead + 1) < 0) {
+				logError(1, "%s %s: ERROR %02X\n",
+					tag, __func__, ERROR_I2C_R);
+				kfree(buff);
+				return ERROR_I2C_R;
+			}
+			memcpy(outBuf, buff + 1, toRead);
+		} else {
+			if (fts_readCmd(rCmd, 3, buff, toRead) < 0)
+				return ERROR_I2C_R;
+			memcpy(outBuf, buff, toRead);
+		}
+
+		address += toRead;
+		outBuf += toRead;
+	}
+	kfree(buff);
+
+	return OK;
+}
+
+
+int writeCmdU16(u8 WriteCmd, u16 address, u8 *dataToWrite, int byteToWrite)
+{
+	int remaining = byteToWrite;
+	int toWrite = 0;
+
+	u8 *buff = (u8 *)kmalloc_array(WRITE_CHUNK + 3, sizeof(u8), GFP_KERNEL);
+
+	if (buff == NULL) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	buff[0] = WriteCmd;
+
+	while (remaining > 0) {
+		if (remaining >= WRITE_CHUNK) {
+			toWrite = WRITE_CHUNK;
+			remaining -= WRITE_CHUNK;
+		} else {
+			toWrite = remaining;
+			remaining = 0;
+		}
+
+		buff[1] = (u8)((address & 0xFF00) >> 8);
+		buff[2] = (u8)(address & 0xFF);
+		memcpy(buff + 3, dataToWrite, toWrite);
+		if (fts_writeCmd(buff, 3 + toWrite) < 0) {
+			logError(1, "%s %s: ERROR %02\n",
+				tag, __func__, ERROR_I2C_W);
+			kfree(buff);
+			return ERROR_I2C_W;
+		}
+		address += toWrite;
+		dataToWrite += toWrite;
+	}
+
+	kfree(buff);
+	return OK;
+}
+
+int writeCmdU32(u8 writeCmd1, u8 writeCmd2, u32 address, u8 *dataToWrite,
+	int byteToWrite)
+{
+	int remaining = byteToWrite;
+	int toWrite = 0;
+	int ret;
+
+	u8 buff1[3] = { writeCmd1, 0x00, 0x00 };
+	u8 *buff2 = (u8 *)kmalloc_array(WRITE_CHUNK + 3,
+				sizeof(u8), GFP_KERNEL);
+
+	if (buff2 == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+	buff2[0] = writeCmd2;
+
+
+	while (remaining > 0) {
+		if (remaining >= WRITE_CHUNK) {
+			toWrite = WRITE_CHUNK;
+			remaining -= WRITE_CHUNK;
+		} else {
+			toWrite = remaining;
+			remaining = 0;
+		}
+
+		buff1[1] = (u8)((address & 0xFF000000) >> 24);
+		buff1[2] = (u8)((address & 0x00FF0000) >> 16);
+		buff2[1] = (u8)((address & 0x0000FF00) >> 8);
+		buff2[2] = (u8)(address & 0xFF);
+		memcpy(buff2 + 3, dataToWrite, toWrite);
+
+		if (fts_writeCmd(buff1, 3) < 0) {
+			logError(1, "%s %s: ERROR %02X\n",
+				tag, __func__, ERROR_I2C_W);
+			ret = ERROR_I2C_W;
+			goto END;
+		}
+		if (fts_writeCmd(buff2, 3 + toWrite) < 0) {
+			logError(1, "%s %s: ERROR %02X\n",
+				tag, __func__, ERROR_I2C_W);
+			ret = ERROR_I2C_W;
+			goto END;
+		}
+
+		address += toWrite;
+		dataToWrite += toWrite;
+	}
+
+	ret = OK;
+END:
+	kfree(buff2);
+	return ret;
+}
+
+int writeReadCmdU32(u8 wCmd, u8 rCmd, u32 address, u8 *outBuf,
+			int byteToRead, int hasDummyByte)
+{
+	int remaining = byteToRead;
+	int toRead = 0;
+	u8 reaCmd[3];
+	u8 wriCmd[3];
+
+	u8 *buff = (u8 *)kmalloc_array(READ_CHUNK + 1, sizeof(u8), GFP_KERNEL);
+
+	if (buff == NULL) {
+		logError(1, "%s writereadCmd32: ERROR %02X\n",
+			tag, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	reaCmd[0] = rCmd;
+	wriCmd[0] = wCmd;
+
+	while (remaining > 0) {
+		if (remaining >= READ_CHUNK) {
+			toRead = READ_CHUNK;
+			remaining -= READ_CHUNK;
+		} else {
+			toRead = remaining;
+			remaining = 0;
+		}
+
+		wriCmd[1] = (u8)((address & 0xFF000000) >> 24);
+		wriCmd[2] = (u8)((address & 0x00FF0000) >> 16);
+
+		reaCmd[1] = (u8)((address & 0x0000FF00) >> 8);
+		reaCmd[2] = (u8)(address & 0x000000FF);
+
+		if (hasDummyByte) {
+			if (writeReadCmd(wriCmd, 3, reaCmd, 3,
+				buff, toRead + 1) < 0) {
+				logError(1, "%s writeCmdU32: ERROR %02X\n",
+					tag, ERROR_I2C_WR);
+				kfree(buff);
+				return ERROR_I2C_WR;
+			}
+			memcpy(outBuf, buff + 1, toRead);
+		} else {
+			if (writeReadCmd(wriCmd, 3, reaCmd,
+				3, buff, toRead) < 0)
+				return ERROR_I2C_WR;
+			memcpy(outBuf, buff, toRead);
+		}
+
+		address += toRead;
+		outBuf += toRead;
+	}
+
+	kfree(buff);
+	return OK;
+}

st/fts_lib/ftsIO.h

@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                     I2C/SPI Communication                             **
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#ifndef __FTS_IO_H
+#define __FTS_IO_H
+
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+
+#include "ftsSoftware.h"
+#include "ftsCrossCompile.h"
+
+#define I2C_RETRY               3 //number of retry
+#define I2C_WAIT_BEFORE_RETRY   2 //ms
+
+int openChannel(struct i2c_client *clt);
+struct device *getDev(void);
+struct i2c_client *getClient(void);
+int fts_readCmd(u8 *cmd, int cmdLenght, u8 *outBuf, int byteToRead);
+int fts_writeCmd(u8 *cmd, int cmdLenght);
+int fts_writeFwCmd(u8 *cmd, int cmdLenght);
+int writeReadCmd(u8 *writeCmd, int writeCmdLenght, u8 *readCmd,
+	int readCmdLenght, u8 *outBuf, int byteToRead);
+int readCmdU16(u8 cmd, u16 address, u8 *outBuf, int byteToRead,
+	int hasDummyByte);
+int writeCmdU16(u8 WriteCmd, u16 address, u8 *dataToWrite, int byteToWrite);
+int writeCmdU32(u8 writeCmd1, u8 writeCmd2, u32 address, u8 *dataToWrite,
+	int byteToWrite);
+int writeReadCmdU32(u8 wCmd, u8 rCmd, u32 address, u8 *outBuf, int byteToRead,
+	int hasDummyByte);
+
+#endif

st/fts_lib/ftsSoftware.h

@@ -0,0 +1,191 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ *************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                          FW related data                               *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#ifndef __FTS_SOFTWARE_H
+#define __FTS_SOFTWARE_H
+
+#include <linux/types.h>
+
+#include "ftsHardware.h"
+
+#define ECHO_ENABLED                    0x00000001
+
+//FTS FW COMMAND
+#define FTS_CMD_MS_MT_SENSE_OFF         0x92
+#define FTS_CMD_MS_MT_SENSE_ON          0x93
+#define FTS_CMD_SS_HOVER_OFF            0x94
+#define FTS_CMD_SS_HOVER_ON             0x95
+#define FTS_CMD_LP_TIMER_CALIB          0x97
+#define FTS_CMD_MS_KEY_OFF              0x9A
+#define FTS_CMD_MS_KEY_ON               0x9B
+#define FTS_CMD_MS_COMP_TUNING          0xA3
+#define FTS_CMD_SS_COMP_TUNING          0xA4
+#define FTS_CMD_FULL_INITIALIZATION     0xA5
+#define FTS_CMD_ITO_CHECK               0xA7
+#define FTS_CMD_RELEASE_INFO            0xAA
+#define        FTS_CMD_GESTURE_MODE     0xAD
+#define FTS_CMD_REQU_FW_CONF            0xB2
+#define FTS_CMD_REQU_FRAME_DATA         0xB7
+#define FTS_CMD_REQU_COMP_DATA          0xB8
+#define FTS_CMD_WRITE_MP_FLAG           0xC0
+#define FTS_CMD_FEATURE_ENABLE          0xC1
+#define FTS_CMD_FEATURE_DISABLE         0xC2
+#define FTS_CMD_GESTURE_CMD             0xC3
+#define FTS_CMD_LOCKDOWN_CMD            0xC4
+#define FTS_CMD_NOISE_WRITE             0xC7
+#define FTS_CMD_NOISE_READ              0xC8
+#define FTS_CMD_LOCKDOWN_FILL           0xCA
+#define FTS_CMD_LOCKDOWN_WRITE          0xCB
+#define FTS_CMD_LOCKDOWN_READ           0xCC
+#define FTS_CMD_SAVE_CX_TUNING          0xFC
+
+//Event ID
+#define EVENTID_NO_EVENT                0x00
+#define EVENTID_ERROR_EVENT             0x0F
+#define EVENTID_CONTROL_READY           0x10
+#define EVENTID_FW_CONFIGURATION        0x12
+#define EVENTID_COMP_DATA_READ          0x13
+#define EVENTID_STATUS_UPDATE           0x16
+#define EVENTID_RELEASE_INFO            0x1C
+#define EVENTID_LOCKDOWN_INFO           0x1E
+#define EVENTID_ENTER_POINTER           0x03
+#define EVENTID_LEAVE_POINTER           0x04
+#define EVENTID_MOTION_POINTER          0x05
+#define EVENTID_HOVER_ENTER_POINTER     0x07
+#define EVENTID_HOVER_LEAVE_POINTER     0x08
+#define EVENTID_HOVER_MOTION_POINTER    0x09
+#define EVENTID_PROXIMITY_ENTER         0x0B
+#define EVENTID_PROXIMITY_LEAVE         0x0C
+#define EVENTID_KEY_STATUS              0x0E
+#define EVENTID_LOCKDOWN_INFO_READ      0x1E
+#define EVENTID_NOISE_READ              0x17
+#define EVENTID_NOISE_WRITE             0x18
+#define EVENTID_GESTURE                 0x22
+#define EVENTID_FRAME_DATA_READ         0x25
+#define EVENTID_ECHO                    0xEC
+#define EVENTID_LAST                    (EVENTID_FRAME_DATA_READ+1)
+
+//EVENT TYPE
+#define EVENT_TYPE_MS_TUNING_CMPL       0x01
+#define EVENT_TYPE_SS_TUNING_CMPL       0x02
+#define EVENT_TYPE_COMP_DATA_SAVED      0x04
+#define EVENT_TYPE_ITO                  0x05
+#define EVENT_TYPE_FULL_INITIALIZATION  0x07
+#define EVENT_TYPE_LPTIMER_TUNING_CMPL  0x20
+#define EVENT_TYPE_ESD_ERROR            0x0A
+#define EVENT_TYPE_WATCHDOG_ERROR       0x01
+#define EVENT_TYPE_CHECKSUM_ERROR       0x03
+#define EVENT_TYPE_LOCKDOWN             0x0A
+#define EVENT_TYPE_LOCKDOWN_WRITE       0x08
+#define EVENT_TYPE_LOCKDOWN_ERROR       0x0B
+
+//CRC type
+#define CRC_CONFIG_SIGNATURE            0x01
+#define CRC_CONFIG                      0x02
+#define CRC_CX_MEMORY                   0x03
+
+// CONFIG ID INFO
+#define CONFIG_ID_ADDR                  0x0001
+#define CONFIG_ID_BYTE                  2
+
+//ADDRESS OFFSET IN SYSINFO
+#define ADDR_RAW_TOUCH                  0x0000
+#define ADDR_FILTER_TOUCH               0x0002
+#define ADDR_NORM_TOUCH                 0x0004
+#define ADDR_CALIB_TOUCH                0x0006
+#define ADDR_RAW_HOVER_FORCE            0x000A
+#define ADDR_RAW_HOVER_SENSE            0x000C
+#define ADDR_FILTER_HOVER_FORCE         0x000E
+#define ADDR_FILTER_HOVER_SENSE         0x0010
+#define ADDR_NORM_HOVER_FORCE           0x0012
+#define ADDR_NORM_HOVER_SENSE           0x0014
+#define ADDR_CALIB_HOVER_FORCE          0x0016
+#define ADDR_CALIB_HOVER_SENSE          0x0018
+#define ADDR_RAW_PRX_FORCE              0x001A
+#define ADDR_RAW_PRX_SENSE              0x001C
+#define ADDR_FILTER_PRX_FORCE           0x001E
+#define ADDR_FILTER_PRX_SENSE           0x0020
+#define ADDR_NORM_PRX_FORCE             0x0022
+#define ADDR_NORM_PRX_SENSE             0x0024
+#define ADDR_CALIB_PRX_FORCE            0x0026
+#define ADDR_CALIB_PRX_SENSE            0x0028
+#define ADDR_RAW_MS_KEY                 0x0032
+#define ADDR_NORM_MS_KEY                0x0036
+#define ADDR_COMP_DATA                  0x0050
+#define ADDR_FRAMEBUFFER_DATA           0x8000
+
+//ADDRESS FW REGISTER
+#define ADDR_SENSE_LEN                  0x0014
+#define ADDR_FORCE_LEN                  0x0015
+#define ADDR_MS_TUNING_VER              0x0729
+#define ADDR_SS_TUNING_VER              0x074E
+
+//B2 INFO
+#define B2_DATA_BYTES                   4
+//number of bytes
+#define B2_CHUNK                       ((FIFO_DEPTH/2)*B2_DATA_BYTES)
+
+//FEATURES
+#define FEAT_GLOVE                      0x00000001
+#define FEAT_STYLUS                     0x00000002
+#define FEAT_COVER                      0x00000004
+#define FEAT_CHARGER                    0x00000008
+#define FEAT_VR                         0x00000010
+#define FEAT_EDGE_REJECTION             0x00000020
+#define FEAT_CORNER_REJECTION           0x00000040
+#define FEAT_EDGE_PALM_REJECTION        0x00000100
+
+//TOUCH SIZE
+#define STYLUS_SIZE                     0x01
+#define FINGER_SIZE                     0x02
+#define LARGE_SIZE                      0x03
+
+//C7/C8 parameters
+#define NOISE_PARAMETERS                0x01
+
+//MP_FLAG_VALUE
+#define INIT_MP                        0xA5A5A501
+#define INIT_FIELD                     0xA5A5A502
+
+//NOISE PARAMS
+#define NOISE_PARAMETERS_SIZE          4 //bytes
+
+//ERROR INFO
+#define ERROR_INFO_SIZE                (20*4) // bytes
+#define ERROR_SIGNATURE                0xFA5005AF
+#define ERROR_SIGN_HEAD                0xA5
+
+#endif

st/fts_lib/ftsTest.c

@@ -0,0 +1,3844 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        FTS API for MP test                           ***
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <stdarg.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/time.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+//#include <linux/sec_sysfs.h>
+
+#include "ftsCrossCompile.h"
+#include "ftsCompensation.h"
+#include "ftsError.h"
+#include "ftsFrame.h"
+#include "ftsHardware.h"
+#include "ftsIO.h"
+#include "ftsSoftware.h"
+#include "ftsTest.h"
+#include "ftsTime.h"
+#include "ftsTool.h"
+#include "../fts.h"
+
+#ifdef LIMITS_H_FILE
+#include <../fts_limits.h>
+#endif
+
+static char tag[8] = "[ FTS ]\0";
+
+int computeAdjHoriz(u8 *data, int row, int column, u8 **result)
+{
+	int i, j;
+	int size = row * (column - 1);
+
+	if (column < 2) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	*result = (u8 *) kmalloc_array(size, sizeof(u8), GFP_KERNEL);
+	if (*result == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	for (i = 0; i < row; i++) {
+		for (j = 1; j < column; j++) {
+			*(*result + (i * (column - 1) + (j - 1))) =
+				abs(data[i * column + j] -
+				data[i * column + (j - 1)]);
+			}
+	}
+	return OK;
+}
+
+int computeAdjHorizTotal(u16 *data, int row, int column, u16 **result)
+{
+	int i, j;
+	int size = row * (column - 1);
+
+	if (column < 2) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	*result = (u16 *)kmalloc_array(size, sizeof(u16), GFP_KERNEL);
+	if (*result == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	for (i = 0; i < row; i++) {
+		for (j = 1; j < column; j++) {
+			*(*result + (i * (column - 1) + (j - 1))) =
+				abs(data[i * column + j] -
+				data[i * column + (j - 1)]);
+		}
+	}
+
+	return OK;
+}
+
+int computeAdjVert(u8 *data, int row, int column, u8 **result)
+{
+	int i, j;
+	int size = (row - 1) * (column);
+
+	if (row < 2) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	*result = (u8 *)kmalloc_array(size, sizeof(u8), GFP_KERNEL);
+	if (*result == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	for (i = 1; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			*(*result + ((i - 1) * column + j)) =
+				abs(data[i * column + j] -
+				data[(i - 1) * column + j]);
+		}
+	}
+
+	return OK;
+}
+
+int computeAdjVertTotal(u16 *data, int row, int column, u16 **result)
+{
+	int i, j;
+	int size = (row - 1) * (column);
+
+	if (row < 2) {
+		logError(1, "%s %s: ERROR % 02X\n",
+			tag, __func__, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	*result = (u16 *)kmalloc_array(size, sizeof(u16), GFP_KERNEL);
+	if (*result == NULL) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	for (i = 1; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			*(*result + ((i - 1) * column + j)) =
+				abs(data[i * column + j] -
+				data[(i - 1) * column + j]);
+		}
+	}
+
+	return OK;
+}
+
+int computeTotal(u8 *data, u8 main, int row, int column,
+		int m, int n, u16 **result)
+{
+	int i, j;
+	int size = (row) * (column);
+
+	*result = (u16 *)kmalloc_array(size, sizeof(u16), GFP_KERNEL);
+	if (*result == NULL) {
+		logError(1, "%s %s : ERROR %02X\n",
+			tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	for (i = 0; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			*(*result + (i * column + j)) =
+				m * main + n * data[i * column + j];
+		}
+	}
+
+	return OK;
+}
+
+int checkLimitsMinMax(short *data, int row, int column, int min, int max)
+{
+	int i, j;
+	int count = 0;
+
+	for (i = 0; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			if (data[i * column + j] < min
+				|| data[i * column + j] > max) {
+				logError(1, "%s %s:Node[%d,%d] = %d ", tag,
+					__func__, i, j, data[i * column + j]);
+				logError(1, "exceed limit [%d,%d]\n", min, max);
+				count++;
+			}
+		}
+	}
+
+	return count;//if count is 0 = OK, test completed successfully
+}
+
+int checkLimitsGap(short *data, int row, int column, int threshold)
+{
+	int i, j;
+	int min_node;
+	int max_node;
+
+	if (row == 0 || column == 0) {
+		logError(1, "%s %s:invalid number of rows = %d ",
+			tag, __func__, row);
+		logError(1, "or columns = %d %02\n",
+			column, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	min_node = data[0];
+	max_node = data[0];
+
+	for (i = 0; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			if (data[i * column + j] < min_node) {
+				min_node = data[i * column + j];
+			} else {
+				if (data[i * column + j] > max_node)
+					max_node = data[i * column + j];
+			}
+		}
+	}
+
+	if (max_node - min_node > threshold) {
+		logError(1, "%s %s: GAP = %d exceed limit %d\n",
+			tag, __func__, max_node - min_node, threshold);
+		return ERROR_TEST_CHECK_FAIL;
+	}
+	return OK;
+}
+
+int checkLimitsMap(u8 *data, int row, int column, int *min, int *max)
+{
+	int i, j;
+	int count = 0;
+
+	for (i = 0; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			if (data[i * column + j] < min[i * column + j]
+				|| data[i * column + j] > max[i * column + j]) {
+				logError(1, "%s %s: Node[%d,%d] = %d ",
+					tag, __func__,
+					i, j,
+					data[i * column + j]);
+				logError(1, "exceed limit [%d, %d]\n",
+					min[i * column + j],
+					max[i * column + j]);
+				count++;
+			}
+		}
+	}
+
+	return count; //if count is 0 = OK, test completed successfully
+}
+
+int checkLimitsMapTotal(u16 *data, int row, int column, int *min, int *max)
+{
+	int i, j;
+	int count = 0;
+
+	for (i = 0; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			if (data[i * column + j] < min[i * column + j]
+				|| data[i * column + j] > max[i * column + j]) {
+				logError(1, "%s %s:Node[%d,%d] = %d\n",
+					tag, __func__, i, j,
+					data[i * column + j]);
+				logError(1, "exceed limit [%d, %d]\n",
+					min[i * column + j],
+					max[i * column + j]);
+				count++;
+			}
+		}
+	}
+
+	return count; //if count is 0 = OK, test completed successfully
+}
+
+int checkLimitsMapAdj(u8 *data, int row, int column, int *max)
+{
+	int i, j;
+	int count = 0;
+
+	for (i = 0; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			if (data[i * column + j] > max[i * column + j]) {
+				logError(1, "%s %s:Node[%d,%d] = %d ",
+					tag, __func__, i, j);
+				logError(1, "exceed limit > %d\n",
+					data[i * column + j],
+					max[i * column + j]);
+				count++;
+			}
+		}
+	}
+	//if count is 0 = OK, test completed successfully
+	return count;
+}
+
+int checkLimitsMapAdjTotal(u16 *data, int row, int column, int *max)
+{
+	int i, j;
+	int count = 0;
+
+	for (i = 0; i < row; i++) {
+		for (j = 0; j < column; j++) {
+			if (data[i * column + j] > max[i * column + j]) {
+				logError(1, "%s %s:Node[%d,%d] = %d ",
+					tag, __func__, i, j);
+				logError(1, "exceed limit > %d\n",
+					data[i * column + j],
+					max[i * column + j]);
+				count++;
+			}
+		}
+	}
+	//if count is 0 = OK, test completed successfully
+	return count;
+}
+
+int production_test_ito(void)
+{
+	int res = OK;
+	u8 cmd;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	//look for ito event
+	int eventToSearch[2] = {EVENTID_ERROR_EVENT, EVENT_TYPE_ITO};
+
+	logError(0, "%s ITO Production test is starting...\n", tag);
+
+	res = fts_system_reset();
+	if (res < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_ITO);
+		return (res | ERROR_PROD_TEST_ITO);
+	}
+
+	cmd = FTS_CMD_ITO_CHECK;
+
+	logError(0, "%s ITO Check command sent...\n", tag);
+	if (fts_writeFwCmd(&cmd, 1) < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, (ERROR_I2C_W | ERROR_PROD_TEST_ITO));
+		return (ERROR_I2C_W | ERROR_PROD_TEST_ITO);
+	}
+
+	logError(0, "%s Looking for ITO Event...\n", tag);
+	res = pollForEvent(eventToSearch, 2,
+			readData, TIMEOUT_ITO_TEST_RESULT);
+	if (res < 0) {
+		logError(1, "%s %s: ITO Production test failed ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_ITO);
+		return (res | ERROR_PROD_TEST_ITO);
+	}
+
+	if (readData[2] != 0x00 || readData[3] != 0x00) {
+		logError(0, "%s ITO Production testes finished! ERROR %02X\n",
+			tag, (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_ITO));
+		res = (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_ITO);
+	} else {
+		logError(0, "%s ITO Production test finished!..OK\n", tag);
+		res = OK;
+	}
+
+	res |= fts_system_reset();
+	if (res < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_ITO);
+		res = (res | ERROR_PROD_TEST_ITO);
+	}
+	return res;
+}
+
+int production_test_initialization(void)
+{
+	int res;
+	u8 cmd;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+
+	int eventToSearch[2] = {EVENTID_STATUS_UPDATE,
+			EVENT_TYPE_FULL_INITIALIZATION};
+	logError(0, "%s INITIALIZATION Production test is starting\n", tag);
+
+	res = fts_system_reset();
+	if (res < 0) {
+		logError(1, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_INITIALIZATION);
+		return (res | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	logError(0, "%s INITIALIZATION command sent...\n", tag);
+	cmd = FTS_CMD_FULL_INITIALIZATION;
+	if (fts_writeFwCmd(&cmd, 1) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__,
+			(ERROR_I2C_W | ERROR_PROD_TEST_INITIALIZATION));
+		return (ERROR_I2C_W | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	logError(0, "%s Looking for INITIALIZATION Event...\n", tag);
+	res = pollForEvent(eventToSearch, 2, readData,
+		TIMEOUT_INITIALIZATION_TEST_RESULT);
+	if (res < 0) {
+		logError(1, "%s %s: INITIALIZATION Production ", tag, __func__);
+		logError(1, "test failed %02X\n",
+			ERROR_PROD_TEST_INITIALIZATION);
+		return (res | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	if (readData[2] != 0x00) {
+		logError(0, "%sINITIALIZATION Production ", tag);
+		logError(0, "testes finished! FAILED %02X\n",
+		    (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_INITIALIZATION));
+		res = (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_INITIALIZATION);
+	} else {
+		logError(0, "%s INITIALIZATION Production test...OK\n", tag);
+		res = OK;
+	}
+
+	logError(0, "%s Refresh Chip Info...\n", tag);
+	//need to update the chipInfo in order to refresh the tuning_versione
+	res |= readChipInfo(1);
+
+	if (res < 0) {
+		logError(1, "%s %s: read chip info ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_INITIALIZATION);
+		res = (res | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	return res;
+}
+
+int ms_compensation_tuning(void)
+{
+	int res;
+	u8 cmd;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	int eventToSearch[2] = {EVENTID_STATUS_UPDATE,
+		EVENT_TYPE_MS_TUNING_CMPL};
+
+	logError(0, "%s MS INITIALIZATION command sent...\n", tag);
+	cmd = FTS_CMD_MS_COMP_TUNING;
+	if (fts_writeFwCmd(&cmd, 1) < 0) {
+		logError(1, "%s %s 2: ERROR %02X\n",
+			tag, __func__, (ERROR_I2C_W | ERROR_MS_TUNING));
+		return (ERROR_I2C_W | ERROR_MS_TUNING);
+	}
+
+	logError(0, "%s Looking for MS INITIALIZATION Event...\n", tag);
+	res = pollForEvent(eventToSearch, 2, readData,
+			TIMEOUT_INITIALIZATION_TEST_RESULT);
+	if (res < 0) {
+		logError(1, "%s %s:MS INITIALIZATION Production\n",
+			tag, __func__);
+		logError(1, "test failed %02X\n", ERROR_MS_TUNING);
+		return (res | ERROR_MS_TUNING);
+	}
+
+	if (readData[2] != 0x00 || readData[3] != 0x00) {
+		logError(0, "%s MS INITIALIZATION Production ", tag);
+		logError(0, "test finished! FAILED %02X\n", ERROR_MS_TUNING);
+		res = ERROR_MS_TUNING;
+	} else {
+		logError(0,
+		"%s MS INITIALIZATION Production test finished! OK\n",
+		tag);
+		res = OK;
+	}
+
+	return res;
+}
+
+int ss_compensation_tuning(void)
+{
+	int res;
+	u8 cmd;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+
+	int eventToSearch[2] = {EVENTID_STATUS_UPDATE,
+			EVENT_TYPE_SS_TUNING_CMPL};
+
+	logError(0, "%s SS INITIALIZATION command sent...\n", tag);
+	cmd = FTS_CMD_SS_COMP_TUNING;
+	if (fts_writeFwCmd(&cmd, 1) < 0) {
+		logError(1, "%s %s 2: ERROR %02X\n",
+			tag, __func__, (ERROR_I2C_W | ERROR_SS_TUNING));
+		return (ERROR_I2C_W | ERROR_SS_TUNING);
+	}
+
+	logError(0, "%s Looking for SS INITIALIZATION Event...\n", tag);
+	res = pollForEvent(eventToSearch,
+		2,
+		readData,
+		TIMEOUT_INITIALIZATION_TEST_RESULT);
+	if (res < 0) {
+		logError(1, "%s %s:SS INITIALIZATION Production ",
+			tag, __func__);
+		logError(1, "test failed %02X\n", ERROR_SS_TUNING);
+		return (res | ERROR_SS_TUNING);
+	}
+	logError(0, "%s SS INITIALIZATION Production test finished!", tag);
+	if (readData[2] != 0x00 || readData[3] != 0x00) {
+		logError(0, "%s.................FAILED ERROR %02X\n",
+			tag, ERROR_SS_TUNING);
+		res = ERROR_SS_TUNING;
+	} else {
+		logError(0, "%s.................OK\n", tag);
+		res = OK;
+	}
+
+	return res;
+}
+
+int lp_timer_calibration(void)
+{
+	int res;
+	u8 cmd;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	int eventToSearch[2] = {EVENTID_STATUS_UPDATE,
+				EVENT_TYPE_LPTIMER_TUNING_CMPL};
+
+	logError(0, "%s LP TIMER CALIBRATION command sent...\n", tag);
+	cmd = FTS_CMD_LP_TIMER_CALIB;
+	if (fts_writeFwCmd(&cmd, 1) < 0) {
+		logError(1, "%s %s 2:ERROR %02X\n", tag, __func__,
+			(ERROR_I2C_W | ERROR_LP_TIMER_TUNING));
+		return (ERROR_I2C_W | ERROR_LP_TIMER_TUNING);
+	}
+
+	logError(0, "%s Looking for LP TIMER CALIBRATION Event...\n", tag);
+	res = pollForEvent(eventToSearch,
+		2,
+		readData,
+		TIMEOUT_INITIALIZATION_TEST_RESULT);
+
+	if (res < 0) {
+		logError(1, "%s:LP TIMER CALIBRATION Production test failed\n",
+			tag);
+		logError(1, "%s %s:  ERROR %02X\n",
+			tag, __func__, ERROR_LP_TIMER_TUNING);
+		return (res | ERROR_LP_TIMER_TUNING);
+	}
+
+	logError(0, "LP TIMER CALIBRATION Production test finished!");
+	if (readData[2] != 0x00 || readData[3] != 0x01) {
+		logError(0, "%s........FAILED ERROR %02X\n",
+			tag, ERROR_LP_TIMER_TUNING);
+		res = ERROR_LP_TIMER_TUNING;
+	} else {
+		logError(0, "%s.................OK\n", tag);
+		res = OK;
+	}
+
+	return res;
+}
+
+int save_cx_tuning(void)
+{
+	int res;
+	u8 cmd;
+	u8 readData[FIFO_EVENT_SIZE] = {0};
+	int eventToSearch[2] = {EVENTID_STATUS_UPDATE,
+					EVENT_TYPE_COMP_DATA_SAVED};
+
+	logError(0, "%s SAVE CX command sent...\n", tag);
+	cmd = FTS_CMD_SAVE_CX_TUNING;
+	if (fts_writeCmd(&cmd, 1) < 0) {
+		logError(1, "%s %s 2:ERROR %02X\n", tag, __func__,
+			(ERROR_I2C_W | ERROR_SAVE_CX_TUNING));
+		return (ERROR_I2C_W | ERROR_SAVE_CX_TUNING);
+	}
+
+	logError(0, "%s Looking for SAVE CX Event...\n", tag);
+	res = pollForEvent(eventToSearch,
+		2,
+		readData,
+		TIMEOUT_INITIALIZATION_TEST_RESULT);
+	if (res < 0) {
+		logError(1, "%s %s: SAVE CX failed... ERROR %02X\n",
+			tag, ERROR_SAVE_CX_TUNING);
+		return (res | ERROR_SAVE_CX_TUNING);
+	}
+
+	if (readData[2] != 0x00 || readData[3] != 0x00) {
+		logError(0, "%s SAVE CX finished! FAILED  ERROR %02X\n",
+			tag, ERROR_SAVE_CX_TUNING);
+		res = ERROR_SAVE_CX_TUNING;
+	} else {
+		logError(0, "%s SAVE CX finished!.................OK\n", tag);
+		res = OK;
+	}
+
+	return res;
+}
+
+int production_test_split_initialization(int saveToFlash)
+{
+	int res;
+
+	logError(0, "%s Split Initialization test is starting...\n", tag);
+	res = fts_system_reset();
+	if (res < 0) {
+		logError(1, "%s %s: ERROR %02X\n",  tag, __func__,
+			ERROR_PROD_TEST_INITIALIZATION);
+		return (res | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	logError(0, "%s MS INITIALIZATION TEST:\n", tag);
+	res = ms_compensation_tuning();
+	if (res < 0) {
+		logError(0, "%s %s:MS INITIALIZATION TEST FAILED! ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_INITIALIZATION);
+		return (res | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	logError(0, "%s MS INITIALIZATION TEST OK!\n", tag);
+	logError(0, "%s\n", tag);
+	logError(0, "%s SS INITIALIZATION TEST:\n", tag);
+	res = ss_compensation_tuning();
+	if (res < 0) {
+		logError(0, "%s %s: SS INITIALIZATION TEST FAILED! ",
+			tag, __func__);
+		logError(0, "ERROR %02X\n", ERROR_PROD_TEST_INITIALIZATION);
+		return (res | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	logError(0, "%s SS INITIALIZATION TEST OK!\n", tag);
+	logError(0, "%s\n", tag);
+	logError(0, "%s LP INITIALIZATION TEST:\n", tag);
+	res = lp_timer_calibration();
+	if (res < 0) {
+		logError(0, "%s %s: LP INITIALIZATION TEST FAILED! ",
+			tag, __func__);
+		logError(0, "ERROR %02X\n", ERROR_PROD_TEST_INITIALIZATION);
+		return (res | ERROR_PROD_TEST_INITIALIZATION);
+	}
+
+	logError(0, "%s LP INITIALIZATION TEST OK!\n", tag);
+	if (saveToFlash) {
+		logError(0, "%s\n", tag);
+		logError(0, "%s SAVE CX TEST:\n", tag);
+		res = save_cx_tuning();
+		if (res < 0) {
+			logError(0, "%s  %s: SAVE CX TEST FAILED! ERROR %02X\n",
+				tag, __func__, res);
+			return (res | ERROR_PROD_TEST_INITIALIZATION);
+		}
+		logError(0, "%s SAVE CX TEST OK!\n", tag);
+	}
+
+	logError(0, "%s Refresh Chip Info...\n", tag);
+	res |= readChipInfo(1);
+	if (res < 0) {
+		logError(1, "%s %s: read chip info ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_INITIALIZATION);
+		res = (res | ERROR_PROD_TEST_INITIALIZATION);
+	} else {
+		logError(0, "%s Split Initialization test finished! OK\n", tag);
+	}
+	return res;
+
+}
+
+int production_test_main(char *pathThresholds, int stop_on_fail, int saveInit,
+	struct TestToDo *todo, u32 signature)
+{
+	int res, ret;
+
+	logError(0, "%s MAIN Production test is starting...\n", tag);
+	logError(0, "%s\n", tag);
+	logError(0, "%s ITO TEST:\n", tag);
+
+	res = production_test_ito();
+	if (res < 0) {
+		logError(0, "%s Error during ITO TEST! ERROR %08X\n", tag, res);
+		//in case of ITO TEST failure is no sense keep going
+		goto END;
+	}
+	logError(0, "%s ITO TEST OK!\n", tag);
+
+	logError(0, "%s:\n", tag);
+	logError(0, "%s INITIALIZATION TEST:\n", tag);
+
+	if (saveInit == 1) {
+		res = production_test_initialization();
+		if (res < 0) {
+			logError(0, "%s Error during  INITIALIZATION TEST!",
+				tag);
+			logError(0, "ERROR %08X\n", res);
+			if (stop_on_fail)
+				goto END;
+		} else {
+			logError(0, "%s INITIALIZATION TEST OK!\n", tag);
+		}
+	} else
+		logError(0, "%s INITIALIZATION TEST:..SKIPPED\n", tag);
+
+	logError(0, "%s\n", tag);
+	if (saveInit == 1) {
+		logError(0, "%s Cleaning up...\n", tag);
+		ret = cleanUp(0);
+		if (ret < 0) {
+			logError(1, "%s %s: clean up ERROR %02X\n",
+				tag, __func__, ret);
+			res |= ret;
+			if (stop_on_fail)
+				goto END;
+		}
+		logError(0, "%s\n", tag);
+	}
+
+	logError(0, "%s PRODUCTION DATA TEST:\n", tag);
+	ret = production_test_data(pathThresholds, stop_on_fail, todo);
+	if (ret < 0) {
+		logError(0, "%sError during PRODUCTION DATA TEST %08X\n",
+			tag, ret);
+	} else {
+		logError(0, "%s PRODUCTION DATA TEST OK!\n", tag);
+	}
+	res |= ret;
+	// the OR is important because if
+	//the data test is OK but the inizialization
+	//test fail, the main production
+	//test result should = FAIL
+
+	if (ret == OK && saveInit == 1) {
+		logError(0, "%s SAVE FLAG:\n", tag);
+		ret = save_mp_flag(signature);
+		if (ret < OK)
+			logError(0, "%s SAVE FLAG:FAIL! ERROR %08X\n",
+				tag, ret);
+		else
+			logError(0, "%s SAVE FLAG:OK!\n", tag);
+		res |= ret;
+		// need to update the MP Flag
+		ret = readChipInfo(1);
+		if (ret < OK)
+			logError(1, "%s %s:read chip info ERROR %08X\n",
+				tag, __func__, ret);
+		res |= ret;
+	}
+	logError(0, "%s\n", tag);
+END:
+	if (res < 0) {
+		logError(0, "%s MAIN Production test finished..FAILED\n", tag);
+		return res;
+	}
+	logError(0, "%s MAIN Production test finished..OK\n", tag);
+	return OK;
+}
+
+int production_test_ms_raw(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo)
+{
+	int ret, count_fail = 0;
+	struct MutualSenseFrame msRawFrame = {0};
+
+	int *thresholds = NULL;
+	int trows, tcolumns;
+
+	//****** Mutual Sense Test ************/
+	logError(0, "%s\n", tag);
+	logError(0, "%s MS RAW DATA TEST is starting...\n", tag);
+	if (todo->MutualRaw == 1 || todo->MutualRawGap == 1) {
+		ret = getMSFrame2(MS_TOUCH_ACTIVE, &msRawFrame);
+		if (ret < 0) {
+			logError(1, "%s %s:getMSFrame failed... ERROR %02X\n",
+				tag, __func__, ERROR_PROD_TEST_DATA);
+			return (ret | ERROR_PROD_TEST_DATA);
+		}
+
+		logError(0, "%s MS RAW MIN MAX TEST:\n", tag);
+		if (todo->MutualRaw == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				MS_RAW_MIN_MAX,
+				&thresholds,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+				logError(0, "%s %s:MS_RAW_MIN_MAX failed...",
+					tag, __func__);
+				logError(0, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsMinMax(msRawFrame.node_data,
+				msRawFrame.header.force_node,
+				msRawFrame.header.sense_node,
+				thresholds[0],
+				thresholds[1]);
+			if (ret != OK) {
+				logError(0, "%s %s:MS RAW failed...",
+					tag, __func__);
+				logError(0, "ERROR COUNT = %d\n", ret);
+				logError(0, "%s MS RAW MIN MAX TEST:...", tag);
+				logError(0, "FAIL\n\n", tag);
+				count_fail += 1;
+				if (stop_on_fail == 1)
+					goto ERROR;
+			} else
+				logError(0, "%s MS RAW MIN MAX TEST:OK\n", tag);
+			kfree(thresholds);
+			thresholds = NULL;
+		} else
+			logError(0, "%s MS RAW MIN MAX TEST:SKIPPED\n", tag);
+
+		logError(0, "%s\n", tag);
+		logError(0, "%s MS RAW GAP TEST:\n", tag);
+		if (todo->MutualRawGap == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				MS_RAW_GAP,
+				&thresholds,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1 || tcolumns != 1)) {
+				logError(1, "%s %s: MS_RAW_GAP failed... ",
+					tag, __func__);
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsGap(msRawFrame.node_data,
+				msRawFrame.header.force_node,
+				msRawFrame.header.sense_node,
+				thresholds[0]);
+			if (ret != OK) {
+				logError(1, "%s %s:checkLimitsGap MS RAW ",
+					tag, __func__);
+				logError(1, "failed ERROR:%02X\n", ret);
+				count_fail += 1;
+				if (stop_on_fail == 1)
+					goto ERROR;
+			} else
+				logError(0, "%s MS RAW GAP TEST:....OK\n\n",
+					tag);
+			kfree(thresholds);
+			thresholds = NULL;
+		} else
+			logError(0, "%s MS RAW GAP TEST:..SKIPPED\n", tag);
+	} else
+		logError(0, "%s MS RAW FRAME TEST:.SKIPPED\n", tag);
+
+	logError(0, "%s\n", tag);
+	logError(0, "%s MS KEY RAW TEST:\n", tag);
+	if (todo->MutualKeyRaw == 1) {
+		ret = production_test_ms_key_raw(path_limits);
+		if (ret < 0) {
+			logError(1, "%s %s:production_test_ms_key_raw ",
+				tag, __func__);
+			logError(1, "failed ERROR:%02X\n", ret);
+			count_fail += 1;
+			if (count_fail == 1) {
+				logError(0, "%s MS RAW DATA TEST:FAIL ", tag);
+				logError(0, "fails_count:%d\n\n", count_fail);
+				goto ERROR_LIMITS;
+			}
+		}
+	} else
+		logError(0, "%s MS KEY RAW TEST:....SKIPPED\n", tag);
+ERROR:
+	logError(0, "%s\n", tag);
+	if (count_fail == 0) {
+		kfree(msRawFrame.node_data);
+		msRawFrame.node_data = NULL;
+		logError(0, "%s MS RAW DATA TEST finished!.OK\n", tag);
+		return OK;
+	}
+	print_frame_short("MS Raw frame =",
+		array1dTo2d_short(msRawFrame.node_data,
+			msRawFrame.node_data_size,
+			msRawFrame.header.sense_node),
+		msRawFrame.header.force_node,
+		msRawFrame.header.sense_node);
+	kfree(msRawFrame.node_data);
+	kfree(thresholds);
+	logError(0, "%s MS RAW DATA TEST: FAIL fails_count = %d\n\n",
+		tag, count_fail);
+	return (ERROR_PROD_TEST_DATA | ERROR_TEST_CHECK_FAIL);
+
+ERROR_LIMITS:
+	kfree(msRawFrame.node_data);
+	kfree(thresholds);
+	return ret;
+}
+
+int production_test_ms_key_raw(char *path_limits)
+{
+	int ret;
+	struct MutualSenseFrame msRawFrame;
+
+	int *thresholds = NULL;
+	int trows, tcolumns;
+
+	//************* Mutual Sense Test ************/
+	logError(0, "%s MS KEY RAW DATA TEST is starting...\n", tag);
+
+	ret = getMSFrame2(MS_KEY, &msRawFrame);
+	if (ret < 0) {
+		logError(1, "%s %s:getMSKeyFrame failed...ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_DATA);
+		return (ret | ERROR_PROD_TEST_DATA);
+	}
+
+	ret = parseProductionTestLimits(path_limits,
+			MS_KEY_RAW_MIN_MAX,
+			&thresholds,
+			&trows,
+			&tcolumns);
+	if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+		logError(1, "%s %s: MS_KEY_RAW_MIN_MAX failed...ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_DATA);
+		ret |= ERROR_PROD_TEST_DATA;
+		goto ERROR_LIMITS;
+	}
+
+	ret = checkLimitsMinMax(msRawFrame.node_data,
+			msRawFrame.header.force_node,
+			msRawFrame.header.sense_node,
+			thresholds[0],
+			thresholds[1]);
+	if (ret != OK) {
+		logError(1, "%s %s:checkLimitsMinMax failed..ERROR COUNT:%d\n",
+			tag, __func__, ret);
+		goto ERROR;
+	} else
+		logError(0, "%s MS KEY RAW TEST:.................OK\n\n", tag);
+
+	kfree(thresholds);
+	thresholds = NULL;
+
+	kfree(msRawFrame.node_data);
+	msRawFrame.node_data = NULL;
+	return OK;
+ERROR:
+	print_frame_short("MS Key Raw frame =",
+		array1dTo2d_short(msRawFrame.node_data,
+			msRawFrame.node_data_size,
+			msRawFrame.header.sense_node),
+		msRawFrame.header.force_node,
+		msRawFrame.header.sense_node);
+	kfree(msRawFrame.node_data);
+	kfree(thresholds);
+	logError(0, "%s MS KEY RAW TEST:......FAIL\n\n", tag);
+	return (ERROR_PROD_TEST_DATA | ERROR_TEST_CHECK_FAIL);
+ERROR_LIMITS:
+	kfree(msRawFrame.node_data);
+	kfree(thresholds);
+	return ret;
+}
+
+int production_test_ms_cx(char *path_limits,
+		int stop_on_fail, struct TestToDo *todo)
+{
+	int ret;
+	int count_fail = 0;
+
+	int *thresholds = NULL;
+	int *thresholds_min = NULL;
+	int *thresholds_max = NULL;
+	int trows, tcolumns;
+
+	struct MutualSenseData msCompData;
+
+	u8 *adjhor = NULL;
+
+	u8 *adjvert = NULL;
+
+	u16 container;
+	u16 *total_cx = NULL;
+	u16 *total_adjhor = NULL;
+	u16 *total_adjvert = NULL;
+
+	//MS CX TEST
+	logError(0, "%s\n", tag);
+	logError(0, "%s MS CX Testes are starting...\n", tag);
+	//read MS compensation data
+	ret = readMutualSenseCompensationData(MS_TOUCH_ACTIVE, &msCompData);
+	if (ret < 0) {
+		logError(1, "%s %s:readMutualSenseCompensationData ",
+			tag, __func__);
+		logError(1, "failed %02X\n", ERROR_PROD_TEST_DATA);
+		return (ret | ERROR_PROD_TEST_DATA);
+	}
+
+	logError(0, "%s MS CX1 TEST:\n", tag);
+	if (todo->MutualCx1 == 1) {
+		ret = parseProductionTestLimits(path_limits,
+			MS_CX1_MIN_MAX,
+			&thresholds,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+			logError(1, "%s %s:parseProductionTestLimits failed ",
+				tag, __func__);
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		container = (u16)msCompData.cx1;
+		ret = checkLimitsMinMax(&container,
+			1,
+			1,
+			thresholds[0],
+			thresholds[1]); //check the limits
+		if (ret != OK) {
+			logError(1, "%s %s:checkLimitsMinMax MS CX1 failed ",
+				tag, __func__);
+			logError(1, "ERROR COUNT:%d\n", ret);
+			logError(0, "%s MS CX1 TEST:.........FAIL\n\n", tag);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else
+			logError(0, "%s MS CX1 TEST:..........OK\n\n", tag);
+	} else
+		logError(0, "%s MS CX1 TEST:.......SKIPPED\n\n", tag);
+
+	kfree(thresholds);
+	thresholds = NULL;
+
+	logError(0, "%s MS CX2 MIN MAX TEST:\n", tag);
+	if (todo->MutualCx2 == 1) {
+		ret = parseProductionTestLimits(path_limits,
+			MS_CX2_MAP_MIN,
+			&thresholds_min,
+			&trows,
+			&tcolumns); //load min thresholds
+		if (ret < 0 || (trows != msCompData.header.force_node
+			|| tcolumns != msCompData.header.sense_node)) {
+			logError(1, "%s %s:parseProductionTestLimits ",
+				tag, __func__);
+			logError(1, "failed %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		ret = parseProductionTestLimits(path_limits,
+			MS_CX2_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns); //load max thresholds
+		if (ret < 0 || (trows != msCompData.header.force_node
+			|| tcolumns != msCompData.header.sense_node)) {
+			logError(1, "%s %s: MS_CX2_MAP_MAX failed ERROR %02X\n",
+				tag, __func__, ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		ret = checkLimitsMap(msCompData.node_data,
+			msCompData.header.force_node,
+			msCompData.header.sense_node,
+			thresholds_min,
+			thresholds_max);//check the limits
+		if (ret != OK) {
+			logError(1, "%s %s:checkLimitsMap MS CX2 MIN MAX ",
+				tag, __func__);
+			logError(1, "failed ERR_COUNT:%d\n", ret);
+			logError(0, "%s MS CX2 MIN MAX TEST:......FAIL\n\n",
+				tag);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else
+			logError(0, "%s MS CX2 MIN MAX TEST:....OK\n\n", tag);
+
+		kfree(thresholds_min);
+		thresholds_min = NULL;
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+	} else
+		logError(0, "%s MS CX2 MIN MAX TEST:....SKIPPED\n\n", tag);
+
+	logError(0, "%s MS CX2 ADJ TEST:\n", tag);
+	if (todo->MutualCx2Adj == 1) {
+		//MS CX2 ADJ HORIZ
+		logError(0, "%s MS CX2 ADJ HORIZ TEST:\n", tag);
+
+		ret = computeAdjHoriz(msCompData.node_data,
+			msCompData.header.force_node,
+			msCompData.header.sense_node,
+			&adjhor);
+		if (ret < 0) {
+			logError(1, "%s %s:computeAdjHoriz failed...",
+				tag, __func__);
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		logError(0, "%s MS CX2 ADJ HORIZ computed!\n", tag);
+
+		ret = parseProductionTestLimits(path_limits,
+			MS_CX2_ADJH_MAP_MAX,
+			&thresholds_max, &trows,
+			&tcolumns);
+		if (ret < 0 || (trows != msCompData.header.force_node
+			|| tcolumns != msCompData.header.sense_node - 1)) {
+
+			logError(1, "%s %s: MS_CX2_ADJH_MAP_MAX failed...",
+				tag, __func__);
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		ret = checkLimitsMapAdj(adjhor,
+			msCompData.header.force_node,
+			msCompData.header.sense_node - 1,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s %s:checkLimitsMapAdj CX2 ADJH failed ",
+				tag, __func__);
+			logError(1, "ERROR COUNT:%d\n", ret);
+			logError(0, "%s MS CX2 ADJ HORIZ TEST:..FAIL\n\n", tag);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else
+			logError(0, "%s MS CX2 ADJ HORIZ TEST:..OK\n\n", tag);
+
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+		kfree(adjhor);
+		adjhor = NULL;
+
+		//MS CX2 ADJ VERT
+		logError(0, "%s MS CX2 ADJ VERT TEST:\n", tag);
+		ret = computeAdjVert(msCompData.node_data,
+			msCompData.header.force_node,
+			msCompData.header.sense_node,
+			&adjvert);
+		if (ret < 0) {
+			logError(1, "%s %s:computeAdjVert failed... ",
+				tag, __func__);
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		logError(0, "%s MS CX2 ADJ VERT computed!\n", tag);
+
+		ret = parseProductionTestLimits(path_limits,
+			MS_CX2_ADJV_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != msCompData.header.force_node - 1
+			|| tcolumns != msCompData.header.sense_node)) {
+			logError(1, "%s %s:MS_CX2_ADJV_MAP_MAX failed ",
+				tag, __func__);
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		ret = checkLimitsMapAdj(adjvert,
+			msCompData.header.force_node - 1,
+			msCompData.header.sense_node - 1,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s %s:checkLimitsMapAdj CX2 ADJV failed ",
+				tag, __func__);
+			logError(1,  "COUNT:%d\n", ret);
+			logError(0, "%s MS CX2 ADJ HORIZ TEST:FAIL\n\n", tag);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else
+			logError(0, "%s MS CX2 ADJ VERT TEST:OK\n\n", tag);
+
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+		kfree(adjvert);
+		adjvert = NULL;
+	} else
+		logError(0, "%s MS CX2 ADJ TEST:SKIPPED\n\n", tag);
+
+	//START OF TOTAL CHECK
+	logError(0, "%s MS TOTAL CX TEST:\n", tag);
+
+	if (todo->MutualCxTotal == 1 || todo->MutualCxTotalAdj == 1) {
+		ret = computeTotal(msCompData.node_data,
+				msCompData.cx1,
+				msCompData.header.force_node,
+				msCompData.header.sense_node,
+				CX1_WEIGHT,
+				CX2_WEIGHT,
+				&total_cx);
+		if (ret < 0) {
+			logError(1, "%s %s:computeTotalCx failed...",
+				tag, __func__);
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		logError(0, "%s MS TOTAL CX MIN MAX TEST:\n", tag);
+		if (todo->MutualCxTotal == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				MS_TOTAL_CX_MAP_MIN,
+				&thresholds_min,
+				&trows,
+				&tcolumns);//load min thresholds
+			if (ret < 0 || (trows != msCompData.header.force_node
+				|| tcolumns != msCompData.header.sense_node)) {
+				logError(1, "%s %s:parseProductionTestLimits ",
+					tag, __func__);
+				logError(1, "failed %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			//load max thresholds
+			ret = parseProductionTestLimits(path_limits,
+				MS_TOTAL_CX_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != msCompData.header.force_node
+				|| tcolumns != msCompData.header.sense_node)) {
+				logError(1, "%s %s:MS_TOTAL_CX_MAP_MAX failed",
+					tag, __func__);
+				logError(1, "...ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsMapTotal(total_cx,
+				msCompData.header.force_node,
+				msCompData.header.sense_node,
+				thresholds_min,
+				thresholds_max);//check the limits
+			if (ret != OK) {
+				logError(1, "%s %s:MS TOTAL CX TEST failed ",
+					tag, __func__);
+				logError(1, "COUNT:%d\n", ret);
+				logError(0,  "%s MS TOTAL CX MIN MAX ", tag);
+				logError(0,  "TEST:FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s MS TOTAL CX MIN MAX TEST", tag);
+				logError(0, ":OK\n\n");
+			}
+
+			kfree(thresholds_min);
+			thresholds_min = NULL;
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+		} else
+			logError(0, "%s MS TOTAL CX MIN MAX TEST:SKIPPED\n\n",
+				tag);
+
+
+		logError(0, "%s MS TOTAL CX ADJ TEST:\n", tag);
+		if (todo->MutualCxTotalAdj == 1) {
+			//MS TOTAL CX ADJ HORIZ
+			logError(0, "%s MS TOTAL CX ADJ HORIZ TEST:\n", tag);
+
+			//thresholds_max = NULL;
+			ret = computeAdjHorizTotal(total_cx,
+				msCompData.header.force_node,
+				msCompData.header.sense_node,
+				&total_adjhor);
+			if (ret < 0) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "computeAdjHoriz failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			logError(0, "%s MS TOTAL CX ADJ HORIZ computed!\n",
+				tag);
+			ret = parseProductionTestLimits(path_limits,
+					MS_TOTAL_CX_ADJH_MAP_MAX,
+					&thresholds_max, &trows,
+					&tcolumns);
+			if (ret < 0 || (trows != msCompData.header.force_node
+			    || tcolumns != msCompData.header.sense_node - 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "MS_TOTAL_CX_ADJH_MAP_MAX ");
+				logError(1, "failed...RROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsMapAdjTotal(total_adjhor,
+				msCompData.header.force_node,
+				msCompData.header.sense_node - 1,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMapAdj MS TOTAL ");
+				logError(1, "CX ADJH failed... ");
+				logError(1, "ERROR COUNT = %d\n", ret);
+				logError(0, "%s MS TOTAL CX ADJ HORIZ ", tag);
+				logError(0, "TEST:.................FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s MS TOTAL CX ADJ HORIZ ", tag);
+				logError(0, "TEST:.................OK\n\n");
+			}
+
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+			kfree(total_adjhor);
+			total_adjhor = NULL;
+
+			//MS TOTAL CX ADJ VERT
+			logError(0, "%s MS TOTAL CX ADJ VERT TEST:\n", tag);
+
+			ret = computeAdjVertTotal(total_cx,
+					msCompData.header.force_node,
+					msCompData.header.sense_node,
+					&total_adjvert);
+			if (ret < 0) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "computeAdjVert failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			logError(0, "%s MS TOTAL CX ADJ VERT computed!\n",
+				tag);
+
+			ret = parseProductionTestLimits(path_limits,
+					MS_TOTAL_CX_ADJV_MAP_MAX,
+					&thresholds_max,
+					&trows,
+					&tcolumns);
+			if (ret < 0 ||
+				(trows != msCompData.header.force_node - 1
+				|| tcolumns != msCompData.header.sense_node)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "MS_TOTAL_CX_ADJV_MAP_MAX failed");
+				logError(1, "... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsMapAdjTotal(total_adjvert,
+					msCompData.header.force_node - 1,
+					msCompData.header.sense_node - 1,
+					thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMapAdj MS TOTAL ");
+				logError(1, "CX ADJV failed... ");
+				logError(1, "ERROR COUNT = %d\n", ret);
+				logError(0, "%s MS TOTAL CX ADJ HORIZ ", tag);
+				logError(0, "TEST:.................FAIL\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s MS TOTAL CX ADJ VERT ", tag);
+				logError(0, "TEST:.................OK\n");
+			}
+
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+			kfree(total_adjvert);
+			total_adjvert = NULL;
+		} else {
+			logError(0, "%s MS TOTAL CX ADJ ", tag);
+			logError(0, "TEST:.................SKIPPED\n");
+		}
+
+		kfree(total_cx);
+		total_cx = NULL;
+	} else
+		logError(0, "%s MS TOTAL CX TEST:.................SKIPPED\n",
+			tag);
+
+	if ((todo->MutualKeyCx1
+		| todo->MutualKeyCx2 | todo->MutualKeyCxTotal) == 1) {
+		ret = production_test_ms_key_cx(path_limits,
+			stop_on_fail,
+			todo);
+		if (ret < 0) {
+			count_fail += 1;
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "production_test_ms_key_cx failed...");
+			logError(1, "ERROR = %02X\n", ret);
+			logError(0, "%s MS CX testes finished!", tag);
+			logError(0, ".................FAILED  ");
+			logError(0, "fails_count = %d\n\n", count_fail);
+			return ret;
+		}
+	} else
+		logError(0, "%s MS KEY CX TEST:.................SKIPPED\n",
+			tag);
+
+	if ((todo->MutualKeyCx1 | todo->MutualKeyCx2
+			| todo->MutualKeyCxTotal) == 1) {
+		ret = production_test_ms_key_cx(path_limits,
+			stop_on_fail,
+			todo);
+
+		if (ret < 0) {
+			count_fail += 1;
+			logError(1, "%s %s:production_test_ms_key_cx ",
+				tag, __func__);
+			logError(1, "failed :%02X\n", ret);
+			logError(0, "%s MS CX testes finished! ", tag);
+			logError(0, "fails_count = %d\n\n", count_fail);
+			return ret;
+		}
+	} else
+		logError(0, "%s MS KEY CX TEST:..SKIPPED\n", tag);
+ERROR:
+	logError(0, "%s\n", tag);
+	if (count_fail == 0) {
+		logError(0, "%s MS CX testes finished! OK\n", tag);
+		kfree(msCompData.node_data);
+		msCompData.node_data = NULL;
+		return OK;
+	}
+	print_frame_u8("MS Init Data (Cx2) =",
+		array1dTo2d_u8(msCompData.node_data,
+			msCompData.node_data_size,
+			msCompData.header.sense_node),
+		msCompData.header.force_node,
+		msCompData.header.sense_node);
+	logError(0, "%s MS CX testes finished! fails_count = %d\n\n",
+		tag, count_fail);
+	kfree(thresholds);
+	kfree(thresholds_min);
+	kfree(thresholds_max);
+	kfree(adjhor);
+	kfree(adjvert);
+	kfree(total_cx);
+	kfree(total_adjhor);
+	kfree(total_adjvert);
+	kfree(msCompData.node_data);
+
+	return (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_DATA);
+
+ERROR_LIMITS:
+	kfree(thresholds);
+	kfree(thresholds_min);
+	kfree(thresholds_max);
+	kfree(adjhor);
+	kfree(adjvert);
+	kfree(total_cx);
+	kfree(total_adjhor);
+	kfree(total_adjvert);
+	kfree(msCompData.node_data);
+	return ret;
+}
+
+int production_test_ms_key_cx(char *path_limits, int stop_on_fail,
+		struct TestToDo *todo)
+{
+	int ret;
+	int count_fail = 0;
+	int num_keys = 0;
+
+	int *thresholds = NULL;
+	int *thresholds_min = NULL;
+	int *thresholds_max = NULL;
+	int trows, tcolumns;
+
+	struct MutualSenseData msCompData;
+
+	u16 container;
+	u16 *total_cx = NULL;
+
+
+	//MS CX TEST
+	logError(0, "%s MS KEY CX Testes are starting...\n", tag);
+
+	 //read MS compensation data
+	ret = readMutualSenseCompensationData(MS_KEY, &msCompData);
+	if (ret < 0) {
+		logError(0, "%s production_test_data: ", tag);
+		logError(0, "readMutualSenseCompensationData failed... ");
+		logError(0, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+		return (ret | ERROR_PROD_TEST_DATA);
+	}
+
+	//the meaningful data are only in the first row, the other rows are
+	// only a copy of the first one
+	if (msCompData.header.force_node > msCompData.header.sense_node)
+		num_keys = msCompData.header.force_node;
+	else
+		num_keys = msCompData.header.sense_node;
+
+	logError(0, "%s MS KEY CX1 TEST:\n", tag);
+	if (todo->MutualKeyCx1 == 1) {
+
+		ret = parseProductionTestLimits(path_limits,
+			MS_KEY_CX1_MIN_MAX,
+			&thresholds,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+			logError(0, "%s production_test_data: ", tag);
+			logError(0, "parseProductionTestLimits ");
+			logError(0, "MS_KEY_CX1_MIN_MAX failed... ");
+			logError(0, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		container = (u16) msCompData.cx1;
+		ret = checkLimitsMinMax(&container,
+			1,
+			1,
+			thresholds[0],
+			thresholds[1]); //check the limits
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMinMax MS CX1 failed... ");
+			logError(1, "ERROR COUNT = %d\n", ret);
+			logError(0, "%s MS KEY CX1 TEST:................", tag);
+			logError(0, ".FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s MS KEY CX1 TEST:................", tag);
+			logError(0, ".OK\n\n");
+		}
+	} else
+		logError(0, "%s MS KEY CX1 TEST:.................SKIPPED\n\n",
+			tag);
+
+	kfree(thresholds);
+	thresholds = NULL;
+
+	logError(0, "%s MS KEY CX2 TEST:\n", tag);
+	if (todo->MutualKeyCx2 == 1) {
+		 //load min thresholds
+		ret = parseProductionTestLimits(path_limits,
+				MS_KEY_CX2_MAP_MIN,
+				&thresholds_min,
+				&trows,
+				&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != num_keys)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "MS_KEY_CX2_MAP_MIN failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//load max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			MS_KEY_CX2_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != num_keys)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "MS_KEY_CX2_MAP_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		 //check the limits
+		ret = checkLimitsMap(msCompData.node_data,
+			1,
+			num_keys,
+			thresholds_min,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMap MS KEY CX2 failed... ");
+			logError(1, "ERROR COUNT = %d\n", ret);
+			logError(0, "%s MS KEY CX2 TEST:................", tag);
+			logError(0, ".FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s MS KEY CX2 TEST:...............", tag);
+			logError(0, "..OK\n\n");
+		}
+
+		kfree(thresholds_min);
+		thresholds_min = NULL;
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+	} else
+		logError(0, "%s MS CX2 TEST:.................SKIPPED\n\n",
+			tag);
+
+	//START OF TOTAL CHECK
+	logError(0, "%s MS KEY TOTAL CX TEST:\n", tag);
+
+	if (todo->MutualKeyCxTotal == 1) {
+		ret = computeTotal(msCompData.node_data,
+			msCompData.cx1, 1,
+			num_keys,
+			CX1_WEIGHT,
+			CX2_WEIGHT,
+			&total_cx);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeTotalCx failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//load min thresholds
+		ret = parseProductionTestLimits(path_limits,
+			MS_KEY_TOTAL_CX_MAP_MIN,
+			&thresholds_min,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != num_keys)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "%parseProductionTestLimits ");
+			logError(1, "MS_KEY_TOTAL_CX_MAP_MIN failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//load max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			MS_KEY_TOTAL_CX_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != num_keys)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "MS_KEY_TOTAL_CX_MAP_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the limits
+		ret = checkLimitsMapTotal(total_cx,
+			1,
+			num_keys,
+			thresholds_min,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMap  MS TOTAL ");
+			logError(1, "KEY CX TEST failed... ERROR COUNT = %d\n",
+				ret);
+			logError(0, "%s MS KEY TOTAL CX TEST:...........", tag);
+			logError(0, "......FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s MS KEY TOTAL CX TEST:...........", tag);
+			logError(0, "......OK\n\n");
+		}
+
+		kfree(thresholds_min);
+		thresholds_min = NULL;
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+
+		kfree(total_cx);
+		total_cx = NULL;
+	} else {
+		logError(0, "%s MS KEY TOTAL CX TEST:.................", tag);
+		logError(0, "SKIPPED\n");
+	}
+
+ERROR:
+	logError(0, "%s\n", tag);
+	if (count_fail == 0) {
+		logError(0,
+			"%s MS KEY CX testes finished! OK\n", tag);
+		kfree(msCompData.node_data);
+		msCompData.node_data = NULL;
+		return  OK;
+	}
+	print_frame_u8("MS Key Init Data (Cx2) =",
+		array1dTo2d_u8(msCompData.node_data,
+			msCompData.node_data_size,
+			msCompData.header.sense_node),
+		1,
+		msCompData.header.sense_node);
+	logError(0, "%s MS Key CX testes finished!..............", tag);
+	logError(0, "...FAILED  fails_count = %d\n\n", count_fail);
+	kfree(thresholds);
+	kfree(thresholds_min);
+	kfree(thresholds_max);
+	kfree(msCompData.node_data);
+	kfree(total_cx);
+	return  (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_DATA);
+ERROR_LIMITS:
+	kfree(thresholds);
+	kfree(thresholds_min);
+	kfree(thresholds_max);
+	kfree(msCompData.node_data);
+	kfree(total_cx);
+	return ret;
+}
+
+int production_test_ss_raw(char *path_limits,
+		int stop_on_fail, struct TestToDo *todo)
+{
+	int ret;
+	int count_fail = 0;
+	int rows, columns;
+
+	//short *ssRawFrame = NULL;
+	struct SelfSenseFrame ssRawFrame;
+
+	int *thresholds = NULL;
+	int trows, tcolumns;
+
+	//MS SS TEST
+	logError(0, "%s\n", tag);
+	logError(0, "%s SS RAW Testes are starting...\n", tag);
+
+	//******* Self Sense Test ***************/
+	logError(0, "%s Getting SS Frame...\n", tag);
+	ret = getSSFrame2(SS_TOUCH, &ssRawFrame);
+	if (ret < 0) {
+		logError(1, "%s %s:getSSFrame failed...ERROR %02X\n",
+			tag, __func__, ERROR_PROD_TEST_DATA);
+		return (ret | ERROR_PROD_TEST_DATA);
+	}
+
+	//SS RAW (PROXIMITY) FORCE TEST
+	logError(0, "%s SS RAW (PROXIMITY) FORCE TEST:\n", tag);
+
+	if (todo->SelfForceRaw == 1 || todo->SelfForceRawGap == 1) {
+		//there are no data for the sense
+		//channels due to the fact that
+		//the force frame is analized
+		columns = 1;
+		rows = ssRawFrame.header.force_node;
+
+		logError(0, "%s SS RAW (PROXIMITY) FORCE MIN MAX TEST:\n", tag);
+		if (todo->SelfForceRaw == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				SS_RAW_FORCE_MIN_MAX,
+				&thresholds,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+				logError(1, "%s %s:parseProductionTestLimits ",
+					tag, __func__);
+				logError(1, "failed %02X\n",
+					ERROR_PROD_TEST_DATA);
+				//return (ret | ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsMinMax(ssRawFrame.force_data,
+				rows, columns,
+				thresholds[0],
+				thresholds[1]);
+			if (ret != OK) {
+				logError(1, "%s %s:checkLimitsMinMax ",
+					tag, __func__);
+				logError(1, "failed ERROR COUNT:%d\n", ret);
+				logError(0, "%s SS RAW (PROXIMITY) FORCE", tag);
+				logError(0, " MIN MAX TEST:FAIL\n\n");
+				count_fail += 1;
+				print_frame_short("SS Raw force frame =",
+					array1dTo2d_short(ssRawFrame.force_data,
+						rows*columns, columns),
+					rows,
+					columns);
+				if (stop_on_fail) {
+					ret = ERROR_PROD_TEST_DATA
+						| ERROR_TEST_CHECK_FAIL;
+					goto ERROR_LIMITS;
+				}
+			} else {
+				logError(0, "%s SS RAW (PROXIMITY) ", tag);
+				logError(0, "FORCE MIN MAX TEST:.............");
+				logError(0, "....OK\n\n");
+			}
+
+			kfree(thresholds);
+			thresholds = NULL;
+		} else {
+			logError(0, "%s SS RAW (PROXIMITY) ", tag);
+			logError(0, "FORCE MIN MAX TEST:.................");
+			logError(0, "SKIPPED\n\n");
+		}
+
+		logError(0, "%s\n", tag);
+		logError(0, "%s SS RAW (PROXIMITY) FORCE GAP TEST:\n", tag);
+		if (todo->SelfForceRawGap == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				SS_RAW_FORCE_GAP,
+				&thresholds,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1 || tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_RAW_FORCE_GAP failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsGap(ssRawFrame.force_data,
+				rows,
+				columns,
+				thresholds[0]);
+
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsGap SS RAW ");
+				logError(1, "(PROXIMITY) FORCE GAP failed...");
+				logError(1, "ERROR = %02X\n", ret);
+				logError(0, "%s SS RAW (PROXIMITY) ", tag);
+				logError(0, "FORCE GAP TEST:.................");
+				logError(0, "FAIL\n\n");
+				count_fail += 1;
+				print_frame_short("SS Raw force frame =",
+					array1dTo2d_short(ssRawFrame.force_data,
+						rows*columns, columns),
+					rows,
+					columns);
+				if (stop_on_fail) {
+					ret = ERROR_PROD_TEST_DATA
+						| ERROR_TEST_CHECK_FAIL;
+					goto ERROR_LIMITS;
+				}
+			} else {
+				logError(0, "%s SS RAW (PROXIMITY) ", tag);
+				logError(0, "FORCE GAP TEST:.................");
+				logError(0, "OK\n\n");
+			}
+
+			kfree(thresholds);
+			thresholds = NULL;
+		} else {
+			logError(0, "%s SS RAW (PROXIMITY) ", tag);
+			logError(0, "FORCE GAP TEST:.................");
+			logError(0, "SKIPPED\n\n");
+		}
+
+		kfree(ssRawFrame.force_data);
+		ssRawFrame.force_data = NULL;
+	} else {
+		logError(0, "%s SS RAW (PROXIMITY) FORCE ", tag);
+		logError(0, "TEST:.................SKIPPED\n\n");
+	}
+
+	logError(0, "%s\n", tag);
+	//SS RAW (PROXIMITY) SENSE TEST
+	logError(0, "%s SS RAW (PROXIMITY) SENSE TEST:\n", tag);
+
+	if (todo->SelfSenseRaw == 1 || todo->SelfSenseRawGap == 1) {
+		columns = ssRawFrame.header.sense_node;
+		// there are no data for the force channels due
+		// to the fact that the sense frame is analized
+		rows = 1;
+
+		logError(0, "%s SS RAW (PROXIMITY) SENSE MIN MAX TEST:\n", tag);
+		if (todo->SelfSenseRaw == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				SS_RAW_SENSE_MIN_MAX,
+				&thresholds,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_RAW_SENSE_MIN_MAX failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsMinMax(ssRawFrame.sense_data,
+				rows,
+				columns,
+				thresholds[0],
+				thresholds[1]);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMinMax SS RAW ");
+				logError(1, "(PROXIMITY) SENSE failed... ");
+				logError(1, "ERROR COUNT = %d\n", ret);
+				logError(0, "%s SS RAW (PROXIMITY) ", tag);
+				logError(0, "SENSE MIN MAX TEST:.............");
+				logError(0, "....FAIL\n");
+				count_fail += 1;
+				print_frame_short("SS Raw sense frame =",
+					array1dTo2d_short(ssRawFrame.sense_data,
+						rows*columns, columns),
+					rows,
+					columns);
+				if (stop_on_fail) {
+					ret = ERROR_PROD_TEST_DATA
+						| ERROR_TEST_CHECK_FAIL;
+					goto ERROR_LIMITS;
+				}
+			} else {
+				logError(0, "%s SS RAW (PROXIMITY) ", tag);
+				logError(0, "SENSE MIN MAX TEST:.............");
+				logError(0, "....OK\n");
+			}
+
+			kfree(thresholds);
+			thresholds = NULL;
+		} else {
+			logError(0, "%s SS RAW (PROXIMITY) SENSE MIN MAX", tag);
+			logError(0, " TEST:.................SKIPPED\n");
+		}
+
+		logError(0, "%s\n", tag);
+		logError(0, "%s SS RAW (PROXIMITY) SENSE GAP TEST:\n", tag);
+		if (todo->SelfSenseRawGap == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				SS_RAW_SENSE_GAP,
+				&thresholds,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1 || tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_RAW_SENSE_GAP failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			ret = checkLimitsGap(ssRawFrame.sense_data,
+					rows,
+					columns,
+					thresholds[0]);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsGap SS RAW ");
+				logError(1, "(PROXIMITY) SENSE GAP failed... ");
+				logError(1, "ERROR = %02X\n", ret);
+				logError(0, "%s SS RAW (PROXIMITY) ", tag);
+				logError(0, "SENSE GAP TEST:.................");
+				logError(0, "FAIL\n");
+				count_fail += 1;
+				print_frame_short("SS Raw sense frame =",
+					array1dTo2d_short(ssRawFrame.sense_data,
+						rows*columns, columns),
+					rows,
+					columns);
+				if (stop_on_fail) {
+					ret = ERROR_PROD_TEST_DATA
+						| ERROR_TEST_CHECK_FAIL;
+					goto ERROR_LIMITS;
+				}
+			} else {
+				logError(0, "%s SS RAW (PROXIMITY) SENSE", tag);
+				logError(0, " GAP TEST:.................OK\n");
+			}
+
+			kfree(thresholds);
+			thresholds = NULL;
+		} else {
+			logError(0, "%s SS RAW (PROXIMITY) SENSE GAP ", tag);
+			logError(0, "TEST:.................SKIPPED\n");
+		}
+
+		kfree(ssRawFrame.sense_data);
+		ssRawFrame.sense_data = NULL;
+	}
+
+	logError(0, "%s\n", tag);
+	if (count_fail == 0) {
+		logError(0, "%s SS RAW testes finished!.................OK\n\n",
+			tag);
+		return OK;
+	}
+	logError(0, "%s SS RAW testes finished!.................", tag);
+	logError(0, "FAILED  fails_count = %d\n\n", count_fail);
+	return (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_DATA);
+
+ERROR_LIMITS:
+	kfree(ssRawFrame.force_data);
+	kfree(ssRawFrame.sense_data);
+	kfree(thresholds);
+	return ret;
+
+}
+
+int production_test_ss_ix_cx(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo)
+{
+	int ret;
+	int count_fail = 0;
+
+	int *thresholds = NULL;
+	int trows, tcolumns;
+	int *thresholds_min = NULL;
+	int *thresholds_max = NULL;
+
+	struct SelfSenseData ssCompData;
+
+	u8 *adjhor = NULL;
+	u8 *adjvert = NULL;
+
+	u16 container;
+	int *ix1_w = NULL;
+	int *ix2_w = NULL;
+	u16 *total_ix = NULL;
+	u16 *total_cx = NULL;
+
+	u16 *total_adjhor = NULL;
+	u16 *total_adjvert = NULL;
+
+	logError(0, "%s\n", tag);
+	logError(0, "%s SS IX CX testes are starting...\n", tag);
+
+	//read the SS compensation data
+	ret = readSelfSenseCompensationData(SS_TOUCH, &ssCompData);
+	if (ret < 0) {
+		logError(1, "%s production_test_data: ", tag);
+		logError(1, "readSelfSenseCompensationData failed... ", tag);
+		logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+		return (ret | ERROR_PROD_TEST_DATA);
+	}
+
+	//********************** SS FORCE IX *********************************/
+	//SS IX1 FORCE TEST
+	logError(0, "%s SS IX1 FORCE TEST:\n", tag);
+	if (todo->SelfForceIx1 == 1) {
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX1_FORCE_MIN_MAX,
+			&thresholds,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX1_FORCE_MIN_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		container = (u16) ssCompData.f_ix1;
+
+		 //check the limits
+		ret = checkLimitsMinMax(&container,
+			1,
+			1,
+			thresholds[0],
+			thresholds[1]);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMinMax ");
+			logError(1, "SS IX1 FORCE TEST failed... ");
+			logError(1, "ERROR COUNT = %d\n", ret);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else
+			logError(0, "%s SS IX1 FORCE TEST:......OK\n\n", tag);
+	}
+
+	kfree(thresholds);
+	thresholds = NULL;
+	//SS IX2 FORCE TEST
+	logError(0, "%s SS IX2 FORCE MIN MAX TEST:\n", tag);
+	if (todo->SelfForceIx2 == 1) {
+		//load the min thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_FORCE_MAP_MIN,
+			&thresholds_min,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != ssCompData.header.force_node
+			|| tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX2_FORCE_MAP_MIN ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//load the max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_FORCE_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != ssCompData.header.force_node
+			|| tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits");
+			logError(1, "SS_IX2_FORCE_MAP_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the values with thresholds
+		ret = checkLimitsMap(ssCompData.ix2_fm,
+			ssCompData.header.force_node,
+			1,
+			thresholds_min,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMap SS IX2 FORCE failed... ");
+			logError(1, "ERROR COUNT = %d\n", ret);
+			logError(0, "%s SS IX2 FORCE MIN MAX TEST:..........");
+			logError(0, "FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS IX2 FORCE MIN MAX TEST:.....", tag);
+			logError(0, "OK\n\n");
+		}
+
+		kfree(thresholds_min);
+		thresholds_min = NULL;
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+	} else {
+		logError(0, "%s SS IX2 FORCE MIN MAX TEST:...........", tag);
+		logError(0, "KIPPED\n\n");
+	}
+
+	logError(0, "%s SS IX2 FORCE ADJ TEST:\n", tag);
+	if (todo->SelfForceIx2Adj == 1) {
+		//SS IX2 FORCE ADJV TEST
+		logError(0, "%s SS IX2 FORCE ADJVERT TEST:\n", tag);
+		ret = computeAdjVert(ssCompData.ix2_fm,
+			ssCompData.header.force_node,
+			1,
+			&adjvert);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeAdjVert SS IX2 FORCE ADJV ");
+			logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		logError(0, "%s SS IX2 FORCE ADJV computed!\n", tag);
+
+		//load the max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_FORCE_ADJV_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != ssCompData.header.force_node - 1
+				|| tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX2_FORCE_ADJV_MAP_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		 //check the values with thresholds
+		ret = checkLimitsMapAdj(adjvert,
+			ssCompData.header.force_node - 1,
+			1,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMap SS IX2 FORCE failed... ");
+			logError(0, "FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS IX2 FORCE ADJV TEST:", tag);
+			logError(0, ".................OK\n\n");
+		}
+
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+		kfree(adjvert);
+		adjvert = NULL;
+
+	} else {
+		logError(0, "%s SS IX2 FORCE ADJ TEST:", tag);
+		logError(0, ".................SKIPPED\n\n");
+	}
+
+	//SS TOTAL FORCE IX
+	logError(0, "%s SS TOTAL IX FORCE TEST:\n", tag);
+	if (todo->SelfForceIxTotal == 1 || todo->SelfForceIxTotalAdj == 1) {
+		logError(0, "%s Reading TOTAL IX FORCE Weights...\n", tag);
+
+		 //load the IX1 weight
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX1_FORCE_W,
+			&ix1_w,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX1_FORCE_W failed... ERROR %02X\n",
+					tag, ERROR_PROD_TEST_DATA);
+			return (ret | ERROR_PROD_TEST_DATA);
+		}
+
+		 //load the IX2 weight
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_FORCE_W,
+			&ix2_w,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX1_FORCE_W failed... ERROR %02X\n",
+				tag, ERROR_PROD_TEST_DATA);
+			return (ret | ERROR_PROD_TEST_DATA);
+		}
+
+		logError(0, "%s Weights: IX1_W = %d   IX2_W = %d\n",
+				tag, *ix1_w, *ix2_w);
+		ret = computeTotal(ssCompData.ix2_fm, ssCompData.f_ix1,
+			ssCompData.header.force_node,
+			1,
+			*ix1_w,
+			*ix2_w,
+			&total_ix);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeTotal Ix Force failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		kfree(ix1_w);
+		ix1_w = NULL;
+		kfree(ix2_w);
+		ix2_w = NULL;
+
+		logError(0, "%s SS TOTAL IX FORCE MIN MAX TEST:\n", tag);
+		if (todo->SelfForceIxTotal == 1) {
+			 //load the min thresholds
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_IX_FORCE_MAP_MIN,
+				&thresholds_min,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != ssCompData.header.force_node
+				|| tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_IX_FORCE_MAP_MIN ");
+				logError(1, "failed... ERROR %02X\n",
+						ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//load the max thresholds
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_IX_FORCE_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != ssCompData.header.force_node
+				|| tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_IX_FORCE_MAP_MAX ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			 //check the values with thresholds
+			ret = checkLimitsMapTotal(total_ix,
+				ssCompData.header.force_node,
+				1,
+				thresholds_min,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMap SS TOTAL IX FORCE");
+				logError(1, "failed... ERROR COUNT = %d\n",
+					ret);
+				logError(0, "%s SS TOTAL IX FORCE MIN MAX ",
+					tag);
+				logError(0, "TEST:.................FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL IX FORCE MIN MAX ",
+					tag);
+				logError(0, "TEST:.................OK\n\n");
+			}
+
+			kfree(thresholds_min);
+			thresholds_min = NULL;
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+		} else {
+			logError(0, "%s SS TOTAL IX FORCE MIN MAX TEST:", tag);
+			logError(0, ".................SKIPPED\n");
+		}
+
+		logError(0, "%s SS TOTAL IX FORCE ADJ TEST:\n", tag);
+		if (todo->SelfForceIxTotalAdj == 1) {
+			//SS TOTAL IX FORCE ADJV TEST
+			logError(0, "%s SS TOTAL IX FORCE ADJVERT TEST:\n",
+				tag);
+			ret = computeAdjVertTotal(total_ix,
+				ssCompData.header.force_node,
+				1,
+				&total_adjvert);
+			if (ret < 0) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "computeAdjVert SS TOTAL IX ");
+				logError(1, "FORCE ADJV failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			logError(0, "%s SS TOTAL IX FORCE ADJV computed!\n",
+				tag);
+
+			 //load the max thresholds
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_IX_FORCE_ADJV_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns);
+			if (ret < 0
+				|| (trows != ssCompData.header.force_node - 1
+				|| tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_IX_FORCE_ADJV_MAP_MAX");
+				logError(1, "... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			 //check the values with thresholds
+			ret = checkLimitsMapAdjTotal(total_adjvert,
+				ssCompData.header.force_node - 1,
+				1,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMap SS TOTAL IX ");
+				logError(1, "FORCE failed... ");
+				logError(1, "ERROR COUNT = %d\n", ret);
+				logError(0, "%s SS TOTAL IX FORCE ADJV TEST:",
+					tag);
+				logError(0, ".................FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL IX FORCE ADJV TEST:",
+					tag);
+				logError(0, ".................OK\n\n");
+			}
+
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+			kfree(total_adjvert);
+			total_adjvert = NULL;
+		} else {
+			logError(0, "%s SS TOTAL IX FORCE ADJ TEST:");
+			logError(0, ".................SKIPPED\n");
+		}
+
+		kfree(total_ix);
+		total_ix = NULL;
+	} else {
+		logError(0, "%s SS TOTAL IX FORCE TEST:", tag);
+		logError(0, ".................SKIPPED\n\n");
+	}
+
+
+	//************** SS SENSE IX *******************/
+	//SS IX1 SENSE TEST
+	logError(0, "%s SS IX1 SENSE TEST:\n", tag);
+	if (todo->SelfSenseIx1 == 1) {
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX1_SENSE_MIN_MAX,
+			&thresholds,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX1_SENSE_MIN_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		container = (u16) ssCompData.s_ix1;
+		ret = checkLimitsMinMax(&container,
+			1,
+			1,
+			thresholds[0],
+			thresholds[1]); //check the limits
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMinMax SS IX1 SENSE TEST ");
+			logError(1, "failed... ERROR COUNT = %d\n", ret);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS IX1 SENSE TEST:..............", tag);
+			logError(0, "...OK\n\n");
+		}
+	} else {
+		logError(0, "%s SS IX1 SENSE TEST:.................", tag);
+		logError(0, "SKIPPED\n\n");
+	}
+
+	kfree(thresholds);
+	thresholds = NULL;
+	//SS IX2 SENSE TEST
+	logError(0, "%s SS IX2 SENSE MIN MAX TEST:\n", tag);
+	if (todo->SelfSenseIx2 == 1) {
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_SENSE_MAP_MIN,
+			&thresholds_min,
+			&trows,
+			&tcolumns); //load the min thresholds
+		if (ret < 0 || (trows != 1
+			|| tcolumns != ssCompData.header.sense_node)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "S_IX2_SENSE_MAP_MIN failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_SENSE_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns); //load the max thresholds
+		if (ret < 0 || (trows != 1
+			|| tcolumns != ssCompData.header.sense_node)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX2_SENSE_MAP_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the values with thresholds
+		ret = checkLimitsMap(ssCompData.ix2_sn,
+			1,
+			ssCompData.header.sense_node,
+			thresholds_min,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMap SS IX2 SENSE failed... ");
+			logError(1, "ERROR COUNT = %d\n", ret);
+			logError(0, "%s SS IX2 SENSE MIN MAX TEST:.....", tag);
+			logError(0, "............FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS IX2 SENSE MIN MAX TEST:", tag);
+			logError(0, ".................OK\n\n");
+		}
+
+		kfree(thresholds_min);
+		thresholds_min = NULL;
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+	} else {
+		logError(0, "%s SS IX2 SENSE MIN MAX TEST:..............", tag);
+		logError(0, "...SKIPPED\n\n");
+	}
+
+	logError(0, "%s SS IX2 SENSE ADJ TEST:\n", tag);
+	if (todo->SelfSenseIx2Adj == 1) {
+		//SS IX2 SENSE ADJH TEST
+		logError(0, "%s SS IX2 SENSE ADJHORIZ TEST:\n", tag);
+		ret = computeAdjHoriz(ssCompData.ix2_sn,
+			1,
+			ssCompData.header.sense_node,
+			&adjhor);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeAdjHoriz SS IX2 SENSE ADJH ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		logError(0, "%s SS IX2 SENSE ADJ HORIZ computed!\n", tag);
+
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_SENSE_ADJH_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns); //load the max thresholds
+		if (ret < 0 || (trows != 1
+			|| tcolumns != ssCompData.header.sense_node - 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX2_SENSE_ADJH_MAP_MAX ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the values with thresholds
+		ret = checkLimitsMapAdj(adjhor,
+			1,
+			ssCompData.header.sense_node - 1,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMapAdj SS IX2 SENSE ADJH ");
+			logError(1, "failed... ERROR COUNT = %d\n", ret);
+			logError(0, "%s SS IX2 SENSE ADJH TEST:.......", tag);
+			logError(0, "..........FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS IX2 SENSE ADJH TEST:........", tag);
+			logError(0, ".........OK\n\n");
+		}
+
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+		kfree(adjhor);
+		adjhor = NULL;
+	} else {
+		logError(0, "%s SS IX2 SENSE ADJ TEST:.................", tag);
+		logError(0, "SKIPPED\n", tag);
+	}
+
+	//SS TOTAL IX SENSE
+	logError(0, "%s SS TOTAL IX SENSE TEST:\n", tag);
+	if (todo->SelfSenseIxTotal == 1 || todo->SelfSenseIxTotalAdj == 1) {
+		logError(0, "%s Reading TOTAL IX SENSE Weights...\n", tag);
+		//load the IX1 weight
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX1_SENSE_W,
+			&ix1_w,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX1_SENSE_W failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//load the IX2 weight
+		ret = parseProductionTestLimits(path_limits,
+			SS_IX2_SENSE_W,
+			&ix2_w,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX1_SENSE_W failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		logError(0, "%s Weights: IX1_W = %d   IX2_W = %d\n",
+			tag, *ix1_w, *ix2_w);
+
+		ret = computeTotal(ssCompData.ix2_sn,
+			ssCompData.s_ix1,
+			1,
+			ssCompData.header.sense_node,
+			*ix1_w,
+			*ix2_w,
+			&total_ix);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeTotal Ix Sense ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		kfree(ix1_w);
+		ix1_w = NULL;
+		kfree(ix2_w);
+		ix2_w = NULL;
+
+		logError(0, "%s SS TOTAL IX SENSE MIN MAX TEST:\n", tag);
+		//load the min thresholds
+		if (todo->SelfSenseIxTotal == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_IX_SENSE_MAP_MIN,
+				&thresholds_min,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1
+				|| tcolumns != ssCompData.header.sense_node)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_IX_SENSE_MAP_MIN ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//load the max thresholds
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_IX_SENSE_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1 ||
+				tcolumns != ssCompData.header.sense_node)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_IX_SENSE_MAP_MAX ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//check the values with thresholds
+			ret = checkLimitsMapTotal(total_ix,
+				1,
+				ssCompData.header.sense_node,
+				thresholds_min,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMap SS TOTAL IX SENSE");
+				logError(1, " failed... ERROR COUNT = %d\n",
+					ret);
+				logError(0, "%s SS TOTAL IX SENSE MIN MAX ",
+					tag);
+				logError(0, "TEST:.................FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL IX SENSE MIN MAX ",
+					tag);
+				logError(0, "TEST:.................OK\n\n");
+			}
+
+			kfree(thresholds_min);
+			thresholds_min = NULL;
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+		} else {
+			logError(0, "%s SS TOTAL IX SENSE MIN MAX ", tag);
+			logError(0, "TEST:.................SKIPPED\n");
+		}
+
+
+		logError(0, "%s SS TOTAL IX SENSE ADJ TEST:\n", tag);
+		if (todo->SelfSenseIxTotalAdj == 1) {
+			//SS TOTAL IX SENSE ADJH TEST
+			logError(0, "%s SS TOTAL IX SENSE ADJHORIZ TEST:\n",
+				tag);
+			ret = computeAdjHorizTotal(total_ix,
+				1,
+				ssCompData.header.sense_node,
+				&total_adjhor);
+			if (ret < 0) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "computeAdjHoriz SS TOTAL ");
+				logError(1, "IXSENSE ADJH failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			logError(0, "%s SS TOTAL IX SENSE ADJ HORIZ ", tag);
+			logError(0, "computed!\n");
+
+			//load the max thresholds
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_IX_SENSE_ADJH_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1
+			    || tcolumns != ssCompData.header.sense_node - 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_IX_SENSE_ADJH_MAP_MAX ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//check the values with thresholds
+			ret = checkLimitsMapAdjTotal(total_adjhor,
+				1,
+				ssCompData.header.sense_node - 1,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMapAdj SS TOTAL ");
+				logError(1, "IX SENSE ADJH failed... ");
+				logError(1, "ERROR COUNT = %d\n", ret);
+				logError(0, "%s SS TOTAL IX SENSE ADJH ", tag);
+				logError(0, "TEST:.................FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL IX SENSE ADJH ", tag);
+				logError(0, "TEST:.................OK\n\n");
+			}
+
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+			kfree(total_adjhor);
+			total_adjhor = NULL;
+		} else {
+			logError(0, "%s SS TOTAL IX SENSE ADJ TEST:.....", tag);
+			logError(0, "............SKIPPED\n");
+		}
+		kfree(total_ix);
+		total_ix = NULL;
+	} else {
+		logError(0, "%s SS TOTAL IX SENSE TEST:............", tag);
+		logError(0, ".....SKIPPED\n");
+	}
+
+	//************************ SS SENSE CX *******************************/
+	//SS CX1 FORCE TEST
+	logError(0, "%s SS CX1 FORCE TEST:\n", tag);
+	if (todo->SelfForceCx1 == 1) {
+
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX1_FORCE_MIN_MAX,
+			&thresholds,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_CX1_FORCE_MIN_MAX ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the limits
+		container = (u16) ssCompData.f_cx1;
+		ret = checkLimitsMinMax(&container,
+			1,
+			1,
+			thresholds[0],
+			thresholds[1]);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMinMax SS CX1 FORCE TEST ");
+			logError(1, "failed... ERROR COUNT = %d\n", ret);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS CX1 FORCE TEST:.............", tag);
+			logError(0, "....OK\n\n");
+		}
+		kfree(thresholds);
+		thresholds = NULL;
+	} else {
+		logError(0, "%s SS CX1 FORCE TEST:.................", tag);
+		logError(0, "SKIPPED\n\n");
+	}
+
+	//SS CX2 FORCE TEST
+	logError(0, "%s SS CX2 FORCE MIN MAX TEST:\n", tag);
+	if (todo->SelfForceCx2 == 1) {
+		//load the min thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX2_FORCE_MAP_MIN,
+			&thresholds_min,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != ssCompData.header.force_node
+			|| tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "SS_CX2_FORCE_MAP_MIN ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//load the max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX2_FORCE_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != ssCompData.header.force_node
+			|| tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_CX2_FORCE_MAP_MAX ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the values with thresholds
+		ret = checkLimitsMap(ssCompData.cx2_fm,
+			ssCompData.header.force_node,
+			1,
+			thresholds_min,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "%checkLimitsMap SS CX2 FORCE ");
+			logError(1, "%failed... ERROR COUNT = %d\n", ret);
+			logError(0, "%s SS CX2 FORCE MIN MAX TEST:.....", tag);
+			logError(0, "............FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS CX2 FORCE MIN MAX TEST:......", tag);
+			logError(0, "...........OK\n\n");
+		}
+
+		kfree(thresholds_min);
+		thresholds_min = NULL;
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+	} else {
+		logError(0, "%s SS CX2 FORCE MIN MAX TEST:..............", tag);
+		logError(0, "...SKIPPED\n");
+	}
+
+	logError(0, "%s SS CX2 FORCE ADJ TEST:\n", tag);
+	if (todo->SelfForceCx2Adj == 1) {
+		//SS CX2 FORCE ADJV TEST
+		logError(0, "%s SS CX2 FORCE ADJVERT TEST:\n", tag);
+		//comepute the ADJV for CX2  FORCE
+		ret = computeAdjVert(ssCompData.cx2_fm,
+			ssCompData.header.force_node,
+			1,
+			&adjvert);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeAdjVert SS CX2 FORCE ADJV ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		logError(0, "%s SS CX2 FORCE ADJV computed!\n", tag);
+
+		//load the max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX2_FORCE_ADJV_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != ssCompData.header.force_node - 1
+			|| tcolumns != 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_CX2_FORCE_ADJV_MAP_MAX ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the values with thresholds
+		ret = checkLimitsMapAdj(adjvert,
+			ssCompData.header.force_node - 1,
+			1,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMap SS IX2 FORCE ");
+			logError(1, "failed... ERROR COUNT = %d\n", ret);
+			logError(0, "%s SS CX2 FORCE ADJV TEST:......", tag);
+			logError(0, "...........FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS CX2 FORCE ADJV TEST:.....", tag);
+			logError(0, "............OK\n\n");
+		}
+
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+		kfree(adjvert);
+		adjvert = NULL;
+	} else {
+		logError(0, "%s SS CX2 FORCE ADJ TEST:.................", tag);
+		logError(0, "SKIPPED\n\n");
+	}
+	//SS TOTAL CX FORCE
+	logError(0, "%s SS TOTAL CX FORCE TEST:\n", tag);
+	if (todo->SelfForceCxTotal == 1 || todo->SelfForceCxTotalAdj == 1) {
+		ret = computeTotal(ssCompData.cx2_fm,
+			ssCompData.f_cx1,
+			ssCompData.header.force_node,
+			1,
+			CX1_WEIGHT,
+			CX2_WEIGHT,
+			&total_cx);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeTotal Cx Force failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			return (ret | ERROR_PROD_TEST_DATA);
+		}
+
+		logError(0, "%s SS TOTAL CX FORCE MIN MAX TEST:\n", tag);
+		//load the min thresholds
+		if (todo->SelfForceCxTotal == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_CX_FORCE_MAP_MIN,
+				&thresholds_min,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != ssCompData.header.force_node
+				|| tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_CX_FORCE_MAP_MIN ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//load the max thresholds
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_CX_FORCE_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != ssCompData.header.force_node
+				|| tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_CX_FORCE_MAP_MAX ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//check the values with thresholds
+			ret = checkLimitsMapTotal(total_cx,
+				ssCompData.header.force_node,
+				1,
+				thresholds_min,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMap SS TOTAL FORCE ");
+				logError(1, "failed... ERROR COUNT = %d\n",
+					ret);
+				logError(0, "%s SS TOTAL FORCE MIN MAX ", tag);
+				logError(0, "TEST:.................FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL FORCE MIN MAX ", tag);
+				logError(0, "TEST:.................OK\n\n");
+			}
+
+			kfree(thresholds_min);
+			thresholds_min = NULL;
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+		} else {
+			logError(0, "%s SS TOTAL CX FORCE MIN MAX TEST:", tag);
+			logError(0, ".................SKIPPED\n");
+		}
+
+		//SS TOTAL CX FORCE ADJV TEST
+		logError(0, "%s SS TOTAL CX FORCE ADJ TEST:\n", tag);
+		if (todo->SelfForceCxTotalAdj == 1) {
+			logError(0, "%s SS TOTAL CX FORCE ADJVERT ", tag);
+			logError(0, "TEST:\n");
+
+			//comepute the ADJV for CX2  FORCE
+			ret = computeAdjVertTotal(total_cx,
+				ssCompData.header.force_node,
+				1,
+				&total_adjvert);
+			if (ret < 0) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "computeAdjVert SS TOTAL CX FORCE");
+				logError(1, " ADJV failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			logError(0, "%s SS TOTAL CX FORCE ADJV computed!\n",
+				tag);
+
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_CX_FORCE_ADJV_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns); //load the max thresholds
+			if (ret < 0
+				|| (trows != ssCompData.header.force_node - 1
+					|| tcolumns != 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_CX_FORCE_ADJV_MAP_MAX ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//check the values with thresholds
+			ret = checkLimitsMapAdjTotal(total_adjvert,
+				ssCompData.header.force_node - 1,
+				1,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMap SS TOTAL CX FORCE");
+				logError(1, " failed... ERROR COUNT = %d\n",
+					ret);
+				logError(0, "%s SS TOTAL CX FORCE ADJV ", tag);
+				logError(0, "TEST:.................FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL CX FORCE ADJV ", tag);
+				logError(0, "TEST:.................OK\n\n");
+			}
+
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+			kfree(total_adjvert);
+			total_adjvert = NULL;
+
+		} else {
+			logError(0, "%s SS TOTAL CX FORCE ADJ TEST:......",
+				tag);
+			logError(0, "..........SKIPPED\n");
+		}
+		kfree(total_cx);
+		total_cx = NULL;
+	} else {
+		logError(0, "%s SS TOTAL CX FORCE TEST:.................", tag);
+		logError(0, "SKIPPED\n\n");
+	}
+
+
+	//**************** SS SENSE CX **************************************/
+	//SS CX1 SENSE TEST
+	logError(0, "%s SS CX1 SENSE TEST:\n", tag);
+	if (todo->SelfSenseCx1 == 1) {
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX1_SENSE_MIN_MAX,
+			&thresholds,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1 || tcolumns != 2)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_CX1_SENSE_MIN_MAX failed");
+			logError(1, "... ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		container = (u16) ssCompData.s_cx1;
+		//check the limits
+		ret = checkLimitsMinMax(&container,
+			1,
+			1,
+			thresholds[0],
+			thresholds[1]);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMinMax SS CX1 SENSE TEST ");
+			logError(1, "failed... ERROR COUNT = %d\n", ret);
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS CX1 SENSE TEST:..............", tag);
+			logError(0, "...OK\n\n");
+		}
+		kfree(thresholds);
+		thresholds = NULL;
+	} else {
+		logError(0, "%s SS CX1 SENSE TEST:.................", tag);
+		logError(0, "SKIPPED\n\n");
+	}
+
+
+	//SS CX2 SENSE TEST
+	logError(0, "%s SS CX2 SENSE MIN MAX TEST:\n", tag);
+	if (todo->SelfSenseCx2 == 1) {
+		//load the min thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX2_SENSE_MAP_MIN,
+			&thresholds_min,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1
+			|| tcolumns != ssCompData.header.sense_node)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_CX2_SENSE_MAP_MIN failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//load the max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX2_SENSE_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1
+			|| tcolumns != ssCompData.header.sense_node)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_CX2_SENSE_MAP_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the values with thresholds
+		ret = checkLimitsMap(ssCompData.cx2_sn,
+			1,
+			ssCompData.header.sense_node,
+			thresholds_min,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMap SS CX2 SENSE failed... ");
+			logError(1, "ERROR COUNT = %d\n", ret);
+			logError(0, "%s SS CX2 SENSE MIN MAX TEST:......", tag);
+			logError(0, "...........FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS CX2 SENSE MIN MAX TEST:", tag);
+			logError(0, ".................OK\n\n");
+		}
+
+		kfree(thresholds_min);
+		thresholds_min = NULL;
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+	} else {
+		logError(0, "%s SS CX2 SENSE MIN MAX TEST:.........", tag);
+		logError(0, "........SKIPPED\n");
+	}
+	logError(0, "%s SS CX2 SENSE ADJ TEST:\n", tag);
+	if (todo->SelfSenseCx2Adj == 1) {
+		//SS CX2 SENSE ADJH TEST
+		logError(0, "%s SS CX2 SENSE ADJHORIZ TEST:\n", tag);
+		ret = computeAdjHoriz(ssCompData.cx2_sn,
+			1,
+			ssCompData.header.sense_node,
+			&adjhor);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeAdjHoriz SS CX2 SENSE ADJH ");
+			logError(1, "failed... ERROR %02X\n",
+				ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+		logError(0, "%s SS CX2 SENSE ADJH computed!\n", tag);
+
+		//load the max thresholds
+		ret = parseProductionTestLimits(path_limits,
+			SS_CX2_SENSE_ADJH_MAP_MAX,
+			&thresholds_max,
+			&trows,
+			&tcolumns);
+		if (ret < 0 || (trows != 1
+			|| tcolumns != ssCompData.header.sense_node - 1)) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "parseProductionTestLimits ");
+			logError(1, "SS_IX2_SENSE_MAP_MAX failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		//check the values with thresholds
+		ret = checkLimitsMapAdj(adjhor,
+			1,
+			ssCompData.header.sense_node - 1,
+			thresholds_max);
+		if (ret != OK) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "checkLimitsMapAdj SS CX2 SENSE ADJH ");
+			logError(1, "failed... ERROR COUNT = %d\n", ret);
+			logError(0, "%s SS CX2 SENSE ADJH TEST:.........", tag);
+			logError(0, "........FAIL\n\n");
+			count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+		} else {
+			logError(0, "%s SS CX2 SENSE ADJH TEST:.........", tag);
+			logError(0, "........OK\n");
+		}
+
+		kfree(thresholds_max);
+		thresholds_max = NULL;
+		kfree(adjhor);
+		adjhor = NULL;
+	} else {
+		logError(0, "%s SS CX2 SENSE ADJ TEST:.................", tag);
+		logError(0, "SKIPPED\n\n");
+	}
+
+	//SS TOTAL CX SENSE
+	logError(0, "%s SS TOTAL CX SENSE TEST:\n", tag);
+	if (todo->SelfSenseCxTotal == 1 || todo->SelfSenseCxTotalAdj == 1) {
+		ret = computeTotal(ssCompData.cx2_sn,
+			ssCompData.s_cx1,
+			1,
+			ssCompData.header.sense_node,
+			CX1_WEIGHT,
+			CX2_WEIGHT,
+			&total_cx);
+		if (ret < 0) {
+			logError(1, "%s production_test_data: ", tag);
+			logError(1, "computeTotal Cx Sense failed... ");
+			logError(1, "ERROR %02X\n", ERROR_PROD_TEST_DATA);
+			ret |= ERROR_PROD_TEST_DATA;
+			goto ERROR_LIMITS;
+		}
+
+		logError(0, "%s SS TOTAL CX SENSE MIN MAX TEST:\n", tag);
+		//load the min thresholds
+		if (todo->SelfSenseCxTotal == 1) {
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_CX_SENSE_MAP_MIN,
+				&thresholds_min,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1
+				|| tcolumns != ssCompData.header.sense_node)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_CX_SENSE_MAP_MIN ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			//load the max thresholds
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_CX_SENSE_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns);
+			if (ret < 0 || (trows != 1
+				|| tcolumns != ssCompData.header.sense_node)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_CX_SENSE_MAP_MAX ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//check the values with thresholds
+			ret = checkLimitsMapTotal(total_cx,
+				1,
+				ssCompData.header.sense_node,
+				thresholds_min,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "heckLimitsMap SS TOTAL CX SENSE ");
+				logError(1, "failed... ERROR COUNT = %d\n",
+					ret);
+				logError(0, "%s SS TOTAL CX SENSE MIN ", tag);
+				logError(0, "MAX TEST:.................");
+				logError(0, "FAIL\n\n");
+				count_fail += 1;
+			if (stop_on_fail)
+				goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL CX SENSE MIN ", tag);
+				logError(0, "MAX TEST:................OK\n\n");
+			}
+
+			kfree(thresholds_min);
+			thresholds_min = NULL;
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+		} else {
+			logError(0, "%s SS TOTAL CX SENSE MIN MAX TEST:", tag);
+			logError(0, ".................SKIPPED\n");
+		}
+
+
+		//SS TOTAL IX SENSE ADJH TEST
+		logError(0, "%s SS TOTAL CX SENSE ADJ TEST:\n", tag);
+		if (todo->SelfSenseCxTotalAdj == 1) {
+			logError(0, "%s SS TOTAL CX SENSE ADJHORIZ TEST:\n",
+				tag);
+			ret = computeAdjHorizTotal(total_cx,
+				1,
+				ssCompData.header.sense_node,
+				&total_adjhor);
+			if (ret < 0) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "computeAdjHoriz SS TOTAL CX ");
+				logError(1, "SENSE ADJH failed... ");
+				logError(1, "ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+			logError(0, "%s SS TOTAL CX SENSE ADJ HORIZ ", tag);
+			logError(0, "computed!\n");
+
+			ret = parseProductionTestLimits(path_limits,
+				SS_TOTAL_CX_SENSE_ADJH_MAP_MAX,
+				&thresholds_max,
+				&trows,
+				&tcolumns); //load the max thresholds
+			if (ret < 0 || (trows != 1 ||
+				tcolumns != ssCompData.header.sense_node - 1)) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "parseProductionTestLimits ");
+				logError(1, "SS_TOTAL_CX_SENSE_ADJH_MAP_MAX ");
+				logError(1, "failed... ERROR %02X\n",
+					ERROR_PROD_TEST_DATA);
+				ret |= ERROR_PROD_TEST_DATA;
+				goto ERROR_LIMITS;
+			}
+
+			//check the values with thresholds
+			ret = checkLimitsMapAdjTotal(total_adjhor,
+				1,
+				ssCompData.header.sense_node - 1,
+				thresholds_max);
+			if (ret != OK) {
+				logError(1, "%s production_test_data: ", tag);
+				logError(1, "checkLimitsMapAdj SS TOTAL ");
+				logError(1, "CX SENSE ADJH failed... ");
+				logError(1, "ERROR COUNT = %d\n", ret);
+				logError(0, "%s SS TOTAL CX SENSE ADJH ", tag);
+				logError(0, "TEST:...FAIL\n\n");
+				count_fail += 1;
+				if (stop_on_fail)
+					goto ERROR;
+			} else {
+				logError(0, "%s SS TOTAL CX SENSE ADJH TEST:",
+					tag);
+				logError(0, ".................OK\n\n");
+			}
+
+			kfree(thresholds_max);
+			thresholds_max = NULL;
+			kfree(total_adjhor);
+			total_adjhor = NULL;
+		} else {
+			logError(0, "%s SS TOTAL CX SENSE ADJ TEST:.", tag);
+			logError(0, "SKIPPED\n");
+		}
+		kfree(total_cx);
+		total_cx = NULL;
+	} else
+		logError(0, "%s SS TOTAL CX SENSE TEST:.....SKIPPED\n", tag);
+
+
+ERROR:
+	logError(0, "%s\n", tag);
+	if (count_fail == 0) {
+		kfree(ssCompData.ix2_fm);
+		ssCompData.ix2_fm = NULL;
+		kfree(ssCompData.ix2_sn);
+		ssCompData.ix2_sn = NULL;
+		kfree(ssCompData.cx2_fm);
+		ssCompData.cx2_fm = NULL;
+		kfree(ssCompData.cx2_sn);
+		ssCompData.cx2_sn = NULL;
+		logError(0, "%s SS IX CX testes finished!........OK\n\n", tag);
+		ret = OK;
+	} else {
+		//print all kind of data in just one row for readability reason
+		print_frame_u8("SS Init Data Ix2_fm = ",
+				array1dTo2d_u8(ssCompData.ix2_fm,
+				ssCompData.header.force_node,
+				ssCompData.header.force_node),
+				1,
+				ssCompData.header.force_node);
+		print_frame_u8("SS Init Data Cx2_fm = ",
+				array1dTo2d_u8(ssCompData.cx2_fm,
+				ssCompData.header.force_node,
+				ssCompData.header.force_node),
+				1,
+				ssCompData.header.force_node);
+		print_frame_u8("SS Init Data Ix2_sn = ",
+				array1dTo2d_u8(ssCompData.ix2_sn,
+				ssCompData.header.sense_node,
+				ssCompData.header.sense_node),
+				1,
+				ssCompData.header.sense_node);
+		print_frame_u8("SS Init Data Cx2_sn = ",
+				array1dTo2d_u8(ssCompData.cx2_sn,
+				ssCompData.header.sense_node,
+				ssCompData.header.sense_node),
+				1,
+				ssCompData.header.sense_node);
+		logError(0, "%s SS IX CX testes finished!.................",
+			tag);
+		logError(0, "FAILED  fails_count = %d\n\n", count_fail);
+		kfree(thresholds);
+		kfree(thresholds_min);
+		kfree(thresholds_max);
+		kfree(adjhor);
+		kfree(adjvert);
+		kfree(ix1_w);
+		kfree(ix2_w);
+		kfree(total_ix);
+		kfree(total_cx);
+		kfree(total_adjhor);
+		kfree(total_adjvert);
+		kfree(ssCompData.ix2_fm);
+		kfree(ssCompData.ix2_sn);
+		kfree(ssCompData.cx2_fm);
+		kfree(ssCompData.cx2_sn);
+		ret = (ERROR_TEST_CHECK_FAIL | ERROR_PROD_TEST_DATA);
+	}
+	return ret;
+ERROR_LIMITS:
+	kfree(thresholds);
+	kfree(thresholds_min);
+	kfree(thresholds_max);
+	kfree(adjhor);
+	kfree(adjvert);
+	kfree(ix1_w);
+	kfree(ix2_w);
+	kfree(total_ix);
+	kfree(total_cx);
+	kfree(total_adjhor);
+	kfree(total_adjvert);
+	kfree(ssCompData.ix2_fm);
+	kfree(ssCompData.ix2_sn);
+	kfree(ssCompData.cx2_fm);
+	kfree(ssCompData.cx2_sn);
+	return ret;
+}
+
+int production_test_data(char *path_limits, int stop_on_fail,
+			struct TestToDo *todo)
+{
+	int res = OK, ret;
+
+	if (todo == NULL) {
+		logError(0, "%s %s: ", tag, __func__);
+		logError(0, "No TestToDo specified!! ");
+		logError(0, "ERROR = %02X\n",
+			(ERROR_OP_NOT_ALLOW | ERROR_PROD_TEST_DATA));
+		return (ERROR_OP_NOT_ALLOW | ERROR_PROD_TEST_DATA);
+	}
+
+	logError(0, "%s DATA Production test is starting...\n", tag);
+	ret = production_test_ms_raw(path_limits, stop_on_fail, todo);
+	res |= ret;
+	if (ret < 0) {
+		logError(0, "%s %s: ", tag, __func__);
+		logError(0, "production_test_ms_raw failed... ");
+		logError(0, "ERROR = %02X\n", ret);
+		if (stop_on_fail == 1)
+			goto END;
+	}
+
+	ret = production_test_ms_cx(path_limits, stop_on_fail, todo);
+	res |= ret;
+	if (ret < 0) {
+		logError(0, "%s %s: ", tag, __func__);
+		logError(0, "production_test_ms_cx failed... ");
+		logError(0, "ERROR = %02X\n", ret);
+		if (stop_on_fail == 1)
+			goto END;
+	}
+
+	ret = production_test_ss_raw(path_limits, stop_on_fail, todo);
+	res |= ret;
+	if (ret < 0) {
+		logError(0, "%s %s: ", tag, __func__);
+		logError(0, "production_test_ss_raw failed... ");
+		logError(0, "ERROR = %02X\n", ret);
+		if (stop_on_fail == 1)
+			goto END;
+	}
+
+	ret = production_test_ss_ix_cx(path_limits, stop_on_fail, todo);
+	res |= ret;
+	if (ret < 0) {
+		logError(0, "%s %s: ", tag, __func__);
+		logError(0, "production_test_ss_ix_cx failed... ");
+		logError(0, "ERROR = %02X\n", ret);
+		if (stop_on_fail == 1)
+			goto END;
+	}
+
+END:
+	if (res < OK)
+		logError(0, "%s DATA Production test failed!\n", tag);
+	else
+		logError(0, "%s DATA Production test finished!\n", tag);
+	return res;
+}
+
+
+int save_mp_flag(u32 signature)
+{
+	int res = -1;
+	int i;
+	u8 cmd[6] = {FTS_CMD_WRITE_MP_FLAG, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	u32ToU8(signature, &cmd[2]);
+
+	logError(0, "%s Starting Saving Flag with signature = %08X ...\n",
+		tag, signature);
+
+	for (i = 0; i < SAVE_FLAG_RETRY && res < OK; i++) {
+		logError(0, "%s Attempt number %d to save mp flag !\n",
+			tag, i+1);
+		logError(0, "%s Command write flag sent...\n", tag);
+		res = fts_writeFwCmd(cmd, 6);
+		if (res >= OK)
+			res = save_cx_tuning();
+	}
+
+	if (res < OK) {
+		logError(1, "%s %s: ERROR %08X ...\n", tag, __func__, res);
+	} else {
+		logError(0, "%s Saving Flag DONE!\n", tag);
+		res = OK;
+	}
+	return res;
+}
+
+int parseProductionTestLimits(char *path, char *label,
+		int **data, int *row, int *column)
+{
+	int find = 0;
+	char *token = NULL;
+	int i = 0;
+	int j = 0;
+	int z = 0;
+
+	char *line2 = NULL;
+	char line[800];
+	int fd = -1;
+	char *buf = NULL;
+	int n, size, pointer = 0, ret = OK;
+	char *data_file = NULL;
+#ifndef LIMITS_H_FILE
+	const struct firmware *fw = NULL;
+	struct device *dev = NULL;
+
+	dev = getDev();
+	if (dev != NULL)
+		fd = request_firmware(&fw, path, dev);
+#else
+	fd = 0;
+#endif
+
+	if (fd != 0) {
+		logError(0, "%s %s: ERROR %02X\n",
+			tag, __func__, ERROR_FILE_NOT_FOUND);
+		return ERROR_FILE_NOT_FOUND;
+	}
+
+#ifndef LIMITS_H_FILE
+	size = fw->size;
+	data_file = (char *)fw->data;
+	logError(0, "%s Start to reading %s...\n", tag, path);
+#else
+	size = LIMITS_SIZE_NAME;
+	data_file = (char *)(LIMITS_ARRAY_NAME);
+#endif
+	logError(0, "%s The size of the limits file is %d bytes\n", tag, size);
+
+	while (find == 0) {
+		//start to look for the wanted label
+		if (readLine(&data_file[pointer], line, size-pointer, &n) < 0) {
+			find = -1;
+			break;
+		}
+		pointer += n;
+		//each header row start with
+		// *ex. *label, n_row, n_colum
+		if (line[0] != '*')
+			continue;
+
+		line2 = kstrdup(line, GFP_KERNEL);
+		if (line2 == NULL) {
+			logError(1, "%s %s:kstrdup ERR %02X\n",
+				tag, __func__, ERROR_ALLOC);
+			ret = ERROR_ALLOC;
+			goto END;
+		}
+		buf = line2;
+		line2 += 1;
+		token = strsep(&line2, ",");
+		//if the row is the wanted one i
+		//retrieve rows and columns info
+		if (strcmp(token, label) == 0) {
+			find = 1;
+			token = strsep(&line2, ",");
+			if (token != NULL) {
+				ret = kstrtoint(token, 10, row);
+				if (ret != 0)
+					return -EINVAL;
+				logError(0, "%s Row = %d\n", tag, *row);
+			} else {
+				logError(1, "%s %s 1:ERROR %02X\n",
+					tag, __func__, ERROR_FILE_PARSE);
+				//release_firmware(fw);
+				//return ERROR_FILE_PARSE;
+				ret = ERROR_FILE_PARSE;
+				goto END;
+			}
+			token = strsep(&line2, ",");
+			if (token != NULL) {
+				ret = kstrtoint(token, 10, column);
+				if (ret != 0)
+					return -EINVAL;
+				logError(0, "%s Column = %d\n", tag, *column);
+			} else {
+				logError(1, "%s %s 2: ERROR %02X\n",
+					tag, __func__, ERROR_FILE_PARSE);
+				//release_firmware(fw);
+				//return ERROR_FILE_PARSE;
+				ret = ERROR_FILE_PARSE;
+				goto END;
+			}
+			kfree(buf);
+			buf = NULL;
+			//allocate memory for containing data
+			*data = (int *)kmalloc_array(((*row) * (*column)),
+					sizeof(int), GFP_KERNEL);
+			j = 0;
+			if (*data == NULL) {
+				logError(1, "%s %s: ERROR %02X\n",
+					tag, __func__, ERROR_ALLOC);
+				//release_firmware(fw);
+				//return ERROR_ALLOC;
+				ret = ERROR_ALLOC;
+				goto END;
+			}
+			//start to read the data
+			for (i = 0; i < *row; i++) {
+				//line =  buf;
+				if (readLine(&data_file[pointer], line,
+					size-pointer, &n) < 0) {
+					logError(1, "%s %s : ERROR %02X\n",
+						tag, __func__, ERROR_FILE_READ);
+					//release_firmware(fw);
+					//return ERROR_FILE_READ
+					ret = ERROR_FILE_READ;
+					goto END;
+				}
+				pointer += n;
+				line2 = kstrdup(line, GFP_KERNEL);
+				if (line2 == NULL) {
+					logError(1, "%s %s: kstrdup ",
+						tag, __func__);
+					logError(1, "ERROR %02X\n",
+						ERROR_ALLOC);
+					ret = ERROR_ALLOC;
+					goto END;
+				}
+				buf = line2;
+				token = strsep(&line2, ",");
+				for (z = 0;
+					(z < *column) && (token != NULL); z++) {
+					ret = kstrtoint(token,
+						10,
+						((*data) + j));
+					if (ret != 0)
+						return -EINVAL;
+					j++;
+					token = strsep(&line2, ",");
+				}
+				kfree(buf);
+				buf = NULL;
+			}
+			//check that all the data are read
+			if (j == ((*row) * (*column))) {
+				logError(0, "%s READ DONE!\n", tag);
+				//release_firmware(fw);
+				//return OK;
+				ret = OK;
+				goto END;
+			}
+			logError(1, "%s %s 3:ERROR %02X\n",
+				tag, __func__, ERROR_FILE_PARSE);
+			//release_firmware(fw);
+			//return ERROR_FILE_PARSE;
+			ret = ERROR_FILE_PARSE;
+			goto END;
+		}
+		kfree(buf);
+		buf = NULL;
+
+	}
+	logError(1, "%s %s: ERROR %02X\n",
+		tag, __func__, ERROR_LABEL_NOT_FOUND);
+	ret = ERROR_LABEL_NOT_FOUND;
+END:
+	kfree(buf);
+#ifndef LIMITS_H_FILE
+	release_firmware(fw);
+#endif
+	return ret;
+
+}
+
+int readLine(char *data, char *line, int size, int *n)
+{
+	int i = 0;
+
+	if (size < 1)
+		return -EINVAL;
+
+	while (data[i] != '\n' && i < size) {
+		line[i] = data[i];
+		i++;
+	}
+	*n = i + 1;
+	line[i] = '\0';
+
+	return OK;
+}
+
+

st/fts_lib/ftsTest.h

@@ -0,0 +1,193 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                        FTS API for MP test                            **
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ */
+
+#ifndef __FTS_TEST_H
+#define __FTS_TEST_H
+
+#include "ftsSoftware.h"
+
+#define LIMITS_FILE                    "stm_fts_production_limits.csv"
+
+#define WAIT_FOR_FRESH_FRAMES          100 //ms
+#define WAIT_AFTER_SENSEOFF            50 //ms
+
+#define TIMEOUT_ITO_TEST_RESULT        200 //ms
+#define TIMEOUT_INITIALIZATION_TEST_RESULT 5000 //ms
+
+//LABELS PRODUCTION TEST LIMITS FILE
+#define MS_RAW_MIN_MAX                 "MS_RAW_DATA_MIN_MAX"
+#define MS_RAW_GAP                     "MS_RAW_DATA_GAP"
+#define MS_CX1_MIN_MAX                 "MS_TOUCH_ACTIVE_CX1_MIN_MAX"
+#define MS_CX2_MAP_MIN                 "MS_TOUCH_ACTIVE_CX2_MIN"
+#define MS_CX2_MAP_MAX                 "MS_TOUCH_ACTIVE_CX2_MAX"
+#define MS_CX2_ADJH_MAP_MAX            "MS_TOUCH_ACTIVE_CX2_ADJ_HORIZONTAL"
+#define MS_CX2_ADJV_MAP_MAX            "MS_TOUCH_ACTIVE_CX2_ADJ_VERTICAL"
+#define MS_TOTAL_CX_MAP_MIN            "MS_TOUCH_ACTIVE_TOTAL_CX_MIN"
+#define MS_TOTAL_CX_MAP_MAX            "MS_TOUCH_ACTIVE_TOTAL_CX_MAX"
+#define MS_TOTAL_CX_ADJH_MAP_MAX       "MS_TOUCH_ACTIVE_TOTAL_CX_ADJ_HORIZONTAL"
+#define MS_TOTAL_CX_ADJV_MAP_MAX       "MS_TOUCH_ACTIVE_TOTAL_CX_ADJ_VERTICAL"
+#define SS_RAW_FORCE_MIN_MAX           "SS_RAW_DATA_FORCE_MIN_MAX"
+#define SS_RAW_SENSE_MIN_MAX           "SS_RAW_DATA_SENSE_MIN_MAX"
+#define SS_RAW_FORCE_GAP               "SS_RAW_DATA_FORCE_GAP"
+#define SS_RAW_SENSE_GAP               "SS_RAW_DATA_SENSE_GAP"
+#define SS_IX1_FORCE_MIN_MAX           "SS_TOUCH_ACTIVE_IX1_FORCE_MIN_MAX"
+#define SS_IX1_SENSE_MIN_MAX           "SS_TOUCH_ACTIVE_IX1_SENSE_MIN_MAX"
+#define SS_CX1_FORCE_MIN_MAX           "SS_TOUCH_ACTIVE_CX1_FORCE_MIN_MAX"
+#define SS_CX1_SENSE_MIN_MAX           "SS_TOUCH_ACTIVE_CX1_SENSE_MIN_MAX"
+#define SS_IX2_FORCE_MAP_MIN           "SS_TOUCH_ACTIVE_IX2_FORCE_MIN"
+#define SS_IX2_FORCE_MAP_MAX           "SS_TOUCH_ACTIVE_IX2_FORCE_MAX"
+#define SS_IX2_SENSE_MAP_MIN           "SS_TOUCH_ACTIVE_IX2_SENSE_MIN"
+#define SS_IX2_SENSE_MAP_MAX           "SS_TOUCH_ACTIVE_IX2_SENSE_MAX"
+#define SS_IX2_FORCE_ADJV_MAP_MAX      "SS_TOUCH_ACTIVE_IX2_ADJ_VERTICAL"
+#define SS_IX2_SENSE_ADJH_MAP_MAX      "SS_TOUCH_ACTIVE_IX2_ADJ_HORIZONTAL"
+#define SS_CX2_FORCE_MAP_MIN           "SS_TOUCH_ACTIVE_CX2_FORCE_MIN"
+#define SS_CX2_FORCE_MAP_MAX           "SS_TOUCH_ACTIVE_CX2_FORCE_MAX"
+#define SS_CX2_SENSE_MAP_MIN           "SS_TOUCH_ACTIVE_CX2_SENSE_MIN"
+#define SS_CX2_SENSE_MAP_MAX           "SS_TOUCH_ACTIVE_CX2_SENSE_MAX"
+#define SS_CX2_FORCE_ADJV_MAP_MAX      "SS_TOUCH_ACTIVE_CX2_ADJ_VERTICAL"
+#define SS_CX2_SENSE_ADJH_MAP_MAX      "SS_TOUCH_ACTIVE_CX2_ADJ_HORIZONTAL"
+
+// TOTAL SS
+#define SS_TOTAL_IX_FORCE_MAP_MIN      "SS_TOUCH_ACTIVE_TOTAL_IX_FORCE_MIN"
+#define SS_TOTAL_IX_FORCE_MAP_MAX      "SS_TOUCH_ACTIVE_TOTAL_IX_FORCE_MAX"
+#define SS_TOTAL_IX_SENSE_MAP_MIN      "SS_TOUCH_ACTIVE_TOTAL_IX_SENSE_MIN"
+#define SS_TOTAL_IX_SENSE_MAP_MAX      "SS_TOUCH_ACTIVE_TOTAL_IX_SENSE_MAX"
+#define SS_TOTAL_IX_FORCE_ADJV_MAP_MAX "SS_TOUCH_ACTIVE_TOTAL_IX_ADJ_VERTICAL"
+#define SS_TOTAL_IX_SENSE_ADJH_MAP_MAX "SS_TOUCH_ACTIVE_TOTAL_IX_ADJ_HORIZONTAL"
+#define SS_TOTAL_CX_FORCE_MAP_MIN      "SS_TOUCH_ACTIVE_TOTAL_CX_FORCE_MIN"
+#define SS_TOTAL_CX_FORCE_MAP_MAX      "SS_TOUCH_ACTIVE_TOTAL_CX_FORCE_MAX"
+#define SS_TOTAL_CX_SENSE_MAP_MIN      "SS_TOUCH_ACTIVE_TOTAL_CX_SENSE_MIN"
+#define SS_TOTAL_CX_SENSE_MAP_MAX      "SS_TOUCH_ACTIVE_TOTAL_CX_SENSE_MAX"
+#define SS_TOTAL_CX_FORCE_ADJV_MAP_MAX "SS_TOUCH_ACTIVE_TOTAL_CX_ADJ_VERTICAL"
+#define SS_TOTAL_CX_SENSE_ADJH_MAP_MAX "SS_TOUCH_ACTIVE_TOTAL_CX_ADJ_HORIZONTAL"
+
+//KEYS
+#define MS_KEY_RAW_MIN_MAX              "MS_KEY_RAW_DATA_MIN_MAX"
+#define MS_KEY_CX1_MIN_MAX              "MS_KEY_CX1_MIN_MAX"
+#define MS_KEY_CX2_MAP_MIN              "MS_KEY_CX2_MIN"
+#define MS_KEY_CX2_MAP_MAX              "MS_KEY_CX2_MAX"
+#define MS_KEY_TOTAL_CX_MAP_MIN         "MS_KEY_TOTAL_CX_MIN"
+#define MS_KEY_TOTAL_CX_MAP_MAX         "MS_KEY_TOTAL_CX_MAX"
+
+//CONSTANT TOTAL IX
+#define SS_IX1_FORCE_W                  "IX1_FORCE_W"
+#define SS_IX2_FORCE_W                  "IX2_FORCE_W"
+#define SS_IX1_SENSE_W                  "IX1_SENSE_W"
+#define SS_IX2_SENSE_W                  "IX2_SENSE_W"
+
+
+#define SAVE_FLAG_RETRY                 3
+
+
+struct TestToDo {
+	int MutualRaw;
+	int MutualRawGap;
+	int MutualCx1;
+	int MutualCx2;
+	int MutualCx2Adj;
+	int MutualCxTotal;
+	int MutualCxTotalAdj;
+
+	int MutualKeyRaw;
+	int MutualKeyCx1;
+	int MutualKeyCx2;
+	int MutualKeyCxTotal;
+
+	int SelfForceRaw;
+	int SelfForceRawGap;
+	int SelfForceIx1;
+	int SelfForceIx2;
+	int SelfForceIx2Adj;
+	int SelfForceIxTotal;
+	int SelfForceIxTotalAdj;
+	int SelfForceCx1;
+	int SelfForceCx2;
+	int SelfForceCx2Adj;
+	int SelfForceCxTotal;
+	int SelfForceCxTotalAdj;
+
+	int SelfSenseRaw;
+	int SelfSenseRawGap;
+	int SelfSenseIx1;
+	int SelfSenseIx2;
+	int SelfSenseIx2Adj;
+	int SelfSenseIxTotal;
+	int SelfSenseIxTotalAdj;
+	int SelfSenseCx1;
+	int SelfSenseCx2;
+	int SelfSenseCx2Adj;
+	int SelfSenseCxTotal;
+	int SelfSenseCxTotalAdj;
+
+};
+
+int computeAdjHoriz(u8 *data, int row, int column, u8 **result);
+int computeAdjHorizTotal(u16 *data, int row, int column, u16 **result);
+int computeAdjVert(u8 *data, int row, int column, u8 **result);
+int computeAdjVertTotal(u16 *data, int row, int column, u16 **result);
+int computeTotal(u8 *data, u8 main, int row, int column, int m,
+	int n, u16 **result);
+int checkLimitsMinMax(short *data, int row, int column, int min, int max);
+int checkLimitsMap(u8 *data, int row, int column, int *min, int *max);
+int checkLimitsMapTotal(u16 *data, int row, int column, int *min, int *max);
+int checkLimitsMapAdj(u8 *data, int row, int column, int *max);
+int checkLimitsMapAdjTotal(u16 *data, int row, int column, int *max);
+int production_test_ito(void);
+int production_test_initialization(void);
+int ms_compensation_tuning(void);
+int ss_compensation_tuning(void);
+int lp_timer_calibration(void);
+int save_cx_tuning(void);
+int production_test_split_initialization(int saveToFlash);
+int production_test_main(char *pathThresholds, int stop_on_fail, int saveInit,
+	struct TestToDo *todo, u32 signature);
+int production_test_ms_raw(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo);
+int production_test_ms_cx(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo);
+int production_test_ss_raw(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo);
+int production_test_ss_ix_cx(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo);
+int production_test_data(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo);
+int production_test_ms_key_cx(char *path_limits, int stop_on_fail,
+	struct TestToDo *todo);
+int production_test_ms_key_raw(char *path_limits);
+int save_mp_flag(u32 signature);
+int parseProductionTestLimits(char *path, char *label, int **data,
+	int *row, int *column);
+int readLine(char *data, char *line, int size, int *n);
+#endif

st/fts_lib/ftsTime.c

@@ -0,0 +1,109 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                  FTS Utility for mesuring/handling the time            *
+ *                                                                        *
+ **************************************************************************
+ ***************************************************************************
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <stdarg.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/time.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_gpio.h>
+#include <linux/timekeeping.h>
+//#include <linux/sec_sysfs.h>
+
+#include "ftsCrossCompile.h"
+#include "ftsTime.h"
+
+
+void startStopWatch(struct StopWatch *w)
+{
+	ktime_get_ts(&w->start);
+}
+
+void stopStopWatch(struct StopWatch *w)
+{
+	ktime_get_ts(&w->end);
+}
+
+int elapsedMillisecond(struct StopWatch *w)
+{
+	int result;
+
+	result = ((w->end.tv_sec - w->start.tv_sec) * 1000)
+		+ (w->end.tv_nsec - w->start.tv_nsec) / 1000000;
+	return result;
+}
+
+int elapsedNanosecond(struct StopWatch *w)
+{
+	int result;
+
+	result = ((w->end.tv_sec - w->start.tv_sec) * 1000000000)
+		+ (w->end.tv_nsec - w->start.tv_nsec);
+	return result;
+}
+
+char *timestamp()
+{
+	char *result = NULL;
+
+	result = (char *)kmalloc_array(1, sizeof(char), GFP_KERNEL);
+	if (result == NULL)
+		return NULL;
+	result[0] = ' ';
+	return result;
+}
+
+void stdelay(unsigned long ms)
+{
+	msleep(ms);
+}

st/fts_lib/ftsTime.h

@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                  FTS Utility for mesuring/handling the time            *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#ifndef __FTS_TIME_H
+#define __FTS_TIME_H
+
+#include <linux/time.h>
+
+#include "ftsCrossCompile.h"
+
+struct StopWatch {
+	struct timespec start, end;
+};
+
+void startStopWatch(struct StopWatch *w);
+void stopStopWatch(struct StopWatch *w);
+int elapsedMillisecond(struct StopWatch *w);
+int elapsedNanosecond(struct StopWatch *w);
+char *timestamp(void);
+void stdelay(unsigned long ms);
+#endif

st/fts_lib/ftsTool.c

@@ -0,0 +1,1348 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2018, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                     FTS Utility Functions                              *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <stdarg.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/serio.h>
+#include <linux/init.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/power_supply.h>
+#include <linux/firmware.h>
+#include <linux/gpio.h>
+
+#include "ftsCompensation.h"
+#include "ftsCrossCompile.h"
+#include "ftsError.h"
+#include "ftsHardware.h"
+#include "ftsIO.h"
+#include "ftsSoftware.h"
+#include "ftsTime.h"
+#include "ftsFlash.h"
+#include "ftsTool.h"
+#include "../fts.h"
+
+static char tag[8] = "[ FTS ]\0";
+static int reset_gpio = GPIO_NOT_DEFINED;
+static int system_resetted_up;
+static int system_resetted_down;
+
+int readB2(u16 address, u8 *outBuf, int len)
+{
+	int remaining = len;
+	int toRead = 0;
+	int retry = 0;
+	int ret;
+	int event_to_search[3];
+	char *temp = NULL;
+	u8 *init_outBuf = outBuf;
+	u16 init_addr = address;
+	u8 readEvent[FIFO_EVENT_SIZE] = {0};
+	u8 cmd[4] = { FTS_CMD_REQU_FW_CONF, 0x00, 0x00, (u8)len };
+
+	if (readEvent == NULL) {
+		logError(1, "%s %s:ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+	u16ToU8_be(address, &cmd[1]);
+	temp = printHex("Command B2 = ", cmd, 4);
+	if (temp != NULL)
+		logError(0, "%s %s", tag, temp);
+	kfree(temp);
+	do {
+		remaining = len;
+		ret = fts_writeFwCmd(cmd, 4);
+		if (ret < 0) {
+			logError(1, "%s %s: ERROR %02X\n",
+				tag, __func__, ERROR_I2C_W);
+			return ret;
+		}
+		//ask to the FW the data
+		logError(0, "%s Command to FW sent!\n", tag);
+		event_to_search[0] = (int)EVENTID_FW_CONFIGURATION;
+		while (remaining > OK) {
+			event_to_search[1] = (int)((address & 0xFF00) >> 8);
+			event_to_search[2] = (int)(address & 0x00FF);
+			if (remaining > B2_DATA_BYTES) {
+				toRead = B2_DATA_BYTES;
+				remaining -= B2_DATA_BYTES;
+			} else {
+				toRead = remaining;
+				remaining = 0;
+			}
+
+			ret = pollForEvent(event_to_search, 3,
+				readEvent, GENERAL_TIMEOUT);
+			if (ret >= OK) {
+				//start the polling for reading the reply
+				memcpy(outBuf, &readEvent[3], toRead);
+				retry = 0;
+				outBuf += toRead;
+			} else {
+				retry += 1;
+				break;
+			}
+			address += B2_DATA_BYTES;
+		}
+		logError(0, "%s %s:B2 failed...attempt = %d\n",
+			tag, __func__, retry);
+		outBuf = init_outBuf;
+		address = init_addr;
+	} while (retry < B2_RETRY && retry != 0);
+
+	if (retry == B2_RETRY) {
+		logError(1, "%s %s:ERROR %02X\n", tag, __func__, ERROR_TIMEOUT);
+		return ERROR_TIMEOUT;
+	}
+	logError(0, "%s B2 read %d bytes\n", tag, len);
+
+	return OK;
+}
+
+int readB2U16(u16 address, u8 *outBuf, int byteToRead)
+{
+	int remaining = byteToRead;
+	int toRead = 0;
+	int ret;
+
+	u8 *buff = (u8 *)kmalloc_array((B2_CHUNK + 1), sizeof(u8), GFP_KERNEL);
+
+	if (buff == NULL) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_ALLOC);
+		return ERROR_ALLOC;
+	}
+
+	while (remaining > 0) {
+		if (remaining >= B2_CHUNK) {
+			toRead = B2_CHUNK;
+			remaining -= B2_CHUNK;
+		} else {
+			toRead = remaining;
+			remaining = 0;
+		}
+
+		ret = readB2(address, buff, toRead);
+		if (ret < 0) {
+			kfree(buff);
+			return ret;
+		}
+		memcpy(outBuf, buff, toRead);
+		address += toRead;
+		outBuf += toRead;
+	}
+	kfree(buff);
+	return OK;
+}
+
+
+int releaseInformation(void)
+{
+	int ret;
+	u8 cmd[1] = { FTS_CMD_RELEASE_INFO };
+	int event_to_search[1];
+	u8 readEvent[FIFO_EVENT_SIZE];
+
+	event_to_search[0] = (int)EVENTID_RELEASE_INFO;
+
+	logError(0, "%s %s: started... Chip INFO:\n", tag, __func__);
+
+	ret = fts_writeFwCmd(cmd, 1);
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ret);
+		return ret;
+	}
+
+	ret = pollForEvent(event_to_search, 1, &readEvent[0],
+			RELEASE_INFO_TIMEOUT);
+	//start the polling for reading the reply
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ret);
+		return ret;
+	}
+
+	logError(0, "%s %s: Finished! %d\n", tag, __func__, ret);
+	return OK;
+}
+
+int lockDownInfo(u8 *data, int len)
+{
+	int ret;
+	int i = 0, num_event;
+	u8 cmd[1] = { FTS_CMD_LOCKDOWN_CMD };
+	int event_to_search[3] = {EVENTID_LOCKDOWN_INFO,
+			EVENT_TYPE_LOCKDOWN, 0x00};
+	u8 readEvent[FIFO_EVENT_SIZE];
+
+	logError(0, "%s %s:started...\n", tag, __func__);
+	if (len <= 0)
+		return ERROR_OP_NOT_ALLOW;
+
+	ret = fts_writeFwCmd(cmd, 1);
+	if (ret < OK) {
+		logError(1, "%s %s:ERROR %02X\n", tag, __func__, ret);
+		return ret;
+	}
+
+	num_event = (len + 3) / 4;
+	logError(0, "%s %s:num_event = %d\n", tag, __func__, num_event);
+
+	for (i = 0; i < num_event; i++) {
+		ret = pollForEvent(event_to_search, 3,
+			&readEvent[0], GENERAL_TIMEOUT);
+		//start the polling for reading the reply
+		if (ret < OK) {
+			logError(1, "%s %s:ERROR %02X\n", tag, __func__, ret);
+			return ret;
+		}
+		data[i * 4] = readEvent[3];
+		data[i * 4 + 1] = readEvent[4];
+		data[i * 4 + 2] = readEvent[5];
+		data[i * 4 + 3] = readEvent[6];
+		event_to_search[2] += 4;
+		//logError(0, "%02X %02X %02X %02X ", readEvent[3],
+		//readEvent[4], readEvent[5], readEvent[6]);
+	}
+
+	logError(0, "%s %s:Finished! %d\n", tag, __func__, ret);
+	return OK;
+}
+
+
+int calculateCRC8(u8 *u8_srcBuff, int size, u8 *crc)
+{
+	u8 u8_remainder;
+	u8 bit;
+	int i = 0;
+
+	u8_remainder = 0x00;
+	logError(0, "%s %s: Start CRC computing...\n", tag, __func__);
+
+	if (size == 0 || u8_srcBuff == NULL) {
+		logError(1, "Arguments passed not valid!");
+		logError(1, "%s %s:Data pointer = NULL ", tag, __func__);
+		logError(1, "or size = 0 (%d) ERROR %08X\n",
+			 size, ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+
+	// Perform modulo-2 division, a byte at a time.
+	//Bring the next byte into the remainder.
+	for (i = 0; i < size; i++) {
+		//Perform modulo-2 division, a bit at a time.
+		u8_remainder ^= u8_srcBuff[i];
+		//Try to divide the current data bit.
+		for (bit = 8; bit > 0; --bit) {
+			if (u8_remainder & (0x1 << 7))
+				u8_remainder = (u8_remainder << 1) ^ 0x9B;
+			else
+				u8_remainder = (u8_remainder << 1);
+		}
+	} //The final remainder is the CRC result.
+	*crc = u8_remainder;
+	logError(0, "%s %s: CRC value = %02X\n", tag, __func__, *crc);
+	return OK;
+}
+
+int writeLockDownInfo(u8 *data, int size)
+{
+	int ret, i, toWrite, retry = 0, offset = size;
+	u8 cmd[2 + LOCKDOWN_CODE_WRITE_CHUNK] = {FTS_CMD_LOCKDOWN_FILL, 0x00};
+	u8 crc = 0;
+	int event_to_search[2] = {EVENTID_STATUS_UPDATE,
+		EVENT_TYPE_LOCKDOWN_WRITE};
+	u8 readEvent[FIFO_EVENT_SIZE];
+	char *temp = NULL;
+
+	logError(0, "%s %s: Writing Lockdown code into the IC...\n",
+		tag, __func__);
+
+	ret = fts_disableInterrupt();
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %08X\n", tag, __func__, ret);
+		ret = (ret | ERROR_LOCKDOWN_CODE);
+		goto ERROR;
+	}
+	if (size > LOCKDOWN_CODE_MAX_SIZE) {
+		logError(1, "%s %s: Lockdown data to write too big! ",
+			tag, __func__);
+		logError(1, "%d>%d ERROR %08X\n",
+			size, LOCKDOWN_CODE_MAX_SIZE, ret);
+		ret = (ERROR_OP_NOT_ALLOW | ERROR_LOCKDOWN_CODE);
+		goto ERROR;
+	}
+
+	temp = printHex("Lockdown Code = ", data, size);
+	if (temp != NULL) {
+		logError(0, "%s %s: %s", tag, __func__, temp);
+		kfree(temp);
+	}
+
+	for (retry = 0; retry < LOCKDOWN_CODE_RETRY; retry++) {
+		logError(0, "%s %s: Filling FW buffer...\n", tag, __func__);
+		i = 0;
+		offset = size;
+		cmd[0] = FTS_CMD_LOCKDOWN_FILL;
+		while (offset > 0) {
+			if (offset > LOCKDOWN_CODE_WRITE_CHUNK)
+				toWrite = LOCKDOWN_CODE_WRITE_CHUNK;
+			else
+				toWrite = offset;
+			memcpy(&cmd[2], &data[i], toWrite);
+			cmd[1] = i;
+
+			temp = printHex("Commmand = ", cmd, 2 + toWrite);
+			if (temp != NULL) {
+				logError(0, "%s %s: %s", tag, __func__, temp);
+				kfree(temp);
+			}
+			ret = fts_writeFwCmd(cmd, 2 + toWrite);
+			if (ret < OK) {
+				logError(1, "Unable to write Lockdown data ");
+				logError(1, "%s %s:Lockdown data at %d ",
+					tag, __func__, i);
+				logError(1, "iteration.%08X\n", ret);
+				ret = (ret | ERROR_LOCKDOWN_CODE);
+				continue;
+			}
+			i += toWrite;//update the offset
+			offset -= toWrite;
+		}
+		logError(0, "%s %s: Compute 8bit CRC...\n", tag, __func__);
+		ret = calculateCRC8(data, size, &crc);
+		if (ret < OK) {
+			logError(1, "%s %s:Unable to compute CRC..ERROR %08X\n",
+				tag, __func__, ret);
+			ret = (ret | ERROR_LOCKDOWN_CODE);
+			continue;
+		}
+		cmd[0] = FTS_CMD_LOCKDOWN_WRITE;
+		cmd[1] = 0x00;
+		cmd[2] = (u8)size;
+		cmd[3] = crc;
+		logError(0, "%s %s: Write Lockdown data...\n",
+			tag, __func__);
+		temp = printHex("Commmand = ", cmd, 4);
+		if (temp != NULL) {
+			logError(0, "%s %s: %s", tag, __func__, temp);
+			kfree(temp);
+		}
+		ret = fts_writeFwCmd(cmd, 4);
+		if (ret < OK) {
+			logError(1, "%s%s:Unable to send Lockdown data ",
+				tag, __func__);
+			logError(1, "write command%08X\n", ret);
+			ret = (ret | ERROR_LOCKDOWN_CODE);
+			continue;
+		}
+
+		ret = pollForEvent(event_to_search,
+			2,
+			&readEvent[0],
+			GENERAL_TIMEOUT);
+		//start the polling for reading the reply
+
+		if (ret < OK) {
+			logError(1, "%s%s:Cann't find lockdown code ",
+				tag, __func__);
+			logError(1, "%write reply %08X\n", ret);
+			continue;
+		}
+
+		if (readEvent[2] != 0x00) {
+			logError(1, "%s %s:Event check FAIL!%02X != 0x00 ",
+				tag, __func__, readEvent[2]);
+			logError(1, "%ERR%08X\n", ERROR_LOCKDOWN_CODE);
+			ret = ERROR_LOCKDOWN_CODE;
+			continue;
+		} else {
+			logError(0, "%s %s:Lockdown Code write DONE!\n",
+				tag, __func__);
+			ret = OK;
+			break;
+		}
+	}
+
+ERROR:
+	//ret = fts_enableInterrupt();
+	//ensure that the interrupt are always renabled when exit from funct
+	if (fts_enableInterrupt() < OK) {
+		logError(1, "%s %s: Error while re-enabling the interrupt!\n",
+			tag, __func__);
+	}
+	return ret;
+}
+
+int rewriteLockDownInfo(u8 *data, int size)
+{
+	int ret, i, toWrite, retry = 0, offset = size;
+	u8 cmd[2 + LOCKDOWN_CODE_WRITE_CHUNK] = {FTS_CMD_LOCKDOWN_FILL, 0x00};
+	u8 crc = 0;
+	int event_to_search[2] = {EVENTID_STATUS_UPDATE,
+			EVENT_TYPE_LOCKDOWN_WRITE};
+	u8 readEvent[FIFO_EVENT_SIZE];
+	char *temp = NULL;
+
+	logError(0, "%s %s: ReWriting Lockdown code into the IC start ...\n",
+		tag, __func__);
+
+	ret = fts_disableInterrupt();
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %08X\n", tag, __func__, ret);
+		ret = (ret | ERROR_LOCKDOWN_CODE);
+			goto ERROR;
+	}
+	if (size > LOCKDOWN_CODE_MAX_SIZE) {
+		logError(1, "%s %s: Lockdown data to write too big! ",
+			tag, __func__);
+		logError(1, "%d>%d  ERROR %08X\n",
+			size, LOCKDOWN_CODE_MAX_SIZE, ret);
+		ret = (ERROR_OP_NOT_ALLOW | ERROR_LOCKDOWN_CODE);
+			goto ERROR;
+	}
+
+	temp = printHex("Lockdown Code = ", data, size);
+	if (temp != NULL) {
+		logError(0, "%s %s: %s", tag, __func__, temp);
+		kfree(temp);
+	}
+
+	for (retry = 0; retry < LOCKDOWN_CODE_RETRY; retry++) {
+		logError(0, "%s %s: Filling FW buffer ...\n", tag, __func__);
+		i = 0;
+		offset = size;
+		cmd[0] = FTS_CMD_LOCKDOWN_FILL;
+		while (offset > 0) {
+			if (offset > LOCKDOWN_CODE_WRITE_CHUNK)
+				toWrite = LOCKDOWN_CODE_WRITE_CHUNK;
+			else
+				toWrite = offset;
+			memcpy(&cmd[2], &data[i], toWrite);
+			cmd[1] = i;
+			temp = printHex("Commmand = ", cmd, 2 + toWrite);
+			if (temp != NULL) {
+				logError(0, "%s %s: %s", tag, __func__, temp);
+				kfree(temp);
+			}
+			ret = fts_writeFwCmd(cmd, 2 + toWrite);
+			if (ret < OK) {
+				logError(1, "Unable to rewrite Lockdown data");
+				logError(1, "%s %s: at %d iteration ",
+					tag, __func__, i);
+				logError(1, "ERROR %08X\n", ret);
+				ret = (ret | ERROR_LOCKDOWN_CODE);
+					continue;
+			}
+			i += toWrite;//update the offset
+			offset -= toWrite;
+		}
+		logError(0, "%s %s: Compute 8bit CRC...\n", tag, __func__);
+		ret = calculateCRC8(data, size, &crc);
+		if (ret < OK) {
+			logError(1, "%s %s:Unable to compute CRC.. ",
+				tag, __func__);
+			logError(1, "ERROR %08X\n", ret);
+			ret = (ret | ERROR_LOCKDOWN_CODE);
+				continue;
+		}
+		cmd[0] = FTS_CMD_LOCKDOWN_WRITE;
+		cmd[1] = 0x01;
+		cmd[2] = (u8)size;
+		cmd[3] = crc;
+
+		temp = printHex("Commmand = ", cmd, 4);
+		if (temp != NULL) {
+			logError(0, "%s %s: %s", tag, __func__, temp);
+			kfree(temp);
+		}
+		logError(0, "%s %s: ReWrite Lockdown data...\n", tag, __func__);
+		ret = fts_writeFwCmd(cmd, 4);
+		if (ret < OK) {
+			logError(1, "Unable to send Lockdown data");
+			logError(1, "%s %s:rewrite command... ERROR %08X\n",
+				tag, __func__, ret);
+			ret = (ret | ERROR_LOCKDOWN_CODE);
+			continue;
+		}
+
+		//start the polling for reading the reply
+		ret = pollForEvent(event_to_search, 2,
+			&readEvent[0], GENERAL_TIMEOUT);
+		if (ret >= OK) {
+			if (readEvent[2] < 0x00) {
+				logError(1, "%s %s:Event check FAIL! ",
+					tag, __func__);
+				logError(1, "%02X != 0x00 %08X\n",
+					readEvent[2], ERROR_LOCKDOWN_CODE);
+				ret = ERROR_LOCKDOWN_CODE;
+					continue;
+			} else {
+				logError(0, "%s %s: Lockdown Code ",
+					tag, __func__);
+				logError(0, "rewrite DONE!\n");
+				ret = OK;
+				break;
+			}
+		} else {
+			logError(1, "Can not find lockdown code write ");
+			logError(1, "reply event!%s %s: ERROR %08X\n",
+				tag, __func__, ret);
+		}
+	}
+ERROR:
+	//ret = fts_enableInterrupt();
+	//ensure that the interrupt are always renabled when exit from funct
+	if (fts_enableInterrupt() < OK) {
+		logError(1, "%s %s: Error while re-enabling the interrupt!\n",
+			tag, __func__);
+	}
+	return ret;
+}
+
+int readLockDownInfo(u8 *lockData, int *size)
+{
+	int ret, retry = 0, toRead = 0, byteToRead;
+	u8 cmd = FTS_CMD_LOCKDOWN_READ;
+	int event_to_search[3] = {EVENTID_LOCKDOWN_INFO_READ, -1, 0x00};
+	u8 readEvent[FIFO_EVENT_SIZE];
+	char *temp = NULL;
+
+	lockData = NULL;
+	logError(0, "%s %s: Reading Lockdown code from the IC...\n",
+		tag, __func__);
+
+	ret = fts_disableInterrupt();
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %08X\n", tag, __func__, ret);
+		ret = (ret | ERROR_LOCKDOWN_CODE);
+			goto ERROR;
+	}
+	for (retry = 0; retry < LOCKDOWN_CODE_RETRY; retry++) {
+		event_to_search[2] = 0x00;
+		logError(0, "%s %s: Read Lockdown data.(%d attempt)\n",
+			tag, __func__, retry + 1);
+		ret = fts_writeFwCmd(&cmd, 1);
+
+		if (ret < OK) {
+			logError(1, "%s%s:Unable to send Lockdown data ",
+				tag, __func__);
+			logError(1, "write CMD %08X\n", ret);
+			ret = (ret | ERROR_LOCKDOWN_CODE);
+			continue;
+		}
+
+		//start the polling for reading the reply
+		ret = pollForEvent(event_to_search, 3,
+			&readEvent[0], GENERAL_TIMEOUT);
+
+		if (ret < OK) {
+			logError(1, "Cann't find first lockdown code read");
+			logError(1, "%s %s:reply event! ERROR %08X\n",
+				tag, __func__, ret);
+			continue;
+		}
+
+		byteToRead = readEvent[1];
+		*size = byteToRead;
+		logError(0, "%s %s:Lockdown Code size = %d\n",
+			tag, __func__, *size);
+		lockData = (u8 *)kmalloc_array((byteToRead),
+			sizeof(u8), GFP_KERNEL);
+		if (lockData == NULL) {
+			logError(1, "%s %s:Unable to allocate lockData %08X\n",
+				tag, __func__, ERROR_ALLOC);
+			ret = (ERROR_ALLOC | ERROR_LOCKDOWN_CODE);
+			continue;
+		}
+		while (byteToRead > 0) {
+			if ((readEvent[1] - readEvent[2])
+				> LOCKDOWN_CODE_READ_CHUNK) {
+				toRead = LOCKDOWN_CODE_READ_CHUNK;
+			} else {
+				toRead = readEvent[1] - readEvent[2];
+			}
+			byteToRead -= toRead;
+			memcpy(&lockData[readEvent[2]],
+				&readEvent[3], toRead);
+			event_to_search[2] += toRead;
+			if (byteToRead <= 0)
+				continue;
+
+			ret = pollForEvent(event_to_search,
+				3,
+				&readEvent[0],
+				GENERAL_TIMEOUT);
+
+			//start polling for reading reply
+			if (ret < OK) {
+				logError(1, "Can not find lockdow");
+				logError(1, "code read reply event ");
+				logError(1, "%s%s:offset%02X%08X\n",
+					tag, __func__, event_to_search[2], ret);
+				ret = (ERROR_ALLOC | ERROR_LOCKDOWN_CODE);
+				break;
+			}
+		}
+		if (byteToRead != 0) {
+			logError(1, "%s %s:Read Lockdown code FAIL! ",
+				tag, __func__);
+			logError(1, "ERROR %08X\n", ret);
+			continue;
+		} else {
+			logError(0, "%s %s: Lockdown Code read DONE!\n",
+				tag, __func__);
+			ret = OK;
+			temp = printHex("Lockdown Code = ", lockData, *size);
+			if (temp != NULL) {
+				logError(0, "%s %s: %s", tag, __func__, temp);
+				kfree(temp);
+			}
+			break;
+		}
+	}
+ERROR:
+	//ret = fts_enableInterrupt();
+	//ensure that the interrupt are always
+	//renabled when exit from funct
+	if (fts_enableInterrupt() < OK) {
+		logError(1, "%s %s:Error while re-enabling the interrupt!\n",
+			tag, __func__);
+	}
+	return ret;
+}
+
+char *printHex(char *label, u8 *buff, int count)
+{
+	int i, offset;
+	char *result = NULL;
+	size_t len = 0;
+
+	offset = strlen(label);
+	len = (offset + 3 * count) + 2;
+	result = (char *)kmalloc_array(len, sizeof(char), GFP_KERNEL);
+	if (result != NULL) {
+		strlcpy(result, label, len);
+		for (i = 0; i < count; i++)
+			snprintf(&result[offset + i * 3], 4, "%02X ", buff[i]);
+		strlcat(result, "\n", len);
+	}
+	return result;
+}
+
+int pollForEvent(int *event_to_search, int event_bytes,
+	u8 *readData, int time_to_wait)
+{
+	int i, find, retry, count_err;
+	int time_to_count;
+	int err_handling = OK;
+	struct StopWatch clock;
+
+	u8 cmd[1] = { FIFO_CMD_READONE };
+	char *temp = NULL;
+
+	find = 0;
+	retry = 0;
+	count_err = 0;
+	time_to_count = time_to_wait / TIMEOUT_RESOLUTION;
+
+	startStopWatch(&clock);
+	while (find != 1 && retry < time_to_count
+		&& fts_readCmd(cmd, 1, readData, FIFO_EVENT_SIZE) >= 0) {
+
+		if (readData[0] == EVENTID_ERROR_EVENT) {
+			temp = printHex("ERROR EVENT = ",
+				readData, FIFO_EVENT_SIZE);
+			if (temp != NULL)
+				logError(0, "%s %s", tag, temp);
+			kfree(temp);
+			count_err++;
+			err_handling = errorHandler(readData, FIFO_EVENT_SIZE);
+			if ((err_handling & 0xF0FF0000)
+				== ERROR_HANDLER_STOP_PROC) {
+				logError(1, "%s %s: forced to be stopped! ",
+					tag, __func__);
+				logError(1, "ERROR %08X\n", err_handling);
+				return err_handling;
+			}
+		} else {
+			if (readData[0] != EVENTID_NO_EVENT) {
+				temp = printHex("READ EVENT = ",
+					readData, FIFO_EVENT_SIZE);
+				if (temp != NULL)
+					logError(0, "%s %s", tag, temp);
+				kfree(temp);
+			}
+			if (readData[0] == EVENTID_CONTROL_READY &&
+				event_to_search[0] != EVENTID_CONTROL_READY) {
+				logError(0, "Unmanned Controller Ready Event!");
+				logError(0, "%s %s:Setting reset flags...\n",
+					tag, __func__);
+				setSystemResettedUp(1);
+				setSystemResettedDown(1);
+			}
+		}
+		find = 1;
+
+		for (i = 0; i < event_bytes; i++) {
+			if (event_to_search[i] != -1
+				&& (int)readData[i] != event_to_search[i]) {
+				find = 0;
+				break;
+			}
+		}
+
+		retry++;
+		msleep(TIMEOUT_RESOLUTION);
+	}
+	stopStopWatch(&clock);
+	if ((retry >= time_to_count) && find != 1) {
+		logError(0, "%s %s: ERROR %02X\n",
+			tag, __func__,  ERROR_TIMEOUT);
+		return ERROR_TIMEOUT;
+	}
+	if (find == 1) {
+		temp = printHex("FOUND EVENT = ", readData, FIFO_EVENT_SIZE);
+		if (temp != NULL)
+			logError(0, "%s %s", tag, temp);
+		kfree(temp);
+		logError(0, "%s Event found in %d ms (%d iterations)!\n",
+			tag, elapsedMillisecond(&clock), retry);
+		logError(0, "Number of errors found = %d\n", count_err);
+		return count_err;
+	}
+	logError(0, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_R);
+	return ERROR_I2C_R;
+}
+
+int flushFIFO(void)
+{
+	u8 cmd = FIFO_CMD_FLUSH;
+
+	if (fts_writeCmd(&cmd, 1) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+
+	logError(0, "%s FIFO flushed!\n", tag);
+	return OK;
+}
+
+int fts_disableInterrupt(void)
+{
+	//disable interrupt
+	u8 cmd[4] = { FTS_CMD_HW_REG_W, 0x00, 0x00, IER_DISABLE };
+
+	u16ToU8_be(IER_ADDR, &cmd[1]);
+
+	if (fts_writeCmd(cmd, 4) < OK) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+	logError(0, "%s Interrupt Disabled!\n", tag);
+	return OK;
+}
+
+
+int fts_enableInterrupt(void)
+{
+	u8 cmd[4] = { FTS_CMD_HW_REG_W, 0x00, 0x00, IER_ENABLE };
+
+	u16ToU8_be(IER_ADDR, &cmd[1]);
+	if (fts_writeCmd(cmd, 4) < 0) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ERROR_I2C_W);
+		return ERROR_I2C_W;
+	}
+	logError(0, "%s Interrupt Enabled!\n", tag);
+	return OK;
+}
+
+int u8ToU16n(u8 *src, int src_length, u16 *dst)
+{
+	int i, j;
+	u16 *buf;
+
+	if (src_length % 2 != 0)
+		return -EINVAL;
+
+	j = 0;
+	buf = (u16 *)kmalloc_array((src_length / 2), sizeof(u16), GFP_KERNEL);
+	if (!buf) {
+		dst = NULL;
+		return -EINVAL;
+	}
+	dst = buf;
+	for (i = 0; i < src_length; i += 2) {
+		dst[j] = ((src[i+1] & 0x00FF) << 8) + (src[i] & 0x00FF);
+		j++;
+	}
+
+	return (src_length / 2);
+}
+
+int u8ToU16(u8 *src, u16 *dst)
+{
+	*dst = (u16)(((src[1] & 0x00FF) << 8) + (src[0] & 0x00FF));
+	return 0;
+}
+
+int u8ToU16_le(u8 *src, u16 *dst)
+{
+	*dst = (u16)(((src[0] & 0x00FF) << 8) + (src[1] & 0x00FF));
+	return 0;
+}
+
+int u16ToU8n(u16 *src, int src_length, u8 *dst)
+{
+	int i, j;
+	u8 *buf = (u8 *)kmalloc_array(2 * src_length, sizeof(u8), GFP_KERNEL);
+
+	if (!buf) {
+		dst = NULL;
+		return -EINVAL;
+	}
+	dst = buf;
+	j = 0;
+	for (i = 0; i < src_length; i++) {
+		dst[j] = (u8) (src[i] & 0xFF00) >> 8;
+		dst[j+1] = (u8) (src[i] & 0x00FF);
+		j += 2;
+	}
+
+	return src_length * 2;
+}
+
+int u16ToU8(u16 src, u8 *dst)
+{
+	dst[0] = (u8)((src & 0xFF00) >> 8);
+	dst[1] = (u8)(src & 0x00FF);
+	return 0;
+}
+
+int u16ToU8_be(u16 src, u8 *dst)
+{
+	dst[0] = (u8)((src & 0xFF00) >> 8);
+	dst[1] = (u8)(src & 0x00FF);
+	return 0;
+}
+
+int u16ToU8_le(u16 src, u8 *dst)
+{
+	dst[1] = (u8)((src & 0xFF00) >> 8);
+	dst[0] = (u8)(src & 0x00FF);
+	return 0;
+}
+
+int u8ToU32(u8 *src, u32 *dst)
+{
+	*dst = (u32)(((src[3] & 0xFF) << 24) + ((src[2] & 0xFF) << 16)
+		+ ((src[1] & 0xFF) << 8) + (src[0] & 0xFF));
+	return 0;
+}
+
+int u32ToU8(u32 src, u8 *dst)
+{
+	dst[3] = (u8)((src & 0xFF000000) >> 24);
+	dst[2] = (u8)((src & 0x00FF0000) >> 16);
+	dst[1] = (u8)((src & 0x0000FF00) >> 8);
+	dst[0] = (u8)(src & 0x000000FF);
+	return 0;
+}
+
+int attempt_function(int(*code)(void), unsigned long wait_before_retry,
+	int retry_count)
+{
+	int result;
+	int count = 0;
+
+	do {
+		result = code();
+		count++;
+		msleep(wait_before_retry);
+	} while (count < retry_count && result < 0);
+
+	if (count == retry_count)
+		result |= ERROR_TIMEOUT;
+
+	return result;
+}
+
+void setResetGpio(int gpio)
+{
+	reset_gpio = gpio;
+	logError(0, "%s %s: reset_gpio = %d\n", tag, __func__, reset_gpio);
+}
+
+int fts_system_reset(void)
+{
+	u8 readData[FIFO_EVENT_SIZE];
+	int event_to_search;
+	int res = -1;
+	int i;
+	u8 cmd[4] = { FTS_CMD_HW_REG_W, 0x00, 0x00, SYSTEM_RESET_VALUE };
+
+	event_to_search = (int)EVENTID_CONTROL_READY;
+
+	u16ToU8_be(SYSTEM_RESET_ADDRESS, &cmd[1]);
+	logError(0, "%s System resetting...\n", tag);
+	for (i = 0; i < SYSTEM_RESET_RETRY && res < 0; i++) {
+		if (reset_gpio == GPIO_NOT_DEFINED) {
+#ifndef FTM3_CHIP
+			res |= fts_warm_boot();
+#endif
+			res = fts_writeCmd(cmd, 4);
+		} else {
+			gpio_set_value(reset_gpio, 0);
+			msleep(20);
+			gpio_set_value(reset_gpio, 1);
+			res = OK;
+		}
+		if (res < OK) {
+			logError(1, "%s %s:ERROR %02X\n",
+				tag, __func__, ERROR_I2C_W);
+		} else {
+			res = pollForEvent(&event_to_search, 1,
+					readData, GENERAL_TIMEOUT);
+			if (res < OK) {
+				logError(0, "%s %s: ERROR %02X\n",
+					tag, __func__, res);
+			}
+		}
+	}
+	if (res < OK) {
+		logError(1, "%s %s:failed after 3 attempts: ERROR %02X\n",
+			tag, __func__, (res | ERROR_SYSTEM_RESET_FAIL));
+		res = (res | ERROR_SYSTEM_RESET_FAIL);
+	} else {
+		logError(0, "%s System reset DONE!\n", tag);
+		system_resetted_down = 1;
+		system_resetted_up = 1;
+		res = OK;
+	}
+	return res;
+}
+
+int isSystemResettedDown(void)
+{
+	return system_resetted_down;
+}
+
+int isSystemResettedUp(void)
+{
+	return system_resetted_up;
+}
+
+void setSystemResettedDown(int val)
+{
+	system_resetted_down = val;
+}
+
+void setSystemResettedUp(int val)
+{
+	system_resetted_up = val;
+}
+
+int senseOn(void)
+{
+	int ret;
+	u8 cmd[1] = { FTS_CMD_MS_MT_SENSE_ON };
+
+	ret = fts_writeFwCmd(cmd, 1);
+	if (ret < OK) {
+		logError(1, "%s %s:ERROR %02X\n",
+			tag, __func__, ERROR_SENSE_ON_FAIL);
+		return (ret|ERROR_SENSE_ON_FAIL);
+	}
+	logError(0, "%s %s: SENSE ON\n", tag, __func__);
+	return OK;
+}
+
+int senseOff(void)
+{
+	int ret;
+	u8 cmd[1] = { FTS_CMD_MS_MT_SENSE_OFF };
+
+	ret = fts_writeFwCmd(cmd, 1);
+	if (ret < OK) {
+		logError(1, "%s %s:ERROR %02X\n",
+			tag, __func__, ERROR_SENSE_OFF_FAIL);
+		return (ret | ERROR_SENSE_OFF_FAIL);
+	}
+	logError(0, "%s %s: SENSE OFF\n", tag, __func__);
+	return OK;
+}
+
+int keyOn(void)
+{
+	int ret;
+	u8 cmd[1] = { FTS_CMD_MS_KEY_ON };
+
+	ret = fts_writeFwCmd(cmd, 1);
+	if (ret < OK) {
+		logError(1, "%s %s:ERROR %02X\n",
+			tag, __func__, ERROR_SENSE_ON_FAIL);
+		return (ret | ERROR_SENSE_ON_FAIL);
+	}
+
+	logError(0, "%s %s: KEY ON\n", tag, __func__);
+	return OK;
+}
+
+int keyOff(void)
+{
+	int ret;
+	u8 cmd[1] = { FTS_CMD_MS_KEY_OFF };
+
+	ret = fts_writeFwCmd(cmd, 1);
+	if (ret < OK) {
+		logError(1, "%s %s:ERROR %02X\n",
+			tag, __func__, ERROR_SENSE_OFF_FAIL);
+		return (ret | ERROR_SENSE_OFF_FAIL);
+	}
+
+	logError(0, "%s %s: KEY OFF\n", tag, __func__);
+	return OK;
+}
+
+int cleanUp(int enableTouch)
+{
+	int res;
+
+	logError(0, "%s %s: system reset...\n", tag, __func__);
+	res = fts_system_reset();
+	if (res < OK)
+		return res;
+
+	if (enableTouch) {
+		logError(0, "%s %s:enabling touches...\n", tag, __func__);
+		res = senseOn();
+		if (res < OK)
+			return res;
+#ifdef PHONE_KEY
+		res = keyOn();
+		if (res < OK)
+			return res;
+#endif
+		logError(0, "%s %s:enabling interrupts...\n", tag, __func__);
+		res = fts_enableInterrupt();
+		if (res < OK)
+			return res;
+	}
+	return OK;
+}
+
+int checkEcho(u8 *cmd, int size)
+{
+	int ret, i;
+	int event_to_search[FIFO_EVENT_SIZE + 1];
+	u8 readData[FIFO_EVENT_SIZE];
+
+	if ((ftsInfo.u32_echoEn & 0x00000001) != ECHO_ENABLED) {
+		logError(0, "%s ECHO Not Enabled!\n", tag);
+		return OK;
+	}
+	if (size < 1) {
+		logError(1, "%s:Error Size = %d not valid!", tag, size);
+		logError(1, " or ECHO not Enabled!%08X\n", ERROR_OP_NOT_ALLOW);
+		return ERROR_OP_NOT_ALLOW;
+	}
+	if ((size + 2) > FIFO_EVENT_SIZE)
+		size = FIFO_EVENT_SIZE - 2;
+	//Echo event EC xx xx xx xx xx xx
+	//fifo_status therefore for command
+	//with more than 6 bytes will echo only the first 6
+	event_to_search[0] = EVENTID_ECHO;
+	for (i = 1; i <= size; i++)
+		event_to_search[i] = cmd[i - 1];
+	ret = pollForEvent(event_to_search, size + 1,
+				readData, GENERAL_TIMEOUT);
+	if (ret < OK) {
+		logError(1, "%s %s:Echo Event not found! ERROR %02X\n",
+			tag, __func__, ret);
+		return (ret | ERROR_CHECK_ECHO_FAIL);
+	}
+
+	logError(0, "%s ECHO OK!\n", tag);
+	ret = OK;
+	return ret;
+}
+
+int featureEnableDisable(int on_off, u32 feature)
+{
+	int ret;
+	u8 cmd[5];
+
+	if (on_off == FEAT_ENABLE) {
+		cmd[0] = FTS_CMD_FEATURE_ENABLE;
+		logError(0, "%s %s: Enabling feature %08X ...\n",
+			tag, __func__, feature);
+	} else {
+		cmd[0] = FTS_CMD_FEATURE_DISABLE;
+		logError(0, "%s %s: Disabling feature %08X ...\n",
+			tag, __func__, feature);
+	}
+	u32ToU8(feature, &cmd[1]);
+
+	//not use writeFwCmd because this function can be
+	//called also during interrupt enable and should be fast
+	ret = fts_writeCmd(cmd, 5);
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ret);
+		return (ret | ERROR_FEATURE_ENABLE_DISABLE);
+	}
+
+	logError(0, "%s %s: DONE!\n", tag, __func__);
+	return OK;
+}
+
+int writeNoiseParameters(u8 *noise)
+{
+	int ret, i;
+	u8 cmd[2+NOISE_PARAMETERS_SIZE];
+	u8 readData[FIFO_EVENT_SIZE];
+	int event_to_search[2] = {EVENTID_NOISE_WRITE, NOISE_PARAMETERS};
+
+	logError(0, "%s %s: Writing noise parameters to the IC ...\n",
+		tag, __func__);
+	ret = fts_disableInterrupt();
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %08X\n", tag, __func__, ret);
+		ret = (ret | ERROR_NOISE_PARAMETERS);
+			goto ERROR;
+	}
+	cmd[0] = FTS_CMD_NOISE_WRITE;
+	cmd[1] = NOISE_PARAMETERS;
+	logError(0, "%s %s: Noise parameters = ", tag, __func__);
+	for (i = 0; i < NOISE_PARAMETERS_SIZE; i++) {
+		cmd[2 + i] = noise[i];
+		logError(0, "%02X", cmd[2 + i]);
+	}
+
+	logError(0, "\n");
+	ret = fts_writeCmd(cmd, NOISE_PARAMETERS_SIZE + 2);
+	//not use writeFwCmd because this function should be fast
+	if (ret < OK) {
+		logError(0, "%s %s:impossible write command... ERROR %02X\n",
+			tag, __func__, ret);
+		ret = (ret | ERROR_NOISE_PARAMETERS);
+			goto ERROR;
+	}
+
+	ret = pollForEvent(event_to_search, 2, readData, GENERAL_TIMEOUT);
+	if (ret < OK) {
+		logError(0, "%s %s: polling FIFO ERROR %02X\n",
+			tag, __func__, ret);
+		ret = (ret | ERROR_NOISE_PARAMETERS);
+			goto ERROR;
+	}
+
+	if (readData[2] != 0x00) {
+		logError(1, "%s %s:Event check FAIL! %02X != 0x00 ERROR%02X\n",
+			tag, __func__, readData[2], ERROR_NOISE_PARAMETERS);
+		ret = ERROR_NOISE_PARAMETERS;
+			goto ERROR;
+	}
+
+	logError(0, "%s %s:DONE!\n", tag, __func__);
+	ret = OK;
+ERROR:
+	ret = fts_enableInterrupt();
+	//ensure that the interrupt are always renabled when exit from funct
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ret);
+		return (ret | ERROR_NOISE_PARAMETERS);
+	}
+	return ret;
+}
+
+int readNoiseParameters(u8 *noise)
+{
+	int ret, i;
+	u8 cmd[2];
+	u8 readData[FIFO_EVENT_SIZE];
+	int event_to_search[2] = {EVENTID_NOISE_READ, NOISE_PARAMETERS};
+
+	logError(0, "%s %s:Reading noise parameters from the IC ...\n",
+		tag, __func__);
+	ret = fts_disableInterrupt();
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ret);
+		ret = (ret | ERROR_NOISE_PARAMETERS);
+			goto ERROR;
+	}
+	cmd[0] = FTS_CMD_NOISE_READ;
+	cmd[1] = NOISE_PARAMETERS;
+	ret = fts_writeCmd(cmd, 2);//not use writeFwCmd should be fast
+	if (ret < OK) {
+		logError(0, "%s %s:impossible write command... ERROR %02X\n",
+			tag, __func__, ret);
+		ret = (ret | ERROR_NOISE_PARAMETERS);
+			goto ERROR;
+	}
+
+	ret = pollForEvent(event_to_search, 2, readData, GENERAL_TIMEOUT);
+	if (ret < OK) {
+		logError(0, "%s %s: polling FIFO ERROR %02X\n",
+			tag, __func__, ret);
+		ret = (ret | ERROR_NOISE_PARAMETERS);
+			goto ERROR;
+	}
+
+	logError(0, "%s %s: Noise parameters = ", tag, __func__);
+	for (i = 0; i < NOISE_PARAMETERS_SIZE; i++) {
+		noise[i] = readData[2 + i];
+		logError(0, "%02X ", noise[i]);
+	}
+
+	logError(0, "\n");
+	logError(0, "%s %s: DONE!\n", tag, __func__);
+	ret = OK;
+ERROR:
+	ret = fts_enableInterrupt();
+	//ensure that the interrupt are always renabled when exit from funct
+	if (ret < OK) {
+		logError(1, "%s %s: ERROR %02X\n", tag, __func__, ret);
+		return (ret | ERROR_NOISE_PARAMETERS);
+	}
+	return ret;
+}
+
+short **array1dTo2d_short(short *data, int size, int columns)
+{
+	int i;
+	int count = size / columns;
+	short **matrix = (short **)kmalloc_array(count,
+				sizeof(short *), GFP_KERNEL);
+	if (matrix != NULL) {
+		for (i = 0; i < count; i++) {
+			matrix[i] = (short *)kmalloc_array(columns,
+				sizeof(short), GFP_KERNEL);
+		}
+
+		for (i = 0; i < size; i++)
+			matrix[i / columns][i % columns] = data[i];
+	}
+
+	return matrix;
+}
+
+u8 **array1dTo2d_u8(u8 *data, int size, int columns)
+{
+	int i;
+	int count = size / columns;
+	u8 **matrix = (u8 **)kmalloc_array(count,
+				sizeof(u8 *), GFP_KERNEL);
+	if (matrix != NULL) {
+		for (i = 0; i < count; i++) {
+			matrix[i] = (u8 *)kmalloc_array(columns,
+				sizeof(u8), GFP_KERNEL);
+		}
+
+		for (i = 0; i < size; i++)
+			matrix[i / columns][i % columns] = data[i];
+	}
+
+	return matrix;
+}
+
+void print_frame_short(char *label, short **matrix, int row, int column)
+{
+	int i, j;
+
+	logError(0, "%s %s\n", tag, label);
+	for (i = 0; i < row; i++) {
+		logError(0, "%s ", tag);
+		for (j = 0; j < column; j++)
+			logError(0, "%d", matrix[i][j]);
+		logError(0, "\n");
+		kfree(matrix[i]);
+	}
+	kfree(matrix);
+}
+
+void print_frame_u8(char *label, u8 **matrix, int row, int column)
+{
+	int i, j;
+
+	logError(0, "%s %s\n", tag, label);
+	for (i = 0; i < row; i++) {
+		logError(0, "%s ", tag);
+		for (j = 0; j < column; j++)
+			logError(0, "%d ", matrix[i][j]);
+		logError(0, "\n");
+		kfree(matrix[i]);
+	}
+	kfree(matrix);
+}
+
+void print_frame_u32(char *label, u32 **matrix, int row, int column)
+{
+	int i, j;
+
+	logError(0, "%s %s\n", tag, label);
+	for (i = 0; i < row; i++) {
+		logError(0, "%s ", tag);
+		for (j = 0; j < column; j++)
+			logError(0, "%d ", matrix[i][j]);
+		logError(0, "\n");
+		kfree(matrix[i]);
+	}
+	kfree(matrix);
+}
+
+void print_frame_int(char *label, int **matrix, int row, int column)
+{
+	int i, j;
+
+	logError(0, "%s %s\n", tag, label);
+	for (i = 0; i < row; i++) {
+		logError(0, "%s ", tag);
+		for (j = 0; j < column; j++)
+			logError(0, "%d ", matrix[i][j]);
+		logError(0, "\n");
+		kfree(matrix[i]);
+	}
+	kfree(matrix);
+}

st/fts_lib/ftsTool.h

@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * FTS Capacitive touch screen controller (FingerTipS)
+ *
+ * Copyright (C) 2016-2019, STMicroelectronics Limited.
+ * Authors: AMG(Analog Mems Group) <[email protected]>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ *
+ **************************************************************************
+ **                        STMicroelectronics                            **
+ **************************************************************************
+ **                        [email protected]                             **
+ **************************************************************************
+ *                                                                        *
+ *                     FTS Utility Functions                              *
+ *                                                                        *
+ **************************************************************************
+ **************************************************************************
+ *
+ */
+
+#ifndef __FTS_TOOL_H
+#define __FTS_TOOL_H
+
+#define GPIO_NOT_DEFINED              -1
+#define TIMEOUT_RESOLUTION            10 //ms
+#define GENERAL_TIMEOUT               (50*TIMEOUT_RESOLUTION) //ms
+#define RELEASE_INFO_TIMEOUT          (15*TIMEOUT_RESOLUTION) //ms
+
+
+#define FEAT_ENABLE                   1
+#define FEAT_DISABLE                  0
+
+#define SYSTEM_RESET_RETRY            3
+
+#define B2_RETRY                      2
+//for FTM4 can not be greater than 13 bytes
+#define LOCKDOWN_CODE_SIZE            10
+
+#define LOCKDOWN_CODE_MAX_SIZE        63
+#define LOCKDOWN_CODE_WRITE_CHUNK     12
+#define LOCKDOWN_CODE_READ_CHUNK      4
+#define LOCKDOWN_CODE_RETRY           2
+
+int readB2(u16 address, u8 *outBuf, int len);
+int readB2U16(u16 address, u8 *outBuf, int byteToRead);
+int releaseInformation(void);
+int lockDownInfo(u8 *data, int len);
+int calculateCRC8(u8 *u8_srcBuff, int size, u8 *crc);
+int writeLockDownInfo(u8 *data, int size);
+int rewriteLockDownInfo(u8 *data, int size);
+int readLockDownInfo(u8 *lockData, int *size);
+char *printHex(char *label, u8 *buff, int count);
+int pollForEvent(int *event_to_search, int event_bytes,
+	u8 *readData, int time_to_wait);
+int fts_disableInterrupt(void);
+int fts_enableInterrupt(void);
+int u8ToU16(u8 *src, u16 *dst);
+int u8ToU16_le(u8 *src, u16 *dst);
+int u8ToU16n(u8 *src, int src_length, u16 *dst);
+int u16ToU8(u16 src, u8 *dst);
+int u16ToU8_le(u16 src, u8 *dst);
+int u16ToU8_be(u16 src, u8 *dst);
+int u16ToU8n(u16 *src, int src_length, u8 *dst);
+int u8ToU32(u8 *src, u32 *dst);
+int u32ToU8(u32 src, u8 *dst);
+int attempt_function(int(*code)(void), unsigned long wait_before_retry,
+	int retry_count);
+void setResetGpio(int gpio);
+int fts_system_reset(void);
+int isSystemResettedUp(void);
+int isSystemResettedDown(void);
+void setSystemResettedUp(int val);
+void setSystemResettedDown(int val);
+int senseOn(void);
+int senseOff(void);
+int keyOn(void);
+int keyOff(void);
+int featureEnableDisable(int on_off, u32 feature);
+int writeNoiseParameters(u8 *noise);
+int readNoiseParameters(u8 *noise);
+int checkEcho(u8 *cmd, int size);
+void print_frame_short(char *label, short **matrix, int row, int column);
+short **array1dTo2d_short(short *data, int size, int columns);
+u8 **array1dTo2d_u8(u8 *data, int size, int columns);
+void print_frame_u8(char *label, u8 **matrix, int row, int column);
+void print_frame_u32(char *label, u32 **matrix, int row, int column);
+void print_frame_int(char *label, int **matrix, int row, int column);
+int cleanUp(int enableTouch);
+int flushFIFO(void);
+
+#endif

synaptics_dsx/synaptics_dsx_active_pen.c

@@ -0,0 +1,606 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define APEN_PHYS_NAME "synaptics_dsx/active_pen"
+
+#define ACTIVE_PEN_MAX_PRESSURE_16BIT 65535
+#define ACTIVE_PEN_MAX_PRESSURE_8BIT 255
+
+struct synaptics_rmi4_f12_query_8 {
+	union {
+		struct {
+			unsigned char size_of_query9;
+			struct {
+				unsigned char data0_is_present:1;
+				unsigned char data1_is_present:1;
+				unsigned char data2_is_present:1;
+				unsigned char data3_is_present:1;
+				unsigned char data4_is_present:1;
+				unsigned char data5_is_present:1;
+				unsigned char data6_is_present:1;
+				unsigned char data7_is_present:1;
+			} __packed;
+		};
+		unsigned char data[2];
+	};
+};
+
+struct apen_data_8b_pressure {
+	union {
+		struct {
+			unsigned char status_pen:1;
+			unsigned char status_invert:1;
+			unsigned char status_barrel:1;
+			unsigned char status_reserved:5;
+			unsigned char x_lsb;
+			unsigned char x_msb;
+			unsigned char y_lsb;
+			unsigned char y_msb;
+			unsigned char pressure_msb;
+			unsigned char battery_state;
+			unsigned char pen_id_0_7;
+			unsigned char pen_id_8_15;
+			unsigned char pen_id_16_23;
+			unsigned char pen_id_24_31;
+		} __packed;
+		unsigned char data[11];
+	};
+};
+
+struct apen_data {
+	union {
+		struct {
+			unsigned char status_pen:1;
+			unsigned char status_invert:1;
+			unsigned char status_barrel:1;
+			unsigned char status_reserved:5;
+			unsigned char x_lsb;
+			unsigned char x_msb;
+			unsigned char y_lsb;
+			unsigned char y_msb;
+			unsigned char pressure_lsb;
+			unsigned char pressure_msb;
+			unsigned char battery_state;
+			unsigned char pen_id_0_7;
+			unsigned char pen_id_8_15;
+			unsigned char pen_id_16_23;
+			unsigned char pen_id_24_31;
+		} __packed;
+		unsigned char data[12];
+	};
+};
+
+struct synaptics_rmi4_apen_handle {
+	bool apen_present;
+	unsigned char intr_mask;
+	unsigned char battery_state;
+	unsigned short query_base_addr;
+	unsigned short control_base_addr;
+	unsigned short data_base_addr;
+	unsigned short command_base_addr;
+	unsigned short apen_data_addr;
+	unsigned short max_pressure;
+	unsigned int pen_id;
+	struct input_dev *apen_dev;
+	struct apen_data *apen_data;
+	struct synaptics_rmi4_data *rmi4_data;
+};
+
+static struct synaptics_rmi4_apen_handle *apen;
+
+DECLARE_COMPLETION(apen_remove_complete);
+
+static void apen_lift(void)
+{
+	input_report_key(apen->apen_dev, BTN_TOUCH, 0);
+	input_report_key(apen->apen_dev, BTN_TOOL_PEN, 0);
+	input_report_key(apen->apen_dev, BTN_TOOL_RUBBER, 0);
+	input_sync(apen->apen_dev);
+	apen->apen_present = false;
+}
+
+static void apen_report(void)
+{
+	int retval;
+	int x;
+	int y;
+	int pressure;
+	static int invert = -1;
+	struct apen_data_8b_pressure *apen_data_8b;
+	struct synaptics_rmi4_data *rmi4_data = apen->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			apen->apen_data_addr,
+			apen->apen_data->data,
+			sizeof(apen->apen_data->data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read active pen data\n",
+				__func__);
+		return;
+	}
+
+	if (apen->apen_data->status_pen == 0) {
+		if (apen->apen_present)
+			apen_lift();
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: No active pen data\n",
+				__func__);
+
+		return;
+	}
+
+	x = (apen->apen_data->x_msb << 8) | (apen->apen_data->x_lsb);
+	y = (apen->apen_data->y_msb << 8) | (apen->apen_data->y_lsb);
+
+	if ((x == -1) && (y == -1)) {
+		if (apen->apen_present)
+			apen_lift();
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Active pen in range but no valid x & y\n",
+				__func__);
+
+		return;
+	}
+
+	if (!apen->apen_present)
+		invert = -1;
+
+	if (invert != -1 && invert != apen->apen_data->status_invert)
+		apen_lift();
+
+	invert = apen->apen_data->status_invert;
+
+	if (apen->max_pressure == ACTIVE_PEN_MAX_PRESSURE_16BIT) {
+		pressure = (apen->apen_data->pressure_msb << 8) |
+				apen->apen_data->pressure_lsb;
+		apen->battery_state = apen->apen_data->battery_state;
+		apen->pen_id = (apen->apen_data->pen_id_24_31 << 24) |
+				(apen->apen_data->pen_id_16_23 << 16) |
+				(apen->apen_data->pen_id_8_15 << 8) |
+				apen->apen_data->pen_id_0_7;
+	} else {
+		apen_data_8b = (struct apen_data_8b_pressure *)apen->apen_data;
+		pressure = apen_data_8b->pressure_msb;
+		apen->battery_state = apen_data_8b->battery_state;
+		apen->pen_id = (apen_data_8b->pen_id_24_31 << 24) |
+				(apen_data_8b->pen_id_16_23 << 16) |
+				(apen_data_8b->pen_id_8_15 << 8) |
+				apen_data_8b->pen_id_0_7;
+	}
+
+	input_report_key(apen->apen_dev, BTN_TOUCH, pressure > 0 ? 1 : 0);
+	input_report_key(apen->apen_dev,
+			apen->apen_data->status_invert > 0 ?
+			BTN_TOOL_RUBBER : BTN_TOOL_PEN, 1);
+	input_report_key(apen->apen_dev,
+			BTN_STYLUS, apen->apen_data->status_barrel > 0 ?
+			1 : 0);
+	input_report_abs(apen->apen_dev, ABS_X, x);
+	input_report_abs(apen->apen_dev, ABS_Y, y);
+	input_report_abs(apen->apen_dev, ABS_PRESSURE, pressure);
+
+	input_sync(apen->apen_dev);
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Active pen: status = %d, invert = %d, barrel = %d, x = %d, y = %d, pressure = %d\n",
+			__func__,
+			apen->apen_data->status_pen,
+			apen->apen_data->status_invert,
+			apen->apen_data->status_barrel,
+			x, y, pressure);
+
+	apen->apen_present = true;
+}
+
+static void apen_set_params(void)
+{
+	input_set_abs_params(apen->apen_dev, ABS_X, 0,
+			apen->rmi4_data->sensor_max_x, 0, 0);
+	input_set_abs_params(apen->apen_dev, ABS_Y, 0,
+			apen->rmi4_data->sensor_max_y, 0, 0);
+	input_set_abs_params(apen->apen_dev, ABS_PRESSURE, 0,
+			apen->max_pressure, 0, 0);
+}
+
+static int apen_pressure(struct synaptics_rmi4_f12_query_8 *query_8)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char data_reg_presence;
+	unsigned char size_of_query_9;
+	unsigned char *query_9;
+	unsigned char *data_desc;
+	struct synaptics_rmi4_data *rmi4_data = apen->rmi4_data;
+
+	data_reg_presence = query_8->data[1];
+
+	size_of_query_9 = query_8->size_of_query9;
+	query_9 = kmalloc(size_of_query_9, GFP_KERNEL);
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			apen->query_base_addr + 9,
+			query_9,
+			size_of_query_9);
+	if (retval < 0)
+		goto exit;
+
+	data_desc = query_9;
+
+	for (ii = 0; ii < 6; ii++) {
+		if (!(data_reg_presence & (1 << ii)))
+			continue; /* The data register is not present */
+		data_desc++; /* Jump over the size entry */
+		while (*data_desc & (1 << 7))
+			data_desc++;
+		data_desc++; /* Go to the next descriptor */
+	}
+
+	data_desc++; /* Jump over the size entry */
+	/* Check for the presence of subpackets 1 and 2 */
+	if ((*data_desc & (3 << 1)) == (3 << 1))
+		apen->max_pressure = ACTIVE_PEN_MAX_PRESSURE_16BIT;
+	else
+		apen->max_pressure = ACTIVE_PEN_MAX_PRESSURE_8BIT;
+
+exit:
+	kfree(query_9);
+
+	return retval;
+}
+
+static int apen_reg_init(void)
+{
+	int retval;
+	unsigned char data_offset;
+	unsigned char size_of_query8;
+	struct synaptics_rmi4_f12_query_8 query_8;
+	struct synaptics_rmi4_data *rmi4_data = apen->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			apen->query_base_addr + 7,
+			&size_of_query8,
+			sizeof(size_of_query8));
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			apen->query_base_addr + 8,
+			query_8.data,
+			sizeof(query_8.data));
+	if (retval < 0)
+		return retval;
+
+	if ((size_of_query8 >= 2) && (query_8.data6_is_present)) {
+		data_offset = query_8.data0_is_present +
+				query_8.data1_is_present +
+				query_8.data2_is_present +
+				query_8.data3_is_present +
+				query_8.data4_is_present +
+				query_8.data5_is_present;
+		apen->apen_data_addr = apen->data_base_addr + data_offset;
+		retval = apen_pressure(&query_8);
+		if (retval < 0)
+			return retval;
+	} else {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Active pen support unavailable\n",
+				__func__);
+		retval = -ENODEV;
+	}
+
+	return retval;
+}
+
+static int apen_scan_pdt(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char page;
+	unsigned char intr_count = 0;
+	unsigned char intr_off;
+	unsigned char intr_src;
+	unsigned short addr;
+	struct synaptics_rmi4_fn_desc fd;
+	struct synaptics_rmi4_data *rmi4_data = apen->rmi4_data;
+
+	for (page = 0; page < PAGES_TO_SERVICE; page++) {
+		for (addr = PDT_START; addr > PDT_END; addr -= PDT_ENTRY_SIZE) {
+			addr |= (page << 8);
+
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					addr,
+					(unsigned char *)&fd,
+					sizeof(fd));
+			if (retval < 0)
+				return retval;
+
+			addr &= ~(MASK_8BIT << 8);
+
+			if (fd.fn_number) {
+				dev_dbg(rmi4_data->pdev->dev.parent,
+						"%s: Found F%02x\n",
+						__func__, fd.fn_number);
+				switch (fd.fn_number) {
+				case SYNAPTICS_RMI4_F12:
+					goto f12_found;
+				}
+			} else {
+				break;
+			}
+
+			intr_count += fd.intr_src_count;
+		}
+	}
+
+	dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Failed to find F12\n",
+			__func__);
+	return -EINVAL;
+
+f12_found:
+	apen->query_base_addr = fd.query_base_addr | (page << 8);
+	apen->control_base_addr = fd.ctrl_base_addr | (page << 8);
+	apen->data_base_addr = fd.data_base_addr | (page << 8);
+	apen->command_base_addr = fd.cmd_base_addr | (page << 8);
+
+	retval = apen_reg_init();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to initialize active pen registers\n",
+				__func__);
+		return retval;
+	}
+
+	apen->intr_mask = 0;
+	intr_src = fd.intr_src_count;
+	intr_off = intr_count % 8;
+	for (ii = intr_off;
+			ii < (intr_src + intr_off);
+			ii++) {
+		apen->intr_mask |= 1 << ii;
+	}
+
+	rmi4_data->intr_mask[0] |= apen->intr_mask;
+
+	addr = rmi4_data->f01_ctrl_base_addr + 1;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			addr,
+			&(rmi4_data->intr_mask[0]),
+			sizeof(rmi4_data->intr_mask[0]));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set interrupt enable bit\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static void synaptics_rmi4_apen_attn(struct synaptics_rmi4_data *rmi4_data,
+		unsigned char intr_mask)
+{
+	if (!apen)
+		return;
+
+	if (apen->intr_mask & intr_mask)
+		apen_report();
+
+	return;
+}
+
+static int synaptics_rmi4_apen_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+
+	if (apen) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Handle already exists\n",
+				__func__);
+		return 0;
+	}
+
+	apen = kzalloc(sizeof(*apen), GFP_KERNEL);
+	if (!apen) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for apen\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	apen->apen_data = kzalloc(sizeof(*(apen->apen_data)), GFP_KERNEL);
+	if (!apen->apen_data) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for apen_data\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_apen;
+	}
+
+	apen->rmi4_data = rmi4_data;
+
+	retval = apen_scan_pdt();
+	if (retval < 0)
+		goto exit_free_apen_data;
+
+	apen->apen_dev = input_allocate_device();
+	if (apen->apen_dev == NULL) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to allocate active pen device\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_apen_data;
+	}
+
+	apen->apen_dev->name = ACTIVE_PEN_DRIVER_NAME;
+	apen->apen_dev->phys = APEN_PHYS_NAME;
+	apen->apen_dev->id.product = SYNAPTICS_DSX_DRIVER_PRODUCT;
+	apen->apen_dev->id.version = SYNAPTICS_DSX_DRIVER_VERSION;
+	apen->apen_dev->dev.parent = rmi4_data->pdev->dev.parent;
+	input_set_drvdata(apen->apen_dev, rmi4_data);
+
+	set_bit(EV_KEY, apen->apen_dev->evbit);
+	set_bit(EV_ABS, apen->apen_dev->evbit);
+	set_bit(BTN_TOUCH, apen->apen_dev->keybit);
+	set_bit(BTN_TOOL_PEN, apen->apen_dev->keybit);
+	set_bit(BTN_TOOL_RUBBER, apen->apen_dev->keybit);
+	set_bit(BTN_STYLUS, apen->apen_dev->keybit);
+#ifdef INPUT_PROP_DIRECT
+	set_bit(INPUT_PROP_DIRECT, apen->apen_dev->propbit);
+#endif
+
+	apen_set_params();
+
+	retval = input_register_device(apen->apen_dev);
+	if (retval) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to register active pen device\n",
+				__func__);
+		goto exit_free_input_device;
+	}
+
+	return 0;
+
+exit_free_input_device:
+	input_free_device(apen->apen_dev);
+
+exit_free_apen_data:
+	kfree(apen->apen_data);
+
+exit_free_apen:
+	kfree(apen);
+	apen = NULL;
+
+exit:
+	return retval;
+}
+
+static void synaptics_rmi4_apen_remove(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!apen)
+		goto exit;
+
+	input_unregister_device(apen->apen_dev);
+	kfree(apen->apen_data);
+	kfree(apen);
+	apen = NULL;
+
+exit:
+	complete(&apen_remove_complete);
+}
+
+static void synaptics_rmi4_apen_reset(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!apen) {
+		synaptics_rmi4_apen_init(rmi4_data);
+		return;
+	}
+
+	apen_lift();
+
+	apen_scan_pdt();
+}
+
+static void synaptics_rmi4_apen_reinit(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!apen)
+		return;
+
+	apen_lift();
+}
+
+static void synaptics_rmi4_apen_e_suspend(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!apen)
+		return;
+
+	apen_lift();
+}
+
+static void synaptics_rmi4_apen_suspend(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!apen)
+		return;
+
+	apen_lift();
+}
+
+static struct synaptics_rmi4_exp_fn active_pen_module = {
+	.fn_type = RMI_ACTIVE_PEN,
+	.init = synaptics_rmi4_apen_init,
+	.remove = synaptics_rmi4_apen_remove,
+	.reset = synaptics_rmi4_apen_reset,
+	.reinit = synaptics_rmi4_apen_reinit,
+	.early_suspend = synaptics_rmi4_apen_e_suspend,
+	.suspend = synaptics_rmi4_apen_suspend,
+	.resume = NULL,
+	.late_resume = NULL,
+	.attn = synaptics_rmi4_apen_attn,
+};
+
+static int __init rmi4_active_pen_module_init(void)
+{
+	synaptics_rmi4_new_function(&active_pen_module, true);
+
+	return 0;
+}
+
+static void __exit rmi4_active_pen_module_exit(void)
+{
+	synaptics_rmi4_new_function(&active_pen_module, false);
+
+	wait_for_completion(&apen_remove_complete);
+}
+
+module_init(rmi4_active_pen_module_init);
+module_exit(rmi4_active_pen_module_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX Active Pen Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_core.c

@@ -0,0 +1,5079 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/input/synaptics_dsx.h>
+#include <linux/soc/qcom/panel_event_notifier.h>
+#include "synaptics_dsx_core.h"
+#ifdef KERNEL_ABOVE_2_6_38
+#include <linux/input/mt.h>
+#endif
+
+#include <linux/completion.h>
+
+#define INPUT_PHYS_NAME "synaptics_dsx/touch_input"
+#define STYLUS_PHYS_NAME "synaptics_dsx/stylus"
+
+#define VIRTUAL_KEY_MAP_FILE_NAME "virtualkeys." PLATFORM_DRIVER_NAME
+
+#ifdef KERNEL_ABOVE_2_6_38
+#define TYPE_B_PROTOCOL
+#endif
+
+/*
+#define USE_DATA_SERVER
+*/
+
+#define WAKEUP_GESTURE false
+
+#define NO_0D_WHILE_2D
+#define REPORT_2D_Z
+#define REPORT_2D_W
+/*
+#define REPORT_2D_PRESSURE
+*/
+
+#define F12_DATA_15_WORKAROUND
+
+#define IGNORE_FN_INIT_FAILURE
+#define FB_READY_RESET
+#define FB_READY_WAIT_MS 100
+#define FB_READY_TIMEOUT_S 30
+#ifdef SYNA_TDDI
+#define TDDI_LPWG_WAIT_US 10
+#endif
+#define RPT_TYPE (1 << 0)
+#define RPT_X_LSB (1 << 1)
+#define RPT_X_MSB (1 << 2)
+#define RPT_Y_LSB (1 << 3)
+#define RPT_Y_MSB (1 << 4)
+#define RPT_Z (1 << 5)
+#define RPT_WX (1 << 6)
+#define RPT_WY (1 << 7)
+#define RPT_DEFAULT (RPT_TYPE | RPT_X_LSB | RPT_X_MSB | RPT_Y_LSB | RPT_Y_MSB)
+
+#define REBUILD_WORK_DELAY_MS 500 /* ms */
+
+#define EXP_FN_WORK_DELAY_MS 500 /* ms */
+#define MAX_F11_TOUCH_WIDTH 15
+#define MAX_F12_TOUCH_WIDTH 255
+
+#define CHECK_STATUS_TIMEOUT_MS 100
+
+#define F01_STD_QUERY_LEN 21
+#define F01_BUID_ID_OFFSET 18
+
+#define STATUS_NO_ERROR 0x00
+#define STATUS_RESET_OCCURRED 0x01
+#define STATUS_INVALID_CONFIG 0x02
+#define STATUS_DEVICE_FAILURE 0x03
+#define STATUS_CONFIG_CRC_FAILURE 0x04
+#define STATUS_FIRMWARE_CRC_FAILURE 0x05
+#define STATUS_CRC_IN_PROGRESS 0x06
+
+#define NORMAL_OPERATION (0 << 0)
+#define SENSOR_SLEEP (1 << 0)
+#define NO_SLEEP_OFF (0 << 2)
+#define NO_SLEEP_ON (1 << 2)
+#define CONFIGURED (1 << 7)
+
+#define F11_CONTINUOUS_MODE 0x00
+#define F11_WAKEUP_GESTURE_MODE 0x04
+#define F12_CONTINUOUS_MODE 0x00
+#define F12_WAKEUP_GESTURE_MODE 0x02
+#define F12_UDG_DETECT 0x0f
+
+static int synaptics_rmi4_check_status(struct synaptics_rmi4_data *rmi4_data,
+		bool *was_in_bl_mode);
+static int synaptics_rmi4_free_fingers(struct synaptics_rmi4_data *rmi4_data);
+static int synaptics_rmi4_reinit_device(struct synaptics_rmi4_data *rmi4_data);
+static int synaptics_rmi4_reset_device(struct synaptics_rmi4_data *rmi4_data,
+		bool rebuild);
+
+#ifdef CONFIG_DRM
+static void synaptics_rmi4_dsi_panel_notifier_cb(
+		enum panel_event_notifier_tag tag,
+		struct panel_event_notification *notification,
+		void *client_data);
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#ifndef CONFIG_FB
+#define USE_EARLYSUSPEND
+#endif
+#endif
+
+#ifdef USE_EARLYSUSPEND
+static int synaptics_rmi4_early_suspend(struct early_suspend *h);
+
+static int synaptics_rmi4_late_resume(struct early_suspend *h);
+#endif
+
+static int synaptics_rmi4_suspend(struct device *dev);
+
+static int synaptics_rmi4_resume(struct device *dev);
+
+static void synaptics_rmi4_defer_probe(struct work_struct *work);
+
+static ssize_t synaptics_rmi4_f01_reset_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t synaptics_rmi4_f01_productinfo_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t synaptics_rmi4_f01_buildid_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t synaptics_rmi4_f01_flashprog_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t synaptics_rmi4_0dbutton_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t synaptics_rmi4_0dbutton_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t synaptics_rmi4_suspend_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t synaptics_rmi4_wake_gesture_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t synaptics_rmi4_wake_gesture_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+#ifdef USE_DATA_SERVER
+static ssize_t synaptics_rmi4_synad_pid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+#endif
+
+static ssize_t synaptics_rmi4_virtual_key_map_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf);
+
+struct synaptics_rmi4_f01_device_status {
+	union {
+		struct {
+			unsigned char status_code:4;
+			unsigned char reserved:2;
+			unsigned char flash_prog:1;
+			unsigned char unconfigured:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct synaptics_rmi4_f11_query_0_5 {
+	union {
+		struct {
+			/* query 0 */
+			unsigned char f11_query0_b0__2:3;
+			unsigned char has_query_9:1;
+			unsigned char has_query_11:1;
+			unsigned char has_query_12:1;
+			unsigned char has_query_27:1;
+			unsigned char has_query_28:1;
+
+			/* query 1 */
+			unsigned char num_of_fingers:3;
+			unsigned char has_rel:1;
+			unsigned char has_abs:1;
+			unsigned char has_gestures:1;
+			unsigned char has_sensitibity_adjust:1;
+			unsigned char f11_query1_b7:1;
+
+			/* query 2 */
+			unsigned char num_of_x_electrodes;
+
+			/* query 3 */
+			unsigned char num_of_y_electrodes;
+
+			/* query 4 */
+			unsigned char max_electrodes:7;
+			unsigned char f11_query4_b7:1;
+
+			/* query 5 */
+			unsigned char abs_data_size:2;
+			unsigned char has_anchored_finger:1;
+			unsigned char has_adj_hyst:1;
+			unsigned char has_dribble:1;
+			unsigned char has_bending_correction:1;
+			unsigned char has_large_object_suppression:1;
+			unsigned char has_jitter_filter:1;
+		} __packed;
+		unsigned char data[6];
+	};
+};
+
+struct synaptics_rmi4_f11_query_7_8 {
+	union {
+		struct {
+			/* query 7 */
+			unsigned char has_single_tap:1;
+			unsigned char has_tap_and_hold:1;
+			unsigned char has_double_tap:1;
+			unsigned char has_early_tap:1;
+			unsigned char has_flick:1;
+			unsigned char has_press:1;
+			unsigned char has_pinch:1;
+			unsigned char has_chiral_scroll:1;
+
+			/* query 8 */
+			unsigned char has_palm_detect:1;
+			unsigned char has_rotate:1;
+			unsigned char has_touch_shapes:1;
+			unsigned char has_scroll_zones:1;
+			unsigned char individual_scroll_zones:1;
+			unsigned char has_multi_finger_scroll:1;
+			unsigned char has_multi_finger_scroll_edge_motion:1;
+			unsigned char has_multi_finger_scroll_inertia:1;
+		} __packed;
+		unsigned char data[2];
+	};
+};
+
+struct synaptics_rmi4_f11_query_9 {
+	union {
+		struct {
+			unsigned char has_pen:1;
+			unsigned char has_proximity:1;
+			unsigned char has_large_object_sensitivity:1;
+			unsigned char has_suppress_on_large_object_detect:1;
+			unsigned char has_two_pen_thresholds:1;
+			unsigned char has_contact_geometry:1;
+			unsigned char has_pen_hover_discrimination:1;
+			unsigned char has_pen_hover_and_edge_filters:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct synaptics_rmi4_f11_query_12 {
+	union {
+		struct {
+			unsigned char has_small_object_detection:1;
+			unsigned char has_small_object_detection_tuning:1;
+			unsigned char has_8bit_w:1;
+			unsigned char has_2d_adjustable_mapping:1;
+			unsigned char has_general_information_2:1;
+			unsigned char has_physical_properties:1;
+			unsigned char has_finger_limit:1;
+			unsigned char has_linear_cofficient_2:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct synaptics_rmi4_f11_query_27 {
+	union {
+		struct {
+			unsigned char f11_query27_b0:1;
+			unsigned char has_pen_position_correction:1;
+			unsigned char has_pen_jitter_filter_coefficient:1;
+			unsigned char has_group_decomposition:1;
+			unsigned char has_wakeup_gesture:1;
+			unsigned char has_small_finger_correction:1;
+			unsigned char has_data_37:1;
+			unsigned char f11_query27_b7:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct synaptics_rmi4_f11_ctrl_6_9 {
+	union {
+		struct {
+			unsigned char sensor_max_x_pos_7_0;
+			unsigned char sensor_max_x_pos_11_8:4;
+			unsigned char f11_ctrl7_b4__7:4;
+			unsigned char sensor_max_y_pos_7_0;
+			unsigned char sensor_max_y_pos_11_8:4;
+			unsigned char f11_ctrl9_b4__7:4;
+		} __packed;
+		unsigned char data[4];
+	};
+};
+
+struct synaptics_rmi4_f11_data_1_5 {
+	union {
+		struct {
+			unsigned char x_position_11_4;
+			unsigned char y_position_11_4;
+			unsigned char x_position_3_0:4;
+			unsigned char y_position_3_0:4;
+			unsigned char wx:4;
+			unsigned char wy:4;
+			unsigned char z;
+		} __packed;
+		unsigned char data[5];
+	};
+};
+
+struct synaptics_rmi4_f12_query_5 {
+	union {
+		struct {
+			unsigned char size_of_query6;
+			struct {
+				unsigned char ctrl0_is_present:1;
+				unsigned char ctrl1_is_present:1;
+				unsigned char ctrl2_is_present:1;
+				unsigned char ctrl3_is_present:1;
+				unsigned char ctrl4_is_present:1;
+				unsigned char ctrl5_is_present:1;
+				unsigned char ctrl6_is_present:1;
+				unsigned char ctrl7_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl8_is_present:1;
+				unsigned char ctrl9_is_present:1;
+				unsigned char ctrl10_is_present:1;
+				unsigned char ctrl11_is_present:1;
+				unsigned char ctrl12_is_present:1;
+				unsigned char ctrl13_is_present:1;
+				unsigned char ctrl14_is_present:1;
+				unsigned char ctrl15_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl16_is_present:1;
+				unsigned char ctrl17_is_present:1;
+				unsigned char ctrl18_is_present:1;
+				unsigned char ctrl19_is_present:1;
+				unsigned char ctrl20_is_present:1;
+				unsigned char ctrl21_is_present:1;
+				unsigned char ctrl22_is_present:1;
+				unsigned char ctrl23_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl24_is_present:1;
+				unsigned char ctrl25_is_present:1;
+				unsigned char ctrl26_is_present:1;
+				unsigned char ctrl27_is_present:1;
+				unsigned char ctrl28_is_present:1;
+				unsigned char ctrl29_is_present:1;
+				unsigned char ctrl30_is_present:1;
+				unsigned char ctrl31_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl32_is_present:1;
+				unsigned char ctrl33_is_present:1;
+				unsigned char ctrl34_is_present:1;
+				unsigned char ctrl35_is_present:1;
+				unsigned char ctrl36_is_present:1;
+				unsigned char ctrl37_is_present:1;
+				unsigned char ctrl38_is_present:1;
+				unsigned char ctrl39_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl40_is_present:1;
+				unsigned char ctrl41_is_present:1;
+				unsigned char ctrl42_is_present:1;
+				unsigned char ctrl43_is_present:1;
+				unsigned char ctrl44_is_present:1;
+				unsigned char ctrl45_is_present:1;
+				unsigned char ctrl46_is_present:1;
+				unsigned char ctrl47_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl48_is_present:1;
+				unsigned char ctrl49_is_present:1;
+				unsigned char ctrl50_is_present:1;
+				unsigned char ctrl51_is_present:1;
+				unsigned char ctrl52_is_present:1;
+				unsigned char ctrl53_is_present:1;
+				unsigned char ctrl54_is_present:1;
+				unsigned char ctrl55_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl56_is_present:1;
+				unsigned char ctrl57_is_present:1;
+				unsigned char ctrl58_is_present:1;
+				unsigned char ctrl59_is_present:1;
+				unsigned char ctrl60_is_present:1;
+				unsigned char ctrl61_is_present:1;
+				unsigned char ctrl62_is_present:1;
+				unsigned char ctrl63_is_present:1;
+			} __packed;
+		};
+		unsigned char data[9];
+	};
+};
+
+struct synaptics_rmi4_f12_query_8 {
+	union {
+		struct {
+			unsigned char size_of_query9;
+			struct {
+				unsigned char data0_is_present:1;
+				unsigned char data1_is_present:1;
+				unsigned char data2_is_present:1;
+				unsigned char data3_is_present:1;
+				unsigned char data4_is_present:1;
+				unsigned char data5_is_present:1;
+				unsigned char data6_is_present:1;
+				unsigned char data7_is_present:1;
+			} __packed;
+			struct {
+				unsigned char data8_is_present:1;
+				unsigned char data9_is_present:1;
+				unsigned char data10_is_present:1;
+				unsigned char data11_is_present:1;
+				unsigned char data12_is_present:1;
+				unsigned char data13_is_present:1;
+				unsigned char data14_is_present:1;
+				unsigned char data15_is_present:1;
+			} __packed;
+			struct {
+				unsigned char data16_is_present:1;
+				unsigned char data17_is_present:1;
+				unsigned char data18_is_present:1;
+				unsigned char data19_is_present:1;
+				unsigned char data20_is_present:1;
+				unsigned char data21_is_present:1;
+				unsigned char data22_is_present:1;
+				unsigned char data23_is_present:1;
+			} __packed;
+			struct {
+				unsigned char data24_is_present:1;
+				unsigned char data25_is_present:1;
+				unsigned char data26_is_present:1;
+				unsigned char data27_is_present:1;
+				unsigned char data28_is_present:1;
+				unsigned char data29_is_present:1;
+				unsigned char data30_is_present:1;
+				unsigned char data31_is_present:1;
+			} __packed;
+		};
+		unsigned char data[5];
+	};
+};
+
+struct synaptics_rmi4_f12_ctrl_8 {
+	union {
+		struct {
+			unsigned char max_x_coord_lsb;
+			unsigned char max_x_coord_msb;
+			unsigned char max_y_coord_lsb;
+			unsigned char max_y_coord_msb;
+			unsigned char rx_pitch_lsb;
+			unsigned char rx_pitch_msb;
+			unsigned char tx_pitch_lsb;
+			unsigned char tx_pitch_msb;
+			unsigned char low_rx_clip;
+			unsigned char high_rx_clip;
+			unsigned char low_tx_clip;
+			unsigned char high_tx_clip;
+			unsigned char num_of_rx;
+			unsigned char num_of_tx;
+		};
+		unsigned char data[14];
+	};
+};
+
+struct synaptics_rmi4_f12_ctrl_23 {
+	union {
+		struct {
+			unsigned char finger_enable:1;
+			unsigned char active_stylus_enable:1;
+			unsigned char palm_enable:1;
+			unsigned char unclassified_object_enable:1;
+			unsigned char hovering_finger_enable:1;
+			unsigned char gloved_finger_enable:1;
+			unsigned char f12_ctr23_00_b6__7:2;
+			unsigned char max_reported_objects;
+			unsigned char f12_ctr23_02_b0:1;
+			unsigned char report_active_stylus_as_finger:1;
+			unsigned char report_palm_as_finger:1;
+			unsigned char report_unclassified_object_as_finger:1;
+			unsigned char report_hovering_finger_as_finger:1;
+			unsigned char report_gloved_finger_as_finger:1;
+			unsigned char report_narrow_object_swipe_as_finger:1;
+			unsigned char report_handedge_as_finger:1;
+			unsigned char cover_enable:1;
+			unsigned char stylus_enable:1;
+			unsigned char eraser_enable:1;
+			unsigned char small_object_enable:1;
+			unsigned char f12_ctr23_03_b4__7:4;
+			unsigned char report_cover_as_finger:1;
+			unsigned char report_stylus_as_finger:1;
+			unsigned char report_eraser_as_finger:1;
+			unsigned char report_small_object_as_finger:1;
+			unsigned char f12_ctr23_04_b4__7:4;
+		};
+		unsigned char data[5];
+	};
+};
+
+struct synaptics_rmi4_f12_ctrl_31 {
+	union {
+		struct {
+			unsigned char max_x_coord_lsb;
+			unsigned char max_x_coord_msb;
+			unsigned char max_y_coord_lsb;
+			unsigned char max_y_coord_msb;
+			unsigned char rx_pitch_lsb;
+			unsigned char rx_pitch_msb;
+			unsigned char rx_clip_low;
+			unsigned char rx_clip_high;
+			unsigned char wedge_clip_low;
+			unsigned char wedge_clip_high;
+			unsigned char num_of_p;
+			unsigned char num_of_q;
+		};
+		unsigned char data[12];
+	};
+};
+
+struct synaptics_rmi4_f12_ctrl_58 {
+	union {
+		struct {
+			unsigned char reporting_format;
+			unsigned char f12_ctr58_00_reserved;
+			unsigned char min_force_lsb;
+			unsigned char min_force_msb;
+			unsigned char max_force_lsb;
+			unsigned char max_force_msb;
+			unsigned char light_press_threshold_lsb;
+			unsigned char light_press_threshold_msb;
+			unsigned char light_press_hysteresis_lsb;
+			unsigned char light_press_hysteresis_msb;
+			unsigned char hard_press_threshold_lsb;
+			unsigned char hard_press_threshold_msb;
+			unsigned char hard_press_hysteresis_lsb;
+			unsigned char hard_press_hysteresis_msb;
+		};
+		unsigned char data[14];
+	};
+};
+
+struct synaptics_rmi4_f12_finger_data {
+	unsigned char object_type_and_status;
+	unsigned char x_lsb;
+	unsigned char x_msb;
+	unsigned char y_lsb;
+	unsigned char y_msb;
+#ifdef REPORT_2D_Z
+	unsigned char z;
+#endif
+#ifdef REPORT_2D_W
+	unsigned char wx;
+	unsigned char wy;
+#endif
+};
+
+struct synaptics_rmi4_f1a_query {
+	union {
+		struct {
+			unsigned char max_button_count:3;
+			unsigned char f1a_query0_b3__4:2;
+			unsigned char has_query4:1;
+			unsigned char has_query3:1;
+			unsigned char has_query2:1;
+			unsigned char has_general_control:1;
+			unsigned char has_interrupt_enable:1;
+			unsigned char has_multibutton_select:1;
+			unsigned char has_tx_rx_map:1;
+			unsigned char has_perbutton_threshold:1;
+			unsigned char has_release_threshold:1;
+			unsigned char has_strongestbtn_hysteresis:1;
+			unsigned char has_filter_strength:1;
+		} __packed;
+		unsigned char data[2];
+	};
+};
+
+struct synaptics_rmi4_f1a_query_4 {
+	union {
+		struct {
+			unsigned char has_ctrl19:1;
+			unsigned char f1a_query4_b1__4:4;
+			unsigned char has_ctrl24:1;
+			unsigned char f1a_query4_b6__7:2;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct synaptics_rmi4_f1a_control_0 {
+	union {
+		struct {
+			unsigned char multibutton_report:2;
+			unsigned char filter_mode:2;
+			unsigned char reserved:4;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct synaptics_rmi4_f1a_control {
+	struct synaptics_rmi4_f1a_control_0 general_control;
+	unsigned char button_int_enable;
+	unsigned char multi_button;
+	unsigned char *txrx_map;
+	unsigned char *button_threshold;
+	unsigned char button_release_threshold;
+	unsigned char strongest_button_hysteresis;
+	unsigned char filter_strength;
+};
+
+struct synaptics_rmi4_f1a_handle {
+	int button_bitmask_size;
+	unsigned char max_count;
+	unsigned char valid_button_count;
+	unsigned char *button_data_buffer;
+	unsigned char *button_map;
+	struct synaptics_rmi4_f1a_query button_query;
+	struct synaptics_rmi4_f1a_control button_control;
+};
+
+struct synaptics_rmi4_exp_fhandler {
+	struct synaptics_rmi4_exp_fn *exp_fn;
+	bool insert;
+	bool remove;
+	struct list_head link;
+};
+
+struct synaptics_rmi4_exp_fn_data {
+	bool initialized;
+	bool queue_work;
+	struct mutex mutex;
+	struct list_head list;
+	struct delayed_work work;
+	struct workqueue_struct *workqueue;
+	struct synaptics_rmi4_data *rmi4_data;
+};
+
+static struct synaptics_rmi4_exp_fn_data exp_data;
+
+static struct synaptics_dsx_button_map *vir_button_map;
+
+#ifdef USE_DATA_SERVER
+static pid_t synad_pid;
+static struct task_struct *synad_task;
+static struct siginfo interrupt_signal;
+#endif
+
+static struct device_attribute attrs[] = {
+	__ATTR(reset, 0220,
+			synaptics_rmi4_show_error,
+			synaptics_rmi4_f01_reset_store),
+	__ATTR(productinfo, 0444,
+			synaptics_rmi4_f01_productinfo_show,
+			synaptics_rmi4_store_error),
+	__ATTR(buildid, 0444,
+			synaptics_rmi4_f01_buildid_show,
+			synaptics_rmi4_store_error),
+	__ATTR(flashprog, 0444,
+			synaptics_rmi4_f01_flashprog_show,
+			synaptics_rmi4_store_error),
+	__ATTR(0dbutton, 0664,
+			synaptics_rmi4_0dbutton_show,
+			synaptics_rmi4_0dbutton_store),
+	__ATTR(suspend, 0220,
+			synaptics_rmi4_show_error,
+			synaptics_rmi4_suspend_store),
+	__ATTR(wake_gesture, 0664,
+			synaptics_rmi4_wake_gesture_show,
+			synaptics_rmi4_wake_gesture_store),
+#ifdef USE_DATA_SERVER
+	__ATTR(synad_pid, 0220,
+			synaptics_rmi4_show_error,
+			synaptics_rmi4_synad_pid_store),
+#endif
+};
+
+static struct kobj_attribute virtual_key_map_attr = {
+	.attr = {
+		.name = VIRTUAL_KEY_MAP_FILE_NAME,
+		.mode = 0444,
+	},
+	.show = synaptics_rmi4_virtual_key_map_show,
+};
+
+static ssize_t synaptics_rmi4_f01_reset_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int reset;
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	if (kstrtouint(buf, 10, &reset) != 1)
+		return -EINVAL;
+
+	if (reset != 1)
+		return -EINVAL;
+
+	retval = synaptics_rmi4_reset_device(rmi4_data, false);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to issue reset command, error = %d\n",
+				__func__, retval);
+		return retval;
+	}
+
+	return count;
+}
+
+static ssize_t synaptics_rmi4_f01_productinfo_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "0x%02x 0x%02x\n",
+			(rmi4_data->rmi4_mod_info.product_info[0]),
+			(rmi4_data->rmi4_mod_info.product_info[1]));
+}
+
+static ssize_t synaptics_rmi4_f01_buildid_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			rmi4_data->firmware_id);
+}
+
+static ssize_t synaptics_rmi4_f01_flashprog_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	struct synaptics_rmi4_f01_device_status device_status;
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_data_base_addr,
+			device_status.data,
+			sizeof(device_status.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read device status, error = %d\n",
+				__func__, retval);
+		return retval;
+	}
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			device_status.flash_prog);
+}
+
+static ssize_t synaptics_rmi4_0dbutton_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			rmi4_data->button_0d_enabled);
+}
+
+static ssize_t synaptics_rmi4_0dbutton_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	unsigned char ii;
+	unsigned char intr_enable;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	input = input > 0 ? 1 : 0;
+
+	if (rmi4_data->button_0d_enabled == input)
+		return count;
+
+	if (list_empty(&rmi->support_fn_list))
+		return -ENODEV;
+
+	list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+		if (fhandler->fn_number == SYNAPTICS_RMI4_F1A) {
+			ii = fhandler->intr_reg_num;
+
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr + 1 + ii,
+					&intr_enable,
+					sizeof(intr_enable));
+			if (retval < 0)
+				return retval;
+
+			if (input == 1)
+				intr_enable |= fhandler->intr_mask;
+			else
+				intr_enable &= ~fhandler->intr_mask;
+
+			retval = synaptics_rmi4_reg_write(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr + 1 + ii,
+					&intr_enable,
+					sizeof(intr_enable));
+			if (retval < 0)
+				return retval;
+		}
+	}
+
+	rmi4_data->button_0d_enabled = input;
+
+	return count;
+}
+
+static ssize_t synaptics_rmi4_suspend_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input == 1)
+		synaptics_rmi4_suspend(dev);
+	else if (input == 0)
+		synaptics_rmi4_resume(dev);
+	else
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t synaptics_rmi4_wake_gesture_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			rmi4_data->enable_wakeup_gesture);
+}
+
+static ssize_t synaptics_rmi4_wake_gesture_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	input = input > 0 ? 1 : 0;
+
+	if (rmi4_data->f11_wakeup_gesture || rmi4_data->f12_wakeup_gesture)
+		rmi4_data->enable_wakeup_gesture = input;
+
+	return count;
+}
+
+#ifdef USE_DATA_SERVER
+static ssize_t synaptics_rmi4_synad_pid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	synad_pid = input;
+
+	if (synad_pid) {
+		synad_task = pid_task(find_vpid(synad_pid), PIDTYPE_PID);
+		if (!synad_task)
+			return -EINVAL;
+	}
+
+	return count;
+}
+#endif
+
+static ssize_t synaptics_rmi4_virtual_key_map_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	int ii;
+	int cnt;
+	int count = 0;
+
+	for (ii = 0; ii < vir_button_map->nbuttons; ii++) {
+		cnt = snprintf(buf, PAGE_SIZE - count, "0x01:%d:%d:%d:%d:%d\n",
+				vir_button_map->map[ii * 5 + 0],
+				vir_button_map->map[ii * 5 + 1],
+				vir_button_map->map[ii * 5 + 2],
+				vir_button_map->map[ii * 5 + 3],
+				vir_button_map->map[ii * 5 + 4]);
+		buf += cnt;
+		count += cnt;
+	}
+
+	return count;
+}
+
+static int synaptics_rmi4_f11_wg(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	int retval;
+	unsigned char reporting_control;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+		if (fhandler->fn_number == SYNAPTICS_RMI4_F11)
+			break;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.ctrl_base,
+			&reporting_control,
+			sizeof(reporting_control));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to change reporting mode\n",
+				__func__);
+		return retval;
+	}
+
+	reporting_control = (reporting_control & ~MASK_3BIT);
+	if (enable)
+		reporting_control |= F11_WAKEUP_GESTURE_MODE;
+	else
+		reporting_control |= F11_CONTINUOUS_MODE;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			fhandler->full_addr.ctrl_base,
+			&reporting_control,
+			sizeof(reporting_control));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to change reporting mode\n",
+				__func__);
+		return retval;
+	}
+
+	return retval;
+}
+
+static int synaptics_rmi4_f12_wg(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	int retval;
+	unsigned char offset;
+	unsigned char reporting_control[3];
+	struct synaptics_rmi4_f12_extra_data *extra_data;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+		if (fhandler->fn_number == SYNAPTICS_RMI4_F12)
+			break;
+	}
+
+	extra_data = (struct synaptics_rmi4_f12_extra_data *)fhandler->extra;
+	offset = extra_data->ctrl20_offset;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.ctrl_base + offset,
+			reporting_control,
+			sizeof(reporting_control));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to change reporting mode\n",
+				__func__);
+		return retval;
+	}
+
+	if (enable)
+		reporting_control[rmi4_data->set_wakeup_gesture] =
+					F12_WAKEUP_GESTURE_MODE;
+	else
+		reporting_control[rmi4_data->set_wakeup_gesture] =
+					F12_CONTINUOUS_MODE;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			fhandler->full_addr.ctrl_base + offset,
+			reporting_control,
+			sizeof(reporting_control));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to change reporting mode\n",
+				__func__);
+		return retval;
+	}
+
+	return retval;
+}
+
+static void synaptics_rmi4_wakeup_gesture(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	if (rmi4_data->f11_wakeup_gesture)
+		synaptics_rmi4_f11_wg(rmi4_data, enable);
+	else if (rmi4_data->f12_wakeup_gesture)
+		synaptics_rmi4_f12_wg(rmi4_data, enable);
+}
+
+static int synaptics_rmi4_f11_abs_report(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler)
+{
+	int retval;
+	unsigned char touch_count = 0; /* number of touch points */
+	unsigned char reg_index;
+	unsigned char finger;
+	unsigned char fingers_supported;
+	unsigned char num_of_finger_status_regs;
+	unsigned char finger_shift;
+	unsigned char finger_status;
+	unsigned char finger_status_reg[3];
+	unsigned char detected_gestures;
+	unsigned short data_addr;
+	unsigned short data_offset;
+	int x;
+	int y;
+	int wx;
+	int wy;
+	int temp;
+	struct synaptics_rmi4_f11_data_1_5 data;
+	struct synaptics_rmi4_f11_extra_data *extra_data;
+
+	/*
+	 * The number of finger status registers is determined by the
+	 * maximum number of fingers supported - 2 bits per finger. So
+	 * the number of finger status registers to read is:
+	 * register_count = ceil(max_num_of_fingers / 4)
+	 */
+	fingers_supported = fhandler->num_of_data_points;
+	num_of_finger_status_regs = (fingers_supported + 3) / 4;
+	data_addr = fhandler->full_addr.data_base;
+
+	extra_data = (struct synaptics_rmi4_f11_extra_data *)fhandler->extra;
+
+	if (rmi4_data->suspend && rmi4_data->enable_wakeup_gesture) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				data_addr + extra_data->data38_offset,
+				&detected_gestures,
+				sizeof(detected_gestures));
+		if (retval < 0)
+			return 0;
+
+		if (detected_gestures) {
+			input_report_key(rmi4_data->input_dev, KEY_WAKEUP, 1);
+			input_sync(rmi4_data->input_dev);
+			input_report_key(rmi4_data->input_dev, KEY_WAKEUP, 0);
+			input_sync(rmi4_data->input_dev);
+			rmi4_data->suspend = false;
+		}
+/*		synaptics_rmi4_wakeup_gesture(rmi4_data, false); */
+		return 0;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			data_addr,
+			finger_status_reg,
+			num_of_finger_status_regs);
+	if (retval < 0)
+		return 0;
+
+	mutex_lock(&(rmi4_data->rmi4_report_mutex));
+
+	for (finger = 0; finger < fingers_supported; finger++) {
+		reg_index = finger / 4;
+		finger_shift = (finger % 4) * 2;
+		finger_status = (finger_status_reg[reg_index] >> finger_shift)
+				& MASK_2BIT;
+
+		/*
+		 * Each 2-bit finger status field represents the following:
+		 * 00 = finger not present
+		 * 01 = finger present and data accurate
+		 * 10 = finger present but data may be inaccurate
+		 * 11 = reserved
+		 */
+#ifdef TYPE_B_PROTOCOL
+		input_mt_slot(rmi4_data->input_dev, finger);
+		input_mt_report_slot_state(rmi4_data->input_dev,
+				MT_TOOL_FINGER, finger_status);
+#endif
+
+		if (finger_status) {
+			data_offset = data_addr +
+					num_of_finger_status_regs +
+					(finger * sizeof(data.data));
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					data_offset,
+					data.data,
+					sizeof(data.data));
+			if (retval < 0) {
+				touch_count = 0;
+				goto exit;
+			}
+
+			x = (data.x_position_11_4 << 4) | data.x_position_3_0;
+			y = (data.y_position_11_4 << 4) | data.y_position_3_0;
+			wx = data.wx;
+			wy = data.wy;
+
+			if (rmi4_data->hw_if->board_data->swap_axes) {
+				temp = x;
+				x = y;
+				y = temp;
+				temp = wx;
+				wx = wy;
+				wy = temp;
+			}
+
+			if (rmi4_data->hw_if->board_data->x_flip)
+				x = rmi4_data->sensor_max_x - x;
+			if (rmi4_data->hw_if->board_data->y_flip)
+				y = rmi4_data->sensor_max_y - y;
+
+			input_report_key(rmi4_data->input_dev,
+					BTN_TOUCH, 1);
+			input_report_key(rmi4_data->input_dev,
+					BTN_TOOL_FINGER, 1);
+			input_report_abs(rmi4_data->input_dev,
+					ABS_MT_POSITION_X, x);
+			input_report_abs(rmi4_data->input_dev,
+					ABS_MT_POSITION_Y, y);
+#ifdef REPORT_2D_W
+			input_report_abs(rmi4_data->input_dev,
+					ABS_MT_TOUCH_MAJOR, max(wx, wy));
+			input_report_abs(rmi4_data->input_dev,
+					ABS_MT_TOUCH_MINOR, min(wx, wy));
+#endif
+#ifndef TYPE_B_PROTOCOL
+			input_mt_sync(rmi4_data->input_dev);
+#endif
+
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Finger %d: status = 0x%02x, x = %d, y = %d, wx = %d, wy = %d\n",
+					__func__, finger,
+					finger_status,
+					x, y, wx, wy);
+
+			touch_count++;
+		}
+	}
+
+	if (touch_count == 0) {
+		input_report_key(rmi4_data->input_dev,
+				BTN_TOUCH, 0);
+		input_report_key(rmi4_data->input_dev,
+				BTN_TOOL_FINGER, 0);
+#ifndef TYPE_B_PROTOCOL
+		input_mt_sync(rmi4_data->input_dev);
+#endif
+	}
+
+	input_sync(rmi4_data->input_dev);
+
+exit:
+	mutex_unlock(&(rmi4_data->rmi4_report_mutex));
+
+	return touch_count;
+}
+
+static int synaptics_rmi4_f12_abs_report(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler)
+{
+	int retval;
+	unsigned char touch_count = 0; /* number of touch points */
+	unsigned char index;
+	unsigned char finger;
+	unsigned char fingers_to_process;
+	unsigned char finger_status;
+	unsigned char size_of_2d_data;
+	unsigned char gesture_type;
+	unsigned short data_addr;
+	int x;
+	int y;
+	int wx;
+	int wy;
+	int temp;
+#if defined(REPORT_2D_PRESSURE) || defined(F51_DISCRETE_FORCE)
+	int pressure;
+#endif
+#ifdef REPORT_2D_PRESSURE
+	unsigned char f_fingers;
+	unsigned char f_lsb;
+	unsigned char f_msb;
+	unsigned char *f_data;
+#endif
+#ifdef F51_DISCRETE_FORCE
+	unsigned char force_level;
+#endif
+	struct synaptics_rmi4_f12_extra_data *extra_data;
+	struct synaptics_rmi4_f12_finger_data *data;
+	struct synaptics_rmi4_f12_finger_data *finger_data;
+	static unsigned char finger_presence;
+	static unsigned char stylus_presence;
+#ifdef F12_DATA_15_WORKAROUND
+	static unsigned char objects_already_present;
+#endif
+
+	fingers_to_process = fhandler->num_of_data_points;
+	data_addr = fhandler->full_addr.data_base;
+	extra_data = (struct synaptics_rmi4_f12_extra_data *)fhandler->extra;
+	size_of_2d_data = sizeof(struct synaptics_rmi4_f12_finger_data);
+
+	if (rmi4_data->suspend && rmi4_data->enable_wakeup_gesture) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				data_addr + extra_data->data4_offset,
+				rmi4_data->gesture_detection,
+				sizeof(rmi4_data->gesture_detection));
+		if (retval < 0)
+			return 0;
+
+		gesture_type = rmi4_data->gesture_detection[0];
+
+		if (gesture_type && gesture_type != F12_UDG_DETECT) {
+			input_report_key(rmi4_data->input_dev, KEY_WAKEUP, 1);
+			input_sync(rmi4_data->input_dev);
+			input_report_key(rmi4_data->input_dev, KEY_WAKEUP, 0);
+			input_sync(rmi4_data->input_dev);
+			/* synaptics_rmi4_wakeup_gesture(rmi4_data, false); */
+			/* rmi4_data->suspend = false; */
+		}
+
+		return 0;
+	}
+
+	/* Determine the total number of fingers to process */
+	if (extra_data->data15_size) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				data_addr + extra_data->data15_offset,
+				extra_data->data15_data,
+				extra_data->data15_size);
+		if (retval < 0)
+			return 0;
+
+		/* Start checking from the highest bit */
+		index = extra_data->data15_size - 1; /* Highest byte */
+		finger = (fingers_to_process - 1) % 8; /* Highest bit */
+		do {
+			if (extra_data->data15_data[index] & (1 << finger))
+				break;
+
+			if (finger) {
+				finger--;
+			} else if (index > 0) {
+				index--; /* Move to the next lower byte */
+				finger = 7;
+			}
+
+			fingers_to_process--;
+		} while (fingers_to_process);
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Number of fingers to process = %d\n",
+			__func__, fingers_to_process);
+	}
+
+#ifdef F12_DATA_15_WORKAROUND
+	fingers_to_process = max(fingers_to_process, objects_already_present);
+#endif
+
+	if (!fingers_to_process) {
+		synaptics_rmi4_free_fingers(rmi4_data);
+		finger_presence = 0;
+		stylus_presence = 0;
+		return 0;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			data_addr + extra_data->data1_offset,
+			(unsigned char *)fhandler->data,
+			fingers_to_process * size_of_2d_data);
+	if (retval < 0)
+		return 0;
+
+	data = (struct synaptics_rmi4_f12_finger_data *)fhandler->data;
+
+#ifdef REPORT_2D_PRESSURE
+	if (rmi4_data->report_pressure) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				data_addr + extra_data->data29_offset,
+				extra_data->data29_data,
+				extra_data->data29_size);
+		if (retval < 0)
+			return 0;
+	}
+#endif
+
+	mutex_lock(&(rmi4_data->rmi4_report_mutex));
+
+	for (finger = 0; finger < fingers_to_process; finger++) {
+		finger_data = data + finger;
+		finger_status = finger_data->object_type_and_status;
+
+#ifdef F12_DATA_15_WORKAROUND
+		objects_already_present = finger + 1;
+#endif
+
+		x = (finger_data->x_msb << 8) | (finger_data->x_lsb);
+		y = (finger_data->y_msb << 8) | (finger_data->y_lsb);
+#ifdef REPORT_2D_W
+		wx = finger_data->wx;
+		wy = finger_data->wy;
+#endif
+
+		if (rmi4_data->hw_if->board_data->swap_axes) {
+			temp = x;
+			x = y;
+			y = temp;
+			temp = wx;
+			wx = wy;
+			wy = temp;
+		}
+
+		if (rmi4_data->hw_if->board_data->x_flip)
+			x = rmi4_data->sensor_max_x - x;
+		if (rmi4_data->hw_if->board_data->y_flip)
+			y = rmi4_data->sensor_max_y - y;
+
+		switch (finger_status) {
+		case F12_FINGER_STATUS:
+		case F12_GLOVED_FINGER_STATUS:
+			/* Stylus has priority over fingers */
+			if (stylus_presence)
+				break;
+#ifdef TYPE_B_PROTOCOL
+			input_mt_slot(rmi4_data->input_dev, finger);
+			input_mt_report_slot_state(rmi4_data->input_dev,
+					MT_TOOL_FINGER, 1);
+#endif
+
+			input_report_key(rmi4_data->input_dev,
+					BTN_TOUCH, 1);
+			input_report_key(rmi4_data->input_dev,
+					BTN_TOOL_FINGER, 1);
+			input_report_abs(rmi4_data->input_dev,
+					ABS_MT_POSITION_X, x);
+			input_report_abs(rmi4_data->input_dev,
+					ABS_MT_POSITION_Y, y);
+#ifdef REPORT_2D_W
+			if (rmi4_data->wedge_sensor) {
+				input_report_abs(rmi4_data->input_dev,
+						ABS_MT_TOUCH_MAJOR, wx);
+				input_report_abs(rmi4_data->input_dev,
+						ABS_MT_TOUCH_MINOR, wx);
+			} else {
+				input_report_abs(rmi4_data->input_dev,
+						ABS_MT_TOUCH_MAJOR,
+						max(wx, wy));
+				input_report_abs(rmi4_data->input_dev,
+						ABS_MT_TOUCH_MINOR,
+						min(wx, wy));
+			}
+#endif
+#ifdef REPORT_2D_PRESSURE
+			if (rmi4_data->report_pressure) {
+				f_fingers = extra_data->data29_size / 2;
+				f_data = extra_data->data29_data;
+				if (finger + 1 > f_fingers) {
+					pressure = 1;
+				} else {
+					f_lsb = finger * 2;
+					f_msb = finger * 2 + 1;
+					pressure = (int)f_data[f_lsb] << 0 |
+							(int)f_data[f_msb] << 8;
+				}
+				pressure = pressure > 0 ? pressure : 1;
+				if (pressure > rmi4_data->force_max)
+					pressure = rmi4_data->force_max;
+				input_report_abs(rmi4_data->input_dev,
+						ABS_MT_PRESSURE, pressure);
+			}
+#elif defined(F51_DISCRETE_FORCE)
+			if (finger == 0) {
+				retval = synaptics_rmi4_reg_read(rmi4_data,
+						FORCE_LEVEL_ADDR,
+						&force_level,
+						sizeof(force_level));
+				if (retval < 0)
+					return 0;
+				pressure = force_level > 0 ? force_level : 1;
+			} else {
+				pressure = 1;
+			}
+			input_report_abs(rmi4_data->input_dev,
+					ABS_MT_PRESSURE, pressure);
+#endif
+#ifndef TYPE_B_PROTOCOL
+			input_mt_sync(rmi4_data->input_dev);
+#endif
+
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Finger %d: status = 0x%02x, x = %d, y = %d, wx = %d, wy = %d\n",
+					__func__, finger,
+					finger_status,
+					x, y, wx, wy);
+
+			finger_presence = 1;
+			touch_count++;
+			break;
+		case F12_PALM_STATUS:
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Finger %d: x = %d, y = %d, wx = %d, wy = %d\n",
+					__func__, finger,
+					x, y, wx, wy);
+			break;
+		case F12_STYLUS_STATUS:
+		case F12_ERASER_STATUS:
+			if (!rmi4_data->stylus_enable)
+				break;
+			/* Stylus has priority over fingers */
+			if (finger_presence) {
+				mutex_unlock(&(rmi4_data->rmi4_report_mutex));
+				synaptics_rmi4_free_fingers(rmi4_data);
+				mutex_lock(&(rmi4_data->rmi4_report_mutex));
+				finger_presence = 0;
+			}
+			if (stylus_presence) {/* Allow one stylus at a timee */
+				if (finger + 1 != stylus_presence)
+					break;
+			}
+			input_report_key(rmi4_data->stylus_dev,
+					BTN_TOUCH, 1);
+			if (finger_status == F12_STYLUS_STATUS) {
+				input_report_key(rmi4_data->stylus_dev,
+						BTN_TOOL_PEN, 1);
+			} else {
+				input_report_key(rmi4_data->stylus_dev,
+						BTN_TOOL_RUBBER, 1);
+			}
+			input_report_abs(rmi4_data->stylus_dev,
+					ABS_X, x);
+			input_report_abs(rmi4_data->stylus_dev,
+					ABS_Y, y);
+			input_sync(rmi4_data->stylus_dev);
+
+			stylus_presence = finger + 1;
+			touch_count++;
+			break;
+		default:
+#ifdef TYPE_B_PROTOCOL
+			input_mt_slot(rmi4_data->input_dev, finger);
+			input_mt_report_slot_state(rmi4_data->input_dev,
+					MT_TOOL_FINGER, 0);
+#endif
+			break;
+		}
+	}
+
+	if (touch_count == 0) {
+		finger_presence = 0;
+#ifdef F12_DATA_15_WORKAROUND
+		objects_already_present = 0;
+#endif
+		input_report_key(rmi4_data->input_dev,
+				BTN_TOUCH, 0);
+		input_report_key(rmi4_data->input_dev,
+				BTN_TOOL_FINGER, 0);
+#ifndef TYPE_B_PROTOCOL
+		input_mt_sync(rmi4_data->input_dev);
+#endif
+
+		if (rmi4_data->stylus_enable) {
+			stylus_presence = 0;
+			input_report_key(rmi4_data->stylus_dev,
+					BTN_TOUCH, 0);
+			input_report_key(rmi4_data->stylus_dev,
+					BTN_TOOL_PEN, 0);
+			if (rmi4_data->eraser_enable) {
+				input_report_key(rmi4_data->stylus_dev,
+						BTN_TOOL_RUBBER, 0);
+			}
+			input_sync(rmi4_data->stylus_dev);
+		}
+	}
+
+	input_sync(rmi4_data->input_dev);
+
+	mutex_unlock(&(rmi4_data->rmi4_report_mutex));
+
+	return touch_count;
+}
+
+static int synaptics_rmi4_f1a_report(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler)
+{
+	int retval;
+	unsigned char touch_count = 0;
+	unsigned char button;
+	unsigned char index;
+	unsigned char shift;
+	unsigned char status;
+	unsigned char *data;
+	unsigned short data_addr = fhandler->full_addr.data_base;
+	struct synaptics_rmi4_f1a_handle *f1a = fhandler->data;
+	static unsigned char do_once = 1;
+	static bool current_status[MAX_NUMBER_OF_BUTTONS];
+#ifdef NO_0D_WHILE_2D
+	static bool before_2d_status[MAX_NUMBER_OF_BUTTONS];
+	static bool while_2d_status[MAX_NUMBER_OF_BUTTONS];
+#endif
+
+	if (do_once) {
+		memset(current_status, 0, sizeof(current_status));
+#ifdef NO_0D_WHILE_2D
+		memset(before_2d_status, 0, sizeof(before_2d_status));
+		memset(while_2d_status, 0, sizeof(while_2d_status));
+#endif
+		do_once = 0;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			data_addr,
+			f1a->button_data_buffer,
+			f1a->button_bitmask_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read button data registers\n",
+				__func__);
+		return retval;
+	}
+
+	data = f1a->button_data_buffer;
+
+	mutex_lock(&(rmi4_data->rmi4_report_mutex));
+
+	for (button = 0; button < f1a->valid_button_count; button++) {
+		index = button / 8;
+		shift = button % 8;
+		status = ((data[index] >> shift) & MASK_1BIT);
+
+		if (current_status[button] == status)
+			continue;
+		else
+			current_status[button] = status;
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Button %d (code %d) ->%d\n",
+				__func__, button,
+				f1a->button_map[button],
+				status);
+#ifdef NO_0D_WHILE_2D
+		if (rmi4_data->fingers_on_2d == false) {
+			if (status == 1) {
+				before_2d_status[button] = 1;
+			} else {
+				if (while_2d_status[button] == 1) {
+					while_2d_status[button] = 0;
+					continue;
+				} else {
+					before_2d_status[button] = 0;
+				}
+			}
+			touch_count++;
+			input_report_key(rmi4_data->input_dev,
+					f1a->button_map[button],
+					status);
+		} else {
+			if (before_2d_status[button] == 1) {
+				before_2d_status[button] = 0;
+				touch_count++;
+				input_report_key(rmi4_data->input_dev,
+						f1a->button_map[button],
+						status);
+			} else {
+				if (status == 1)
+					while_2d_status[button] = 1;
+				else
+					while_2d_status[button] = 0;
+			}
+		}
+#else
+		touch_count++;
+		input_report_key(rmi4_data->input_dev,
+				f1a->button_map[button],
+				status);
+#endif
+	}
+
+	if (touch_count)
+		input_sync(rmi4_data->input_dev);
+
+	mutex_unlock(&(rmi4_data->rmi4_report_mutex));
+
+	return retval;
+}
+
+static void synaptics_rmi4_report_touch(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler)
+{
+	unsigned char touch_count_2d;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Function %02x reporting\n",
+			__func__, fhandler->fn_number);
+
+	switch (fhandler->fn_number) {
+	case SYNAPTICS_RMI4_F11:
+		touch_count_2d = synaptics_rmi4_f11_abs_report(rmi4_data,
+				fhandler);
+
+		if (touch_count_2d)
+			rmi4_data->fingers_on_2d = true;
+		else
+			rmi4_data->fingers_on_2d = false;
+		break;
+	case SYNAPTICS_RMI4_F12:
+		touch_count_2d = synaptics_rmi4_f12_abs_report(rmi4_data,
+				fhandler);
+
+		if (touch_count_2d)
+			rmi4_data->fingers_on_2d = true;
+		else
+			rmi4_data->fingers_on_2d = false;
+		break;
+	case SYNAPTICS_RMI4_F1A:
+		synaptics_rmi4_f1a_report(rmi4_data, fhandler);
+		break;
+#ifdef USE_DATA_SERVER
+	case SYNAPTICS_RMI4_F21:
+		if (synad_pid)
+			send_sig_info(SIGIO, &interrupt_signal, synad_task);
+		break;
+#endif
+	default:
+		break;
+	}
+}
+
+static int synaptics_rmi4_sensor_report(struct synaptics_rmi4_data *rmi4_data,
+		bool report)
+{
+	int retval;
+	unsigned char data[MAX_INTR_REGISTERS + 1];
+	unsigned char *intr = &data[1];
+	bool was_in_bl_mode;
+	struct synaptics_rmi4_f01_device_status status;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	/*
+	 * Get interrupt status information from F01 Data1 register to
+	 * determine the source(s) that are flagging the interrupt.
+	 */
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_data_base_addr,
+			data,
+			rmi4_data->num_of_intr_regs + 1);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read interrupt status\n",
+				__func__);
+		return retval;
+	}
+
+	status.data[0] = data[0];
+	if (status.status_code == STATUS_CRC_IN_PROGRESS) {
+		retval = synaptics_rmi4_check_status(rmi4_data,
+				&was_in_bl_mode);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to check status\n",
+					__func__);
+			return retval;
+		}
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				rmi4_data->f01_data_base_addr,
+				status.data,
+				sizeof(status.data));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read device status\n",
+					__func__);
+			return retval;
+		}
+	}
+	if (status.unconfigured && !status.flash_prog) {
+		pr_notice("%s: spontaneous reset detected\n", __func__);
+		retval = synaptics_rmi4_reinit_device(rmi4_data);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to reinit device\n",
+					__func__);
+		}
+	}
+
+	if (!report)
+		return retval;
+
+	/*
+	 * Traverse the function handler list and service the source(s)
+	 * of the interrupt accordingly.
+	 */
+	if (!list_empty(&rmi->support_fn_list)) {
+		list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+			if (fhandler->num_of_data_sources) {
+				if (fhandler->intr_mask &
+						intr[fhandler->intr_reg_num]) {
+					synaptics_rmi4_report_touch(rmi4_data,
+							fhandler);
+				}
+			}
+		}
+	}
+
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link) {
+			if (!exp_fhandler->insert &&
+					!exp_fhandler->remove &&
+					(exp_fhandler->exp_fn->attn != NULL))
+				exp_fhandler->exp_fn->attn(rmi4_data, intr[0]);
+		}
+	}
+	mutex_unlock(&exp_data.mutex);
+
+	return retval;
+}
+
+static irqreturn_t synaptics_rmi4_irq(int irq, void *data)
+{
+	struct synaptics_rmi4_data *rmi4_data = data;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	if (gpio_get_value(bdata->irq_gpio) != bdata->irq_on_state)
+		goto exit;
+
+	synaptics_rmi4_sensor_report(rmi4_data, true);
+
+exit:
+	return IRQ_HANDLED;
+}
+
+static int synaptics_rmi4_int_enable(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	int retval = 0;
+	unsigned char ii;
+	unsigned char zero = 0x00;
+	unsigned char *intr_mask;
+	unsigned short intr_addr;
+
+	intr_mask = rmi4_data->intr_mask;
+
+	for (ii = 0; ii < rmi4_data->num_of_intr_regs; ii++) {
+		if (intr_mask[ii] != 0x00) {
+			intr_addr = rmi4_data->f01_ctrl_base_addr + 1 + ii;
+			if (enable) {
+				retval = synaptics_rmi4_reg_write(rmi4_data,
+						intr_addr,
+						&(intr_mask[ii]),
+						sizeof(intr_mask[ii]));
+				if (retval < 0)
+					return retval;
+			} else {
+				retval = synaptics_rmi4_reg_write(rmi4_data,
+						intr_addr,
+						&zero,
+						sizeof(zero));
+				if (retval < 0)
+					return retval;
+			}
+		}
+	}
+
+	return retval;
+}
+
+static int synaptics_rmi4_irq_enable(struct synaptics_rmi4_data *rmi4_data,
+		bool enable, bool attn_only)
+{
+	int retval = 0;
+	unsigned char data[MAX_INTR_REGISTERS];
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	mutex_lock(&(rmi4_data->rmi4_irq_enable_mutex));
+
+	if (attn_only) {
+		retval = synaptics_rmi4_int_enable(rmi4_data, enable);
+		goto exit;
+	}
+
+	if (enable) {
+		if (rmi4_data->irq_enabled) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Interrupt already enabled\n",
+					__func__);
+			goto exit;
+		}
+
+		retval = synaptics_rmi4_int_enable(rmi4_data, false);
+		if (retval < 0)
+			goto exit;
+
+		/* Clear interrupts */
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				rmi4_data->f01_data_base_addr + 1,
+				data,
+				rmi4_data->num_of_intr_regs);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read interrupt status\n",
+					__func__);
+			goto exit;
+		}
+
+		retval = request_threaded_irq(rmi4_data->irq, NULL,
+				synaptics_rmi4_irq, bdata->irq_flags,
+				PLATFORM_DRIVER_NAME, rmi4_data);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create irq thread\n",
+					__func__);
+			goto exit;
+		}
+
+		retval = synaptics_rmi4_int_enable(rmi4_data, true);
+		if (retval < 0)
+			goto exit;
+
+		rmi4_data->irq_enabled = true;
+	} else {
+		if (rmi4_data->irq_enabled) {
+			disable_irq(rmi4_data->irq);
+			free_irq(rmi4_data->irq, rmi4_data);
+			rmi4_data->irq_enabled = false;
+		}
+	}
+
+exit:
+	mutex_unlock(&(rmi4_data->rmi4_irq_enable_mutex));
+
+	return retval;
+}
+
+static void synaptics_rmi4_set_intr_mask(struct synaptics_rmi4_fn *fhandler,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned int intr_count)
+{
+	unsigned char ii;
+	unsigned char intr_offset;
+
+	fhandler->intr_reg_num = (intr_count + 7) / 8;
+	if (fhandler->intr_reg_num != 0)
+		fhandler->intr_reg_num -= 1;
+
+	/* Set an enable bit for each data source */
+	intr_offset = intr_count % 8;
+	fhandler->intr_mask = 0;
+	for (ii = intr_offset;
+			ii < (fd->intr_src_count + intr_offset);
+			ii++)
+		fhandler->intr_mask |= 1 << ii;
+}
+
+static int synaptics_rmi4_f01_init(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned int intr_count)
+{
+	fhandler->fn_number = fd->fn_number;
+	fhandler->num_of_data_sources = fd->intr_src_count;
+	fhandler->data = NULL;
+	fhandler->extra = NULL;
+
+	synaptics_rmi4_set_intr_mask(fhandler, fd, intr_count);
+
+	rmi4_data->f01_query_base_addr = fd->query_base_addr;
+	rmi4_data->f01_ctrl_base_addr = fd->ctrl_base_addr;
+	rmi4_data->f01_data_base_addr = fd->data_base_addr;
+	rmi4_data->f01_cmd_base_addr = fd->cmd_base_addr;
+
+	return 0;
+}
+
+static int synaptics_rmi4_f11_init(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned int intr_count)
+{
+	int retval;
+	int temp;
+	unsigned char offset;
+	unsigned char fingers_supported;
+	struct synaptics_rmi4_f11_extra_data *extra_data;
+	struct synaptics_rmi4_f11_query_0_5 query_0_5;
+	struct synaptics_rmi4_f11_query_7_8 query_7_8;
+	struct synaptics_rmi4_f11_query_9 query_9;
+	struct synaptics_rmi4_f11_query_12 query_12;
+	struct synaptics_rmi4_f11_query_27 query_27;
+	struct synaptics_rmi4_f11_ctrl_6_9 control_6_9;
+	const struct synaptics_dsx_board_data *bdata =
+				rmi4_data->hw_if->board_data;
+
+	fhandler->fn_number = fd->fn_number;
+	fhandler->num_of_data_sources = fd->intr_src_count;
+	fhandler->extra = kmalloc(sizeof(*extra_data), GFP_KERNEL);
+	if (!fhandler->extra) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for fhandler->extra\n",
+				__func__);
+		return -ENOMEM;
+	}
+	extra_data = (struct synaptics_rmi4_f11_extra_data *)fhandler->extra;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.query_base,
+			query_0_5.data,
+			sizeof(query_0_5.data));
+	if (retval < 0)
+		return retval;
+
+	/* Maximum number of fingers supported */
+	if (query_0_5.num_of_fingers <= 4)
+		fhandler->num_of_data_points = query_0_5.num_of_fingers + 1;
+	else if (query_0_5.num_of_fingers == 5)
+		fhandler->num_of_data_points = 10;
+
+	rmi4_data->num_of_fingers = fhandler->num_of_data_points;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.ctrl_base + 6,
+			control_6_9.data,
+			sizeof(control_6_9.data));
+	if (retval < 0)
+		return retval;
+
+	/* Maximum x and y */
+	rmi4_data->sensor_max_x = control_6_9.sensor_max_x_pos_7_0 |
+			(control_6_9.sensor_max_x_pos_11_8 << 8);
+	rmi4_data->sensor_max_y = control_6_9.sensor_max_y_pos_7_0 |
+			(control_6_9.sensor_max_y_pos_11_8 << 8);
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Function %02x max x = %d max y = %d\n",
+			__func__, fhandler->fn_number,
+			rmi4_data->sensor_max_x,
+			rmi4_data->sensor_max_y);
+
+	rmi4_data->max_touch_width = MAX_F11_TOUCH_WIDTH;
+
+	if (bdata->swap_axes) {
+		temp = rmi4_data->sensor_max_x;
+		rmi4_data->sensor_max_x = rmi4_data->sensor_max_y;
+		rmi4_data->sensor_max_y = temp;
+	}
+
+	synaptics_rmi4_set_intr_mask(fhandler, fd, intr_count);
+
+	fhandler->data = NULL;
+
+	offset = sizeof(query_0_5.data);
+
+	/* query 6 */
+	if (query_0_5.has_rel)
+		offset += 1;
+
+	/* queries 7 8 */
+	if (query_0_5.has_gestures) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.query_base + offset,
+				query_7_8.data,
+				sizeof(query_7_8.data));
+		if (retval < 0)
+			return retval;
+
+		offset += sizeof(query_7_8.data);
+	}
+
+	/* query 9 */
+	if (query_0_5.has_query_9) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.query_base + offset,
+				query_9.data,
+				sizeof(query_9.data));
+		if (retval < 0)
+			return retval;
+
+		offset += sizeof(query_9.data);
+	}
+
+	/* query 10 */
+	if (query_0_5.has_gestures && query_7_8.has_touch_shapes)
+		offset += 1;
+
+	/* query 11 */
+	if (query_0_5.has_query_11)
+		offset += 1;
+
+	/* query 12 */
+	if (query_0_5.has_query_12) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.query_base + offset,
+				query_12.data,
+				sizeof(query_12.data));
+		if (retval < 0)
+			return retval;
+
+		offset += sizeof(query_12.data);
+	}
+
+	/* query 13 */
+	if (query_0_5.has_jitter_filter)
+		offset += 1;
+
+	/* query 14 */
+	if (query_0_5.has_query_12 && query_12.has_general_information_2)
+		offset += 1;
+
+	/* queries 15 16 17 18 19 20 21 22 23 24 25 26*/
+	if (query_0_5.has_query_12 && query_12.has_physical_properties)
+		offset += 12;
+
+	/* query 27 */
+	if (query_0_5.has_query_27) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.query_base + offset,
+				query_27.data,
+				sizeof(query_27.data));
+		if (retval < 0)
+			return retval;
+
+		rmi4_data->f11_wakeup_gesture = query_27.has_wakeup_gesture;
+	}
+
+	if (!rmi4_data->f11_wakeup_gesture)
+		return retval;
+
+	/* data 0 */
+	fingers_supported = fhandler->num_of_data_points;
+	offset = (fingers_supported + 3) / 4;
+
+	/* data 1 2 3 4 5 */
+	offset += 5 * fingers_supported;
+
+	/* data 6 7 */
+	if (query_0_5.has_rel)
+		offset += 2 * fingers_supported;
+
+	/* data 8 */
+	if (query_0_5.has_gestures && query_7_8.data[0])
+		offset += 1;
+
+	/* data 9 */
+	if (query_0_5.has_gestures && (query_7_8.data[0] || query_7_8.data[1]))
+		offset += 1;
+
+	/* data 10 */
+	if (query_0_5.has_gestures &&
+			(query_7_8.has_pinch || query_7_8.has_flick))
+		offset += 1;
+
+	/* data 11 12 */
+	if (query_0_5.has_gestures &&
+			(query_7_8.has_flick || query_7_8.has_rotate))
+		offset += 2;
+
+	/* data 13 */
+	if (query_0_5.has_gestures && query_7_8.has_touch_shapes)
+		offset += (fingers_supported + 3) / 4;
+
+	/* data 14 15 */
+	if (query_0_5.has_gestures &&
+			(query_7_8.has_scroll_zones ||
+			query_7_8.has_multi_finger_scroll ||
+			query_7_8.has_chiral_scroll))
+		offset += 2;
+
+	/* data 16 17 */
+	if (query_0_5.has_gestures &&
+			(query_7_8.has_scroll_zones &&
+			query_7_8.individual_scroll_zones))
+		offset += 2;
+
+	/* data 18 19 20 21 22 23 24 25 26 27 */
+	if (query_0_5.has_query_9 && query_9.has_contact_geometry)
+		offset += 10 * fingers_supported;
+
+	/* data 28 */
+	if (query_0_5.has_bending_correction ||
+			query_0_5.has_large_object_suppression)
+		offset += 1;
+
+	/* data 29 30 31 */
+	if (query_0_5.has_query_9 && query_9.has_pen_hover_discrimination)
+		offset += 3;
+
+	/* data 32 */
+	if (query_0_5.has_query_12 &&
+			query_12.has_small_object_detection_tuning)
+		offset += 1;
+
+	/* data 33 34 */
+	if (query_0_5.has_query_27 && query_27.f11_query27_b0)
+		offset += 2;
+
+	/* data 35 */
+	if (query_0_5.has_query_12 && query_12.has_8bit_w)
+		offset += fingers_supported;
+
+	/* data 36 */
+	if (query_0_5.has_bending_correction)
+		offset += 1;
+
+	/* data 37 */
+	if (query_0_5.has_query_27 && query_27.has_data_37)
+		offset += 1;
+
+	/* data 38 */
+	if (query_0_5.has_query_27 && query_27.has_wakeup_gesture)
+		extra_data->data38_offset = offset;
+
+	return retval;
+}
+
+static int synaptics_rmi4_f12_set_enables(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short ctrl28)
+{
+	int retval;
+	static unsigned short ctrl_28_address;
+
+	if (ctrl28)
+		ctrl_28_address = ctrl28;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			ctrl_28_address,
+			&rmi4_data->report_enable,
+			sizeof(rmi4_data->report_enable));
+	if (retval < 0)
+		return retval;
+
+	return retval;
+}
+
+static int synaptics_rmi4_f12_find_sub(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler,
+		unsigned char *presence, unsigned char presence_size,
+		unsigned char structure_offset, unsigned char reg,
+		unsigned char sub)
+{
+	int retval;
+	unsigned char cnt;
+	unsigned char regnum;
+	unsigned char bitnum;
+	unsigned char p_index;
+	unsigned char s_index;
+	unsigned char offset;
+	unsigned char max_reg;
+	unsigned char *structure;
+
+	max_reg = (presence_size - 1) * 8 - 1;
+
+	if (reg > max_reg) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Register number (%d) over limit\n",
+				__func__, reg);
+		return -EINVAL;
+	}
+
+	p_index = reg / 8 + 1;
+	bitnum = reg % 8;
+	if ((presence[p_index] & (1 << bitnum)) == 0x00) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Register %d is not present\n",
+				__func__, reg);
+		return -EINVAL;
+	}
+
+	structure = kmalloc(presence[0], GFP_KERNEL);
+	if (!structure) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for structure register\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.query_base + structure_offset,
+			structure,
+			presence[0]);
+	if (retval < 0)
+		goto exit;
+
+	s_index = 0;
+
+	for (regnum = 0; regnum < reg; regnum++) {
+		p_index = regnum / 8 + 1;
+		bitnum = regnum % 8;
+		if ((presence[p_index] & (1 << bitnum)) == 0x00)
+			continue;
+
+		if (structure[s_index] == 0x00)
+			s_index += 3;
+		else
+			s_index++;
+
+		while (structure[s_index] & ~MASK_7BIT)
+			s_index++;
+
+		s_index++;
+	}
+
+	cnt = 0;
+	s_index++;
+	offset = sub / 7;
+	bitnum = sub % 7;
+
+	do {
+		if (cnt == offset) {
+			if (structure[s_index + cnt] & (1 << bitnum))
+				retval = 1;
+			else
+				retval = 0;
+			goto exit;
+		}
+		cnt++;
+	} while (structure[s_index + cnt - 1] & ~MASK_7BIT);
+
+	retval = 0;
+
+exit:
+	kfree(structure);
+
+	return retval;
+}
+
+static int synaptics_rmi4_f12_init(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned int intr_count)
+{
+	int retval = 0;
+	int temp;
+	unsigned char subpacket;
+	unsigned char ctrl_23_size;
+	unsigned char size_of_2d_data;
+	unsigned char size_of_query5;
+	unsigned char size_of_query8;
+	unsigned char ctrl_8_offset;
+	unsigned char ctrl_20_offset;
+	unsigned char ctrl_23_offset;
+	unsigned char ctrl_28_offset;
+	unsigned char ctrl_31_offset;
+	unsigned char ctrl_58_offset;
+	unsigned char num_of_fingers;
+	struct synaptics_rmi4_f12_extra_data *extra_data;
+	struct synaptics_rmi4_f12_query_5 *query_5 = NULL;
+	struct synaptics_rmi4_f12_query_8 *query_8 = NULL;
+	struct synaptics_rmi4_f12_ctrl_8 *ctrl_8 = NULL;
+	struct synaptics_rmi4_f12_ctrl_23 *ctrl_23 = NULL;
+	struct synaptics_rmi4_f12_ctrl_31 *ctrl_31 = NULL;
+	struct synaptics_rmi4_f12_ctrl_58 *ctrl_58 = NULL;
+	const struct synaptics_dsx_board_data *bdata =
+				rmi4_data->hw_if->board_data;
+
+	fhandler->fn_number = fd->fn_number;
+	fhandler->num_of_data_sources = fd->intr_src_count;
+	fhandler->extra = kmalloc(sizeof(*extra_data), GFP_KERNEL);
+	if (!fhandler->extra) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for fhandler->extra\n",
+				__func__);
+		return -ENOMEM;
+	}
+	extra_data = (struct synaptics_rmi4_f12_extra_data *)fhandler->extra;
+	size_of_2d_data = sizeof(struct synaptics_rmi4_f12_finger_data);
+
+	query_5 = kzalloc(sizeof(*query_5), GFP_KERNEL);
+	if (!query_5) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for query_5\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	query_8 = kzalloc(sizeof(*query_8), GFP_KERNEL);
+	if (!query_8) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for query_8\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	ctrl_8 = kzalloc(sizeof(*ctrl_8), GFP_KERNEL);
+	if (!ctrl_8) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for ctrl_8\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	ctrl_23 = kzalloc(sizeof(*ctrl_23), GFP_KERNEL);
+	if (!ctrl_23) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for ctrl_23\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	ctrl_31 = kzalloc(sizeof(*ctrl_31), GFP_KERNEL);
+	if (!ctrl_31) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for ctrl_31\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	ctrl_58 = kzalloc(sizeof(*ctrl_58), GFP_KERNEL);
+	if (!ctrl_58) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for ctrl_58\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.query_base + 4,
+			&size_of_query5,
+			sizeof(size_of_query5));
+	if (retval < 0)
+		goto exit;
+
+	if (size_of_query5 > sizeof(query_5->data))
+		size_of_query5 = sizeof(query_5->data);
+	memset(query_5->data, 0x00, sizeof(query_5->data));
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.query_base + 5,
+			query_5->data,
+			size_of_query5);
+	if (retval < 0)
+		goto exit;
+
+	ctrl_8_offset = query_5->ctrl0_is_present +
+			query_5->ctrl1_is_present +
+			query_5->ctrl2_is_present +
+			query_5->ctrl3_is_present +
+			query_5->ctrl4_is_present +
+			query_5->ctrl5_is_present +
+			query_5->ctrl6_is_present +
+			query_5->ctrl7_is_present;
+
+	ctrl_20_offset = ctrl_8_offset +
+			query_5->ctrl8_is_present +
+			query_5->ctrl9_is_present +
+			query_5->ctrl10_is_present +
+			query_5->ctrl11_is_present +
+			query_5->ctrl12_is_present +
+			query_5->ctrl13_is_present +
+			query_5->ctrl14_is_present +
+			query_5->ctrl15_is_present +
+			query_5->ctrl16_is_present +
+			query_5->ctrl17_is_present +
+			query_5->ctrl18_is_present +
+			query_5->ctrl19_is_present;
+
+	ctrl_23_offset = ctrl_20_offset +
+			query_5->ctrl20_is_present +
+			query_5->ctrl21_is_present +
+			query_5->ctrl22_is_present;
+
+	ctrl_28_offset = ctrl_23_offset +
+			query_5->ctrl23_is_present +
+			query_5->ctrl24_is_present +
+			query_5->ctrl25_is_present +
+			query_5->ctrl26_is_present +
+			query_5->ctrl27_is_present;
+
+	ctrl_31_offset = ctrl_28_offset +
+			query_5->ctrl28_is_present +
+			query_5->ctrl29_is_present +
+			query_5->ctrl30_is_present;
+
+	ctrl_58_offset = ctrl_31_offset +
+			query_5->ctrl31_is_present +
+			query_5->ctrl32_is_present +
+			query_5->ctrl33_is_present +
+			query_5->ctrl34_is_present +
+			query_5->ctrl35_is_present +
+			query_5->ctrl36_is_present +
+			query_5->ctrl37_is_present +
+			query_5->ctrl38_is_present +
+			query_5->ctrl39_is_present +
+			query_5->ctrl40_is_present +
+			query_5->ctrl41_is_present +
+			query_5->ctrl42_is_present +
+			query_5->ctrl43_is_present +
+			query_5->ctrl44_is_present +
+			query_5->ctrl45_is_present +
+			query_5->ctrl46_is_present +
+			query_5->ctrl47_is_present +
+			query_5->ctrl48_is_present +
+			query_5->ctrl49_is_present +
+			query_5->ctrl50_is_present +
+			query_5->ctrl51_is_present +
+			query_5->ctrl52_is_present +
+			query_5->ctrl53_is_present +
+			query_5->ctrl54_is_present +
+			query_5->ctrl55_is_present +
+			query_5->ctrl56_is_present +
+			query_5->ctrl57_is_present;
+
+	ctrl_23_size = 2;
+	for (subpacket = 2; subpacket <= 4; subpacket++) {
+		retval = synaptics_rmi4_f12_find_sub(rmi4_data,
+				fhandler, query_5->data, sizeof(query_5->data),
+				6, 23, subpacket);
+		if (retval == 1)
+			ctrl_23_size++;
+		else if (retval < 0)
+			goto exit;
+
+	}
+
+	retval = synaptics_rmi4_f12_find_sub(rmi4_data,
+			fhandler, query_5->data, sizeof(query_5->data),
+			6, 20, 0);
+	if (retval == 1)
+		rmi4_data->set_wakeup_gesture = 2;
+	else if (retval == 0)
+		rmi4_data->set_wakeup_gesture = 0;
+	else if (retval < 0)
+		goto exit;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.ctrl_base + ctrl_23_offset,
+			ctrl_23->data,
+			ctrl_23_size);
+	if (retval < 0)
+		goto exit;
+
+	/* Maximum number of fingers supported */
+	fhandler->num_of_data_points = min_t(unsigned char,
+			ctrl_23->max_reported_objects,
+			(unsigned char)F12_FINGERS_TO_SUPPORT);
+
+	num_of_fingers = fhandler->num_of_data_points;
+	rmi4_data->num_of_fingers = num_of_fingers;
+
+	rmi4_data->stylus_enable = ctrl_23->stylus_enable;
+	rmi4_data->eraser_enable = ctrl_23->eraser_enable;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.query_base + 7,
+			&size_of_query8,
+			sizeof(size_of_query8));
+	if (retval < 0)
+		goto exit;
+
+	if (size_of_query8 > sizeof(query_8->data))
+		size_of_query8 = sizeof(query_8->data);
+	memset(query_8->data, 0x00, sizeof(query_8->data));
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.query_base + 8,
+			query_8->data,
+			size_of_query8);
+	if (retval < 0)
+		goto exit;
+
+	/* Determine the presence of the Data0 register */
+	extra_data->data1_offset = query_8->data0_is_present;
+
+	if ((size_of_query8 >= 3) && (query_8->data15_is_present)) {
+		extra_data->data15_offset = query_8->data0_is_present +
+				query_8->data1_is_present +
+				query_8->data2_is_present +
+				query_8->data3_is_present +
+				query_8->data4_is_present +
+				query_8->data5_is_present +
+				query_8->data6_is_present +
+				query_8->data7_is_present +
+				query_8->data8_is_present +
+				query_8->data9_is_present +
+				query_8->data10_is_present +
+				query_8->data11_is_present +
+				query_8->data12_is_present +
+				query_8->data13_is_present +
+				query_8->data14_is_present;
+		extra_data->data15_size = (num_of_fingers + 7) / 8;
+	} else {
+		extra_data->data15_size = 0;
+	}
+
+#ifdef REPORT_2D_PRESSURE
+	if ((size_of_query8 >= 5) && (query_8->data29_is_present)) {
+		extra_data->data29_offset = query_8->data0_is_present +
+				query_8->data1_is_present +
+				query_8->data2_is_present +
+				query_8->data3_is_present +
+				query_8->data4_is_present +
+				query_8->data5_is_present +
+				query_8->data6_is_present +
+				query_8->data7_is_present +
+				query_8->data8_is_present +
+				query_8->data9_is_present +
+				query_8->data10_is_present +
+				query_8->data11_is_present +
+				query_8->data12_is_present +
+				query_8->data13_is_present +
+				query_8->data14_is_present +
+				query_8->data15_is_present +
+				query_8->data16_is_present +
+				query_8->data17_is_present +
+				query_8->data18_is_present +
+				query_8->data19_is_present +
+				query_8->data20_is_present +
+				query_8->data21_is_present +
+				query_8->data22_is_present +
+				query_8->data23_is_present +
+				query_8->data24_is_present +
+				query_8->data25_is_present +
+				query_8->data26_is_present +
+				query_8->data27_is_present +
+				query_8->data28_is_present;
+		extra_data->data29_size = 0;
+		for (subpacket = 0; subpacket <= num_of_fingers; subpacket++) {
+			retval = synaptics_rmi4_f12_find_sub(rmi4_data,
+					fhandler, query_8->data,
+					sizeof(query_8->data),
+					9, 29, subpacket);
+			if (retval == 1)
+				extra_data->data29_size += 2;
+			else if (retval < 0)
+				goto exit;
+		}
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.ctrl_base + ctrl_58_offset,
+				ctrl_58->data,
+				sizeof(ctrl_58->data));
+		if (retval < 0)
+			goto exit;
+		rmi4_data->force_min =
+				(int)(ctrl_58->min_force_lsb << 0) |
+				(int)(ctrl_58->min_force_msb << 8);
+		rmi4_data->force_max =
+				(int)(ctrl_58->max_force_lsb << 0) |
+				(int)(ctrl_58->max_force_msb << 8);
+		rmi4_data->report_pressure = true;
+	} else {
+		extra_data->data29_size = 0;
+		rmi4_data->report_pressure = false;
+	}
+#endif
+
+	rmi4_data->report_enable = RPT_DEFAULT;
+#ifdef REPORT_2D_Z
+	rmi4_data->report_enable |= RPT_Z;
+#endif
+#ifdef REPORT_2D_W
+	rmi4_data->report_enable |= (RPT_WX | RPT_WY);
+#endif
+
+	retval = synaptics_rmi4_f12_set_enables(rmi4_data,
+			fhandler->full_addr.ctrl_base + ctrl_28_offset);
+	if (retval < 0)
+		goto exit;
+
+	if (query_5->ctrl8_is_present) {
+		rmi4_data->wedge_sensor = false;
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.ctrl_base + ctrl_8_offset,
+				ctrl_8->data,
+				sizeof(ctrl_8->data));
+		if (retval < 0)
+			goto exit;
+
+		/* Maximum x and y */
+		rmi4_data->sensor_max_x =
+				((unsigned int)ctrl_8->max_x_coord_lsb << 0) |
+				((unsigned int)ctrl_8->max_x_coord_msb << 8);
+		rmi4_data->sensor_max_y =
+				((unsigned int)ctrl_8->max_y_coord_lsb << 0) |
+				((unsigned int)ctrl_8->max_y_coord_msb << 8);
+
+		rmi4_data->max_touch_width = MAX_F12_TOUCH_WIDTH;
+	} else {
+		rmi4_data->wedge_sensor = true;
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.ctrl_base + ctrl_31_offset,
+				ctrl_31->data,
+				sizeof(ctrl_31->data));
+		if (retval < 0)
+			goto exit;
+
+		/* Maximum x and y */
+		rmi4_data->sensor_max_x =
+				((unsigned int)ctrl_31->max_x_coord_lsb << 0) |
+				((unsigned int)ctrl_31->max_x_coord_msb << 8);
+		rmi4_data->sensor_max_y =
+				((unsigned int)ctrl_31->max_y_coord_lsb << 0) |
+				((unsigned int)ctrl_31->max_y_coord_msb << 8);
+
+		rmi4_data->max_touch_width = MAX_F12_TOUCH_WIDTH;
+	}
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Function %02x max x = %d max y = %d\n",
+			__func__, fhandler->fn_number,
+			rmi4_data->sensor_max_x,
+			rmi4_data->sensor_max_y);
+
+	if (bdata->swap_axes) {
+		temp = rmi4_data->sensor_max_x;
+		rmi4_data->sensor_max_x = rmi4_data->sensor_max_y;
+		rmi4_data->sensor_max_y = temp;
+	}
+
+	rmi4_data->f12_wakeup_gesture = query_5->ctrl27_is_present;
+	if (rmi4_data->f12_wakeup_gesture) {
+		extra_data->ctrl20_offset = ctrl_20_offset;
+		extra_data->data4_offset = query_8->data0_is_present +
+				query_8->data1_is_present +
+				query_8->data2_is_present +
+				query_8->data3_is_present;
+	}
+
+	synaptics_rmi4_set_intr_mask(fhandler, fd, intr_count);
+
+	/* Allocate memory for finger data storage space */
+	fhandler->data_size = num_of_fingers * size_of_2d_data;
+	fhandler->data = kmalloc(fhandler->data_size, GFP_KERNEL);
+	if (!fhandler->data) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for fhandler->data\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+exit:
+	kfree(query_5);
+	kfree(query_8);
+	kfree(ctrl_8);
+	kfree(ctrl_23);
+	kfree(ctrl_31);
+	kfree(ctrl_58);
+
+	return retval;
+}
+
+static int synaptics_rmi4_f1a_alloc_mem(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler)
+{
+	int retval;
+	struct synaptics_rmi4_f1a_handle *f1a;
+
+	f1a = kzalloc(sizeof(*f1a), GFP_KERNEL);
+	if (!f1a) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for function handle\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	fhandler->data = (void *)f1a;
+	fhandler->extra = NULL;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fhandler->full_addr.query_base,
+			f1a->button_query.data,
+			sizeof(f1a->button_query.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read query registers\n",
+				__func__);
+		return retval;
+	}
+
+	f1a->max_count = f1a->button_query.max_button_count + 1;
+
+	f1a->button_control.txrx_map = kzalloc(f1a->max_count * 2, GFP_KERNEL);
+	if (!f1a->button_control.txrx_map) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for tx rx mapping\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	f1a->button_bitmask_size = (f1a->max_count + 7) / 8;
+
+	f1a->button_data_buffer = kcalloc(f1a->button_bitmask_size,
+			sizeof(*(f1a->button_data_buffer)), GFP_KERNEL);
+	if (!f1a->button_data_buffer) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for data buffer\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	f1a->button_map = kcalloc(f1a->max_count,
+			sizeof(*(f1a->button_map)), GFP_KERNEL);
+	if (!f1a->button_map) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for button map\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int synaptics_rmi4_f1a_button_map(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char offset = 0;
+	struct synaptics_rmi4_f1a_query_4 query_4;
+	struct synaptics_rmi4_f1a_handle *f1a = fhandler->data;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	rmi4_data->valid_button_count = f1a->valid_button_count;
+
+	offset = f1a->button_query.has_general_control +
+			f1a->button_query.has_interrupt_enable +
+			f1a->button_query.has_multibutton_select;
+
+	if (f1a->button_query.has_tx_rx_map) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.ctrl_base + offset,
+				f1a->button_control.txrx_map,
+				f1a->max_count * 2);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read tx rx mapping\n",
+					__func__);
+			return retval;
+		}
+
+		rmi4_data->button_txrx_mapping = f1a->button_control.txrx_map;
+	}
+
+	if (f1a->button_query.has_query4) {
+		offset = 2 + f1a->button_query.has_query2 +
+				f1a->button_query.has_query3;
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				fhandler->full_addr.query_base + offset,
+				query_4.data,
+				sizeof(query_4.data));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read button features 4\n",
+					__func__);
+			return retval;
+		}
+
+		if (query_4.has_ctrl24)
+			rmi4_data->external_afe_buttons = true;
+		else
+			rmi4_data->external_afe_buttons = false;
+	}
+
+	if (!bdata->cap_button_map) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: cap_button_map is NULL in board file\n",
+				__func__);
+		return -ENODEV;
+	} else if (!bdata->cap_button_map->map) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Button map is missing in board file\n",
+				__func__);
+		return -ENODEV;
+	} else {
+		if (bdata->cap_button_map->nbuttons != f1a->max_count) {
+			f1a->valid_button_count = min(f1a->max_count,
+					bdata->cap_button_map->nbuttons);
+		} else {
+			f1a->valid_button_count = f1a->max_count;
+		}
+
+		for (ii = 0; ii < f1a->valid_button_count; ii++)
+			f1a->button_map[ii] = bdata->cap_button_map->map[ii];
+
+		rmi4_data->valid_button_count = f1a->valid_button_count;
+	}
+
+	return 0;
+}
+
+static void synaptics_rmi4_f1a_kfree(struct synaptics_rmi4_fn *fhandler)
+{
+	struct synaptics_rmi4_f1a_handle *f1a = fhandler->data;
+
+	if (f1a) {
+		kfree(f1a->button_control.txrx_map);
+		kfree(f1a->button_data_buffer);
+		kfree(f1a->button_map);
+		kfree(f1a);
+		fhandler->data = NULL;
+	}
+}
+
+static int synaptics_rmi4_f1a_init(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn *fhandler,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned int intr_count)
+{
+	int retval;
+
+	fhandler->fn_number = fd->fn_number;
+	fhandler->num_of_data_sources = fd->intr_src_count;
+
+	synaptics_rmi4_set_intr_mask(fhandler, fd, intr_count);
+
+	retval = synaptics_rmi4_f1a_alloc_mem(rmi4_data, fhandler);
+	if (retval < 0)
+		goto error_exit;
+
+	retval = synaptics_rmi4_f1a_button_map(rmi4_data, fhandler);
+	if (retval < 0)
+		goto error_exit;
+
+	rmi4_data->button_0d_enabled = 1;
+
+	return 0;
+
+error_exit:
+	synaptics_rmi4_f1a_kfree(fhandler);
+
+	return retval;
+}
+
+static void synaptics_rmi4_empty_fn_list(struct synaptics_rmi4_data *rmi4_data)
+{
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_fn *fhandler_temp;
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	if (!list_empty(&rmi->support_fn_list)) {
+		list_for_each_entry_safe(fhandler,
+				fhandler_temp,
+				&rmi->support_fn_list,
+				link) {
+			if (fhandler->fn_number == SYNAPTICS_RMI4_F1A) {
+				synaptics_rmi4_f1a_kfree(fhandler);
+			} else {
+				kfree(fhandler->extra);
+				kfree(fhandler->data);
+			}
+			list_del(&fhandler->link);
+			kfree(fhandler);
+		}
+	}
+	INIT_LIST_HEAD(&rmi->support_fn_list);
+}
+
+static int synaptics_rmi4_check_status(struct synaptics_rmi4_data *rmi4_data,
+		bool *was_in_bl_mode)
+{
+	int retval;
+	int timeout = CHECK_STATUS_TIMEOUT_MS;
+	struct synaptics_rmi4_f01_device_status status;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_data_base_addr,
+			status.data,
+			sizeof(status.data));
+	if (retval < 0)
+		return retval;
+
+	while (status.status_code == STATUS_CRC_IN_PROGRESS) {
+		if (timeout > 0)
+			msleep(20);
+		else
+			return -EINVAL;
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				rmi4_data->f01_data_base_addr,
+				status.data,
+				sizeof(status.data));
+		if (retval < 0)
+			return retval;
+
+		timeout -= 20;
+	}
+
+	if (timeout != CHECK_STATUS_TIMEOUT_MS)
+		*was_in_bl_mode = true;
+
+	if (status.flash_prog == 1) {
+		rmi4_data->flash_prog_mode = true;
+		pr_notice("%s: In flash prog mode, status = 0x%02x\n",
+				__func__,
+				status.status_code);
+	} else {
+		rmi4_data->flash_prog_mode = false;
+	}
+
+	return 0;
+}
+
+static int synaptics_rmi4_set_configured(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char device_ctrl;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set configured\n",
+				__func__);
+		return retval;
+	}
+
+	rmi4_data->no_sleep_setting = device_ctrl & NO_SLEEP_ON;
+	device_ctrl |= CONFIGURED;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set configured\n",
+				__func__);
+	}
+
+	return retval;
+}
+
+static int synaptics_rmi4_alloc_fh(struct synaptics_rmi4_fn **fhandler,
+		struct synaptics_rmi4_fn_desc *rmi_fd, int page_number)
+{
+	*fhandler = kzalloc(sizeof(**fhandler), GFP_KERNEL);
+	if (!(*fhandler))
+		return -ENOMEM;
+
+	(*fhandler)->full_addr.data_base =
+			(rmi_fd->data_base_addr |
+			(page_number << 8));
+	(*fhandler)->full_addr.ctrl_base =
+			(rmi_fd->ctrl_base_addr |
+			(page_number << 8));
+	(*fhandler)->full_addr.cmd_base =
+			(rmi_fd->cmd_base_addr |
+			(page_number << 8));
+	(*fhandler)->full_addr.query_base =
+			(rmi_fd->query_base_addr |
+			(page_number << 8));
+
+	return 0;
+}
+
+static int synaptics_rmi4_query_device(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char page_number;
+	unsigned char intr_count;
+	unsigned char *f01_query;
+	unsigned short pdt_entry_addr;
+	bool f01found;
+	bool f35found;
+	bool was_in_bl_mode;
+	struct synaptics_rmi4_fn_desc rmi_fd;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+rescan_pdt:
+	f01found = false;
+	f35found = false;
+	was_in_bl_mode = false;
+	intr_count = 0;
+	INIT_LIST_HEAD(&rmi->support_fn_list);
+
+	/* Scan the page description tables of the pages to service */
+	for (page_number = 0; page_number < PAGES_TO_SERVICE; page_number++) {
+		for (pdt_entry_addr = PDT_START; pdt_entry_addr > PDT_END;
+				pdt_entry_addr -= PDT_ENTRY_SIZE) {
+			pdt_entry_addr |= (page_number << 8);
+
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					pdt_entry_addr,
+					(unsigned char *)&rmi_fd,
+					sizeof(rmi_fd));
+			if (retval < 0)
+				return retval;
+
+			pdt_entry_addr &= ~(MASK_8BIT << 8);
+
+			fhandler = NULL;
+
+			if (rmi_fd.fn_number == 0) {
+				dev_dbg(rmi4_data->pdev->dev.parent,
+						"%s: Reached end of PDT\n",
+						__func__);
+				break;
+			}
+
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: F%02x found (page %d)\n",
+					__func__, rmi_fd.fn_number,
+					page_number);
+
+			switch (rmi_fd.fn_number) {
+			case SYNAPTICS_RMI4_F01:
+				if (rmi_fd.intr_src_count == 0)
+					break;
+
+				f01found = true;
+
+				retval = synaptics_rmi4_alloc_fh(&fhandler,
+						&rmi_fd, page_number);
+				if (retval < 0) {
+					dev_err(rmi4_data->pdev->dev.parent,
+							"%s: Failed to alloc for F%d\n",
+							__func__,
+							rmi_fd.fn_number);
+					return retval;
+				}
+
+				retval = synaptics_rmi4_f01_init(rmi4_data,
+						fhandler, &rmi_fd, intr_count);
+				if (retval < 0)
+					return retval;
+
+				retval = synaptics_rmi4_check_status(rmi4_data,
+						&was_in_bl_mode);
+				if (retval < 0) {
+					dev_err(rmi4_data->pdev->dev.parent,
+							"%s: Failed to check status\n",
+							__func__);
+					return retval;
+				}
+
+				if (was_in_bl_mode) {
+					kfree(fhandler);
+					fhandler = NULL;
+					goto rescan_pdt;
+				}
+
+				if (rmi4_data->flash_prog_mode)
+					goto flash_prog_mode;
+
+				break;
+			case SYNAPTICS_RMI4_F11:
+				if (rmi_fd.intr_src_count == 0)
+					break;
+
+				retval = synaptics_rmi4_alloc_fh(&fhandler,
+						&rmi_fd, page_number);
+				if (retval < 0) {
+					dev_err(rmi4_data->pdev->dev.parent,
+							"%s: Failed to alloc for F%d\n",
+							__func__,
+							rmi_fd.fn_number);
+					return retval;
+				}
+
+				retval = synaptics_rmi4_f11_init(rmi4_data,
+						fhandler, &rmi_fd, intr_count);
+				if (retval < 0)
+					return retval;
+				break;
+			case SYNAPTICS_RMI4_F12:
+				if (rmi_fd.intr_src_count == 0)
+					break;
+
+				retval = synaptics_rmi4_alloc_fh(&fhandler,
+						&rmi_fd, page_number);
+				if (retval < 0) {
+					dev_err(rmi4_data->pdev->dev.parent,
+							"%s: Failed to alloc for F%d\n",
+							__func__,
+							rmi_fd.fn_number);
+					return retval;
+				}
+
+				retval = synaptics_rmi4_f12_init(rmi4_data,
+						fhandler, &rmi_fd, intr_count);
+				if (retval < 0)
+					return retval;
+				break;
+			case SYNAPTICS_RMI4_F1A:
+				if (rmi_fd.intr_src_count == 0)
+					break;
+
+				retval = synaptics_rmi4_alloc_fh(&fhandler,
+						&rmi_fd, page_number);
+				if (retval < 0) {
+					dev_err(rmi4_data->pdev->dev.parent,
+							"%s: Failed to alloc for F%d\n",
+							__func__,
+							rmi_fd.fn_number);
+					return retval;
+				}
+
+				retval = synaptics_rmi4_f1a_init(rmi4_data,
+						fhandler, &rmi_fd, intr_count);
+				if (retval < 0) {
+#ifdef IGNORE_FN_INIT_FAILURE
+					kfree(fhandler);
+					fhandler = NULL;
+#else
+					return retval;
+#endif
+				}
+				break;
+#ifdef USE_DATA_SERVER
+			case SYNAPTICS_RMI4_F21:
+				if (rmi_fd.intr_src_count == 0)
+					break;
+
+				retval = synaptics_rmi4_alloc_fh(&fhandler,
+						&rmi_fd, page_number);
+				if (retval < 0) {
+					dev_err(rmi4_data->pdev->dev.parent,
+							"%s: Failed to alloc for F%d\n",
+							__func__,
+							rmi_fd.fn_number);
+					return retval;
+				}
+
+				fhandler->fn_number = rmi_fd.fn_number;
+				fhandler->num_of_data_sources =
+						rmi_fd.intr_src_count;
+
+				synaptics_rmi4_set_intr_mask(fhandler, &rmi_fd,
+						intr_count);
+				break;
+#endif
+			case SYNAPTICS_RMI4_F35:
+				f35found = true;
+				break;
+#ifdef F51_DISCRETE_FORCE
+			case SYNAPTICS_RMI4_F51:
+				rmi4_data->f51_query_base_addr =
+						rmi_fd.query_base_addr |
+						(page_number << 8);
+				break;
+#endif
+			}
+
+			/* Accumulate the interrupt count */
+			intr_count += rmi_fd.intr_src_count;
+
+			if (fhandler && rmi_fd.intr_src_count) {
+				list_add_tail(&fhandler->link,
+						&rmi->support_fn_list);
+			}
+		}
+	}
+
+	if (!f01found) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to find F01\n",
+				__func__);
+		if (!f35found) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to find F35\n",
+					__func__);
+			return -EINVAL;
+		} else {
+			pr_notice("%s: In microbootloader mode\n",
+					__func__);
+			return 0;
+		}
+	}
+
+flash_prog_mode:
+	rmi4_data->num_of_intr_regs = (intr_count + 7) / 8;
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Number of interrupt registers = %d\n",
+			__func__, rmi4_data->num_of_intr_regs);
+
+	f01_query = kmalloc(F01_STD_QUERY_LEN, GFP_KERNEL);
+	if (!f01_query) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for f01_query\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_query_base_addr,
+			f01_query,
+			F01_STD_QUERY_LEN);
+	if (retval < 0) {
+		kfree(f01_query);
+		return retval;
+	}
+
+	/* RMI Version 4.0 currently supported */
+	rmi->version_major = 4;
+	rmi->version_minor = 0;
+
+	rmi->manufacturer_id = f01_query[0];
+	rmi->product_props = f01_query[1];
+	rmi->product_info[0] = f01_query[2];
+	rmi->product_info[1] = f01_query[3];
+	retval = secure_memcpy(rmi->product_id_string,
+			sizeof(rmi->product_id_string),
+			&f01_query[11],
+			F01_STD_QUERY_LEN - 11,
+			PRODUCT_ID_SIZE);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy product ID string\n",
+				__func__);
+	}
+
+	kfree(f01_query);
+
+	if (rmi->manufacturer_id != 1) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Non-Synaptics device found, manufacturer ID = %d\n",
+				__func__, rmi->manufacturer_id);
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_query_base_addr + F01_BUID_ID_OFFSET,
+			rmi->build_id,
+			sizeof(rmi->build_id));
+	if (retval < 0)
+		return retval;
+
+	rmi4_data->firmware_id = (unsigned int)rmi->build_id[0] +
+			(unsigned int)rmi->build_id[1] * 0x100 +
+			(unsigned int)rmi->build_id[2] * 0x10000;
+
+	memset(rmi4_data->intr_mask, 0x00, sizeof(rmi4_data->intr_mask));
+
+	/*
+	 * Map out the interrupt bit masks for the interrupt sources
+	 * from the registered function handlers.
+	 */
+	if (!list_empty(&rmi->support_fn_list)) {
+		list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+			if (fhandler->num_of_data_sources) {
+				rmi4_data->intr_mask[fhandler->intr_reg_num] |=
+						fhandler->intr_mask;
+			}
+		}
+	}
+
+	if (rmi4_data->f11_wakeup_gesture || rmi4_data->f12_wakeup_gesture)
+		rmi4_data->enable_wakeup_gesture = WAKEUP_GESTURE;
+	else
+		rmi4_data->enable_wakeup_gesture = false;
+
+	synaptics_rmi4_set_configured(rmi4_data);
+
+	return 0;
+}
+
+static int synaptics_rmi4_gpio_setup(int gpio, bool config, int dir, int state)
+{
+	int retval = 0;
+	unsigned char buf[16];
+
+	if (config) {
+		snprintf(buf, sizeof(buf), "dsx_gpio_%u\n", gpio);
+
+		retval = gpio_request(gpio, buf);
+		if (retval) {
+			pr_err("%s: Failed to get gpio %d (code: %d)",
+					__func__, gpio, retval);
+			return retval;
+		}
+
+		if (dir == 0)
+			retval = gpio_direction_input(gpio);
+		else
+			retval = gpio_direction_output(gpio, state);
+		if (retval) {
+			pr_err("%s: Failed to set gpio %d direction",
+					__func__, gpio);
+			return retval;
+		}
+	} else {
+		gpio_free(gpio);
+	}
+
+	return retval;
+}
+
+static void synaptics_rmi4_set_params(struct synaptics_rmi4_data *rmi4_data)
+{
+	unsigned char ii;
+	struct synaptics_rmi4_f1a_handle *f1a;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	input_set_abs_params(rmi4_data->input_dev,
+			ABS_MT_POSITION_X, 0,
+			rmi4_data->sensor_max_x, 0, 0);
+	input_set_abs_params(rmi4_data->input_dev,
+			ABS_MT_POSITION_Y, 0,
+			rmi4_data->sensor_max_y, 0, 0);
+#ifdef REPORT_2D_W
+	input_set_abs_params(rmi4_data->input_dev,
+			ABS_MT_TOUCH_MAJOR, 0,
+			rmi4_data->max_touch_width, 0, 0);
+	input_set_abs_params(rmi4_data->input_dev,
+			ABS_MT_TOUCH_MINOR, 0,
+			rmi4_data->max_touch_width, 0, 0);
+#endif
+
+	rmi4_data->input_settings.sensor_max_x = rmi4_data->sensor_max_x;
+	rmi4_data->input_settings.sensor_max_y = rmi4_data->sensor_max_y;
+	rmi4_data->input_settings.max_touch_width = rmi4_data->max_touch_width;
+
+#ifdef REPORT_2D_PRESSURE
+	if (rmi4_data->report_pressure) {
+		input_set_abs_params(rmi4_data->input_dev,
+				ABS_MT_PRESSURE, rmi4_data->force_min,
+				rmi4_data->force_max, 0, 0);
+
+		rmi4_data->input_settings.force_min = rmi4_data->force_min;
+		rmi4_data->input_settings.force_max = rmi4_data->force_max;
+	}
+#elif defined(F51_DISCRETE_FORCE)
+	input_set_abs_params(rmi4_data->input_dev,
+			ABS_MT_PRESSURE, 0,
+			FORCE_LEVEL_MAX, 0, 0);
+#endif
+
+#ifdef TYPE_B_PROTOCOL
+#ifdef KERNEL_ABOVE_3_6
+	input_mt_init_slots(rmi4_data->input_dev,
+			rmi4_data->num_of_fingers, INPUT_MT_DIRECT);
+#else
+	input_mt_init_slots(rmi4_data->input_dev,
+			rmi4_data->num_of_fingers);
+#endif
+#endif
+
+	rmi4_data->input_settings.num_of_fingers = rmi4_data->num_of_fingers;
+
+	f1a = NULL;
+	if (!list_empty(&rmi->support_fn_list)) {
+		list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+			if (fhandler->fn_number == SYNAPTICS_RMI4_F1A)
+				f1a = fhandler->data;
+		}
+	}
+
+	if (f1a) {
+		for (ii = 0; ii < f1a->valid_button_count; ii++) {
+			set_bit(f1a->button_map[ii],
+					rmi4_data->input_dev->keybit);
+			input_set_capability(rmi4_data->input_dev,
+					EV_KEY, f1a->button_map[ii]);
+		}
+
+		rmi4_data->input_settings.valid_button_count =
+				f1a->valid_button_count;
+	}
+
+	if (vir_button_map->nbuttons) {
+		for (ii = 0; ii < vir_button_map->nbuttons; ii++) {
+			set_bit(vir_button_map->map[ii * 5],
+					rmi4_data->input_dev->keybit);
+			input_set_capability(rmi4_data->input_dev,
+					EV_KEY, vir_button_map->map[ii * 5]);
+		}
+	}
+
+	if (rmi4_data->f11_wakeup_gesture || rmi4_data->f12_wakeup_gesture) {
+		set_bit(KEY_WAKEUP, rmi4_data->input_dev->keybit);
+		input_set_capability(rmi4_data->input_dev, EV_KEY, KEY_WAKEUP);
+	}
+}
+
+static int synaptics_rmi4_set_input_dev(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	const struct synaptics_dsx_board_data *bdata =
+				rmi4_data->hw_if->board_data;
+
+	rmi4_data->input_dev = input_allocate_device();
+	if (rmi4_data->input_dev == NULL) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to allocate input device\n",
+				__func__);
+		retval = -ENOMEM;
+		goto err_input_device;
+	}
+
+	retval = synaptics_rmi4_query_device(rmi4_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to query device\n",
+				__func__);
+		goto err_query_device;
+	}
+
+	rmi4_data->input_dev->name = PLATFORM_DRIVER_NAME;
+	rmi4_data->input_dev->phys = INPUT_PHYS_NAME;
+	rmi4_data->input_dev->id.product = SYNAPTICS_DSX_DRIVER_PRODUCT;
+	rmi4_data->input_dev->id.version = SYNAPTICS_DSX_DRIVER_VERSION;
+	rmi4_data->input_dev->dev.parent = rmi4_data->pdev->dev.parent;
+	input_set_drvdata(rmi4_data->input_dev, rmi4_data);
+
+	set_bit(EV_SYN, rmi4_data->input_dev->evbit);
+	set_bit(EV_KEY, rmi4_data->input_dev->evbit);
+	set_bit(EV_ABS, rmi4_data->input_dev->evbit);
+	set_bit(BTN_TOUCH, rmi4_data->input_dev->keybit);
+	set_bit(BTN_TOOL_FINGER, rmi4_data->input_dev->keybit);
+#ifdef INPUT_PROP_DIRECT
+	set_bit(INPUT_PROP_DIRECT, rmi4_data->input_dev->propbit);
+#endif
+
+	if (bdata->max_y_for_2d >= 0)
+		rmi4_data->sensor_max_y = bdata->max_y_for_2d;
+
+	synaptics_rmi4_set_params(rmi4_data);
+
+	retval = input_register_device(rmi4_data->input_dev);
+	if (retval) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to register input device\n",
+				__func__);
+		goto err_register_input;
+	}
+
+	rmi4_data->input_settings.stylus_enable = rmi4_data->stylus_enable;
+	rmi4_data->input_settings.eraser_enable = rmi4_data->eraser_enable;
+
+	if (!rmi4_data->stylus_enable)
+		return 0;
+
+	rmi4_data->stylus_dev = input_allocate_device();
+	if (rmi4_data->stylus_dev == NULL) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to allocate stylus device\n",
+				__func__);
+		retval = -ENOMEM;
+		goto err_stylus_device;
+	}
+
+	rmi4_data->stylus_dev->name = STYLUS_DRIVER_NAME;
+	rmi4_data->stylus_dev->phys = STYLUS_PHYS_NAME;
+	rmi4_data->stylus_dev->id.product = SYNAPTICS_DSX_DRIVER_PRODUCT;
+	rmi4_data->stylus_dev->id.version = SYNAPTICS_DSX_DRIVER_VERSION;
+	rmi4_data->stylus_dev->dev.parent = rmi4_data->pdev->dev.parent;
+	input_set_drvdata(rmi4_data->stylus_dev, rmi4_data);
+
+	set_bit(EV_KEY, rmi4_data->stylus_dev->evbit);
+	set_bit(EV_ABS, rmi4_data->stylus_dev->evbit);
+	set_bit(BTN_TOUCH, rmi4_data->stylus_dev->keybit);
+	set_bit(BTN_TOOL_PEN, rmi4_data->stylus_dev->keybit);
+	if (rmi4_data->eraser_enable)
+		set_bit(BTN_TOOL_RUBBER, rmi4_data->stylus_dev->keybit);
+#ifdef INPUT_PROP_DIRECT
+	set_bit(INPUT_PROP_DIRECT, rmi4_data->stylus_dev->propbit);
+#endif
+
+	input_set_abs_params(rmi4_data->stylus_dev, ABS_X, 0,
+			rmi4_data->sensor_max_x, 0, 0);
+	input_set_abs_params(rmi4_data->stylus_dev, ABS_Y, 0,
+			rmi4_data->sensor_max_y, 0, 0);
+
+	retval = input_register_device(rmi4_data->stylus_dev);
+	if (retval) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to register stylus device\n",
+				__func__);
+		goto err_register_stylus;
+	}
+
+	return 0;
+
+err_register_stylus:
+	rmi4_data->stylus_dev = NULL;
+
+err_stylus_device:
+	input_unregister_device(rmi4_data->input_dev);
+	rmi4_data->input_dev = NULL;
+
+err_register_input:
+err_query_device:
+	synaptics_rmi4_empty_fn_list(rmi4_data);
+	input_free_device(rmi4_data->input_dev);
+
+err_input_device:
+	return retval;
+}
+
+static int synaptics_rmi4_set_gpio(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	retval = synaptics_rmi4_gpio_setup(
+			bdata->irq_gpio,
+			true, 0, 0);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to configure attention GPIO\n",
+				__func__);
+		goto err_gpio_irq;
+	}
+
+	if (bdata->power_gpio >= 0) {
+		retval = synaptics_rmi4_gpio_setup(
+				bdata->power_gpio,
+				true, 1, !bdata->power_on_state);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to configure power GPIO\n",
+					__func__);
+			goto err_gpio_power;
+		}
+	}
+
+	if (bdata->reset_gpio >= 0) {
+		retval = synaptics_rmi4_gpio_setup(
+				bdata->reset_gpio,
+				true, 1, !bdata->reset_on_state);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to configure reset GPIO\n",
+					__func__);
+			goto err_gpio_reset;
+		}
+	}
+
+	if (bdata->power_gpio >= 0) {
+		gpio_set_value(bdata->power_gpio, bdata->power_on_state);
+		msleep(bdata->power_delay_ms);
+	}
+
+	if (bdata->reset_gpio >= 0) {
+		gpio_set_value(bdata->reset_gpio, bdata->reset_on_state);
+		msleep(bdata->reset_active_ms);
+		gpio_set_value(bdata->reset_gpio, !bdata->reset_on_state);
+		msleep(bdata->reset_delay_ms);
+	}
+
+	return 0;
+
+err_gpio_reset:
+	if (bdata->power_gpio >= 0)
+		synaptics_rmi4_gpio_setup(bdata->power_gpio, false, 0, 0);
+
+err_gpio_power:
+	synaptics_rmi4_gpio_setup(bdata->irq_gpio, false, 0, 0);
+
+err_gpio_irq:
+	return retval;
+}
+
+static int synaptics_dsx_pinctrl_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+
+	/* Get pinctrl if target uses pinctrl */
+	rmi4_data->ts_pinctrl = devm_pinctrl_get((rmi4_data->pdev->dev.parent));
+	if (IS_ERR_OR_NULL(rmi4_data->ts_pinctrl)) {
+		retval = PTR_ERR(rmi4_data->ts_pinctrl);
+		dev_err(rmi4_data->pdev->dev.parent,
+			"Target does not use pinctrl %d\n", retval);
+		goto err_pinctrl_get;
+	}
+
+	rmi4_data->pinctrl_state_active
+		= pinctrl_lookup_state(rmi4_data->ts_pinctrl, "pmx_ts_active");
+	if (IS_ERR_OR_NULL(rmi4_data->pinctrl_state_active)) {
+		retval = PTR_ERR(rmi4_data->pinctrl_state_active);
+		dev_err(rmi4_data->pdev->dev.parent,
+			"Can not lookup %s pinstate %d\n",
+			PINCTRL_STATE_ACTIVE, retval);
+		goto err_pinctrl_lookup;
+	}
+
+	rmi4_data->pinctrl_state_suspend
+		= pinctrl_lookup_state(rmi4_data->ts_pinctrl, "pmx_ts_suspend");
+	if (IS_ERR_OR_NULL(rmi4_data->pinctrl_state_suspend)) {
+		retval = PTR_ERR(rmi4_data->pinctrl_state_suspend);
+		dev_err(rmi4_data->pdev->dev.parent,
+			"Can not lookup %s pinstate %d\n",
+			PINCTRL_STATE_SUSPEND, retval);
+		goto err_pinctrl_lookup;
+	}
+
+	rmi4_data->pinctrl_state_release
+		= pinctrl_lookup_state(rmi4_data->ts_pinctrl, "pmx_ts_release");
+	if (IS_ERR_OR_NULL(rmi4_data->pinctrl_state_release)) {
+		retval = PTR_ERR(rmi4_data->pinctrl_state_release);
+		dev_err(rmi4_data->pdev->dev.parent,
+			"Can not lookup %s pinstate %d\n",
+			PINCTRL_STATE_RELEASE, retval);
+	}
+
+	return 0;
+
+err_pinctrl_lookup:
+	devm_pinctrl_put(rmi4_data->ts_pinctrl);
+err_pinctrl_get:
+	rmi4_data->ts_pinctrl = NULL;
+	return retval;
+}
+
+static int synaptics_rmi4_regulator_configure(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	int retval;
+
+	if (enable) {
+		retval = regulator_set_load(rmi4_data->pwr_reg,
+				20000);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to set active regulator load avdd\n",
+					__func__);
+			return retval;
+		}
+
+		retval = regulator_set_voltage(rmi4_data->pwr_reg,
+			3296000,
+			3304000);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to set active regulator voltage avdd\n",
+					__func__);
+			goto err_avdd_load;
+		}
+
+		retval = regulator_set_load(rmi4_data->bus_reg,
+			62000);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to set active regulator load vdd\n",
+					__func__);
+			goto err_avdd_load;
+		}
+
+		retval = regulator_set_voltage(rmi4_data->bus_reg,
+				1800000,
+				1800000);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to set active regulator voltage vdd\n",
+					__func__);
+			goto err_vdd_load;
+		}
+
+	} else {
+		retval = regulator_set_load(rmi4_data->pwr_reg, 0);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to set inactive regulator load avdd\n",
+					__func__);
+			return retval;
+		}
+
+		retval = regulator_set_load(rmi4_data->bus_reg, 0);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to set inactive regulator load vdd\n",
+					__func__);
+			return retval;
+		}
+
+	}
+	return retval;
+
+err_vdd_load:
+	regulator_set_load(rmi4_data->bus_reg, 0);
+err_avdd_load:
+	regulator_set_load(rmi4_data->pwr_reg, 0);
+	return retval;
+}
+
+static int synaptics_rmi4_get_reg(struct synaptics_rmi4_data *rmi4_data,
+		bool get)
+{
+	int retval;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	if (!get) {
+		retval = 0;
+		goto regulator_put;
+	}
+
+	if ((bdata->pwr_reg_name != NULL) && (*bdata->pwr_reg_name != 0)) {
+		rmi4_data->pwr_reg = regulator_get(rmi4_data->pdev->dev.parent,
+				bdata->pwr_reg_name);
+		if (IS_ERR(rmi4_data->pwr_reg)) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to get power regulator\n",
+					__func__);
+			retval = PTR_ERR(rmi4_data->pwr_reg);
+			goto regulator_put;
+		}
+	}
+
+	retval = regulator_set_load(rmi4_data->pwr_reg,
+		20000);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Failed to set regulator current avdd\n",
+				__func__);
+		goto regulator_put;
+	}
+
+	retval = regulator_set_voltage(rmi4_data->pwr_reg,
+			3296000,
+			3304000);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set regulator voltage avdd\n",
+				__func__);
+		goto regulator_put;
+	}
+
+	if ((bdata->bus_reg_name != NULL) && (*bdata->bus_reg_name != 0)) {
+		rmi4_data->bus_reg = regulator_get(rmi4_data->pdev->dev.parent,
+				bdata->bus_reg_name);
+		if (IS_ERR(rmi4_data->bus_reg)) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to get bus pullup regulator\n",
+					__func__);
+			retval = PTR_ERR(rmi4_data->bus_reg);
+			goto regulator_put;
+		}
+	}
+
+	retval = regulator_set_load(rmi4_data->bus_reg,
+		62000);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set regulator current vdd\n",
+				__func__);
+		goto regulator_put;
+	}
+
+	retval = regulator_set_voltage(rmi4_data->bus_reg,
+			1800000,
+			1800000);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set regulator voltage vdd\n",
+				__func__);
+		goto regulator_put;
+	}
+
+	return 0;
+
+regulator_put:
+	if (rmi4_data->pwr_reg) {
+		regulator_put(rmi4_data->pwr_reg);
+		rmi4_data->pwr_reg = NULL;
+	}
+
+	if (rmi4_data->bus_reg) {
+		regulator_put(rmi4_data->bus_reg);
+		rmi4_data->bus_reg = NULL;
+	}
+
+	return retval;
+}
+
+static int synaptics_rmi4_enable_reg(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	int retval;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	if (!enable) {
+		retval = 0;
+		goto disable_pwr_reg;
+	}
+
+	if (rmi4_data->bus_reg && rmi4_data->vdd_status == 0) {
+		retval = regulator_enable(rmi4_data->bus_reg);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to enable bus pullup regulator\n",
+					__func__);
+			goto exit;
+		}
+		rmi4_data->vdd_status = 1;
+	}
+
+	if (rmi4_data->pwr_reg && rmi4_data->avdd_status == 0) {
+		retval = regulator_enable(rmi4_data->pwr_reg);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to enable power regulator\n",
+					__func__);
+			goto disable_bus_reg;
+		}
+		rmi4_data->avdd_status = 1;
+		msleep(bdata->power_delay_ms);
+	}
+
+	return 0;
+
+disable_pwr_reg:
+	if (rmi4_data->pwr_reg && rmi4_data->avdd_status == 1) {
+		regulator_disable(rmi4_data->pwr_reg);
+		rmi4_data->avdd_status = 0;
+	}
+
+disable_bus_reg:
+	if (rmi4_data->bus_reg && rmi4_data->vdd_status == 1) {
+		regulator_disable(rmi4_data->bus_reg);
+		rmi4_data->vdd_status = 0;
+	}
+
+exit:
+	synaptics_rmi4_regulator_configure(rmi4_data, false);
+	return retval;
+}
+
+static int synaptics_rmi4_free_fingers(struct synaptics_rmi4_data *rmi4_data)
+{
+	unsigned char ii;
+
+	mutex_lock(&(rmi4_data->rmi4_report_mutex));
+
+#ifdef TYPE_B_PROTOCOL
+	for (ii = 0; ii < rmi4_data->num_of_fingers; ii++) {
+		input_mt_slot(rmi4_data->input_dev, ii);
+		input_mt_report_slot_state(rmi4_data->input_dev,
+				MT_TOOL_FINGER, 0);
+	}
+#endif
+	input_report_key(rmi4_data->input_dev,
+			BTN_TOUCH, 0);
+	input_report_key(rmi4_data->input_dev,
+			BTN_TOOL_FINGER, 0);
+#ifndef TYPE_B_PROTOCOL
+	input_mt_sync(rmi4_data->input_dev);
+#endif
+	input_sync(rmi4_data->input_dev);
+
+	if (rmi4_data->stylus_enable) {
+		input_report_key(rmi4_data->stylus_dev,
+				BTN_TOUCH, 0);
+		input_report_key(rmi4_data->stylus_dev,
+				BTN_TOOL_PEN, 0);
+		if (rmi4_data->eraser_enable) {
+			input_report_key(rmi4_data->stylus_dev,
+					BTN_TOOL_RUBBER, 0);
+		}
+		input_sync(rmi4_data->stylus_dev);
+	}
+
+	mutex_unlock(&(rmi4_data->rmi4_report_mutex));
+
+	rmi4_data->fingers_on_2d = false;
+
+	return 0;
+}
+
+static int synaptics_rmi4_sw_reset(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char command = 0x01;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			rmi4_data->f01_cmd_base_addr,
+			&command,
+			sizeof(command));
+	if (retval < 0)
+		return retval;
+
+	msleep(rmi4_data->hw_if->board_data->reset_delay_ms);
+
+	if (rmi4_data->hw_if->ui_hw_init) {
+		retval = rmi4_data->hw_if->ui_hw_init(rmi4_data);
+		if (retval < 0)
+			return retval;
+	}
+
+	return 0;
+}
+
+static int synaptics_rmi4_do_rebuild(struct synaptics_rmi4_data *rmi4_data)
+{
+	struct synaptics_rmi4_input_settings *settings;
+
+	settings = &(rmi4_data->input_settings);
+
+	if (settings->num_of_fingers != rmi4_data->num_of_fingers)
+		return 1;
+
+	if (settings->valid_button_count != rmi4_data->valid_button_count)
+		return 1;
+
+	if (settings->max_touch_width != rmi4_data->max_touch_width)
+		return 1;
+
+	if (settings->sensor_max_x != rmi4_data->sensor_max_x)
+		return 1;
+
+	if (settings->sensor_max_y != rmi4_data->sensor_max_y)
+		return 1;
+
+	if (settings->force_min != rmi4_data->force_min)
+		return 1;
+
+	if (settings->force_max != rmi4_data->force_max)
+		return 1;
+
+	if (settings->stylus_enable != rmi4_data->stylus_enable)
+		return 1;
+
+	if (settings->eraser_enable != rmi4_data->eraser_enable)
+		return 1;
+
+	return 0;
+}
+
+static void synaptics_rmi4_rebuild_work(struct work_struct *work)
+{
+	int retval;
+	unsigned char attr_count;
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct delayed_work *delayed_work =
+			container_of(work, struct delayed_work, work);
+	struct synaptics_rmi4_data *rmi4_data =
+			container_of(delayed_work, struct synaptics_rmi4_data,
+			rb_work);
+
+	mutex_lock(&(rmi4_data->rmi4_reset_mutex));
+
+	mutex_lock(&exp_data.mutex);
+
+	synaptics_rmi4_irq_enable(rmi4_data, false, false);
+
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->remove != NULL)
+				exp_fhandler->exp_fn->remove(rmi4_data);
+	}
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+	synaptics_rmi4_free_fingers(rmi4_data);
+	synaptics_rmi4_empty_fn_list(rmi4_data);
+	input_unregister_device(rmi4_data->input_dev);
+	rmi4_data->input_dev = NULL;
+	if (rmi4_data->stylus_enable) {
+		input_unregister_device(rmi4_data->stylus_dev);
+		rmi4_data->stylus_dev = NULL;
+	}
+
+	retval = synaptics_rmi4_set_input_dev(rmi4_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set up input device\n",
+				__func__);
+		goto exit;
+	}
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		retval = sysfs_create_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create sysfs attributes\n",
+					__func__);
+			goto exit;
+		}
+	}
+
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->init != NULL)
+				exp_fhandler->exp_fn->init(rmi4_data);
+	}
+
+exit:
+	synaptics_rmi4_irq_enable(rmi4_data, true, false);
+
+	mutex_unlock(&exp_data.mutex);
+
+	mutex_unlock(&(rmi4_data->rmi4_reset_mutex));
+}
+
+static int synaptics_rmi4_reinit_device(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	mutex_lock(&(rmi4_data->rmi4_reset_mutex));
+
+	synaptics_rmi4_free_fingers(rmi4_data);
+
+	if (!list_empty(&rmi->support_fn_list)) {
+		list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+			if (fhandler->fn_number == SYNAPTICS_RMI4_F12) {
+				synaptics_rmi4_f12_set_enables(rmi4_data, 0);
+				break;
+			}
+		}
+	}
+
+	retval = synaptics_rmi4_int_enable(rmi4_data, true);
+	if (retval < 0)
+		goto exit;
+
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->reinit != NULL)
+				exp_fhandler->exp_fn->reinit(rmi4_data);
+	}
+	mutex_unlock(&exp_data.mutex);
+
+	synaptics_rmi4_set_configured(rmi4_data);
+
+	retval = 0;
+
+exit:
+	mutex_unlock(&(rmi4_data->rmi4_reset_mutex));
+	return retval;
+}
+
+static int synaptics_rmi4_reset_device(struct synaptics_rmi4_data *rmi4_data,
+		bool rebuild)
+{
+	int retval;
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+
+	mutex_lock(&(rmi4_data->rmi4_reset_mutex));
+
+	synaptics_rmi4_irq_enable(rmi4_data, false, false);
+
+	retval = synaptics_rmi4_sw_reset(rmi4_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to issue reset command\n",
+				__func__);
+		goto exit;
+	}
+
+	synaptics_rmi4_free_fingers(rmi4_data);
+
+	synaptics_rmi4_empty_fn_list(rmi4_data);
+
+	retval = synaptics_rmi4_query_device(rmi4_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to query device\n",
+				__func__);
+		goto exit;
+	}
+
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->reset != NULL)
+				exp_fhandler->exp_fn->reset(rmi4_data);
+	}
+	mutex_unlock(&exp_data.mutex);
+
+	retval = 0;
+
+exit:
+	synaptics_rmi4_irq_enable(rmi4_data, true, false);
+
+	mutex_unlock(&(rmi4_data->rmi4_reset_mutex));
+
+	if (rebuild && synaptics_rmi4_do_rebuild(rmi4_data)) {
+		queue_delayed_work(rmi4_data->rb_workqueue,
+				&rmi4_data->rb_work,
+				msecs_to_jiffies(REBUILD_WORK_DELAY_MS));
+	}
+
+	return retval;
+}
+
+#ifdef FB_READY_RESET
+static void synaptics_rmi4_reset_work(struct work_struct *work)
+{
+	int retval = 0;
+	unsigned int timeout;
+	struct synaptics_rmi4_data *rmi4_data =
+			container_of(work, struct synaptics_rmi4_data,
+			reset_work);
+
+	timeout = FB_READY_TIMEOUT_S * 1000 / FB_READY_WAIT_MS + 1;
+
+	while (!rmi4_data->fb_ready) {
+		msleep(FB_READY_WAIT_MS);
+		timeout--;
+		if (timeout == 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Timed out waiting for FB ready\n",
+					__func__);
+			goto err;
+		}
+	}
+
+	mutex_lock(&rmi4_data->rmi4_exp_init_mutex);
+
+	retval = synaptics_rmi4_reset_device(rmi4_data, false);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to issue reset command\n",
+				__func__);
+	}
+
+	mutex_unlock(&rmi4_data->rmi4_exp_init_mutex);
+err:
+
+	dev_err(rmi4_data->pdev->dev.parent,
+		"%s: Timed out waiting for FB ready\n",
+		__func__);
+
+}
+#endif
+
+static int synaptics_rmi4_sleep_enable(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	int retval;
+	unsigned char device_ctrl;
+	unsigned char no_sleep_setting = rmi4_data->no_sleep_setting;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read device control\n",
+				__func__);
+		return retval;
+	}
+
+	device_ctrl = device_ctrl & ~MASK_3BIT;
+	if (enable)
+		device_ctrl = device_ctrl | SENSOR_SLEEP;
+	else
+		device_ctrl = device_ctrl | no_sleep_setting | NORMAL_OPERATION;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write device control\n",
+				__func__);
+		return retval;
+	}
+
+	rmi4_data->sensor_sleep = enable;
+
+	return retval;
+}
+
+static void synaptics_rmi4_exp_fn_work(struct work_struct *work)
+{
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler_temp;
+	struct synaptics_rmi4_data *rmi4_data = exp_data.rmi4_data;
+
+	mutex_lock(&rmi4_data->rmi4_exp_init_mutex);
+	mutex_lock(&rmi4_data->rmi4_reset_mutex);
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry_safe(exp_fhandler,
+				exp_fhandler_temp,
+				&exp_data.list,
+				link) {
+			if ((exp_fhandler->exp_fn->init != NULL) &&
+					exp_fhandler->insert) {
+				exp_fhandler->exp_fn->init(rmi4_data);
+				exp_fhandler->insert = false;
+			} else if ((exp_fhandler->exp_fn->remove != NULL) &&
+					exp_fhandler->remove) {
+				exp_fhandler->exp_fn->remove(rmi4_data);
+				list_del(&exp_fhandler->link);
+				kfree(exp_fhandler);
+			}
+		}
+	}
+	mutex_unlock(&exp_data.mutex);
+	mutex_unlock(&rmi4_data->rmi4_reset_mutex);
+	mutex_unlock(&rmi4_data->rmi4_exp_init_mutex);
+}
+
+void synaptics_rmi4_new_function(struct synaptics_rmi4_exp_fn *exp_fn,
+		bool insert)
+{
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+
+	if (!exp_data.initialized) {
+		mutex_init(&exp_data.mutex);
+		INIT_LIST_HEAD(&exp_data.list);
+		exp_data.initialized = true;
+	}
+
+	mutex_lock(&exp_data.mutex);
+	if (insert) {
+		exp_fhandler = kzalloc(sizeof(*exp_fhandler), GFP_KERNEL);
+		if (!exp_fhandler) {
+			pr_err("%s: Failed to alloc mem for expansion function\n",
+					__func__);
+			goto exit;
+		}
+		exp_fhandler->exp_fn = exp_fn;
+		exp_fhandler->insert = true;
+		exp_fhandler->remove = false;
+		list_add_tail(&exp_fhandler->link, &exp_data.list);
+	} else if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link) {
+			if (exp_fhandler->exp_fn->fn_type == exp_fn->fn_type) {
+				exp_fhandler->insert = false;
+				exp_fhandler->remove = true;
+				goto exit;
+			}
+		}
+	}
+
+exit:
+	mutex_unlock(&exp_data.mutex);
+
+	if (exp_data.queue_work) {
+		queue_delayed_work(exp_data.workqueue,
+				&exp_data.work,
+				msecs_to_jiffies(EXP_FN_WORK_DELAY_MS));
+	}
+}
+EXPORT_SYMBOL(synaptics_rmi4_new_function);
+
+#ifdef CONFIG_DRM
+static void synaptics_register_for_panel_events(
+		struct synaptics_rmi4_data *rmi4_data, struct device *dev)
+{
+	void *cookie;
+
+	cookie = panel_event_notifier_register(
+			PANEL_EVENT_NOTIFICATION_PRIMARY,
+			PANEL_EVENT_NOTIFIER_CLIENT_PRIMARY_TOUCH,
+			active_panel,
+			&synaptics_rmi4_dsi_panel_notifier_cb,
+			&rmi4_data->fb_notifier);
+	if (!cookie) {
+		pr_err("Failed to register for panel events\n");
+		return;
+	}
+
+	pr_debug("Registered for panel notifications on panel: 0x%x\n",
+			active_panel);
+	rmi4_data->notifier_cookie = cookie;
+
+}
+#endif
+
+static int synaptics_rmi4_probe(struct platform_device *pdev)
+{
+	int retval = 0;
+	struct synaptics_rmi4_data *rmi4_data;
+	const struct synaptics_dsx_hw_interface *hw_if;
+	const struct synaptics_dsx_board_data *bdata;
+
+	hw_if = pdev->dev.platform_data;
+	if (!hw_if) {
+		dev_err(&pdev->dev,
+				"%s: No hardware interface found\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	bdata = hw_if->board_data;
+	if (!bdata) {
+		dev_err(&pdev->dev,
+				"%s: No board data found\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	rmi4_data = kzalloc(sizeof(*rmi4_data), GFP_KERNEL);
+	if (!rmi4_data) {
+		dev_err(&pdev->dev,
+				"%s: Failed to alloc mem for rmi4_data\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	rmi4_data->pdev = pdev;
+	rmi4_data->current_page = MASK_8BIT;
+	rmi4_data->hw_if = hw_if;
+	rmi4_data->suspend = false;
+	rmi4_data->irq_enabled = false;
+	rmi4_data->fingers_on_2d = false;
+
+	rmi4_data->reset_device = synaptics_rmi4_reset_device;
+	rmi4_data->irq_enable = synaptics_rmi4_irq_enable;
+	rmi4_data->sleep_enable = synaptics_rmi4_sleep_enable;
+	rmi4_data->report_touch = synaptics_rmi4_report_touch;
+
+	mutex_init(&(rmi4_data->rmi4_reset_mutex));
+	mutex_init(&(rmi4_data->rmi4_report_mutex));
+	mutex_init(&(rmi4_data->rmi4_io_ctrl_mutex));
+	mutex_init(&(rmi4_data->rmi4_exp_init_mutex));
+	mutex_init(&(rmi4_data->rmi4_irq_enable_mutex));
+
+	platform_set_drvdata(pdev, rmi4_data);
+
+	vir_button_map = bdata->vir_button_map;
+
+	rmi4_data->initialized = false;
+
+#ifdef CONFIG_DRM
+	if (active_panel) {
+		pr_debug("panel detected, registering notifier\n");
+		synaptics_register_for_panel_events(rmi4_data,
+			&pdev->dev);
+	} else {
+		pr_debug("panel not detected, freeing data\n");
+		retval = -1;
+		goto err_drm_reg;
+	}
+#endif
+	rmi4_data->rmi4_probe_wq = create_singlethread_workqueue(
+						"Synaptics_rmi4_probe_wq");
+	if (!rmi4_data->rmi4_probe_wq) {
+		dev_err(&pdev->dev,
+				"%s: Failed to create probe workqueue\n",
+				__func__);
+		goto err_probe_wq;
+	}
+	INIT_WORK(&rmi4_data->rmi4_probe_work, synaptics_rmi4_defer_probe);
+	queue_work(rmi4_data->rmi4_probe_wq, &rmi4_data->rmi4_probe_work);
+
+	return retval;
+
+err_probe_wq:
+#ifdef CONFIG_DRM
+	if (active_panel && rmi4_data->notifier_cookie) {
+		pr_debug("unregistering panel notifications\n");
+		panel_event_notifier_unregister(
+				&rmi4_data->notifier_cookie);
+	}
+#endif
+err_drm_reg:
+	kfree(rmi4_data);
+
+	return retval;
+}
+
+static void synaptics_rmi4_defer_probe(struct work_struct *work)
+{
+	int retval;
+	unsigned char attr_count;
+	struct synaptics_rmi4_data *rmi4_data = container_of(work,
+				struct synaptics_rmi4_data, rmi4_probe_work);
+	struct platform_device *pdev;
+	const struct synaptics_dsx_hw_interface *hw_if;
+	const struct synaptics_dsx_board_data *bdata;
+
+	pdev = rmi4_data->pdev;
+	hw_if = rmi4_data->hw_if;
+	bdata = hw_if->board_data;
+
+	init_completion(&rmi4_data->drm_init_done);
+	retval = wait_for_completion_interruptible(&rmi4_data->drm_init_done);
+	if (retval < 0) {
+		dev_err(&pdev->dev,
+				"%s: Wait for DRM init was interrupted\n",
+				__func__);
+		goto err_drm_init_wait;
+	}
+
+	retval = synaptics_rmi4_get_reg(rmi4_data, true);
+	if (retval < 0) {
+		dev_err(&pdev->dev,
+				"%s: Failed to get regulators\n",
+				__func__);
+		goto err_get_reg;
+	}
+
+	retval = synaptics_rmi4_enable_reg(rmi4_data, true);
+	if (retval < 0) {
+		dev_err(&pdev->dev,
+				"%s: Failed to enable regulators\n",
+				__func__);
+		goto err_enable_reg;
+	}
+
+	retval = synaptics_rmi4_set_gpio(rmi4_data);
+	if (retval < 0) {
+		dev_err(&pdev->dev,
+				"%s: Failed to set up GPIO's\n",
+				__func__);
+		goto err_set_gpio;
+	}
+
+	retval = synaptics_dsx_pinctrl_init(rmi4_data);
+	if (!retval && rmi4_data->ts_pinctrl) {
+		/*
+		 * Pinctrl handle is optional.
+		 * If pinctrl handle is found let pins to be
+		 * configured in active state. If not found continue
+		 * further without error.
+		 */
+		retval = pinctrl_select_state(rmi4_data->ts_pinctrl,
+				rmi4_data->pinctrl_state_active);
+		if (retval < 0) {
+			dev_err(&pdev->dev,
+				"%s: Failed to select %s pinstate %d\n",
+				__func__, PINCTRL_STATE_ACTIVE, retval);
+		}
+	}
+
+	if (hw_if->ui_hw_init) {
+		retval = hw_if->ui_hw_init(rmi4_data);
+		if (retval < 0) {
+			dev_err(&pdev->dev,
+					"%s: Failed to initialize hardware interface\n",
+					__func__);
+			goto err_ui_hw_init;
+		}
+	}
+
+	retval = synaptics_rmi4_set_input_dev(rmi4_data);
+	if (retval < 0) {
+		dev_err(&pdev->dev,
+				"%s: Failed to set up input device\n",
+				__func__);
+		goto err_set_input_dev;
+	}
+
+#ifdef USE_EARLYSUSPEND
+	rmi4_data->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	rmi4_data->early_suspend.suspend = synaptics_rmi4_early_suspend;
+	rmi4_data->early_suspend.resume = synaptics_rmi4_late_resume;
+	register_early_suspend(&rmi4_data->early_suspend);
+#endif
+
+	if (!exp_data.initialized) {
+		mutex_init(&exp_data.mutex);
+		INIT_LIST_HEAD(&exp_data.list);
+		exp_data.initialized = true;
+	}
+
+	rmi4_data->irq = gpio_to_irq(bdata->irq_gpio);
+
+	retval = synaptics_rmi4_irq_enable(rmi4_data, true, false);
+	if (retval < 0) {
+		dev_err(&pdev->dev,
+				"%s: Failed to enable attention interrupt\n",
+				__func__);
+		goto err_enable_irq;
+	}
+
+	if (vir_button_map->nbuttons) {
+		rmi4_data->board_prop_dir = kobject_create_and_add(
+				"board_properties", NULL);
+		if (!rmi4_data->board_prop_dir) {
+			dev_err(&pdev->dev,
+					"%s: Failed to create board_properties directory\n",
+					__func__);
+			goto err_virtual_buttons;
+		} else {
+			retval = sysfs_create_file(rmi4_data->board_prop_dir,
+					&virtual_key_map_attr.attr);
+			if (retval < 0) {
+				dev_err(&pdev->dev,
+						"%s: Failed to create virtual key map file\n",
+						__func__);
+				goto err_virtual_buttons;
+			}
+		}
+	}
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		retval = sysfs_create_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+		if (retval < 0) {
+			dev_err(&pdev->dev,
+					"%s: Failed to create sysfs attributes\n",
+					__func__);
+			goto err_sysfs;
+		}
+	}
+
+#ifdef USE_DATA_SERVER
+	memset(&interrupt_signal, 0, sizeof(interrupt_signal));
+	interrupt_signal.si_signo = SIGIO;
+	interrupt_signal.si_code = SI_USER;
+#endif
+
+	rmi4_data->rb_workqueue =
+			create_singlethread_workqueue("dsx_rebuild_workqueue");
+	INIT_DELAYED_WORK(&rmi4_data->rb_work, synaptics_rmi4_rebuild_work);
+
+	exp_data.workqueue = create_singlethread_workqueue("dsx_exp_workqueue");
+	INIT_DELAYED_WORK(&exp_data.work, synaptics_rmi4_exp_fn_work);
+	exp_data.rmi4_data = rmi4_data;
+	exp_data.queue_work = true;
+	queue_delayed_work(exp_data.workqueue,
+			&exp_data.work,
+			0);
+
+#ifdef FB_READY_RESET
+	rmi4_data->reset_workqueue =
+			create_singlethread_workqueue("dsx_reset_workqueue");
+	INIT_WORK(&rmi4_data->reset_work, synaptics_rmi4_reset_work);
+	queue_work(rmi4_data->reset_workqueue, &rmi4_data->reset_work);
+#endif
+	rmi4_data->initialized = true;
+
+	return;
+
+err_sysfs:
+	for (attr_count--; attr_count >= 0; attr_count--) {
+		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+err_virtual_buttons:
+	if (rmi4_data->board_prop_dir) {
+		sysfs_remove_file(rmi4_data->board_prop_dir,
+				&virtual_key_map_attr.attr);
+		kobject_put(rmi4_data->board_prop_dir);
+	}
+
+	synaptics_rmi4_irq_enable(rmi4_data, false, false);
+
+err_enable_irq:
+#ifdef USE_EARLYSUSPEND
+	unregister_early_suspend(&rmi4_data->early_suspend);
+#endif
+
+	synaptics_rmi4_empty_fn_list(rmi4_data);
+	input_unregister_device(rmi4_data->input_dev);
+	rmi4_data->input_dev = NULL;
+	if (rmi4_data->stylus_enable) {
+		input_unregister_device(rmi4_data->stylus_dev);
+		rmi4_data->stylus_dev = NULL;
+	}
+
+err_set_input_dev:
+	synaptics_rmi4_gpio_setup(bdata->irq_gpio, false, 0, 0);
+
+	if (bdata->reset_gpio >= 0)
+		synaptics_rmi4_gpio_setup(bdata->reset_gpio, false, 0, 0);
+
+	if (bdata->power_gpio >= 0)
+		synaptics_rmi4_gpio_setup(bdata->power_gpio, false, 0, 0);
+
+err_ui_hw_init:
+err_set_gpio:
+	synaptics_rmi4_enable_reg(rmi4_data, false);
+	synaptics_rmi4_regulator_configure(rmi4_data, false);
+
+	if (rmi4_data->ts_pinctrl) {
+		if (IS_ERR_OR_NULL(rmi4_data->pinctrl_state_release)) {
+			devm_pinctrl_put(rmi4_data->ts_pinctrl);
+			rmi4_data->ts_pinctrl = NULL;
+		} else {
+			if (pinctrl_select_state(
+					rmi4_data->ts_pinctrl,
+					rmi4_data->pinctrl_state_release))
+				dev_err(&pdev->dev,
+					"%s: Failed to select %s pinstate\n",
+					__func__, PINCTRL_STATE_RELEASE);
+		}
+	}
+
+err_enable_reg:
+	synaptics_rmi4_get_reg(rmi4_data, false);
+
+err_get_reg:
+err_drm_init_wait:
+#ifdef CONFIG_DRM
+	if (active_panel && rmi4_data->notifier_cookie) {
+		pr_debug("unregistering panel_events\n");
+		panel_event_notifier_unregister(&rmi4_data->notifier_cookie);
+	}
+#endif
+	cancel_work_sync(&rmi4_data->rmi4_probe_work);
+	destroy_workqueue(rmi4_data->rmi4_probe_wq);
+	kfree(rmi4_data);
+}
+
+static int synaptics_rmi4_remove(struct platform_device *pdev)
+{
+	unsigned char attr_count;
+	struct synaptics_rmi4_data *rmi4_data = platform_get_drvdata(pdev);
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+#ifdef FB_READY_RESET
+	cancel_work_sync(&rmi4_data->reset_work);
+	flush_workqueue(rmi4_data->reset_workqueue);
+	destroy_workqueue(rmi4_data->reset_workqueue);
+#endif
+
+	cancel_delayed_work_sync(&exp_data.work);
+	flush_workqueue(exp_data.workqueue);
+	destroy_workqueue(exp_data.workqueue);
+
+	cancel_delayed_work_sync(&rmi4_data->rb_work);
+	flush_workqueue(rmi4_data->rb_workqueue);
+	destroy_workqueue(rmi4_data->rb_workqueue);
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+	if (rmi4_data->board_prop_dir) {
+		sysfs_remove_file(rmi4_data->board_prop_dir,
+				&virtual_key_map_attr.attr);
+		kobject_put(rmi4_data->board_prop_dir);
+	}
+
+	synaptics_rmi4_irq_enable(rmi4_data, false, false);
+
+#ifdef CONFIG_DRM
+	if (active_panel) {
+		pr_debug("unregistering panel_events\n");
+		panel_event_notifier_unregister(&rmi4_data->notifier_cookie);
+	}
+#endif
+
+#ifdef USE_EARLYSUSPEND
+	unregister_early_suspend(&rmi4_data->early_suspend);
+#endif
+
+	synaptics_rmi4_empty_fn_list(rmi4_data);
+	input_unregister_device(rmi4_data->input_dev);
+	rmi4_data->input_dev = NULL;
+	if (rmi4_data->stylus_enable) {
+		input_unregister_device(rmi4_data->stylus_dev);
+		rmi4_data->stylus_dev = NULL;
+	}
+
+	synaptics_rmi4_gpio_setup(bdata->irq_gpio, false, 0, 0);
+
+	if (bdata->reset_gpio >= 0)
+		synaptics_rmi4_gpio_setup(bdata->reset_gpio, false, 0, 0);
+
+	if (bdata->power_gpio >= 0)
+		synaptics_rmi4_gpio_setup(bdata->power_gpio, false, 0, 0);
+
+	if (rmi4_data->ts_pinctrl) {
+			if (IS_ERR_OR_NULL(rmi4_data->pinctrl_state_release)) {
+				devm_pinctrl_put(rmi4_data->ts_pinctrl);
+				rmi4_data->ts_pinctrl = NULL;
+			} else {
+				pinctrl_select_state(
+					rmi4_data->ts_pinctrl,
+					rmi4_data->pinctrl_state_release);
+			}
+		}
+
+	synaptics_rmi4_enable_reg(rmi4_data, false);
+	synaptics_rmi4_regulator_configure(rmi4_data, false);
+	synaptics_rmi4_get_reg(rmi4_data, false);
+
+	cancel_work_sync(&rmi4_data->rmi4_probe_work);
+	destroy_workqueue(rmi4_data->rmi4_probe_wq);
+
+	kfree(rmi4_data);
+
+	return 0;
+}
+
+#ifdef CONFIG_DRM
+static void synaptics_rmi4_dsi_panel_notifier_cb(
+		enum panel_event_notifier_tag tag,
+		struct panel_event_notification *notification,
+		void *client_data)
+{
+	struct synaptics_rmi4_data *rmi4_data =
+			container_of(client_data, struct synaptics_rmi4_data,
+			fb_notifier);
+
+	if (!notification) {
+		pr_err("Invalid notification\n");
+		return;
+	}
+	pr_debug("Notification type: %d, early trigger:%d\n",
+			notification->notif_type,
+			notification->notif_data.early_trigger);
+
+	if (!client_data) {
+		pr_err("No client data\n");
+		return;
+	}
+
+	switch (notification->notif_type) {
+	case DRM_PANEL_EVENT_UNBLANK:
+		if (rmi4_data->initialized) {
+			synaptics_rmi4_regulator_configure(rmi4_data, true);
+			synaptics_rmi4_resume(&rmi4_data->pdev->dev);
+		} else {
+			complete(&rmi4_data->drm_init_done);
+		}
+		rmi4_data->fb_ready = true;
+		break;
+	case DRM_PANEL_EVENT_BLANK:
+		if (rmi4_data->initialized) {
+			synaptics_rmi4_suspend(&rmi4_data->pdev->dev);
+			synaptics_rmi4_regulator_configure(rmi4_data, false);
+		}
+		rmi4_data->fb_ready = false;
+		break;
+	case DRM_PANEL_EVENT_BLANK_LP:
+		pr_debug("received lp event\n");
+		break;
+	case DRM_PANEL_EVENT_FPS_CHANGE:
+		pr_debug("received fps change old fps:%d new fps:%d\n",
+			notification->notif_data.old_fps,
+			notification->notif_data.new_fps);
+		break;
+	default:
+		pr_debug("notification serviced :%d\n",
+			notification->notif_type);
+		break;
+	}
+
+}
+#endif
+
+#ifdef USE_EARLYSUSPEND
+static int synaptics_rmi4_early_suspend(struct early_suspend *h)
+{
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct synaptics_rmi4_data *rmi4_data =
+			container_of(h, struct synaptics_rmi4_data,
+			early_suspend);
+	unsigned char device_ctrl;
+
+	if (rmi4_data->stay_awake)
+		return retval;
+
+	if (rmi4_data->enable_wakeup_gesture) {
+		if (rmi4_data->no_sleep_setting) {
+			synaptics_rmi4_reg_read(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr,
+					&device_ctrl,
+					sizeof(device_ctrl));
+			device_ctrl = device_ctrl & ~NO_SLEEP_ON;
+			synaptics_rmi4_reg_write(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr,
+					&device_ctrl,
+					sizeof(device_ctrl));
+		}
+		synaptics_rmi4_wakeup_gesture(rmi4_data, true);
+		enable_irq_wake(rmi4_data->irq);
+		goto exit;
+	}
+
+#ifdef SYNA_TDDI
+	if (rmi4_data->no_sleep_setting) {
+		synaptics_rmi4_reg_read(rmi4_data,
+				rmi4_data->f01_ctrl_base_addr,
+				&device_ctrl,
+				sizeof(device_ctrl));
+		device_ctrl = device_ctrl & ~NO_SLEEP_ON;
+		synaptics_rmi4_reg_write(rmi4_data,
+				rmi4_data->f01_ctrl_base_addr,
+				&device_ctrl,
+				sizeof(device_ctrl));
+	}
+	synaptics_rmi4_wakeup_gesture(rmi4_data, true);
+	usleep(TDDI_LPWG_WAIT_US);
+#endif
+	synaptics_rmi4_irq_enable(rmi4_data, false, false);
+	synaptics_rmi4_sleep_enable(rmi4_data, true);
+	synaptics_rmi4_free_fingers(rmi4_data);
+
+exit:
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->early_suspend != NULL)
+				exp_fhandler->exp_fn->early_suspend(rmi4_data);
+	}
+	mutex_unlock(&exp_data.mutex);
+
+	rmi4_data->suspend = true;
+
+	return retval;
+}
+
+static int synaptics_rmi4_late_resume(struct early_suspend *h)
+{
+#ifdef FB_READY_RESET
+	int retval;
+#endif
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct synaptics_rmi4_data *rmi4_data =
+			container_of(h, struct synaptics_rmi4_data,
+			early_suspend);
+
+	if (rmi4_data->stay_awake)
+		return retval;
+
+	if (rmi4_data->enable_wakeup_gesture) {
+		disable_irq_wake(rmi4_data->irq);
+		goto exit;
+	}
+
+	rmi4_data->current_page = MASK_8BIT;
+
+	if (rmi4_data->suspend) {
+		synaptics_rmi4_sleep_enable(rmi4_data, false);
+		synaptics_rmi4_irq_enable(rmi4_data, true, false);
+	}
+
+exit:
+#ifdef FB_READY_RESET
+	if (rmi4_data->suspend) {
+		retval = synaptics_rmi4_reset_device(rmi4_data, false);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to issue reset command\n",
+					__func__);
+		}
+	}
+#endif
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->late_resume != NULL)
+				exp_fhandler->exp_fn->late_resume(rmi4_data);
+	}
+	mutex_unlock(&exp_data.mutex);
+
+	rmi4_data->suspend = false;
+
+	return retval;
+}
+#endif
+
+static int synaptics_rmi4_suspend(struct device *dev)
+{
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+	unsigned char device_ctrl;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	if (rmi4_data->stay_awake)
+		return 0;
+
+	if (rmi4_data->enable_wakeup_gesture) {
+		if (rmi4_data->no_sleep_setting) {
+			synaptics_rmi4_reg_read(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr,
+					&device_ctrl,
+					sizeof(device_ctrl));
+			device_ctrl = device_ctrl & ~NO_SLEEP_ON;
+			synaptics_rmi4_reg_write(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr,
+					&device_ctrl,
+					sizeof(device_ctrl));
+		}
+		synaptics_rmi4_wakeup_gesture(rmi4_data, true);
+		enable_irq_wake(rmi4_data->irq);
+		goto exit;
+	}
+
+	if (!rmi4_data->suspend) {
+#ifdef SYNA_TDDI
+		if (rmi4_data->no_sleep_setting) {
+			synaptics_rmi4_reg_read(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr,
+					&device_ctrl,
+					sizeof(device_ctrl));
+			device_ctrl = device_ctrl & ~NO_SLEEP_ON;
+			synaptics_rmi4_reg_write(rmi4_data,
+					rmi4_data->f01_ctrl_base_addr,
+					&device_ctrl,
+					sizeof(device_ctrl));
+		}
+		synaptics_rmi4_wakeup_gesture(rmi4_data, true);
+		usleep(TDDI_LPWG_WAIT_US);
+#endif
+		synaptics_rmi4_irq_enable(rmi4_data, false, false);
+		synaptics_rmi4_sleep_enable(rmi4_data, true);
+		synaptics_rmi4_free_fingers(rmi4_data);
+	}
+
+	if (bdata->reset_gpio >= 0) {
+		gpio_set_value(bdata->reset_gpio, bdata->reset_on_state);
+		msleep(bdata->reset_active_ms);
+	}
+
+	synaptics_rmi4_enable_reg(rmi4_data, false);
+
+exit:
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->suspend != NULL)
+				exp_fhandler->exp_fn->suspend(rmi4_data);
+	}
+	mutex_unlock(&exp_data.mutex);
+
+	rmi4_data->suspend = true;
+
+	return 0;
+}
+
+static int synaptics_rmi4_resume(struct device *dev)
+{
+#ifdef FB_READY_RESET
+	int retval;
+#endif
+	struct synaptics_rmi4_exp_fhandler *exp_fhandler;
+	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
+
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+	if (rmi4_data->stay_awake)
+		return 0;
+
+	if (rmi4_data->enable_wakeup_gesture) {
+		disable_irq_wake(rmi4_data->irq);
+		synaptics_rmi4_wakeup_gesture(rmi4_data, false);
+		goto exit;
+	}
+
+	synaptics_rmi4_enable_reg(rmi4_data, true);
+
+	if (bdata->reset_gpio >= 0) {
+		gpio_set_value(bdata->reset_gpio, bdata->reset_on_state);
+		msleep(bdata->reset_active_ms);
+		gpio_set_value(bdata->reset_gpio, !bdata->reset_on_state);
+		msleep(bdata->reset_delay_ms);
+	}
+
+
+	rmi4_data->current_page = MASK_8BIT;
+
+	synaptics_rmi4_sleep_enable(rmi4_data, false);
+	synaptics_rmi4_irq_enable(rmi4_data, true, false);
+
+exit:
+#ifdef FB_READY_RESET
+	retval = synaptics_rmi4_reset_device(rmi4_data, false);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to issue reset command\n",
+				__func__);
+	}
+#endif
+	mutex_lock(&exp_data.mutex);
+	if (!list_empty(&exp_data.list)) {
+		list_for_each_entry(exp_fhandler, &exp_data.list, link)
+			if (exp_fhandler->exp_fn->resume != NULL)
+				exp_fhandler->exp_fn->resume(rmi4_data);
+	}
+	mutex_unlock(&exp_data.mutex);
+
+	rmi4_data->suspend = false;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static const struct dev_pm_ops synaptics_rmi4_dev_pm_ops = {
+	.suspend = synaptics_rmi4_suspend,
+	.resume = synaptics_rmi4_resume,
+};
+#endif
+
+static struct platform_driver synaptics_rmi4_driver = {
+	.driver = {
+		.name = PLATFORM_DRIVER_NAME,
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &synaptics_rmi4_dev_pm_ops,
+#endif
+	},
+	.probe = synaptics_rmi4_probe,
+	.remove = synaptics_rmi4_remove,
+};
+
+static int __init synaptics_rmi4_init(void)
+{
+	int retval;
+
+	retval = synaptics_rmi4_bus_init();
+	if (retval)
+		return retval;
+
+	return platform_driver_register(&synaptics_rmi4_driver);
+}
+
+static void __exit synaptics_rmi4_exit(void)
+{
+	platform_driver_unregister(&synaptics_rmi4_driver);
+
+	synaptics_rmi4_bus_exit();
+}
+
+module_init(synaptics_rmi4_init);
+module_exit(synaptics_rmi4_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX Touch Driver");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_core.h

@@ -0,0 +1,536 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#ifndef _SYNAPTICS_DSX_RMI4_H_
+#define _SYNAPTICS_DSX_RMI4_H_
+
+#define SYNAPTICS_DS4 (1 << 0)
+#define SYNAPTICS_DS5 (1 << 1)
+#define SYNAPTICS_DSX_DRIVER_PRODUCT (SYNAPTICS_DS4 | SYNAPTICS_DS5)
+#define SYNAPTICS_DSX_DRIVER_VERSION 0x2070
+
+#include <linux/version.h>
+#ifdef CONFIG_FB
+#include <linux/notifier.h>
+#include <linux/fb.h>
+#endif
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#include <drm/drm_panel.h>
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 38))
+#define KERNEL_ABOVE_2_6_38
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0))
+#define KERNEL_ABOVE_3_6
+#endif
+
+#ifdef KERNEL_ABOVE_2_6_38
+#define sstrtoul(...) kstrtoul(__VA_ARGS__)
+#else
+#define sstrtoul(...) strict_strtoul(__VA_ARGS__)
+#endif
+
+#define PDT_PROPS (0X00EF)
+#define PDT_START (0x00E9)
+#define PDT_END (0x00D0)
+#define PDT_ENTRY_SIZE (0x0006)
+#define PAGES_TO_SERVICE (10)
+#define PAGE_SELECT_LEN (2)
+#define ADDRESS_LEN (2)
+
+#define SYNAPTICS_RMI4_F01 (0x01)
+#define SYNAPTICS_RMI4_F11 (0x11)
+#define SYNAPTICS_RMI4_F12 (0x12)
+#define SYNAPTICS_RMI4_F1A (0x1A)
+#define SYNAPTICS_RMI4_F21 (0x21)
+#define SYNAPTICS_RMI4_F34 (0x34)
+#define SYNAPTICS_RMI4_F35 (0x35)
+#define SYNAPTICS_RMI4_F38 (0x38)
+#define SYNAPTICS_RMI4_F51 (0x51)
+#define SYNAPTICS_RMI4_F54 (0x54)
+#define SYNAPTICS_RMI4_F55 (0x55)
+#define SYNAPTICS_RMI4_FDB (0xDB)
+
+#define PRODUCT_INFO_SIZE 2
+#define PRODUCT_ID_SIZE 10
+#define BUILD_ID_SIZE 3
+
+#define F12_FINGERS_TO_SUPPORT 10
+#define F12_NO_OBJECT_STATUS 0x00
+#define F12_FINGER_STATUS 0x01
+#define F12_ACTIVE_STYLUS_STATUS 0x02
+#define F12_PALM_STATUS 0x03
+#define F12_HOVERING_FINGER_STATUS 0x05
+#define F12_GLOVED_FINGER_STATUS 0x06
+#define F12_NARROW_OBJECT_STATUS 0x07
+#define F12_HAND_EDGE_STATUS 0x08
+#define F12_COVER_STATUS 0x0A
+#define F12_STYLUS_STATUS 0x0B
+#define F12_ERASER_STATUS 0x0C
+#define F12_SMALL_OBJECT_STATUS 0x0D
+
+#define F12_GESTURE_DETECTION_LEN 5
+
+#define MAX_NUMBER_OF_BUTTONS 4
+#define MAX_INTR_REGISTERS 4
+
+#define MASK_16BIT 0xFFFF
+#define MASK_8BIT 0xFF
+#define MASK_7BIT 0x7F
+#define MASK_6BIT 0x3F
+#define MASK_5BIT 0x1F
+#define MASK_4BIT 0x0F
+#define MASK_3BIT 0x07
+#define MASK_2BIT 0x03
+#define MASK_1BIT 0x01
+
+#define PINCTRL_STATE_ACTIVE    "pmx_ts_active"
+#define PINCTRL_STATE_SUSPEND   "pmx_ts_suspend"
+#define PINCTRL_STATE_RELEASE   "pmx_ts_release"
+
+enum exp_fn {
+	RMI_DEV = 0,
+	RMI_FW_UPDATER,
+	RMI_TEST_REPORTING,
+	RMI_PROXIMITY,
+	RMI_ACTIVE_PEN,
+	RMI_GESTURE,
+	RMI_VIDEO,
+	RMI_DEBUG,
+	RMI_LAST,
+};
+
+extern struct drm_panel *active_panel;
+
+/*
+ * struct synaptics_rmi4_fn_desc - function descriptor fields in PDT entry
+ * @query_base_addr: base address for query registers
+ * @cmd_base_addr: base address for command registers
+ * @ctrl_base_addr: base address for control registers
+ * @data_base_addr: base address for data registers
+ * @intr_src_count: number of interrupt sources
+ * @fn_version: version of function
+ * @fn_number: function number
+ */
+struct synaptics_rmi4_fn_desc {
+	union {
+		struct {
+			unsigned char query_base_addr;
+			unsigned char cmd_base_addr;
+			unsigned char ctrl_base_addr;
+			unsigned char data_base_addr;
+			unsigned char intr_src_count:3;
+			unsigned char reserved_1:2;
+			unsigned char fn_version:2;
+			unsigned char reserved_2:1;
+			unsigned char fn_number;
+		} __packed;
+		unsigned char data[6];
+	};
+};
+
+/*
+ * synaptics_rmi4_fn_full_addr - full 16-bit base addresses
+ * @query_base: 16-bit base address for query registers
+ * @cmd_base: 16-bit base address for command registers
+ * @ctrl_base: 16-bit base address for control registers
+ * @data_base: 16-bit base address for data registers
+ */
+struct synaptics_rmi4_fn_full_addr {
+	unsigned short query_base;
+	unsigned short cmd_base;
+	unsigned short ctrl_base;
+	unsigned short data_base;
+};
+
+/*
+ * struct synaptics_rmi4_f11_extra_data - extra data of F$11
+ * @data38_offset: offset to F11_2D_DATA38 register
+ */
+struct synaptics_rmi4_f11_extra_data {
+	unsigned char data38_offset;
+};
+
+/*
+ * struct synaptics_rmi4_f12_extra_data - extra data of F$12
+ * @data1_offset: offset to F12_2D_DATA01 register
+ * @data4_offset: offset to F12_2D_DATA04 register
+ * @data15_offset: offset to F12_2D_DATA15 register
+ * @data15_size: size of F12_2D_DATA15 register
+ * @data15_data: buffer for reading F12_2D_DATA15 register
+ * @data29_offset: offset to F12_2D_DATA29 register
+ * @data29_size: size of F12_2D_DATA29 register
+ * @data29_data: buffer for reading F12_2D_DATA29 register
+ * @ctrl20_offset: offset to F12_2D_CTRL20 register
+ */
+struct synaptics_rmi4_f12_extra_data {
+	unsigned char data1_offset;
+	unsigned char data4_offset;
+	unsigned char data15_offset;
+	unsigned char data15_size;
+	unsigned char data15_data[(F12_FINGERS_TO_SUPPORT + 7) / 8];
+	unsigned char data29_offset;
+	unsigned char data29_size;
+	unsigned char data29_data[F12_FINGERS_TO_SUPPORT * 2];
+	unsigned char ctrl20_offset;
+};
+
+/*
+ * struct synaptics_rmi4_fn - RMI function handler
+ * @fn_number: function number
+ * @num_of_data_sources: number of data sources
+ * @num_of_data_points: maximum number of fingers supported
+ * @intr_reg_num: index to associated interrupt register
+ * @intr_mask: interrupt mask
+ * @full_addr: full 16-bit base addresses of function registers
+ * @link: linked list for function handlers
+ * @data_size: size of private data
+ * @data: pointer to private data
+ * @extra: pointer to extra data
+ */
+struct synaptics_rmi4_fn {
+	unsigned char fn_number;
+	unsigned char num_of_data_sources;
+	unsigned char num_of_data_points;
+	unsigned char intr_reg_num;
+	unsigned char intr_mask;
+	struct synaptics_rmi4_fn_full_addr full_addr;
+	struct list_head link;
+	int data_size;
+	void *data;
+	void *extra;
+};
+
+/*
+ * struct synaptics_rmi4_input_settings - current input settings
+ * @num_of_fingers: maximum number of fingers for 2D touch
+ * @valid_button_count: number of valid 0D buttons
+ * @max_touch_width: maximum touch width
+ * @sensor_max_x: maximum x coordinate for 2D touch
+ * @sensor_max_y: maximum y coordinate for 2D touch
+ * @force_min: minimum force value
+ * @force_max: maximum force value
+ * @stylus_enable: flag to indicate reporting of stylus data
+ * @eraser_enable: flag to indicate reporting of eraser data
+ */
+struct synaptics_rmi4_input_settings {
+	unsigned char num_of_fingers;
+	unsigned char valid_button_count;
+	unsigned char max_touch_width;
+	int sensor_max_x;
+	int sensor_max_y;
+	int force_min;
+	int force_max;
+	bool stylus_enable;
+	bool eraser_enable;
+};
+
+/*
+ * struct synaptics_rmi4_device_info - device information
+ * @version_major: RMI protocol major version number
+ * @version_minor: RMI protocol minor version number
+ * @manufacturer_id: manufacturer ID
+ * @product_props: product properties
+ * @product_info: product information
+ * @product_id_string: product ID
+ * @build_id: firmware build ID
+ * @support_fn_list: linked list for function handlers
+ */
+struct synaptics_rmi4_device_info {
+	unsigned int version_major;
+	unsigned int version_minor;
+	unsigned char manufacturer_id;
+	unsigned char product_props;
+	unsigned char product_info[PRODUCT_INFO_SIZE];
+	unsigned char product_id_string[PRODUCT_ID_SIZE + 1];
+	unsigned char build_id[BUILD_ID_SIZE];
+	struct list_head support_fn_list;
+};
+
+/*
+ * struct synaptics_rmi4_data - RMI4 device instance data
+ * @pdev: pointer to platform device
+ * @input_dev: pointer to associated input device
+ * @stylus_dev: pointer to associated stylus device
+ * @hw_if: pointer to hardware interface data
+ * @rmi4_mod_info: device information
+ * @board_prop_dir: /sys/board_properties directory for virtual key map file
+ * @pwr_reg: pointer to regulator for power control
+ * @bus_reg: pointer to regulator for bus pullup control
+ * @rmi4_reset_mutex: mutex for software reset
+ * @rmi4_report_mutex: mutex for input event reporting
+ * @rmi4_io_ctrl_mutex: mutex for communication interface I/O
+ * @rmi4_exp_init_mutex: mutex for expansion function module initialization
+ * @rmi4_irq_enable_mutex: mutex for enabling/disabling interrupt
+ * @rb_work: work for rebuilding input device
+ * @rb_workqueue: workqueue for rebuilding input device
+ * @fb_notifier: framebuffer notifier client
+ * @reset_work: work for issuing reset after display framebuffer ready
+ * @reset_workqueue: workqueue for issuing reset after display framebuffer ready
+ * @early_suspend: early suspend power management
+ * @current_page: current RMI page for register access
+ * @button_0d_enabled: switch for enabling 0d button support
+ * @num_of_tx: number of Tx channels for 2D touch
+ * @num_of_rx: number of Rx channels for 2D touch
+ * @num_of_fingers: maximum number of fingers for 2D touch
+ * @max_touch_width: maximum touch width
+ * @valid_button_count: number of valid 0D buttons
+ * @report_enable: input data to report for F$12
+ * @no_sleep_setting: default setting of NoSleep in F01_RMI_CTRL00 register
+ * @gesture_detection: detected gesture type and properties
+ * @intr_mask: interrupt enable mask
+ * @button_txrx_mapping: Tx Rx mapping of 0D buttons
+ * @num_of_intr_regs: number of interrupt registers
+ * @f01_query_base_addr: query base address for f$01
+ * @f01_cmd_base_addr: command base address for f$01
+ * @f01_ctrl_base_addr: control base address for f$01
+ * @f01_data_base_addr: data base address for f$01
+ * @f51_query_base_addr: query base address for f$51
+ * @firmware_id: firmware build ID
+ * @irq: attention interrupt
+ * @sensor_max_x: maximum x coordinate for 2D touch
+ * @sensor_max_y: maximum y coordinate for 2D touch
+ * @force_min: minimum force value
+ * @force_max: maximum force value
+ * @set_wakeup_gesture: location of set wakeup gesture
+ * @flash_prog_mode: flag to indicate flash programming mode status
+ * @irq_enabled: flag to indicate attention interrupt enable status
+ * @fingers_on_2d: flag to indicate presence of fingers in 2D area
+ * @suspend: flag to indicate whether in suspend state
+ * @sensor_sleep: flag to indicate sleep state of sensor
+ * @stay_awake: flag to indicate whether to stay awake during suspend
+ * @fb_ready: flag to indicate whether display framebuffer in ready state
+ * @f11_wakeup_gesture: flag to indicate support for wakeup gestures in F$11
+ * @f12_wakeup_gesture: flag to indicate support for wakeup gestures in F$12
+ * @enable_wakeup_gesture: flag to indicate usage of wakeup gestures
+ * @wedge_sensor: flag to indicate use of wedge sensor
+ * @report_pressure: flag to indicate reporting of pressure data
+ * @stylus_enable: flag to indicate reporting of stylus data
+ * @eraser_enable: flag to indicate reporting of eraser data
+ * @external_afe_buttons: flag to indicate presence of external AFE buttons
+ * @notifier_cookie: touch notifier for panel events
+ * @reset_device: pointer to device reset function
+ * @irq_enable: pointer to interrupt enable function
+ * @sleep_enable: pointer to sleep enable function
+ * @report_touch: pointer to touch reporting function
+ */
+struct synaptics_rmi4_data {
+	bool initialized;
+	struct platform_device *pdev;
+	struct input_dev *input_dev;
+	struct input_dev *stylus_dev;
+	const struct synaptics_dsx_hw_interface *hw_if;
+	struct synaptics_rmi4_device_info rmi4_mod_info;
+	struct synaptics_rmi4_input_settings input_settings;
+	struct kobject *board_prop_dir;
+	struct regulator *pwr_reg;
+	struct regulator *bus_reg;
+	struct mutex rmi4_reset_mutex;
+	struct mutex rmi4_report_mutex;
+	struct mutex rmi4_io_ctrl_mutex;
+	struct mutex rmi4_exp_init_mutex;
+	struct mutex rmi4_irq_enable_mutex;
+	struct delayed_work rb_work;
+	struct workqueue_struct *rb_workqueue;
+	struct work_struct rmi4_probe_work;
+	struct workqueue_struct *rmi4_probe_wq;
+	struct completion drm_init_done;
+	struct pinctrl *ts_pinctrl;
+	struct pinctrl_state *pinctrl_state_active;
+	struct pinctrl_state *pinctrl_state_suspend;
+	struct pinctrl_state *pinctrl_state_release;
+	struct notifier_block fb_notifier;
+	struct work_struct reset_work;
+	struct workqueue_struct *reset_workqueue;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+	unsigned char current_page;
+	unsigned char button_0d_enabled;
+	unsigned char num_of_tx;
+	unsigned char num_of_rx;
+	unsigned char num_of_fingers;
+	unsigned char max_touch_width;
+	unsigned char valid_button_count;
+	unsigned char report_enable;
+	unsigned char no_sleep_setting;
+	unsigned char gesture_detection[F12_GESTURE_DETECTION_LEN];
+	unsigned char intr_mask[MAX_INTR_REGISTERS];
+	unsigned char *button_txrx_mapping;
+	unsigned short num_of_intr_regs;
+	unsigned short f01_query_base_addr;
+	unsigned short f01_cmd_base_addr;
+	unsigned short f01_ctrl_base_addr;
+	unsigned short f01_data_base_addr;
+#ifdef F51_DISCRETE_FORCE
+	unsigned short f51_query_base_addr;
+#endif
+	unsigned int firmware_id;
+	int irq;
+	int sensor_max_x;
+	int sensor_max_y;
+	int force_min;
+	int force_max;
+	int set_wakeup_gesture;
+	int avdd_status;
+	int vdd_status;
+	bool flash_prog_mode;
+	bool irq_enabled;
+	bool fingers_on_2d;
+	bool suspend;
+	bool sensor_sleep;
+	bool stay_awake;
+	bool fb_ready;
+	bool f11_wakeup_gesture;
+	bool f12_wakeup_gesture;
+	bool enable_wakeup_gesture;
+	bool wedge_sensor;
+	bool report_pressure;
+	bool stylus_enable;
+	bool eraser_enable;
+	bool external_afe_buttons;
+	void *notifier_cookie;
+	int (*reset_device)(struct synaptics_rmi4_data *rmi4_data,
+			bool rebuild);
+	int (*irq_enable)(struct synaptics_rmi4_data *rmi4_data, bool enable,
+			bool attn_only);
+	int (*sleep_enable)(struct synaptics_rmi4_data *rmi4_data,
+			bool enable);
+	void (*report_touch)(struct synaptics_rmi4_data *rmi4_data,
+			struct synaptics_rmi4_fn *fhandler);
+};
+
+struct synaptics_dsx_bus_access {
+	unsigned char type;
+	int (*read)(struct synaptics_rmi4_data *rmi4_data, unsigned short addr,
+		unsigned char *data, unsigned int length);
+	int (*write)(struct synaptics_rmi4_data *rmi4_data, unsigned short addr,
+		unsigned char *data, unsigned int length);
+};
+
+struct synaptics_dsx_hw_interface {
+	struct synaptics_dsx_board_data *board_data;
+	const struct synaptics_dsx_bus_access *bus_access;
+	int (*bl_hw_init)(struct synaptics_rmi4_data *rmi4_data);
+	int (*ui_hw_init)(struct synaptics_rmi4_data *rmi4_data);
+};
+
+struct synaptics_rmi4_exp_fn {
+	enum exp_fn fn_type;
+	int (*init)(struct synaptics_rmi4_data *rmi4_data);
+	void (*remove)(struct synaptics_rmi4_data *rmi4_data);
+	void (*reset)(struct synaptics_rmi4_data *rmi4_data);
+	void (*reinit)(struct synaptics_rmi4_data *rmi4_data);
+	void (*early_suspend)(struct synaptics_rmi4_data *rmi4_data);
+	void (*suspend)(struct synaptics_rmi4_data *rmi4_data);
+	void (*resume)(struct synaptics_rmi4_data *rmi4_data);
+	void (*late_resume)(struct synaptics_rmi4_data *rmi4_data);
+	void (*attn)(struct synaptics_rmi4_data *rmi4_data,
+			unsigned char intr_mask);
+};
+
+int synaptics_rmi4_bus_init(void);
+
+void synaptics_rmi4_bus_exit(void);
+
+void synaptics_rmi4_new_function(struct synaptics_rmi4_exp_fn *exp_fn_module,
+		bool insert);
+
+int synaptics_fw_updater(const unsigned char *fw_data);
+
+static inline int synaptics_rmi4_reg_read(
+		struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr,
+		unsigned char *data,
+		unsigned int len)
+{
+	return rmi4_data->hw_if->bus_access->read(rmi4_data, addr, data, len);
+}
+
+static inline int synaptics_rmi4_reg_write(
+		struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr,
+		unsigned char *data,
+		unsigned int len)
+{
+	return rmi4_data->hw_if->bus_access->write(rmi4_data, addr, data, len);
+}
+
+static inline ssize_t synaptics_rmi4_show_error(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	dev_warn(dev, "%s Attempted to read from write-only attribute %s\n",
+			__func__, attr->attr.name);
+	return -EPERM;
+}
+
+static inline ssize_t synaptics_rmi4_store_error(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	dev_warn(dev, "%s Attempted to write to read-only attribute %s\n",
+			__func__, attr->attr.name);
+	return -EPERM;
+}
+
+static inline int secure_memcpy(unsigned char *dest, unsigned int dest_size,
+		const unsigned char *src, unsigned int src_size,
+		unsigned int count)
+{
+	if (dest == NULL || src == NULL)
+		return -EINVAL;
+
+	if (count > dest_size || count > src_size)
+		return -EINVAL;
+
+	memcpy((void *)dest, (const void *)src, count);
+
+	return 0;
+}
+
+static inline void batohs(unsigned short *dest, unsigned char *src)
+{
+	*dest = src[1] * 0x100 + src[0];
+}
+
+static inline void hstoba(unsigned char *dest, unsigned short src)
+{
+	dest[0] = src % 0x100;
+	dest[1] = src / 0x100;
+}
+
+#endif

synaptics_dsx/synaptics_dsx_fw_update.c

@@ -0,0 +1,5797 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/firmware.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define FW_IHEX_NAME "synaptics/startup_fw_update.bin"
+#define FW_IMAGE_NAME "synaptics/startup_fw_update.img"
+
+#define ENABLE_SYS_REFLASH false
+#define FORCE_UPDATE false
+#define DO_LOCKDOWN false
+
+#define MAX_IMAGE_NAME_LEN 256
+#define MAX_FIRMWARE_ID_LEN 10
+
+#define IMAGE_HEADER_VERSION_05 0x05
+#define IMAGE_HEADER_VERSION_06 0x06
+#define IMAGE_HEADER_VERSION_10 0x10
+
+#define IMAGE_AREA_OFFSET 0x100
+#define LOCKDOWN_SIZE 0x50
+
+#define MAX_UTILITY_PARAMS 20
+
+#define V5V6_BOOTLOADER_ID_OFFSET 0
+#define V5V6_CONFIG_ID_SIZE 4
+
+#define V5_PROPERTIES_OFFSET 2
+#define V5_BLOCK_SIZE_OFFSET 3
+#define V5_BLOCK_COUNT_OFFSET 5
+#define V5_BLOCK_NUMBER_OFFSET 0
+#define V5_BLOCK_DATA_OFFSET 2
+
+#define V6_PROPERTIES_OFFSET 1
+#define V6_BLOCK_SIZE_OFFSET 2
+#define V6_BLOCK_COUNT_OFFSET 3
+#define V6_PROPERTIES_2_OFFSET 4
+#define V6_GUEST_CODE_BLOCK_COUNT_OFFSET 5
+#define V6_BLOCK_NUMBER_OFFSET 0
+#define V6_BLOCK_DATA_OFFSET 1
+#define V6_FLASH_COMMAND_OFFSET 2
+#define V6_FLASH_STATUS_OFFSET 3
+
+#define V7_CONFIG_ID_SIZE 32
+
+#define V7_FLASH_STATUS_OFFSET 0
+#define V7_PARTITION_ID_OFFSET 1
+#define V7_BLOCK_NUMBER_OFFSET 2
+#define V7_TRANSFER_LENGTH_OFFSET 3
+#define V7_COMMAND_OFFSET 4
+#define V7_PAYLOAD_OFFSET 5
+
+#define V7_PARTITION_SUPPORT_BYTES 4
+
+#define F35_ERROR_CODE_OFFSET 0
+#define F35_FLASH_STATUS_OFFSET 5
+#define F35_CHUNK_NUM_LSB_OFFSET 0
+#define F35_CHUNK_NUM_MSB_OFFSET 1
+#define F35_CHUNK_DATA_OFFSET 2
+#define F35_CHUNK_COMMAND_OFFSET 18
+
+#define F35_CHUNK_SIZE 16
+#define F35_ERASE_ALL_WAIT_MS 5000
+#define F35_RESET_WAIT_MS 250
+
+#define SLEEP_MODE_NORMAL (0x00)
+#define SLEEP_MODE_SENSOR_SLEEP (0x01)
+#define SLEEP_MODE_RESERVED0 (0x02)
+#define SLEEP_MODE_RESERVED1 (0x03)
+
+#define ENABLE_WAIT_MS (1 * 1000)
+#define WRITE_WAIT_MS (3 * 1000)
+#define ERASE_WAIT_MS (5 * 1000)
+
+#define MIN_SLEEP_TIME_US 50
+#define MAX_SLEEP_TIME_US 100
+
+#define INT_DISABLE_WAIT_MS 20
+#define ENTER_FLASH_PROG_WAIT_MS 20
+#define READ_CONFIG_WAIT_MS 20
+
+static int fwu_do_reflash(void);
+
+static int fwu_recovery_check_status(void);
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+static ssize_t fwu_sysfs_show_image(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static ssize_t fwu_sysfs_store_image(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static ssize_t fwu_sysfs_do_recovery_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_do_reflash_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_write_config_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_read_config_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_config_area_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_image_name_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_image_size_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_block_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_firmware_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_configuration_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_disp_config_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_perm_config_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_bl_config_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_utility_parameter_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_guest_code_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t fwu_sysfs_write_guest_code_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+#ifdef SYNA_TDDI
+static ssize_t fwu_sysfs_write_lockdown_code_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t fwu_sysfs_read_lockdown_code_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+#endif
+
+#endif
+
+enum f34_version {
+	F34_V0 = 0,
+	F34_V1,
+	F34_V2,
+};
+
+enum bl_version {
+	BL_V5 = 5,
+	BL_V6 = 6,
+	BL_V7 = 7,
+	BL_V8 = 8,
+};
+
+enum flash_area {
+	NONE = 0,
+	UI_FIRMWARE,
+	UI_CONFIG,
+};
+
+enum update_mode {
+	NORMAL = 1,
+	FORCE = 2,
+	LOCKDOWN = 8,
+};
+
+enum config_area {
+	UI_CONFIG_AREA = 0,
+	PM_CONFIG_AREA,
+	BL_CONFIG_AREA,
+	DP_CONFIG_AREA,
+	FLASH_CONFIG_AREA,
+#ifdef SYNA_TDDI
+	TDDI_FORCE_CONFIG_AREA,
+	TDDI_LCM_DATA_AREA,
+	TDDI_OEM_DATA_AREA,
+#endif
+	UPP_AREA,
+};
+
+enum v7_status {
+	SUCCESS = 0x00,
+	DEVICE_NOT_IN_BOOTLOADER_MODE,
+	INVALID_PARTITION,
+	INVALID_COMMAND,
+	INVALID_BLOCK_OFFSET,
+	INVALID_TRANSFER,
+	NOT_ERASED,
+	FLASH_PROGRAMMING_KEY_INCORRECT,
+	BAD_PARTITION_TABLE,
+	CHECKSUM_FAILED,
+	FLASH_HARDWARE_FAILURE = 0x1f,
+};
+
+enum v7_partition_id {
+	BOOTLOADER_PARTITION = 0x01,
+	DEVICE_CONFIG_PARTITION,
+	FLASH_CONFIG_PARTITION,
+	MANUFACTURING_BLOCK_PARTITION,
+	GUEST_SERIALIZATION_PARTITION,
+	GLOBAL_PARAMETERS_PARTITION,
+	CORE_CODE_PARTITION,
+	CORE_CONFIG_PARTITION,
+	GUEST_CODE_PARTITION,
+	DISPLAY_CONFIG_PARTITION,
+	EXTERNAL_TOUCH_AFE_CONFIG_PARTITION,
+	UTILITY_PARAMETER_PARTITION,
+};
+
+enum v7_flash_command {
+	CMD_V7_IDLE = 0x00,
+	CMD_V7_ENTER_BL,
+	CMD_V7_READ,
+	CMD_V7_WRITE,
+	CMD_V7_ERASE,
+	CMD_V7_ERASE_AP,
+	CMD_V7_SENSOR_ID,
+};
+
+enum v5v6_flash_command {
+	CMD_V5V6_IDLE = 0x0,
+	CMD_V5V6_WRITE_FW = 0x2,
+	CMD_V5V6_ERASE_ALL = 0x3,
+	CMD_V5V6_WRITE_LOCKDOWN = 0x4,
+	CMD_V5V6_READ_CONFIG = 0x5,
+	CMD_V5V6_WRITE_CONFIG = 0x6,
+	CMD_V5V6_ERASE_UI_CONFIG = 0x7,
+	CMD_V5V6_ERASE_BL_CONFIG = 0x9,
+	CMD_V5V6_ERASE_DISP_CONFIG = 0xa,
+	CMD_V5V6_ERASE_GUEST_CODE = 0xb,
+	CMD_V5V6_WRITE_GUEST_CODE = 0xc,
+	CMD_V5V6_ERASE_CHIP = 0x0d,
+	CMD_V5V6_ENABLE_FLASH_PROG = 0xf,
+#ifdef SYNA_TDDI
+	CMD_V5V6_ERASE_FORCE_CONFIG = 0x11,
+	CMD_V5V6_READ_FORCE_CONFIG = 0x12,
+	CMD_V5V6_WRITE_FORCE_CONFIG = 0x13,
+	CMD_V5V6_ERASE_LOCKDOWN_DATA = 0x1a,
+	CMD_V5V6_READ_LOCKDOWN_DATA = 0x1b,
+	CMD_V5V6_WRITE_LOCKDOWN_DATA = 0x1c,
+	CMD_V5V6_ERASE_LCM_DATA = 0x1d,
+	CMD_V5V6_ERASE_OEM_DATA = 0x1e,
+#endif
+};
+
+enum flash_command {
+	CMD_IDLE = 0,
+	CMD_WRITE_FW,
+	CMD_WRITE_CONFIG,
+	CMD_WRITE_LOCKDOWN,
+	CMD_WRITE_GUEST_CODE,
+	CMD_WRITE_BOOTLOADER,
+	CMD_WRITE_UTILITY_PARAM,
+	CMD_READ_CONFIG,
+	CMD_ERASE_ALL,
+	CMD_ERASE_UI_FIRMWARE,
+	CMD_ERASE_UI_CONFIG,
+	CMD_ERASE_BL_CONFIG,
+	CMD_ERASE_DISP_CONFIG,
+	CMD_ERASE_FLASH_CONFIG,
+	CMD_ERASE_GUEST_CODE,
+	CMD_ERASE_BOOTLOADER,
+	CMD_ERASE_UTILITY_PARAMETER,
+	CMD_ENABLE_FLASH_PROG,
+#ifdef SYNA_TDDI
+	CMD_ERASE_CHIP,
+	CMD_ERASE_FORCE_CONFIG,
+	CMD_READ_FORCE_CONFIG,
+	CMD_WRITE_FORCE_CONFIG,
+	CMD_ERASE_LOCKDOWN_DATA,
+	CMD_READ_LOCKDOWN_DATA,
+	CMD_WRITE_LOCKDOWN_DATA,
+	CMD_ERASE_LCM_DATA,
+	CMD_READ_LCM_DATA,
+	CMD_WRITE_LCM_DATA,
+	CMD_ERASE_OEM_DATA,
+	CMD_READ_OEM_DATA,
+	CMD_WRITE_OEM_DATA,
+#endif
+};
+
+enum f35_flash_command {
+	CMD_F35_IDLE = 0x0,
+	CMD_F35_RESERVED = 0x1,
+	CMD_F35_WRITE_CHUNK = 0x2,
+	CMD_F35_ERASE_ALL = 0x3,
+	CMD_F35_RESET = 0x10,
+};
+
+enum container_id {
+	TOP_LEVEL_CONTAINER = 0,
+	UI_CONTAINER,
+	UI_CONFIG_CONTAINER,
+	BL_CONTAINER,
+	BL_IMAGE_CONTAINER,
+	BL_CONFIG_CONTAINER,
+	BL_LOCKDOWN_INFO_CONTAINER,
+	PERMANENT_CONFIG_CONTAINER,
+	GUEST_CODE_CONTAINER,
+	BL_PROTOCOL_DESCRIPTOR_CONTAINER,
+	UI_PROTOCOL_DESCRIPTOR_CONTAINER,
+	RMI_SELF_DISCOVERY_CONTAINER,
+	RMI_PAGE_CONTENT_CONTAINER,
+	GENERAL_INFORMATION_CONTAINER,
+	DEVICE_CONFIG_CONTAINER,
+	FLASH_CONFIG_CONTAINER,
+	GUEST_SERIALIZATION_CONTAINER,
+	GLOBAL_PARAMETERS_CONTAINER,
+	CORE_CODE_CONTAINER,
+	CORE_CONFIG_CONTAINER,
+	DISPLAY_CONFIG_CONTAINER,
+	EXTERNAL_TOUCH_AFE_CONFIG_CONTAINER,
+	UTILITY_CONTAINER,
+	UTILITY_PARAMETER_CONTAINER,
+};
+
+enum utility_parameter_id {
+	UNUSED = 0,
+	FORCE_PARAMETER,
+	ANTI_BENDING_PARAMETER,
+};
+
+struct pdt_properties {
+	union {
+		struct {
+			unsigned char reserved_1:6;
+			unsigned char has_bsr:1;
+			unsigned char reserved_2:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct partition_table {
+	unsigned char partition_id:5;
+	unsigned char byte_0_reserved:3;
+	unsigned char byte_1_reserved;
+	unsigned char partition_length_7_0;
+	unsigned char partition_length_15_8;
+	unsigned char start_physical_address_7_0;
+	unsigned char start_physical_address_15_8;
+	unsigned char partition_properties_7_0;
+	unsigned char partition_properties_15_8;
+} __packed;
+
+struct f01_device_control {
+	union {
+		struct {
+			unsigned char sleep_mode:2;
+			unsigned char nosleep:1;
+			unsigned char reserved:2;
+			unsigned char charger_connected:1;
+			unsigned char report_rate:1;
+			unsigned char configured:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f34_v7_query_0 {
+	union {
+		struct {
+			unsigned char subpacket_1_size:3;
+			unsigned char has_config_id:1;
+			unsigned char f34_query0_b4:1;
+			unsigned char has_thqa:1;
+			unsigned char f34_query0_b6__7:2;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f34_v7_query_1_7 {
+	union {
+		struct {
+			/* query 1 */
+			unsigned char bl_minor_revision;
+			unsigned char bl_major_revision;
+
+			/* query 2 */
+			unsigned char bl_fw_id_7_0;
+			unsigned char bl_fw_id_15_8;
+			unsigned char bl_fw_id_23_16;
+			unsigned char bl_fw_id_31_24;
+
+			/* query 3 */
+			unsigned char minimum_write_size;
+			unsigned char block_size_7_0;
+			unsigned char block_size_15_8;
+			unsigned char flash_page_size_7_0;
+			unsigned char flash_page_size_15_8;
+
+			/* query 4 */
+			unsigned char adjustable_partition_area_size_7_0;
+			unsigned char adjustable_partition_area_size_15_8;
+
+			/* query 5 */
+			unsigned char flash_config_length_7_0;
+			unsigned char flash_config_length_15_8;
+
+			/* query 6 */
+			unsigned char payload_length_7_0;
+			unsigned char payload_length_15_8;
+
+			/* query 7 */
+			unsigned char f34_query7_b0:1;
+			unsigned char has_bootloader:1;
+			unsigned char has_device_config:1;
+			unsigned char has_flash_config:1;
+			unsigned char has_manufacturing_block:1;
+			unsigned char has_guest_serialization:1;
+			unsigned char has_global_parameters:1;
+			unsigned char has_core_code:1;
+			unsigned char has_core_config:1;
+			unsigned char has_guest_code:1;
+			unsigned char has_display_config:1;
+			unsigned char f34_query7_b11__15:5;
+			unsigned char f34_query7_b16__23;
+			unsigned char f34_query7_b24__31;
+		} __packed;
+		unsigned char data[21];
+	};
+};
+
+struct f34_v7_data0 {
+	union {
+		struct {
+			unsigned char operation_status:5;
+			unsigned char device_cfg_status:2;
+			unsigned char bl_mode:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f34_v7_data_1_5 {
+	union {
+		struct {
+			unsigned char partition_id:5;
+			unsigned char f34_data1_b5__7:3;
+			unsigned char block_offset_7_0;
+			unsigned char block_offset_15_8;
+			unsigned char transfer_length_7_0;
+			unsigned char transfer_length_15_8;
+			unsigned char command;
+			unsigned char payload_0;
+			unsigned char payload_1;
+		} __packed;
+		unsigned char data[8];
+	};
+};
+
+struct f34_v5v6_flash_properties {
+	union {
+		struct {
+			unsigned char reg_map:1;
+			unsigned char unlocked:1;
+			unsigned char has_config_id:1;
+			unsigned char has_pm_config:1;
+			unsigned char has_bl_config:1;
+			unsigned char has_disp_config:1;
+			unsigned char has_ctrl1:1;
+			unsigned char has_query4:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f34_v5v6_flash_properties_2 {
+	union {
+		struct {
+			unsigned char has_guest_code:1;
+			unsigned char f34_query4_b1:1;
+			unsigned char has_gesture_config:1;
+			unsigned char has_force_config:1;
+			unsigned char has_lockdown_data:1;
+			unsigned char has_lcm_data:1;
+			unsigned char has_oem_data:1;
+			unsigned char f34_query4_b7:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct register_offset {
+	unsigned char properties;
+	unsigned char properties_2;
+	unsigned char block_size;
+	unsigned char block_count;
+	unsigned char gc_block_count;
+	unsigned char flash_status;
+	unsigned char partition_id;
+	unsigned char block_number;
+	unsigned char transfer_length;
+	unsigned char flash_cmd;
+	unsigned char payload;
+};
+
+struct block_count {
+	unsigned short ui_firmware;
+	unsigned short ui_config;
+	unsigned short dp_config;
+	unsigned short pm_config;
+	unsigned short fl_config;
+	unsigned short bl_image;
+	unsigned short bl_config;
+	unsigned short utility_param;
+	unsigned short lockdown;
+	unsigned short guest_code;
+#ifdef SYNA_TDDI
+	unsigned short tddi_force_config;
+	unsigned short tddi_lockdown_data;
+	unsigned short tddi_lcm_data;
+	unsigned short tddi_oem_data;
+#endif
+	unsigned short total_count;
+};
+
+struct physical_address {
+	unsigned short ui_firmware;
+	unsigned short ui_config;
+	unsigned short dp_config;
+	unsigned short pm_config;
+	unsigned short fl_config;
+	unsigned short bl_image;
+	unsigned short bl_config;
+	unsigned short utility_param;
+	unsigned short lockdown;
+	unsigned short guest_code;
+};
+
+struct container_descriptor {
+	unsigned char content_checksum[4];
+	unsigned char container_id[2];
+	unsigned char minor_version;
+	unsigned char major_version;
+	unsigned char reserved_08;
+	unsigned char reserved_09;
+	unsigned char reserved_0a;
+	unsigned char reserved_0b;
+	unsigned char container_option_flags[4];
+	unsigned char content_options_length[4];
+	unsigned char content_options_address[4];
+	unsigned char content_length[4];
+	unsigned char content_address[4];
+};
+
+struct image_header_10 {
+	unsigned char checksum[4];
+	unsigned char reserved_04;
+	unsigned char reserved_05;
+	unsigned char minor_header_version;
+	unsigned char major_header_version;
+	unsigned char reserved_08;
+	unsigned char reserved_09;
+	unsigned char reserved_0a;
+	unsigned char reserved_0b;
+	unsigned char top_level_container_start_addr[4];
+};
+
+struct image_header_05_06 {
+	/* 0x00 - 0x0f */
+	unsigned char checksum[4];
+	unsigned char reserved_04;
+	unsigned char reserved_05;
+	unsigned char options_firmware_id:1;
+	unsigned char options_bootloader:1;
+	unsigned char options_guest_code:1;
+	unsigned char options_tddi:1;
+	unsigned char options_reserved:4;
+	unsigned char header_version;
+	unsigned char firmware_size[4];
+	unsigned char config_size[4];
+	/* 0x10 - 0x1f */
+	unsigned char product_id[PRODUCT_ID_SIZE];
+	unsigned char package_id[2];
+	unsigned char package_id_revision[2];
+	unsigned char product_info[PRODUCT_INFO_SIZE];
+	/* 0x20 - 0x2f */
+	unsigned char bootloader_addr[4];
+	unsigned char bootloader_size[4];
+	unsigned char ui_addr[4];
+	unsigned char ui_size[4];
+	/* 0x30 - 0x3f */
+	unsigned char ds_id[16];
+	/* 0x40 - 0x4f */
+	union {
+		struct {
+			unsigned char cstmr_product_id[PRODUCT_ID_SIZE];
+			unsigned char reserved_4a_4f[6];
+		};
+		struct {
+			unsigned char dsp_cfg_addr[4];
+			unsigned char dsp_cfg_size[4];
+			unsigned char reserved_48_4f[8];
+		};
+	};
+	/* 0x50 - 0x53 */
+	unsigned char firmware_id[4];
+};
+
+struct block_data {
+	unsigned int size;
+	const unsigned char *data;
+};
+
+struct image_metadata {
+	bool contains_firmware_id;
+	bool contains_bootloader;
+	bool contains_guest_code;
+	bool contains_disp_config;
+	bool contains_perm_config;
+	bool contains_flash_config;
+	bool contains_utility_param;
+	unsigned int firmware_id;
+	unsigned int checksum;
+	unsigned int bootloader_size;
+	unsigned int disp_config_offset;
+	unsigned char bl_version;
+	unsigned char product_id[PRODUCT_ID_SIZE + 1];
+	unsigned char cstmr_product_id[PRODUCT_ID_SIZE + 1];
+	unsigned char utility_param_id[MAX_UTILITY_PARAMS];
+	struct block_data bootloader;
+	struct block_data utility;
+	struct block_data ui_firmware;
+	struct block_data ui_config;
+	struct block_data dp_config;
+	struct block_data pm_config;
+	struct block_data fl_config;
+	struct block_data bl_image;
+	struct block_data bl_config;
+	struct block_data utility_param[MAX_UTILITY_PARAMS];
+	struct block_data lockdown;
+	struct block_data guest_code;
+	struct block_count blkcount;
+	struct physical_address phyaddr;
+};
+
+struct synaptics_rmi4_fwu_handle {
+	enum bl_version bl_version;
+	bool initialized;
+	bool in_bl_mode;
+	bool in_ub_mode;
+	bool bl_mode_device;
+	bool force_update;
+	bool do_lockdown;
+	bool has_guest_code;
+#ifdef SYNA_TDDI
+	bool has_force_config;
+	bool has_lockdown_data;
+	bool has_lcm_data;
+	bool has_oem_data;
+#endif
+	bool has_utility_param;
+	bool new_partition_table;
+	bool incompatible_partition_tables;
+	bool write_bootloader;
+	unsigned int data_pos;
+	unsigned char *ext_data_source;
+	unsigned char *read_config_buf;
+	unsigned char intr_mask;
+	unsigned char command;
+	unsigned char bootloader_id[2];
+	unsigned char config_id[32];
+	unsigned char flash_status;
+	unsigned char partitions;
+#ifdef F51_DISCRETE_FORCE
+	unsigned char *cal_data;
+	unsigned short cal_data_off;
+	unsigned short cal_data_size;
+	unsigned short cal_data_buf_size;
+	unsigned short cal_packet_data_size;
+#endif
+	unsigned short block_size;
+	unsigned short config_size;
+	unsigned short config_area;
+	unsigned short config_block_count;
+	unsigned short flash_config_length;
+	unsigned short payload_length;
+	unsigned short partition_table_bytes;
+	unsigned short read_config_buf_size;
+	const unsigned char *config_data;
+	const unsigned char *image;
+	unsigned char *image_name;
+	unsigned int image_size;
+	struct image_metadata img;
+	struct register_offset off;
+	struct block_count blkcount;
+	struct physical_address phyaddr;
+	struct f34_v5v6_flash_properties flash_properties;
+	struct synaptics_rmi4_fn_desc f34_fd;
+	struct synaptics_rmi4_fn_desc f35_fd;
+	struct synaptics_rmi4_data *rmi4_data;
+	struct workqueue_struct *fwu_workqueue;
+	struct work_struct fwu_work;
+};
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+static struct bin_attribute dev_attr_data = {
+	.attr = {
+		.name = "data",
+		.mode = 0664,
+	},
+	.size = 0,
+	.read = fwu_sysfs_show_image,
+	.write = fwu_sysfs_store_image,
+};
+#endif
+
+static struct device_attribute attrs[] = {
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+	__ATTR(dorecovery, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_do_recovery_store),
+	__ATTR(doreflash, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_do_reflash_store),
+	__ATTR(writeconfig, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_write_config_store),
+	__ATTR(readconfig, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_read_config_store),
+	__ATTR(configarea, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_config_area_store),
+	__ATTR(imagename, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_image_name_store),
+	__ATTR(imagesize, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_image_size_store),
+	__ATTR(blocksize, 0444,
+			fwu_sysfs_block_size_show,
+			synaptics_rmi4_store_error),
+	__ATTR(fwblockcount, 0444,
+			fwu_sysfs_firmware_block_count_show,
+			synaptics_rmi4_store_error),
+	__ATTR(configblockcount, 0444,
+			fwu_sysfs_configuration_block_count_show,
+			synaptics_rmi4_store_error),
+	__ATTR(dispconfigblockcount, 0444,
+			fwu_sysfs_disp_config_block_count_show,
+			synaptics_rmi4_store_error),
+	__ATTR(permconfigblockcount, 0444,
+			fwu_sysfs_perm_config_block_count_show,
+			synaptics_rmi4_store_error),
+	__ATTR(blconfigblockcount, 0444,
+			fwu_sysfs_bl_config_block_count_show,
+			synaptics_rmi4_store_error),
+	__ATTR(uppblockcount, 0444,
+			fwu_sysfs_utility_parameter_block_count_show,
+			synaptics_rmi4_store_error),
+	__ATTR(guestcodeblockcount, 0444,
+			fwu_sysfs_guest_code_block_count_show,
+			synaptics_rmi4_store_error),
+	__ATTR(writeguestcode, 0220,
+			synaptics_rmi4_show_error,
+			fwu_sysfs_write_guest_code_store),
+#ifdef SYNA_TDDI
+	__ATTR(lockdowncode, 0664,
+			fwu_sysfs_read_lockdown_code_show,
+			fwu_sysfs_write_lockdown_code_store),
+#endif
+#endif
+};
+
+static struct synaptics_rmi4_fwu_handle *fwu;
+
+DECLARE_COMPLETION(fwu_remove_complete);
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+DEFINE_MUTEX(fwu_sysfs_mutex);
+#endif
+
+static void calculate_checksum(unsigned short *data, unsigned long len,
+		unsigned long *result)
+{
+	unsigned long temp;
+	unsigned long sum1 = 0xffff;
+	unsigned long sum2 = 0xffff;
+
+	*result = 0xffffffff;
+
+	while (len--) {
+		temp = *data;
+		sum1 += temp;
+		sum2 += sum1;
+		sum1 = (sum1 & 0xffff) + (sum1 >> 16);
+		sum2 = (sum2 & 0xffff) + (sum2 >> 16);
+		data++;
+	}
+
+	*result = sum2 << 16 | sum1;
+}
+
+static void convert_to_little_endian(unsigned char *dest, unsigned long src)
+{
+	dest[0] = (unsigned char)(src & 0xff);
+	dest[1] = (unsigned char)((src >> 8) & 0xff);
+	dest[2] = (unsigned char)((src >> 16) & 0xff);
+	dest[3] = (unsigned char)((src >> 24) & 0xff);
+}
+
+static unsigned int le_to_uint(const unsigned char *ptr)
+{
+	return (unsigned int)ptr[0] +
+			(unsigned int)ptr[1] * 0x100 +
+			(unsigned int)ptr[2] * 0x10000 +
+			(unsigned int)ptr[3] * 0x1000000;
+}
+
+#ifdef F51_DISCRETE_FORCE
+static int fwu_f51_force_data_init(void)
+{
+	int retval;
+	unsigned char query_count;
+	unsigned char packet_info;
+	unsigned char offset[2];
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f51_query_base_addr + 7,
+			offset,
+			sizeof(offset));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read force data offset\n",
+				__func__);
+		return retval;
+	}
+
+	fwu->cal_data_off = offset[0] | offset[1] << 8;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f51_query_base_addr,
+			&query_count,
+			sizeof(query_count));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read number of F51 query registers\n",
+				__func__);
+		return retval;
+	}
+
+	if (query_count >= 10) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				rmi4_data->f51_query_base_addr + 9,
+				&packet_info,
+				sizeof(packet_info));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read F51 packet register info\n",
+					__func__);
+			return retval;
+		}
+
+		if (packet_info & MASK_1BIT) {
+			fwu->cal_packet_data_size = packet_info >> 1;
+			fwu->cal_packet_data_size *= 2;
+		} else {
+			fwu->cal_packet_data_size = 0;
+		}
+	} else {
+		fwu->cal_packet_data_size = 0;
+	}
+
+	fwu->cal_data_size = CAL_DATA_SIZE + fwu->cal_packet_data_size;
+	if (fwu->cal_data_size > fwu->cal_data_buf_size) {
+		kfree(fwu->cal_data);
+		fwu->cal_data_buf_size = fwu->cal_data_size;
+		fwu->cal_data = kmalloc(fwu->cal_data_buf_size, GFP_KERNEL);
+		if (!fwu->cal_data) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for fwu->cal_data\n",
+					__func__);
+			fwu->cal_data_buf_size = 0;
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static int fwu_allocate_read_config_buf(unsigned int count)
+{
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (count > fwu->read_config_buf_size) {
+		kfree(fwu->read_config_buf);
+		fwu->read_config_buf = kzalloc(count, GFP_KERNEL);
+		if (!fwu->read_config_buf) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for fwu->read_config_buf\n",
+					__func__);
+			fwu->read_config_buf_size = 0;
+			return -ENOMEM;
+		}
+		fwu->read_config_buf_size = count;
+	}
+
+	return 0;
+}
+
+static void fwu_compare_partition_tables(void)
+{
+	fwu->incompatible_partition_tables = false;
+
+	if (fwu->phyaddr.bl_image != fwu->img.phyaddr.bl_image)
+		fwu->incompatible_partition_tables = true;
+	else if (fwu->phyaddr.lockdown != fwu->img.phyaddr.lockdown)
+		fwu->incompatible_partition_tables = true;
+	else if (fwu->phyaddr.bl_config != fwu->img.phyaddr.bl_config)
+		fwu->incompatible_partition_tables = true;
+	else if (fwu->phyaddr.utility_param != fwu->img.phyaddr.utility_param)
+		fwu->incompatible_partition_tables = true;
+
+	if (fwu->bl_version == BL_V7) {
+		if (fwu->phyaddr.fl_config != fwu->img.phyaddr.fl_config)
+			fwu->incompatible_partition_tables = true;
+	}
+
+	fwu->new_partition_table = false;
+
+	if (fwu->phyaddr.ui_firmware != fwu->img.phyaddr.ui_firmware)
+		fwu->new_partition_table = true;
+	else if (fwu->phyaddr.ui_config != fwu->img.phyaddr.ui_config)
+		fwu->new_partition_table = true;
+
+	if (fwu->flash_properties.has_disp_config) {
+		if (fwu->phyaddr.dp_config != fwu->img.phyaddr.dp_config)
+			fwu->new_partition_table = true;
+	}
+
+	if (fwu->has_guest_code) {
+		if (fwu->phyaddr.guest_code != fwu->img.phyaddr.guest_code)
+			fwu->new_partition_table = true;
+	}
+}
+
+static void fwu_parse_partition_table(const unsigned char *partition_table,
+		struct block_count *blkcount, struct physical_address *phyaddr)
+{
+	unsigned char ii;
+	unsigned char index;
+	unsigned char offset;
+	unsigned short partition_length;
+	unsigned short physical_address;
+	struct partition_table *ptable;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	for (ii = 0; ii < fwu->partitions; ii++) {
+		index = ii * 8 + 2;
+		ptable = (struct partition_table *)&partition_table[index];
+		partition_length = ptable->partition_length_15_8 << 8 |
+				ptable->partition_length_7_0;
+		physical_address = ptable->start_physical_address_15_8 << 8 |
+				ptable->start_physical_address_7_0;
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Partition entry %d:\n",
+				__func__, ii);
+		for (offset = 0; offset < 8; offset++) {
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: 0x%02x\n",
+					__func__,
+					partition_table[index + offset]);
+		}
+		switch (ptable->partition_id) {
+		case CORE_CODE_PARTITION:
+			blkcount->ui_firmware = partition_length;
+			phyaddr->ui_firmware = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Core code block count: %d\n",
+					__func__, blkcount->ui_firmware);
+			blkcount->total_count += partition_length;
+			break;
+		case CORE_CONFIG_PARTITION:
+			blkcount->ui_config = partition_length;
+			phyaddr->ui_config = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Core config block count: %d\n",
+					__func__, blkcount->ui_config);
+			blkcount->total_count += partition_length;
+			break;
+		case BOOTLOADER_PARTITION:
+			blkcount->bl_image = partition_length;
+			phyaddr->bl_image = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Bootloader block count: %d\n",
+					__func__, blkcount->bl_image);
+			blkcount->total_count += partition_length;
+			break;
+		case UTILITY_PARAMETER_PARTITION:
+			blkcount->utility_param = partition_length;
+			phyaddr->utility_param = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Utility parameter block count: %d\n",
+					__func__, blkcount->utility_param);
+			blkcount->total_count += partition_length;
+			break;
+		case DISPLAY_CONFIG_PARTITION:
+			blkcount->dp_config = partition_length;
+			phyaddr->dp_config = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Display config block count: %d\n",
+					__func__, blkcount->dp_config);
+			blkcount->total_count += partition_length;
+			break;
+		case FLASH_CONFIG_PARTITION:
+			blkcount->fl_config = partition_length;
+			phyaddr->fl_config = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Flash config block count: %d\n",
+					__func__, blkcount->fl_config);
+			blkcount->total_count += partition_length;
+			break;
+		case GUEST_CODE_PARTITION:
+			blkcount->guest_code = partition_length;
+			phyaddr->guest_code = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Guest code block count: %d\n",
+					__func__, blkcount->guest_code);
+			blkcount->total_count += partition_length;
+			break;
+		case GUEST_SERIALIZATION_PARTITION:
+			blkcount->pm_config = partition_length;
+			phyaddr->pm_config = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Guest serialization block count: %d\n",
+					__func__, blkcount->pm_config);
+			blkcount->total_count += partition_length;
+			break;
+		case GLOBAL_PARAMETERS_PARTITION:
+			blkcount->bl_config = partition_length;
+			phyaddr->bl_config = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Global parameters block count: %d\n",
+					__func__, blkcount->bl_config);
+			blkcount->total_count += partition_length;
+			break;
+		case DEVICE_CONFIG_PARTITION:
+			blkcount->lockdown = partition_length;
+			phyaddr->lockdown = physical_address;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Device config block count: %d\n",
+					__func__, blkcount->lockdown);
+			blkcount->total_count += partition_length;
+			break;
+		};
+	}
+}
+
+static void fwu_parse_image_header_10_utility(const unsigned char *image)
+{
+	unsigned char ii;
+	unsigned char num_of_containers;
+	unsigned int addr;
+	unsigned int container_id;
+	unsigned int length;
+	const unsigned char *content;
+	struct container_descriptor *descriptor;
+
+	num_of_containers = fwu->img.utility.size / 4;
+
+	for (ii = 0; ii < num_of_containers; ii++) {
+		if (ii >= MAX_UTILITY_PARAMS)
+			continue;
+		addr = le_to_uint(fwu->img.utility.data + (ii * 4));
+		descriptor = (struct container_descriptor *)(image + addr);
+		container_id = descriptor->container_id[0] |
+				descriptor->container_id[1] << 8;
+		content = image + le_to_uint(descriptor->content_address);
+		length = le_to_uint(descriptor->content_length);
+		switch (container_id) {
+		case UTILITY_PARAMETER_CONTAINER:
+			fwu->img.utility_param[ii].data = content;
+			fwu->img.utility_param[ii].size = length;
+			fwu->img.utility_param_id[ii] = content[0];
+			break;
+		default:
+			break;
+		};
+	}
+}
+
+static void fwu_parse_image_header_10_bootloader(const unsigned char *image)
+{
+	unsigned char ii;
+	unsigned char num_of_containers;
+	unsigned int addr;
+	unsigned int container_id;
+	unsigned int length;
+	const unsigned char *content;
+	struct container_descriptor *descriptor;
+
+	num_of_containers = (fwu->img.bootloader.size - 4) / 4;
+
+	for (ii = 1; ii <= num_of_containers; ii++) {
+		addr = le_to_uint(fwu->img.bootloader.data + (ii * 4));
+		descriptor = (struct container_descriptor *)(image + addr);
+		container_id = descriptor->container_id[0] |
+				descriptor->container_id[1] << 8;
+		content = image + le_to_uint(descriptor->content_address);
+		length = le_to_uint(descriptor->content_length);
+		switch (container_id) {
+		case BL_IMAGE_CONTAINER:
+			fwu->img.bl_image.data = content;
+			fwu->img.bl_image.size = length;
+			break;
+		case BL_CONFIG_CONTAINER:
+		case GLOBAL_PARAMETERS_CONTAINER:
+			fwu->img.bl_config.data = content;
+			fwu->img.bl_config.size = length;
+			break;
+		case BL_LOCKDOWN_INFO_CONTAINER:
+		case DEVICE_CONFIG_CONTAINER:
+			fwu->img.lockdown.data = content;
+			fwu->img.lockdown.size = length;
+			break;
+		default:
+			break;
+		};
+	}
+}
+
+static void fwu_parse_image_header_10(void)
+{
+	unsigned char ii;
+	unsigned char num_of_containers;
+	unsigned int addr;
+	unsigned int offset;
+	unsigned int container_id;
+	unsigned int length;
+	const unsigned char *image;
+	const unsigned char *content;
+	struct container_descriptor *descriptor;
+	struct image_header_10 *header;
+
+	image = fwu->image;
+	header = (struct image_header_10 *)image;
+
+	fwu->img.checksum = le_to_uint(header->checksum);
+
+	/* address of top level container */
+	offset = le_to_uint(header->top_level_container_start_addr);
+	descriptor = (struct container_descriptor *)(image + offset);
+
+	/* address of top level container content */
+	offset = le_to_uint(descriptor->content_address);
+	num_of_containers = le_to_uint(descriptor->content_length) / 4;
+
+	for (ii = 0; ii < num_of_containers; ii++) {
+		addr = le_to_uint(image + offset);
+		offset += 4;
+		descriptor = (struct container_descriptor *)(image + addr);
+		container_id = descriptor->container_id[0] |
+				descriptor->container_id[1] << 8;
+		content = image + le_to_uint(descriptor->content_address);
+		length = le_to_uint(descriptor->content_length);
+		switch (container_id) {
+		case UI_CONTAINER:
+		case CORE_CODE_CONTAINER:
+			fwu->img.ui_firmware.data = content;
+			fwu->img.ui_firmware.size = length;
+			break;
+		case UI_CONFIG_CONTAINER:
+		case CORE_CONFIG_CONTAINER:
+			fwu->img.ui_config.data = content;
+			fwu->img.ui_config.size = length;
+			break;
+		case BL_CONTAINER:
+			fwu->img.bl_version = *content;
+			fwu->img.bootloader.data = content;
+			fwu->img.bootloader.size = length;
+			fwu_parse_image_header_10_bootloader(image);
+			break;
+		case UTILITY_CONTAINER:
+			fwu->img.utility.data = content;
+			fwu->img.utility.size = length;
+			fwu_parse_image_header_10_utility(image);
+			break;
+		case GUEST_CODE_CONTAINER:
+			fwu->img.contains_guest_code = true;
+			fwu->img.guest_code.data = content;
+			fwu->img.guest_code.size = length;
+			break;
+		case DISPLAY_CONFIG_CONTAINER:
+			fwu->img.contains_disp_config = true;
+			fwu->img.dp_config.data = content;
+			fwu->img.dp_config.size = length;
+			break;
+		case PERMANENT_CONFIG_CONTAINER:
+		case GUEST_SERIALIZATION_CONTAINER:
+			fwu->img.contains_perm_config = true;
+			fwu->img.pm_config.data = content;
+			fwu->img.pm_config.size = length;
+			break;
+		case FLASH_CONFIG_CONTAINER:
+			fwu->img.contains_flash_config = true;
+			fwu->img.fl_config.data = content;
+			fwu->img.fl_config.size = length;
+			break;
+		case GENERAL_INFORMATION_CONTAINER:
+			fwu->img.contains_firmware_id = true;
+			fwu->img.firmware_id = le_to_uint(content + 4);
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+static void fwu_parse_image_header_05_06(void)
+{
+	int retval;
+	const unsigned char *image;
+	struct image_header_05_06 *header;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	image = fwu->image;
+	header = (struct image_header_05_06 *)image;
+
+	fwu->img.checksum = le_to_uint(header->checksum);
+
+	fwu->img.bl_version = header->header_version;
+
+	fwu->img.contains_bootloader = header->options_bootloader;
+	if (fwu->img.contains_bootloader)
+		fwu->img.bootloader_size = le_to_uint(header->bootloader_size);
+
+	fwu->img.ui_firmware.size = le_to_uint(header->firmware_size);
+	if (fwu->img.ui_firmware.size) {
+		fwu->img.ui_firmware.data = image + IMAGE_AREA_OFFSET;
+		if (fwu->img.contains_bootloader)
+			fwu->img.ui_firmware.data += fwu->img.bootloader_size;
+	}
+
+	if ((fwu->img.bl_version == BL_V6) && header->options_tddi)
+		fwu->img.ui_firmware.data = image + IMAGE_AREA_OFFSET;
+
+	fwu->img.ui_config.size = le_to_uint(header->config_size);
+	if (fwu->img.ui_config.size) {
+		fwu->img.ui_config.data = fwu->img.ui_firmware.data +
+				fwu->img.ui_firmware.size;
+	}
+
+	if (fwu->img.contains_bootloader || header->options_tddi)
+		fwu->img.contains_disp_config = true;
+	else
+		fwu->img.contains_disp_config = false;
+
+	if (fwu->img.contains_disp_config) {
+		fwu->img.disp_config_offset = le_to_uint(header->dsp_cfg_addr);
+		fwu->img.dp_config.size = le_to_uint(header->dsp_cfg_size);
+		fwu->img.dp_config.data = image + fwu->img.disp_config_offset;
+	} else {
+		retval = secure_memcpy(fwu->img.cstmr_product_id,
+				sizeof(fwu->img.cstmr_product_id),
+				header->cstmr_product_id,
+				sizeof(header->cstmr_product_id),
+				PRODUCT_ID_SIZE);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to copy custom product ID string\n",
+					__func__);
+		}
+		fwu->img.cstmr_product_id[PRODUCT_ID_SIZE] = 0;
+	}
+
+	fwu->img.contains_firmware_id = header->options_firmware_id;
+	if (fwu->img.contains_firmware_id)
+		fwu->img.firmware_id = le_to_uint(header->firmware_id);
+
+	retval = secure_memcpy(fwu->img.product_id,
+			sizeof(fwu->img.product_id),
+			header->product_id,
+			sizeof(header->product_id),
+			PRODUCT_ID_SIZE);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy product ID string\n",
+				__func__);
+	}
+	fwu->img.product_id[PRODUCT_ID_SIZE] = 0;
+
+	fwu->img.lockdown.size = LOCKDOWN_SIZE;
+	fwu->img.lockdown.data = image + IMAGE_AREA_OFFSET - LOCKDOWN_SIZE;
+}
+
+static int fwu_parse_image_info(void)
+{
+	struct image_header_10 *header;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	header = (struct image_header_10 *)fwu->image;
+
+	memset(&fwu->img, 0x00, sizeof(fwu->img));
+
+	switch (header->major_header_version) {
+	case IMAGE_HEADER_VERSION_10:
+		fwu_parse_image_header_10();
+		break;
+	case IMAGE_HEADER_VERSION_05:
+	case IMAGE_HEADER_VERSION_06:
+		fwu_parse_image_header_05_06();
+		break;
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Unsupported image file format (0x%02x)\n",
+				__func__, header->major_header_version);
+		return -EINVAL;
+	}
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8) {
+		if (!fwu->img.contains_flash_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: No flash config found in firmware image\n",
+					__func__);
+			return -EINVAL;
+		}
+
+		fwu_parse_partition_table(fwu->img.fl_config.data,
+				&fwu->img.blkcount, &fwu->img.phyaddr);
+
+		if (fwu->img.blkcount.utility_param)
+			fwu->img.contains_utility_param = true;
+
+		fwu_compare_partition_tables();
+	} else {
+		fwu->new_partition_table = false;
+		fwu->incompatible_partition_tables = false;
+	}
+
+	return 0;
+}
+
+static int fwu_read_flash_status(void)
+{
+	int retval;
+	unsigned char status;
+	unsigned char command;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fwu->f34_fd.data_base_addr + fwu->off.flash_status,
+			&status,
+			sizeof(status));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read flash status\n",
+				__func__);
+		return retval;
+	}
+
+	fwu->in_bl_mode = status >> 7;
+
+	if (fwu->bl_version == BL_V5)
+		fwu->flash_status = (status >> 4) & MASK_3BIT;
+	else if (fwu->bl_version == BL_V6)
+		fwu->flash_status = status & MASK_3BIT;
+	else if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		fwu->flash_status = status & MASK_5BIT;
+
+	if (fwu->write_bootloader)
+		fwu->flash_status = 0x00;
+
+	if (fwu->flash_status != 0x00) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Flash status = %d, command = 0x%02x\n",
+				__func__, fwu->flash_status, fwu->command);
+	}
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8) {
+		if (fwu->flash_status == 0x08)
+			fwu->flash_status = 0x00;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fwu->f34_fd.data_base_addr + fwu->off.flash_cmd,
+			&command,
+			sizeof(command));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read flash command\n",
+				__func__);
+		return retval;
+	}
+
+	if (fwu->bl_version == BL_V5)
+		fwu->command = command & MASK_4BIT;
+	else if (fwu->bl_version == BL_V6)
+		fwu->command = command & MASK_6BIT;
+	else if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		fwu->command = command;
+
+	if (fwu->write_bootloader)
+		fwu->command = 0x00;
+
+	return 0;
+}
+
+static int fwu_wait_for_idle(int timeout_ms, bool poll)
+{
+	int count = 0;
+	int timeout_count = ((timeout_ms * 1000) / MAX_SLEEP_TIME_US) + 1;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	do {
+		usleep_range(MIN_SLEEP_TIME_US, MAX_SLEEP_TIME_US);
+
+		count++;
+		if (poll || (count == timeout_count))
+			fwu_read_flash_status();
+
+		if ((fwu->command == CMD_IDLE) && (fwu->flash_status == 0x00))
+			return 0;
+	} while (count < timeout_count);
+
+	dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Timed out waiting for idle status\n",
+			__func__);
+
+	return -ETIMEDOUT;
+}
+
+static int fwu_write_f34_v7_command_single_transaction(unsigned char cmd)
+{
+	int retval;
+	unsigned char data_base;
+	struct f34_v7_data_1_5 data_1_5;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	memset(data_1_5.data, 0x00, sizeof(data_1_5.data));
+
+	switch (cmd) {
+	case CMD_ERASE_ALL:
+		data_1_5.partition_id = CORE_CODE_PARTITION;
+		data_1_5.command = CMD_V7_ERASE_AP;
+		break;
+	case CMD_ERASE_UI_FIRMWARE:
+		data_1_5.partition_id = CORE_CODE_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ERASE_BL_CONFIG:
+		data_1_5.partition_id = GLOBAL_PARAMETERS_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ERASE_UI_CONFIG:
+		data_1_5.partition_id = CORE_CONFIG_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ERASE_DISP_CONFIG:
+		data_1_5.partition_id = DISPLAY_CONFIG_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ERASE_FLASH_CONFIG:
+		data_1_5.partition_id = FLASH_CONFIG_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ERASE_GUEST_CODE:
+		data_1_5.partition_id = GUEST_CODE_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ERASE_BOOTLOADER:
+		data_1_5.partition_id = BOOTLOADER_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ERASE_UTILITY_PARAMETER:
+		data_1_5.partition_id = UTILITY_PARAMETER_PARTITION;
+		data_1_5.command = CMD_V7_ERASE;
+		break;
+	case CMD_ENABLE_FLASH_PROG:
+		data_1_5.partition_id = BOOTLOADER_PARTITION;
+		data_1_5.command = CMD_V7_ENTER_BL;
+		break;
+	};
+
+	data_1_5.payload_0 = fwu->bootloader_id[0];
+	data_1_5.payload_1 = fwu->bootloader_id[1];
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.partition_id,
+			data_1_5.data,
+			sizeof(data_1_5.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write single transaction command\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_write_f34_v7_command(unsigned char cmd)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char command;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	switch (cmd) {
+	case CMD_WRITE_FW:
+	case CMD_WRITE_CONFIG:
+	case CMD_WRITE_LOCKDOWN:
+	case CMD_WRITE_GUEST_CODE:
+	case CMD_WRITE_BOOTLOADER:
+	case CMD_WRITE_UTILITY_PARAM:
+		command = CMD_V7_WRITE;
+		break;
+	case CMD_READ_CONFIG:
+		command = CMD_V7_READ;
+		break;
+	case CMD_ERASE_ALL:
+		command = CMD_V7_ERASE_AP;
+		break;
+	case CMD_ERASE_UI_FIRMWARE:
+	case CMD_ERASE_BL_CONFIG:
+	case CMD_ERASE_UI_CONFIG:
+	case CMD_ERASE_DISP_CONFIG:
+	case CMD_ERASE_FLASH_CONFIG:
+	case CMD_ERASE_GUEST_CODE:
+	case CMD_ERASE_BOOTLOADER:
+	case CMD_ERASE_UTILITY_PARAMETER:
+		command = CMD_V7_ERASE;
+		break;
+	case CMD_ENABLE_FLASH_PROG:
+		command = CMD_V7_ENTER_BL;
+		break;
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid command 0x%02x\n",
+				__func__, cmd);
+		return -EINVAL;
+	};
+
+	fwu->command = command;
+
+	switch (cmd) {
+	case CMD_ERASE_ALL:
+	case CMD_ERASE_UI_FIRMWARE:
+	case CMD_ERASE_BL_CONFIG:
+	case CMD_ERASE_UI_CONFIG:
+	case CMD_ERASE_DISP_CONFIG:
+	case CMD_ERASE_FLASH_CONFIG:
+	case CMD_ERASE_GUEST_CODE:
+	case CMD_ERASE_BOOTLOADER:
+	case CMD_ERASE_UTILITY_PARAMETER:
+	case CMD_ENABLE_FLASH_PROG:
+		retval = fwu_write_f34_v7_command_single_transaction(cmd);
+		if (retval < 0)
+			return retval;
+		else
+			return 0;
+	default:
+		break;
+	};
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.flash_cmd,
+			&command,
+			sizeof(command));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write flash command\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_write_f34_v5v6_command(unsigned char cmd)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char command;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	switch (cmd) {
+	case CMD_IDLE:
+		command = CMD_V5V6_IDLE;
+		break;
+	case CMD_WRITE_FW:
+		command = CMD_V5V6_WRITE_FW;
+		break;
+	case CMD_WRITE_CONFIG:
+		command = CMD_V5V6_WRITE_CONFIG;
+		break;
+	case CMD_WRITE_LOCKDOWN:
+		command = CMD_V5V6_WRITE_LOCKDOWN;
+		break;
+	case CMD_WRITE_GUEST_CODE:
+		command = CMD_V5V6_WRITE_GUEST_CODE;
+		break;
+	case CMD_READ_CONFIG:
+		command = CMD_V5V6_READ_CONFIG;
+		break;
+	case CMD_ERASE_ALL:
+		command = CMD_V5V6_ERASE_ALL;
+		break;
+	case CMD_ERASE_UI_CONFIG:
+		command = CMD_V5V6_ERASE_UI_CONFIG;
+		break;
+	case CMD_ERASE_DISP_CONFIG:
+		command = CMD_V5V6_ERASE_DISP_CONFIG;
+		break;
+	case CMD_ERASE_GUEST_CODE:
+		command = CMD_V5V6_ERASE_GUEST_CODE;
+		break;
+	case CMD_ENABLE_FLASH_PROG:
+		command = CMD_V5V6_ENABLE_FLASH_PROG;
+		break;
+#ifdef SYNA_TDDI
+	case CMD_ERASE_CHIP:
+		command = CMD_V5V6_ERASE_CHIP;
+		break;
+	case CMD_ERASE_FORCE_CONFIG:
+		command = CMD_V5V6_ERASE_FORCE_CONFIG;
+		break;
+	case CMD_READ_FORCE_CONFIG:
+		command = CMD_V5V6_READ_FORCE_CONFIG;
+		break;
+	case CMD_WRITE_FORCE_CONFIG:
+		command = CMD_V5V6_WRITE_CONFIG;
+		break;
+	case CMD_ERASE_LOCKDOWN_DATA:
+		command = CMD_V5V6_ERASE_LOCKDOWN_DATA;
+		break;
+	case CMD_READ_LOCKDOWN_DATA:
+		command = CMD_V5V6_READ_LOCKDOWN_DATA;
+		break;
+	case CMD_WRITE_LOCKDOWN_DATA:
+		command = CMD_V5V6_WRITE_LOCKDOWN_DATA;
+		break;
+	case CMD_ERASE_LCM_DATA:
+		command = CMD_V5V6_ERASE_LCM_DATA;
+		break;
+	case CMD_ERASE_OEM_DATA:
+		command = CMD_V5V6_ERASE_OEM_DATA;
+		break;
+#endif
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid command 0x%02x\n",
+				__func__, cmd);
+		return -EINVAL;
+	}
+
+	switch (cmd) {
+	case CMD_ERASE_ALL:
+	case CMD_ERASE_UI_CONFIG:
+	case CMD_ERASE_DISP_CONFIG:
+	case CMD_ERASE_GUEST_CODE:
+#ifdef SYNA_TDDI
+	case CMD_ERASE_CHIP:
+	case CMD_ERASE_FORCE_CONFIG:
+	case CMD_ERASE_LOCKDOWN_DATA:
+	case CMD_ERASE_LCM_DATA:
+	case CMD_ERASE_OEM_DATA:
+#endif
+	case CMD_ENABLE_FLASH_PROG:
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				data_base + fwu->off.payload,
+				fwu->bootloader_id,
+				sizeof(fwu->bootloader_id));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write bootloader ID\n",
+					__func__);
+			return retval;
+		}
+		break;
+	default:
+		break;
+	};
+
+	fwu->command = command;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.flash_cmd,
+			&command,
+			sizeof(command));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write command 0x%02x\n",
+				__func__, command);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_write_f34_command(unsigned char cmd)
+{
+	int retval;
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		retval = fwu_write_f34_v7_command(cmd);
+	else
+		retval = fwu_write_f34_v5v6_command(cmd);
+
+	return retval;
+}
+
+static int fwu_write_f34_v7_partition_id(unsigned char cmd)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char partition;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	switch (cmd) {
+	case CMD_WRITE_FW:
+		partition = CORE_CODE_PARTITION;
+		break;
+	case CMD_WRITE_CONFIG:
+	case CMD_READ_CONFIG:
+		if (fwu->config_area == UI_CONFIG_AREA)
+			partition = CORE_CONFIG_PARTITION;
+		else if (fwu->config_area == DP_CONFIG_AREA)
+			partition = DISPLAY_CONFIG_PARTITION;
+		else if (fwu->config_area == PM_CONFIG_AREA)
+			partition = GUEST_SERIALIZATION_PARTITION;
+		else if (fwu->config_area == BL_CONFIG_AREA)
+			partition = GLOBAL_PARAMETERS_PARTITION;
+		else if (fwu->config_area == FLASH_CONFIG_AREA)
+			partition = FLASH_CONFIG_PARTITION;
+		else if (fwu->config_area == UPP_AREA)
+			partition = UTILITY_PARAMETER_PARTITION;
+		break;
+	case CMD_WRITE_LOCKDOWN:
+		partition = DEVICE_CONFIG_PARTITION;
+		break;
+	case CMD_WRITE_GUEST_CODE:
+		partition = GUEST_CODE_PARTITION;
+		break;
+	case CMD_WRITE_BOOTLOADER:
+		partition = BOOTLOADER_PARTITION;
+		break;
+	case CMD_WRITE_UTILITY_PARAM:
+		partition = UTILITY_PARAMETER_PARTITION;
+		break;
+	case CMD_ERASE_ALL:
+		partition = CORE_CODE_PARTITION;
+		break;
+	case CMD_ERASE_BL_CONFIG:
+		partition = GLOBAL_PARAMETERS_PARTITION;
+		break;
+	case CMD_ERASE_UI_CONFIG:
+		partition = CORE_CONFIG_PARTITION;
+		break;
+	case CMD_ERASE_DISP_CONFIG:
+		partition = DISPLAY_CONFIG_PARTITION;
+		break;
+	case CMD_ERASE_FLASH_CONFIG:
+		partition = FLASH_CONFIG_PARTITION;
+		break;
+	case CMD_ERASE_GUEST_CODE:
+		partition = GUEST_CODE_PARTITION;
+		break;
+	case CMD_ERASE_BOOTLOADER:
+		partition = BOOTLOADER_PARTITION;
+		break;
+	case CMD_ENABLE_FLASH_PROG:
+		partition = BOOTLOADER_PARTITION;
+		break;
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid command 0x%02x\n",
+				__func__, cmd);
+		return -EINVAL;
+	};
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.partition_id,
+			&partition,
+			sizeof(partition));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write partition ID\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_write_f34_partition_id(unsigned char cmd)
+{
+	int retval;
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		retval = fwu_write_f34_v7_partition_id(cmd);
+	else
+		retval = 0;
+
+	return retval;
+}
+
+static int fwu_read_f34_v7_partition_table(unsigned char *partition_table)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char length[2];
+	unsigned short block_number = 0;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	fwu->config_area = FLASH_CONFIG_AREA;
+
+	retval = fwu_write_f34_partition_id(CMD_READ_CONFIG);
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.block_number,
+			(unsigned char *)&block_number,
+			sizeof(block_number));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write block number\n",
+				__func__);
+		return retval;
+	}
+
+	length[0] = (unsigned char)(fwu->flash_config_length & MASK_8BIT);
+	length[1] = (unsigned char)(fwu->flash_config_length >> 8);
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.transfer_length,
+			length,
+			sizeof(length));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write transfer length\n",
+				__func__);
+		return retval;
+	}
+
+	retval = fwu_write_f34_command(CMD_READ_CONFIG);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write command\n",
+				__func__);
+		return retval;
+	}
+
+	msleep(READ_CONFIG_WAIT_MS);
+
+	retval = fwu_wait_for_idle(WRITE_WAIT_MS, true);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to wait for idle status\n",
+				__func__);
+		return retval;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			data_base + fwu->off.payload,
+			partition_table,
+			fwu->partition_table_bytes);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read block data\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_read_f34_v7_queries(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char query_base;
+	unsigned char index;
+	unsigned char offset;
+	unsigned char *ptable;
+	struct f34_v7_query_0 query_0;
+	struct f34_v7_query_1_7 query_1_7;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	query_base = fwu->f34_fd.query_base_addr;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			query_base,
+			query_0.data,
+			sizeof(query_0.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read query 0\n",
+				__func__);
+		return retval;
+	}
+
+	offset = query_0.subpacket_1_size + 1;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			query_base + offset,
+			query_1_7.data,
+			sizeof(query_1_7.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read queries 1 to 7\n",
+				__func__);
+		return retval;
+	}
+
+	fwu->bootloader_id[0] = query_1_7.bl_minor_revision;
+	fwu->bootloader_id[1] = query_1_7.bl_major_revision;
+
+	if (fwu->bootloader_id[1] == BL_V8)
+		fwu->bl_version = BL_V8;
+
+	fwu->block_size = query_1_7.block_size_15_8 << 8 |
+			query_1_7.block_size_7_0;
+
+	fwu->flash_config_length = query_1_7.flash_config_length_15_8 << 8 |
+			query_1_7.flash_config_length_7_0;
+
+	fwu->payload_length = query_1_7.payload_length_15_8 << 8 |
+			query_1_7.payload_length_7_0;
+
+	fwu->off.flash_status = V7_FLASH_STATUS_OFFSET;
+	fwu->off.partition_id = V7_PARTITION_ID_OFFSET;
+	fwu->off.block_number = V7_BLOCK_NUMBER_OFFSET;
+	fwu->off.transfer_length = V7_TRANSFER_LENGTH_OFFSET;
+	fwu->off.flash_cmd = V7_COMMAND_OFFSET;
+	fwu->off.payload = V7_PAYLOAD_OFFSET;
+
+	index = sizeof(query_1_7.data) - V7_PARTITION_SUPPORT_BYTES;
+
+	fwu->partitions = 0;
+	for (offset = 0; offset < V7_PARTITION_SUPPORT_BYTES; offset++) {
+		for (ii = 0; ii < 8; ii++) {
+			if (query_1_7.data[index + offset] & (1 << ii))
+				fwu->partitions++;
+		}
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Supported partitions: 0x%02x\n",
+				__func__, query_1_7.data[index + offset]);
+	}
+
+	fwu->partition_table_bytes = fwu->partitions * 8 + 2;
+
+	ptable = kzalloc(fwu->partition_table_bytes, GFP_KERNEL);
+	if (!ptable) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for partition table\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	retval = fwu_read_f34_v7_partition_table(ptable);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read partition table\n",
+				__func__);
+		kfree(ptable);
+		return retval;
+	}
+
+	fwu_parse_partition_table(ptable, &fwu->blkcount, &fwu->phyaddr);
+
+	if (fwu->blkcount.dp_config)
+		fwu->flash_properties.has_disp_config = 1;
+	else
+		fwu->flash_properties.has_disp_config = 0;
+
+	if (fwu->blkcount.pm_config)
+		fwu->flash_properties.has_pm_config = 1;
+	else
+		fwu->flash_properties.has_pm_config = 0;
+
+	if (fwu->blkcount.bl_config)
+		fwu->flash_properties.has_bl_config = 1;
+	else
+		fwu->flash_properties.has_bl_config = 0;
+
+	if (fwu->blkcount.guest_code)
+		fwu->has_guest_code = 1;
+	else
+		fwu->has_guest_code = 0;
+
+	if (fwu->blkcount.utility_param)
+		fwu->has_utility_param = 1;
+	else
+		fwu->has_utility_param = 0;
+
+	kfree(ptable);
+
+	return 0;
+}
+
+static int fwu_read_f34_v5v6_queries(void)
+{
+	int retval;
+	unsigned char count;
+	unsigned char base;
+	unsigned char offset;
+	unsigned char buf[10];
+	struct f34_v5v6_flash_properties_2 properties_2;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	base = fwu->f34_fd.query_base_addr;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			base + V5V6_BOOTLOADER_ID_OFFSET,
+			fwu->bootloader_id,
+			sizeof(fwu->bootloader_id));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read bootloader ID\n",
+				__func__);
+		return retval;
+	}
+
+	if (fwu->bl_version == BL_V5) {
+		fwu->off.properties = V5_PROPERTIES_OFFSET;
+		fwu->off.block_size = V5_BLOCK_SIZE_OFFSET;
+		fwu->off.block_count = V5_BLOCK_COUNT_OFFSET;
+		fwu->off.block_number = V5_BLOCK_NUMBER_OFFSET;
+		fwu->off.payload = V5_BLOCK_DATA_OFFSET;
+	} else if (fwu->bl_version == BL_V6) {
+		fwu->off.properties = V6_PROPERTIES_OFFSET;
+		fwu->off.properties_2 = V6_PROPERTIES_2_OFFSET;
+		fwu->off.block_size = V6_BLOCK_SIZE_OFFSET;
+		fwu->off.block_count = V6_BLOCK_COUNT_OFFSET;
+		fwu->off.gc_block_count = V6_GUEST_CODE_BLOCK_COUNT_OFFSET;
+		fwu->off.block_number = V6_BLOCK_NUMBER_OFFSET;
+		fwu->off.payload = V6_BLOCK_DATA_OFFSET;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			base + fwu->off.block_size,
+			buf,
+			2);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read block size info\n",
+				__func__);
+		return retval;
+	}
+
+	batohs(&fwu->block_size, &(buf[0]));
+
+	if (fwu->bl_version == BL_V5) {
+		fwu->off.flash_cmd = fwu->off.payload + fwu->block_size;
+		fwu->off.flash_status = fwu->off.flash_cmd;
+	} else if (fwu->bl_version == BL_V6) {
+		fwu->off.flash_cmd = V6_FLASH_COMMAND_OFFSET;
+		fwu->off.flash_status = V6_FLASH_STATUS_OFFSET;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			base + fwu->off.properties,
+			fwu->flash_properties.data,
+			sizeof(fwu->flash_properties.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read flash properties\n",
+				__func__);
+		return retval;
+	}
+
+	count = 4;
+
+	if (fwu->flash_properties.has_pm_config)
+		count += 2;
+
+	if (fwu->flash_properties.has_bl_config)
+		count += 2;
+
+	if (fwu->flash_properties.has_disp_config)
+		count += 2;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			base + fwu->off.block_count,
+			buf,
+			count);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read block count info\n",
+				__func__);
+		return retval;
+	}
+
+	batohs(&fwu->blkcount.ui_firmware, &(buf[0]));
+	batohs(&fwu->blkcount.ui_config, &(buf[2]));
+
+	count = 4;
+
+	if (fwu->flash_properties.has_pm_config) {
+		batohs(&fwu->blkcount.pm_config, &(buf[count]));
+		count += 2;
+	}
+
+	if (fwu->flash_properties.has_bl_config) {
+		batohs(&fwu->blkcount.bl_config, &(buf[count]));
+		count += 2;
+	}
+
+	if (fwu->flash_properties.has_disp_config)
+		batohs(&fwu->blkcount.dp_config, &(buf[count]));
+
+	fwu->has_guest_code = false;
+#ifdef SYNA_TDDI
+	fwu->has_force_config = false;
+	fwu->has_lockdown_data = false;
+	fwu->has_lcm_data = false;
+	fwu->has_oem_data = false;
+#endif
+
+	if (fwu->flash_properties.has_query4) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				base + fwu->off.properties_2,
+				properties_2.data,
+				sizeof(properties_2.data));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read flash properties 2\n",
+					__func__);
+			return retval;
+		}
+		offset = fwu->off.properties_2 + 1;
+		count = 0;
+		if (properties_2.has_guest_code) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					base + offset + count,
+					buf,
+					2);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to read guest code block count\n",
+						__func__);
+				return retval;
+			}
+
+			batohs(&fwu->blkcount.guest_code, &(buf[0]));
+			count++;
+			fwu->has_guest_code = true;
+		}
+#ifdef SYNA_TDDI
+		if (properties_2.has_force_config) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					base + offset + count,
+					buf,
+					2);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read tddi force block count\n",
+					__func__);
+				return retval;
+			}
+			batohs(&fwu->blkcount.tddi_force_config, &(buf[0]));
+			count++;
+			fwu->has_force_config = true;
+		}
+		if (properties_2.has_lockdown_data) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					base + offset + count,
+					buf,
+					2);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read tddi lockdown block count\n",
+					__func__);
+				return retval;
+			}
+			batohs(&fwu->blkcount.tddi_lockdown_data, &(buf[0]));
+			count++;
+			fwu->has_lockdown_data = true;
+		}
+		if (properties_2.has_lcm_data) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					base + offset + count,
+					buf,
+					2);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read tddi lcm block count\n",
+					__func__);
+				return retval;
+			}
+			batohs(&fwu->blkcount.tddi_lcm_data, &(buf[0]));
+			count++;
+			fwu->has_lcm_data = true;
+		}
+		if (properties_2.has_oem_data) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					base + offset + count,
+					buf,
+					2);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read tddi oem block count\n",
+					__func__);
+				return retval;
+			}
+			batohs(&fwu->blkcount.tddi_oem_data, &(buf[0]));
+			fwu->has_oem_data = true;
+		}
+#endif
+	}
+
+	fwu->has_utility_param = false;
+
+	return 0;
+}
+
+static int fwu_read_f34_queries(void)
+{
+	int retval;
+
+	memset(&fwu->blkcount, 0x00, sizeof(fwu->blkcount));
+	memset(&fwu->phyaddr, 0x00, sizeof(fwu->phyaddr));
+
+	if (fwu->bl_version == BL_V7)
+		retval = fwu_read_f34_v7_queries();
+	else
+		retval = fwu_read_f34_v5v6_queries();
+
+	return retval;
+}
+
+static int fwu_write_f34_v7_blocks(unsigned char *block_ptr,
+		unsigned short block_cnt, unsigned char command)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char length[2];
+	unsigned short transfer;
+	unsigned short remaining = block_cnt;
+	unsigned short block_number = 0;
+	unsigned short left_bytes;
+	unsigned short write_size;
+	unsigned short max_write_size;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	retval = fwu_write_f34_partition_id(command);
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.block_number,
+			(unsigned char *)&block_number,
+			sizeof(block_number));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write block number\n",
+				__func__);
+		return retval;
+	}
+
+	do {
+		if (remaining / fwu->payload_length)
+			transfer = fwu->payload_length;
+		else
+			transfer = remaining;
+
+		length[0] = (unsigned char)(transfer & MASK_8BIT);
+		length[1] = (unsigned char)(transfer >> 8);
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				data_base + fwu->off.transfer_length,
+				length,
+				sizeof(length));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write transfer length (remaining = %d)\n",
+					__func__, remaining);
+			return retval;
+		}
+
+		retval = fwu_write_f34_command(command);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write command (remaining = %d)\n",
+					__func__, remaining);
+			return retval;
+		}
+
+#ifdef MAX_WRITE_SIZE
+		max_write_size = MAX_WRITE_SIZE;
+		if (max_write_size >= transfer * fwu->block_size)
+			max_write_size = transfer * fwu->block_size;
+		else if (max_write_size > fwu->block_size)
+			max_write_size -= max_write_size % fwu->block_size;
+		else
+			max_write_size = fwu->block_size;
+#else
+		max_write_size = transfer * fwu->block_size;
+#endif
+		left_bytes = transfer * fwu->block_size;
+
+		do {
+			if (left_bytes / max_write_size)
+				write_size = max_write_size;
+			else
+				write_size = left_bytes;
+
+			retval = synaptics_rmi4_reg_write(rmi4_data,
+					data_base + fwu->off.payload,
+					block_ptr,
+					write_size);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to write block data (remaining = %d)\n",
+						__func__, remaining);
+				return retval;
+			}
+
+			block_ptr += write_size;
+			left_bytes -= write_size;
+		} while (left_bytes);
+
+		retval = fwu_wait_for_idle(WRITE_WAIT_MS, false);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to wait for idle status (remaining = %d)\n",
+					__func__, remaining);
+			return retval;
+		}
+
+		remaining -= transfer;
+	} while (remaining);
+
+	return 0;
+}
+
+static int fwu_write_f34_v5v6_blocks(unsigned char *block_ptr,
+		unsigned short block_cnt, unsigned char command)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char block_number[] = {0, 0};
+	unsigned short blk;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	block_number[1] |= (fwu->config_area << 5);
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.block_number,
+			block_number,
+			sizeof(block_number));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write block number\n",
+				__func__);
+		return retval;
+	}
+
+	for (blk = 0; blk < block_cnt; blk++) {
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				data_base + fwu->off.payload,
+				block_ptr,
+				fwu->block_size);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write block data (block %d)\n",
+					__func__, blk);
+			return retval;
+		}
+
+		retval = fwu_write_f34_command(command);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write command for block %d\n",
+					__func__, blk);
+			return retval;
+		}
+
+		retval = fwu_wait_for_idle(WRITE_WAIT_MS, false);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to wait for idle status (block %d)\n",
+					__func__, blk);
+			return retval;
+		}
+
+		block_ptr += fwu->block_size;
+	}
+
+	return 0;
+}
+
+static int fwu_write_f34_blocks(unsigned char *block_ptr,
+		unsigned short block_cnt, unsigned char cmd)
+{
+	int retval;
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		retval = fwu_write_f34_v7_blocks(block_ptr, block_cnt, cmd);
+	else
+		retval = fwu_write_f34_v5v6_blocks(block_ptr, block_cnt, cmd);
+
+	return retval;
+}
+
+static int fwu_read_f34_v7_blocks(unsigned short block_cnt,
+		unsigned char command)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char length[2];
+	unsigned short transfer;
+	unsigned short remaining = block_cnt;
+	unsigned short block_number = 0;
+	unsigned short index = 0;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	retval = fwu_write_f34_partition_id(command);
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.block_number,
+			(unsigned char *)&block_number,
+			sizeof(block_number));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write block number\n",
+				__func__);
+		return retval;
+	}
+
+	do {
+		if (remaining / fwu->payload_length)
+			transfer = fwu->payload_length;
+		else
+			transfer = remaining;
+
+		length[0] = (unsigned char)(transfer & MASK_8BIT);
+		length[1] = (unsigned char)(transfer >> 8);
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				data_base + fwu->off.transfer_length,
+				length,
+				sizeof(length));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write transfer length (remaining = %d)\n",
+					__func__, remaining);
+			return retval;
+		}
+
+		retval = fwu_write_f34_command(command);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write command (remaining = %d)\n",
+					__func__, remaining);
+			return retval;
+		}
+
+		retval = fwu_wait_for_idle(WRITE_WAIT_MS, false);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to wait for idle status (remaining = %d)\n",
+					__func__, remaining);
+			return retval;
+		}
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				data_base + fwu->off.payload,
+				&fwu->read_config_buf[index],
+				transfer * fwu->block_size);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read block data (remaining = %d)\n",
+					__func__, remaining);
+			return retval;
+		}
+
+		index += (transfer * fwu->block_size);
+		remaining -= transfer;
+	} while (remaining);
+
+	return 0;
+}
+
+static int fwu_read_f34_v5v6_blocks(unsigned short block_cnt,
+		unsigned char command)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char block_number[] = {0, 0};
+	unsigned short blk;
+	unsigned short index = 0;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f34_fd.data_base_addr;
+
+	block_number[1] |= (fwu->config_area << 5);
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			data_base + fwu->off.block_number,
+			block_number,
+			sizeof(block_number));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write block number\n",
+				__func__);
+		return retval;
+	}
+
+	for (blk = 0; blk < block_cnt; blk++) {
+		retval = fwu_write_f34_command(command);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write read config command\n",
+					__func__);
+			return retval;
+		}
+
+		retval = fwu_wait_for_idle(WRITE_WAIT_MS, false);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to wait for idle status\n",
+					__func__);
+			return retval;
+		}
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				data_base + fwu->off.payload,
+				&fwu->read_config_buf[index],
+				fwu->block_size);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read block data (block %d)\n",
+					__func__, blk);
+			return retval;
+		}
+
+		index += fwu->block_size;
+	}
+
+	return 0;
+}
+
+static int fwu_read_f34_blocks(unsigned short block_cnt, unsigned char cmd)
+{
+	int retval;
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		retval = fwu_read_f34_v7_blocks(block_cnt, cmd);
+	else
+		retval = fwu_read_f34_v5v6_blocks(block_cnt, cmd);
+
+	return retval;
+}
+
+static int fwu_get_image_firmware_id(unsigned int *fw_id)
+{
+	int retval;
+	unsigned char index = 0;
+	char *strptr;
+	char *firmware_id;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (fwu->img.contains_firmware_id) {
+		*fw_id = fwu->img.firmware_id;
+	} else {
+		strptr = strnstr(fwu->image_name, "PR", MAX_IMAGE_NAME_LEN);
+		if (!strptr) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: No valid PR number (PRxxxxxxx) found in image file name (%s)\n",
+					__func__, fwu->image_name);
+			return -EINVAL;
+		}
+
+		strptr += 2;
+		firmware_id = kzalloc(MAX_FIRMWARE_ID_LEN, GFP_KERNEL);
+		if (!firmware_id) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for firmware_id\n",
+					__func__);
+			return -ENOMEM;
+		}
+		while (strptr[index] >= '0' && strptr[index] <= '9') {
+			firmware_id[index] = strptr[index];
+			index++;
+			if (index == MAX_FIRMWARE_ID_LEN - 1)
+				break;
+		}
+
+		retval = sstrtoul(firmware_id, 10, (unsigned long *)fw_id);
+		kfree(firmware_id);
+		if (retval) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to obtain image firmware ID\n",
+					__func__);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int fwu_get_device_config_id(void)
+{
+	int retval;
+	unsigned char config_id_size;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		config_id_size = V7_CONFIG_ID_SIZE;
+	else
+		config_id_size = V5V6_CONFIG_ID_SIZE;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+				fwu->f34_fd.ctrl_base_addr,
+				fwu->config_id,
+				config_id_size);
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static enum flash_area fwu_go_nogo(void)
+{
+	int retval;
+	enum flash_area flash_area = NONE;
+	unsigned char ii;
+	unsigned char config_id_size;
+	unsigned int device_fw_id;
+	unsigned int image_fw_id;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (fwu->force_update) {
+		flash_area = UI_FIRMWARE;
+		goto exit;
+	}
+
+	/* Update both UI and config if device is in bootloader mode */
+	if (fwu->bl_mode_device) {
+		flash_area = UI_FIRMWARE;
+		goto exit;
+	}
+
+	/* Get device firmware ID */
+	device_fw_id = rmi4_data->firmware_id;
+	dev_info(rmi4_data->pdev->dev.parent,
+			"%s: Device firmware ID = %d\n",
+			__func__, device_fw_id);
+
+	/* Get image firmware ID */
+	retval = fwu_get_image_firmware_id(&image_fw_id);
+	if (retval < 0) {
+		flash_area = NONE;
+		goto exit;
+	}
+	dev_info(rmi4_data->pdev->dev.parent,
+			"%s: Image firmware ID = %d\n",
+			__func__, image_fw_id);
+
+	if (image_fw_id > device_fw_id) {
+		flash_area = UI_FIRMWARE;
+		goto exit;
+	} else if (image_fw_id < device_fw_id) {
+		dev_info(rmi4_data->pdev->dev.parent,
+				"%s: Image firmware ID older than device firmware ID\n",
+				__func__);
+		flash_area = NONE;
+		goto exit;
+	}
+
+	/* Get device config ID */
+	retval = fwu_get_device_config_id();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read device config ID\n",
+				__func__);
+		flash_area = NONE;
+		goto exit;
+	}
+
+	if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+		config_id_size = V7_CONFIG_ID_SIZE;
+	else
+		config_id_size = V5V6_CONFIG_ID_SIZE;
+
+	for (ii = 0; ii < config_id_size; ii++) {
+		if (fwu->img.ui_config.data[ii] > fwu->config_id[ii]) {
+			flash_area = UI_CONFIG;
+			goto exit;
+		} else if (fwu->img.ui_config.data[ii] < fwu->config_id[ii]) {
+			flash_area = NONE;
+			goto exit;
+		}
+	}
+
+	flash_area = NONE;
+
+exit:
+	if (flash_area == NONE) {
+		dev_info(rmi4_data->pdev->dev.parent,
+				"%s: No need to do reflash\n",
+				__func__);
+	} else {
+		dev_info(rmi4_data->pdev->dev.parent,
+				"%s: Updating %s\n",
+				__func__,
+				flash_area == UI_FIRMWARE ?
+				"UI firmware and config" :
+				"UI config only");
+	}
+
+	return flash_area;
+}
+
+static int fwu_scan_pdt(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char intr_count = 0;
+	unsigned char intr_off;
+	unsigned char intr_src;
+	unsigned short addr;
+	bool f01found = false;
+	bool f34found = false;
+	bool f35found = false;
+	struct synaptics_rmi4_fn_desc rmi_fd;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	fwu->in_ub_mode = false;
+
+	for (addr = PDT_START; addr > PDT_END; addr -= PDT_ENTRY_SIZE) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				addr,
+				(unsigned char *)&rmi_fd,
+				sizeof(rmi_fd));
+		if (retval < 0)
+			return retval;
+
+		if (rmi_fd.fn_number) {
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Found F%02x\n",
+					__func__, rmi_fd.fn_number);
+			switch (rmi_fd.fn_number) {
+			case SYNAPTICS_RMI4_F01:
+				f01found = true;
+
+				rmi4_data->f01_query_base_addr =
+						rmi_fd.query_base_addr;
+				rmi4_data->f01_ctrl_base_addr =
+						rmi_fd.ctrl_base_addr;
+				rmi4_data->f01_data_base_addr =
+						rmi_fd.data_base_addr;
+				rmi4_data->f01_cmd_base_addr =
+						rmi_fd.cmd_base_addr;
+				break;
+			case SYNAPTICS_RMI4_F34:
+				f34found = true;
+				fwu->f34_fd.query_base_addr =
+						rmi_fd.query_base_addr;
+				fwu->f34_fd.ctrl_base_addr =
+						rmi_fd.ctrl_base_addr;
+				fwu->f34_fd.data_base_addr =
+						rmi_fd.data_base_addr;
+
+				switch (rmi_fd.fn_version) {
+				case F34_V0:
+					fwu->bl_version = BL_V5;
+					break;
+				case F34_V1:
+					fwu->bl_version = BL_V6;
+					break;
+				case F34_V2:
+					fwu->bl_version = BL_V7;
+					break;
+				default:
+					dev_err(rmi4_data->pdev->dev.parent,
+							"%s: Unrecognized F34 version\n",
+							__func__);
+					return -EINVAL;
+				}
+
+				fwu->intr_mask = 0;
+				intr_src = rmi_fd.intr_src_count;
+				intr_off = intr_count % 8;
+				for (ii = intr_off;
+						ii < (intr_src + intr_off);
+						ii++) {
+					fwu->intr_mask |= 1 << ii;
+				}
+				break;
+			case SYNAPTICS_RMI4_F35:
+				f35found = true;
+				fwu->f35_fd.query_base_addr =
+						rmi_fd.query_base_addr;
+				fwu->f35_fd.ctrl_base_addr =
+						rmi_fd.ctrl_base_addr;
+				fwu->f35_fd.data_base_addr =
+						rmi_fd.data_base_addr;
+				fwu->f35_fd.cmd_base_addr =
+						rmi_fd.cmd_base_addr;
+				break;
+			}
+		} else {
+			break;
+		}
+
+		intr_count += rmi_fd.intr_src_count;
+	}
+
+	if (!f01found || !f34found) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to find both F01 and F34\n",
+				__func__);
+		if (!f35found) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to find F35\n",
+					__func__);
+			return -EINVAL;
+		} else {
+			fwu->in_ub_mode = true;
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: In microbootloader mode\n",
+					__func__);
+			fwu_recovery_check_status();
+			return 0;
+		}
+	}
+
+	rmi4_data->intr_mask[0] |= fwu->intr_mask;
+
+	addr = rmi4_data->f01_ctrl_base_addr + 1;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			addr,
+			&(rmi4_data->intr_mask[0]),
+			sizeof(rmi4_data->intr_mask[0]));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set interrupt enable bit\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_enter_flash_prog(void)
+{
+	int retval;
+	struct f01_device_control f01_device_control;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_read_flash_status();
+	if (retval < 0)
+		return retval;
+
+	if (fwu->in_bl_mode)
+		return 0;
+
+	retval = rmi4_data->irq_enable(rmi4_data, false, true);
+	if (retval < 0)
+		return retval;
+
+	msleep(INT_DISABLE_WAIT_MS);
+
+	retval = fwu_write_f34_command(CMD_ENABLE_FLASH_PROG);
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_wait_for_idle(ENABLE_WAIT_MS, false);
+	if (retval < 0)
+		return retval;
+
+	if (!fwu->in_bl_mode) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: BL mode not entered\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (rmi4_data->hw_if->bl_hw_init) {
+		retval = rmi4_data->hw_if->bl_hw_init(rmi4_data);
+		if (retval < 0)
+			return retval;
+	}
+
+	retval = fwu_scan_pdt();
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_read_f34_queries();
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			f01_device_control.data,
+			sizeof(f01_device_control.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read F01 device control\n",
+				__func__);
+		return retval;
+	}
+
+	f01_device_control.nosleep = true;
+	f01_device_control.sleep_mode = SLEEP_MODE_NORMAL;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			f01_device_control.data,
+			sizeof(f01_device_control.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write F01 device control\n",
+				__func__);
+		return retval;
+	}
+
+	msleep(ENTER_FLASH_PROG_WAIT_MS);
+
+	return retval;
+}
+
+static int fwu_check_ui_firmware_size(void)
+{
+	unsigned short block_count;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->img.ui_firmware.size / fwu->block_size;
+
+	if (block_count != fwu->blkcount.ui_firmware) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: UI firmware size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fwu_check_ui_configuration_size(void)
+{
+	unsigned short block_count;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->img.ui_config.size / fwu->block_size;
+
+	if (block_count != fwu->blkcount.ui_config) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: UI configuration size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fwu_check_dp_configuration_size(void)
+{
+	unsigned short block_count;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->img.dp_config.size / fwu->block_size;
+
+	if (block_count != fwu->blkcount.dp_config) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Display configuration size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+static int fwu_check_pm_configuration_size(void)
+{
+	unsigned short block_count;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->img.pm_config.size / fwu->block_size;
+
+	if (block_count != fwu->blkcount.pm_config) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Permanent configuration size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+#endif
+
+static int fwu_check_bl_configuration_size(void)
+{
+	unsigned short block_count;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->img.bl_config.size / fwu->block_size;
+
+	if (block_count != fwu->blkcount.bl_config) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Bootloader configuration size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fwu_check_guest_code_size(void)
+{
+	unsigned short block_count;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->img.guest_code.size / fwu->block_size;
+	if (block_count != fwu->blkcount.guest_code) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Guest code size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fwu_erase_configuration(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	switch (fwu->config_area) {
+	case UI_CONFIG_AREA:
+		retval = fwu_write_f34_command(CMD_ERASE_UI_CONFIG);
+		if (retval < 0)
+			return retval;
+		break;
+	case DP_CONFIG_AREA:
+		retval = fwu_write_f34_command(CMD_ERASE_DISP_CONFIG);
+		if (retval < 0)
+			return retval;
+		break;
+	case BL_CONFIG_AREA:
+		retval = fwu_write_f34_command(CMD_ERASE_BL_CONFIG);
+		if (retval < 0)
+			return retval;
+		break;
+	case FLASH_CONFIG_AREA:
+		retval = fwu_write_f34_command(CMD_ERASE_FLASH_CONFIG);
+		if (retval < 0)
+			return retval;
+		break;
+	case UPP_AREA:
+		retval = fwu_write_f34_command(CMD_ERASE_UTILITY_PARAMETER);
+		if (retval < 0)
+			return retval;
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid config area\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Erase command written\n",
+			__func__);
+
+	retval = fwu_wait_for_idle(ERASE_WAIT_MS, false);
+	if (retval < 0)
+		return retval;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Idle status detected\n",
+			__func__);
+
+	return retval;
+}
+
+static int fwu_erase_bootloader(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_write_f34_command(CMD_ERASE_BOOTLOADER);
+	if (retval < 0)
+		return retval;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Erase command written\n",
+			__func__);
+
+	retval = fwu_wait_for_idle(ERASE_WAIT_MS, false);
+	if (retval < 0)
+		return retval;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Idle status detected\n",
+			__func__);
+
+	return 0;
+}
+
+#ifdef SYNA_TDDI
+static int fwu_erase_lockdown_data(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_write_f34_command(CMD_ERASE_LOCKDOWN_DATA);
+	if (retval < 0)
+		return retval;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Erase command written\n",
+			__func__);
+
+	msleep(100);
+
+	retval = fwu_wait_for_idle(ERASE_WAIT_MS, false);
+	if (retval < 0)
+		return retval;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Idle status detected\n",
+			__func__);
+
+	return 0;
+}
+
+#endif
+
+static int fwu_erase_guest_code(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_write_f34_command(CMD_ERASE_GUEST_CODE);
+	if (retval < 0)
+		return retval;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Erase command written\n",
+			__func__);
+
+	retval = fwu_wait_for_idle(ERASE_WAIT_MS, false);
+	if (retval < 0)
+		return retval;
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Idle status detected\n",
+			__func__);
+
+	return 0;
+}
+
+static int fwu_erase_all(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (fwu->bl_version == BL_V7) {
+		retval = fwu_write_f34_command(CMD_ERASE_UI_FIRMWARE);
+		if (retval < 0)
+			return retval;
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Erase command written\n",
+				__func__);
+
+		retval = fwu_wait_for_idle(ERASE_WAIT_MS, false);
+		if (retval < 0)
+			return retval;
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Idle status detected\n",
+				__func__);
+
+		fwu->config_area = UI_CONFIG_AREA;
+		retval = fwu_erase_configuration();
+		if (retval < 0)
+			return retval;
+	} else {
+		retval = fwu_write_f34_command(CMD_ERASE_ALL);
+		if (retval < 0)
+			return retval;
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Erase all command written\n",
+				__func__);
+
+		retval = fwu_wait_for_idle(ERASE_WAIT_MS, false);
+		if (!(fwu->bl_version == BL_V8 &&
+				fwu->flash_status == BAD_PARTITION_TABLE)) {
+			if (retval < 0)
+				return retval;
+		}
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Idle status detected\n",
+				__func__);
+
+		if (fwu->bl_version == BL_V8)
+			return 0;
+	}
+
+	if (fwu->flash_properties.has_disp_config) {
+		fwu->config_area = DP_CONFIG_AREA;
+		retval = fwu_erase_configuration();
+		if (retval < 0)
+			return retval;
+	}
+
+	if (fwu->has_guest_code) {
+		retval = fwu_erase_guest_code();
+		if (retval < 0)
+			return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_write_firmware(void)
+{
+	unsigned short firmware_block_count;
+
+	firmware_block_count = fwu->img.ui_firmware.size / fwu->block_size;
+
+	return fwu_write_f34_blocks((unsigned char *)fwu->img.ui_firmware.data,
+			firmware_block_count, CMD_WRITE_FW);
+}
+
+static int fwu_write_bootloader(void)
+{
+	int retval;
+	unsigned short bootloader_block_count;
+
+	bootloader_block_count = fwu->img.bl_image.size / fwu->block_size;
+
+	fwu->write_bootloader = true;
+	retval = fwu_write_f34_blocks((unsigned char *)fwu->img.bl_image.data,
+			bootloader_block_count, CMD_WRITE_BOOTLOADER);
+	fwu->write_bootloader = false;
+
+	return retval;
+}
+
+static int fwu_write_utility_parameter(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char checksum_array[4];
+	unsigned char *pbuf;
+	unsigned short remaining_size;
+	unsigned short utility_param_size;
+	unsigned long checksum;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	utility_param_size = fwu->blkcount.utility_param * fwu->block_size;
+	retval = fwu_allocate_read_config_buf(utility_param_size);
+	if (retval < 0)
+		return retval;
+	memset(fwu->read_config_buf, 0x00, utility_param_size);
+
+	pbuf = fwu->read_config_buf;
+	remaining_size = utility_param_size - 4;
+
+	for (ii = 0; ii < MAX_UTILITY_PARAMS; ii++) {
+		if (fwu->img.utility_param_id[ii] == UNUSED)
+			continue;
+
+#ifdef F51_DISCRETE_FORCE
+		if (fwu->img.utility_param_id[ii] == FORCE_PARAMETER) {
+			if (fwu->bl_mode_device) {
+				dev_info(rmi4_data->pdev->dev.parent,
+						"%s: Device in bootloader mode, skipping calibration data restoration\n",
+						__func__);
+				goto image_param;
+			}
+			retval = secure_memcpy(&(pbuf[4]),
+					remaining_size - 4,
+					fwu->cal_data,
+					fwu->cal_data_buf_size,
+					fwu->cal_data_size);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to copy force calibration data\n",
+						__func__);
+				return retval;
+			}
+			pbuf[0] = FORCE_PARAMETER;
+			pbuf[1] = 0x00;
+			pbuf[2] = (4 + fwu->cal_data_size) / 2;
+			pbuf += (fwu->cal_data_size + 4);
+			remaining_size -= (fwu->cal_data_size + 4);
+			continue;
+		}
+image_param:
+#endif
+
+		retval = secure_memcpy(pbuf,
+				remaining_size,
+				fwu->img.utility_param[ii].data,
+				fwu->img.utility_param[ii].size,
+				fwu->img.utility_param[ii].size);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to copy utility parameter data\n",
+					__func__);
+			return retval;
+		}
+		pbuf += fwu->img.utility_param[ii].size;
+		remaining_size -= fwu->img.utility_param[ii].size;
+	}
+
+	calculate_checksum((unsigned short *)fwu->read_config_buf,
+			((utility_param_size - 4) / 2),
+			&checksum);
+
+	convert_to_little_endian(checksum_array, checksum);
+
+	fwu->read_config_buf[utility_param_size - 4] = checksum_array[0];
+	fwu->read_config_buf[utility_param_size - 3] = checksum_array[1];
+	fwu->read_config_buf[utility_param_size - 2] = checksum_array[2];
+	fwu->read_config_buf[utility_param_size - 1] = checksum_array[3];
+
+	retval = fwu_write_f34_blocks((unsigned char *)fwu->read_config_buf,
+			fwu->blkcount.utility_param, CMD_WRITE_UTILITY_PARAM);
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int fwu_write_configuration(void)
+{
+	return fwu_write_f34_blocks((unsigned char *)fwu->config_data,
+			fwu->config_block_count, CMD_WRITE_CONFIG);
+}
+
+static int fwu_write_ui_configuration(void)
+{
+	fwu->config_area = UI_CONFIG_AREA;
+	fwu->config_data = fwu->img.ui_config.data;
+	fwu->config_size = fwu->img.ui_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+
+	return fwu_write_configuration();
+}
+
+static int fwu_write_dp_configuration(void)
+{
+	fwu->config_area = DP_CONFIG_AREA;
+	fwu->config_data = fwu->img.dp_config.data;
+	fwu->config_size = fwu->img.dp_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+
+	return fwu_write_configuration();
+}
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+static int fwu_write_pm_configuration(void)
+{
+	fwu->config_area = PM_CONFIG_AREA;
+	fwu->config_data = fwu->img.pm_config.data;
+	fwu->config_size = fwu->img.pm_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+
+	return fwu_write_configuration();
+}
+
+#ifdef SYNA_TDDI
+static int fwu_write_tddi_lockdown_data(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_write_f34_blocks(fwu->read_config_buf,
+			fwu->blkcount.tddi_lockdown_data,
+			CMD_WRITE_LOCKDOWN_DATA);
+	if (retval < 0)
+		return retval;
+	rmi4_data->reset_device(rmi4_data, false);
+	return 0;
+}
+#endif
+#endif
+
+static int fwu_write_flash_configuration(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	fwu->config_area = FLASH_CONFIG_AREA;
+	fwu->config_data = fwu->img.fl_config.data;
+	fwu->config_size = fwu->img.fl_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+
+	if (fwu->config_block_count != fwu->blkcount.fl_config) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Flash configuration size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	retval = fwu_erase_configuration();
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_write_configuration();
+	if (retval < 0)
+		return retval;
+
+	rmi4_data->reset_device(rmi4_data, false);
+
+	return 0;
+}
+
+static int fwu_write_guest_code(void)
+{
+	int retval;
+	unsigned short guest_code_block_count;
+
+	guest_code_block_count = fwu->img.guest_code.size / fwu->block_size;
+
+	retval = fwu_write_f34_blocks((unsigned char *)fwu->img.guest_code.data,
+			guest_code_block_count, CMD_WRITE_GUEST_CODE);
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int fwu_write_lockdown(void)
+{
+	unsigned short lockdown_block_count;
+
+	lockdown_block_count = fwu->img.lockdown.size / fwu->block_size;
+
+	return fwu_write_f34_blocks((unsigned char *)fwu->img.lockdown.data,
+			lockdown_block_count, CMD_WRITE_LOCKDOWN);
+}
+
+static int fwu_write_partition_table_v8(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	fwu->config_area = FLASH_CONFIG_AREA;
+	fwu->config_data = fwu->img.fl_config.data;
+	fwu->config_size = fwu->img.fl_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+
+	if (fwu->config_block_count != fwu->blkcount.fl_config) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Flash configuration size mismatch\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	retval = fwu_write_configuration();
+	if (retval < 0)
+		return retval;
+
+	rmi4_data->reset_device(rmi4_data, false);
+
+	return 0;
+}
+
+static int fwu_write_partition_table_v7(void)
+{
+	int retval;
+	unsigned short block_count;
+
+	block_count = fwu->blkcount.bl_config;
+	fwu->config_area = BL_CONFIG_AREA;
+	fwu->config_size = fwu->block_size * block_count;
+
+	retval = fwu_allocate_read_config_buf(fwu->config_size);
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_read_f34_blocks(block_count, CMD_READ_CONFIG);
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_erase_configuration();
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_write_flash_configuration();
+	if (retval < 0)
+		return retval;
+
+	fwu->config_area = BL_CONFIG_AREA;
+	fwu->config_data = fwu->read_config_buf;
+	fwu->config_size = fwu->img.bl_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+
+	retval = fwu_write_configuration();
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int fwu_write_bl_area_v7(void)
+{
+	int retval;
+	bool has_utility_param;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	has_utility_param = fwu->has_utility_param;
+
+	if (fwu->has_utility_param) {
+		fwu->config_area = UPP_AREA;
+		retval = fwu_erase_configuration();
+		if (retval < 0)
+			return retval;
+	}
+
+	fwu->config_area = BL_CONFIG_AREA;
+	retval = fwu_erase_configuration();
+	if (retval < 0)
+		return retval;
+
+	fwu->config_area = FLASH_CONFIG_AREA;
+	retval = fwu_erase_configuration();
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_erase_bootloader();
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_write_bootloader();
+	if (retval < 0)
+		return retval;
+
+	msleep(rmi4_data->hw_if->board_data->reset_delay_ms);
+	rmi4_data->reset_device(rmi4_data, false);
+
+	fwu->config_area = FLASH_CONFIG_AREA;
+	fwu->config_data = fwu->img.fl_config.data;
+	fwu->config_size = fwu->img.fl_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+	retval = fwu_write_configuration();
+	if (retval < 0)
+		return retval;
+	rmi4_data->reset_device(rmi4_data, false);
+
+	fwu->config_area = BL_CONFIG_AREA;
+	fwu->config_data = fwu->img.bl_config.data;
+	fwu->config_size = fwu->img.bl_config.size;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+	retval = fwu_write_configuration();
+	if (retval < 0)
+		return retval;
+
+	if (fwu->img.contains_utility_param) {
+		retval = fwu_write_utility_parameter();
+		if (retval < 0)
+			return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_do_reflash(void)
+{
+	int retval;
+	bool do_bl_update = false;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!fwu->new_partition_table) {
+		retval = fwu_check_ui_firmware_size();
+		if (retval < 0)
+			return retval;
+
+		retval = fwu_check_ui_configuration_size();
+		if (retval < 0)
+			return retval;
+
+		if (fwu->flash_properties.has_disp_config &&
+				fwu->img.contains_disp_config) {
+			retval = fwu_check_dp_configuration_size();
+			if (retval < 0)
+				return retval;
+		}
+
+		if (fwu->has_guest_code && fwu->img.contains_guest_code) {
+			retval = fwu_check_guest_code_size();
+			if (retval < 0)
+				return retval;
+		}
+	} else if (fwu->bl_version == BL_V7) {
+		retval = fwu_check_bl_configuration_size();
+		if (retval < 0)
+			return retval;
+	}
+
+	if (!fwu->has_utility_param && fwu->img.contains_utility_param) {
+		if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+			do_bl_update = true;
+	}
+
+	if (fwu->has_utility_param && !fwu->img.contains_utility_param) {
+		if (fwu->bl_version == BL_V7 || fwu->bl_version == BL_V8)
+			do_bl_update = true;
+	}
+
+	if (!do_bl_update && fwu->incompatible_partition_tables) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Incompatible partition tables\n",
+				__func__);
+		return -EINVAL;
+	} else if (!do_bl_update && fwu->new_partition_table) {
+		if (!fwu->force_update) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Partition table mismatch\n",
+					__func__);
+			return -EINVAL;
+		}
+	}
+
+	retval = fwu_erase_all();
+	if (retval < 0)
+		return retval;
+
+	if (do_bl_update) {
+		retval = fwu_write_bl_area_v7();
+		if (retval < 0)
+			return retval;
+		pr_notice("%s: Bootloader area programmed\n", __func__);
+	} else if (fwu->bl_version == BL_V7 && fwu->new_partition_table) {
+		retval = fwu_write_partition_table_v7();
+		if (retval < 0)
+			return retval;
+		pr_notice("%s: Partition table programmed\n", __func__);
+	} else if (fwu->bl_version == BL_V8) {
+		retval = fwu_write_partition_table_v8();
+		if (retval < 0)
+			return retval;
+		pr_notice("%s: Partition table programmed\n", __func__);
+	}
+
+	fwu->config_area = UI_CONFIG_AREA;
+	if (fwu->flash_properties.has_disp_config &&
+			fwu->img.contains_disp_config) {
+		retval = fwu_write_dp_configuration();
+		if (retval < 0)
+			return retval;
+		pr_notice("%s: Display configuration programmed\n", __func__);
+	}
+
+	retval = fwu_write_ui_configuration();
+	if (retval < 0)
+		return retval;
+	pr_notice("%s: Configuration programmed\n", __func__);
+
+	if (fwu->has_guest_code && fwu->img.contains_guest_code) {
+		retval = fwu_write_guest_code();
+		if (retval < 0)
+			return retval;
+		pr_notice("%s: Guest code programmed\n", __func__);
+	}
+
+	retval = fwu_write_firmware();
+	if (retval < 0)
+		return retval;
+	pr_notice("%s: Firmware programmed\n", __func__);
+
+	return retval;
+}
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+static int fwu_do_read_config(void)
+{
+	int retval;
+	unsigned short block_count;
+	unsigned short config_area;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	switch (fwu->config_area) {
+	case UI_CONFIG_AREA:
+		block_count = fwu->blkcount.ui_config;
+		break;
+	case DP_CONFIG_AREA:
+		if (!fwu->flash_properties.has_disp_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Display configuration not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		block_count = fwu->blkcount.dp_config;
+		break;
+	case PM_CONFIG_AREA:
+		if (!fwu->flash_properties.has_pm_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Permanent configuration not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		block_count = fwu->blkcount.pm_config;
+		break;
+	case BL_CONFIG_AREA:
+		if (!fwu->flash_properties.has_bl_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Bootloader configuration not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		block_count = fwu->blkcount.bl_config;
+		break;
+	case UPP_AREA:
+		if (!fwu->has_utility_param) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Utility parameter not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		block_count = fwu->blkcount.utility_param;
+		break;
+#ifdef SYNA_TDDI
+	case TDDI_FORCE_CONFIG_AREA:
+		if (!fwu->has_force_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: force configuration not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		block_count = fwu->blkcount.tddi_force_config;
+		break;
+	case TDDI_OEM_DATA_AREA:
+		if (!fwu->has_oem_data) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: oem data not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		block_count = fwu->blkcount.tddi_oem_data;
+		break;
+	case TDDI_LCM_DATA_AREA:
+		if (!fwu->has_lcm_data) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: lcm data not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		block_count = fwu->blkcount.tddi_lcm_data;
+		break;
+#endif
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid config area\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (block_count == 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid block count\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&rmi4_data->rmi4_exp_init_mutex);
+
+	if (fwu->bl_version == BL_V5 || fwu->bl_version == BL_V6) {
+		config_area = fwu->config_area;
+		retval = fwu_enter_flash_prog();
+		fwu->config_area = config_area;
+		if (retval < 0)
+			goto exit;
+	}
+
+	fwu->config_size = fwu->block_size * block_count;
+
+	retval = fwu_allocate_read_config_buf(fwu->config_size);
+	if (retval < 0)
+		goto exit;
+
+	retval = fwu_read_f34_blocks(block_count, CMD_READ_CONFIG);
+
+exit:
+	if (fwu->bl_version == BL_V5 || fwu->bl_version == BL_V6)
+		rmi4_data->reset_device(rmi4_data, false);
+
+	mutex_unlock(&rmi4_data->rmi4_exp_init_mutex);
+
+	return retval;
+}
+
+#ifdef SYNA_TDDI
+static int fwu_do_read_tddi_lockdown_data(void)
+{
+	int retval = -EINVAL;
+	unsigned short block_count;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->blkcount.tddi_lockdown_data;
+	fwu->config_size = fwu->block_size * block_count;
+
+	if (fwu->bl_version != BL_V6) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not support lockdown data in bl v.%d\n",
+				__func__,
+				fwu->bl_version);
+		goto exit;
+	} else if (!fwu->has_lockdown_data) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not support lockdown data\n", __func__);
+		goto exit;
+	}
+
+	kfree(fwu->read_config_buf);
+
+	fwu->read_config_buf = kzalloc(fwu->config_size, GFP_KERNEL);
+
+	if (!fwu->read_config_buf) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for fwu->read_config_buf\n",
+				__func__);
+		fwu->read_config_buf_size = 0;
+		retval = -ENOMEM;
+		goto exit;
+	}
+	fwu->read_config_buf_size = fwu->config_size;
+	retval = fwu_read_f34_blocks(block_count, CMD_READ_LOCKDOWN_DATA);
+exit:
+	return retval;
+}
+
+int get_tddi_lockdown_data(unsigned char *lockdown_data, unsigned short leng)
+{
+	int retval;
+
+	retval = fwu_do_read_tddi_lockdown_data();
+	if (retval < 0)
+		return retval;
+	memcpy(lockdown_data, fwu->read_config_buf, leng);
+	return retval;
+}
+
+int set_tddi_lockdown_data(unsigned char *lockdown_data, unsigned short leng)
+{
+	int retval = -EINVAL;
+	unsigned long checksum;
+	unsigned char checksum_array[4];
+	unsigned short blk_cnt;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (fwu->bl_version != BL_V6) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not support lockdown data in bl v.%d\n",
+				__func__,
+				fwu->bl_version);
+		goto exit;
+	} else if (!fwu->has_lockdown_data) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not support lockdown data\n", __func__);
+		goto exit;
+	}
+
+	retval = fwu_enter_flash_prog();
+	if (retval < 0)
+		goto exit;
+
+	retval = fwu_erase_lockdown_data();
+	if (retval < 0)
+		goto exit;
+
+	blk_cnt = fwu->blkcount.tddi_lockdown_data;
+
+	fwu->config_size = fwu->blkcount.tddi_lockdown_data * fwu->block_size;
+	retval = fwu_allocate_read_config_buf(fwu->config_size);
+	if (retval < 0)
+		goto exit;
+	memset(fwu->read_config_buf, 0x00, fwu->config_size);
+	retval = secure_memcpy(fwu->read_config_buf, fwu->config_size,
+			lockdown_data, leng, leng);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy tddi lockdwon data\n",
+				__func__);
+		goto exit;
+	}
+
+	calculate_checksum((unsigned short *)fwu->read_config_buf,
+			((fwu->config_size - 4) / 2),
+			&checksum);
+
+	convert_to_little_endian(checksum_array, checksum);
+
+	fwu->read_config_buf[blk_cnt * fwu->block_size - 4] = checksum_array[0];
+	fwu->read_config_buf[blk_cnt * fwu->block_size - 3] = checksum_array[1];
+	fwu->read_config_buf[blk_cnt * fwu->block_size - 2] = checksum_array[2];
+	fwu->read_config_buf[blk_cnt * fwu->block_size - 1] = checksum_array[3];
+	retval = fwu_write_tddi_lockdown_data();
+exit:
+	return retval;
+}
+#endif
+#endif
+
+static int fwu_do_lockdown_v7(void)
+{
+	int retval;
+	struct f34_v7_data0 status;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_enter_flash_prog();
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fwu->f34_fd.data_base_addr + fwu->off.flash_status,
+			status.data,
+			sizeof(status.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read flash status\n",
+				__func__);
+		return retval;
+	}
+
+	if (status.device_cfg_status == 2) {
+		dev_info(rmi4_data->pdev->dev.parent,
+				"%s: Device already locked down\n",
+				__func__);
+		return 0;
+	}
+
+	retval = fwu_write_lockdown();
+	if (retval < 0)
+		return retval;
+
+	pr_notice("%s: Lockdown programmed\n", __func__);
+
+	return retval;
+}
+
+static int fwu_do_lockdown_v5v6(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+#ifdef SYNA_TDDI
+	unsigned char *img_ld;
+
+	img_ld = (unsigned char *)fwu->img.lockdown.data;
+	if (fwu->has_lockdown_data) {
+		retval = set_tddi_lockdown_data(img_ld,
+				LOCKDOWN_SIZE);
+		if (retval < 0)
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write lockdown data\n",
+					__func__);
+		return retval;
+	}
+#endif
+
+	retval = fwu_enter_flash_prog();
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			fwu->f34_fd.query_base_addr + fwu->off.properties,
+			fwu->flash_properties.data,
+			sizeof(fwu->flash_properties.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read flash properties\n",
+				__func__);
+		return retval;
+	}
+
+	if (fwu->flash_properties.unlocked == 0) {
+		dev_info(rmi4_data->pdev->dev.parent,
+				"%s: Device already locked down\n",
+				__func__);
+		return 0;
+	}
+
+	retval = fwu_write_lockdown();
+	if (retval < 0)
+		return retval;
+
+	pr_notice("%s: Lockdown programmed\n", __func__);
+
+	return retval;
+}
+
+#ifdef F51_DISCRETE_FORCE
+static int fwu_do_restore_f51_cal_data(void)
+{
+	int retval;
+	unsigned char checksum_array[4];
+	unsigned short block_count;
+	unsigned long checksum;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	block_count = fwu->blkcount.ui_config;
+	fwu->config_size = fwu->block_size * block_count;
+	fwu->config_area = UI_CONFIG_AREA;
+
+	retval = fwu_allocate_read_config_buf(fwu->config_size);
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_read_f34_blocks(block_count, CMD_READ_CONFIG);
+	if (retval < 0)
+		return retval;
+
+	retval = secure_memcpy(&fwu->read_config_buf[fwu->cal_data_off],
+			fwu->cal_data_size, fwu->cal_data,
+			fwu->cal_data_buf_size, fwu->cal_data_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to restore calibration data\n",
+				__func__);
+		return retval;
+	}
+
+	calculate_checksum((unsigned short *)fwu->read_config_buf,
+			((fwu->config_size - 4) / 2),
+			&checksum);
+
+	convert_to_little_endian(checksum_array, checksum);
+
+	fwu->read_config_buf[fwu->config_size - 4] = checksum_array[0];
+	fwu->read_config_buf[fwu->config_size - 3] = checksum_array[1];
+	fwu->read_config_buf[fwu->config_size - 2] = checksum_array[2];
+	fwu->read_config_buf[fwu->config_size - 1] = checksum_array[3];
+
+	retval = fwu_enter_flash_prog();
+	if (retval < 0)
+		return retval;
+
+	fwu->config_area = UI_CONFIG_AREA;
+	fwu->config_data = fwu->read_config_buf;
+	fwu->config_block_count = fwu->config_size / fwu->block_size;
+
+	retval = fwu_erase_configuration();
+	if (retval < 0)
+		return retval;
+
+	retval = fwu_write_configuration();
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+static int fwu_start_write_guest_code(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_parse_image_info();
+	if (retval < 0)
+		return -EINVAL;
+
+	if (!fwu->has_guest_code) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Guest code not supported\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (!fwu->img.contains_guest_code) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: No guest code in firmware image\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (rmi4_data->sensor_sleep) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Sensor sleeping\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	rmi4_data->stay_awake = true;
+
+	mutex_lock(&rmi4_data->rmi4_exp_init_mutex);
+
+	pr_notice("%s: Start of write guest code process\n", __func__);
+
+	retval = fwu_enter_flash_prog();
+	if (retval < 0)
+		goto exit;
+
+	retval = fwu_check_guest_code_size();
+	if (retval < 0)
+		goto exit;
+
+	retval = fwu_erase_guest_code();
+	if (retval < 0)
+		goto exit;
+
+	retval = fwu_write_guest_code();
+	if (retval < 0)
+		goto exit;
+
+	pr_notice("%s: Guest code programmed\n", __func__);
+
+exit:
+	rmi4_data->reset_device(rmi4_data, false);
+
+	pr_notice("%s: End of write guest code process\n", __func__);
+
+	mutex_unlock(&rmi4_data->rmi4_exp_init_mutex);
+
+	rmi4_data->stay_awake = false;
+
+	return retval;
+}
+
+static int fwu_start_write_config(void)
+{
+	int retval;
+	unsigned short config_area;
+	unsigned int device_fw_id;
+	unsigned int image_fw_id;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = fwu_parse_image_info();
+	if (retval < 0)
+		return -EINVAL;
+
+	switch (fwu->config_area) {
+	case UI_CONFIG_AREA:
+		device_fw_id = rmi4_data->firmware_id;
+		retval = fwu_get_image_firmware_id(&image_fw_id);
+		if (retval < 0)
+			return retval;
+		if (device_fw_id != image_fw_id) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Device and image firmware IDs don't match\n",
+					__func__);
+			return -EINVAL;
+		}
+		retval = fwu_check_ui_configuration_size();
+		if (retval < 0)
+			return retval;
+		break;
+	case DP_CONFIG_AREA:
+		if (!fwu->flash_properties.has_disp_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Display configuration not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		if (!fwu->img.contains_disp_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: No display configuration in firmware image\n",
+					__func__);
+			return -EINVAL;
+		}
+		retval = fwu_check_dp_configuration_size();
+		if (retval < 0)
+			return retval;
+		break;
+	case PM_CONFIG_AREA:
+		if (!fwu->flash_properties.has_pm_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Permanent configuration not supported\n",
+					__func__);
+			return -EINVAL;
+		}
+		if (!fwu->img.contains_perm_config) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: No permanent configuration in firmware image\n",
+					__func__);
+			return -EINVAL;
+		}
+		retval = fwu_check_pm_configuration_size();
+		if (retval < 0)
+			return retval;
+		break;
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Configuration not supported\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (rmi4_data->sensor_sleep) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Sensor sleeping\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	rmi4_data->stay_awake = true;
+
+	mutex_lock(&rmi4_data->rmi4_exp_init_mutex);
+
+	pr_notice("%s: Start of write config process\n", __func__);
+
+	config_area = fwu->config_area;
+
+	retval = fwu_enter_flash_prog();
+	if (retval < 0)
+		goto exit;
+
+	fwu->config_area = config_area;
+
+	if (fwu->config_area != PM_CONFIG_AREA) {
+		retval = fwu_erase_configuration();
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to erase config\n",
+					__func__);
+			goto exit;
+		}
+	}
+
+	switch (fwu->config_area) {
+	case UI_CONFIG_AREA:
+		retval = fwu_write_ui_configuration();
+		if (retval < 0)
+			goto exit;
+		break;
+	case DP_CONFIG_AREA:
+		retval = fwu_write_dp_configuration();
+		if (retval < 0)
+			goto exit;
+		break;
+	case PM_CONFIG_AREA:
+		retval = fwu_write_pm_configuration();
+		if (retval < 0)
+			goto exit;
+		break;
+	}
+
+	pr_notice("%s: Config written\n", __func__);
+
+exit:
+	switch (fwu->config_area) {
+	case UI_CONFIG_AREA:
+		rmi4_data->reset_device(rmi4_data, true);
+		break;
+	case DP_CONFIG_AREA:
+	case PM_CONFIG_AREA:
+		rmi4_data->reset_device(rmi4_data, false);
+		break;
+	}
+
+	pr_notice("%s: End of write config process\n", __func__);
+
+	mutex_unlock(&rmi4_data->rmi4_exp_init_mutex);
+
+	rmi4_data->stay_awake = false;
+
+	return retval;
+}
+#endif
+
+static int fwu_start_reflash(void)
+{
+	int retval = 0;
+	enum flash_area flash_area;
+	bool do_rebuild = false;
+	const struct firmware *fw_entry = NULL;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (rmi4_data->sensor_sleep) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Sensor sleeping\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	rmi4_data->stay_awake = true;
+
+	mutex_lock(&rmi4_data->rmi4_exp_init_mutex);
+
+	pr_notice("%s: Start of reflash process\n", __func__);
+
+	if (fwu->image == NULL) {
+		retval = secure_memcpy(fwu->image_name, MAX_IMAGE_NAME_LEN,
+				FW_IMAGE_NAME, sizeof(FW_IMAGE_NAME),
+				sizeof(FW_IMAGE_NAME));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to copy image file name\n",
+					__func__);
+			goto exit;
+		}
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Requesting firmware image %s\n",
+				__func__, fwu->image_name);
+
+		retval = request_firmware(&fw_entry, fwu->image_name,
+				rmi4_data->pdev->dev.parent);
+		if (retval != 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Firmware image %s not available\n",
+					__func__, fwu->image_name);
+			retval = -EINVAL;
+			goto exit;
+		}
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Firmware image size = %d\n",
+				__func__, (unsigned int)fw_entry->size);
+
+		fwu->image = fw_entry->data;
+	}
+
+	retval = fwu_parse_image_info();
+	if (retval < 0)
+		goto exit;
+
+	if (fwu->blkcount.total_count != fwu->img.blkcount.total_count) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Flash size mismatch\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (fwu->bl_version != fwu->img.bl_version) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Bootloader version mismatch\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	retval = fwu_read_flash_status();
+	if (retval < 0)
+		goto exit;
+
+	if (fwu->in_bl_mode) {
+		fwu->bl_mode_device = true;
+		dev_info(rmi4_data->pdev->dev.parent,
+				"%s: Device in bootloader mode\n",
+				__func__);
+	} else {
+		fwu->bl_mode_device = false;
+	}
+
+	flash_area = fwu_go_nogo();
+
+	if (flash_area != NONE) {
+		retval = fwu_enter_flash_prog();
+		if (retval < 0) {
+			rmi4_data->reset_device(rmi4_data, false);
+			goto exit;
+		}
+	}
+
+#ifdef F51_DISCRETE_FORCE
+	if (flash_area != NONE && !fwu->bl_mode_device) {
+		fwu->config_size = fwu->block_size * fwu->blkcount.ui_config;
+		fwu->config_area = UI_CONFIG_AREA;
+
+		retval = fwu_allocate_read_config_buf(fwu->config_size);
+		if (retval < 0) {
+			rmi4_data->reset_device(rmi4_data, false);
+			goto exit;
+		}
+
+		retval = fwu_read_f34_blocks(fwu->blkcount.ui_config,
+				CMD_READ_CONFIG);
+		if (retval < 0) {
+			rmi4_data->reset_device(rmi4_data, false);
+			goto exit;
+		}
+
+		retval = secure_memcpy(fwu->cal_data, fwu->cal_data_buf_size,
+				&fwu->read_config_buf[fwu->cal_data_off],
+				fwu->cal_data_size, fwu->cal_data_size);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to save calibration data\n",
+					__func__);
+			rmi4_data->reset_device(rmi4_data, false);
+			goto exit;
+		}
+	}
+#endif
+
+	switch (flash_area) {
+	case UI_FIRMWARE:
+		do_rebuild = true;
+		retval = fwu_do_reflash();
+#ifdef F51_DISCRETE_FORCE
+		if (retval < 0)
+			break;
+
+		if (fwu->has_utility_param || fwu->img.contains_utility_param)
+			break;
+
+		rmi4_data->reset_device(rmi4_data, false);
+
+		if (fwu->bl_mode_device || fwu->in_bl_mode) {
+			dev_info(rmi4_data->pdev->dev.parent,
+					"%s: Device in bootloader mode, skipping calibration data restoration\n",
+					__func__);
+			break;
+		}
+
+		retval = fwu_do_restore_f51_cal_data();
+#endif
+		break;
+	case UI_CONFIG:
+		do_rebuild = true;
+		retval = fwu_check_ui_configuration_size();
+		if (retval < 0)
+			break;
+		fwu->config_area = UI_CONFIG_AREA;
+		retval = fwu_erase_configuration();
+		if (retval < 0)
+			break;
+		retval = fwu_write_ui_configuration();
+#ifdef F51_DISCRETE_FORCE
+		if (retval < 0)
+			break;
+
+		if (fwu->has_utility_param)
+			break;
+
+		retval = fwu_do_restore_f51_cal_data();
+#endif
+		break;
+	case NONE:
+	default:
+		break;
+	}
+
+	if (retval < 0) {
+		do_rebuild = false;
+		rmi4_data->reset_device(rmi4_data, false);
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to do reflash\n",
+				__func__);
+		goto exit;
+	}
+
+	if (fwu->do_lockdown && (fwu->img.lockdown.data != NULL)) {
+		switch (fwu->bl_version) {
+		case BL_V5:
+		case BL_V6:
+			retval = fwu_do_lockdown_v5v6();
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to do lockdown\n",
+						__func__);
+			}
+			rmi4_data->reset_device(rmi4_data, false);
+			break;
+		case BL_V7:
+		case BL_V8:
+			retval = fwu_do_lockdown_v7();
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to do lockdown\n",
+						__func__);
+			}
+			rmi4_data->reset_device(rmi4_data, false);
+			break;
+		default:
+			break;
+		}
+	}
+
+exit:
+	if (fw_entry)
+		release_firmware(fw_entry);
+
+	if (do_rebuild)
+		rmi4_data->reset_device(rmi4_data, true);
+
+	pr_notice("%s: End of reflash process\n", __func__);
+
+	mutex_unlock(&rmi4_data->rmi4_exp_init_mutex);
+
+	rmi4_data->stay_awake = false;
+
+	return retval;
+}
+
+static int fwu_recovery_check_status(void)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char status;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f35_fd.data_base_addr;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			data_base + F35_ERROR_CODE_OFFSET,
+			&status,
+			1);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read status\n",
+				__func__);
+		return retval;
+	}
+
+	status = status & MASK_5BIT;
+
+	if (status != 0x00) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Recovery mode status = %d\n",
+				__func__, status);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fwu_recovery_erase_completion(void)
+{
+	int retval;
+	unsigned char data_base;
+	unsigned char command;
+	unsigned char status;
+	unsigned int timeout = F35_ERASE_ALL_WAIT_MS / 20;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	data_base = fwu->f35_fd.data_base_addr;
+
+	do {
+		command = 0x01;
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				fwu->f35_fd.cmd_base_addr,
+				&command,
+				sizeof(command));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to issue command\n",
+					__func__);
+			return retval;
+		}
+
+		do {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					fwu->f35_fd.cmd_base_addr,
+					&command,
+					sizeof(command));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to read command status\n",
+						__func__);
+				return retval;
+			}
+
+			if ((command & 0x01) == 0x00)
+				break;
+
+			msleep(20);
+			timeout--;
+		} while (timeout > 0);
+
+		if (timeout == 0)
+			goto exit;
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				data_base + F35_FLASH_STATUS_OFFSET,
+				&status,
+				sizeof(status));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read flash status\n",
+					__func__);
+			return retval;
+		}
+
+		if ((status & 0x01) == 0x00)
+			break;
+
+		msleep(20);
+		timeout--;
+	} while (timeout > 0);
+
+exit:
+	if (timeout == 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Timed out waiting for flash erase completion\n",
+				__func__);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int fwu_recovery_erase_all(void)
+{
+	int retval;
+	unsigned char ctrl_base;
+	unsigned char command = CMD_F35_ERASE_ALL;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	ctrl_base = fwu->f35_fd.ctrl_base_addr;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			ctrl_base + F35_CHUNK_COMMAND_OFFSET,
+			&command,
+			sizeof(command));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to issue erase all command\n",
+				__func__);
+		return retval;
+	}
+
+	if (fwu->f35_fd.cmd_base_addr) {
+		retval = fwu_recovery_erase_completion();
+		if (retval < 0)
+			return retval;
+	} else {
+		msleep(F35_ERASE_ALL_WAIT_MS);
+	}
+
+	retval = fwu_recovery_check_status();
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int fwu_recovery_write_chunk(void)
+{
+	int retval;
+	unsigned char ctrl_base;
+	unsigned char chunk_number[] = {0, 0};
+	unsigned char chunk_spare;
+	unsigned char chunk_size;
+	unsigned char buf[F35_CHUNK_SIZE + 1];
+	unsigned short chunk;
+	unsigned short chunk_total;
+	unsigned short bytes_written = 0;
+	unsigned char *chunk_ptr = (unsigned char *)fwu->image;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	ctrl_base = fwu->f35_fd.ctrl_base_addr;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			ctrl_base + F35_CHUNK_NUM_LSB_OFFSET,
+			chunk_number,
+			sizeof(chunk_number));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write chunk number\n",
+				__func__);
+		return retval;
+	}
+
+	buf[sizeof(buf) - 1] = CMD_F35_WRITE_CHUNK;
+
+	chunk_total = fwu->image_size / F35_CHUNK_SIZE;
+	chunk_spare = fwu->image_size % F35_CHUNK_SIZE;
+	if (chunk_spare)
+		chunk_total++;
+
+	for (chunk = 0; chunk < chunk_total; chunk++) {
+		if (chunk_spare && chunk == chunk_total - 1)
+			chunk_size = chunk_spare;
+		else
+			chunk_size = F35_CHUNK_SIZE;
+
+		memset(buf, 0x00, F35_CHUNK_SIZE);
+		secure_memcpy(buf, sizeof(buf), chunk_ptr,
+					fwu->image_size - bytes_written,
+					chunk_size);
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				ctrl_base + F35_CHUNK_DATA_OFFSET,
+				buf,
+				sizeof(buf));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write chunk data (chunk %d)\n",
+					__func__, chunk);
+			return retval;
+		}
+		chunk_ptr += chunk_size;
+		bytes_written += chunk_size;
+	}
+
+	retval = fwu_recovery_check_status();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write chunk data\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int fwu_recovery_reset(void)
+{
+	int retval;
+	unsigned char ctrl_base;
+	unsigned char command = CMD_F35_RESET;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	ctrl_base = fwu->f35_fd.ctrl_base_addr;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			ctrl_base + F35_CHUNK_COMMAND_OFFSET,
+			&command,
+			sizeof(command));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to issue reset command\n",
+				__func__);
+		return retval;
+	}
+
+	msleep(F35_RESET_WAIT_MS);
+
+	return 0;
+}
+
+static int fwu_start_recovery(void)
+{
+	int retval;
+	const struct firmware *fw_entry = NULL;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (rmi4_data->sensor_sleep) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Sensor sleeping\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	rmi4_data->stay_awake = true;
+
+	mutex_lock(&rmi4_data->rmi4_exp_init_mutex);
+
+	pr_notice("%s: Start of recovery process\n", __func__);
+
+	if (fwu->image == NULL) {
+		retval = secure_memcpy(fwu->image_name, MAX_IMAGE_NAME_LEN,
+				FW_IHEX_NAME, sizeof(FW_IHEX_NAME),
+				sizeof(FW_IHEX_NAME));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to copy ihex file name\n",
+					__func__);
+			goto exit;
+		}
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Requesting firmware ihex %s\n",
+				__func__, fwu->image_name);
+
+		retval = request_firmware(&fw_entry, fwu->image_name,
+				rmi4_data->pdev->dev.parent);
+		if (retval != 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Firmware ihex %s not available\n",
+					__func__, fwu->image_name);
+			retval = -EINVAL;
+			goto exit;
+		}
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Firmware image size = %d\n",
+				__func__, (unsigned int)fw_entry->size);
+
+		fwu->image = fw_entry->data;
+		fwu->image_size = fw_entry->size;
+	}
+
+	retval = rmi4_data->irq_enable(rmi4_data, false, false);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to disable interrupt\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = fwu_recovery_erase_all();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to do erase all in recovery mode\n",
+				__func__);
+		goto exit;
+	}
+
+	pr_notice("%s: External flash erased\n", __func__);
+
+	retval = fwu_recovery_write_chunk();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write chunk data in recovery mode\n",
+				__func__);
+		goto exit;
+	}
+
+	pr_notice("%s: Chunk data programmed\n", __func__);
+
+	retval = fwu_recovery_reset();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to reset device in recovery mode\n",
+				__func__);
+		goto exit;
+	}
+
+	pr_notice("%s: Recovery mode reset issued\n", __func__);
+
+	rmi4_data->reset_device(rmi4_data, true);
+
+	retval = 0;
+
+exit:
+	if (fw_entry)
+		release_firmware(fw_entry);
+
+	pr_notice("%s: End of recovery process\n", __func__);
+
+	mutex_unlock(&rmi4_data->rmi4_exp_init_mutex);
+
+	rmi4_data->stay_awake = false;
+
+	return retval;
+}
+
+int synaptics_fw_updater(const unsigned char *fw_data)
+{
+	int retval;
+
+	if (!fwu)
+		return -ENODEV;
+
+	if (!fwu->initialized)
+		return -ENODEV;
+
+	if (fwu->in_ub_mode) {
+		fwu->image = NULL;
+		retval = fwu_start_recovery();
+		if (retval < 0)
+			return retval;
+	}
+
+	fwu->image = fw_data;
+
+	retval = fwu_start_reflash();
+
+	fwu->image = NULL;
+
+	return retval;
+}
+EXPORT_SYMBOL(synaptics_fw_updater);
+
+#ifdef DO_STARTUP_FW_UPDATE
+static void fwu_startup_fw_update_work(struct work_struct *work)
+{
+	static unsigned char do_once = 1;
+#ifdef WAIT_FOR_FB_READY
+	unsigned int timeout;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+#endif
+
+	if (!do_once)
+		return;
+	do_once = 0;
+
+#ifdef WAIT_FOR_FB_READY
+	timeout = FB_READY_TIMEOUT_S * 1000 / FB_READY_WAIT_MS + 1;
+
+	while (!rmi4_data->fb_ready) {
+		msleep(FB_READY_WAIT_MS);
+		timeout--;
+		if (timeout == 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Timed out waiting for FB ready\n",
+					__func__);
+			return;
+		}
+	}
+#endif
+
+	synaptics_fw_updater(NULL);
+}
+#endif
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+static ssize_t fwu_sysfs_show_image(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	if (count < fwu->config_size) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not enough space (%d bytes) in buffer\n",
+				__func__, (unsigned int)count);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	retval = secure_memcpy(buf, count, fwu->read_config_buf,
+			fwu->read_config_buf_size, fwu->config_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy config data\n",
+				__func__);
+		goto exit;
+	} else {
+		retval = fwu->config_size;
+	}
+
+exit:
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval;
+}
+
+static ssize_t fwu_sysfs_store_image(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = secure_memcpy(&fwu->ext_data_source[fwu->data_pos],
+			fwu->image_size - fwu->data_pos, buf, count, count);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy image data\n",
+				__func__);
+		goto exit;
+	} else {
+		retval = count;
+	}
+
+	fwu->data_pos += count;
+
+exit:
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval;
+}
+
+static ssize_t fwu_sysfs_do_recovery_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	if (kstrtouint(buf, 10, &input) != 1) {
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (!fwu->in_ub_mode) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not in microbootloader mode\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (!fwu->ext_data_source) {
+		retval = -EINVAL;
+		goto exit;
+	} else {
+		fwu->image = fwu->ext_data_source;
+	}
+
+	retval = fwu_start_recovery();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to do recovery\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	kfree(fwu->ext_data_source);
+	fwu->ext_data_source = NULL;
+	fwu->image = NULL;
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval;
+}
+
+static ssize_t fwu_sysfs_do_reflash_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	if (kstrtouint(buf, 10, &input) != 1) {
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (fwu->in_ub_mode) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: In microbootloader mode\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (!fwu->ext_data_source) {
+		retval =  -EINVAL;
+		goto exit;
+	} else {
+		fwu->image = fwu->ext_data_source;
+	}
+
+	if (input & LOCKDOWN) {
+		fwu->do_lockdown = true;
+		input &= ~LOCKDOWN;
+	}
+
+	if ((input != NORMAL) && (input != FORCE)) {
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (input == FORCE)
+		fwu->force_update = true;
+
+	retval = synaptics_fw_updater(fwu->image);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to do reflash\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	kfree(fwu->ext_data_source);
+	fwu->ext_data_source = NULL;
+	fwu->image = NULL;
+	fwu->force_update = FORCE_UPDATE;
+	fwu->do_lockdown = DO_LOCKDOWN;
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval;
+}
+
+static ssize_t fwu_sysfs_write_config_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	if (kstrtouint(buf, 10, &input) != 1) {
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (input != 1) {
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (fwu->in_ub_mode) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: In microbootloader mode\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (!fwu->ext_data_source) {
+		retval = -EINVAL;
+		goto exit;
+	} else {
+		fwu->image = fwu->ext_data_source;
+	}
+
+	retval = fwu_start_write_config();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write config\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	kfree(fwu->ext_data_source);
+	fwu->ext_data_source = NULL;
+	fwu->image = NULL;
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval;
+}
+
+static ssize_t fwu_sysfs_read_config_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input != 1)
+		return -EINVAL;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	if (fwu->in_ub_mode) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: In microbootloader mode\n",
+				__func__);
+		retval =  -EINVAL;
+		goto exit;
+	}
+
+	retval = fwu_do_read_config();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read config\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval;
+}
+
+static ssize_t fwu_sysfs_config_area_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long config_area;
+
+	retval = sstrtoul(buf, 10, &config_area);
+	if (retval)
+		return retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	fwu->config_area = config_area;
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return count;
+}
+
+static ssize_t fwu_sysfs_image_name_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = secure_memcpy(fwu->image_name, MAX_IMAGE_NAME_LEN,
+			buf, count, count);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy image file name\n",
+				__func__);
+	} else {
+		retval = count;
+	}
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_image_size_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long size;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &size);
+	if (retval)
+		return retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	fwu->image_size = size;
+	fwu->data_pos = 0;
+
+	kfree(fwu->ext_data_source);
+	fwu->ext_data_source = kzalloc(fwu->image_size, GFP_KERNEL);
+	if (!fwu->ext_data_source) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for image data\n",
+				__func__);
+		retval = -ENOMEM;
+	}
+	retval = count;
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_block_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval =  snprintf(buf, PAGE_SIZE, "%u\n", fwu->block_size);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_firmware_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", fwu->blkcount.ui_firmware);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_configuration_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", fwu->blkcount.ui_config);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_disp_config_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", fwu->blkcount.dp_config);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_perm_config_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", fwu->blkcount.pm_config);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_bl_config_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", fwu->blkcount.bl_config);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_utility_parameter_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", fwu->blkcount.utility_param);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_guest_code_block_count_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", fwu->blkcount.guest_code);
+
+	mutex_unlock(&fwu_sysfs_mutex);
+
+	return retval;
+}
+
+static ssize_t fwu_sysfs_write_guest_code_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = fwu->rmi4_data;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	if (kstrtouint(buf, 10, &input) != 1) {
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (input != 1) {
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (fwu->in_ub_mode) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: In microbootloader mode\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if (!fwu->ext_data_source) {
+		retval = -EINVAL;
+		goto exit;
+	} else {
+		fwu->image = fwu->ext_data_source;
+	}
+
+	retval = fwu_start_write_guest_code();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write guest code\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	kfree(fwu->ext_data_source);
+	fwu->ext_data_source = NULL;
+	fwu->image = NULL;
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval;
+}
+
+#ifdef SYNA_TDDI
+static ssize_t fwu_sysfs_read_lockdown_code_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	unsigned short lockdown_data_size;
+	unsigned char *lockdown_data;
+	char ld_val[2];
+	int retval = 0;
+	int i = 0;
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	lockdown_data_size = fwu->blkcount.tddi_lockdown_data * fwu->block_size;
+	lockdown_data = kzalloc(lockdown_data_size, GFP_KERNEL);
+	if (!lockdown_data) {
+		mutex_unlock(&fwu_sysfs_mutex);
+		return -ENOMEM;
+	}
+
+	if (get_tddi_lockdown_data(lockdown_data, lockdown_data_size) < 0) {
+		kfree(lockdown_data);
+		mutex_unlock(&fwu_sysfs_mutex);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < lockdown_data_size; i++) {
+		retval += snprintf(ld_val, PAGE_SIZE, "%02x",
+				*(lockdown_data + i));
+		strlcat(buf, ld_val, lockdown_data_size);
+	}
+	*(buf + retval) = '\n';
+	kfree(lockdown_data);
+	mutex_unlock(&fwu_sysfs_mutex);
+	return retval + 1;
+}
+
+static ssize_t fwu_sysfs_write_lockdown_code_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned short lockdown_data_size = (count - 1) / 2;
+	unsigned char *lockdown_data;
+	unsigned char temp[2];
+	int ld_val;
+	int i = 0;
+
+	for (i = 0; i < (count - 1); i++) {
+		if (((*buf >= '0') && (*buf <= '9')) ||
+				(('a' < *buf) && (*buf > 'f')) ||
+				(('A' < *buf) && (*buf > 'F')))
+			continue;
+		else
+			return -EINVAL;
+	}
+
+	if (count % 2 != 1)
+		return -EINVAL;
+
+	lockdown_data = kzalloc(lockdown_data_size, GFP_KERNEL);
+	if (!lockdown_data)
+		return -ENOMEM;
+
+	for (i = 0; i < lockdown_data_size; i++) {
+		memcpy(temp, (buf + 2 * i), sizeof(temp));
+		if (kstrtoint(temp, 16, &ld_val) == 1)
+			*(lockdown_data + i) = ld_val & 0xff;
+	}
+
+	if (!mutex_trylock(&fwu_sysfs_mutex))
+		return -EBUSY;
+
+	if (set_tddi_lockdown_data(lockdown_data, lockdown_data_size) < 0) {
+		kfree(lockdown_data);
+		mutex_unlock(&fwu_sysfs_mutex);
+		return -EINVAL;
+	}
+	kfree(lockdown_data);
+	mutex_unlock(&fwu_sysfs_mutex);
+	return count;
+}
+#endif
+#endif
+static void synaptics_rmi4_fwu_attn(struct synaptics_rmi4_data *rmi4_data,
+		unsigned char intr_mask)
+{
+	if (!fwu)
+		return;
+
+	if (fwu->intr_mask & intr_mask)
+		fwu_read_flash_status();
+
+	return;
+}
+
+static int synaptics_rmi4_fwu_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char attr_count;
+	struct pdt_properties pdt_props;
+
+	if (fwu) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Handle already exists\n",
+				__func__);
+		return 0;
+	}
+
+	fwu = kzalloc(sizeof(*fwu), GFP_KERNEL);
+	if (!fwu) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for fwu\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	fwu->image_name = kzalloc(MAX_IMAGE_NAME_LEN, GFP_KERNEL);
+	if (!fwu->image_name) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for image name\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_fwu;
+	}
+
+	fwu->rmi4_data = rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			PDT_PROPS,
+			pdt_props.data,
+			sizeof(pdt_props.data));
+	if (retval < 0) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read PDT properties, assuming 0x00\n",
+				__func__);
+	} else if (pdt_props.has_bsr) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Reflash for LTS not currently supported\n",
+				__func__);
+		retval = -ENODEV;
+		goto exit_free_mem;
+	}
+
+	retval = fwu_scan_pdt();
+	if (retval < 0)
+		goto exit_free_mem;
+
+	if (!fwu->in_ub_mode) {
+		retval = fwu_read_f34_queries();
+		if (retval < 0)
+			goto exit_free_mem;
+
+		retval = fwu_get_device_config_id();
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read device config ID\n",
+					__func__);
+			goto exit_free_mem;
+		}
+	}
+
+	fwu->force_update = FORCE_UPDATE;
+	fwu->do_lockdown = DO_LOCKDOWN;
+	fwu->initialized = true;
+
+#ifdef DO_STARTUP_FW_UPDATE
+	fwu->fwu_workqueue = create_singlethread_workqueue("fwu_workqueue");
+	INIT_WORK(&fwu->fwu_work, fwu_startup_fw_update_work);
+	queue_work(fwu->fwu_workqueue,
+			&fwu->fwu_work);
+#endif
+
+#ifdef F51_DISCRETE_FORCE
+	fwu_read_flash_status();
+	if (!fwu->in_bl_mode) {
+		retval = fwu_f51_force_data_init();
+		if (retval < 0)
+			goto exit_free_mem;
+	}
+#endif
+
+	if (ENABLE_SYS_REFLASH == false)
+		return 0;
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+	retval = sysfs_create_bin_file(&rmi4_data->input_dev->dev.kobj,
+			&dev_attr_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs bin file\n",
+				__func__);
+		goto exit_free_mem;
+	}
+#endif
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		retval = sysfs_create_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create sysfs attributes\n",
+					__func__);
+			retval = -ENODEV;
+			goto exit_remove_attrs;
+		}
+	}
+
+	return 0;
+
+exit_remove_attrs:
+	for (attr_count--; attr_count >= 0; attr_count--) {
+		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+	sysfs_remove_bin_file(&rmi4_data->input_dev->dev.kobj, &dev_attr_data);
+#endif
+
+exit_free_mem:
+	kfree(fwu->image_name);
+
+exit_free_fwu:
+	kfree(fwu);
+	fwu = NULL;
+
+exit:
+	return retval;
+}
+
+static void synaptics_rmi4_fwu_remove(struct synaptics_rmi4_data *rmi4_data)
+{
+	unsigned char attr_count;
+
+	if (!fwu)
+		goto exit;
+
+#ifdef DO_STARTUP_FW_UPDATE
+	cancel_work_sync(&fwu->fwu_work);
+	flush_workqueue(fwu->fwu_workqueue);
+	destroy_workqueue(fwu->fwu_workqueue);
+#endif
+
+#ifdef F51_DISCRETE_FORCE
+	kfree(fwu->cal_data);
+#endif
+	kfree(fwu->read_config_buf);
+	kfree(fwu->image_name);
+	kfree(fwu);
+	fwu = NULL;
+
+	if (ENABLE_SYS_REFLASH == false)
+		goto exit;
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+#ifdef CONFIG_TOUCHSCREEN_SYNAPTICS_DSX_FW_UPDATE_EXTRA_SYSFS
+	sysfs_remove_bin_file(&rmi4_data->input_dev->dev.kobj, &dev_attr_data);
+#endif
+
+exit:
+	complete(&fwu_remove_complete);
+}
+
+static void synaptics_rmi4_fwu_reset(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+
+	if (!fwu) {
+		synaptics_rmi4_fwu_init(rmi4_data);
+		return;
+	}
+
+	retval = fwu_scan_pdt();
+	if (retval < 0)
+		return;
+
+	if (!fwu->in_ub_mode)
+		fwu_read_f34_queries();
+
+#ifdef F51_DISCRETE_FORCE
+	fwu_read_flash_status();
+	if (!fwu->in_bl_mode)
+		fwu_f51_force_data_init();
+#endif
+
+	return;
+}
+
+static struct synaptics_rmi4_exp_fn fwu_module = {
+	.fn_type = RMI_FW_UPDATER,
+	.init = synaptics_rmi4_fwu_init,
+	.remove = synaptics_rmi4_fwu_remove,
+	.reset = synaptics_rmi4_fwu_reset,
+	.reinit = NULL,
+	.early_suspend = NULL,
+	.suspend = NULL,
+	.resume = NULL,
+	.late_resume = NULL,
+	.attn = synaptics_rmi4_fwu_attn,
+};
+
+static int __init rmi4_fw_update_module_init(void)
+{
+	synaptics_rmi4_new_function(&fwu_module, true);
+
+	return 0;
+}
+
+static void __exit rmi4_fw_update_module_exit(void)
+{
+	synaptics_rmi4_new_function(&fwu_module, false);
+
+	wait_for_completion(&fwu_remove_complete);
+}
+
+module_init(rmi4_fw_update_module_init);
+module_exit(rmi4_fw_update_module_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX FW Update Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_gesture.c

@@ -0,0 +1,2291 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define GESTURE_PHYS_NAME "synaptics_dsx/gesture"
+
+#define TUNING_SYSFS_DIR_NAME "tuning"
+
+#define STORE_GESTURES
+#ifdef STORE_GESTURES
+#define GESTURES_TO_STORE 10
+#endif
+
+#define CTRL23_FINGER_REPORT_ENABLE_BIT 0
+#define CTRL27_UDG_ENABLE_BIT 4
+#define WAKEUP_GESTURE_MODE 0x02
+
+static ssize_t udg_sysfs_engine_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_detection_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_detection_score_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_detection_index_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_registration_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_registration_begin_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_registration_status_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_template_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_template_max_index_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_template_detection_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_template_index_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_template_valid_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_template_valid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_template_clear_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_trace_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_template_data_show(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static ssize_t udg_sysfs_template_data_store(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static ssize_t udg_sysfs_trace_data_show(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static ssize_t udg_sysfs_template_displacement_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_template_displacement_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_rotation_invariance_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_rotation_invariance_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_scale_invariance_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_scale_invariance_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_threshold_factor_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_threshold_factor_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_match_metric_threshold_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_match_metric_threshold_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t udg_sysfs_max_inter_stroke_time_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t udg_sysfs_max_inter_stroke_time_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static int udg_read_tuning_params(void);
+
+static int udg_write_tuning_params(void);
+
+static int udg_detection_enable(bool enable);
+
+static int udg_engine_enable(bool enable);
+
+static int udg_set_index(unsigned char index);
+
+#ifdef STORE_GESTURES
+static int udg_read_valid_data(void);
+static int udg_write_valid_data(void);
+static int udg_read_template_data(unsigned char index);
+static int udg_write_template_data(void);
+#endif
+
+enum gesture_type {
+	DETECTION = 0x0f,
+	REGISTRATION = 0x10,
+};
+
+struct udg_tuning {
+	union {
+		struct {
+			unsigned char maximum_number_of_templates;
+			unsigned char template_size;
+			unsigned char template_disp_lsb;
+			unsigned char template_disp_msb;
+			unsigned char rotation_inv_lsb;
+			unsigned char rotation_inv_msb;
+			unsigned char scale_inv_lsb;
+			unsigned char scale_inv_msb;
+			unsigned char thres_factor_lsb;
+			unsigned char thres_factor_msb;
+			unsigned char metric_thres_lsb;
+			unsigned char metric_thres_msb;
+			unsigned char inter_stroke_lsb;
+			unsigned char inter_stroke_msb;
+		} __packed;
+		unsigned char data[14];
+	};
+};
+
+struct udg_addr {
+	unsigned short data_4;
+	unsigned short ctrl_18;
+	unsigned short ctrl_20;
+	unsigned short ctrl_23;
+	unsigned short ctrl_27;
+	unsigned short ctrl_41;
+	unsigned short trace_x;
+	unsigned short trace_y;
+	unsigned short trace_segment;
+	unsigned short template_helper;
+	unsigned short template_data;
+	unsigned short template_flags;
+};
+
+struct synaptics_rmi4_f12_query_0 {
+	union {
+		struct {
+			struct {
+				unsigned char has_register_descriptors:1;
+				unsigned char has_closed_cover:1;
+				unsigned char has_fast_glove_detect:1;
+				unsigned char has_dribble:1;
+				unsigned char has_4p4_jitter_filter_strength:1;
+				unsigned char f12_query0_s0_b5__7:3;
+			} __packed;
+			struct {
+				unsigned char max_num_templates:4;
+				unsigned char f12_query0_s1_b4__7:4;
+				unsigned char template_size_lsb;
+				unsigned char template_size_msb;
+			} __packed;
+		};
+		unsigned char data[4];
+	};
+};
+
+struct synaptics_rmi4_f12_query_5 {
+	union {
+		struct {
+			unsigned char size_of_query6;
+			struct {
+				unsigned char ctrl0_is_present:1;
+				unsigned char ctrl1_is_present:1;
+				unsigned char ctrl2_is_present:1;
+				unsigned char ctrl3_is_present:1;
+				unsigned char ctrl4_is_present:1;
+				unsigned char ctrl5_is_present:1;
+				unsigned char ctrl6_is_present:1;
+				unsigned char ctrl7_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl8_is_present:1;
+				unsigned char ctrl9_is_present:1;
+				unsigned char ctrl10_is_present:1;
+				unsigned char ctrl11_is_present:1;
+				unsigned char ctrl12_is_present:1;
+				unsigned char ctrl13_is_present:1;
+				unsigned char ctrl14_is_present:1;
+				unsigned char ctrl15_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl16_is_present:1;
+				unsigned char ctrl17_is_present:1;
+				unsigned char ctrl18_is_present:1;
+				unsigned char ctrl19_is_present:1;
+				unsigned char ctrl20_is_present:1;
+				unsigned char ctrl21_is_present:1;
+				unsigned char ctrl22_is_present:1;
+				unsigned char ctrl23_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl24_is_present:1;
+				unsigned char ctrl25_is_present:1;
+				unsigned char ctrl26_is_present:1;
+				unsigned char ctrl27_is_present:1;
+				unsigned char ctrl28_is_present:1;
+				unsigned char ctrl29_is_present:1;
+				unsigned char ctrl30_is_present:1;
+				unsigned char ctrl31_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl32_is_present:1;
+				unsigned char ctrl33_is_present:1;
+				unsigned char ctrl34_is_present:1;
+				unsigned char ctrl35_is_present:1;
+				unsigned char ctrl36_is_present:1;
+				unsigned char ctrl37_is_present:1;
+				unsigned char ctrl38_is_present:1;
+				unsigned char ctrl39_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl40_is_present:1;
+				unsigned char ctrl41_is_present:1;
+				unsigned char ctrl42_is_present:1;
+				unsigned char ctrl43_is_present:1;
+				unsigned char ctrl44_is_present:1;
+				unsigned char ctrl45_is_present:1;
+				unsigned char ctrl46_is_present:1;
+				unsigned char ctrl47_is_present:1;
+			} __packed;
+		};
+		unsigned char data[7];
+	};
+};
+
+struct synaptics_rmi4_f12_query_8 {
+	union {
+		struct {
+			unsigned char size_of_query9;
+			struct {
+				unsigned char data0_is_present:1;
+				unsigned char data1_is_present:1;
+				unsigned char data2_is_present:1;
+				unsigned char data3_is_present:1;
+				unsigned char data4_is_present:1;
+				unsigned char data5_is_present:1;
+				unsigned char data6_is_present:1;
+				unsigned char data7_is_present:1;
+			} __packed;
+			struct {
+				unsigned char data8_is_present:1;
+				unsigned char data9_is_present:1;
+				unsigned char data10_is_present:1;
+				unsigned char data11_is_present:1;
+				unsigned char data12_is_present:1;
+				unsigned char data13_is_present:1;
+				unsigned char data14_is_present:1;
+				unsigned char data15_is_present:1;
+			} __packed;
+			struct {
+				unsigned char data16_is_present:1;
+				unsigned char data17_is_present:1;
+				unsigned char data18_is_present:1;
+				unsigned char data19_is_present:1;
+				unsigned char data20_is_present:1;
+				unsigned char data21_is_present:1;
+				unsigned char data22_is_present:1;
+				unsigned char data23_is_present:1;
+			} __packed;
+		};
+		unsigned char data[4];
+	};
+};
+
+struct synaptics_rmi4_f12_control_41 {
+	union {
+		struct {
+			unsigned char enable_registration:1;
+			unsigned char template_index:4;
+			unsigned char begin:1;
+			unsigned char f12_ctrl41_b6__7:2;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct synaptics_rmi4_udg_handle {
+	atomic_t attn_event;
+	unsigned char intr_mask;
+	unsigned char report_flags;
+	unsigned char object_type_enable1;
+	unsigned char object_type_enable2;
+	unsigned char trace_size;
+	unsigned char template_index;
+	unsigned char max_num_templates;
+	unsigned char detection_score;
+	unsigned char detection_index;
+	unsigned char detection_status;
+	unsigned char registration_status;
+	unsigned char *ctrl_buf;
+	unsigned char *trace_data_buf;
+	unsigned char *template_data_buf;
+#ifdef STORE_GESTURES
+	unsigned char gestures_to_store;
+	unsigned char *storage_buf;
+	unsigned char valid_buf[2];
+#endif
+	unsigned short trace_data_buf_size;
+	unsigned short template_size;
+	unsigned short template_data_size;
+	unsigned short query_base_addr;
+	unsigned short control_base_addr;
+	unsigned short data_base_addr;
+	unsigned short command_base_addr;
+	unsigned short ctrl_18_sub10_off;
+	unsigned short ctrl_20_sub1_off;
+	unsigned short ctrl_23_sub3_off;
+	unsigned short ctrl_27_sub5_off;
+	struct input_dev *udg_dev;
+	struct kobject *tuning_dir;
+	struct udg_addr addr;
+	struct udg_tuning tuning;
+	struct synaptics_rmi4_data *rmi4_data;
+};
+
+static struct device_attribute attrs[] = {
+	__ATTR(engine_enable, 0220,
+			synaptics_rmi4_show_error,
+			udg_sysfs_engine_enable_store),
+	__ATTR(detection_enable, 0220,
+			synaptics_rmi4_show_error,
+			udg_sysfs_detection_enable_store),
+	__ATTR(detection_score, 0444,
+			udg_sysfs_detection_score_show,
+			synaptics_rmi4_store_error),
+	__ATTR(detection_index, 0444,
+			udg_sysfs_detection_index_show,
+			synaptics_rmi4_store_error),
+	__ATTR(registration_enable, 0220,
+			synaptics_rmi4_show_error,
+			udg_sysfs_registration_enable_store),
+	__ATTR(registration_begin, 0220,
+			synaptics_rmi4_show_error,
+			udg_sysfs_registration_begin_store),
+	__ATTR(registration_status, 0444,
+			udg_sysfs_registration_status_show,
+			synaptics_rmi4_store_error),
+	__ATTR(template_size, 0444,
+			udg_sysfs_template_size_show,
+			synaptics_rmi4_store_error),
+	__ATTR(template_max_index, 0444,
+			udg_sysfs_template_max_index_show,
+			synaptics_rmi4_store_error),
+	__ATTR(template_detection, 0444,
+			udg_sysfs_template_detection_show,
+			synaptics_rmi4_store_error),
+	__ATTR(template_index, 0220,
+			synaptics_rmi4_show_error,
+			udg_sysfs_template_index_store),
+	__ATTR(template_valid, 0664,
+			udg_sysfs_template_valid_show,
+			udg_sysfs_template_valid_store),
+	__ATTR(template_clear, 0220,
+			synaptics_rmi4_show_error,
+			udg_sysfs_template_clear_store),
+	__ATTR(trace_size, 0444,
+			udg_sysfs_trace_size_show,
+			synaptics_rmi4_store_error),
+};
+
+static struct bin_attribute template_data = {
+	.attr = {
+		.name = "template_data",
+		.mode = 0664,
+	},
+	.size = 0,
+	.read = udg_sysfs_template_data_show,
+	.write = udg_sysfs_template_data_store,
+};
+
+static struct bin_attribute trace_data = {
+	.attr = {
+		.name = "trace_data",
+		.mode = 0444,
+	},
+	.size = 0,
+	.read = udg_sysfs_trace_data_show,
+	.write = NULL,
+};
+
+static struct device_attribute params[] = {
+	__ATTR(template_displacement, 0664,
+			udg_sysfs_template_displacement_show,
+			udg_sysfs_template_displacement_store),
+	__ATTR(rotation_invariance, 0664,
+			udg_sysfs_rotation_invariance_show,
+			udg_sysfs_rotation_invariance_store),
+	__ATTR(scale_invariance, 0664,
+			udg_sysfs_scale_invariance_show,
+			udg_sysfs_scale_invariance_store),
+	__ATTR(threshold_factor, 0664,
+			udg_sysfs_threshold_factor_show,
+			udg_sysfs_threshold_factor_store),
+	__ATTR(match_metric_threshold, 0664,
+			udg_sysfs_match_metric_threshold_show,
+			udg_sysfs_match_metric_threshold_store),
+	__ATTR(max_inter_stroke_time, 0664,
+			udg_sysfs_max_inter_stroke_time_show,
+			udg_sysfs_max_inter_stroke_time_store),
+};
+
+static struct synaptics_rmi4_udg_handle *udg;
+
+static unsigned char ctrl_18_sub_size[] = {10, 10, 10, 2, 3, 4, 3, 3, 1, 1};
+static unsigned char ctrl_20_sub_size[] = {2};
+static unsigned char ctrl_23_sub_size[] = {1, 1, 1};
+static unsigned char ctrl_27_sub_size[] = {1, 5, 2, 1, 7};
+
+DECLARE_COMPLETION(udg_remove_complete);
+
+static ssize_t udg_sysfs_engine_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	bool enable;
+	unsigned int input;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input == 1)
+		enable = true;
+	else if (input == 0)
+		enable = false;
+	else
+		return -EINVAL;
+
+	retval = udg_engine_enable(enable);
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_detection_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	bool enable;
+	unsigned int input;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input == 1)
+		enable = true;
+	else if (input == 0)
+		enable = false;
+	else
+		return -EINVAL;
+
+	udg->detection_status = 0;
+
+	retval = udg_detection_enable(enable);
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_detection_score_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", udg->detection_score);
+}
+
+static ssize_t udg_sysfs_detection_index_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", udg->detection_index);
+}
+
+static ssize_t udg_sysfs_registration_enable_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	bool enable;
+	unsigned int input;
+	struct synaptics_rmi4_f12_control_41 control_41;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input == 1)
+		enable = true;
+	else if (input == 0)
+		enable = false;
+	else
+		return -EINVAL;
+
+	if (enable) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				udg->addr.ctrl_23,
+				udg->ctrl_buf,
+				udg->ctrl_23_sub3_off + 1);
+		if (retval < 0)
+			return retval;
+
+		udg->ctrl_buf[0] = 0;
+		udg->ctrl_buf[0] |= (1 << CTRL23_FINGER_REPORT_ENABLE_BIT);
+		if (udg->ctrl_23_sub3_off)
+			udg->ctrl_buf[udg->ctrl_23_sub3_off] = 0;
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				udg->addr.ctrl_23,
+				udg->ctrl_buf,
+				udg->ctrl_23_sub3_off + 1);
+		if (retval < 0)
+			return retval;
+	} else {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				udg->addr.ctrl_23,
+				udg->ctrl_buf,
+				udg->ctrl_23_sub3_off + 1);
+		if (retval < 0)
+			return retval;
+
+		udg->ctrl_buf[0] = udg->object_type_enable1;
+		if (udg->ctrl_23_sub3_off) {
+			udg->ctrl_buf[udg->ctrl_23_sub3_off] =
+					udg->object_type_enable2;
+		}
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				udg->addr.ctrl_23,
+				udg->ctrl_buf,
+				udg->ctrl_23_sub3_off + 1);
+		if (retval < 0)
+			return retval;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.ctrl_41,
+			control_41.data,
+			sizeof(control_41.data));
+	if (retval < 0)
+		return retval;
+
+	control_41.enable_registration = enable ? 1 : 0;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.ctrl_41,
+			control_41.data,
+			sizeof(control_41.data));
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_registration_begin_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	bool begin;
+	unsigned int input;
+	struct synaptics_rmi4_f12_control_41 control_41;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input == 1)
+		begin = true;
+	else if (input == 0)
+		begin = false;
+	else
+		return -EINVAL;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.ctrl_41,
+			control_41.data,
+			sizeof(control_41.data));
+	if (retval < 0)
+		return retval;
+
+	control_41.begin = begin ? 1 : 0;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.ctrl_41,
+			control_41.data,
+			sizeof(control_41.data));
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_registration_status_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "0x%02x\n", udg->registration_status);
+}
+
+static ssize_t udg_sysfs_template_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", udg->template_size);
+}
+
+static ssize_t udg_sysfs_template_max_index_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", udg->max_num_templates - 1);
+}
+
+static ssize_t udg_sysfs_template_detection_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	int attn_event;
+	unsigned char detection_status;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	attn_event = atomic_read(&udg->attn_event);
+	atomic_set(&udg->attn_event, 0);
+
+	if (attn_event == 0)
+		return snprintf(buf, PAGE_SIZE, "0\n");
+
+	if (udg->detection_status == 0) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				udg->addr.data_4,
+				rmi4_data->gesture_detection,
+				sizeof(rmi4_data->gesture_detection));
+		if (retval < 0)
+			return retval;
+
+		udg->detection_status = rmi4_data->gesture_detection[0];
+	}
+
+	detection_status = udg->detection_status;
+	udg->detection_status = 0;
+
+	switch (detection_status) {
+	case DETECTION:
+		udg->detection_score = rmi4_data->gesture_detection[1];
+		udg->detection_index = rmi4_data->gesture_detection[4];
+		udg->trace_size = rmi4_data->gesture_detection[3];
+		break;
+	case REGISTRATION:
+		udg->registration_status = rmi4_data->gesture_detection[1];
+		udg->trace_size = rmi4_data->gesture_detection[3];
+		break;
+	default:
+		return snprintf(buf, PAGE_SIZE, "0\n");
+	}
+
+	return snprintf(buf, PAGE_SIZE, "0x%02x\n", detection_status);
+}
+
+static ssize_t udg_sysfs_template_index_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long index;
+
+	retval = sstrtoul(buf, 10, &index);
+	if (retval)
+		return retval;
+
+	retval = udg_set_index((unsigned char)index);
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_template_valid_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned char valid;
+	unsigned char offset;
+	unsigned char byte_num;
+	unsigned char template_flags[2];
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	byte_num = udg->template_index / 8;
+	offset = udg->template_index % 8;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.template_flags,
+			template_flags,
+			sizeof(template_flags));
+	if (retval < 0)
+		return retval;
+
+	valid = (template_flags[byte_num] & (1 << offset)) >> offset;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", valid);
+}
+
+static ssize_t udg_sysfs_template_valid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long valid;
+	unsigned char offset;
+	unsigned char byte_num;
+	unsigned char template_flags[2];
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &valid);
+	if (retval)
+		return retval;
+
+	if (valid > 0)
+		valid = 1;
+
+	byte_num = udg->template_index / 8;
+	offset = udg->template_index % 8;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.template_flags,
+			template_flags,
+			sizeof(template_flags));
+	if (retval < 0)
+		return retval;
+
+	if (valid)
+		template_flags[byte_num] |= (1 << offset);
+	else
+		template_flags[byte_num] &= ~(1 << offset);
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.template_flags,
+			template_flags,
+			sizeof(template_flags));
+	if (retval < 0)
+		return retval;
+
+#ifdef STORE_GESTURES
+	udg_read_valid_data();
+#endif
+
+	return count;
+}
+
+static ssize_t udg_sysfs_template_clear_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	const char cmd[] = {'0', 0};
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input != 1)
+		return -EINVAL;
+
+	memset(udg->template_data_buf, 0x00, udg->template_data_size);
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.template_data,
+			udg->template_data_buf,
+			udg->template_data_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to clear template data\n",
+				__func__);
+		return retval;
+	}
+
+	retval = udg_sysfs_template_valid_store(dev, attr, cmd, 1);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to clear valid bit\n",
+				__func__);
+		return retval;
+	}
+
+#ifdef STORE_GESTURES
+	udg_read_template_data(udg->template_index);
+	udg_read_valid_data();
+#endif
+
+	return count;
+}
+
+static ssize_t udg_sysfs_trace_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", udg->trace_size);
+}
+
+static ssize_t udg_sysfs_trace_data_show(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	unsigned short index = 0;
+	unsigned short trace_data_size;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	trace_data_size = udg->trace_size * 5;
+
+	if (trace_data_size == 0)
+		return -EINVAL;
+
+	if (count < trace_data_size) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not enough space (%d bytes) in buffer\n",
+				__func__, (unsigned int)count);
+		return -EINVAL;
+	}
+
+	if (udg->trace_data_buf_size < trace_data_size) {
+		if (udg->trace_data_buf_size)
+			kfree(udg->trace_data_buf);
+		udg->trace_data_buf = kzalloc(trace_data_size, GFP_KERNEL);
+		if (!udg->trace_data_buf) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for trace data buffer\n",
+					__func__);
+			udg->trace_data_buf_size = 0;
+			return -ENOMEM;
+		}
+		udg->trace_data_buf_size = trace_data_size;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.trace_x,
+			&udg->trace_data_buf[index],
+			udg->trace_size * 2);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read trace X data\n",
+				__func__);
+		return retval;
+	}
+	index += udg->trace_size * 2;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.trace_y,
+			&udg->trace_data_buf[index],
+			udg->trace_size * 2);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read trace Y data\n",
+				__func__);
+		return retval;
+	}
+	index += udg->trace_size * 2;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.trace_segment,
+			&udg->trace_data_buf[index],
+			udg->trace_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read trace segment data\n",
+				__func__);
+		return retval;
+	}
+
+	retval = secure_memcpy(buf, count, udg->trace_data_buf,
+			udg->trace_data_buf_size, trace_data_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy trace data\n",
+				__func__);
+		return retval;
+	}
+
+	return trace_data_size;
+}
+
+static ssize_t udg_sysfs_template_data_show(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	if (count < udg->template_data_size) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Not enough space (%d bytes) in buffer\n",
+				__func__, (unsigned int)count);
+		return -EINVAL;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.template_data,
+			udg->template_data_buf,
+			udg->template_data_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read template data\n",
+				__func__);
+		return retval;
+	}
+
+	retval = secure_memcpy(buf, count, udg->template_data_buf,
+			udg->template_data_size, udg->template_data_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy template data\n",
+				__func__);
+		return retval;
+	}
+
+#ifdef STORE_GESTURES
+	udg_read_template_data(udg->template_index);
+	udg_read_valid_data();
+#endif
+
+	return udg->template_data_size;
+}
+
+static ssize_t udg_sysfs_template_data_store(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	retval = secure_memcpy(udg->template_data_buf, udg->template_data_size,
+			buf, count, count);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy template data\n",
+				__func__);
+		return retval;
+	}
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.template_data,
+			udg->template_data_buf,
+			count);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write template data\n",
+				__func__);
+		return retval;
+	}
+
+#ifdef STORE_GESTURES
+	udg_read_template_data(udg->template_index);
+	udg_read_valid_data();
+#endif
+
+	return count;
+}
+
+static ssize_t udg_sysfs_template_displacement_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned short template_displacement;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	template_displacement =
+			((unsigned short)udg->tuning.template_disp_lsb << 0) |
+			((unsigned short)udg->tuning.template_disp_msb << 8);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", template_displacement);
+}
+
+static ssize_t udg_sysfs_template_displacement_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long input;
+
+	retval = sstrtoul(buf, 10, &input);
+	if (retval)
+		return retval;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	udg->tuning.template_disp_lsb = (unsigned char)(input >> 0);
+	udg->tuning.template_disp_msb = (unsigned char)(input >> 8);
+
+	retval = udg_write_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_rotation_invariance_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned short rotation_invariance;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	rotation_invariance =
+			((unsigned short)udg->tuning.rotation_inv_lsb << 0) |
+			((unsigned short)udg->tuning.rotation_inv_msb << 8);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", rotation_invariance);
+}
+
+static ssize_t udg_sysfs_rotation_invariance_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long input;
+
+	retval = sstrtoul(buf, 10, &input);
+	if (retval)
+		return retval;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	udg->tuning.rotation_inv_lsb = (unsigned char)(input >> 0);
+	udg->tuning.rotation_inv_msb = (unsigned char)(input >> 8);
+
+	retval = udg_write_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_scale_invariance_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned short scale_invariance;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	scale_invariance =
+			((unsigned short)udg->tuning.scale_inv_lsb << 0) |
+			((unsigned short)udg->tuning.scale_inv_msb << 8);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", scale_invariance);
+}
+
+static ssize_t udg_sysfs_scale_invariance_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long input;
+
+	retval = sstrtoul(buf, 10, &input);
+	if (retval)
+		return retval;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	udg->tuning.scale_inv_lsb = (unsigned char)(input >> 0);
+	udg->tuning.scale_inv_msb = (unsigned char)(input >> 8);
+
+	retval = udg_write_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_threshold_factor_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned short threshold_factor;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	threshold_factor =
+			((unsigned short)udg->tuning.thres_factor_lsb << 0) |
+			((unsigned short)udg->tuning.thres_factor_msb << 8);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", threshold_factor);
+}
+
+static ssize_t udg_sysfs_threshold_factor_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long input;
+
+	retval = sstrtoul(buf, 10, &input);
+	if (retval)
+		return retval;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	udg->tuning.thres_factor_lsb = (unsigned char)(input >> 0);
+	udg->tuning.thres_factor_msb = (unsigned char)(input >> 8);
+
+	retval = udg_write_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_match_metric_threshold_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned short match_metric_threshold;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	match_metric_threshold =
+			((unsigned short)udg->tuning.metric_thres_lsb << 0) |
+			((unsigned short)udg->tuning.metric_thres_msb << 8);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", match_metric_threshold);
+}
+
+static ssize_t udg_sysfs_match_metric_threshold_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long input;
+
+	retval = sstrtoul(buf, 10, &input);
+	if (retval)
+		return retval;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	udg->tuning.metric_thres_lsb = (unsigned char)(input >> 0);
+	udg->tuning.metric_thres_msb = (unsigned char)(input >> 8);
+
+	retval = udg_write_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t udg_sysfs_max_inter_stroke_time_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned short max_inter_stroke_time;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	max_inter_stroke_time =
+			((unsigned short)udg->tuning.inter_stroke_lsb << 0) |
+			((unsigned short)udg->tuning.inter_stroke_msb << 8);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", max_inter_stroke_time);
+}
+
+static ssize_t udg_sysfs_max_inter_stroke_time_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long input;
+
+	retval = sstrtoul(buf, 10, &input);
+	if (retval)
+		return retval;
+
+	retval = udg_read_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	udg->tuning.inter_stroke_lsb = (unsigned char)(input >> 0);
+	udg->tuning.inter_stroke_msb = (unsigned char)(input >> 8);
+
+	retval = udg_write_tuning_params();
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static int udg_ctrl_subpacket(unsigned char ctrlreg,
+		unsigned char subpacket,
+		struct synaptics_rmi4_f12_query_5 *query_5)
+{
+	int retval;
+	unsigned char cnt;
+	unsigned char regnum;
+	unsigned char bitnum;
+	unsigned char q5_index;
+	unsigned char q6_index;
+	unsigned char offset;
+	unsigned char max_ctrlreg;
+	unsigned char *query_6;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	max_ctrlreg = (sizeof(query_5->data) - 1) * 8 - 1;
+
+	if (ctrlreg > max_ctrlreg) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Control register number (%d) over limit\n",
+				__func__, ctrlreg);
+		return -EINVAL;
+	}
+
+	q5_index = ctrlreg / 8 + 1;
+	bitnum = ctrlreg % 8;
+	if ((query_5->data[q5_index] & (1 << bitnum)) == 0x00) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Control %d is not present\n",
+				__func__, ctrlreg);
+		return -EINVAL;
+	}
+
+	query_6 = kmalloc(query_5->size_of_query6, GFP_KERNEL);
+	if (!query_6) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for query 6\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->query_base_addr + 6,
+			query_6,
+			query_5->size_of_query6);
+	if (retval < 0)
+		goto exit;
+
+	q6_index = 0;
+
+	for (regnum = 0; regnum < ctrlreg; regnum++) {
+		q5_index = regnum / 8 + 1;
+		bitnum = regnum % 8;
+		if ((query_5->data[q5_index] & (1 << bitnum)) == 0x00)
+			continue;
+
+		if (query_6[q6_index] == 0x00)
+			q6_index += 3;
+		else
+			q6_index++;
+
+		while (query_6[q6_index] & ~MASK_7BIT)
+			q6_index++;
+
+		q6_index++;
+	}
+
+	cnt = 0;
+	q6_index++;
+	offset = subpacket / 7;
+	bitnum = subpacket % 7;
+
+	do {
+		if (cnt == offset) {
+			if (query_6[q6_index + cnt] & (1 << bitnum))
+				retval = 1;
+			else
+				retval = 0;
+			goto exit;
+		}
+		cnt++;
+	} while (query_6[q6_index + cnt - 1] & ~MASK_7BIT);
+
+	retval = 0;
+
+exit:
+	kfree(query_6);
+
+	return retval;
+}
+
+static int udg_read_tuning_params(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.ctrl_18,
+			udg->ctrl_buf,
+			udg->ctrl_18_sub10_off + sizeof(struct udg_tuning));
+	if (retval < 0)
+		return retval;
+
+	secure_memcpy(udg->tuning.data,
+			sizeof(udg->tuning.data),
+			(unsigned char *)&udg->ctrl_buf[udg->ctrl_18_sub10_off],
+			sizeof(struct udg_tuning),
+			sizeof(struct udg_tuning));
+
+	return 0;
+}
+
+static int udg_write_tuning_params(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	secure_memcpy((unsigned char *)&udg->ctrl_buf[udg->ctrl_18_sub10_off],
+			sizeof(struct udg_tuning),
+			udg->tuning.data,
+			sizeof(udg->tuning.data),
+			sizeof(struct udg_tuning));
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.ctrl_18,
+			udg->ctrl_buf,
+			udg->ctrl_18_sub10_off + sizeof(struct udg_tuning));
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int udg_detection_enable(bool enable)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.ctrl_20,
+			udg->ctrl_buf,
+			udg->ctrl_20_sub1_off + 1);
+	if (retval < 0)
+		return retval;
+
+	if (enable)
+		udg->ctrl_buf[udg->ctrl_20_sub1_off] = WAKEUP_GESTURE_MODE;
+	else
+		udg->ctrl_buf[udg->ctrl_20_sub1_off] = udg->report_flags;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.ctrl_20,
+			udg->ctrl_buf,
+			udg->ctrl_20_sub1_off + 1);
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int udg_engine_enable(bool enable)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	if (enable) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				udg->addr.ctrl_27,
+				udg->ctrl_buf,
+				udg->ctrl_27_sub5_off + 1);
+		if (retval < 0)
+			return retval;
+
+		udg->ctrl_buf[udg->ctrl_27_sub5_off] |=
+				(1 << CTRL27_UDG_ENABLE_BIT);
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				udg->addr.ctrl_27,
+				udg->ctrl_buf,
+				udg->ctrl_27_sub5_off + 1);
+		if (retval < 0)
+			return retval;
+	} else {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				udg->addr.ctrl_27,
+				udg->ctrl_buf,
+				udg->ctrl_27_sub5_off + 1);
+		if (retval < 0)
+			return retval;
+
+		udg->ctrl_buf[udg->ctrl_27_sub5_off] &=
+				~(1 << CTRL27_UDG_ENABLE_BIT);
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				udg->addr.ctrl_27,
+				udg->ctrl_buf,
+				udg->ctrl_27_sub5_off + 1);
+		if (retval < 0)
+			return retval;
+	}
+
+	return 0;
+}
+
+static void udg_report(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	atomic_set(&udg->attn_event, 1);
+
+	if (rmi4_data->suspend) {
+		if (rmi4_data->gesture_detection[0] == 0) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					udg->addr.data_4,
+					rmi4_data->gesture_detection,
+					sizeof(rmi4_data->gesture_detection));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to read gesture detection\n",
+						__func__);
+				return;
+			}
+		}
+
+		udg->detection_status = rmi4_data->gesture_detection[0];
+		rmi4_data->gesture_detection[0] = 0;
+
+		if (udg->detection_status == DETECTION) {
+			input_report_key(udg->udg_dev, KEY_WAKEUP, 1);
+			input_sync(udg->udg_dev);
+			input_report_key(udg->udg_dev, KEY_WAKEUP, 0);
+			input_sync(udg->udg_dev);
+			rmi4_data->suspend = false;
+		}
+	}
+
+	return;
+}
+
+static int udg_set_index(unsigned char index)
+{
+	int retval;
+	struct synaptics_rmi4_f12_control_41 control_41;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	if (index >= udg->max_num_templates)
+		return -EINVAL;
+
+	udg->template_index = index;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.ctrl_41,
+			control_41.data,
+			sizeof(control_41.data));
+	if (retval < 0)
+		return retval;
+
+	control_41.template_index = udg->template_index;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.ctrl_41,
+			control_41.data,
+			sizeof(control_41.data));
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+#ifdef STORE_GESTURES
+static int udg_read_valid_data(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.template_flags,
+			udg->valid_buf,
+			sizeof(udg->valid_buf));
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int udg_write_valid_data(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			udg->addr.template_flags,
+			udg->valid_buf,
+			sizeof(udg->valid_buf));
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int udg_read_template_data(unsigned char index)
+{
+	int retval;
+	unsigned char *storage;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	udg_set_index(index);
+	storage = &(udg->storage_buf[index * udg->template_data_size]);
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.template_data,
+			storage,
+			udg->template_data_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read template data\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int udg_write_template_data(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char *storage;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	for (ii = 0; ii < udg->gestures_to_store; ii++) {
+		udg_set_index(ii);
+		storage = &(udg->storage_buf[ii * udg->template_data_size]);
+
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				udg->addr.template_data,
+				storage,
+				udg->template_data_size);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write template data\n",
+					__func__);
+			return retval;
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static int udg_reg_init(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char data_offset;
+	unsigned char size_of_query;
+	unsigned char ctrl_18_offset;
+	unsigned char ctrl_20_offset;
+	unsigned char ctrl_23_offset;
+	unsigned char ctrl_27_offset;
+	unsigned char ctrl_41_offset;
+	struct synaptics_rmi4_f12_query_0 query_0;
+	struct synaptics_rmi4_f12_query_5 query_5;
+	struct synaptics_rmi4_f12_query_8 query_8;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->query_base_addr + 7,
+			&size_of_query,
+			sizeof(size_of_query));
+	if (retval < 0)
+		return retval;
+
+	if (size_of_query < 4) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: User defined gesture support unavailable (missing data registers)\n",
+				__func__);
+		retval = -ENODEV;
+		return retval;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->query_base_addr + 8,
+			query_8.data,
+			sizeof(query_8.data));
+	if (retval < 0)
+		return retval;
+
+	if ((query_8.data16_is_present) &&
+			(query_8.data17_is_present) &&
+			(query_8.data18_is_present) &&
+			(query_8.data19_is_present) &&
+			(query_8.data20_is_present) &&
+			(query_8.data21_is_present)) {
+		data_offset = query_8.data0_is_present +
+				query_8.data1_is_present +
+				query_8.data2_is_present +
+				query_8.data3_is_present;
+		udg->addr.data_4 = udg->data_base_addr + data_offset;
+		data_offset = data_offset +
+				query_8.data4_is_present +
+				query_8.data5_is_present +
+				query_8.data6_is_present +
+				query_8.data7_is_present +
+				query_8.data8_is_present +
+				query_8.data9_is_present +
+				query_8.data10_is_present +
+				query_8.data11_is_present +
+				query_8.data12_is_present +
+				query_8.data13_is_present +
+				query_8.data14_is_present +
+				query_8.data15_is_present;
+		udg->addr.trace_x = udg->data_base_addr + data_offset;
+		udg->addr.trace_y = udg->addr.trace_x + 1;
+		udg->addr.trace_segment = udg->addr.trace_y + 1;
+		udg->addr.template_helper = udg->addr.trace_segment + 1;
+		udg->addr.template_data = udg->addr.template_helper + 1;
+		udg->addr.template_flags = udg->addr.template_data + 1;
+	} else {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: User defined gesture support unavailable (missing data registers)\n",
+				__func__);
+		retval = -ENODEV;
+		return retval;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->query_base_addr + 4,
+			&size_of_query,
+			sizeof(size_of_query));
+	if (retval < 0)
+		return retval;
+
+	if (size_of_query < 7) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: User defined gesture support unavailable (missing control registers)\n",
+				__func__);
+		retval = -ENODEV;
+		return retval;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->query_base_addr + 5,
+			query_5.data,
+			sizeof(query_5.data));
+	if (retval < 0)
+		return retval;
+
+	ctrl_18_offset = query_5.ctrl0_is_present +
+			query_5.ctrl1_is_present +
+			query_5.ctrl2_is_present +
+			query_5.ctrl3_is_present +
+			query_5.ctrl4_is_present +
+			query_5.ctrl5_is_present +
+			query_5.ctrl6_is_present +
+			query_5.ctrl7_is_present +
+			query_5.ctrl8_is_present +
+			query_5.ctrl9_is_present +
+			query_5.ctrl10_is_present +
+			query_5.ctrl11_is_present +
+			query_5.ctrl12_is_present +
+			query_5.ctrl13_is_present +
+			query_5.ctrl14_is_present +
+			query_5.ctrl15_is_present +
+			query_5.ctrl16_is_present +
+			query_5.ctrl17_is_present;
+
+	ctrl_20_offset = ctrl_18_offset +
+			query_5.ctrl18_is_present +
+			query_5.ctrl19_is_present;
+
+	ctrl_23_offset = ctrl_20_offset +
+			query_5.ctrl20_is_present +
+			query_5.ctrl21_is_present +
+			query_5.ctrl22_is_present;
+
+	ctrl_27_offset = ctrl_23_offset+
+			query_5.ctrl23_is_present +
+			query_5.ctrl24_is_present +
+			query_5.ctrl25_is_present +
+			query_5.ctrl26_is_present;
+
+	ctrl_41_offset = ctrl_27_offset+
+			query_5.ctrl27_is_present +
+			query_5.ctrl28_is_present +
+			query_5.ctrl29_is_present +
+			query_5.ctrl30_is_present +
+			query_5.ctrl31_is_present +
+			query_5.ctrl32_is_present +
+			query_5.ctrl33_is_present +
+			query_5.ctrl34_is_present +
+			query_5.ctrl35_is_present +
+			query_5.ctrl36_is_present +
+			query_5.ctrl37_is_present +
+			query_5.ctrl38_is_present +
+			query_5.ctrl39_is_present +
+			query_5.ctrl40_is_present;
+
+	udg->addr.ctrl_18 = udg->control_base_addr + ctrl_18_offset;
+	udg->addr.ctrl_20 = udg->control_base_addr + ctrl_20_offset;
+	udg->addr.ctrl_23 = udg->control_base_addr + ctrl_23_offset;
+	udg->addr.ctrl_27 = udg->control_base_addr + ctrl_27_offset;
+	udg->addr.ctrl_41 = udg->control_base_addr + ctrl_41_offset;
+
+	udg->ctrl_18_sub10_off = 0;
+	for (ii = 0; ii < 10; ii++) {
+		retval = udg_ctrl_subpacket(18, ii, &query_5);
+		if (retval == 1)
+			udg->ctrl_18_sub10_off += ctrl_18_sub_size[ii];
+		else if (retval < 0)
+			return retval;
+	}
+
+	udg->ctrl_20_sub1_off = 0;
+	for (ii = 0; ii < 1; ii++) {
+		retval = udg_ctrl_subpacket(20, ii, &query_5);
+		if (retval == 1)
+			udg->ctrl_20_sub1_off += ctrl_20_sub_size[ii];
+		else if (retval < 0)
+			return retval;
+	}
+
+	udg->ctrl_23_sub3_off = 0;
+	for (ii = 0; ii < 3; ii++) {
+		retval = udg_ctrl_subpacket(23, ii, &query_5);
+		if (retval == 1)
+			udg->ctrl_23_sub3_off += ctrl_23_sub_size[ii];
+		else if (retval < 0)
+			return retval;
+	}
+
+	retval = udg_ctrl_subpacket(23, 3, &query_5);
+	if (retval == 0)
+		udg->ctrl_23_sub3_off = 0;
+	else if (retval < 0)
+		return retval;
+
+	udg->ctrl_27_sub5_off = 0;
+	for (ii = 0; ii < 5; ii++) {
+		retval = udg_ctrl_subpacket(27, ii, &query_5);
+		if (retval == 1)
+			udg->ctrl_27_sub5_off += ctrl_27_sub_size[ii];
+		else if (retval < 0)
+			return retval;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->query_base_addr + 0,
+			query_0.data,
+			sizeof(query_0.data));
+	if (retval < 0)
+		return retval;
+
+	udg->max_num_templates = query_0.max_num_templates;
+	udg->template_size =
+			((unsigned short)query_0.template_size_lsb << 0) |
+			((unsigned short)query_0.template_size_msb << 8);
+	udg->template_data_size = udg->template_size * 4 * 2 + 4 + 1;
+
+#ifdef STORE_GESTURES
+	udg->gestures_to_store = udg->max_num_templates;
+	if (GESTURES_TO_STORE < udg->gestures_to_store)
+		udg->gestures_to_store = GESTURES_TO_STORE;
+#endif
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.ctrl_20,
+			udg->ctrl_buf,
+			udg->ctrl_20_sub1_off + 1);
+	if (retval < 0)
+		return retval;
+
+	udg->report_flags = udg->ctrl_buf[udg->ctrl_20_sub1_off];
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			udg->addr.ctrl_23,
+			udg->ctrl_buf,
+			udg->ctrl_23_sub3_off + 1);
+	if (retval < 0)
+		return retval;
+
+	udg->object_type_enable1 = udg->ctrl_buf[0];
+	if (udg->ctrl_23_sub3_off)
+		udg->object_type_enable2 = udg->ctrl_buf[udg->ctrl_23_sub3_off];
+
+	return retval;
+}
+
+static int udg_scan_pdt(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char page;
+	unsigned char intr_count = 0;
+	unsigned char intr_off;
+	unsigned char intr_src;
+	unsigned short addr;
+	struct synaptics_rmi4_fn_desc fd;
+	struct synaptics_rmi4_data *rmi4_data = udg->rmi4_data;
+
+	for (page = 0; page < PAGES_TO_SERVICE; page++) {
+		for (addr = PDT_START; addr > PDT_END; addr -= PDT_ENTRY_SIZE) {
+			addr |= (page << 8);
+
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					addr,
+					(unsigned char *)&fd,
+					sizeof(fd));
+			if (retval < 0)
+				return retval;
+
+			addr &= ~(MASK_8BIT << 8);
+
+			if (fd.fn_number) {
+				dev_dbg(rmi4_data->pdev->dev.parent,
+						"%s: Found F%02x\n",
+						__func__, fd.fn_number);
+				switch (fd.fn_number) {
+				case SYNAPTICS_RMI4_F12:
+					goto f12_found;
+					break;
+				}
+			} else {
+				break;
+			}
+
+			intr_count += fd.intr_src_count;
+		}
+	}
+
+	dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Failed to find F12\n",
+			__func__);
+	return -EINVAL;
+
+f12_found:
+	udg->query_base_addr = fd.query_base_addr | (page << 8);
+	udg->control_base_addr = fd.ctrl_base_addr | (page << 8);
+	udg->data_base_addr = fd.data_base_addr | (page << 8);
+	udg->command_base_addr = fd.cmd_base_addr | (page << 8);
+
+	retval = udg_reg_init();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to initialize user defined gesture registers\n",
+				__func__);
+		return retval;
+	}
+
+	udg->intr_mask = 0;
+	intr_src = fd.intr_src_count;
+	intr_off = intr_count % 8;
+	for (ii = intr_off;
+			ii < (intr_src + intr_off);
+			ii++) {
+		udg->intr_mask |= 1 << ii;
+	}
+
+	rmi4_data->intr_mask[0] |= udg->intr_mask;
+
+	addr = rmi4_data->f01_ctrl_base_addr + 1;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			addr,
+			&rmi4_data->intr_mask[0],
+			sizeof(rmi4_data->intr_mask[0]));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set interrupt enable bit\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static void synaptics_rmi4_udg_attn(struct synaptics_rmi4_data *rmi4_data,
+		unsigned char intr_mask)
+{
+	if (!udg)
+		return;
+
+	if (udg->intr_mask & intr_mask)
+		udg_report();
+
+	return;
+}
+
+static int synaptics_rmi4_udg_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char size;
+	unsigned char attr_count;
+	unsigned char param_count;
+
+	if (udg) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Handle already exists\n",
+				__func__);
+		return 0;
+	}
+
+	udg = kzalloc(sizeof(*udg), GFP_KERNEL);
+	if (!udg) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for udg\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	size = 0;
+	for (ii = 0; ii < sizeof(ctrl_18_sub_size); ii++)
+		size += ctrl_18_sub_size[ii];
+	size += sizeof(struct udg_tuning);
+	udg->ctrl_buf = kzalloc(size, GFP_KERNEL);
+	if (!udg->ctrl_buf) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for ctrl_buf\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_udg;
+	}
+
+	udg->rmi4_data = rmi4_data;
+
+	retval = udg_scan_pdt();
+	if (retval < 0)
+		goto exit_free_ctrl_buf;
+
+	udg->template_data_buf = kzalloc(udg->template_data_size, GFP_KERNEL);
+	if (!udg->template_data_buf) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for template_data_buf\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_ctrl_buf;
+	}
+
+#ifdef STORE_GESTURES
+	udg->storage_buf = kzalloc(
+			udg->template_data_size * udg->gestures_to_store,
+			GFP_KERNEL);
+	if (!udg->storage_buf) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for storage_buf\n",
+				__func__);
+		kfree(udg->template_data_buf);
+		retval = -ENOMEM;
+		goto exit_free_ctrl_buf;
+	}
+#endif
+
+	udg->udg_dev = input_allocate_device();
+	if (udg->udg_dev == NULL) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to allocate gesture device\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_template_data_buf;
+	}
+
+	udg->udg_dev->name = GESTURE_DRIVER_NAME;
+	udg->udg_dev->phys = GESTURE_PHYS_NAME;
+	udg->udg_dev->id.product = SYNAPTICS_DSX_DRIVER_PRODUCT;
+	udg->udg_dev->id.version = SYNAPTICS_DSX_DRIVER_VERSION;
+	udg->udg_dev->dev.parent = rmi4_data->pdev->dev.parent;
+	input_set_drvdata(udg->udg_dev, rmi4_data);
+
+	set_bit(EV_KEY, udg->udg_dev->evbit);
+	set_bit(KEY_WAKEUP, udg->udg_dev->keybit);
+	input_set_capability(udg->udg_dev, EV_KEY, KEY_WAKEUP);
+
+	retval = input_register_device(udg->udg_dev);
+	if (retval) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to register gesture device\n",
+				__func__);
+		input_free_device(udg->udg_dev);
+		goto exit_free_template_data_buf;
+	}
+
+	udg->tuning_dir = kobject_create_and_add(TUNING_SYSFS_DIR_NAME,
+			&udg->udg_dev->dev.kobj);
+	if (!udg->tuning_dir) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create tuning sysfs directory\n",
+				__func__);
+		goto exit_unregister_input_device;
+	}
+
+	retval = sysfs_create_bin_file(&udg->udg_dev->dev.kobj, &template_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create template data bin file\n",
+				__func__);
+		goto exit_remove_sysfs_directory;
+	}
+
+	retval = sysfs_create_bin_file(&udg->udg_dev->dev.kobj, &trace_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create trace data bin file\n",
+				__func__);
+		goto exit_remove_bin_file;
+	}
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		retval = sysfs_create_file(&udg->udg_dev->dev.kobj,
+				&attrs[attr_count].attr);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create sysfs attributes\n",
+					__func__);
+			retval = -ENODEV;
+			goto exit_remove_attrs;
+		}
+	}
+
+	for (param_count = 0; param_count < ARRAY_SIZE(params); param_count++) {
+		retval = sysfs_create_file(udg->tuning_dir,
+				&params[param_count].attr);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create tuning parameters\n",
+					__func__);
+			retval = -ENODEV;
+			goto exit_remove_params;
+		}
+	}
+
+	retval = udg_engine_enable(true);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to enable gesture engine\n",
+				__func__);
+		goto exit_remove_params;
+	}
+
+	return 0;
+
+exit_remove_params:
+	for (param_count--; param_count >= 0; param_count--) {
+		sysfs_remove_file(udg->tuning_dir,
+				&params[param_count].attr);
+	}
+
+exit_remove_attrs:
+	for (attr_count--; attr_count >= 0; attr_count--) {
+		sysfs_remove_file(&udg->udg_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+	sysfs_remove_bin_file(&udg->udg_dev->dev.kobj, &trace_data);
+
+exit_remove_bin_file:
+	sysfs_remove_bin_file(&udg->udg_dev->dev.kobj, &template_data);
+
+exit_remove_sysfs_directory:
+	kobject_put(udg->tuning_dir);
+
+exit_unregister_input_device:
+	input_unregister_device(udg->udg_dev);
+
+exit_free_template_data_buf:
+#ifdef STORE_GESTURES
+	kfree(udg->storage_buf);
+#endif
+	kfree(udg->template_data_buf);
+
+exit_free_ctrl_buf:
+	kfree(udg->ctrl_buf);
+
+exit_free_udg:
+	kfree(udg);
+	udg = NULL;
+
+exit:
+	return retval;
+}
+
+static void synaptics_rmi4_udg_remove(struct synaptics_rmi4_data *rmi4_data)
+{
+	unsigned char count;
+
+	if (!udg)
+		goto exit;
+
+	for (count = 0; count < ARRAY_SIZE(params); count++) {
+		sysfs_remove_file(udg->tuning_dir,
+				&params[count].attr);
+	}
+
+	for (count = 0; count < ARRAY_SIZE(attrs); count++) {
+		sysfs_remove_file(&udg->udg_dev->dev.kobj,
+				&attrs[count].attr);
+	}
+
+	sysfs_remove_bin_file(&udg->udg_dev->dev.kobj, &trace_data);
+	sysfs_remove_bin_file(&udg->udg_dev->dev.kobj, &template_data);
+	kobject_put(udg->tuning_dir);
+
+	input_unregister_device(udg->udg_dev);
+#ifdef STORE_GESTURES
+	kfree(udg->storage_buf);
+#endif
+	kfree(udg->template_data_buf);
+	kfree(udg->trace_data_buf);
+	kfree(udg->ctrl_buf);
+	kfree(udg);
+	udg = NULL;
+
+exit:
+	complete(&udg_remove_complete);
+}
+
+static void synaptics_rmi4_udg_reset(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!udg) {
+		synaptics_rmi4_udg_init(rmi4_data);
+		return;
+	}
+
+	udg_scan_pdt();
+	udg_engine_enable(true);
+#ifdef STORE_GESTURES
+	udg_write_template_data();
+	udg_write_valid_data();
+#endif
+}
+
+static void synaptics_rmi4_udg_reinit(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!udg)
+		return;
+
+	udg_engine_enable(true);
+#ifdef STORE_GESTURES
+	udg_write_template_data();
+	udg_write_valid_data();
+#endif
+}
+
+static void synaptics_rmi4_udg_e_suspend(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!udg)
+		return;
+
+	rmi4_data->sleep_enable(rmi4_data, false);
+	rmi4_data->irq_enable(rmi4_data, true, false);
+	enable_irq_wake(rmi4_data->irq);
+
+	udg_engine_enable(true);
+	udg_detection_enable(true);
+}
+
+static void synaptics_rmi4_udg_suspend(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!udg)
+		return;
+
+	rmi4_data->sleep_enable(rmi4_data, false);
+	rmi4_data->irq_enable(rmi4_data, true, false);
+	enable_irq_wake(rmi4_data->irq);
+
+	udg_engine_enable(true);
+	udg_detection_enable(true);
+}
+
+static void synaptics_rmi4_udg_resume(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!udg)
+		return;
+
+	disable_irq_wake(rmi4_data->irq);
+	udg_detection_enable(false);
+}
+
+static void synaptics_rmi4_udg_l_resume(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!udg)
+		return;
+
+	disable_irq_wake(rmi4_data->irq);
+	udg_detection_enable(false);
+}
+
+static struct synaptics_rmi4_exp_fn gesture_module = {
+	.fn_type = RMI_GESTURE,
+	.init = synaptics_rmi4_udg_init,
+	.remove = synaptics_rmi4_udg_remove,
+	.reset = synaptics_rmi4_udg_reset,
+	.reinit = synaptics_rmi4_udg_reinit,
+	.early_suspend = synaptics_rmi4_udg_e_suspend,
+	.suspend = synaptics_rmi4_udg_suspend,
+	.resume = synaptics_rmi4_udg_resume,
+	.late_resume = synaptics_rmi4_udg_l_resume,
+	.attn = synaptics_rmi4_udg_attn,
+};
+
+static int __init rmi4_gesture_module_init(void)
+{
+	synaptics_rmi4_new_function(&gesture_module, true);
+
+	return 0;
+}
+
+static void __exit rmi4_gesture_module_exit(void)
+{
+	synaptics_rmi4_new_function(&gesture_module, false);
+
+	wait_for_completion(&udg_remove_complete);
+}
+
+module_init(rmi4_gesture_module_init);
+module_exit(rmi4_gesture_module_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX User Defined Gesture Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_i2c.c

@@ -0,0 +1,672 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/types.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+#include "linux/moduleparam.h"
+
+#define SYN_I2C_RETRY_TIMES 10
+#define rd_msgs  1
+
+#ifdef CONFIG_DRM
+#include <drm/drm_panel.h>
+struct drm_panel *active_panel;
+#endif
+
+static unsigned char *wr_buf;
+
+static struct synaptics_dsx_hw_interface hw_if;
+
+static struct platform_device *synaptics_dsx_i2c_device;
+
+
+#ifdef CONFIG_OF
+static int parse_dt(struct device *dev, struct synaptics_dsx_board_data *bdata)
+{
+	int retval;
+	u32 value;
+	const char *name;
+	struct property *prop;
+	struct device_node *np = dev->of_node;
+
+	bdata->irq_gpio = of_get_named_gpio_flags(np,
+			"synaptics,irq-gpio", 0,
+			(enum of_gpio_flags *)&bdata->irq_flags);
+
+	retval = of_property_read_u32(np, "synaptics,irq-on-state",
+			&value);
+	if (retval < 0)
+		bdata->irq_on_state = 0;
+	else
+		bdata->irq_on_state = value;
+
+	retval = of_property_read_string(np, "synaptics,pwr-reg-name", &name);
+	if (retval < 0)
+		bdata->pwr_reg_name = NULL;
+	else
+		bdata->pwr_reg_name = name;
+
+	retval = of_property_read_string(np, "synaptics,bus-reg-name", &name);
+	if (retval < 0)
+		bdata->bus_reg_name = NULL;
+	else
+		bdata->bus_reg_name = name;
+
+	prop = of_find_property(np, "synaptics,power-gpio", NULL);
+	if (prop && prop->length) {
+		bdata->power_gpio = of_get_named_gpio_flags(np,
+				"synaptics,power-gpio", 0, NULL);
+		retval = of_property_read_u32(np, "synaptics,power-on-state",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,power-on-state property\n",
+					__func__);
+			return retval;
+		}
+		bdata->power_on_state = value;
+	} else {
+		bdata->power_gpio = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,power-delay-ms", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,power-delay-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,power-delay-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->power_delay_ms = value;
+	} else {
+		bdata->power_delay_ms = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,reset-gpio", NULL);
+	if (prop && prop->length) {
+		bdata->reset_gpio = of_get_named_gpio_flags(np,
+				"synaptics,reset-gpio", 0, NULL);
+		retval = of_property_read_u32(np, "synaptics,reset-on-state",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-on-state property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_on_state = value;
+		retval = of_property_read_u32(np, "synaptics,reset-active-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-active-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_active_ms = value;
+	} else {
+		bdata->reset_gpio = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,reset-delay-ms", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,reset-delay-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-delay-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_delay_ms = value;
+	} else {
+		bdata->reset_delay_ms = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,max-y-for-2d", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,max-y-for-2d",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,max-y-for-2d property\n",
+					__func__);
+			return retval;
+		}
+		bdata->max_y_for_2d = value;
+	} else {
+		bdata->max_y_for_2d = -1;
+	}
+
+	bdata->swap_axes = of_property_read_bool(np, "synaptics,swap-axes");
+	bdata->x_flip = of_property_read_bool(np, "synaptics,x-flip");
+	bdata->y_flip = of_property_read_bool(np, "synaptics,y-flip");
+
+	prop = of_find_property(np, "synaptics,ub-i2c-addr", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,ub-i2c-addr",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,ub-i2c-addr property\n",
+					__func__);
+			return retval;
+		}
+		bdata->ub_i2c_addr = (unsigned short)value;
+	} else {
+		bdata->ub_i2c_addr = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,cap-button-codes", NULL);
+	if (prop && prop->length) {
+		bdata->cap_button_map->map = devm_kzalloc(dev,
+				prop->length,
+				GFP_KERNEL);
+		if (!bdata->cap_button_map->map)
+			return -ENOMEM;
+		bdata->cap_button_map->nbuttons = prop->length / sizeof(u32);
+		retval = of_property_read_u32_array(np,
+				"synaptics,cap-button-codes",
+				bdata->cap_button_map->map,
+				bdata->cap_button_map->nbuttons);
+		if (retval < 0) {
+			bdata->cap_button_map->nbuttons = 0;
+			bdata->cap_button_map->map = NULL;
+		}
+	} else {
+		bdata->cap_button_map->nbuttons = 0;
+		bdata->cap_button_map->map = NULL;
+	}
+
+	prop = of_find_property(np, "synaptics,vir-button-codes", NULL);
+	if (prop && prop->length) {
+		bdata->vir_button_map->map = devm_kzalloc(dev,
+				prop->length,
+				GFP_KERNEL);
+		if (!bdata->vir_button_map->map)
+			return -ENOMEM;
+		bdata->vir_button_map->nbuttons = prop->length / sizeof(u32);
+		bdata->vir_button_map->nbuttons /= 5;
+		retval = of_property_read_u32_array(np,
+				"synaptics,vir-button-codes",
+				bdata->vir_button_map->map,
+				bdata->vir_button_map->nbuttons * 5);
+		if (retval < 0) {
+			bdata->vir_button_map->nbuttons = 0;
+			bdata->vir_button_map->map = NULL;
+		}
+	} else {
+		bdata->vir_button_map->nbuttons = 0;
+		bdata->vir_button_map->map = NULL;
+	}
+
+	return 0;
+}
+#endif
+
+static int synaptics_rmi4_i2c_alloc_buf(struct synaptics_rmi4_data *rmi4_data,
+		unsigned int count)
+{
+	static unsigned int buf_size;
+
+	if (count > buf_size) {
+		if (buf_size)
+			kfree(wr_buf);
+		wr_buf = kzalloc(count, GFP_KERNEL);
+		if (!wr_buf) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for buffer\n",
+					__func__);
+			buf_size = 0;
+			return -ENOMEM;
+		}
+		buf_size = count;
+	}
+
+	return 0;
+}
+
+static void synaptics_rmi4_i2c_check_addr(struct synaptics_rmi4_data *rmi4_data,
+		struct i2c_client *i2c)
+{
+	if (hw_if.board_data->ub_i2c_addr == -1)
+		return;
+
+	if (hw_if.board_data->i2c_addr == i2c->addr)
+		hw_if.board_data->i2c_addr = hw_if.board_data->ub_i2c_addr;
+	else
+		hw_if.board_data->i2c_addr = i2c->addr;
+}
+
+static int synaptics_rmi4_i2c_set_page(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr)
+{
+	int retval = 0;
+	unsigned char retry;
+	unsigned char buf[PAGE_SELECT_LEN];
+	unsigned char page;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+	struct i2c_msg msg[2];
+
+	msg[0].addr = hw_if.board_data->i2c_addr;
+	msg[0].flags = 0;
+	msg[0].len = PAGE_SELECT_LEN;
+	msg[0].buf = buf;
+
+	page = ((addr >> 8) & MASK_8BIT);
+	buf[0] = MASK_8BIT;
+	buf[1] = page;
+
+	if (page != rmi4_data->current_page) {
+		for (retry = 0; retry < SYN_I2C_RETRY_TIMES; retry++) {
+			if (i2c_transfer(i2c->adapter, &msg[0], 1) == 1) {
+				rmi4_data->current_page = page;
+				retval = PAGE_SELECT_LEN;
+				break;
+			}
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: I2C retry %d\n",
+					__func__, retry + 1);
+			msleep(20);
+
+			if (retry == SYN_I2C_RETRY_TIMES / 2) {
+				synaptics_rmi4_i2c_check_addr(rmi4_data, i2c);
+				msg[0].addr = hw_if.board_data->i2c_addr;
+			}
+		}
+	} else {
+		retval = PAGE_SELECT_LEN;
+	}
+
+	return retval;
+}
+
+static int synaptics_rmi4_i2c_read(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr, unsigned char *data, unsigned int length)
+{
+	int retval = 0;
+	unsigned char retry;
+	unsigned char buf;
+	unsigned char index = 0;
+	unsigned char xfer_msgs;
+	unsigned char remaining_msgs;
+	unsigned short i2c_addr;
+	unsigned short data_offset = 0;
+	unsigned int remaining_length = length;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+	struct i2c_adapter *adap = i2c->adapter;
+	struct i2c_msg msg[rd_msgs + 1];
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	retval = synaptics_rmi4_i2c_set_page(rmi4_data, addr);
+	if (retval != PAGE_SELECT_LEN) {
+		retval = -EIO;
+		goto exit;
+	}
+
+	msg[0].addr = hw_if.board_data->i2c_addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf;
+	msg[rd_msgs].addr = hw_if.board_data->i2c_addr;
+	msg[rd_msgs].flags = I2C_M_RD;
+	msg[rd_msgs].len = (unsigned short)remaining_length;
+	msg[rd_msgs].buf = &data[data_offset];
+
+	buf = addr & MASK_8BIT;
+
+	remaining_msgs = rd_msgs + 1;
+
+	while (remaining_msgs) {
+		xfer_msgs = remaining_msgs;
+		for (retry = 0; retry < SYN_I2C_RETRY_TIMES; retry++) {
+			retval = i2c_transfer(adap, &msg[index], xfer_msgs);
+			if (retval == xfer_msgs)
+				break;
+
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: I2C retry %d\n",
+					__func__, retry + 1);
+			msleep(20);
+
+			if (retry == SYN_I2C_RETRY_TIMES / 2) {
+				synaptics_rmi4_i2c_check_addr(rmi4_data, i2c);
+				i2c_addr = hw_if.board_data->i2c_addr;
+				msg[0].addr = i2c_addr;
+				msg[rd_msgs].addr = i2c_addr;
+			}
+		}
+
+		if (retry == SYN_I2C_RETRY_TIMES) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: I2C read over retry limit\n",
+					__func__);
+			retval = -EIO;
+			goto exit;
+		}
+
+		remaining_msgs -= xfer_msgs;
+		index += xfer_msgs;
+	}
+
+	retval = length;
+
+exit:
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	return retval;
+}
+
+static int synaptics_rmi4_i2c_write(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr, unsigned char *data, unsigned int length)
+{
+	int retval;
+	unsigned char retry;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+	struct i2c_msg msg[2];
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	retval = synaptics_rmi4_i2c_alloc_buf(rmi4_data, length + 1);
+	if (retval < 0)
+		goto exit;
+
+	retval = synaptics_rmi4_i2c_set_page(rmi4_data, addr);
+	if (retval != PAGE_SELECT_LEN) {
+		retval = -EIO;
+		goto exit;
+	}
+
+	msg[0].addr = hw_if.board_data->i2c_addr;
+	msg[0].flags = 0;
+	msg[0].len = (unsigned short)(length + 1);
+	msg[0].buf = wr_buf;
+
+	wr_buf[0] = addr & MASK_8BIT;
+	retval = secure_memcpy(&wr_buf[1], length, &data[0], length, length);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy data\n",
+				__func__);
+		goto exit;
+	}
+
+	for (retry = 0; retry < SYN_I2C_RETRY_TIMES; retry++) {
+		if (i2c_transfer(i2c->adapter, &msg[0], 1) == 1) {
+			retval = length;
+			break;
+		}
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: I2C retry %d\n",
+				__func__, retry + 1);
+		msleep(20);
+
+		if (retry == SYN_I2C_RETRY_TIMES / 2) {
+			synaptics_rmi4_i2c_check_addr(rmi4_data, i2c);
+			msg[0].addr = hw_if.board_data->i2c_addr;
+		}
+	}
+
+	if (retry == SYN_I2C_RETRY_TIMES) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: I2C write over retry limit\n",
+				__func__);
+		retval = -EIO;
+	}
+
+exit:
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	return retval;
+}
+
+#ifdef CONFIG_DRM
+static int check_dt(struct device_node *np)
+{
+	int i;
+	int count;
+	struct device_node *node;
+	struct drm_panel *panel;
+
+	count = of_count_phandle_with_args(np, "panel", NULL);
+	if (count <= 0)
+		return 0;
+
+	for (i = 0; i < count; i++) {
+		node = of_parse_phandle(np, "panel", i);
+		panel = of_drm_find_panel(node);
+		of_node_put(node);
+		if (!IS_ERR(panel)) {
+			active_panel = panel;
+			return 0;
+		}
+	}
+
+	return PTR_ERR(panel);
+}
+#endif
+
+static int check_default_tp(struct device_node *dt, const char *prop)
+{
+	const char *active_tp;
+	const char *compatible;
+	char *start;
+	int ret;
+
+	ret = of_property_read_string(dt->parent, prop, &active_tp);
+	if (ret) {
+		pr_err(" %s:fail to read %s %d\n", __func__, prop, ret);
+		return -ENODEV;
+	}
+
+	ret = of_property_read_string(dt, "compatible", &compatible);
+	if (ret < 0) {
+		pr_err(" %s:fail to read %s %d\n", __func__, "compatible", ret);
+		return -ENODEV;
+	}
+
+	start = strnstr(active_tp, compatible, strlen(active_tp));
+	if (start == NULL) {
+		pr_err(" %s:no match compatible, %s, %s\n",
+			__func__, compatible, active_tp);
+		ret = -ENODEV;
+	}
+
+	return ret;
+}
+
+static struct synaptics_dsx_bus_access bus_access = {
+	.type = BUS_I2C,
+	.read = synaptics_rmi4_i2c_read,
+	.write = synaptics_rmi4_i2c_write,
+};
+
+static void synaptics_rmi4_i2c_dev_release(struct device *dev)
+{
+	kfree(synaptics_dsx_i2c_device);
+}
+
+static int synaptics_rmi4_i2c_probe(struct i2c_client *client,
+		const struct i2c_device_id *dev_id)
+{
+	int retval;
+	struct device_node *dp = client->dev.of_node;
+
+#ifdef CONFIG_DRM
+	retval = check_dt(dp);
+	if (retval == -EPROBE_DEFER)
+		return retval;
+
+	if (retval) {
+		if (!check_default_tp(dp, "qcom,i2c-touch-active"))
+			retval = -EPROBE_DEFER;
+		else
+			retval = -ENODEV;
+
+		return retval;
+	}
+#endif
+
+	if (!i2c_check_functionality(client->adapter,
+			I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&client->dev,
+				"%s: SMBus byte data commands not supported by host\n",
+				__func__);
+		return -EIO;
+	}
+
+	synaptics_dsx_i2c_device = kzalloc(
+			sizeof(struct platform_device),
+			GFP_KERNEL);
+	if (!synaptics_dsx_i2c_device) {
+		dev_err(&client->dev,
+				"%s: Failed to allocate memory for synaptics_dsx_i2c_device\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+#ifdef CONFIG_OF
+	if (client->dev.of_node) {
+		hw_if.board_data = devm_kzalloc(&client->dev,
+				sizeof(struct synaptics_dsx_board_data),
+				GFP_KERNEL);
+		if (!hw_if.board_data) {
+			dev_err(&client->dev,
+					"%s: Failed to allocate memory for board data\n",
+					__func__);
+			return -ENOMEM;
+		}
+		hw_if.board_data->cap_button_map = devm_kzalloc(&client->dev,
+				sizeof(struct synaptics_dsx_button_map),
+				GFP_KERNEL);
+		if (!hw_if.board_data->cap_button_map) {
+			dev_err(&client->dev,
+					"%s: Failed to allocate memory for 0D button map\n",
+					__func__);
+			return -ENOMEM;
+		}
+		hw_if.board_data->vir_button_map = devm_kzalloc(&client->dev,
+				sizeof(struct synaptics_dsx_button_map),
+				GFP_KERNEL);
+		if (!hw_if.board_data->vir_button_map) {
+			dev_err(&client->dev,
+					"%s: Failed to allocate memory for virtual button map\n",
+					__func__);
+			return -ENOMEM;
+		}
+		parse_dt(&client->dev, hw_if.board_data);
+	}
+#else
+	hw_if.board_data = client->dev.platform_data;
+#endif
+
+	hw_if.bus_access = &bus_access;
+	hw_if.board_data->i2c_addr = client->addr;
+
+	synaptics_dsx_i2c_device->name = PLATFORM_DRIVER_NAME;
+	synaptics_dsx_i2c_device->id = 0;
+	synaptics_dsx_i2c_device->num_resources = 0;
+	synaptics_dsx_i2c_device->dev.parent = &client->dev;
+	synaptics_dsx_i2c_device->dev.platform_data = &hw_if;
+	synaptics_dsx_i2c_device->dev.release = synaptics_rmi4_i2c_dev_release;
+
+	retval = platform_device_register(synaptics_dsx_i2c_device);
+	if (retval) {
+		dev_err(&client->dev,
+				"%s: Failed to register platform device\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int synaptics_rmi4_i2c_remove(struct i2c_client *client)
+{
+	platform_device_unregister(synaptics_dsx_i2c_device);
+
+	return 0;
+}
+
+static const struct i2c_device_id synaptics_rmi4_id_table[] = {
+	{I2C_DRIVER_NAME, 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, synaptics_rmi4_id_table);
+
+#ifdef CONFIG_OF
+static const struct of_device_id synaptics_rmi4_of_match_table[] = {
+	{
+		.compatible = "synaptics,dsx-i2c",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, synaptics_rmi4_of_match_table);
+#else
+#define synaptics_rmi4_of_match_table NULL
+#endif
+
+static struct i2c_driver synaptics_rmi4_i2c_driver = {
+	.driver = {
+		.name = I2C_DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = synaptics_rmi4_of_match_table,
+	},
+	.probe = synaptics_rmi4_i2c_probe,
+	.remove = synaptics_rmi4_i2c_remove,
+	.id_table = synaptics_rmi4_id_table,
+};
+
+int synaptics_rmi4_bus_init(void)
+{
+	return i2c_add_driver(&synaptics_rmi4_i2c_driver);
+}
+EXPORT_SYMBOL(synaptics_rmi4_bus_init);
+
+void synaptics_rmi4_bus_exit(void)
+{
+	kfree(wr_buf);
+
+	i2c_del_driver(&synaptics_rmi4_i2c_driver);
+}
+EXPORT_SYMBOL(synaptics_rmi4_bus_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX I2C Bus Support Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_proximity.c

@@ -0,0 +1,673 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define PROX_PHYS_NAME "synaptics_dsx/proximity"
+
+#define HOVER_Z_MAX (255)
+
+#define HOVERING_FINGER_EN (1 << 4)
+
+static ssize_t synaptics_rmi4_hover_finger_en_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t synaptics_rmi4_hover_finger_en_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static struct device_attribute attrs[] = {
+	__ATTR(hover_finger_en, 0664,
+			synaptics_rmi4_hover_finger_en_show,
+			synaptics_rmi4_hover_finger_en_store),
+};
+
+struct synaptics_rmi4_f12_query_5 {
+	union {
+		struct {
+			unsigned char size_of_query6;
+			struct {
+				unsigned char ctrl0_is_present:1;
+				unsigned char ctrl1_is_present:1;
+				unsigned char ctrl2_is_present:1;
+				unsigned char ctrl3_is_present:1;
+				unsigned char ctrl4_is_present:1;
+				unsigned char ctrl5_is_present:1;
+				unsigned char ctrl6_is_present:1;
+				unsigned char ctrl7_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl8_is_present:1;
+				unsigned char ctrl9_is_present:1;
+				unsigned char ctrl10_is_present:1;
+				unsigned char ctrl11_is_present:1;
+				unsigned char ctrl12_is_present:1;
+				unsigned char ctrl13_is_present:1;
+				unsigned char ctrl14_is_present:1;
+				unsigned char ctrl15_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl16_is_present:1;
+				unsigned char ctrl17_is_present:1;
+				unsigned char ctrl18_is_present:1;
+				unsigned char ctrl19_is_present:1;
+				unsigned char ctrl20_is_present:1;
+				unsigned char ctrl21_is_present:1;
+				unsigned char ctrl22_is_present:1;
+				unsigned char ctrl23_is_present:1;
+			} __packed;
+		};
+		unsigned char data[4];
+	};
+};
+
+struct synaptics_rmi4_f12_query_8 {
+	union {
+		struct {
+			unsigned char size_of_query9;
+			struct {
+				unsigned char data0_is_present:1;
+				unsigned char data1_is_present:1;
+				unsigned char data2_is_present:1;
+				unsigned char data3_is_present:1;
+				unsigned char data4_is_present:1;
+				unsigned char data5_is_present:1;
+				unsigned char data6_is_present:1;
+				unsigned char data7_is_present:1;
+			} __packed;
+		};
+		unsigned char data[2];
+	};
+};
+
+struct prox_finger_data {
+	union {
+		struct {
+			unsigned char object_type_and_status;
+			unsigned char x_lsb;
+			unsigned char x_msb;
+			unsigned char y_lsb;
+			unsigned char y_msb;
+			unsigned char z;
+		} __packed;
+		unsigned char proximity_data[6];
+	};
+};
+
+struct synaptics_rmi4_prox_handle {
+	bool hover_finger_present;
+	bool hover_finger_en;
+	unsigned char intr_mask;
+	unsigned short query_base_addr;
+	unsigned short control_base_addr;
+	unsigned short data_base_addr;
+	unsigned short command_base_addr;
+	unsigned short hover_finger_en_addr;
+	unsigned short hover_finger_data_addr;
+	struct input_dev *prox_dev;
+	struct prox_finger_data *finger_data;
+	struct synaptics_rmi4_data *rmi4_data;
+};
+
+static struct synaptics_rmi4_prox_handle *prox;
+
+DECLARE_COMPLETION(prox_remove_complete);
+
+static void prox_hover_finger_lift(void)
+{
+	input_report_key(prox->prox_dev, BTN_TOUCH, 0);
+	input_report_key(prox->prox_dev, BTN_TOOL_FINGER, 0);
+	input_sync(prox->prox_dev);
+	prox->hover_finger_present = false;
+}
+
+static void prox_hover_finger_report(void)
+{
+	int retval;
+	int x;
+	int y;
+	int z;
+	struct prox_finger_data *data;
+	struct synaptics_rmi4_data *rmi4_data = prox->rmi4_data;
+
+	data = prox->finger_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			prox->hover_finger_data_addr,
+			data->proximity_data,
+			sizeof(data->proximity_data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read hovering finger data\n",
+				__func__);
+		return;
+	}
+
+	if (data->object_type_and_status != F12_HOVERING_FINGER_STATUS) {
+		if (prox->hover_finger_present)
+			prox_hover_finger_lift();
+
+		return;
+	}
+
+	x = (data->x_msb << 8) | (data->x_lsb);
+	y = (data->y_msb << 8) | (data->y_lsb);
+	z = HOVER_Z_MAX - data->z;
+
+	input_report_key(prox->prox_dev, BTN_TOUCH, 0);
+	input_report_key(prox->prox_dev, BTN_TOOL_FINGER, 1);
+	input_report_abs(prox->prox_dev, ABS_X, x);
+	input_report_abs(prox->prox_dev, ABS_Y, y);
+	input_report_abs(prox->prox_dev, ABS_DISTANCE, z);
+
+	input_sync(prox->prox_dev);
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: x = %d y = %d z = %d\n",
+			__func__, x, y, z);
+
+	prox->hover_finger_present = true;
+}
+
+static int prox_set_hover_finger_en(void)
+{
+	int retval;
+	unsigned char object_report_enable;
+	struct synaptics_rmi4_data *rmi4_data = prox->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			prox->hover_finger_en_addr,
+			&object_report_enable,
+			sizeof(object_report_enable));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read from object report enable register\n",
+				__func__);
+		return retval;
+	}
+
+	if (prox->hover_finger_en)
+		object_report_enable |= HOVERING_FINGER_EN;
+	else
+		object_report_enable &= ~HOVERING_FINGER_EN;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			prox->hover_finger_en_addr,
+			&object_report_enable,
+			sizeof(object_report_enable));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write to object report enable register\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static void prox_set_params(void)
+{
+	input_set_abs_params(prox->prox_dev, ABS_X, 0,
+			prox->rmi4_data->sensor_max_x, 0, 0);
+	input_set_abs_params(prox->prox_dev, ABS_Y, 0,
+			prox->rmi4_data->sensor_max_y, 0, 0);
+	input_set_abs_params(prox->prox_dev, ABS_DISTANCE, 0,
+			HOVER_Z_MAX, 0, 0);
+}
+
+static int prox_reg_init(void)
+{
+	int retval;
+	unsigned char ctrl_23_offset;
+	unsigned char data_1_offset;
+	struct synaptics_rmi4_f12_query_5 query_5;
+	struct synaptics_rmi4_f12_query_8 query_8;
+	struct synaptics_rmi4_data *rmi4_data = prox->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			prox->query_base_addr + 5,
+			query_5.data,
+			sizeof(query_5.data));
+	if (retval < 0)
+		return retval;
+
+	ctrl_23_offset = query_5.ctrl0_is_present +
+			query_5.ctrl1_is_present +
+			query_5.ctrl2_is_present +
+			query_5.ctrl3_is_present +
+			query_5.ctrl4_is_present +
+			query_5.ctrl5_is_present +
+			query_5.ctrl6_is_present +
+			query_5.ctrl7_is_present +
+			query_5.ctrl8_is_present +
+			query_5.ctrl9_is_present +
+			query_5.ctrl10_is_present +
+			query_5.ctrl11_is_present +
+			query_5.ctrl12_is_present +
+			query_5.ctrl13_is_present +
+			query_5.ctrl14_is_present +
+			query_5.ctrl15_is_present +
+			query_5.ctrl16_is_present +
+			query_5.ctrl17_is_present +
+			query_5.ctrl18_is_present +
+			query_5.ctrl19_is_present +
+			query_5.ctrl20_is_present +
+			query_5.ctrl21_is_present +
+			query_5.ctrl22_is_present;
+
+	prox->hover_finger_en_addr = prox->control_base_addr + ctrl_23_offset;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			prox->query_base_addr + 8,
+			query_8.data,
+			sizeof(query_8.data));
+	if (retval < 0)
+		return retval;
+
+	data_1_offset = query_8.data0_is_present;
+	prox->hover_finger_data_addr = prox->data_base_addr + data_1_offset;
+
+	return retval;
+}
+
+static int prox_scan_pdt(void)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char page;
+	unsigned char intr_count = 0;
+	unsigned char intr_off;
+	unsigned char intr_src;
+	unsigned short addr;
+	struct synaptics_rmi4_fn_desc fd;
+	struct synaptics_rmi4_data *rmi4_data = prox->rmi4_data;
+
+	for (page = 0; page < PAGES_TO_SERVICE; page++) {
+		for (addr = PDT_START; addr > PDT_END; addr -= PDT_ENTRY_SIZE) {
+			addr |= (page << 8);
+
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					addr,
+					(unsigned char *)&fd,
+					sizeof(fd));
+			if (retval < 0)
+				return retval;
+
+			addr &= ~(MASK_8BIT << 8);
+
+			if (fd.fn_number) {
+				dev_dbg(rmi4_data->pdev->dev.parent,
+						"%s: Found F%02x\n",
+						__func__, fd.fn_number);
+				switch (fd.fn_number) {
+				case SYNAPTICS_RMI4_F12:
+					goto f12_found;
+					break;
+				}
+			} else {
+				break;
+			}
+
+			intr_count += fd.intr_src_count;
+		}
+	}
+
+	dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Failed to find F12\n",
+			__func__);
+	return -EINVAL;
+
+f12_found:
+	prox->query_base_addr = fd.query_base_addr | (page << 8);
+	prox->control_base_addr = fd.ctrl_base_addr | (page << 8);
+	prox->data_base_addr = fd.data_base_addr | (page << 8);
+	prox->command_base_addr = fd.cmd_base_addr | (page << 8);
+
+	retval = prox_reg_init();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to initialize proximity registers\n",
+				__func__);
+		return retval;
+	}
+
+	prox->intr_mask = 0;
+	intr_src = fd.intr_src_count;
+	intr_off = intr_count % 8;
+	for (ii = intr_off;
+			ii < (intr_src + intr_off);
+			ii++) {
+		prox->intr_mask |= 1 << ii;
+	}
+
+	rmi4_data->intr_mask[0] |= prox->intr_mask;
+
+	addr = rmi4_data->f01_ctrl_base_addr + 1;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			addr,
+			&(rmi4_data->intr_mask[0]),
+			sizeof(rmi4_data->intr_mask[0]));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set interrupt enable bit\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static ssize_t synaptics_rmi4_hover_finger_en_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	if (!prox)
+		return -ENODEV;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			prox->hover_finger_en);
+}
+
+static ssize_t synaptics_rmi4_hover_finger_en_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = prox->rmi4_data;
+
+	if (!prox)
+		return -ENODEV;
+
+	if (kstrtouint(buf, 16, &input) != 1)
+		return -EINVAL;
+
+	if (input == 1)
+		prox->hover_finger_en = true;
+	else if (input == 0)
+		prox->hover_finger_en = false;
+	else
+		return -EINVAL;
+
+	retval = prox_set_hover_finger_en();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to change hovering finger enable setting\n",
+				__func__);
+		return retval;
+	}
+
+	return count;
+}
+
+int synaptics_rmi4_prox_hover_finger_en(bool enable)
+{
+	int retval;
+
+	if (!prox)
+		return -ENODEV;
+
+	prox->hover_finger_en = enable;
+
+	retval = prox_set_hover_finger_en();
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+EXPORT_SYMBOL(synaptics_rmi4_prox_hover_finger_en);
+
+static void synaptics_rmi4_prox_attn(struct synaptics_rmi4_data *rmi4_data,
+		unsigned char intr_mask)
+{
+	if (!prox)
+		return;
+
+	if (prox->intr_mask & intr_mask)
+		prox_hover_finger_report();
+}
+
+static int synaptics_rmi4_prox_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char attr_count;
+
+	if (prox) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Handle already exists\n",
+				__func__);
+		return 0;
+	}
+
+	prox = kzalloc(sizeof(*prox), GFP_KERNEL);
+	if (!prox) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for prox\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	prox->finger_data = kzalloc(sizeof(*(prox->finger_data)), GFP_KERNEL);
+	if (!prox->finger_data) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for finger_data\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_prox;
+	}
+
+	prox->rmi4_data = rmi4_data;
+
+	retval = prox_scan_pdt();
+	if (retval < 0)
+		goto exit_free_finger_data;
+
+	prox->hover_finger_en = true;
+
+	retval = prox_set_hover_finger_en();
+	if (retval < 0)
+		return retval;
+
+	prox->prox_dev = input_allocate_device();
+	if (prox->prox_dev == NULL) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to allocate proximity device\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit_free_finger_data;
+	}
+
+	prox->prox_dev->name = PROXIMITY_DRIVER_NAME;
+	prox->prox_dev->phys = PROX_PHYS_NAME;
+	prox->prox_dev->id.product = SYNAPTICS_DSX_DRIVER_PRODUCT;
+	prox->prox_dev->id.version = SYNAPTICS_DSX_DRIVER_VERSION;
+	prox->prox_dev->dev.parent = rmi4_data->pdev->dev.parent;
+	input_set_drvdata(prox->prox_dev, rmi4_data);
+
+	set_bit(EV_KEY, prox->prox_dev->evbit);
+	set_bit(EV_ABS, prox->prox_dev->evbit);
+	set_bit(BTN_TOUCH, prox->prox_dev->keybit);
+	set_bit(BTN_TOOL_FINGER, prox->prox_dev->keybit);
+#ifdef INPUT_PROP_DIRECT
+	set_bit(INPUT_PROP_DIRECT, prox->prox_dev->propbit);
+#endif
+
+	prox_set_params();
+
+	retval = input_register_device(prox->prox_dev);
+	if (retval) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to register proximity device\n",
+				__func__);
+		goto exit_free_input_device;
+	}
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		retval = sysfs_create_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create sysfs attributes\n",
+					__func__);
+			goto exit_free_sysfs;
+		}
+	}
+
+	return 0;
+
+exit_free_sysfs:
+	for (attr_count--; attr_count >= 0; attr_count--) {
+		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+	input_unregister_device(prox->prox_dev);
+	prox->prox_dev = NULL;
+
+exit_free_input_device:
+	if (prox->prox_dev)
+		input_free_device(prox->prox_dev);
+
+exit_free_finger_data:
+	kfree(prox->finger_data);
+
+exit_free_prox:
+	kfree(prox);
+	prox = NULL;
+
+exit:
+	return retval;
+}
+
+static void synaptics_rmi4_prox_remove(struct synaptics_rmi4_data *rmi4_data)
+{
+	unsigned char attr_count;
+
+	if (!prox)
+		goto exit;
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		sysfs_remove_file(&rmi4_data->input_dev->dev.kobj,
+				&attrs[attr_count].attr);
+	}
+
+	input_unregister_device(prox->prox_dev);
+	kfree(prox->finger_data);
+	kfree(prox);
+	prox = NULL;
+
+exit:
+	complete(&prox_remove_complete);
+}
+
+static void synaptics_rmi4_prox_reset(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!prox) {
+		synaptics_rmi4_prox_init(rmi4_data);
+		return;
+	}
+
+	prox_hover_finger_lift();
+
+	prox_scan_pdt();
+
+	prox_set_hover_finger_en();
+}
+
+static void synaptics_rmi4_prox_reinit(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!prox)
+		return;
+
+	prox_hover_finger_lift();
+
+	prox_set_hover_finger_en();
+}
+
+static void synaptics_rmi4_prox_e_suspend(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!prox)
+		return;
+
+	prox_hover_finger_lift();
+}
+
+static void synaptics_rmi4_prox_suspend(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!prox)
+		return;
+
+	prox_hover_finger_lift();
+}
+
+static struct synaptics_rmi4_exp_fn proximity_module = {
+	.fn_type = RMI_PROXIMITY,
+	.init = synaptics_rmi4_prox_init,
+	.remove = synaptics_rmi4_prox_remove,
+	.reset = synaptics_rmi4_prox_reset,
+	.reinit = synaptics_rmi4_prox_reinit,
+	.early_suspend = synaptics_rmi4_prox_e_suspend,
+	.suspend = synaptics_rmi4_prox_suspend,
+	.resume = NULL,
+	.late_resume = NULL,
+	.attn = synaptics_rmi4_prox_attn,
+};
+
+static int __init rmi4_proximity_module_init(void)
+{
+	synaptics_rmi4_new_function(&proximity_module, true);
+
+	return 0;
+}
+
+static void __exit rmi4_proximity_module_exit(void)
+{
+	synaptics_rmi4_new_function(&proximity_module, false);
+
+	wait_for_completion(&prox_remove_complete);
+}
+
+module_init(rmi4_proximity_module_init);
+module_exit(rmi4_proximity_module_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX Proximity Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_rmi_dev.c

@@ -0,0 +1,1075 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/signal.h>
+#include <linux/sched.h>
+#include <linux/gpio.h>
+#include <linux/uaccess.h>
+#include <linux/cdev.h>
+#include <linux/sched/signal.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define CHAR_DEVICE_NAME "rmi"
+#define DEVICE_CLASS_NAME "rmidev"
+#define SYSFS_FOLDER_NAME "rmidev"
+#define DEV_NUMBER 1
+#define REG_ADDR_LIMIT 0xFFFF
+
+#define RMIDEV_MAJOR_NUM 0
+
+static ssize_t rmidev_sysfs_data_show(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static ssize_t rmidev_sysfs_data_store(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static ssize_t rmidev_sysfs_open_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t rmidev_sysfs_release_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t rmidev_sysfs_attn_state_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t rmidev_sysfs_pid_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t rmidev_sysfs_pid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t rmidev_sysfs_term_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t rmidev_sysfs_intr_mask_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t rmidev_sysfs_intr_mask_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t rmidev_sysfs_concurrent_show(struct device *dev,
+		struct device_attribute *attr, char *buf);
+
+static ssize_t rmidev_sysfs_concurrent_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+struct rmidev_handle {
+	dev_t dev_no;
+	pid_t pid;
+	unsigned char intr_mask;
+	unsigned char *tmpbuf;
+	unsigned int tmpbuf_size;
+	struct device dev;
+	struct synaptics_rmi4_data *rmi4_data;
+	struct kobject *sysfs_dir;
+	struct kernel_siginfo interrupt_signal;
+	struct kernel_siginfo terminate_signal;
+	struct task_struct *task;
+	void *data;
+	bool concurrent;
+};
+
+struct rmidev_data {
+	int ref_count;
+	struct cdev main_dev;
+	struct class *device_class;
+	struct mutex file_mutex;
+	struct rmidev_handle *rmi_dev;
+};
+
+static struct bin_attribute attr_data = {
+	.attr = {
+		.name = "data",
+		.mode = 0664,
+	},
+	.size = 0,
+	.read = rmidev_sysfs_data_show,
+	.write = rmidev_sysfs_data_store,
+};
+
+static struct device_attribute attrs[] = {
+	__ATTR(open, 0220,
+			synaptics_rmi4_show_error,
+			rmidev_sysfs_open_store),
+	__ATTR(release, 0220,
+			synaptics_rmi4_show_error,
+			rmidev_sysfs_release_store),
+	__ATTR(attn_state, 0444,
+			rmidev_sysfs_attn_state_show,
+			synaptics_rmi4_store_error),
+	__ATTR(pid, 0664,
+			rmidev_sysfs_pid_show,
+			rmidev_sysfs_pid_store),
+	__ATTR(term, 0220,
+			synaptics_rmi4_show_error,
+			rmidev_sysfs_term_store),
+	__ATTR(intr_mask, 0664,
+			rmidev_sysfs_intr_mask_show,
+			rmidev_sysfs_intr_mask_store),
+	__ATTR(concurrent, 0664,
+			rmidev_sysfs_concurrent_show,
+			rmidev_sysfs_concurrent_store),
+};
+
+static int rmidev_major_num = RMIDEV_MAJOR_NUM;
+
+static struct class *rmidev_device_class;
+
+static struct rmidev_handle *rmidev;
+
+DECLARE_COMPLETION(rmidev_remove_complete);
+
+static irqreturn_t rmidev_sysfs_irq(int irq, void *data)
+{
+	struct synaptics_rmi4_data *rmi4_data = data;
+
+	sysfs_notify(&rmi4_data->input_dev->dev.kobj,
+			SYSFS_FOLDER_NAME, "attn_state");
+
+	return IRQ_HANDLED;
+}
+
+static int rmidev_sysfs_irq_enable(struct synaptics_rmi4_data *rmi4_data,
+		bool enable)
+{
+	int retval = 0;
+	unsigned char intr_status[MAX_INTR_REGISTERS];
+	unsigned long irq_flags = IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING |
+			IRQF_ONESHOT;
+
+	mutex_lock(&(rmi4_data->rmi4_irq_enable_mutex));
+
+	if (enable) {
+		if (rmi4_data->irq_enabled) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Interrupt already enabled\n",
+					__func__);
+			goto exit;
+		}
+
+		/* Clear interrupts first */
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				rmi4_data->f01_data_base_addr + 1,
+				intr_status,
+				rmi4_data->num_of_intr_regs);
+		if (retval < 0)
+			goto exit;
+
+		retval = request_threaded_irq(rmi4_data->irq, NULL,
+				rmidev_sysfs_irq, irq_flags,
+				PLATFORM_DRIVER_NAME, rmi4_data);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create irq thread\n",
+					__func__);
+			goto exit;
+		}
+
+		rmi4_data->irq_enabled = true;
+	} else {
+		if (rmi4_data->irq_enabled) {
+			disable_irq(rmi4_data->irq);
+			free_irq(rmi4_data->irq, rmi4_data);
+			rmi4_data->irq_enabled = false;
+		}
+	}
+
+exit:
+	mutex_unlock(&(rmi4_data->rmi4_irq_enable_mutex));
+
+	return retval;
+}
+
+static ssize_t rmidev_sysfs_data_show(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	unsigned char intr_status = 0;
+	unsigned int length = (unsigned int)count;
+	unsigned short address = (unsigned short)pos;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	if (length > (REG_ADDR_LIMIT - address)) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Out of register map limit\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (length) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				address,
+				(unsigned char *)buf,
+				length);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read data\n",
+					__func__);
+			return retval;
+		}
+	} else {
+		return -EINVAL;
+	}
+
+	if (!rmidev->concurrent)
+		goto exit;
+
+	if (address != rmi4_data->f01_data_base_addr)
+		goto exit;
+
+	if (length <= 1)
+		goto exit;
+
+	intr_status = buf[1];
+
+	if (!list_empty(&rmi->support_fn_list)) {
+		list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+			if (fhandler->num_of_data_sources) {
+				if (fhandler->intr_mask & intr_status) {
+					rmi4_data->report_touch(rmi4_data,
+							fhandler);
+				}
+			}
+		}
+	}
+
+exit:
+	return length;
+}
+
+static ssize_t rmidev_sysfs_data_store(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	unsigned int length = (unsigned int)count;
+	unsigned short address = (unsigned short)pos;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	if (length > (REG_ADDR_LIMIT - address)) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Out of register map limit\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (length) {
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				address,
+				(unsigned char *)buf,
+				length);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to write data\n",
+					__func__);
+			return retval;
+		}
+	} else {
+		return -EINVAL;
+	}
+
+	return length;
+}
+
+static ssize_t rmidev_sysfs_open_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input != 1)
+		return -EINVAL;
+
+	if (rmi4_data->sensor_sleep) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Sensor sleeping\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	rmi4_data->stay_awake = true;
+
+	rmi4_data->irq_enable(rmi4_data, false, false);
+	rmidev_sysfs_irq_enable(rmi4_data, true);
+
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Attention interrupt disabled\n",
+			__func__);
+
+	return count;
+}
+
+static ssize_t rmidev_sysfs_release_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input != 1)
+		return -EINVAL;
+
+	rmidev_sysfs_irq_enable(rmi4_data, false);
+
+	rmi4_data->reset_device(rmi4_data, false);
+
+	rmi4_data->stay_awake = false;
+
+	return count;
+}
+
+static ssize_t rmidev_sysfs_attn_state_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int attn_state;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	attn_state = gpio_get_value(bdata->irq_gpio);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", attn_state);
+}
+
+static ssize_t rmidev_sysfs_pid_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", rmidev->pid);
+}
+
+static ssize_t rmidev_sysfs_pid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	rmidev->pid = input;
+
+	if (rmidev->pid) {
+		rmidev->task = pid_task(find_vpid(rmidev->pid), PIDTYPE_PID);
+		if (!rmidev->task) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to locate PID of data logging tool\n",
+					__func__);
+			return -EINVAL;
+		}
+	}
+
+	return count;
+}
+
+static ssize_t rmidev_sysfs_term_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	if (input != 1)
+		return -EINVAL;
+
+	if (rmidev->pid)
+		send_sig_info(SIGTERM, &rmidev->terminate_signal, rmidev->task);
+
+	return count;
+}
+
+static ssize_t rmidev_sysfs_intr_mask_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "0x%02x\n", rmidev->intr_mask);
+}
+
+static ssize_t rmidev_sysfs_intr_mask_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	rmidev->intr_mask = (unsigned char)input;
+
+	return count;
+}
+
+static ssize_t rmidev_sysfs_concurrent_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", rmidev->concurrent);
+}
+
+static ssize_t rmidev_sysfs_concurrent_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+
+	if (kstrtouint(buf, 10, &input) != 1)
+		return -EINVAL;
+
+	rmidev->concurrent = input > 0 ? true : false;
+
+	return count;
+}
+
+static int rmidev_allocate_buffer(int count)
+{
+	if (count + 1 > rmidev->tmpbuf_size) {
+		if (rmidev->tmpbuf_size)
+			kfree(rmidev->tmpbuf);
+		rmidev->tmpbuf = kzalloc(count + 1, GFP_KERNEL);
+		if (!rmidev->tmpbuf) {
+			dev_err(rmidev->rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for buffer\n",
+					__func__);
+			rmidev->tmpbuf_size = 0;
+			return -ENOMEM;
+		}
+		rmidev->tmpbuf_size = count + 1;
+	}
+
+	return 0;
+}
+
+/*
+ * rmidev_llseek - set register address to access for RMI device
+ *
+ * @filp: pointer to file structure
+ * @off:
+ *	if whence == SEEK_SET,
+ *		off: 16-bit RMI register address
+ *	if whence == SEEK_CUR,
+ *		off: offset from current position
+ *	if whence == SEEK_END,
+ *		off: offset from end position (0xFFFF)
+ * @whence: SEEK_SET, SEEK_CUR, or SEEK_END
+ */
+static loff_t rmidev_llseek(struct file *filp, loff_t off, int whence)
+{
+	loff_t newpos;
+	struct rmidev_data *dev_data = filp->private_data;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	if (IS_ERR(dev_data)) {
+		pr_err("%s: Pointer of char device data is invalid", __func__);
+		return -EBADF;
+	}
+
+	mutex_lock(&(dev_data->file_mutex));
+
+	switch (whence) {
+	case SEEK_SET:
+		newpos = off;
+		break;
+	case SEEK_CUR:
+		newpos = filp->f_pos + off;
+		break;
+	case SEEK_END:
+		newpos = REG_ADDR_LIMIT + off;
+		break;
+	default:
+		newpos = -EINVAL;
+		goto clean_up;
+	}
+
+	if (newpos < 0 || newpos > REG_ADDR_LIMIT) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: New position 0x%04x is invalid\n",
+				__func__, (unsigned int)newpos);
+		newpos = -EINVAL;
+		goto clean_up;
+	}
+
+	filp->f_pos = newpos;
+
+clean_up:
+	mutex_unlock(&(dev_data->file_mutex));
+
+	return newpos;
+}
+
+/*
+ * rmidev_read: read register data from RMI device
+ *
+ * @filp: pointer to file structure
+ * @buf: pointer to user space buffer
+ * @count: number of bytes to read
+ * @f_pos: starting RMI register address
+ */
+static ssize_t rmidev_read(struct file *filp, char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	ssize_t retval;
+	unsigned char intr_status = 0;
+	unsigned short address;
+	struct rmidev_data *dev_data = filp->private_data;
+	struct synaptics_rmi4_fn *fhandler;
+	struct synaptics_rmi4_device_info *rmi;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	rmi = &(rmi4_data->rmi4_mod_info);
+
+	if (IS_ERR(dev_data)) {
+		pr_err("%s: Pointer of char device data is invalid", __func__);
+		return -EBADF;
+	}
+
+	mutex_lock(&(dev_data->file_mutex));
+
+	if (*f_pos > REG_ADDR_LIMIT) {
+		retval = -EFAULT;
+		goto clean_up;
+	}
+
+	if (count > (REG_ADDR_LIMIT - *f_pos))
+		count = REG_ADDR_LIMIT - *f_pos;
+
+	if (count == 0) {
+		retval = 0;
+		goto clean_up;
+	}
+	address = (unsigned short)(*f_pos);
+
+	rmidev_allocate_buffer(count);
+
+	retval = synaptics_rmi4_reg_read(rmidev->rmi4_data,
+			*f_pos,
+			rmidev->tmpbuf,
+			count);
+	if (retval < 0)
+		goto clean_up;
+
+	if (copy_to_user(buf, rmidev->tmpbuf, count))
+		retval = -EFAULT;
+	else
+		*f_pos += retval;
+
+	if (!rmidev->concurrent)
+		goto clean_up;
+
+	if (address != rmi4_data->f01_data_base_addr)
+		goto clean_up;
+
+	if (count <= 1)
+		goto clean_up;
+
+	intr_status = rmidev->tmpbuf[1];
+
+	if (!list_empty(&rmi->support_fn_list)) {
+		list_for_each_entry(fhandler, &rmi->support_fn_list, link) {
+			if (fhandler->num_of_data_sources) {
+				if (fhandler->intr_mask & intr_status) {
+					rmi4_data->report_touch(rmi4_data,
+							fhandler);
+				}
+			}
+		}
+	}
+
+clean_up:
+	mutex_unlock(&(dev_data->file_mutex));
+
+	return retval;
+}
+
+/*
+ * rmidev_write: write register data to RMI device
+ *
+ * @filp: pointer to file structure
+ * @buf: pointer to user space buffer
+ * @count: number of bytes to write
+ * @f_pos: starting RMI register address
+ */
+static ssize_t rmidev_write(struct file *filp, const char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	ssize_t retval;
+	struct rmidev_data *dev_data = filp->private_data;
+
+	if (IS_ERR(dev_data)) {
+		pr_err("%s: Pointer of char device data is invalid", __func__);
+		return -EBADF;
+	}
+
+	mutex_lock(&(dev_data->file_mutex));
+
+	if (*f_pos > REG_ADDR_LIMIT) {
+		retval = -EFAULT;
+		goto unlock;
+	}
+
+	if (count > (REG_ADDR_LIMIT - *f_pos))
+		count = REG_ADDR_LIMIT - *f_pos;
+
+	if (count == 0) {
+		retval = 0;
+		goto unlock;
+	}
+	rmidev_allocate_buffer(count);
+
+	if (copy_from_user(rmidev->tmpbuf, buf, count)) {
+		retval = -EFAULT;
+		goto unlock;
+	}
+
+	retval = synaptics_rmi4_reg_write(rmidev->rmi4_data,
+			*f_pos,
+			rmidev->tmpbuf,
+			count);
+	if (retval >= 0)
+		*f_pos += retval;
+
+unlock:
+	mutex_unlock(&(dev_data->file_mutex));
+
+	return retval;
+}
+
+static int rmidev_open(struct inode *inp, struct file *filp)
+{
+	int retval = 0;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+	struct rmidev_data *dev_data =
+			container_of(inp->i_cdev, struct rmidev_data, main_dev);
+
+	if (!dev_data)
+		return -EACCES;
+
+	if (rmi4_data->sensor_sleep) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Sensor sleeping\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	rmi4_data->stay_awake = true;
+
+	filp->private_data = dev_data;
+
+	mutex_lock(&(dev_data->file_mutex));
+
+	rmi4_data->irq_enable(rmi4_data, false, false);
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Attention interrupt disabled\n",
+			__func__);
+
+	if (dev_data->ref_count < 1)
+		dev_data->ref_count++;
+	else
+		retval = -EACCES;
+
+	mutex_unlock(&(dev_data->file_mutex));
+
+	return retval;
+}
+
+static int rmidev_release(struct inode *inp, struct file *filp)
+{
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+	struct rmidev_data *dev_data =
+			container_of(inp->i_cdev, struct rmidev_data, main_dev);
+
+	if (!dev_data)
+		return -EACCES;
+
+	mutex_lock(&(dev_data->file_mutex));
+
+	dev_data->ref_count--;
+	if (dev_data->ref_count < 0)
+		dev_data->ref_count = 0;
+
+	rmi4_data->reset_device(rmi4_data, false);
+
+	rmi4_data->stay_awake = false;
+
+	mutex_unlock(&(dev_data->file_mutex));
+
+	return 0;
+}
+
+static const struct file_operations rmidev_fops = {
+	.owner = THIS_MODULE,
+	.llseek = rmidev_llseek,
+	.read = rmidev_read,
+	.write = rmidev_write,
+	.open = rmidev_open,
+	.release = rmidev_release,
+};
+
+static void rmidev_device_cleanup(struct rmidev_data *dev_data)
+{
+	dev_t devno;
+	struct synaptics_rmi4_data *rmi4_data = rmidev->rmi4_data;
+
+	if (dev_data) {
+		devno = dev_data->main_dev.dev;
+
+		if (dev_data->device_class)
+			device_destroy(dev_data->device_class, devno);
+
+		cdev_del(&dev_data->main_dev);
+
+		unregister_chrdev_region(devno, 1);
+
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: rmidev device removed\n",
+				__func__);
+	}
+}
+
+static char *rmi_char_devnode(struct device *dev, umode_t *mode)
+{
+	if (!mode)
+		return NULL;
+
+	*mode = (S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
+
+	return kasprintf(GFP_KERNEL, "rmi/%s", dev_name(dev));
+}
+
+static int rmidev_create_device_class(void)
+{
+	if (rmidev_device_class != NULL)
+		return 0;
+
+	rmidev_device_class = class_create(THIS_MODULE, DEVICE_CLASS_NAME);
+
+	if (IS_ERR(rmidev_device_class)) {
+		pr_err("%s: Failed to create /dev/%s\n",
+				__func__, CHAR_DEVICE_NAME);
+		return -ENODEV;
+	}
+
+	rmidev_device_class->devnode = rmi_char_devnode;
+
+	return 0;
+}
+
+static void rmidev_attn(struct synaptics_rmi4_data *rmi4_data,
+		unsigned char intr_mask)
+{
+	if (!rmidev)
+		return;
+
+	if (rmidev->pid && (rmidev->intr_mask & intr_mask))
+		send_sig_info(SIGIO, &rmidev->interrupt_signal, rmidev->task);
+
+	return;
+}
+
+static int rmidev_init_device(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	dev_t dev_no;
+	unsigned char attr_count;
+	struct rmidev_data *dev_data;
+	struct device *device_ptr;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	if (rmidev) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Handle already exists\n",
+				__func__);
+		return 0;
+	}
+
+	rmidev = kzalloc(sizeof(*rmidev), GFP_KERNEL);
+	if (!rmidev) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for rmidev\n",
+				__func__);
+		retval = -ENOMEM;
+		goto err_rmidev;
+	}
+
+	rmidev->rmi4_data = rmi4_data;
+
+	memset(&rmidev->interrupt_signal, 0, sizeof(rmidev->interrupt_signal));
+	rmidev->interrupt_signal.si_signo = SIGIO;
+	rmidev->interrupt_signal.si_code = SI_USER;
+
+	memset(&rmidev->terminate_signal, 0, sizeof(rmidev->terminate_signal));
+	rmidev->terminate_signal.si_signo = SIGTERM;
+	rmidev->terminate_signal.si_code = SI_USER;
+
+	retval = rmidev_create_device_class();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create device class\n",
+				__func__);
+		goto err_device_class;
+	}
+
+	if (rmidev_major_num) {
+		dev_no = MKDEV(rmidev_major_num, DEV_NUMBER);
+		retval = register_chrdev_region(dev_no, 1, CHAR_DEVICE_NAME);
+	} else {
+		retval = alloc_chrdev_region(&dev_no, 0, 1, CHAR_DEVICE_NAME);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to allocate char device region\n",
+					__func__);
+			goto err_device_region;
+		}
+
+		rmidev_major_num = MAJOR(dev_no);
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Major number of rmidev = %d\n",
+				__func__, rmidev_major_num);
+	}
+
+	dev_data = kzalloc(sizeof(*dev_data), GFP_KERNEL);
+	if (!dev_data) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for dev_data\n",
+				__func__);
+		retval = -ENOMEM;
+		goto err_dev_data;
+	}
+
+	mutex_init(&dev_data->file_mutex);
+	dev_data->rmi_dev = rmidev;
+	rmidev->data = dev_data;
+
+	cdev_init(&dev_data->main_dev, &rmidev_fops);
+
+	retval = cdev_add(&dev_data->main_dev, dev_no, 1);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to add rmi char device\n",
+				__func__);
+		goto err_char_device;
+	}
+
+	dev_set_name(&rmidev->dev, "rmidev%d", MINOR(dev_no));
+	dev_data->device_class = rmidev_device_class;
+
+	device_ptr = device_create(dev_data->device_class, NULL, dev_no,
+			NULL, CHAR_DEVICE_NAME"%d", MINOR(dev_no));
+	if (IS_ERR(device_ptr)) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create rmi char device\n",
+				__func__);
+		retval = -ENODEV;
+		goto err_char_device;
+	}
+
+	retval = gpio_export(bdata->irq_gpio, false);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to export attention gpio\n",
+				__func__);
+	} else {
+		retval = gpio_export_link(&(rmi4_data->input_dev->dev),
+				"attn", bdata->irq_gpio);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s Failed to create gpio symlink\n",
+					__func__);
+		} else {
+			dev_dbg(rmi4_data->pdev->dev.parent,
+					"%s: Exported attention gpio %d\n",
+					__func__, bdata->irq_gpio);
+		}
+	}
+
+	rmidev->sysfs_dir = kobject_create_and_add(SYSFS_FOLDER_NAME,
+			&rmi4_data->input_dev->dev.kobj);
+	if (!rmidev->sysfs_dir) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs directory\n",
+				__func__);
+		retval = -ENODEV;
+		goto err_sysfs_dir;
+	}
+
+	retval = sysfs_create_bin_file(rmidev->sysfs_dir,
+			&attr_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs bin file\n",
+				__func__);
+		goto err_sysfs_bin;
+	}
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		retval = sysfs_create_file(rmidev->sysfs_dir,
+				&attrs[attr_count].attr);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create sysfs attributes\n",
+					__func__);
+			retval = -ENODEV;
+			goto err_sysfs_attrs;
+		}
+	}
+
+	return 0;
+
+err_sysfs_attrs:
+	for (attr_count--; attr_count >= 0; attr_count--)
+		sysfs_remove_file(rmidev->sysfs_dir, &attrs[attr_count].attr);
+
+	sysfs_remove_bin_file(rmidev->sysfs_dir, &attr_data);
+
+err_sysfs_bin:
+	kobject_put(rmidev->sysfs_dir);
+
+err_sysfs_dir:
+	sysfs_remove_link(&(rmi4_data->input_dev->dev.kobj), "attn");
+	gpio_unexport(bdata->irq_gpio);
+
+err_char_device:
+	rmidev_device_cleanup(dev_data);
+	kfree(dev_data);
+
+err_dev_data:
+	unregister_chrdev_region(dev_no, 1);
+
+err_device_region:
+	if (rmidev_device_class != NULL) {
+		class_destroy(rmidev_device_class);
+		rmidev_device_class = NULL;
+	}
+
+err_device_class:
+	kfree(rmidev);
+	rmidev = NULL;
+
+err_rmidev:
+	return retval;
+}
+
+static void rmidev_remove_device(struct synaptics_rmi4_data *rmi4_data)
+{
+	unsigned char attr_count;
+	struct rmidev_data *dev_data;
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	if (!rmidev)
+		goto exit;
+
+	rmidev_major_num = RMIDEV_MAJOR_NUM;
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++)
+		sysfs_remove_file(rmidev->sysfs_dir, &attrs[attr_count].attr);
+
+	sysfs_remove_bin_file(rmidev->sysfs_dir, &attr_data);
+
+	kobject_put(rmidev->sysfs_dir);
+
+	sysfs_remove_link(&(rmi4_data->input_dev->dev.kobj), "attn");
+	gpio_unexport(bdata->irq_gpio);
+
+	dev_data = rmidev->data;
+	if (dev_data) {
+		rmidev_device_cleanup(dev_data);
+		kfree(dev_data);
+	}
+
+	unregister_chrdev_region(rmidev->dev_no, 1);
+
+	if (rmidev_device_class != NULL) {
+		class_destroy(rmidev_device_class);
+		rmidev_device_class = NULL;
+	}
+
+	kfree(rmidev->tmpbuf);
+
+	kfree(rmidev);
+	rmidev = NULL;
+
+exit:
+	complete(&rmidev_remove_complete);
+}
+
+static struct synaptics_rmi4_exp_fn rmidev_module = {
+	.fn_type = RMI_DEV,
+	.init = rmidev_init_device,
+	.remove = rmidev_remove_device,
+	.reset = NULL,
+	.reinit = NULL,
+	.early_suspend = NULL,
+	.suspend = NULL,
+	.resume = NULL,
+	.late_resume = NULL,
+	.attn = rmidev_attn,
+};
+
+static int __init rmidev_module_init(void)
+{
+	synaptics_rmi4_new_function(&rmidev_module, true);
+
+	return 0;
+}
+
+static void __exit rmidev_module_exit(void)
+{
+	synaptics_rmi4_new_function(&rmidev_module, false);
+
+	wait_for_completion(&rmidev_remove_complete);
+}
+
+module_init(rmidev_module_init);
+module_exit(rmidev_module_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX RMI Dev Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_rmi_hid_i2c.c

@@ -0,0 +1,989 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/gpio.h>
+#include <linux/types.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define SYN_I2C_RETRY_TIMES 10
+
+#define REPORT_ID_GET_BLOB 0x07
+#define REPORT_ID_WRITE 0x09
+#define REPORT_ID_READ_ADDRESS 0x0a
+#define REPORT_ID_READ_DATA 0x0b
+#define REPORT_ID_SET_RMI_MODE 0x0f
+
+#define PREFIX_USAGE_PAGE_1BYTE 0x05
+#define PREFIX_USAGE_PAGE_2BYTES 0x06
+#define PREFIX_USAGE 0x09
+#define PREFIX_REPORT_ID 0x85
+#define PREFIX_REPORT_COUNT_1BYTE 0x95
+#define PREFIX_REPORT_COUNT_2BYTES 0x96
+
+#define USAGE_GET_BLOB 0xc5
+#define USAGE_WRITE 0x02
+#define USAGE_READ_ADDRESS 0x03
+#define USAGE_READ_DATA 0x04
+#define USAGE_SET_MODE 0x06
+
+#define FEATURE_REPORT_TYPE 0x03
+
+#define VENDOR_DEFINED_PAGE 0xff00
+
+#define BLOB_REPORT_SIZE 256
+
+#define RESET_COMMAND 0x01
+#define GET_REPORT_COMMAND 0x02
+#define SET_REPORT_COMMAND 0x03
+#define SET_POWER_COMMAND 0x08
+
+#define FINGER_MODE 0x00
+#define RMI_MODE 0x02
+
+struct hid_report_info {
+	unsigned char get_blob_id;
+	unsigned char write_id;
+	unsigned char read_addr_id;
+	unsigned char read_data_id;
+	unsigned char set_mode_id;
+	unsigned int blob_size;
+};
+
+static struct hid_report_info hid_report;
+
+struct hid_device_descriptor {
+	unsigned short device_descriptor_length;
+	unsigned short format_version;
+	unsigned short report_descriptor_length;
+	unsigned short report_descriptor_index;
+	unsigned short input_register_index;
+	unsigned short input_report_max_length;
+	unsigned short output_register_index;
+	unsigned short output_report_max_length;
+	unsigned short command_register_index;
+	unsigned short data_register_index;
+	unsigned short vendor_id;
+	unsigned short product_id;
+	unsigned short version_id;
+	unsigned int reserved;
+};
+
+static struct hid_device_descriptor hid_dd;
+
+struct i2c_rw_buffer {
+	unsigned char *read;
+	unsigned char *write;
+	unsigned int read_size;
+	unsigned int write_size;
+};
+
+static struct i2c_rw_buffer buffer;
+
+#ifdef CONFIG_OF
+static int parse_dt(struct device *dev, struct synaptics_dsx_board_data *bdata)
+{
+	int retval;
+	u32 value;
+	const char *name;
+	struct property *prop;
+	struct device_node *np = dev->of_node;
+
+	bdata->irq_gpio = of_get_named_gpio_flags(np,
+			"synaptics,irq-gpio", 0,
+			(enum of_gpio_flags *)&bdata->irq_flags);
+
+	retval = of_property_read_u32(np, "synaptics,irq-on-state",
+			&value);
+	if (retval < 0)
+		bdata->irq_on_state = 0;
+	else
+		bdata->irq_on_state = value;
+
+	retval = of_property_read_string(np, "synaptics,pwr-reg-name", &name);
+	if (retval < 0)
+		bdata->pwr_reg_name = NULL;
+	else
+		bdata->pwr_reg_name = name;
+
+	retval = of_property_read_string(np, "synaptics,bus-reg-name", &name);
+	if (retval < 0)
+		bdata->bus_reg_name = NULL;
+	else
+		bdata->bus_reg_name = name;
+
+	prop = of_find_property(np, "synaptics,power-gpio", NULL);
+	if (prop && prop->length) {
+		bdata->power_gpio = of_get_named_gpio_flags(np,
+				"synaptics,power-gpio", 0, NULL);
+		retval = of_property_read_u32(np, "synaptics,power-on-state",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,power-on-state property\n",
+					__func__);
+			return retval;
+		}
+		bdata->power_on_state = value;
+	} else {
+		bdata->power_gpio = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,power-delay-ms", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,power-delay-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,power-delay-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->power_delay_ms = value;
+	} else {
+		bdata->power_delay_ms = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,reset-gpio", NULL);
+	if (prop && prop->length) {
+		bdata->reset_gpio = of_get_named_gpio_flags(np,
+				"synaptics,reset-gpio", 0, NULL);
+		retval = of_property_read_u32(np, "synaptics,reset-on-state",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-on-state property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_on_state = value;
+		retval = of_property_read_u32(np, "synaptics,reset-active-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-active-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_active_ms = value;
+	} else {
+		bdata->reset_gpio = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,reset-delay-ms", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,reset-delay-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-delay-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_delay_ms = value;
+	} else {
+		bdata->reset_delay_ms = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,dev-dscrptr-addr", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,dev-dscrptr-addr",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,dev-dscrptr-addr property\n",
+					__func__);
+			return retval;
+		}
+		bdata->device_descriptor_addr = (unsigned short)value;
+	} else {
+		bdata->device_descriptor_addr = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,max-y-for-2d", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,max-y-for-2d",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,max-y-for-2d property\n",
+					__func__);
+			return retval;
+		}
+		bdata->max_y_for_2d = value;
+	} else {
+		bdata->max_y_for_2d = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,swap-axes", NULL);
+	bdata->swap_axes = prop > 0 ? true : false;
+
+	prop = of_find_property(np, "synaptics,x-flip", NULL);
+	bdata->x_flip = prop > 0 ? true : false;
+
+	prop = of_find_property(np, "synaptics,y-flip", NULL);
+	bdata->y_flip = prop > 0 ? true : false;
+
+	prop = of_find_property(np, "synaptics,ub-i2c-addr", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,ub-i2c-addr",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,ub-i2c-addr property\n",
+					__func__);
+			return retval;
+		}
+		bdata->ub_i2c_addr = (unsigned short)value;
+	} else {
+		bdata->ub_i2c_addr = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,cap-button-codes", NULL);
+	if (prop && prop->length) {
+		bdata->cap_button_map->map = devm_kzalloc(dev,
+				prop->length,
+				GFP_KERNEL);
+		if (!bdata->cap_button_map->map)
+			return -ENOMEM;
+		bdata->cap_button_map->nbuttons = prop->length / sizeof(u32);
+		retval = of_property_read_u32_array(np,
+				"synaptics,cap-button-codes",
+				bdata->cap_button_map->map,
+				bdata->cap_button_map->nbuttons);
+		if (retval < 0) {
+			bdata->cap_button_map->nbuttons = 0;
+			bdata->cap_button_map->map = NULL;
+		}
+	} else {
+		bdata->cap_button_map->nbuttons = 0;
+		bdata->cap_button_map->map = NULL;
+	}
+
+	prop = of_find_property(np, "synaptics,vir-button-codes", NULL);
+	if (prop && prop->length) {
+		bdata->vir_button_map->map = devm_kzalloc(dev,
+				prop->length,
+				GFP_KERNEL);
+		if (!bdata->vir_button_map->map)
+			return -ENOMEM;
+		bdata->vir_button_map->nbuttons = prop->length / sizeof(u32);
+		bdata->vir_button_map->nbuttons /= 5;
+		retval = of_property_read_u32_array(np,
+				"synaptics,vir-button-codes",
+				bdata->vir_button_map->map,
+				bdata->vir_button_map->nbuttons * 5);
+		if (retval < 0) {
+			bdata->vir_button_map->nbuttons = 0;
+			bdata->vir_button_map->map = NULL;
+		}
+	} else {
+		bdata->vir_button_map->nbuttons = 0;
+		bdata->vir_button_map->map = NULL;
+	}
+
+	return 0;
+}
+#endif
+
+static int do_i2c_transfer(struct i2c_client *client, struct i2c_msg *msg)
+{
+	unsigned char retry;
+
+	for (retry = 0; retry < SYN_I2C_RETRY_TIMES; retry++) {
+		if (i2c_transfer(client->adapter, msg, 1) == 1)
+			break;
+		dev_err(&client->dev,
+				"%s: I2C retry %d\n",
+				__func__, retry + 1);
+		msleep(20);
+	}
+
+	if (retry == SYN_I2C_RETRY_TIMES) {
+		dev_err(&client->dev,
+				"%s: I2C transfer over retry limit\n",
+				__func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int check_buffer(unsigned char **buffer, unsigned int *buffer_size,
+		unsigned int length)
+{
+	if (*buffer_size < length) {
+		if (*buffer_size)
+			kfree(*buffer);
+		*buffer = kzalloc(length, GFP_KERNEL);
+		if (!(*buffer))
+			return -ENOMEM;
+		*buffer_size = length;
+	}
+
+	return 0;
+}
+
+static int generic_read(struct i2c_client *client, unsigned short length)
+{
+	int retval;
+	struct i2c_msg msg[] = {
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = length,
+		}
+	};
+
+	check_buffer(&buffer.read, &buffer.read_size, length);
+	msg[0].buf = buffer.read;
+
+	retval = do_i2c_transfer(client, msg);
+
+	return retval;
+}
+
+static int generic_write(struct i2c_client *client, unsigned short length)
+{
+	int retval;
+	struct i2c_msg msg[] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = length,
+			.buf = buffer.write,
+		}
+	};
+
+	retval = do_i2c_transfer(client, msg);
+
+	return retval;
+}
+
+static void traverse_report_descriptor(unsigned int *index)
+{
+	unsigned char size;
+	unsigned char *buf = buffer.read;
+
+	size = buf[*index] & MASK_2BIT;
+	switch (size) {
+	case 0: /* 0 bytes */
+		*index += 1;
+		break;
+	case 1: /* 1 byte */
+		*index += 2;
+		break;
+	case 2: /* 2 bytes */
+		*index += 3;
+		break;
+	case 3: /* 4 bytes */
+		*index += 5;
+		break;
+	default:
+		break;
+	}
+}
+
+static void find_blob_size(unsigned int index)
+{
+	unsigned int ii = index;
+	unsigned char *buf = buffer.read;
+
+	while (ii < hid_dd.report_descriptor_length) {
+		if (buf[ii] == PREFIX_REPORT_COUNT_1BYTE) {
+			hid_report.blob_size = buf[ii + 1];
+			return;
+		} else if (buf[ii] == PREFIX_REPORT_COUNT_2BYTES) {
+			hid_report.blob_size = buf[ii + 1] | (buf[ii + 2] << 8);
+			return;
+		}
+		traverse_report_descriptor(&ii);
+	}
+}
+
+static void find_reports(unsigned int index)
+{
+	unsigned int ii = index;
+	unsigned char *buf = buffer.read;
+	static unsigned int report_id_index;
+	static unsigned char report_id;
+	static unsigned short usage_page;
+
+	if (buf[ii] == PREFIX_REPORT_ID) {
+		report_id = buf[ii + 1];
+		report_id_index = ii;
+		return;
+	}
+
+	if (buf[ii] == PREFIX_USAGE_PAGE_1BYTE) {
+		usage_page = buf[ii + 1];
+		return;
+	} else if (buf[ii] == PREFIX_USAGE_PAGE_2BYTES) {
+		usage_page = buf[ii + 1] | (buf[ii + 2] << 8);
+		return;
+	}
+
+	if ((usage_page == VENDOR_DEFINED_PAGE) && (buf[ii] == PREFIX_USAGE)) {
+		switch (buf[ii + 1]) {
+		case USAGE_GET_BLOB:
+			hid_report.get_blob_id = report_id;
+			find_blob_size(report_id_index);
+			break;
+		case USAGE_WRITE:
+			hid_report.write_id = report_id;
+			break;
+		case USAGE_READ_ADDRESS:
+			hid_report.read_addr_id = report_id;
+			break;
+		case USAGE_READ_DATA:
+			hid_report.read_data_id = report_id;
+			break;
+		case USAGE_SET_MODE:
+			hid_report.set_mode_id = report_id;
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+static int parse_report_descriptor(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned int ii = 0;
+	unsigned char *buf;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+
+	buffer.write[0] = hid_dd.report_descriptor_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.report_descriptor_index >> 8;
+	retval = generic_write(i2c, 2);
+	if (retval < 0)
+		return retval;
+	retval = generic_read(i2c, hid_dd.report_descriptor_length);
+	if (retval < 0)
+		return retval;
+
+	buf = buffer.read;
+
+	hid_report.get_blob_id = REPORT_ID_GET_BLOB;
+	hid_report.write_id = REPORT_ID_WRITE;
+	hid_report.read_addr_id = REPORT_ID_READ_ADDRESS;
+	hid_report.read_data_id = REPORT_ID_READ_DATA;
+	hid_report.set_mode_id = REPORT_ID_SET_RMI_MODE;
+	hid_report.blob_size = BLOB_REPORT_SIZE;
+
+	while (ii < hid_dd.report_descriptor_length) {
+		find_reports(ii);
+		traverse_report_descriptor(&ii);
+	}
+
+	return 0;
+}
+
+static int switch_to_rmi(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	check_buffer(&buffer.write, &buffer.write_size, 11);
+
+	/* set rmi mode */
+	buffer.write[0] = hid_dd.command_register_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.command_register_index >> 8;
+	buffer.write[2] = (FEATURE_REPORT_TYPE << 4) | hid_report.set_mode_id;
+	buffer.write[3] = SET_REPORT_COMMAND;
+	buffer.write[4] = hid_report.set_mode_id;
+	buffer.write[5] = hid_dd.data_register_index & MASK_8BIT;
+	buffer.write[6] = hid_dd.data_register_index >> 8;
+	buffer.write[7] = 0x04;
+	buffer.write[8] = 0x00;
+	buffer.write[9] = hid_report.set_mode_id;
+	buffer.write[10] = RMI_MODE;
+
+	retval = generic_write(i2c, 11);
+
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	return retval;
+}
+
+static int check_report_mode(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned short report_size;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	check_buffer(&buffer.write, &buffer.write_size, 7);
+
+	buffer.write[0] = hid_dd.command_register_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.command_register_index >> 8;
+	buffer.write[2] = (FEATURE_REPORT_TYPE << 4) | hid_report.set_mode_id;
+	buffer.write[3] = GET_REPORT_COMMAND;
+	buffer.write[4] = hid_report.set_mode_id;
+	buffer.write[5] = hid_dd.data_register_index & MASK_8BIT;
+	buffer.write[6] = hid_dd.data_register_index >> 8;
+
+	retval = generic_write(i2c, 7);
+	if (retval < 0)
+		goto exit;
+
+	retval = generic_read(i2c, 2);
+	if (retval < 0)
+		goto exit;
+
+	report_size = (buffer.read[1] << 8) | buffer.read[0];
+
+	retval = generic_write(i2c, 7);
+	if (retval < 0)
+		goto exit;
+
+	retval = generic_read(i2c, report_size);
+	if (retval < 0)
+		goto exit;
+
+	retval = buffer.read[3];
+	dev_dbg(rmi4_data->pdev->dev.parent,
+			"%s: Report mode = %d\n",
+			__func__, retval);
+
+exit:
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	return retval;
+}
+
+static int hid_i2c_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	check_buffer(&buffer.write, &buffer.write_size, 6);
+
+	/* read device descriptor */
+	buffer.write[0] = bdata->device_descriptor_addr & MASK_8BIT;
+	buffer.write[1] = bdata->device_descriptor_addr >> 8;
+	retval = generic_write(i2c, 2);
+	if (retval < 0)
+		goto exit;
+	retval = generic_read(i2c, sizeof(hid_dd));
+	if (retval < 0)
+		goto exit;
+	retval = secure_memcpy((unsigned char *)&hid_dd,
+			sizeof(struct hid_device_descriptor),
+			buffer.read,
+			buffer.read_size,
+			sizeof(hid_dd));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy device descriptor data\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = parse_report_descriptor(rmi4_data);
+	if (retval < 0)
+		goto exit;
+
+	/* set power */
+	buffer.write[0] = hid_dd.command_register_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.command_register_index >> 8;
+	buffer.write[2] = 0x00;
+	buffer.write[3] = SET_POWER_COMMAND;
+	retval = generic_write(i2c, 4);
+	if (retval < 0)
+		goto exit;
+
+	/* reset */
+	buffer.write[0] = hid_dd.command_register_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.command_register_index >> 8;
+	buffer.write[2] = 0x00;
+	buffer.write[3] = RESET_COMMAND;
+	retval = generic_write(i2c, 4);
+	if (retval < 0)
+		goto exit;
+
+	while (gpio_get_value(bdata->irq_gpio))
+		msleep(20);
+
+	retval = generic_read(i2c, hid_dd.input_report_max_length);
+	if (retval < 0)
+		goto exit;
+
+	/* get blob */
+	buffer.write[0] = hid_dd.command_register_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.command_register_index >> 8;
+	buffer.write[2] = (FEATURE_REPORT_TYPE << 4) | hid_report.get_blob_id;
+	buffer.write[3] = 0x02;
+	buffer.write[4] = hid_dd.data_register_index & MASK_8BIT;
+	buffer.write[5] = hid_dd.data_register_index >> 8;
+
+	retval = generic_write(i2c, 6);
+	if (retval < 0)
+		goto exit;
+
+	msleep(20);
+
+	retval = generic_read(i2c, hid_report.blob_size + 3);
+	if (retval < 0)
+		goto exit;
+
+exit:
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to initialize HID/I2C interface\n",
+				__func__);
+		return retval;
+	}
+
+	retval = switch_to_rmi(rmi4_data);
+
+	return retval;
+}
+
+static int synaptics_rmi4_i2c_read(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr, unsigned char *data, unsigned int length)
+{
+	int retval;
+	unsigned char retry;
+	unsigned char recover = 1;
+	unsigned short report_length;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+	struct i2c_msg msg[] = {
+		{
+			.addr = i2c->addr,
+			.flags = 0,
+			.len = hid_dd.output_report_max_length + 2,
+		},
+		{
+			.addr = i2c->addr,
+			.flags = I2C_M_RD,
+			.len = (unsigned short)(length + 4),
+		},
+	};
+
+recover:
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	check_buffer(&buffer.write, &buffer.write_size,
+			hid_dd.output_report_max_length + 2);
+	msg[0].buf = buffer.write;
+	buffer.write[0] = hid_dd.output_register_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.output_register_index >> 8;
+	buffer.write[2] = hid_dd.output_report_max_length & MASK_8BIT;
+	buffer.write[3] = hid_dd.output_report_max_length >> 8;
+	buffer.write[4] = hid_report.read_addr_id;
+	buffer.write[5] = 0x00;
+	buffer.write[6] = addr & MASK_8BIT;
+	buffer.write[7] = addr >> 8;
+	buffer.write[8] = (unsigned char)length;
+	buffer.write[9] = (unsigned char)(length >> 8);
+
+	check_buffer(&buffer.read, &buffer.read_size, length + 4);
+	msg[1].buf = buffer.read;
+
+	retval = do_i2c_transfer(i2c, &msg[0]);
+	if (retval != 0)
+		goto exit;
+
+	retry = 0;
+	do {
+		retval = do_i2c_transfer(i2c, &msg[1]);
+		if (retval == 0)
+			retval = length;
+		else
+			goto exit;
+
+		report_length = (buffer.read[1] << 8) | buffer.read[0];
+		if (report_length == hid_dd.input_report_max_length) {
+			retval = secure_memcpy(&data[0], length,
+					&buffer.read[4], buffer.read_size - 4,
+					length);
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to copy data\n",
+						__func__);
+			} else {
+				retval = length;
+			}
+			goto exit;
+		}
+
+		msleep(20);
+		retry++;
+	} while (retry < SYN_I2C_RETRY_TIMES);
+
+	dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Failed to receive read report\n",
+			__func__);
+	retval = -EIO;
+
+exit:
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	if ((retval != length) && (recover == 1)) {
+		recover = 0;
+		if (check_report_mode(rmi4_data) != RMI_MODE) {
+			retval = hid_i2c_init(rmi4_data);
+			if (retval == 0)
+				goto recover;
+		}
+	}
+
+	return retval;
+}
+
+static int synaptics_rmi4_i2c_write(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr, unsigned char *data, unsigned int length)
+{
+	int retval;
+	unsigned char recover = 1;
+	unsigned int msg_length;
+	struct i2c_client *i2c = to_i2c_client(rmi4_data->pdev->dev.parent);
+	struct i2c_msg msg[] = {
+		{
+			.addr = i2c->addr,
+			.flags = 0,
+		}
+	};
+
+	if ((length + 10) < (hid_dd.output_report_max_length + 2))
+		msg_length = hid_dd.output_report_max_length + 2;
+	else
+		msg_length = length + 10;
+
+recover:
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	check_buffer(&buffer.write, &buffer.write_size, msg_length);
+	msg[0].len = (unsigned short)msg_length;
+	msg[0].buf = buffer.write;
+	buffer.write[0] = hid_dd.output_register_index & MASK_8BIT;
+	buffer.write[1] = hid_dd.output_register_index >> 8;
+	buffer.write[2] = hid_dd.output_report_max_length & MASK_8BIT;
+	buffer.write[3] = hid_dd.output_report_max_length >> 8;
+	buffer.write[4] = hid_report.write_id;
+	buffer.write[5] = 0x00;
+	buffer.write[6] = addr & MASK_8BIT;
+	buffer.write[7] = addr >> 8;
+	buffer.write[8] = (unsigned char)length;
+	buffer.write[9] = (unsigned char)(length >> 8);
+	retval = secure_memcpy(&buffer.write[10], buffer.write_size - 10,
+			&data[0], length, length);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy data\n",
+				__func__);
+	} else {
+		retval = do_i2c_transfer(i2c, msg);
+		if (retval == 0)
+			retval = length;
+	}
+
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	if ((retval != length) && (recover == 1)) {
+		recover = 0;
+		if (check_report_mode(rmi4_data) != RMI_MODE) {
+			retval = hid_i2c_init(rmi4_data);
+			if (retval == 0)
+				goto recover;
+		}
+	}
+
+	return retval;
+}
+
+static struct synaptics_dsx_bus_access bus_access = {
+	.type = BUS_I2C,
+	.read = synaptics_rmi4_i2c_read,
+	.write = synaptics_rmi4_i2c_write,
+};
+
+static struct synaptics_dsx_hw_interface hw_if;
+
+static struct platform_device *synaptics_dsx_i2c_device;
+
+static void synaptics_rmi4_i2c_dev_release(struct device *dev)
+{
+	kfree(synaptics_dsx_i2c_device);
+}
+
+static int synaptics_rmi4_i2c_probe(struct i2c_client *client,
+		const struct i2c_device_id *dev_id)
+{
+	int retval;
+
+	if (!i2c_check_functionality(client->adapter,
+			I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&client->dev,
+				"%s: SMBus byte data commands not supported by host\n",
+				__func__);
+		return -EIO;
+	}
+
+	synaptics_dsx_i2c_device = kzalloc(
+			sizeof(struct platform_device),
+			GFP_KERNEL);
+	if (!synaptics_dsx_i2c_device) {
+		dev_err(&client->dev,
+				"%s: Failed to allocate memory for synaptics_dsx_i2c_device\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+#ifdef CONFIG_OF
+	if (client->dev.of_node) {
+		hw_if.board_data = devm_kzalloc(&client->dev,
+				sizeof(struct synaptics_dsx_board_data),
+				GFP_KERNEL);
+		if (!hw_if.board_data) {
+			dev_err(&client->dev,
+					"%s: Failed to allocate memory for board data\n",
+					__func__);
+			return -ENOMEM;
+		}
+		hw_if.board_data->cap_button_map = devm_kzalloc(&client->dev,
+				sizeof(struct synaptics_dsx_button_map),
+				GFP_KERNEL);
+		if (!hw_if.board_data->cap_button_map) {
+			dev_err(&client->dev,
+					"%s: Failed to allocate memory for 0D button map\n",
+					__func__);
+			return -ENOMEM;
+		}
+		hw_if.board_data->vir_button_map = devm_kzalloc(&client->dev,
+				sizeof(struct synaptics_dsx_button_map),
+				GFP_KERNEL);
+		if (!hw_if.board_data->vir_button_map) {
+			dev_err(&client->dev,
+					"%s: Failed to allocate memory for virtual button map\n",
+					__func__);
+			return -ENOMEM;
+		}
+		parse_dt(&client->dev, hw_if.board_data);
+	}
+#else
+	hw_if.board_data = client->dev.platform_data;
+#endif
+
+	hw_if.bus_access = &bus_access;
+	hw_if.bl_hw_init = switch_to_rmi;
+	hw_if.ui_hw_init = hid_i2c_init;
+
+	synaptics_dsx_i2c_device->name = PLATFORM_DRIVER_NAME;
+	synaptics_dsx_i2c_device->id = 0;
+	synaptics_dsx_i2c_device->num_resources = 0;
+	synaptics_dsx_i2c_device->dev.parent = &client->dev;
+	synaptics_dsx_i2c_device->dev.platform_data = &hw_if;
+	synaptics_dsx_i2c_device->dev.release = synaptics_rmi4_i2c_dev_release;
+
+	retval = platform_device_register(synaptics_dsx_i2c_device);
+	if (retval) {
+		dev_err(&client->dev,
+				"%s: Failed to register platform device\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int synaptics_rmi4_i2c_remove(struct i2c_client *client)
+{
+	if (buffer.read_size)
+		kfree(buffer.read);
+
+	if (buffer.write_size)
+		kfree(buffer.write);
+
+	platform_device_unregister(synaptics_dsx_i2c_device);
+
+	return 0;
+}
+
+static const struct i2c_device_id synaptics_rmi4_id_table[] = {
+	{I2C_DRIVER_NAME, 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, synaptics_rmi4_id_table);
+
+#ifdef CONFIG_OF
+static const struct of_device_id synaptics_rmi4_of_match_table[] = {
+	{
+		.compatible = "synaptics,dsx-rmi-hid-i2c",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, synaptics_rmi4_of_match_table);
+#else
+#define synaptics_rmi4_of_match_table NULL
+#endif
+
+static struct i2c_driver synaptics_rmi4_i2c_driver = {
+	.driver = {
+		.name = I2C_DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = synaptics_rmi4_of_match_table,
+	},
+	.probe = synaptics_rmi4_i2c_probe,
+	.remove = synaptics_rmi4_i2c_remove,
+	.id_table = synaptics_rmi4_id_table,
+};
+
+int synaptics_rmi4_bus_init(void)
+{
+	return i2c_add_driver(&synaptics_rmi4_i2c_driver);
+}
+EXPORT_SYMBOL(synaptics_rmi4_bus_init);
+
+void synaptics_rmi4_bus_exit(void)
+{
+	i2c_del_driver(&synaptics_rmi4_i2c_driver);
+}
+EXPORT_SYMBOL(synaptics_rmi4_bus_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX I2C Bus Support Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_spi.c

@@ -0,0 +1,698 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/types.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define SPI_READ 0x80
+#define SPI_WRITE 0x00
+
+static unsigned char *buf;
+
+static struct spi_transfer *xfer;
+
+#ifdef CONFIG_OF
+static int parse_dt(struct device *dev, struct synaptics_dsx_board_data *bdata)
+{
+	int retval;
+	u32 value;
+	const char *name;
+	struct property *prop;
+	struct device_node *np = dev->of_node;
+
+	bdata->irq_gpio = of_get_named_gpio_flags(np,
+			"synaptics,irq-gpio", 0,
+			(enum of_gpio_flags *)&bdata->irq_flags);
+
+	retval = of_property_read_u32(np, "synaptics,irq-on-state",
+			&value);
+	if (retval < 0)
+		bdata->irq_on_state = 0;
+	else
+		bdata->irq_on_state = value;
+
+	retval = of_property_read_string(np, "synaptics,pwr-reg-name", &name);
+	if (retval < 0)
+		bdata->pwr_reg_name = NULL;
+	else
+		bdata->pwr_reg_name = name;
+
+	retval = of_property_read_string(np, "synaptics,bus-reg-name", &name);
+	if (retval < 0)
+		bdata->bus_reg_name = NULL;
+	else
+		bdata->bus_reg_name = name;
+
+	prop = of_find_property(np, "synaptics,power-gpio", NULL);
+	if (prop && prop->length) {
+		bdata->power_gpio = of_get_named_gpio_flags(np,
+				"synaptics,power-gpio", 0, NULL);
+		retval = of_property_read_u32(np, "synaptics,power-on-state",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,power-on-state property\n",
+					__func__);
+			return retval;
+		}
+		bdata->power_on_state = value;
+	} else {
+		bdata->power_gpio = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,power-delay-ms", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,power-delay-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,power-delay-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->power_delay_ms = value;
+	} else {
+		bdata->power_delay_ms = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,reset-gpio", NULL);
+	if (prop && prop->length) {
+		bdata->reset_gpio = of_get_named_gpio_flags(np,
+				"synaptics,reset-gpio", 0, NULL);
+		retval = of_property_read_u32(np, "synaptics,reset-on-state",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-on-state property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_on_state = value;
+		retval = of_property_read_u32(np, "synaptics,reset-active-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-active-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_active_ms = value;
+	} else {
+		bdata->reset_gpio = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,reset-delay-ms", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,reset-delay-ms",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,reset-delay-ms property\n",
+					__func__);
+			return retval;
+		}
+		bdata->reset_delay_ms = value;
+	} else {
+		bdata->reset_delay_ms = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,byte-delay-us", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,byte-delay-us",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,byte-delay-us property\n",
+					__func__);
+			return retval;
+		}
+		bdata->byte_delay_us = value;
+	} else {
+		bdata->byte_delay_us = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,block-delay-us", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,block-delay-us",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,block-delay-us property\n",
+					__func__);
+			return retval;
+		}
+		bdata->block_delay_us = value;
+	} else {
+		bdata->block_delay_us = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,address-delay-us", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,address-delay-us",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,address-delay-us property\n",
+					__func__);
+			return retval;
+		}
+		bdata->addr_delay_us = value;
+	} else {
+		bdata->addr_delay_us = 0;
+	}
+
+	prop = of_find_property(np, "synaptics,max-y-for-2d", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,max-y-for-2d",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,max-y-for-2d property\n",
+					__func__);
+			return retval;
+		}
+		bdata->max_y_for_2d = value;
+	} else {
+		bdata->max_y_for_2d = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,swap-axes", NULL);
+	bdata->swap_axes = prop > 0 ? true : false;
+
+	prop = of_find_property(np, "synaptics,x-flip", NULL);
+	bdata->x_flip = prop > 0 ? true : false;
+
+	prop = of_find_property(np, "synaptics,y-flip", NULL);
+	bdata->y_flip = prop > 0 ? true : false;
+
+	prop = of_find_property(np, "synaptics,ub-i2c-addr", NULL);
+	if (prop && prop->length) {
+		retval = of_property_read_u32(np, "synaptics,ub-i2c-addr",
+				&value);
+		if (retval < 0) {
+			dev_err(dev, "%s: Unable to read synaptics,ub-i2c-addr property\n",
+					__func__);
+			return retval;
+		}
+		bdata->ub_i2c_addr = (unsigned short)value;
+	} else {
+		bdata->ub_i2c_addr = -1;
+	}
+
+	prop = of_find_property(np, "synaptics,cap-button-codes", NULL);
+	if (prop && prop->length) {
+		bdata->cap_button_map->map = devm_kzalloc(dev,
+				prop->length,
+				GFP_KERNEL);
+		if (!bdata->cap_button_map->map)
+			return -ENOMEM;
+		bdata->cap_button_map->nbuttons = prop->length / sizeof(u32);
+		retval = of_property_read_u32_array(np,
+				"synaptics,cap-button-codes",
+				bdata->cap_button_map->map,
+				bdata->cap_button_map->nbuttons);
+		if (retval < 0) {
+			bdata->cap_button_map->nbuttons = 0;
+			bdata->cap_button_map->map = NULL;
+		}
+	} else {
+		bdata->cap_button_map->nbuttons = 0;
+		bdata->cap_button_map->map = NULL;
+	}
+
+	prop = of_find_property(np, "synaptics,vir-button-codes", NULL);
+	if (prop && prop->length) {
+		bdata->vir_button_map->map = devm_kzalloc(dev,
+				prop->length,
+				GFP_KERNEL);
+		if (!bdata->vir_button_map->map)
+			return -ENOMEM;
+		bdata->vir_button_map->nbuttons = prop->length / sizeof(u32);
+		bdata->vir_button_map->nbuttons /= 5;
+		retval = of_property_read_u32_array(np,
+				"synaptics,vir-button-codes",
+				bdata->vir_button_map->map,
+				bdata->vir_button_map->nbuttons * 5);
+		if (retval < 0) {
+			bdata->vir_button_map->nbuttons = 0;
+			bdata->vir_button_map->map = NULL;
+		}
+	} else {
+		bdata->vir_button_map->nbuttons = 0;
+		bdata->vir_button_map->map = NULL;
+	}
+
+	return 0;
+}
+#endif
+
+static int synaptics_rmi4_spi_alloc_buf(struct synaptics_rmi4_data *rmi4_data,
+		unsigned int size, unsigned int count)
+{
+	static unsigned int buf_size;
+	static unsigned int xfer_count;
+
+	if (size > buf_size) {
+		if (buf_size)
+			kfree(buf);
+		buf = kmalloc(size, GFP_KERNEL);
+		if (!buf) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for buf\n",
+					__func__);
+			buf_size = 0;
+			return -ENOMEM;
+		}
+		buf_size = size;
+	}
+
+	if (count > xfer_count) {
+		if (xfer_count)
+			kfree(xfer);
+		xfer = kcalloc(count, sizeof(struct spi_transfer), GFP_KERNEL);
+		if (!xfer) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for xfer\n",
+					__func__);
+			xfer_count = 0;
+			return -ENOMEM;
+		}
+		xfer_count = count;
+	} else {
+		memset(xfer, 0, count * sizeof(struct spi_transfer));
+	}
+
+	return 0;
+}
+
+static int synaptics_rmi4_spi_set_page(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr)
+{
+	int retval;
+	unsigned int index;
+	unsigned int byte_count = PAGE_SELECT_LEN + 1;
+	unsigned char page;
+	struct spi_message msg;
+	struct spi_device *spi = to_spi_device(rmi4_data->pdev->dev.parent);
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	page = ((addr >> 8) & MASK_8BIT);
+	if ((page >> 7) == (rmi4_data->current_page >> 7))
+		return PAGE_SELECT_LEN;
+
+	spi_message_init(&msg);
+
+	retval = synaptics_rmi4_spi_alloc_buf(rmi4_data, byte_count,
+			byte_count);
+	if (retval < 0)
+		return retval;
+
+	buf[0] = SPI_WRITE;
+	buf[1] = MASK_8BIT;
+	buf[2] = page;
+
+	if (bdata->byte_delay_us == 0) {
+		xfer[0].len = byte_count;
+		xfer[0].tx_buf = &buf[0];
+		if (bdata->block_delay_us)
+			xfer[0].delay_usecs = bdata->block_delay_us;
+		spi_message_add_tail(&xfer[0], &msg);
+	} else {
+		for (index = 0; index < byte_count; index++) {
+			xfer[index].len = 1;
+			xfer[index].tx_buf = &buf[index];
+			if (index == 1)
+				xfer[index].delay_usecs = bdata->addr_delay_us;
+			else
+				xfer[index].delay_usecs = bdata->byte_delay_us;
+			spi_message_add_tail(&xfer[index], &msg);
+		}
+		if (bdata->block_delay_us)
+			xfer[index - 1].delay_usecs = bdata->block_delay_us;
+	}
+
+	retval = spi_sync(spi, &msg);
+	if (retval == 0) {
+		rmi4_data->current_page = page;
+		retval = PAGE_SELECT_LEN;
+	} else {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to complete SPI transfer, error = %d\n",
+				__func__, retval);
+	}
+
+	return retval;
+}
+
+static int synaptics_rmi4_spi_read(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr, unsigned char *data, unsigned int length)
+{
+	int retval;
+	unsigned int index;
+	unsigned int byte_count = length + ADDRESS_LEN;
+	unsigned char txbuf[ADDRESS_LEN];
+	struct spi_message msg;
+	struct spi_device *spi = to_spi_device(rmi4_data->pdev->dev.parent);
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	spi_message_init(&msg);
+
+	txbuf[0] = (addr >> 8) | SPI_READ;
+	txbuf[1] = addr & MASK_8BIT;
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	retval = synaptics_rmi4_spi_set_page(rmi4_data, addr);
+	if (retval != PAGE_SELECT_LEN) {
+		mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+		return -EIO;
+	}
+
+	if (bdata->byte_delay_us == 0) {
+		retval = synaptics_rmi4_spi_alloc_buf(rmi4_data, length,
+				2);
+	} else {
+		retval = synaptics_rmi4_spi_alloc_buf(rmi4_data, length,
+				byte_count);
+	}
+	if (retval < 0) {
+		mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+		return retval;
+	}
+
+	if (bdata->byte_delay_us == 0) {
+		xfer[0].len = ADDRESS_LEN;
+		xfer[0].tx_buf = &txbuf[0];
+		spi_message_add_tail(&xfer[0], &msg);
+		xfer[1].len = length;
+		xfer[1].rx_buf = &buf[0];
+		if (bdata->block_delay_us)
+			xfer[1].delay_usecs = bdata->block_delay_us;
+		spi_message_add_tail(&xfer[1], &msg);
+	} else {
+		for (index = 0; index < byte_count; index++) {
+			xfer[index].len = 1;
+			if (index < ADDRESS_LEN)
+				xfer[index].tx_buf = &txbuf[index];
+			else
+				xfer[index].rx_buf = &buf[index - ADDRESS_LEN];
+			if (index == 1)
+				xfer[index].delay_usecs = bdata->addr_delay_us;
+			else
+				xfer[index].delay_usecs = bdata->byte_delay_us;
+			spi_message_add_tail(&xfer[index], &msg);
+		}
+		if (bdata->block_delay_us)
+			xfer[index - 1].delay_usecs = bdata->block_delay_us;
+	}
+
+	retval = spi_sync(spi, &msg);
+	if (retval == 0) {
+		retval = secure_memcpy(data, length, buf, length, length);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to copy data\n",
+					__func__);
+		} else {
+			retval = length;
+		}
+	} else {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to complete SPI transfer, error = %d\n",
+				__func__, retval);
+	}
+
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	return retval;
+}
+
+static int synaptics_rmi4_spi_write(struct synaptics_rmi4_data *rmi4_data,
+		unsigned short addr, unsigned char *data, unsigned int length)
+{
+	int retval;
+	unsigned int index;
+	unsigned int byte_count = length + ADDRESS_LEN;
+	struct spi_message msg;
+	struct spi_device *spi = to_spi_device(rmi4_data->pdev->dev.parent);
+	const struct synaptics_dsx_board_data *bdata =
+			rmi4_data->hw_if->board_data;
+
+	spi_message_init(&msg);
+
+	mutex_lock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	retval = synaptics_rmi4_spi_set_page(rmi4_data, addr);
+	if (retval != PAGE_SELECT_LEN) {
+		mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+		return -EIO;
+	}
+
+	if (bdata->byte_delay_us == 0) {
+		retval = synaptics_rmi4_spi_alloc_buf(rmi4_data, byte_count,
+				1);
+	} else {
+		retval = synaptics_rmi4_spi_alloc_buf(rmi4_data, byte_count,
+				byte_count);
+	}
+	if (retval < 0) {
+		mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+		return retval;
+	}
+
+	buf[0] = (addr >> 8) & ~SPI_READ;
+	buf[1] = addr & MASK_8BIT;
+	retval = secure_memcpy(&buf[ADDRESS_LEN],
+			byte_count - ADDRESS_LEN, data, length, length);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy data\n",
+				__func__);
+		mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+		return retval;
+	}
+
+	if (bdata->byte_delay_us == 0) {
+		xfer[0].len = byte_count;
+		xfer[0].tx_buf = &buf[0];
+		if (bdata->block_delay_us)
+			xfer[0].delay_usecs = bdata->block_delay_us;
+		spi_message_add_tail(xfer, &msg);
+	} else {
+		for (index = 0; index < byte_count; index++) {
+			xfer[index].len = 1;
+			xfer[index].tx_buf = &buf[index];
+			if (index == 1)
+				xfer[index].delay_usecs = bdata->addr_delay_us;
+			else
+				xfer[index].delay_usecs = bdata->byte_delay_us;
+			spi_message_add_tail(&xfer[index], &msg);
+		}
+		if (bdata->block_delay_us)
+			xfer[index - 1].delay_usecs = bdata->block_delay_us;
+	}
+
+	retval = spi_sync(spi, &msg);
+	if (retval == 0) {
+		retval = length;
+	} else {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to complete SPI transfer, error = %d\n",
+				__func__, retval);
+	}
+
+	mutex_unlock(&rmi4_data->rmi4_io_ctrl_mutex);
+
+	return retval;
+}
+
+static struct synaptics_dsx_bus_access bus_access = {
+	.type = BUS_SPI,
+	.read = synaptics_rmi4_spi_read,
+	.write = synaptics_rmi4_spi_write,
+};
+
+static struct synaptics_dsx_hw_interface hw_if;
+
+static struct platform_device *synaptics_dsx_spi_device;
+
+static void synaptics_rmi4_spi_dev_release(struct device *dev)
+{
+	kfree(synaptics_dsx_spi_device);
+}
+
+static int synaptics_rmi4_spi_probe(struct spi_device *spi)
+{
+	int retval;
+
+	if (spi->master->flags & SPI_MASTER_HALF_DUPLEX) {
+		dev_err(&spi->dev,
+				"%s: Full duplex not supported by host\n",
+				__func__);
+		return -EIO;
+	}
+
+	synaptics_dsx_spi_device = kzalloc(
+			sizeof(struct platform_device),
+			GFP_KERNEL);
+	if (!synaptics_dsx_spi_device) {
+		dev_err(&spi->dev,
+				"%s: Failed to allocate memory for synaptics_dsx_spi_device\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+#ifdef CONFIG_OF
+	if (spi->dev.of_node) {
+		hw_if.board_data = devm_kzalloc(&spi->dev,
+				sizeof(struct synaptics_dsx_board_data),
+				GFP_KERNEL);
+		if (!hw_if.board_data) {
+			dev_err(&spi->dev,
+					"%s: Failed to allocate memory for board data\n",
+					__func__);
+			return -ENOMEM;
+		}
+		hw_if.board_data->cap_button_map = devm_kzalloc(&spi->dev,
+				sizeof(struct synaptics_dsx_button_map),
+				GFP_KERNEL);
+		if (!hw_if.board_data->cap_button_map) {
+			dev_err(&spi->dev,
+					"%s: Failed to allocate memory for 0D button map\n",
+					__func__);
+			return -ENOMEM;
+		}
+		hw_if.board_data->vir_button_map = devm_kzalloc(&spi->dev,
+				sizeof(struct synaptics_dsx_button_map),
+				GFP_KERNEL);
+		if (!hw_if.board_data->vir_button_map) {
+			dev_err(&spi->dev,
+					"%s: Failed to allocate memory for virtual button map\n",
+					__func__);
+			return -ENOMEM;
+		}
+		parse_dt(&spi->dev, hw_if.board_data);
+	}
+#else
+	hw_if.board_data = spi->dev.platform_data;
+#endif
+
+	hw_if.bus_access = &bus_access;
+
+	spi->bits_per_word = 8;
+	spi->mode = SPI_MODE_3;
+
+	retval = spi_setup(spi);
+	if (retval < 0) {
+		dev_err(&spi->dev,
+				"%s: Failed to perform SPI setup\n",
+				__func__);
+		return retval;
+	}
+
+	synaptics_dsx_spi_device->name = PLATFORM_DRIVER_NAME;
+	synaptics_dsx_spi_device->id = 0;
+	synaptics_dsx_spi_device->num_resources = 0;
+	synaptics_dsx_spi_device->dev.parent = &spi->dev;
+	synaptics_dsx_spi_device->dev.platform_data = &hw_if;
+	synaptics_dsx_spi_device->dev.release = synaptics_rmi4_spi_dev_release;
+
+	retval = platform_device_register(synaptics_dsx_spi_device);
+	if (retval) {
+		dev_err(&spi->dev,
+				"%s: Failed to register platform device\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int synaptics_rmi4_spi_remove(struct spi_device *spi)
+{
+	platform_device_unregister(synaptics_dsx_spi_device);
+
+	return 0;
+}
+
+static const struct spi_device_id synaptics_rmi4_id_table[] = {
+	{SPI_DRIVER_NAME, 0},
+	{},
+};
+MODULE_DEVICE_TABLE(spi, synaptics_rmi4_id_table);
+
+#ifdef CONFIG_OF
+static const struct of_device_id synaptics_rmi4_of_match_table[] = {
+	{
+		.compatible = "synaptics,dsx-spi",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, synaptics_rmi4_of_match_table);
+#else
+#define synaptics_rmi4_of_match_table NULL
+#endif
+
+static struct spi_driver synaptics_rmi4_spi_driver = {
+	.driver = {
+		.name = SPI_DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = synaptics_rmi4_of_match_table,
+	},
+	.probe = synaptics_rmi4_spi_probe,
+	.remove = synaptics_rmi4_spi_remove,
+	.id_table = synaptics_rmi4_id_table,
+};
+
+int synaptics_rmi4_bus_init(void)
+{
+	return spi_register_driver(&synaptics_rmi4_spi_driver);
+}
+EXPORT_SYMBOL(synaptics_rmi4_bus_init);
+
+void synaptics_rmi4_bus_exit(void)
+{
+	kfree(buf);
+
+	kfree(xfer);
+
+	spi_unregister_driver(&synaptics_rmi4_spi_driver);
+}
+EXPORT_SYMBOL(synaptics_rmi4_bus_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX SPI Bus Support Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_test_reporting.c

@@ -0,0 +1,5324 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/ctype.h>
+#include <linux/hrtimer.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define SYSFS_FOLDER_NAME "f54"
+
+#define GET_REPORT_TIMEOUT_S 3
+#define CALIBRATION_TIMEOUT_S 10
+#define COMMAND_TIMEOUT_100MS 20
+
+#define NO_SLEEP_OFF (0 << 2)
+#define NO_SLEEP_ON (1 << 2)
+
+#define STATUS_IDLE 0
+#define STATUS_BUSY 1
+#define STATUS_ERROR 2
+
+#define REPORT_INDEX_OFFSET 1
+#define REPORT_DATA_OFFSET 3
+
+#define SENSOR_RX_MAPPING_OFFSET 1
+#define SENSOR_TX_MAPPING_OFFSET 2
+
+#define COMMAND_GET_REPORT 1
+#define COMMAND_FORCE_CAL 2
+#define COMMAND_FORCE_UPDATE 4
+
+#define CONTROL_NO_AUTO_CAL 1
+
+#define CONTROL_0_SIZE 1
+#define CONTROL_1_SIZE 1
+#define CONTROL_2_SIZE 2
+#define CONTROL_3_SIZE 1
+#define CONTROL_4_6_SIZE 3
+#define CONTROL_7_SIZE 1
+#define CONTROL_8_9_SIZE 3
+#define CONTROL_10_SIZE 1
+#define CONTROL_11_SIZE 2
+#define CONTROL_12_13_SIZE 2
+#define CONTROL_14_SIZE 1
+#define CONTROL_15_SIZE 1
+#define CONTROL_16_SIZE 1
+#define CONTROL_17_SIZE 1
+#define CONTROL_18_SIZE 1
+#define CONTROL_19_SIZE 1
+#define CONTROL_20_SIZE 1
+#define CONTROL_21_SIZE 2
+#define CONTROL_22_26_SIZE 7
+#define CONTROL_27_SIZE 1
+#define CONTROL_28_SIZE 2
+#define CONTROL_29_SIZE 1
+#define CONTROL_30_SIZE 1
+#define CONTROL_31_SIZE 1
+#define CONTROL_32_35_SIZE 8
+#define CONTROL_36_SIZE 1
+#define CONTROL_37_SIZE 1
+#define CONTROL_38_SIZE 1
+#define CONTROL_39_SIZE 1
+#define CONTROL_40_SIZE 1
+#define CONTROL_41_SIZE 1
+#define CONTROL_42_SIZE 2
+#define CONTROL_43_54_SIZE 13
+#define CONTROL_55_56_SIZE 2
+#define CONTROL_57_SIZE 1
+#define CONTROL_58_SIZE 1
+#define CONTROL_59_SIZE 2
+#define CONTROL_60_62_SIZE 3
+#define CONTROL_63_SIZE 1
+#define CONTROL_64_67_SIZE 4
+#define CONTROL_68_73_SIZE 8
+#define CONTROL_70_73_SIZE 6
+#define CONTROL_74_SIZE 2
+#define CONTROL_75_SIZE 1
+#define CONTROL_76_SIZE 1
+#define CONTROL_77_78_SIZE 2
+#define CONTROL_79_83_SIZE 5
+#define CONTROL_84_85_SIZE 2
+#define CONTROL_86_SIZE 1
+#define CONTROL_87_SIZE 1
+#define CONTROL_88_SIZE 1
+#define CONTROL_89_SIZE 1
+#define CONTROL_90_SIZE 1
+#define CONTROL_91_SIZE 1
+#define CONTROL_92_SIZE 1
+#define CONTROL_93_SIZE 1
+#define CONTROL_94_SIZE 1
+#define CONTROL_95_SIZE 1
+#define CONTROL_96_SIZE 1
+#define CONTROL_97_SIZE 1
+#define CONTROL_98_SIZE 1
+#define CONTROL_99_SIZE 1
+#define CONTROL_100_SIZE 1
+#define CONTROL_101_SIZE 1
+#define CONTROL_102_SIZE 1
+#define CONTROL_103_SIZE 1
+#define CONTROL_104_SIZE 1
+#define CONTROL_105_SIZE 1
+#define CONTROL_106_SIZE 1
+#define CONTROL_107_SIZE 1
+#define CONTROL_108_SIZE 1
+#define CONTROL_109_SIZE 1
+#define CONTROL_110_SIZE 1
+#define CONTROL_111_SIZE 1
+#define CONTROL_112_SIZE 1
+#define CONTROL_113_SIZE 1
+#define CONTROL_114_SIZE 1
+#define CONTROL_115_SIZE 1
+#define CONTROL_116_SIZE 1
+#define CONTROL_117_SIZE 1
+#define CONTROL_118_SIZE 1
+#define CONTROL_119_SIZE 1
+#define CONTROL_120_SIZE 1
+#define CONTROL_121_SIZE 1
+#define CONTROL_122_SIZE 1
+#define CONTROL_123_SIZE 1
+#define CONTROL_124_SIZE 1
+#define CONTROL_125_SIZE 1
+#define CONTROL_126_SIZE 1
+#define CONTROL_127_SIZE 1
+#define CONTROL_128_SIZE 1
+#define CONTROL_129_SIZE 1
+#define CONTROL_130_SIZE 1
+#define CONTROL_131_SIZE 1
+#define CONTROL_132_SIZE 1
+#define CONTROL_133_SIZE 1
+#define CONTROL_134_SIZE 1
+#define CONTROL_135_SIZE 1
+#define CONTROL_136_SIZE 1
+#define CONTROL_137_SIZE 1
+#define CONTROL_138_SIZE 1
+#define CONTROL_139_SIZE 1
+#define CONTROL_140_SIZE 1
+#define CONTROL_141_SIZE 1
+#define CONTROL_142_SIZE 1
+#define CONTROL_143_SIZE 1
+#define CONTROL_144_SIZE 1
+#define CONTROL_145_SIZE 1
+#define CONTROL_146_SIZE 1
+#define CONTROL_147_SIZE 1
+#define CONTROL_148_SIZE 1
+#define CONTROL_149_SIZE 1
+#define CONTROL_150_SIZE 1
+#define CONTROL_151_SIZE 1
+#define CONTROL_152_SIZE 1
+#define CONTROL_153_SIZE 1
+#define CONTROL_154_SIZE 1
+#define CONTROL_155_SIZE 1
+#define CONTROL_156_SIZE 1
+#define CONTROL_157_158_SIZE 2
+#define CONTROL_163_SIZE 1
+#define CONTROL_165_SIZE 1
+#define CONTROL_166_SIZE 1
+#define CONTROL_167_SIZE 1
+#define CONTROL_168_SIZE 1
+#define CONTROL_169_SIZE 1
+#define CONTROL_171_SIZE 1
+#define CONTROL_172_SIZE 1
+#define CONTROL_173_SIZE 1
+#define CONTROL_174_SIZE 1
+#define CONTROL_175_SIZE 1
+#define CONTROL_176_SIZE 1
+#define CONTROL_177_178_SIZE 2
+#define CONTROL_179_SIZE 1
+#define CONTROL_182_SIZE 1
+#define CONTROL_183_SIZE 1
+#define CONTROL_185_SIZE 1
+#define CONTROL_186_SIZE 1
+#define CONTROL_187_SIZE 1
+#define CONTROL_188_SIZE 1
+
+#define HIGH_RESISTANCE_DATA_SIZE 6
+#define FULL_RAW_CAP_MIN_MAX_DATA_SIZE 4
+#define TRX_OPEN_SHORT_DATA_SIZE 7
+
+#define attrify(propname) (&dev_attr_##propname.attr)
+
+#define show_prototype(propname)\
+static ssize_t propname##_show(\
+		struct device *dev,\
+		struct device_attribute *attr,\
+		char *buf);\
+\
+static struct device_attribute dev_attr_##propname =\
+		__ATTR_RO(propname)
+
+#define store_prototype(propname)\
+static ssize_t propname##_store(\
+		struct device *dev,\
+		struct device_attribute *attr,\
+		const char *buf, size_t count);\
+\
+static struct device_attribute dev_attr_##propname =\
+		__ATTR_WO(propname)
+
+#define show_store_prototype(propname)\
+static ssize_t propname##_show(\
+		struct device *dev,\
+		struct device_attribute *attr,\
+		char *buf);\
+\
+static ssize_t propname##_store(\
+		struct device *dev,\
+		struct device_attribute *attr,\
+		const char *buf, size_t count);\
+\
+static struct device_attribute dev_attr_##propname =\
+		__ATTR_RW(propname)
+
+#define disable_cbc(ctrl_num)\
+do {\
+	retval = synaptics_rmi4_reg_read(rmi4_data,\
+			f54->control.ctrl_num->address,\
+			f54->control.ctrl_num->data,\
+			sizeof(f54->control.ctrl_num->data));\
+	if (retval < 0) {\
+		dev_err(rmi4_data->pdev->dev.parent,\
+				"%s: Failed to disable CBC (" #ctrl_num ")\n",\
+				__func__);\
+		return retval;\
+	} \
+	f54->control.ctrl_num->cbc_tx_carrier_selection = 0;\
+	retval = synaptics_rmi4_reg_write(rmi4_data,\
+			f54->control.ctrl_num->address,\
+			f54->control.ctrl_num->data,\
+			sizeof(f54->control.ctrl_num->data));\
+	if (retval < 0) {\
+		dev_err(rmi4_data->pdev->dev.parent,\
+				"%s: Failed to disable CBC (" #ctrl_num ")\n",\
+				__func__);\
+		return retval;\
+	} \
+} while (0)
+
+enum f54_report_types {
+	F54_8BIT_IMAGE = 1,
+	F54_16BIT_IMAGE = 2,
+	F54_RAW_16BIT_IMAGE = 3,
+	F54_HIGH_RESISTANCE = 4,
+	F54_TX_TO_TX_SHORTS = 5,
+	F54_RX_TO_RX_SHORTS_1 = 7,
+	F54_TRUE_BASELINE = 9,
+	F54_FULL_RAW_CAP_MIN_MAX = 13,
+	F54_RX_OPENS_1 = 14,
+	F54_TX_OPENS = 15,
+	F54_TX_TO_GND_SHORTS = 16,
+	F54_RX_TO_RX_SHORTS_2 = 17,
+	F54_RX_OPENS_2 = 18,
+	F54_FULL_RAW_CAP = 19,
+	F54_FULL_RAW_CAP_NO_RX_COUPLING = 20,
+	F54_SENSOR_SPEED = 22,
+	F54_ADC_RANGE = 23,
+	F54_TRX_OPENS = 24,
+	F54_TRX_TO_GND_SHORTS = 25,
+	F54_TRX_SHORTS = 26,
+	F54_ABS_RAW_CAP = 38,
+	F54_ABS_DELTA_CAP = 40,
+	F54_ABS_HYBRID_DELTA_CAP = 59,
+	F54_ABS_HYBRID_RAW_CAP = 63,
+	F54_AMP_FULL_RAW_CAP = 78,
+	F54_AMP_RAW_ADC = 83,
+	F54_FULL_RAW_CAP_TDDI = 92,
+	INVALID_REPORT_TYPE = -1,
+};
+
+enum f54_afe_cal {
+	F54_AFE_CAL,
+	F54_AFE_IS_CAL,
+};
+
+struct f54_query {
+	union {
+		struct {
+			/* query 0 */
+			unsigned char num_of_rx_electrodes;
+
+			/* query 1 */
+			unsigned char num_of_tx_electrodes;
+
+			/* query 2 */
+			unsigned char f54_query2_b0__1:2;
+			unsigned char has_baseline:1;
+			unsigned char has_image8:1;
+			unsigned char f54_query2_b4__5:2;
+			unsigned char has_image16:1;
+			unsigned char f54_query2_b7:1;
+
+			/* queries 3.0 and 3.1 */
+			unsigned short clock_rate;
+
+			/* query 4 */
+			unsigned char touch_controller_family;
+
+			/* query 5 */
+			unsigned char has_pixel_touch_threshold_adjustment:1;
+			unsigned char f54_query5_b1__7:7;
+
+			/* query 6 */
+			unsigned char has_sensor_assignment:1;
+			unsigned char has_interference_metric:1;
+			unsigned char has_sense_frequency_control:1;
+			unsigned char has_firmware_noise_mitigation:1;
+			unsigned char has_ctrl11:1;
+			unsigned char has_two_byte_report_rate:1;
+			unsigned char has_one_byte_report_rate:1;
+			unsigned char has_relaxation_control:1;
+
+			/* query 7 */
+			unsigned char curve_compensation_mode:2;
+			unsigned char f54_query7_b2__7:6;
+
+			/* query 8 */
+			unsigned char f54_query8_b0:1;
+			unsigned char has_iir_filter:1;
+			unsigned char has_cmn_removal:1;
+			unsigned char has_cmn_maximum:1;
+			unsigned char has_touch_hysteresis:1;
+			unsigned char has_edge_compensation:1;
+			unsigned char has_per_frequency_noise_control:1;
+			unsigned char has_enhanced_stretch:1;
+
+			/* query 9 */
+			unsigned char has_force_fast_relaxation:1;
+			unsigned char has_multi_metric_state_machine:1;
+			unsigned char has_signal_clarity:1;
+			unsigned char has_variance_metric:1;
+			unsigned char has_0d_relaxation_control:1;
+			unsigned char has_0d_acquisition_control:1;
+			unsigned char has_status:1;
+			unsigned char has_slew_metric:1;
+
+			/* query 10 */
+			unsigned char has_h_blank:1;
+			unsigned char has_v_blank:1;
+			unsigned char has_long_h_blank:1;
+			unsigned char has_startup_fast_relaxation:1;
+			unsigned char has_esd_control:1;
+			unsigned char has_noise_mitigation2:1;
+			unsigned char has_noise_state:1;
+			unsigned char has_energy_ratio_relaxation:1;
+
+			/* query 11 */
+			unsigned char has_excessive_noise_reporting:1;
+			unsigned char has_slew_option:1;
+			unsigned char has_two_overhead_bursts:1;
+			unsigned char has_query13:1;
+			unsigned char has_one_overhead_burst:1;
+			unsigned char f54_query11_b5:1;
+			unsigned char has_ctrl88:1;
+			unsigned char has_query15:1;
+
+			/* query 12 */
+			unsigned char number_of_sensing_frequencies:4;
+			unsigned char f54_query12_b4__7:4;
+		} __packed;
+		unsigned char data[14];
+	};
+};
+
+struct f54_query_13 {
+	union {
+		struct {
+			unsigned char has_ctrl86:1;
+			unsigned char has_ctrl87:1;
+			unsigned char has_ctrl87_sub0:1;
+			unsigned char has_ctrl87_sub1:1;
+			unsigned char has_ctrl87_sub2:1;
+			unsigned char has_cidim:1;
+			unsigned char has_noise_mitigation_enhancement:1;
+			unsigned char has_rail_im:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_15 {
+	union {
+		struct {
+			unsigned char has_ctrl90:1;
+			unsigned char has_transmit_strength:1;
+			unsigned char has_ctrl87_sub3:1;
+			unsigned char has_query16:1;
+			unsigned char has_query20:1;
+			unsigned char has_query21:1;
+			unsigned char has_query22:1;
+			unsigned char has_query25:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_16 {
+	union {
+		struct {
+			unsigned char has_query17:1;
+			unsigned char has_data17:1;
+			unsigned char has_ctrl92:1;
+			unsigned char has_ctrl93:1;
+			unsigned char has_ctrl94_query18:1;
+			unsigned char has_ctrl95_query19:1;
+			unsigned char has_ctrl99:1;
+			unsigned char has_ctrl100:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_21 {
+	union {
+		struct {
+			unsigned char has_abs_rx:1;
+			unsigned char has_abs_tx:1;
+			unsigned char has_ctrl91:1;
+			unsigned char has_ctrl96:1;
+			unsigned char has_ctrl97:1;
+			unsigned char has_ctrl98:1;
+			unsigned char has_data19:1;
+			unsigned char has_query24_data18:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_22 {
+	union {
+		struct {
+			unsigned char has_packed_image:1;
+			unsigned char has_ctrl101:1;
+			unsigned char has_dynamic_sense_display_ratio:1;
+			unsigned char has_query23:1;
+			unsigned char has_ctrl103_query26:1;
+			unsigned char has_ctrl104:1;
+			unsigned char has_ctrl105:1;
+			unsigned char has_query28:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_23 {
+	union {
+		struct {
+			unsigned char has_ctrl102:1;
+			unsigned char has_ctrl102_sub1:1;
+			unsigned char has_ctrl102_sub2:1;
+			unsigned char has_ctrl102_sub4:1;
+			unsigned char has_ctrl102_sub5:1;
+			unsigned char has_ctrl102_sub9:1;
+			unsigned char has_ctrl102_sub10:1;
+			unsigned char has_ctrl102_sub11:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_25 {
+	union {
+		struct {
+			unsigned char has_ctrl106:1;
+			unsigned char has_ctrl102_sub12:1;
+			unsigned char has_ctrl107:1;
+			unsigned char has_ctrl108:1;
+			unsigned char has_ctrl109:1;
+			unsigned char has_data20:1;
+			unsigned char f54_query25_b6:1;
+			unsigned char has_query27:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_27 {
+	union {
+		struct {
+			unsigned char has_ctrl110:1;
+			unsigned char has_data21:1;
+			unsigned char has_ctrl111:1;
+			unsigned char has_ctrl112:1;
+			unsigned char has_ctrl113:1;
+			unsigned char has_data22:1;
+			unsigned char has_ctrl114:1;
+			unsigned char has_query29:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_29 {
+	union {
+		struct {
+			unsigned char has_ctrl115:1;
+			unsigned char has_ground_ring_options:1;
+			unsigned char has_lost_bursts_tuning:1;
+			unsigned char has_aux_exvcom2_select:1;
+			unsigned char has_ctrl116:1;
+			unsigned char has_data23:1;
+			unsigned char has_ctrl117:1;
+			unsigned char has_query30:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_30 {
+	union {
+		struct {
+			unsigned char has_ctrl118:1;
+			unsigned char has_ctrl119:1;
+			unsigned char has_ctrl120:1;
+			unsigned char has_ctrl121:1;
+			unsigned char has_ctrl122_query31:1;
+			unsigned char has_ctrl123:1;
+			unsigned char has_ctrl124:1;
+			unsigned char has_query32:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_32 {
+	union {
+		struct {
+			unsigned char has_ctrl125:1;
+			unsigned char has_ctrl126:1;
+			unsigned char has_ctrl127:1;
+			unsigned char has_abs_charge_pump_disable:1;
+			unsigned char has_query33:1;
+			unsigned char has_data24:1;
+			unsigned char has_query34:1;
+			unsigned char has_query35:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_33 {
+	union {
+		struct {
+			unsigned char has_ctrl128:1;
+			unsigned char has_ctrl129:1;
+			unsigned char has_ctrl130:1;
+			unsigned char has_ctrl131:1;
+			unsigned char has_ctrl132:1;
+			unsigned char has_ctrl133:1;
+			unsigned char has_ctrl134:1;
+			unsigned char has_query36:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_35 {
+	union {
+		struct {
+			unsigned char has_data25:1;
+			unsigned char has_ctrl135:1;
+			unsigned char has_ctrl136:1;
+			unsigned char has_ctrl137:1;
+			unsigned char has_ctrl138:1;
+			unsigned char has_ctrl139:1;
+			unsigned char has_data26:1;
+			unsigned char has_ctrl140:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_36 {
+	union {
+		struct {
+			unsigned char has_ctrl141:1;
+			unsigned char has_ctrl142:1;
+			unsigned char has_query37:1;
+			unsigned char has_ctrl143:1;
+			unsigned char has_ctrl144:1;
+			unsigned char has_ctrl145:1;
+			unsigned char has_ctrl146:1;
+			unsigned char has_query38:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_38 {
+	union {
+		struct {
+			unsigned char has_ctrl147:1;
+			unsigned char has_ctrl148:1;
+			unsigned char has_ctrl149:1;
+			unsigned char has_ctrl150:1;
+			unsigned char has_ctrl151:1;
+			unsigned char has_ctrl152:1;
+			unsigned char has_ctrl153:1;
+			unsigned char has_query39:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_39 {
+	union {
+		struct {
+			unsigned char has_ctrl154:1;
+			unsigned char has_ctrl155:1;
+			unsigned char has_ctrl156:1;
+			unsigned char has_ctrl160:1;
+			unsigned char has_ctrl157_ctrl158:1;
+			unsigned char f54_query39_b5__6:2;
+			unsigned char has_query40:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_40 {
+	union {
+		struct {
+			unsigned char has_ctrl169:1;
+			unsigned char has_ctrl163_query41:1;
+			unsigned char f54_query40_b2:1;
+			unsigned char has_ctrl165_query42:1;
+			unsigned char has_ctrl166:1;
+			unsigned char has_ctrl167:1;
+			unsigned char has_ctrl168:1;
+			unsigned char has_query43:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_43 {
+	union {
+		struct {
+			unsigned char f54_query43_b0__1:2;
+			unsigned char has_ctrl171:1;
+			unsigned char has_ctrl172_query44_query45:1;
+			unsigned char has_ctrl173:1;
+			unsigned char has_ctrl174:1;
+			unsigned char has_ctrl175:1;
+			unsigned char has_query46:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_46 {
+	union {
+		struct {
+			unsigned char has_ctrl176:1;
+			unsigned char has_ctrl177_ctrl178:1;
+			unsigned char has_ctrl179:1;
+			unsigned char f54_query46_b3:1;
+			unsigned char has_data27:1;
+			unsigned char has_data28:1;
+			unsigned char f54_query46_b6:1;
+			unsigned char has_query47:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_47 {
+	union {
+		struct {
+			unsigned char f54_query47_b0:1;
+			unsigned char has_ctrl182:1;
+			unsigned char has_ctrl183:1;
+			unsigned char f54_query47_b3:1;
+			unsigned char has_ctrl185:1;
+			unsigned char has_ctrl186:1;
+			unsigned char has_ctrl187:1;
+			unsigned char has_query49:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_49 {
+	union {
+		struct {
+			unsigned char f54_query49_b0__1:2;
+			unsigned char has_ctrl188:1;
+			unsigned char has_data31:1;
+			unsigned char f54_query49_b4__6:3;
+			unsigned char has_query50:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_50 {
+	union {
+		struct {
+			unsigned char f54_query50_b0__6:7;
+			unsigned char has_query51:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_51 {
+	union {
+		struct {
+			unsigned char f54_query51_b0__4:5;
+			unsigned char has_query53_query54_ctrl198:1;
+			unsigned char has_ctrl199:1;
+			unsigned char has_query55:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_55 {
+	union {
+		struct {
+			unsigned char has_query56:1;
+			unsigned char has_data33_data34:1;
+			unsigned char has_alt_report_rate:1;
+			unsigned char has_ctrl200:1;
+			unsigned char has_ctrl201_ctrl202:1;
+			unsigned char has_ctrl203:1;
+			unsigned char has_ctrl204:1;
+			unsigned char has_query57:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_57 {
+	union {
+		struct {
+			unsigned char has_ctrl205:1;
+			unsigned char has_ctrl206:1;
+			unsigned char has_usb_bulk_read:1;
+			unsigned char has_ctrl207:1;
+			unsigned char has_ctrl208:1;
+			unsigned char has_ctrl209:1;
+			unsigned char has_ctrl210:1;
+			unsigned char has_query58:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_58 {
+	union {
+		struct {
+			unsigned char has_query59:1;
+			unsigned char has_query60:1;
+			unsigned char has_ctrl211:1;
+			unsigned char has_ctrl212:1;
+			unsigned char has_hybrid_abs_tx_axis_filtering:1;
+			unsigned char has_hybrid_abs_tx_interpolation:1;
+			unsigned char has_ctrl213:1;
+			unsigned char has_query61:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_61 {
+	union {
+		struct {
+			unsigned char has_ctrl214:1;
+			unsigned char has_ctrl215_query62_query63:1;
+			unsigned char f54_query_61_b2:1;
+			unsigned char has_ctrl216:1;
+			unsigned char has_ctrl217:1;
+			unsigned char has_misc_host_ctrl:1;
+			unsigned char hybrid_abs_buttons:1;
+			unsigned char has_query64:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_64 {
+	union {
+		struct {
+			unsigned char has_ctrl101_sub1:1;
+			unsigned char has_ctrl220:1;
+			unsigned char has_ctrl221:1;
+			unsigned char has_ctrl222:1;
+			unsigned char has_ctrl219_sub1:1;
+			unsigned char has_ctrl103_sub3:1;
+			unsigned char has_ctrl224_ctrl226_ctrl227:1;
+			unsigned char has_query65:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_65 {
+	union {
+		struct {
+			unsigned char f54_query_65_b0__1:2;
+			unsigned char has_ctrl101_sub2:1;
+			unsigned char f54_query_65_b3__4:2;
+			unsigned char has_query66_ctrl231:1;
+			unsigned char has_ctrl232:1;
+			unsigned char has_query67:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_67 {
+	union {
+		struct {
+			unsigned char has_abs_doze_spatial_filter_en:1;
+			unsigned char has_abs_doze_avg_filter_enhancement_en:1;
+			unsigned char has_single_display_pulse:1;
+			unsigned char f54_query_67_b3__4:2;
+			unsigned char has_ctrl235_ctrl236:1;
+			unsigned char f54_query_67_b6:1;
+			unsigned char has_query68:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_68 {
+	union {
+		struct {
+			unsigned char f54_query_68_b0:1;
+			unsigned char has_ctrl238:1;
+			unsigned char has_ctrl238_sub1:1;
+			unsigned char has_ctrl238_sub2:1;
+			unsigned char has_ctrl239:1;
+			unsigned char has_freq_filter_bw_ext:1;
+			unsigned char is_tddi_hic:1;
+			unsigned char has_query69:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_query_69 {
+	union {
+		struct {
+			unsigned char has_ctrl240_sub0:1;
+			unsigned char has_ctrl240_sub1_sub2:1;
+			unsigned char has_ctrl240_sub3:1;
+			unsigned char has_ctrl240_sub4:1;
+			unsigned char f54_query_69_b4__7:4;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f54_data_31 {
+	union {
+		struct {
+			unsigned char is_calibration_crc:1;
+			unsigned char calibration_crc:1;
+			unsigned char short_test_row_number:5;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_7 {
+	union {
+		struct {
+			unsigned char cbc_cap:3;
+			unsigned char cbc_polarity:1;
+			unsigned char cbc_tx_carrier_selection:1;
+			unsigned char f54_ctrl7_b5__7:3;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_41 {
+	union {
+		struct {
+			unsigned char no_signal_clarity:1;
+			unsigned char f54_ctrl41_b1__7:7;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_57 {
+	union {
+		struct {
+			unsigned char cbc_cap:3;
+			unsigned char cbc_polarity:1;
+			unsigned char cbc_tx_carrier_selection:1;
+			unsigned char f54_ctrl57_b5__7:3;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_86 {
+	union {
+		struct {
+			unsigned char enable_high_noise_state:1;
+			unsigned char dynamic_sense_display_ratio:2;
+			unsigned char f54_ctrl86_b3__7:5;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_88 {
+	union {
+		struct {
+			unsigned char tx_low_reference_polarity:1;
+			unsigned char tx_high_reference_polarity:1;
+			unsigned char abs_low_reference_polarity:1;
+			unsigned char abs_polarity:1;
+			unsigned char cbc_polarity:1;
+			unsigned char cbc_tx_carrier_selection:1;
+			unsigned char charge_pump_enable:1;
+			unsigned char cbc_abs_auto_servo:1;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_110 {
+	union {
+		struct {
+			unsigned char active_stylus_rx_feedback_cap;
+			unsigned char active_stylus_rx_feedback_cap_reference;
+			unsigned char active_stylus_low_reference;
+			unsigned char active_stylus_high_reference;
+			unsigned char active_stylus_gain_control;
+			unsigned char active_stylus_gain_control_reference;
+			unsigned char active_stylus_timing_mode;
+			unsigned char active_stylus_discovery_bursts;
+			unsigned char active_stylus_detection_bursts;
+			unsigned char active_stylus_discovery_noise_multiplier;
+			unsigned char active_stylus_detection_envelope_min;
+			unsigned char active_stylus_detection_envelope_max;
+			unsigned char active_stylus_lose_count;
+		} __packed;
+		struct {
+			unsigned char data[13];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_149 {
+	union {
+		struct {
+			unsigned char trans_cbc_global_cap_enable:1;
+			unsigned char f54_ctrl149_b1__7:7;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control_188 {
+	union {
+		struct {
+			unsigned char start_calibration:1;
+			unsigned char start_is_calibration:1;
+			unsigned char frequency:2;
+			unsigned char start_production_test:1;
+			unsigned char short_test_calibration:1;
+			unsigned char f54_ctrl188_b7:1;
+		} __packed;
+		struct {
+			unsigned char data[1];
+			unsigned short address;
+		} __packed;
+	};
+};
+
+struct f54_control {
+	struct f54_control_7 *reg_7;
+	struct f54_control_41 *reg_41;
+	struct f54_control_57 *reg_57;
+	struct f54_control_86 *reg_86;
+	struct f54_control_88 *reg_88;
+	struct f54_control_110 *reg_110;
+	struct f54_control_149 *reg_149;
+	struct f54_control_188 *reg_188;
+};
+
+struct synaptics_rmi4_f54_handle {
+	bool no_auto_cal;
+	bool skip_preparation;
+	unsigned char status;
+	unsigned char intr_mask;
+	unsigned char intr_reg_num;
+	unsigned char tx_assigned;
+	unsigned char rx_assigned;
+	unsigned char *report_data;
+	unsigned short query_base_addr;
+	unsigned short control_base_addr;
+	unsigned short data_base_addr;
+	unsigned short command_base_addr;
+	unsigned short fifoindex;
+	unsigned int report_size;
+	unsigned int data_buffer_size;
+	unsigned int data_pos;
+	enum f54_report_types report_type;
+	struct f54_query query;
+	struct f54_query_13 query_13;
+	struct f54_query_15 query_15;
+	struct f54_query_16 query_16;
+	struct f54_query_21 query_21;
+	struct f54_query_22 query_22;
+	struct f54_query_23 query_23;
+	struct f54_query_25 query_25;
+	struct f54_query_27 query_27;
+	struct f54_query_29 query_29;
+	struct f54_query_30 query_30;
+	struct f54_query_32 query_32;
+	struct f54_query_33 query_33;
+	struct f54_query_35 query_35;
+	struct f54_query_36 query_36;
+	struct f54_query_38 query_38;
+	struct f54_query_39 query_39;
+	struct f54_query_40 query_40;
+	struct f54_query_43 query_43;
+	struct f54_query_46 query_46;
+	struct f54_query_47 query_47;
+	struct f54_query_49 query_49;
+	struct f54_query_50 query_50;
+	struct f54_query_51 query_51;
+	struct f54_query_55 query_55;
+	struct f54_query_57 query_57;
+	struct f54_query_58 query_58;
+	struct f54_query_61 query_61;
+	struct f54_query_64 query_64;
+	struct f54_query_65 query_65;
+	struct f54_query_67 query_67;
+	struct f54_query_68 query_68;
+	struct f54_query_69 query_69;
+	struct f54_data_31 data_31;
+	struct f54_control control;
+	struct mutex status_mutex;
+	struct kobject *sysfs_dir;
+	struct hrtimer watchdog;
+	struct work_struct timeout_work;
+	struct work_struct test_report_work;
+	struct workqueue_struct *test_report_workqueue;
+	struct synaptics_rmi4_data *rmi4_data;
+};
+
+struct f55_query {
+	union {
+		struct {
+			/* query 0 */
+			unsigned char num_of_rx_electrodes;
+
+			/* query 1 */
+			unsigned char num_of_tx_electrodes;
+
+			/* query 2 */
+			unsigned char has_sensor_assignment:1;
+			unsigned char has_edge_compensation:1;
+			unsigned char curve_compensation_mode:2;
+			unsigned char has_ctrl6:1;
+			unsigned char has_alternate_transmitter_assignment:1;
+			unsigned char has_single_layer_multi_touch:1;
+			unsigned char has_query5:1;
+		} __packed;
+		unsigned char data[3];
+	};
+};
+
+struct f55_query_3 {
+	union {
+		struct {
+			unsigned char has_ctrl8:1;
+			unsigned char has_ctrl9:1;
+			unsigned char has_oncell_pattern_support:1;
+			unsigned char has_data0:1;
+			unsigned char has_single_wide_pattern_support:1;
+			unsigned char has_mirrored_tx_pattern_support:1;
+			unsigned char has_discrete_pattern_support:1;
+			unsigned char has_query9:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_5 {
+	union {
+		struct {
+			unsigned char has_corner_compensation:1;
+			unsigned char has_ctrl12:1;
+			unsigned char has_trx_configuration:1;
+			unsigned char has_ctrl13:1;
+			unsigned char f55_query5_b4:1;
+			unsigned char has_ctrl14:1;
+			unsigned char has_basis_function:1;
+			unsigned char has_query17:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_17 {
+	union {
+		struct {
+			unsigned char f55_query17_b0:1;
+			unsigned char has_ctrl16:1;
+			unsigned char has_ctrl18_ctrl19:1;
+			unsigned char has_ctrl17:1;
+			unsigned char has_ctrl20:1;
+			unsigned char has_ctrl21:1;
+			unsigned char has_ctrl22:1;
+			unsigned char has_query18:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_18 {
+	union {
+		struct {
+			unsigned char has_ctrl23:1;
+			unsigned char has_ctrl24:1;
+			unsigned char has_query19:1;
+			unsigned char has_ctrl25:1;
+			unsigned char has_ctrl26:1;
+			unsigned char has_ctrl27_query20:1;
+			unsigned char has_ctrl28_query21:1;
+			unsigned char has_query22:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_22 {
+	union {
+		struct {
+			unsigned char has_ctrl29:1;
+			unsigned char has_query23:1;
+			unsigned char has_guard_disable:1;
+			unsigned char has_ctrl30:1;
+			unsigned char has_ctrl31:1;
+			unsigned char has_ctrl32:1;
+			unsigned char has_query24_through_query27:1;
+			unsigned char has_query28:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_23 {
+	union {
+		struct {
+			unsigned char amp_sensor_enabled:1;
+			unsigned char image_transposed:1;
+			unsigned char first_column_at_left_side:1;
+			unsigned char size_of_column2mux:5;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_28 {
+	union {
+		struct {
+			unsigned char f55_query28_b0__4:5;
+			unsigned char has_ctrl37:1;
+			unsigned char has_query29:1;
+			unsigned char has_query30:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_30 {
+	union {
+		struct {
+			unsigned char has_ctrl38:1;
+			unsigned char has_query31_query32:1;
+			unsigned char has_ctrl39:1;
+			unsigned char has_ctrl40:1;
+			unsigned char has_ctrl41:1;
+			unsigned char has_ctrl42:1;
+			unsigned char has_ctrl43_ctrl44:1;
+			unsigned char has_query33:1;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_query_33 {
+	union {
+		struct {
+			unsigned char has_extended_amp_pad:1;
+			unsigned char has_extended_amp_btn:1;
+			unsigned char has_ctrl45_ctrl46:1;
+			unsigned char f55_query33_b3:1;
+			unsigned char has_ctrl47_sub0_sub1:1;
+			unsigned char f55_query33_b5__7:3;
+		} __packed;
+		unsigned char data[1];
+	};
+};
+
+struct f55_control_43 {
+	union {
+		struct {
+			unsigned char swap_sensor_side:1;
+			unsigned char f55_ctrl43_b1__7:7;
+			unsigned char afe_l_mux_size:4;
+			unsigned char afe_r_mux_size:4;
+		} __packed;
+		unsigned char data[2];
+	};
+};
+
+struct synaptics_rmi4_f55_handle {
+	bool amp_sensor;
+	bool extended_amp;
+	bool has_force;
+	unsigned char size_of_column2mux;
+	unsigned char afe_mux_offset;
+	unsigned char force_tx_offset;
+	unsigned char force_rx_offset;
+	unsigned char *tx_assignment;
+	unsigned char *rx_assignment;
+	unsigned char *force_tx_assignment;
+	unsigned char *force_rx_assignment;
+	unsigned short query_base_addr;
+	unsigned short control_base_addr;
+	unsigned short data_base_addr;
+	unsigned short command_base_addr;
+	struct f55_query query;
+	struct f55_query_3 query_3;
+	struct f55_query_5 query_5;
+	struct f55_query_17 query_17;
+	struct f55_query_18 query_18;
+	struct f55_query_22 query_22;
+	struct f55_query_23 query_23;
+	struct f55_query_28 query_28;
+	struct f55_query_30 query_30;
+	struct f55_query_33 query_33;
+};
+
+struct f21_query_2 {
+	union {
+		struct {
+			unsigned char size_of_query3;
+			struct {
+				unsigned char query0_is_present:1;
+				unsigned char query1_is_present:1;
+				unsigned char query2_is_present:1;
+				unsigned char query3_is_present:1;
+				unsigned char query4_is_present:1;
+				unsigned char query5_is_present:1;
+				unsigned char query6_is_present:1;
+				unsigned char query7_is_present:1;
+			} __packed;
+			struct {
+				unsigned char query8_is_present:1;
+				unsigned char query9_is_present:1;
+				unsigned char query10_is_present:1;
+				unsigned char query11_is_present:1;
+				unsigned char query12_is_present:1;
+				unsigned char query13_is_present:1;
+				unsigned char query14_is_present:1;
+				unsigned char query15_is_present:1;
+			} __packed;
+		};
+		unsigned char data[3];
+	};
+};
+
+struct f21_query_5 {
+	union {
+		struct {
+			unsigned char size_of_query6;
+			struct {
+				unsigned char ctrl0_is_present:1;
+				unsigned char ctrl1_is_present:1;
+				unsigned char ctrl2_is_present:1;
+				unsigned char ctrl3_is_present:1;
+				unsigned char ctrl4_is_present:1;
+				unsigned char ctrl5_is_present:1;
+				unsigned char ctrl6_is_present:1;
+				unsigned char ctrl7_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl8_is_present:1;
+				unsigned char ctrl9_is_present:1;
+				unsigned char ctrl10_is_present:1;
+				unsigned char ctrl11_is_present:1;
+				unsigned char ctrl12_is_present:1;
+				unsigned char ctrl13_is_present:1;
+				unsigned char ctrl14_is_present:1;
+				unsigned char ctrl15_is_present:1;
+			} __packed;
+			struct {
+				unsigned char ctrl16_is_present:1;
+				unsigned char ctrl17_is_present:1;
+				unsigned char ctrl18_is_present:1;
+				unsigned char ctrl19_is_present:1;
+				unsigned char ctrl20_is_present:1;
+				unsigned char ctrl21_is_present:1;
+				unsigned char ctrl22_is_present:1;
+				unsigned char ctrl23_is_present:1;
+			} __packed;
+		};
+		unsigned char data[4];
+	};
+};
+
+struct f21_query_11 {
+	union {
+		struct {
+			unsigned char has_high_resolution_force:1;
+			unsigned char has_force_sensing_txrx_mapping:1;
+			unsigned char f21_query11_00_b2__7:6;
+			unsigned char f21_query11_00_reserved;
+			unsigned char max_number_of_force_sensors;
+			unsigned char max_number_of_force_txs;
+			unsigned char max_number_of_force_rxs;
+			unsigned char f21_query11_01_reserved;
+		} __packed;
+		unsigned char data[6];
+	};
+};
+
+struct synaptics_rmi4_f21_handle {
+	bool has_force;
+	unsigned char tx_assigned;
+	unsigned char rx_assigned;
+	unsigned char max_num_of_tx;
+	unsigned char max_num_of_rx;
+	unsigned char max_num_of_txrx;
+	unsigned char *force_txrx_assignment;
+	unsigned short query_base_addr;
+	unsigned short control_base_addr;
+	unsigned short data_base_addr;
+	unsigned short command_base_addr;
+};
+
+show_prototype(num_of_mapped_tx);
+show_prototype(num_of_mapped_rx);
+show_prototype(tx_mapping);
+show_prototype(rx_mapping);
+show_prototype(num_of_mapped_force_tx);
+show_prototype(num_of_mapped_force_rx);
+show_prototype(force_tx_mapping);
+show_prototype(force_rx_mapping);
+show_prototype(report_size);
+show_prototype(status);
+
+store_prototype(do_preparation);
+store_prototype(force_cal);
+store_prototype(get_report);
+store_prototype(resume_touch);
+store_prototype(do_afe_calibration);
+
+show_store_prototype(report_type);
+show_store_prototype(fifoindex);
+show_store_prototype(no_auto_cal);
+show_store_prototype(read_report);
+
+static struct attribute *attrs[] = {
+	attrify(num_of_mapped_tx),
+	attrify(num_of_mapped_rx),
+	attrify(tx_mapping),
+	attrify(rx_mapping),
+	attrify(num_of_mapped_force_tx),
+	attrify(num_of_mapped_force_rx),
+	attrify(force_tx_mapping),
+	attrify(force_rx_mapping),
+	attrify(report_size),
+	attrify(status),
+	attrify(do_preparation),
+	attrify(force_cal),
+	attrify(get_report),
+	attrify(resume_touch),
+	attrify(do_afe_calibration),
+	attrify(report_type),
+	attrify(fifoindex),
+	attrify(no_auto_cal),
+	attrify(read_report),
+	NULL,
+};
+
+static struct attribute_group attr_group = {
+	.attrs = attrs,
+};
+
+static ssize_t test_sysfs_data_read(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count);
+
+static struct bin_attribute test_report_data = {
+	.attr = {
+		.name = "report_data",
+		.mode = 0444,
+	},
+	.size = 0,
+	.read = test_sysfs_data_read,
+};
+
+static struct synaptics_rmi4_f54_handle *f54;
+static struct synaptics_rmi4_f55_handle *f55;
+static struct synaptics_rmi4_f21_handle *f21;
+
+DECLARE_COMPLETION(test_remove_complete);
+
+static bool test_report_type_valid(enum f54_report_types report_type)
+{
+	switch (report_type) {
+	case F54_8BIT_IMAGE:
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_HIGH_RESISTANCE:
+	case F54_TX_TO_TX_SHORTS:
+	case F54_RX_TO_RX_SHORTS_1:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP_MIN_MAX:
+	case F54_RX_OPENS_1:
+	case F54_TX_OPENS:
+	case F54_TX_TO_GND_SHORTS:
+	case F54_RX_TO_RX_SHORTS_2:
+	case F54_RX_OPENS_2:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_NO_RX_COUPLING:
+	case F54_SENSOR_SPEED:
+	case F54_ADC_RANGE:
+	case F54_TRX_OPENS:
+	case F54_TRX_TO_GND_SHORTS:
+	case F54_TRX_SHORTS:
+	case F54_ABS_RAW_CAP:
+	case F54_ABS_DELTA_CAP:
+	case F54_ABS_HYBRID_DELTA_CAP:
+	case F54_ABS_HYBRID_RAW_CAP:
+	case F54_AMP_FULL_RAW_CAP:
+	case F54_AMP_RAW_ADC:
+	case F54_FULL_RAW_CAP_TDDI:
+		return true;
+	default:
+		f54->report_type = INVALID_REPORT_TYPE;
+		f54->report_size = 0;
+		return false;
+	}
+}
+
+static void test_set_report_size(void)
+{
+	int retval;
+	unsigned char tx = f54->tx_assigned;
+	unsigned char rx = f54->rx_assigned;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	switch (f54->report_type) {
+	case F54_8BIT_IMAGE:
+		f54->report_size = tx * rx;
+		break;
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_NO_RX_COUPLING:
+	case F54_SENSOR_SPEED:
+	case F54_AMP_FULL_RAW_CAP:
+	case F54_AMP_RAW_ADC:
+	case F54_FULL_RAW_CAP_TDDI:
+		f54->report_size = 2 * tx * rx;
+		break;
+	case F54_HIGH_RESISTANCE:
+		f54->report_size = HIGH_RESISTANCE_DATA_SIZE;
+		break;
+	case F54_TX_TO_TX_SHORTS:
+	case F54_TX_OPENS:
+	case F54_TX_TO_GND_SHORTS:
+		f54->report_size = (tx + 7) / 8;
+		break;
+	case F54_RX_TO_RX_SHORTS_1:
+	case F54_RX_OPENS_1:
+		if (rx < tx)
+			f54->report_size = 2 * rx * rx;
+		else
+			f54->report_size = 2 * tx * rx;
+		break;
+	case F54_FULL_RAW_CAP_MIN_MAX:
+		f54->report_size = FULL_RAW_CAP_MIN_MAX_DATA_SIZE;
+		break;
+	case F54_RX_TO_RX_SHORTS_2:
+	case F54_RX_OPENS_2:
+		if (rx <= tx)
+			f54->report_size = 0;
+		else
+			f54->report_size = 2 * rx * (rx - tx);
+		break;
+	case F54_ADC_RANGE:
+		if (f54->query.has_signal_clarity) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					f54->control.reg_41->address,
+					f54->control.reg_41->data,
+					sizeof(f54->control.reg_41->data));
+			if (retval < 0) {
+				dev_dbg(rmi4_data->pdev->dev.parent,
+						"%s: Failed to read control reg_41\n",
+						__func__);
+				f54->report_size = 0;
+				break;
+			}
+			if (!f54->control.reg_41->no_signal_clarity) {
+				if (tx % 4)
+					tx += 4 - (tx % 4);
+			}
+		}
+		f54->report_size = 2 * tx * rx;
+		break;
+	case F54_TRX_OPENS:
+	case F54_TRX_TO_GND_SHORTS:
+	case F54_TRX_SHORTS:
+		f54->report_size = TRX_OPEN_SHORT_DATA_SIZE;
+		break;
+	case F54_ABS_RAW_CAP:
+	case F54_ABS_DELTA_CAP:
+	case F54_ABS_HYBRID_DELTA_CAP:
+	case F54_ABS_HYBRID_RAW_CAP:
+		tx += f21->tx_assigned;
+		rx += f21->rx_assigned;
+		f54->report_size = 4 * (tx + rx);
+		break;
+	default:
+		f54->report_size = 0;
+	}
+}
+
+static int test_set_interrupt(bool set)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char zero = 0x00;
+	unsigned char *intr_mask;
+	unsigned short f01_ctrl_reg;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	intr_mask = rmi4_data->intr_mask;
+	f01_ctrl_reg = rmi4_data->f01_ctrl_base_addr + 1 + f54->intr_reg_num;
+
+	if (!set) {
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				f01_ctrl_reg,
+				&zero,
+				sizeof(zero));
+		if (retval < 0)
+			return retval;
+	}
+
+	for (ii = 0; ii < rmi4_data->num_of_intr_regs; ii++) {
+		if (intr_mask[ii] != 0x00) {
+			f01_ctrl_reg = rmi4_data->f01_ctrl_base_addr + 1 + ii;
+			if (set) {
+				retval = synaptics_rmi4_reg_write(rmi4_data,
+						f01_ctrl_reg,
+						&zero,
+						sizeof(zero));
+				if (retval < 0)
+					return retval;
+			} else {
+				retval = synaptics_rmi4_reg_write(rmi4_data,
+						f01_ctrl_reg,
+						&(intr_mask[ii]),
+						sizeof(intr_mask[ii]));
+				if (retval < 0)
+					return retval;
+			}
+		}
+	}
+
+	f01_ctrl_reg = rmi4_data->f01_ctrl_base_addr + 1 + f54->intr_reg_num;
+
+	if (set) {
+		retval = synaptics_rmi4_reg_write(rmi4_data,
+				f01_ctrl_reg,
+				&f54->intr_mask,
+				1);
+		if (retval < 0)
+			return retval;
+	}
+
+	return 0;
+}
+
+static int test_wait_for_command_completion(void)
+{
+	int retval;
+	unsigned char value;
+	unsigned char timeout_count;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	timeout_count = 0;
+	do {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->command_base_addr,
+				&value,
+				sizeof(value));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read command register\n",
+					__func__);
+			return retval;
+		}
+
+		if (value == 0x00)
+			break;
+
+		msleep(100);
+		timeout_count++;
+	} while (timeout_count < COMMAND_TIMEOUT_100MS);
+
+	if (timeout_count == COMMAND_TIMEOUT_100MS) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Timed out waiting for command completion\n",
+				__func__);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int test_do_command(unsigned char command)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			f54->command_base_addr,
+			&command,
+			sizeof(command));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write command\n",
+				__func__);
+		return retval;
+	}
+
+	retval = test_wait_for_command_completion();
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static int test_do_preparation(void)
+{
+	int retval;
+	unsigned char value;
+	unsigned char zero = 0x00;
+	unsigned char device_ctrl;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set no sleep\n",
+				__func__);
+		return retval;
+	}
+
+	device_ctrl |= NO_SLEEP_ON;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set no sleep\n",
+				__func__);
+		return retval;
+	}
+
+	if (f54->skip_preparation)
+		return 0;
+
+	switch (f54->report_type) {
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_SENSOR_SPEED:
+	case F54_ADC_RANGE:
+	case F54_ABS_RAW_CAP:
+	case F54_ABS_DELTA_CAP:
+	case F54_ABS_HYBRID_DELTA_CAP:
+	case F54_ABS_HYBRID_RAW_CAP:
+	case F54_FULL_RAW_CAP_TDDI:
+		break;
+	case F54_AMP_RAW_ADC:
+		if (f54->query_49.has_ctrl188) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					f54->control.reg_188->address,
+					f54->control.reg_188->data,
+					sizeof(f54->control.reg_188->data));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to set start production test\n",
+						__func__);
+				return retval;
+			}
+			f54->control.reg_188->start_production_test = 1;
+			retval = synaptics_rmi4_reg_write(rmi4_data,
+					f54->control.reg_188->address,
+					f54->control.reg_188->data,
+					sizeof(f54->control.reg_188->data));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to set start production test\n",
+						__func__);
+				return retval;
+			}
+		}
+		break;
+	default:
+		if (f54->query.touch_controller_family == 1)
+			disable_cbc(reg_7);
+		else if (f54->query.has_ctrl88)
+			disable_cbc(reg_88);
+
+		if (f54->query.has_0d_acquisition_control)
+			disable_cbc(reg_57);
+
+		if ((f54->query.has_query15) &&
+				(f54->query_15.has_query25) &&
+				(f54->query_25.has_query27) &&
+				(f54->query_27.has_query29) &&
+				(f54->query_29.has_query30) &&
+				(f54->query_30.has_query32) &&
+				(f54->query_32.has_query33) &&
+				(f54->query_33.has_query36) &&
+				(f54->query_36.has_query38) &&
+				(f54->query_38.has_ctrl149)) {
+			retval = synaptics_rmi4_reg_write(rmi4_data,
+					f54->control.reg_149->address,
+					&zero,
+					sizeof(f54->control.reg_149->data));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to disable global CBC\n",
+						__func__);
+				return retval;
+			}
+		}
+
+		if (f54->query.has_signal_clarity) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					f54->control.reg_41->address,
+					&value,
+					sizeof(f54->control.reg_41->data));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to disable signal clarity\n",
+						__func__);
+				return retval;
+			}
+			value |= 0x01;
+			retval = synaptics_rmi4_reg_write(rmi4_data,
+					f54->control.reg_41->address,
+					&value,
+					sizeof(f54->control.reg_41->data));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to disable signal clarity\n",
+						__func__);
+				return retval;
+			}
+		}
+
+		retval = test_do_command(COMMAND_FORCE_UPDATE);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to do force update\n",
+					__func__);
+			return retval;
+		}
+
+		retval = test_do_command(COMMAND_FORCE_CAL);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to do force cal\n",
+					__func__);
+			return retval;
+		}
+	}
+
+	return 0;
+}
+
+static int test_do_afe_calibration(enum f54_afe_cal mode)
+{
+	int retval;
+	unsigned char timeout = CALIBRATION_TIMEOUT_S;
+	unsigned char timeout_count = 0;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f54->control.reg_188->address,
+			f54->control.reg_188->data,
+			sizeof(f54->control.reg_188->data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to start calibration\n",
+				__func__);
+		return retval;
+	}
+
+	if (mode == F54_AFE_CAL)
+		f54->control.reg_188->start_calibration = 1;
+	else if (mode == F54_AFE_IS_CAL)
+		f54->control.reg_188->start_is_calibration = 1;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			f54->control.reg_188->address,
+			f54->control.reg_188->data,
+			sizeof(f54->control.reg_188->data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to start calibration\n",
+				__func__);
+		return retval;
+	}
+
+	do {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->control.reg_188->address,
+				f54->control.reg_188->data,
+				sizeof(f54->control.reg_188->data));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to complete calibration\n",
+					__func__);
+			return retval;
+		}
+
+		if (mode == F54_AFE_CAL) {
+			if (!f54->control.reg_188->start_calibration)
+				break;
+		} else if (mode == F54_AFE_IS_CAL) {
+			if (!f54->control.reg_188->start_is_calibration)
+				break;
+		}
+
+		if (timeout_count == timeout) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Timed out waiting for calibration completion\n",
+					__func__);
+			return -EBUSY;
+		}
+
+		timeout_count++;
+		msleep(1000);
+	} while (true);
+
+	/* check CRC */
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f54->data_31.address,
+			f54->data_31.data,
+			sizeof(f54->data_31.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read calibration CRC\n",
+				__func__);
+		return retval;
+	}
+
+	if (mode == F54_AFE_CAL) {
+		if (f54->data_31.calibration_crc == 0)
+			return 0;
+	} else if (mode == F54_AFE_IS_CAL) {
+		if (f54->data_31.is_calibration_crc == 0)
+			return 0;
+	}
+
+	dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Failed to read calibration CRC\n",
+			__func__);
+
+	return -EINVAL;
+}
+
+static int test_check_for_idle_status(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	switch (f54->status) {
+	case STATUS_IDLE:
+		retval = 0;
+		break;
+	case STATUS_BUSY:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Status busy\n",
+				__func__);
+		retval = -EINVAL;
+		break;
+	case STATUS_ERROR:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Status error\n",
+				__func__);
+		retval = -EINVAL;
+		break;
+	default:
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid status (%d)\n",
+				__func__, f54->status);
+		retval = -EINVAL;
+	}
+
+	return retval;
+}
+
+static void test_timeout_work(struct work_struct *work)
+{
+	int retval;
+	unsigned char command;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	mutex_lock(&f54->status_mutex);
+
+	if (f54->status == STATUS_BUSY) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->command_base_addr,
+				&command,
+				sizeof(command));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read command register\n",
+					__func__);
+		} else if (command & COMMAND_GET_REPORT) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Report type not supported by FW\n",
+					__func__);
+		} else {
+			queue_work(f54->test_report_workqueue,
+					&f54->test_report_work);
+			goto exit;
+		}
+		f54->status = STATUS_ERROR;
+		f54->report_size = 0;
+	}
+
+exit:
+	mutex_unlock(&f54->status_mutex);
+}
+
+static enum hrtimer_restart test_get_report_timeout(struct hrtimer *timer)
+{
+	schedule_work(&(f54->timeout_work));
+
+	return HRTIMER_NORESTART;
+}
+
+static ssize_t num_of_mapped_tx_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", f54->tx_assigned);
+}
+
+static ssize_t num_of_mapped_rx_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", f54->rx_assigned);
+}
+
+static ssize_t tx_mapping_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int cnt;
+	int count = 0;
+	unsigned char ii;
+	unsigned char tx_num;
+	unsigned char tx_electrodes;
+
+	if (!f55)
+		return -EINVAL;
+
+	tx_electrodes = f55->query.num_of_tx_electrodes;
+
+	for (ii = 0; ii < tx_electrodes; ii++) {
+		tx_num = f55->tx_assignment[ii];
+		if (tx_num == 0xff)
+			cnt = snprintf(buf, PAGE_SIZE - count, "xx ");
+		else
+			cnt = snprintf(buf, PAGE_SIZE - count, "%02u ", tx_num);
+		buf += cnt;
+		count += cnt;
+	}
+
+	snprintf(buf, PAGE_SIZE - count, "\n");
+	count++;
+
+	return count;
+}
+
+static ssize_t rx_mapping_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int cnt;
+	int count = 0;
+	unsigned char ii;
+	unsigned char rx_num;
+	unsigned char rx_electrodes;
+
+	if (!f55)
+		return -EINVAL;
+
+	rx_electrodes = f55->query.num_of_rx_electrodes;
+
+	for (ii = 0; ii < rx_electrodes; ii++) {
+		rx_num = f55->rx_assignment[ii];
+		if (rx_num == 0xff)
+			cnt = snprintf(buf, PAGE_SIZE - count, "xx ");
+		else
+			cnt = snprintf(buf, PAGE_SIZE - count, "%02u ", rx_num);
+		buf += cnt;
+		count += cnt;
+	}
+
+	snprintf(buf, PAGE_SIZE - count, "\n");
+	count++;
+
+	return count;
+}
+
+static ssize_t num_of_mapped_force_tx_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", f21->tx_assigned);
+}
+
+static ssize_t num_of_mapped_force_rx_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", f21->rx_assigned);
+}
+
+static ssize_t force_tx_mapping_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int cnt;
+	int count = 0;
+	unsigned char ii;
+	unsigned char tx_num;
+	unsigned char tx_electrodes;
+
+	if ((!f55 || !f55->has_force) && (!f21 || !f21->has_force))
+		return -EINVAL;
+
+	if (f55->has_force) {
+		tx_electrodes = f55->query.num_of_tx_electrodes;
+
+		for (ii = 0; ii < tx_electrodes; ii++) {
+			tx_num = f55->force_tx_assignment[ii];
+			if (tx_num == 0xff) {
+				cnt = snprintf(buf, PAGE_SIZE - count, "xx ");
+			} else {
+				cnt = snprintf(buf, PAGE_SIZE - count, "%02u ",
+						tx_num);
+			}
+			buf += cnt;
+			count += cnt;
+		}
+	} else if (f21->has_force) {
+		tx_electrodes = f21->max_num_of_tx;
+
+		for (ii = 0; ii < tx_electrodes; ii++) {
+			tx_num = f21->force_txrx_assignment[ii];
+			if (tx_num == 0xff) {
+				cnt = snprintf(buf, PAGE_SIZE - count, "xx ");
+			} else {
+				cnt = snprintf(buf, PAGE_SIZE - count, "%02u ",
+						tx_num);
+			}
+			buf += cnt;
+			count += cnt;
+		}
+	}
+
+	snprintf(buf, PAGE_SIZE - count, "\n");
+	count++;
+
+	return count;
+}
+
+static ssize_t force_rx_mapping_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int cnt;
+	int count = 0;
+	unsigned char ii;
+	unsigned char offset;
+	unsigned char rx_num;
+	unsigned char rx_electrodes;
+
+	if ((!f55 || !f55->has_force) && (!f21 || !f21->has_force))
+		return -EINVAL;
+
+	if (f55->has_force) {
+		rx_electrodes = f55->query.num_of_rx_electrodes;
+
+		for (ii = 0; ii < rx_electrodes; ii++) {
+			rx_num = f55->force_rx_assignment[ii];
+			if (rx_num == 0xff)
+				cnt = snprintf(buf, PAGE_SIZE - count, "xx ");
+			else
+				cnt = snprintf(buf, PAGE_SIZE - count, "%02u ",
+						rx_num);
+			buf += cnt;
+			count += cnt;
+		}
+	} else if (f21->has_force) {
+		offset = f21->max_num_of_tx;
+		rx_electrodes = f21->max_num_of_rx;
+
+		for (ii = offset; ii < (rx_electrodes + offset); ii++) {
+			rx_num = f21->force_txrx_assignment[ii];
+			if (rx_num == 0xff)
+				cnt = snprintf(buf, PAGE_SIZE - count, "xx ");
+			else
+				cnt = snprintf(buf, PAGE_SIZE - count, "%02u ",
+						rx_num);
+			buf += cnt;
+			count += cnt;
+		}
+	}
+
+	snprintf(buf, PAGE_SIZE - count, "\n");
+	count++;
+
+	return count;
+}
+
+static ssize_t report_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", f54->report_size);
+}
+
+static ssize_t status_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+
+	mutex_lock(&f54->status_mutex);
+
+	retval = snprintf(buf, PAGE_SIZE, "%u\n", f54->status);
+
+	mutex_unlock(&f54->status_mutex);
+
+	return retval;
+}
+
+static ssize_t do_preparation_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	if (setting != 1)
+		return -EINVAL;
+
+	mutex_lock(&f54->status_mutex);
+
+	retval = test_check_for_idle_status();
+	if (retval < 0)
+		goto exit;
+
+	retval = test_do_preparation();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to do preparation\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	mutex_unlock(&f54->status_mutex);
+
+	return retval;
+}
+
+static ssize_t force_cal_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	if (setting != 1)
+		return -EINVAL;
+
+	mutex_lock(&f54->status_mutex);
+
+	retval = test_check_for_idle_status();
+	if (retval < 0)
+		goto exit;
+
+	retval = test_do_command(COMMAND_FORCE_CAL);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to do force cal\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	mutex_unlock(&f54->status_mutex);
+
+	return retval;
+}
+
+static ssize_t get_report_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned char command;
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	if (setting != 1)
+		return -EINVAL;
+
+	mutex_lock(&f54->status_mutex);
+
+	retval = test_check_for_idle_status();
+	if (retval < 0)
+		goto exit;
+
+	if (!test_report_type_valid(f54->report_type)) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Invalid report type\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	test_set_interrupt(true);
+
+	command = (unsigned char)COMMAND_GET_REPORT;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			f54->command_base_addr,
+			&command,
+			sizeof(command));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write get report command\n",
+				__func__);
+		goto exit;
+	}
+
+	f54->status = STATUS_BUSY;
+	f54->report_size = 0;
+	f54->data_pos = 0;
+
+	hrtimer_start(&f54->watchdog,
+			ktime_set(GET_REPORT_TIMEOUT_S, 0),
+			HRTIMER_MODE_REL);
+
+	retval = count;
+
+exit:
+	mutex_unlock(&f54->status_mutex);
+
+	return retval;
+}
+
+static ssize_t resume_touch_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned char device_ctrl;
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	if (setting != 1)
+		return -EINVAL;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to restore no sleep setting\n",
+				__func__);
+		return retval;
+	}
+
+	device_ctrl = device_ctrl & ~NO_SLEEP_ON;
+	device_ctrl |= rmi4_data->no_sleep_setting;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			rmi4_data->f01_ctrl_base_addr,
+			&device_ctrl,
+			sizeof(device_ctrl));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to restore no sleep setting\n",
+				__func__);
+		return retval;
+	}
+
+	test_set_interrupt(false);
+
+	if (f54->skip_preparation)
+		return count;
+
+	switch (f54->report_type) {
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_SENSOR_SPEED:
+	case F54_ADC_RANGE:
+	case F54_ABS_RAW_CAP:
+	case F54_ABS_DELTA_CAP:
+	case F54_ABS_HYBRID_DELTA_CAP:
+	case F54_ABS_HYBRID_RAW_CAP:
+	case F54_FULL_RAW_CAP_TDDI:
+		break;
+	case F54_AMP_RAW_ADC:
+		if (f54->query_49.has_ctrl188) {
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					f54->control.reg_188->address,
+					f54->control.reg_188->data,
+					sizeof(f54->control.reg_188->data));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to set start production test\n",
+						__func__);
+				return retval;
+			}
+			f54->control.reg_188->start_production_test = 0;
+			retval = synaptics_rmi4_reg_write(rmi4_data,
+					f54->control.reg_188->address,
+					f54->control.reg_188->data,
+					sizeof(f54->control.reg_188->data));
+			if (retval < 0) {
+				dev_err(rmi4_data->pdev->dev.parent,
+						"%s: Failed to set start production test\n",
+						__func__);
+				return retval;
+			}
+		}
+		break;
+	default:
+		rmi4_data->reset_device(rmi4_data, false);
+	}
+
+	return count;
+}
+
+static ssize_t do_afe_calibration_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	if (!f54->query_49.has_ctrl188) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: F54_ANALOG_Ctrl188 not found\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (setting == 0 || setting == 1)
+		retval = test_do_afe_calibration((enum f54_afe_cal)setting);
+	else
+		return -EINVAL;
+
+	if (retval)
+		return retval;
+	else
+		return count;
+}
+
+static ssize_t report_type_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", f54->report_type);
+}
+
+static ssize_t report_type_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned char data;
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	mutex_lock(&f54->status_mutex);
+
+	retval = test_check_for_idle_status();
+	if (retval < 0)
+		goto exit;
+
+	if (!test_report_type_valid((enum f54_report_types)setting)) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Report type not supported by driver\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	f54->report_type = (enum f54_report_types)setting;
+	data = (unsigned char)setting;
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			f54->data_base_addr,
+			&data,
+			sizeof(data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write report type\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = count;
+
+exit:
+	mutex_unlock(&f54->status_mutex);
+
+	return retval;
+}
+
+static ssize_t fifoindex_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int retval;
+	unsigned char data[2];
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f54->data_base_addr + REPORT_INDEX_OFFSET,
+			data,
+			sizeof(data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read report index\n",
+				__func__);
+		return retval;
+	}
+
+	batohs(&f54->fifoindex, data);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", f54->fifoindex);
+}
+
+static ssize_t fifoindex_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned char data[2];
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	f54->fifoindex = setting;
+
+	hstoba(data, (unsigned short)setting);
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			f54->data_base_addr + REPORT_INDEX_OFFSET,
+			data,
+			sizeof(data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write report index\n",
+				__func__);
+		return retval;
+	}
+
+	return count;
+}
+
+static ssize_t no_auto_cal_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%u\n", f54->no_auto_cal);
+}
+
+static ssize_t no_auto_cal_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned char data;
+	unsigned long setting;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = sstrtoul(buf, 10, &setting);
+	if (retval)
+		return retval;
+
+	if (setting > 1)
+		return -EINVAL;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f54->control_base_addr,
+			&data,
+			sizeof(data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read no auto cal setting\n",
+				__func__);
+		return retval;
+	}
+
+	if (setting)
+		data |= CONTROL_NO_AUTO_CAL;
+	else
+		data &= ~CONTROL_NO_AUTO_CAL;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			f54->control_base_addr,
+			&data,
+			sizeof(data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write no auto cal setting\n",
+				__func__);
+		return retval;
+	}
+
+	f54->no_auto_cal = (setting == 1);
+
+	return count;
+}
+
+static ssize_t read_report_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	unsigned int ii;
+	unsigned int jj;
+	int cnt;
+	int count = 0;
+	int tx_num = f54->tx_assigned;
+	int rx_num = f54->rx_assigned;
+	char *report_data_8;
+	short *report_data_16;
+	int *report_data_32;
+	unsigned short *report_data_u16;
+	unsigned int *report_data_u32;
+
+	switch (f54->report_type) {
+	case F54_8BIT_IMAGE:
+		report_data_8 = (char *)f54->report_data;
+		for (ii = 0; ii < f54->report_size; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "%03d: %d\n",
+					ii, *report_data_8);
+			report_data_8++;
+			buf += cnt;
+			count += cnt;
+		}
+		break;
+	case F54_AMP_RAW_ADC:
+		report_data_u16 = (unsigned short *)f54->report_data;
+		cnt = snprintf(buf, PAGE_SIZE - count, "tx = %d\nrx = %d\n",
+				tx_num, rx_num);
+		buf += cnt;
+		count += cnt;
+
+		for (ii = 0; ii < tx_num; ii++) {
+			for (jj = 0; jj < (rx_num - 1); jj++) {
+				cnt = snprintf(buf, PAGE_SIZE - count, "%-4d ",
+						*report_data_u16);
+				report_data_u16++;
+				buf += cnt;
+				count += cnt;
+			}
+			cnt = snprintf(buf, PAGE_SIZE - count, "%-4d\n",
+					*report_data_u16);
+			report_data_u16++;
+			buf += cnt;
+			count += cnt;
+		}
+		break;
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_NO_RX_COUPLING:
+	case F54_SENSOR_SPEED:
+	case F54_AMP_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_TDDI:
+		report_data_16 = (short *)f54->report_data;
+		cnt = snprintf(buf, PAGE_SIZE - count, "tx = %d\nrx = %d\n",
+				tx_num, rx_num);
+		buf += cnt;
+		count += cnt;
+
+		for (ii = 0; ii < tx_num; ii++) {
+			for (jj = 0; jj < (rx_num - 1); jj++) {
+				cnt = snprintf(buf, PAGE_SIZE - count, "%-4d ",
+						*report_data_16);
+				report_data_16++;
+				buf += cnt;
+				count += cnt;
+			}
+			cnt = snprintf(buf, PAGE_SIZE - count, "%-4d\n",
+					*report_data_16);
+			report_data_16++;
+			buf += cnt;
+			count += cnt;
+		}
+		break;
+	case F54_HIGH_RESISTANCE:
+	case F54_FULL_RAW_CAP_MIN_MAX:
+		report_data_16 = (short *)f54->report_data;
+		for (ii = 0; ii < f54->report_size; ii += 2) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "%03d: %d\n",
+					ii / 2, *report_data_16);
+			report_data_16++;
+			buf += cnt;
+			count += cnt;
+		}
+		break;
+	case F54_ABS_RAW_CAP:
+	case F54_ABS_HYBRID_RAW_CAP:
+		tx_num += f21->tx_assigned;
+		rx_num += f21->rx_assigned;
+		report_data_u32 = (unsigned int *)f54->report_data;
+		cnt = snprintf(buf, PAGE_SIZE - count, "rx ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < rx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "     %2d", ii);
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+
+		cnt = snprintf(buf, PAGE_SIZE - count, "   ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < rx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "  %5u",
+					*report_data_u32);
+			report_data_u32++;
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+
+		cnt = snprintf(buf, PAGE_SIZE - count, "tx ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < tx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "     %2d", ii);
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+
+		cnt = snprintf(buf, PAGE_SIZE - count, "   ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < tx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "  %5u",
+					*report_data_u32);
+			report_data_u32++;
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+		break;
+	case F54_ABS_DELTA_CAP:
+	case F54_ABS_HYBRID_DELTA_CAP:
+		tx_num += f21->tx_assigned;
+		rx_num += f21->rx_assigned;
+		report_data_32 = (int *)f54->report_data;
+		cnt = snprintf(buf, PAGE_SIZE - count, "rx ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < rx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "     %2d", ii);
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+
+		cnt = snprintf(buf, PAGE_SIZE - count, "   ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < rx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "  %5d",
+					*report_data_32);
+			report_data_32++;
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+
+		cnt = snprintf(buf, PAGE_SIZE - count, "tx ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < tx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "     %2d", ii);
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+
+		cnt = snprintf(buf, PAGE_SIZE - count, "   ");
+		buf += cnt;
+		count += cnt;
+		for (ii = 0; ii < tx_num; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "  %5d",
+					*report_data_32);
+			report_data_32++;
+			buf += cnt;
+			count += cnt;
+		}
+		cnt = snprintf(buf, PAGE_SIZE - count, "\n");
+		buf += cnt;
+		count += cnt;
+		break;
+	default:
+		for (ii = 0; ii < f54->report_size; ii++) {
+			cnt = snprintf(buf, PAGE_SIZE - count, "%03d: 0x%02x\n",
+					ii, f54->report_data[ii]);
+			buf += cnt;
+			count += cnt;
+		}
+	}
+
+	snprintf(buf, PAGE_SIZE - count, "\n");
+	count++;
+
+	return count;
+}
+
+static ssize_t read_report_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned char timeout = GET_REPORT_TIMEOUT_S * 10;
+	unsigned char timeout_count;
+	const char cmd[] = {'1', 0};
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = report_type_store(dev, attr, buf, count);
+	if (retval < 0)
+		goto exit;
+
+	retval = do_preparation_store(dev, attr, cmd, 1);
+	if (retval < 0)
+		goto exit;
+
+	retval = get_report_store(dev, attr, cmd, 1);
+	if (retval < 0)
+		goto exit;
+
+	timeout_count = 0;
+	do {
+		if (f54->status != STATUS_BUSY)
+			break;
+		msleep(100);
+		timeout_count++;
+	} while (timeout_count < timeout);
+
+	if ((f54->status != STATUS_IDLE) || (f54->report_size == 0)) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read report\n",
+				__func__);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	retval = resume_touch_store(dev, attr, cmd, 1);
+	if (retval < 0)
+		goto exit;
+
+	return count;
+
+exit:
+	rmi4_data->reset_device(rmi4_data, false);
+
+	return retval;
+}
+
+static ssize_t test_sysfs_data_read(struct file *data_file,
+		struct kobject *kobj, struct bin_attribute *attributes,
+		char *buf, loff_t pos, size_t count)
+{
+	int retval;
+	unsigned int read_size;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	mutex_lock(&f54->status_mutex);
+
+	retval = test_check_for_idle_status();
+	if (retval < 0)
+		goto exit;
+
+	if (!f54->report_data) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Report type %d data not available\n",
+				__func__, f54->report_type);
+		retval = -EINVAL;
+		goto exit;
+	}
+
+	if ((f54->data_pos + count) > f54->report_size)
+		read_size = f54->report_size - f54->data_pos;
+	else
+		read_size = min_t(unsigned int, count, f54->report_size);
+
+	retval = secure_memcpy(buf, count, f54->report_data + f54->data_pos,
+			f54->data_buffer_size - f54->data_pos, read_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to copy report data\n",
+				__func__);
+		goto exit;
+	}
+	f54->data_pos += read_size;
+	retval = read_size;
+
+exit:
+	mutex_unlock(&f54->status_mutex);
+
+	return retval;
+}
+
+static void test_report_work(struct work_struct *work)
+{
+	int retval;
+	unsigned char report_index[2];
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	mutex_lock(&f54->status_mutex);
+
+	if (f54->status != STATUS_BUSY) {
+		retval = f54->status;
+		goto exit;
+	}
+
+	retval = test_wait_for_command_completion();
+	if (retval < 0) {
+		retval = STATUS_ERROR;
+		goto exit;
+	}
+
+	test_set_report_size();
+	if (f54->report_size == 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Report data size = 0\n",
+				__func__);
+		retval = STATUS_ERROR;
+		goto exit;
+	}
+
+	if (f54->data_buffer_size < f54->report_size) {
+		if (f54->data_buffer_size)
+			kfree(f54->report_data);
+		f54->report_data = kzalloc(f54->report_size, GFP_KERNEL);
+		if (!f54->report_data) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to alloc mem for data buffer\n",
+					__func__);
+			f54->data_buffer_size = 0;
+			retval = STATUS_ERROR;
+			goto exit;
+		}
+		f54->data_buffer_size = f54->report_size;
+	}
+
+	report_index[0] = 0;
+	report_index[1] = 0;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			f54->data_base_addr + REPORT_INDEX_OFFSET,
+			report_index,
+			sizeof(report_index));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to write report data index\n",
+				__func__);
+		retval = STATUS_ERROR;
+		goto exit;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f54->data_base_addr + REPORT_DATA_OFFSET,
+			f54->report_data,
+			f54->report_size);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read report data\n",
+				__func__);
+		retval = STATUS_ERROR;
+		goto exit;
+	}
+
+	retval = STATUS_IDLE;
+
+exit:
+	mutex_unlock(&f54->status_mutex);
+
+	if (retval == STATUS_ERROR)
+		f54->report_size = 0;
+
+	f54->status = retval;
+}
+
+static void test_remove_sysfs(void)
+{
+	sysfs_remove_group(f54->sysfs_dir, &attr_group);
+	sysfs_remove_bin_file(f54->sysfs_dir, &test_report_data);
+	kobject_put(f54->sysfs_dir);
+}
+
+static int test_set_sysfs(void)
+{
+	int retval;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	f54->sysfs_dir = kobject_create_and_add(SYSFS_FOLDER_NAME,
+			&rmi4_data->input_dev->dev.kobj);
+	if (!f54->sysfs_dir) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs directory\n",
+				__func__);
+		goto exit_directory;
+	}
+
+	retval = sysfs_create_bin_file(f54->sysfs_dir, &test_report_data);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs bin file\n",
+				__func__);
+		goto exit_bin_file;
+	}
+
+	retval = sysfs_create_group(f54->sysfs_dir, &attr_group);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs attributes\n",
+				__func__);
+		goto exit_attributes;
+	}
+
+	return 0;
+
+exit_attributes:
+	sysfs_remove_group(f54->sysfs_dir, &attr_group);
+	sysfs_remove_bin_file(f54->sysfs_dir, &test_report_data);
+
+exit_bin_file:
+	kobject_put(f54->sysfs_dir);
+
+exit_directory:
+	return -ENODEV;
+}
+
+static void test_free_control_mem(void)
+{
+	struct f54_control control = f54->control;
+
+	kfree(control.reg_7);
+	kfree(control.reg_41);
+	kfree(control.reg_57);
+	kfree(control.reg_86);
+	kfree(control.reg_88);
+	kfree(control.reg_110);
+	kfree(control.reg_149);
+	kfree(control.reg_188);
+}
+
+static void test_set_data(void)
+{
+	unsigned short reg_addr;
+
+	reg_addr = f54->data_base_addr + REPORT_DATA_OFFSET + 1;
+
+	/* data 4 */
+	if (f54->query.has_sense_frequency_control)
+		reg_addr++;
+
+	/* data 5 reserved */
+
+	/* data 6 */
+	if (f54->query.has_interference_metric)
+		reg_addr += 2;
+
+	/* data 7 */
+	if (f54->query.has_one_byte_report_rate |
+			f54->query.has_two_byte_report_rate)
+		reg_addr++;
+	if (f54->query.has_two_byte_report_rate)
+		reg_addr++;
+
+	/* data 8 */
+	if (f54->query.has_variance_metric)
+		reg_addr += 2;
+
+	/* data 9 */
+	if (f54->query.has_multi_metric_state_machine)
+		reg_addr += 2;
+
+	/* data 10 */
+	if (f54->query.has_multi_metric_state_machine |
+			f54->query.has_noise_state)
+		reg_addr++;
+
+	/* data 11 */
+	if (f54->query.has_status)
+		reg_addr++;
+
+	/* data 12 */
+	if (f54->query.has_slew_metric)
+		reg_addr += 2;
+
+	/* data 13 */
+	if (f54->query.has_multi_metric_state_machine)
+		reg_addr += 2;
+
+	/* data 14 */
+	if (f54->query_13.has_cidim)
+		reg_addr++;
+
+	/* data 15 */
+	if (f54->query_13.has_rail_im)
+		reg_addr++;
+
+	/* data 16 */
+	if (f54->query_13.has_noise_mitigation_enhancement)
+		reg_addr++;
+
+	/* data 17 */
+	if (f54->query_16.has_data17)
+		reg_addr++;
+
+	/* data 18 */
+	if (f54->query_21.has_query24_data18)
+		reg_addr++;
+
+	/* data 19 */
+	if (f54->query_21.has_data19)
+		reg_addr++;
+
+	/* data_20 */
+	if (f54->query_25.has_ctrl109)
+		reg_addr++;
+
+	/* data 21 */
+	if (f54->query_27.has_data21)
+		reg_addr++;
+
+	/* data 22 */
+	if (f54->query_27.has_data22)
+		reg_addr++;
+
+	/* data 23 */
+	if (f54->query_29.has_data23)
+		reg_addr++;
+
+	/* data 24 */
+	if (f54->query_32.has_data24)
+		reg_addr++;
+
+	/* data 25 */
+	if (f54->query_35.has_data25)
+		reg_addr++;
+
+	/* data 26 */
+	if (f54->query_35.has_data26)
+		reg_addr++;
+
+	/* data 27 */
+	if (f54->query_46.has_data27)
+		reg_addr++;
+
+	/* data 28 */
+	if (f54->query_46.has_data28)
+		reg_addr++;
+
+	/* data 29 30 reserved */
+
+	/* data 31 */
+	if (f54->query_49.has_data31) {
+		f54->data_31.address = reg_addr;
+		reg_addr++;
+	}
+}
+
+static int test_set_controls(void)
+{
+	int retval;
+	unsigned char length = 0;
+	unsigned char num_of_sensing_freqs;
+	unsigned short reg_addr = f54->control_base_addr;
+	struct f54_control *control = &f54->control;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	num_of_sensing_freqs = f54->query.number_of_sensing_frequencies;
+
+	/* control 0 */
+	reg_addr += CONTROL_0_SIZE;
+
+	/* control 1 */
+	if ((f54->query.touch_controller_family == 0) ||
+			(f54->query.touch_controller_family == 1))
+		reg_addr += CONTROL_1_SIZE;
+
+	/* control 2 */
+	reg_addr += CONTROL_2_SIZE;
+
+	/* control 3 */
+	if (f54->query.has_pixel_touch_threshold_adjustment)
+		reg_addr += CONTROL_3_SIZE;
+
+	/* controls 4 5 6 */
+	if ((f54->query.touch_controller_family == 0) ||
+			(f54->query.touch_controller_family == 1))
+		reg_addr += CONTROL_4_6_SIZE;
+
+	/* control 7 */
+	if (f54->query.touch_controller_family == 1) {
+		control->reg_7 = kzalloc(sizeof(*(control->reg_7)),
+				GFP_KERNEL);
+		if (!control->reg_7)
+			goto exit_no_mem;
+		control->reg_7->address = reg_addr;
+		reg_addr += CONTROL_7_SIZE;
+	}
+
+	/* controls 8 9 */
+	if ((f54->query.touch_controller_family == 0) ||
+			(f54->query.touch_controller_family == 1))
+		reg_addr += CONTROL_8_9_SIZE;
+
+	/* control 10 */
+	if (f54->query.has_interference_metric)
+		reg_addr += CONTROL_10_SIZE;
+
+	/* control 11 */
+	if (f54->query.has_ctrl11)
+		reg_addr += CONTROL_11_SIZE;
+
+	/* controls 12 13 */
+	if (f54->query.has_relaxation_control)
+		reg_addr += CONTROL_12_13_SIZE;
+
+	/* controls 14 15 16 */
+	if (f54->query.has_sensor_assignment) {
+		reg_addr += CONTROL_14_SIZE;
+		reg_addr += CONTROL_15_SIZE * f54->query.num_of_rx_electrodes;
+		reg_addr += CONTROL_16_SIZE * f54->query.num_of_tx_electrodes;
+	}
+
+	/* controls 17 18 19 */
+	if (f54->query.has_sense_frequency_control) {
+		reg_addr += CONTROL_17_SIZE * num_of_sensing_freqs;
+		reg_addr += CONTROL_18_SIZE * num_of_sensing_freqs;
+		reg_addr += CONTROL_19_SIZE * num_of_sensing_freqs;
+	}
+
+	/* control 20 */
+	reg_addr += CONTROL_20_SIZE;
+
+	/* control 21 */
+	if (f54->query.has_sense_frequency_control)
+		reg_addr += CONTROL_21_SIZE;
+
+	/* controls 22 23 24 25 26 */
+	if (f54->query.has_firmware_noise_mitigation)
+		reg_addr += CONTROL_22_26_SIZE;
+
+	/* control 27 */
+	if (f54->query.has_iir_filter)
+		reg_addr += CONTROL_27_SIZE;
+
+	/* control 28 */
+	if (f54->query.has_firmware_noise_mitigation)
+		reg_addr += CONTROL_28_SIZE;
+
+	/* control 29 */
+	if (f54->query.has_cmn_removal)
+		reg_addr += CONTROL_29_SIZE;
+
+	/* control 30 */
+	if (f54->query.has_cmn_maximum)
+		reg_addr += CONTROL_30_SIZE;
+
+	/* control 31 */
+	if (f54->query.has_touch_hysteresis)
+		reg_addr += CONTROL_31_SIZE;
+
+	/* controls 32 33 34 35 */
+	if (f54->query.has_edge_compensation)
+		reg_addr += CONTROL_32_35_SIZE;
+
+	/* control 36 */
+	if ((f54->query.curve_compensation_mode == 1) ||
+			(f54->query.curve_compensation_mode == 2)) {
+		if (f54->query.curve_compensation_mode == 1) {
+			length = max(f54->query.num_of_rx_electrodes,
+					f54->query.num_of_tx_electrodes);
+		} else if (f54->query.curve_compensation_mode == 2) {
+			length = f54->query.num_of_rx_electrodes;
+		}
+		reg_addr += CONTROL_36_SIZE * length;
+	}
+
+	/* control 37 */
+	if (f54->query.curve_compensation_mode == 2)
+		reg_addr += CONTROL_37_SIZE * f54->query.num_of_tx_electrodes;
+
+	/* controls 38 39 40 */
+	if (f54->query.has_per_frequency_noise_control) {
+		reg_addr += CONTROL_38_SIZE * num_of_sensing_freqs;
+		reg_addr += CONTROL_39_SIZE * num_of_sensing_freqs;
+		reg_addr += CONTROL_40_SIZE * num_of_sensing_freqs;
+	}
+
+	/* control 41 */
+	if (f54->query.has_signal_clarity) {
+		control->reg_41 = kzalloc(sizeof(*(control->reg_41)),
+				GFP_KERNEL);
+		if (!control->reg_41)
+			goto exit_no_mem;
+		control->reg_41->address = reg_addr;
+		reg_addr += CONTROL_41_SIZE;
+	}
+
+	/* control 42 */
+	if (f54->query.has_variance_metric)
+		reg_addr += CONTROL_42_SIZE;
+
+	/* controls 43 44 45 46 47 48 49 50 51 52 53 54 */
+	if (f54->query.has_multi_metric_state_machine)
+		reg_addr += CONTROL_43_54_SIZE;
+
+	/* controls 55 56 */
+	if (f54->query.has_0d_relaxation_control)
+		reg_addr += CONTROL_55_56_SIZE;
+
+	/* control 57 */
+	if (f54->query.has_0d_acquisition_control) {
+		control->reg_57 = kzalloc(sizeof(*(control->reg_57)),
+				GFP_KERNEL);
+		if (!control->reg_57)
+			goto exit_no_mem;
+		control->reg_57->address = reg_addr;
+		reg_addr += CONTROL_57_SIZE;
+	}
+
+	/* control 58 */
+	if (f54->query.has_0d_acquisition_control)
+		reg_addr += CONTROL_58_SIZE;
+
+	/* control 59 */
+	if (f54->query.has_h_blank)
+		reg_addr += CONTROL_59_SIZE;
+
+	/* controls 60 61 62 */
+	if ((f54->query.has_h_blank) ||
+			(f54->query.has_v_blank) ||
+			(f54->query.has_long_h_blank))
+		reg_addr += CONTROL_60_62_SIZE;
+
+	/* control 63 */
+	if ((f54->query.has_h_blank) ||
+			(f54->query.has_v_blank) ||
+			(f54->query.has_long_h_blank) ||
+			(f54->query.has_slew_metric) ||
+			(f54->query.has_slew_option) ||
+			(f54->query.has_noise_mitigation2))
+		reg_addr += CONTROL_63_SIZE;
+
+	/* controls 64 65 66 67 */
+	if (f54->query.has_h_blank)
+		reg_addr += CONTROL_64_67_SIZE * 7;
+	else if ((f54->query.has_v_blank) ||
+			(f54->query.has_long_h_blank))
+		reg_addr += CONTROL_64_67_SIZE;
+
+	/* controls 68 69 70 71 72 73 */
+	if ((f54->query.has_h_blank) ||
+			(f54->query.has_v_blank) ||
+			(f54->query.has_long_h_blank)) {
+		if (f54->query_68.is_tddi_hic)
+			reg_addr += CONTROL_70_73_SIZE;
+		else
+			reg_addr += CONTROL_68_73_SIZE;
+	}
+
+	/* control 74 */
+	if (f54->query.has_slew_metric)
+		reg_addr += CONTROL_74_SIZE;
+
+	/* control 75 */
+	if (f54->query.has_enhanced_stretch)
+		reg_addr += CONTROL_75_SIZE * num_of_sensing_freqs;
+
+	/* control 76 */
+	if (f54->query.has_startup_fast_relaxation)
+		reg_addr += CONTROL_76_SIZE;
+
+	/* controls 77 78 */
+	if (f54->query.has_esd_control)
+		reg_addr += CONTROL_77_78_SIZE;
+
+	/* controls 79 80 81 82 83 */
+	if (f54->query.has_noise_mitigation2)
+		reg_addr += CONTROL_79_83_SIZE;
+
+	/* controls 84 85 */
+	if (f54->query.has_energy_ratio_relaxation)
+		reg_addr += CONTROL_84_85_SIZE;
+
+	/* control 86 */
+	if (f54->query_13.has_ctrl86) {
+		control->reg_86 = kzalloc(sizeof(*(control->reg_86)),
+				GFP_KERNEL);
+		if (!control->reg_86)
+			goto exit_no_mem;
+		control->reg_86->address = reg_addr;
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->control.reg_86->address,
+				f54->control.reg_86->data,
+				sizeof(f54->control.reg_86->data));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read sense display ratio\n",
+					__func__);
+			return retval;
+		}
+		reg_addr += CONTROL_86_SIZE;
+	}
+
+	/* control 87 */
+	if (f54->query_13.has_ctrl87)
+		reg_addr += CONTROL_87_SIZE;
+
+	/* control 88 */
+	if (f54->query.has_ctrl88) {
+		control->reg_88 = kzalloc(sizeof(*(control->reg_88)),
+				GFP_KERNEL);
+		if (!control->reg_88)
+			goto exit_no_mem;
+		control->reg_88->address = reg_addr;
+		reg_addr += CONTROL_88_SIZE;
+	}
+
+	/* control 89 */
+	if (f54->query_13.has_cidim ||
+			f54->query_13.has_noise_mitigation_enhancement ||
+			f54->query_13.has_rail_im)
+		reg_addr += CONTROL_89_SIZE;
+
+	/* control 90 */
+	if (f54->query_15.has_ctrl90)
+		reg_addr += CONTROL_90_SIZE;
+
+	/* control 91 */
+	if (f54->query_21.has_ctrl91)
+		reg_addr += CONTROL_91_SIZE;
+
+	/* control 92 */
+	if (f54->query_16.has_ctrl92)
+		reg_addr += CONTROL_92_SIZE;
+
+	/* control 93 */
+	if (f54->query_16.has_ctrl93)
+		reg_addr += CONTROL_93_SIZE;
+
+	/* control 94 */
+	if (f54->query_16.has_ctrl94_query18)
+		reg_addr += CONTROL_94_SIZE;
+
+	/* control 95 */
+	if (f54->query_16.has_ctrl95_query19)
+		reg_addr += CONTROL_95_SIZE;
+
+	/* control 96 */
+	if (f54->query_21.has_ctrl96)
+		reg_addr += CONTROL_96_SIZE;
+
+	/* control 97 */
+	if (f54->query_21.has_ctrl97)
+		reg_addr += CONTROL_97_SIZE;
+
+	/* control 98 */
+	if (f54->query_21.has_ctrl98)
+		reg_addr += CONTROL_98_SIZE;
+
+	/* control 99 */
+	if (f54->query.touch_controller_family == 2)
+		reg_addr += CONTROL_99_SIZE;
+
+	/* control 100 */
+	if (f54->query_16.has_ctrl100)
+		reg_addr += CONTROL_100_SIZE;
+
+	/* control 101 */
+	if (f54->query_22.has_ctrl101)
+		reg_addr += CONTROL_101_SIZE;
+
+	/* control 102 */
+	if (f54->query_23.has_ctrl102)
+		reg_addr += CONTROL_102_SIZE;
+
+	/* control 103 */
+	if (f54->query_22.has_ctrl103_query26) {
+		f54->skip_preparation = true;
+		reg_addr += CONTROL_103_SIZE;
+	}
+
+	/* control 104 */
+	if (f54->query_22.has_ctrl104)
+		reg_addr += CONTROL_104_SIZE;
+
+	/* control 105 */
+	if (f54->query_22.has_ctrl105)
+		reg_addr += CONTROL_105_SIZE;
+
+	/* control 106 */
+	if (f54->query_25.has_ctrl106)
+		reg_addr += CONTROL_106_SIZE;
+
+	/* control 107 */
+	if (f54->query_25.has_ctrl107)
+		reg_addr += CONTROL_107_SIZE;
+
+	/* control 108 */
+	if (f54->query_25.has_ctrl108)
+		reg_addr += CONTROL_108_SIZE;
+
+	/* control 109 */
+	if (f54->query_25.has_ctrl109)
+		reg_addr += CONTROL_109_SIZE;
+
+	/* control 110 */
+	if (f54->query_27.has_ctrl110) {
+		control->reg_110 = kzalloc(sizeof(*(control->reg_110)),
+				GFP_KERNEL);
+		if (!control->reg_110)
+			goto exit_no_mem;
+		control->reg_110->address = reg_addr;
+		reg_addr += CONTROL_110_SIZE;
+	}
+
+	/* control 111 */
+	if (f54->query_27.has_ctrl111)
+		reg_addr += CONTROL_111_SIZE;
+
+	/* control 112 */
+	if (f54->query_27.has_ctrl112)
+		reg_addr += CONTROL_112_SIZE;
+
+	/* control 113 */
+	if (f54->query_27.has_ctrl113)
+		reg_addr += CONTROL_113_SIZE;
+
+	/* control 114 */
+	if (f54->query_27.has_ctrl114)
+		reg_addr += CONTROL_114_SIZE;
+
+	/* control 115 */
+	if (f54->query_29.has_ctrl115)
+		reg_addr += CONTROL_115_SIZE;
+
+	/* control 116 */
+	if (f54->query_29.has_ctrl116)
+		reg_addr += CONTROL_116_SIZE;
+
+	/* control 117 */
+	if (f54->query_29.has_ctrl117)
+		reg_addr += CONTROL_117_SIZE;
+
+	/* control 118 */
+	if (f54->query_30.has_ctrl118)
+		reg_addr += CONTROL_118_SIZE;
+
+	/* control 119 */
+	if (f54->query_30.has_ctrl119)
+		reg_addr += CONTROL_119_SIZE;
+
+	/* control 120 */
+	if (f54->query_30.has_ctrl120)
+		reg_addr += CONTROL_120_SIZE;
+
+	/* control 121 */
+	if (f54->query_30.has_ctrl121)
+		reg_addr += CONTROL_121_SIZE;
+
+	/* control 122 */
+	if (f54->query_30.has_ctrl122_query31)
+		reg_addr += CONTROL_122_SIZE;
+
+	/* control 123 */
+	if (f54->query_30.has_ctrl123)
+		reg_addr += CONTROL_123_SIZE;
+
+	/* control 124 */
+	if (f54->query_30.has_ctrl124)
+		reg_addr += CONTROL_124_SIZE;
+
+	/* control 125 */
+	if (f54->query_32.has_ctrl125)
+		reg_addr += CONTROL_125_SIZE;
+
+	/* control 126 */
+	if (f54->query_32.has_ctrl126)
+		reg_addr += CONTROL_126_SIZE;
+
+	/* control 127 */
+	if (f54->query_32.has_ctrl127)
+		reg_addr += CONTROL_127_SIZE;
+
+	/* control 128 */
+	if (f54->query_33.has_ctrl128)
+		reg_addr += CONTROL_128_SIZE;
+
+	/* control 129 */
+	if (f54->query_33.has_ctrl129)
+		reg_addr += CONTROL_129_SIZE;
+
+	/* control 130 */
+	if (f54->query_33.has_ctrl130)
+		reg_addr += CONTROL_130_SIZE;
+
+	/* control 131 */
+	if (f54->query_33.has_ctrl131)
+		reg_addr += CONTROL_131_SIZE;
+
+	/* control 132 */
+	if (f54->query_33.has_ctrl132)
+		reg_addr += CONTROL_132_SIZE;
+
+	/* control 133 */
+	if (f54->query_33.has_ctrl133)
+		reg_addr += CONTROL_133_SIZE;
+
+	/* control 134 */
+	if (f54->query_33.has_ctrl134)
+		reg_addr += CONTROL_134_SIZE;
+
+	/* control 135 */
+	if (f54->query_35.has_ctrl135)
+		reg_addr += CONTROL_135_SIZE;
+
+	/* control 136 */
+	if (f54->query_35.has_ctrl136)
+		reg_addr += CONTROL_136_SIZE;
+
+	/* control 137 */
+	if (f54->query_35.has_ctrl137)
+		reg_addr += CONTROL_137_SIZE;
+
+	/* control 138 */
+	if (f54->query_35.has_ctrl138)
+		reg_addr += CONTROL_138_SIZE;
+
+	/* control 139 */
+	if (f54->query_35.has_ctrl139)
+		reg_addr += CONTROL_139_SIZE;
+
+	/* control 140 */
+	if (f54->query_35.has_ctrl140)
+		reg_addr += CONTROL_140_SIZE;
+
+	/* control 141 */
+	if (f54->query_36.has_ctrl141)
+		reg_addr += CONTROL_141_SIZE;
+
+	/* control 142 */
+	if (f54->query_36.has_ctrl142)
+		reg_addr += CONTROL_142_SIZE;
+
+	/* control 143 */
+	if (f54->query_36.has_ctrl143)
+		reg_addr += CONTROL_143_SIZE;
+
+	/* control 144 */
+	if (f54->query_36.has_ctrl144)
+		reg_addr += CONTROL_144_SIZE;
+
+	/* control 145 */
+	if (f54->query_36.has_ctrl145)
+		reg_addr += CONTROL_145_SIZE;
+
+	/* control 146 */
+	if (f54->query_36.has_ctrl146)
+		reg_addr += CONTROL_146_SIZE;
+
+	/* control 147 */
+	if (f54->query_38.has_ctrl147)
+		reg_addr += CONTROL_147_SIZE;
+
+	/* control 148 */
+	if (f54->query_38.has_ctrl148)
+		reg_addr += CONTROL_148_SIZE;
+
+	/* control 149 */
+	if (f54->query_38.has_ctrl149) {
+		control->reg_149 = kzalloc(sizeof(*(control->reg_149)),
+				GFP_KERNEL);
+		if (!control->reg_149)
+			goto exit_no_mem;
+		control->reg_149->address = reg_addr;
+		reg_addr += CONTROL_149_SIZE;
+	}
+
+	/* control 150 */
+	if (f54->query_38.has_ctrl150)
+		reg_addr += CONTROL_150_SIZE;
+
+	/* control 151 */
+	if (f54->query_38.has_ctrl151)
+		reg_addr += CONTROL_151_SIZE;
+
+	/* control 152 */
+	if (f54->query_38.has_ctrl152)
+		reg_addr += CONTROL_152_SIZE;
+
+	/* control 153 */
+	if (f54->query_38.has_ctrl153)
+		reg_addr += CONTROL_153_SIZE;
+
+	/* control 154 */
+	if (f54->query_39.has_ctrl154)
+		reg_addr += CONTROL_154_SIZE;
+
+	/* control 155 */
+	if (f54->query_39.has_ctrl155)
+		reg_addr += CONTROL_155_SIZE;
+
+	/* control 156 */
+	if (f54->query_39.has_ctrl156)
+		reg_addr += CONTROL_156_SIZE;
+
+	/* controls 157 158 */
+	if (f54->query_39.has_ctrl157_ctrl158)
+		reg_addr += CONTROL_157_158_SIZE;
+
+	/* controls 159 to 162 reserved */
+
+	/* control 163 */
+	if (f54->query_40.has_ctrl163_query41)
+		reg_addr += CONTROL_163_SIZE;
+
+	/* control 164 reserved */
+
+	/* control 165 */
+	if (f54->query_40.has_ctrl165_query42)
+		reg_addr += CONTROL_165_SIZE;
+
+	/* control 166 */
+	if (f54->query_40.has_ctrl166)
+		reg_addr += CONTROL_166_SIZE;
+
+	/* control 167 */
+	if (f54->query_40.has_ctrl167)
+		reg_addr += CONTROL_167_SIZE;
+
+	/* control 168 */
+	if (f54->query_40.has_ctrl168)
+		reg_addr += CONTROL_168_SIZE;
+
+	/* control 169 */
+	if (f54->query_40.has_ctrl169)
+		reg_addr += CONTROL_169_SIZE;
+
+	/* control 170 reserved */
+
+	/* control 171 */
+	if (f54->query_43.has_ctrl171)
+		reg_addr += CONTROL_171_SIZE;
+
+	/* control 172 */
+	if (f54->query_43.has_ctrl172_query44_query45)
+		reg_addr += CONTROL_172_SIZE;
+
+	/* control 173 */
+	if (f54->query_43.has_ctrl173)
+		reg_addr += CONTROL_173_SIZE;
+
+	/* control 174 */
+	if (f54->query_43.has_ctrl174)
+		reg_addr += CONTROL_174_SIZE;
+
+	/* control 175 */
+	if (f54->query_43.has_ctrl175)
+		reg_addr += CONTROL_175_SIZE;
+
+	/* control 176 */
+	if (f54->query_46.has_ctrl176)
+		reg_addr += CONTROL_176_SIZE;
+
+	/* controls 177 178 */
+	if (f54->query_46.has_ctrl177_ctrl178)
+		reg_addr += CONTROL_177_178_SIZE;
+
+	/* control 179 */
+	if (f54->query_46.has_ctrl179)
+		reg_addr += CONTROL_179_SIZE;
+
+	/* controls 180 to 181 reserved */
+
+	/* control 182 */
+	if (f54->query_47.has_ctrl182)
+		reg_addr += CONTROL_182_SIZE;
+
+	/* control 183 */
+	if (f54->query_47.has_ctrl183)
+		reg_addr += CONTROL_183_SIZE;
+
+	/* control 184 reserved */
+
+	/* control 185 */
+	if (f54->query_47.has_ctrl185)
+		reg_addr += CONTROL_185_SIZE;
+
+	/* control 186 */
+	if (f54->query_47.has_ctrl186)
+		reg_addr += CONTROL_186_SIZE;
+
+	/* control 187 */
+	if (f54->query_47.has_ctrl187)
+		reg_addr += CONTROL_187_SIZE;
+
+	/* control 188 */
+	if (f54->query_49.has_ctrl188) {
+		control->reg_188 = kzalloc(sizeof(*(control->reg_188)),
+				GFP_KERNEL);
+		if (!control->reg_188)
+			goto exit_no_mem;
+		control->reg_188->address = reg_addr;
+		reg_addr += CONTROL_188_SIZE;
+	}
+
+	return 0;
+
+exit_no_mem:
+	dev_err(rmi4_data->pdev->dev.parent,
+			"%s: Failed to alloc mem for control registers\n",
+			__func__);
+	return -ENOMEM;
+}
+
+static int test_set_queries(void)
+{
+	int retval;
+	unsigned char offset;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f54->query_base_addr,
+			f54->query.data,
+			sizeof(f54->query.data));
+	if (retval < 0)
+		return retval;
+
+	offset = sizeof(f54->query.data);
+
+	/* query 12 */
+	if (f54->query.has_sense_frequency_control == 0)
+		offset -= 1;
+
+	/* query 13 */
+	if (f54->query.has_query13) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_13.data,
+				sizeof(f54->query_13.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 14 */
+	if (f54->query_13.has_ctrl87)
+		offset += 1;
+
+	/* query 15 */
+	if (f54->query.has_query15) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_15.data,
+				sizeof(f54->query_15.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 16 */
+	if (f54->query_15.has_query16) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_16.data,
+				sizeof(f54->query_16.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 17 */
+	if (f54->query_16.has_query17)
+		offset += 1;
+
+	/* query 18 */
+	if (f54->query_16.has_ctrl94_query18)
+		offset += 1;
+
+	/* query 19 */
+	if (f54->query_16.has_ctrl95_query19)
+		offset += 1;
+
+	/* query 20 */
+	if (f54->query_15.has_query20)
+		offset += 1;
+
+	/* query 21 */
+	if (f54->query_15.has_query21) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_21.data,
+				sizeof(f54->query_21.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 22 */
+	if (f54->query_15.has_query22) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_22.data,
+				sizeof(f54->query_22.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 23 */
+	if (f54->query_22.has_query23) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_23.data,
+				sizeof(f54->query_23.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 24 */
+	if (f54->query_21.has_query24_data18)
+		offset += 1;
+
+	/* query 25 */
+	if (f54->query_15.has_query25) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_25.data,
+				sizeof(f54->query_25.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 26 */
+	if (f54->query_22.has_ctrl103_query26)
+		offset += 1;
+
+	/* query 27 */
+	if (f54->query_25.has_query27) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_27.data,
+				sizeof(f54->query_27.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 28 */
+	if (f54->query_22.has_query28)
+		offset += 1;
+
+	/* query 29 */
+	if (f54->query_27.has_query29) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_29.data,
+				sizeof(f54->query_29.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 30 */
+	if (f54->query_29.has_query30) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_30.data,
+				sizeof(f54->query_30.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 31 */
+	if (f54->query_30.has_ctrl122_query31)
+		offset += 1;
+
+	/* query 32 */
+	if (f54->query_30.has_query32) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_32.data,
+				sizeof(f54->query_32.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 33 */
+	if (f54->query_32.has_query33) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_33.data,
+				sizeof(f54->query_33.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 34 */
+	if (f54->query_32.has_query34)
+		offset += 1;
+
+	/* query 35 */
+	if (f54->query_32.has_query35) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_35.data,
+				sizeof(f54->query_35.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 36 */
+	if (f54->query_33.has_query36) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_36.data,
+				sizeof(f54->query_36.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 37 */
+	if (f54->query_36.has_query37)
+		offset += 1;
+
+	/* query 38 */
+	if (f54->query_36.has_query38) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_38.data,
+				sizeof(f54->query_38.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 39 */
+	if (f54->query_38.has_query39) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_39.data,
+				sizeof(f54->query_39.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 40 */
+	if (f54->query_39.has_query40) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_40.data,
+				sizeof(f54->query_40.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 41 */
+	if (f54->query_40.has_ctrl163_query41)
+		offset += 1;
+
+	/* query 42 */
+	if (f54->query_40.has_ctrl165_query42)
+		offset += 1;
+
+	/* query 43 */
+	if (f54->query_40.has_query43) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_43.data,
+				sizeof(f54->query_43.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	if (f54->query_43.has_ctrl172_query44_query45)
+		offset += 2;
+
+	/* query 46 */
+	if (f54->query_43.has_query46) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_46.data,
+				sizeof(f54->query_46.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 47 */
+	if (f54->query_46.has_query47) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_47.data,
+				sizeof(f54->query_47.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 48 reserved */
+
+	/* query 49 */
+	if (f54->query_47.has_query49) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_49.data,
+				sizeof(f54->query_49.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 50 */
+	if (f54->query_49.has_query50) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_50.data,
+				sizeof(f54->query_50.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 51 */
+	if (f54->query_50.has_query51) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_51.data,
+				sizeof(f54->query_51.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 53 54 */
+	if (f54->query_51.has_query53_query54_ctrl198)
+		offset += 2;
+
+	/* query 55 */
+	if (f54->query_51.has_query55) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_55.data,
+				sizeof(f54->query_55.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 56 */
+	if (f54->query_55.has_query56)
+		offset += 1;
+
+	/* query 57 */
+	if (f54->query_55.has_query57) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_57.data,
+				sizeof(f54->query_57.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 58 */
+	if (f54->query_57.has_query58) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_58.data,
+				sizeof(f54->query_58.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 59 */
+	if (f54->query_58.has_query59)
+		offset += 1;
+
+	/* query 60 */
+	if (f54->query_58.has_query60)
+		offset += 1;
+
+	/* query 61 */
+	if (f54->query_58.has_query61) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_61.data,
+				sizeof(f54->query_61.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 62 63 */
+	if (f54->query_61.has_ctrl215_query62_query63)
+		offset += 2;
+
+	/* query 64 */
+	if (f54->query_61.has_query64) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_64.data,
+				sizeof(f54->query_64.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 65 */
+	if (f54->query_64.has_query65) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_65.data,
+				sizeof(f54->query_65.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 66 */
+	if (f54->query_65.has_query66_ctrl231)
+		offset += 1;
+
+	/* query 67 */
+	if (f54->query_65.has_query67) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_67.data,
+				sizeof(f54->query_67.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 68 */
+	if (f54->query_67.has_query68) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_68.data,
+				sizeof(f54->query_68.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 68 */
+	if (f54->query_68.has_query69) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f54->query_base_addr + offset,
+				f54->query_69.data,
+				sizeof(f54->query_69.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	return 0;
+}
+
+static void test_f54_set_regs(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned int intr_count,
+		unsigned char page)
+{
+	unsigned char ii;
+	unsigned char intr_offset;
+
+	f54->query_base_addr = fd->query_base_addr | (page << 8);
+	f54->control_base_addr = fd->ctrl_base_addr | (page << 8);
+	f54->data_base_addr = fd->data_base_addr | (page << 8);
+	f54->command_base_addr = fd->cmd_base_addr | (page << 8);
+
+	f54->intr_reg_num = (intr_count + 7) / 8;
+	if (f54->intr_reg_num != 0)
+		f54->intr_reg_num -= 1;
+
+	f54->intr_mask = 0;
+	intr_offset = intr_count % 8;
+	for (ii = intr_offset;
+			ii < (fd->intr_src_count + intr_offset);
+			ii++) {
+		f54->intr_mask |= 1 << ii;
+	}
+}
+
+static int test_f55_set_controls(void)
+{
+	unsigned char offset = 0;
+
+	/* controls 0 1 2 */
+	if (f55->query.has_sensor_assignment)
+		offset += 3;
+
+	/* control 3 */
+	if (f55->query.has_edge_compensation)
+		offset++;
+
+	/* control 4 */
+	if (f55->query.curve_compensation_mode == 0x1 ||
+			f55->query.curve_compensation_mode == 0x2)
+		offset++;
+
+	/* control 5 */
+	if (f55->query.curve_compensation_mode == 0x2)
+		offset++;
+
+	/* control 6 */
+	if (f55->query.has_ctrl6)
+		offset++;
+
+	/* control 7 */
+	if (f55->query.has_alternate_transmitter_assignment)
+		offset++;
+
+	/* control 8 */
+	if (f55->query_3.has_ctrl8)
+		offset++;
+
+	/* control 9 */
+	if (f55->query_3.has_ctrl9)
+		offset++;
+
+	/* control 10 */
+	if (f55->query_5.has_corner_compensation)
+		offset++;
+
+	/* control 11 */
+	if (f55->query.curve_compensation_mode == 0x3)
+		offset++;
+
+	/* control 12 */
+	if (f55->query_5.has_ctrl12)
+		offset++;
+
+	/* control 13 */
+	if (f55->query_5.has_ctrl13)
+		offset++;
+
+	/* control 14 */
+	if (f55->query_5.has_ctrl14)
+		offset++;
+
+	/* control 15 */
+	if (f55->query_5.has_basis_function)
+		offset++;
+
+	/* control 16 */
+	if (f55->query_17.has_ctrl16)
+		offset++;
+
+	/* control 17 */
+	if (f55->query_17.has_ctrl17)
+		offset++;
+
+	/* controls 18 19 */
+	if (f55->query_17.has_ctrl18_ctrl19)
+		offset += 2;
+
+	/* control 20 */
+	if (f55->query_17.has_ctrl20)
+		offset++;
+
+	/* control 21 */
+	if (f55->query_17.has_ctrl21)
+		offset++;
+
+	/* control 22 */
+	if (f55->query_17.has_ctrl22)
+		offset++;
+
+	/* control 23 */
+	if (f55->query_18.has_ctrl23)
+		offset++;
+
+	/* control 24 */
+	if (f55->query_18.has_ctrl24)
+		offset++;
+
+	/* control 25 */
+	if (f55->query_18.has_ctrl25)
+		offset++;
+
+	/* control 26 */
+	if (f55->query_18.has_ctrl26)
+		offset++;
+
+	/* control 27 */
+	if (f55->query_18.has_ctrl27_query20)
+		offset++;
+
+	/* control 28 */
+	if (f55->query_18.has_ctrl28_query21)
+		offset++;
+
+	/* control 29 */
+	if (f55->query_22.has_ctrl29)
+		offset++;
+
+	/* control 30 */
+	if (f55->query_22.has_ctrl30)
+		offset++;
+
+	/* control 31 */
+	if (f55->query_22.has_ctrl31)
+		offset++;
+
+	/* control 32 */
+	if (f55->query_22.has_ctrl32)
+		offset++;
+
+	/* controls 33 34 35 36 reserved */
+
+	/* control 37 */
+	if (f55->query_28.has_ctrl37)
+		offset++;
+
+	/* control 38 */
+	if (f55->query_30.has_ctrl38)
+		offset++;
+
+	/* control 39 */
+	if (f55->query_30.has_ctrl39)
+		offset++;
+
+	/* control 40 */
+	if (f55->query_30.has_ctrl40)
+		offset++;
+
+	/* control 41 */
+	if (f55->query_30.has_ctrl41)
+		offset++;
+
+	/* control 42 */
+	if (f55->query_30.has_ctrl42)
+		offset++;
+
+	/* controls 43 44 */
+	if (f55->query_30.has_ctrl43_ctrl44) {
+		f55->afe_mux_offset = offset;
+		offset += 2;
+	}
+
+	/* controls 45 46 */
+	if (f55->query_33.has_ctrl45_ctrl46) {
+		f55->has_force = true;
+		f55->force_tx_offset = offset;
+		f55->force_rx_offset = offset + 1;
+		offset += 2;
+	}
+
+	return 0;
+}
+
+static int test_f55_set_queries(void)
+{
+	int retval;
+	unsigned char offset;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f55->query_base_addr,
+			f55->query.data,
+			sizeof(f55->query.data));
+	if (retval < 0)
+		return retval;
+
+	offset = sizeof(f55->query.data);
+
+	/* query 3 */
+	if (f55->query.has_single_layer_multi_touch) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_3.data,
+				sizeof(f55->query_3.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 4 */
+	if (f55->query_3.has_ctrl9)
+		offset += 1;
+
+	/* query 5 */
+	if (f55->query.has_query5) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_5.data,
+				sizeof(f55->query_5.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* queries 6 7 */
+	if (f55->query.curve_compensation_mode == 0x3)
+		offset += 2;
+
+	/* query 8 */
+	if (f55->query_3.has_ctrl8)
+		offset += 1;
+
+	/* query 9 */
+	if (f55->query_3.has_query9)
+		offset += 1;
+
+	/* queries 10 11 12 13 14 15 16 */
+	if (f55->query_5.has_basis_function)
+		offset += 7;
+
+	/* query 17 */
+	if (f55->query_5.has_query17) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_17.data,
+				sizeof(f55->query_17.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 18 */
+	if (f55->query_17.has_query18) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_18.data,
+				sizeof(f55->query_18.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 19 */
+	if (f55->query_18.has_query19)
+		offset += 1;
+
+	/* query 20 */
+	if (f55->query_18.has_ctrl27_query20)
+		offset += 1;
+
+	/* query 21 */
+	if (f55->query_18.has_ctrl28_query21)
+		offset += 1;
+
+	/* query 22 */
+	if (f55->query_18.has_query22) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_22.data,
+				sizeof(f55->query_22.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 23 */
+	if (f55->query_22.has_query23) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_23.data,
+				sizeof(f55->query_23.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+
+		f55->amp_sensor = f55->query_23.amp_sensor_enabled;
+		f55->size_of_column2mux = f55->query_23.size_of_column2mux;
+	}
+
+	/* queries 24 25 26 27 reserved */
+
+	/* query 28 */
+	if (f55->query_22.has_query28) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_28.data,
+				sizeof(f55->query_28.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* query 29 */
+	if (f55->query_28.has_query29)
+		offset += 1;
+
+	/* query 30 */
+	if (f55->query_28.has_query30) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_30.data,
+				sizeof(f55->query_30.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+	}
+
+	/* queries 31 32 */
+	if (f55->query_30.has_query31_query32)
+		offset += 2;
+
+	/* query 33 */
+	if (f55->query_30.has_query33) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->query_base_addr + offset,
+				f55->query_33.data,
+				sizeof(f55->query_33.data));
+		if (retval < 0)
+			return retval;
+		offset += 1;
+
+		f55->extended_amp = f55->query_33.has_extended_amp_pad;
+	}
+
+	return 0;
+}
+
+static void test_f55_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char rx_electrodes;
+	unsigned char tx_electrodes;
+	struct f55_control_43 ctrl_43;
+
+	retval = test_f55_set_queries();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read F55 query registers\n",
+				__func__);
+		return;
+	}
+
+	if (!f55->query.has_sensor_assignment)
+		return;
+
+	retval = test_f55_set_controls();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set up F55 control registers\n",
+				__func__);
+		return;
+	}
+
+	tx_electrodes = f55->query.num_of_tx_electrodes;
+	rx_electrodes = f55->query.num_of_rx_electrodes;
+
+	f55->tx_assignment = kzalloc(tx_electrodes, GFP_KERNEL);
+	f55->rx_assignment = kzalloc(rx_electrodes, GFP_KERNEL);
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f55->control_base_addr + SENSOR_TX_MAPPING_OFFSET,
+			f55->tx_assignment,
+			tx_electrodes);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read F55 tx assignment\n",
+				__func__);
+		return;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f55->control_base_addr + SENSOR_RX_MAPPING_OFFSET,
+			f55->rx_assignment,
+			rx_electrodes);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read F55 rx assignment\n",
+				__func__);
+		return;
+	}
+
+	f54->tx_assigned = 0;
+	for (ii = 0; ii < tx_electrodes; ii++) {
+		if (f55->tx_assignment[ii] != 0xff)
+			f54->tx_assigned++;
+	}
+
+	f54->rx_assigned = 0;
+	for (ii = 0; ii < rx_electrodes; ii++) {
+		if (f55->rx_assignment[ii] != 0xff)
+			f54->rx_assigned++;
+	}
+
+	if (f55->amp_sensor) {
+		f54->tx_assigned = f55->size_of_column2mux;
+		f54->rx_assigned /= 2;
+	}
+
+	if (f55->extended_amp) {
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->control_base_addr + f55->afe_mux_offset,
+				ctrl_43.data,
+				sizeof(ctrl_43.data));
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read F55 AFE mux sizes\n",
+					__func__);
+			return;
+		}
+
+		f54->tx_assigned = ctrl_43.afe_l_mux_size +
+				ctrl_43.afe_r_mux_size;
+	}
+
+	/* force mapping */
+	if (f55->has_force) {
+		f55->force_tx_assignment = kzalloc(tx_electrodes, GFP_KERNEL);
+		f55->force_rx_assignment = kzalloc(rx_electrodes, GFP_KERNEL);
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->control_base_addr + f55->force_tx_offset,
+				f55->force_tx_assignment,
+				tx_electrodes);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read F55 force tx assignment\n",
+					__func__);
+			return;
+		}
+
+		retval = synaptics_rmi4_reg_read(rmi4_data,
+				f55->control_base_addr + f55->force_rx_offset,
+				f55->force_rx_assignment,
+				rx_electrodes);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to read F55 force rx assignment\n",
+					__func__);
+			return;
+		}
+
+		for (ii = 0; ii < tx_electrodes; ii++) {
+			if (f55->force_tx_assignment[ii] != 0xff)
+				f54->tx_assigned++;
+		}
+
+		for (ii = 0; ii < rx_electrodes; ii++) {
+			if (f55->force_rx_assignment[ii] != 0xff)
+				f54->rx_assigned++;
+		}
+	}
+}
+
+static void test_f55_set_regs(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned char page)
+{
+	f55 = kzalloc(sizeof(*f55), GFP_KERNEL);
+	if (!f55) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for F55\n",
+				__func__);
+		return;
+	}
+
+	f55->query_base_addr = fd->query_base_addr | (page << 8);
+	f55->control_base_addr = fd->ctrl_base_addr | (page << 8);
+	f55->data_base_addr = fd->data_base_addr | (page << 8);
+	f55->command_base_addr = fd->cmd_base_addr | (page << 8);
+}
+
+static void test_f21_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char size_of_query2;
+	unsigned char size_of_query5;
+	unsigned char query_11_offset;
+	unsigned char ctrl_4_offset;
+	struct f21_query_2 *query_2 = NULL;
+	struct f21_query_5 *query_5 = NULL;
+	struct f21_query_11 *query_11 = NULL;
+
+	query_2 = kzalloc(sizeof(*query_2), GFP_KERNEL);
+	if (!query_2) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for query_2\n",
+				__func__);
+		goto exit;
+	}
+
+	query_5 = kzalloc(sizeof(*query_5), GFP_KERNEL);
+	if (!query_5) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for query_5\n",
+				__func__);
+		goto exit;
+	}
+
+	query_11 = kzalloc(sizeof(*query_11), GFP_KERNEL);
+	if (!query_11) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for query_11\n",
+				__func__);
+		goto exit;
+	}
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f21->query_base_addr + 1,
+			&size_of_query2,
+			sizeof(size_of_query2));
+	if (retval < 0)
+		goto exit;
+
+	if (size_of_query2 > sizeof(query_2->data))
+		size_of_query2 = sizeof(query_2->data);
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f21->query_base_addr + 2,
+			query_2->data,
+			size_of_query2);
+	if (retval < 0)
+		goto exit;
+
+	if (!query_2->query11_is_present) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: No F21 force capabilities\n",
+				__func__);
+		goto exit;
+	}
+
+	query_11_offset = query_2->query0_is_present +
+			query_2->query1_is_present +
+			query_2->query2_is_present +
+			query_2->query3_is_present +
+			query_2->query4_is_present +
+			query_2->query5_is_present +
+			query_2->query6_is_present +
+			query_2->query7_is_present +
+			query_2->query8_is_present +
+			query_2->query9_is_present +
+			query_2->query10_is_present;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f21->query_base_addr + 11,
+			query_11->data,
+			sizeof(query_11->data));
+	if (retval < 0)
+		goto exit;
+
+	if (!query_11->has_force_sensing_txrx_mapping) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: No F21 force mapping\n",
+				__func__);
+		goto exit;
+	}
+
+	f21->max_num_of_tx = query_11->max_number_of_force_txs;
+	f21->max_num_of_rx = query_11->max_number_of_force_rxs;
+	f21->max_num_of_txrx = f21->max_num_of_tx + f21->max_num_of_rx;
+
+	f21->force_txrx_assignment = kzalloc(f21->max_num_of_txrx, GFP_KERNEL);
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f21->query_base_addr + 4,
+			&size_of_query5,
+			sizeof(size_of_query5));
+	if (retval < 0)
+		goto exit;
+
+	if (size_of_query5 > sizeof(query_5->data))
+		size_of_query5 = sizeof(query_5->data);
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f21->query_base_addr + 5,
+			query_5->data,
+			size_of_query5);
+	if (retval < 0)
+		goto exit;
+
+	ctrl_4_offset = query_5->ctrl0_is_present +
+			query_5->ctrl1_is_present +
+			query_5->ctrl2_is_present +
+			query_5->ctrl3_is_present;
+
+	retval = synaptics_rmi4_reg_read(rmi4_data,
+			f21->control_base_addr + ctrl_4_offset,
+			f21->force_txrx_assignment,
+			f21->max_num_of_txrx);
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read F21 force txrx assignment\n",
+				__func__);
+		goto exit;
+	}
+
+	f21->has_force = true;
+
+	for (ii = 0; ii < f21->max_num_of_tx; ii++) {
+		if (f21->force_txrx_assignment[ii] != 0xff)
+			f21->tx_assigned++;
+	}
+
+	for (ii = f21->max_num_of_tx; ii < f21->max_num_of_txrx; ii++) {
+		if (f21->force_txrx_assignment[ii] != 0xff)
+			f21->rx_assigned++;
+	}
+
+exit:
+	kfree(query_2);
+	kfree(query_5);
+	kfree(query_11);
+}
+
+static void test_f21_set_regs(struct synaptics_rmi4_data *rmi4_data,
+		struct synaptics_rmi4_fn_desc *fd,
+		unsigned char page)
+{
+	f21 = kzalloc(sizeof(*f21), GFP_KERNEL);
+	if (!f21) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for F21\n",
+				__func__);
+		return;
+	}
+
+	f21->query_base_addr = fd->query_base_addr | (page << 8);
+	f21->control_base_addr = fd->ctrl_base_addr | (page << 8);
+	f21->data_base_addr = fd->data_base_addr | (page << 8);
+	f21->command_base_addr = fd->cmd_base_addr | (page << 8);
+}
+
+static int test_scan_pdt(void)
+{
+	int retval;
+	unsigned char intr_count = 0;
+	unsigned char page;
+	unsigned short addr;
+	bool f54found = false;
+	bool f55found = false;
+	struct synaptics_rmi4_fn_desc rmi_fd;
+	struct synaptics_rmi4_data *rmi4_data = f54->rmi4_data;
+
+	for (page = 0; page < PAGES_TO_SERVICE; page++) {
+		for (addr = PDT_START; addr > PDT_END; addr -= PDT_ENTRY_SIZE) {
+			addr |= (page << 8);
+
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					addr,
+					(unsigned char *)&rmi_fd,
+					sizeof(rmi_fd));
+			if (retval < 0)
+				return retval;
+
+			addr &= ~(MASK_8BIT << 8);
+
+			if (!rmi_fd.fn_number)
+				break;
+
+			switch (rmi_fd.fn_number) {
+			case SYNAPTICS_RMI4_F54:
+				test_f54_set_regs(rmi4_data,
+						&rmi_fd, intr_count, page);
+				f54found = true;
+				break;
+			case SYNAPTICS_RMI4_F55:
+				test_f55_set_regs(rmi4_data,
+						&rmi_fd, page);
+				f55found = true;
+				break;
+			case SYNAPTICS_RMI4_F21:
+				test_f21_set_regs(rmi4_data,
+						&rmi_fd, page);
+				break;
+			default:
+				break;
+			}
+
+			if (f54found && f55found)
+				goto pdt_done;
+
+			intr_count += rmi_fd.intr_src_count;
+		}
+	}
+
+	if (!f54found) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to find F54\n",
+				__func__);
+		return -EINVAL;
+	}
+
+pdt_done:
+	return 0;
+}
+
+static void synaptics_rmi4_test_attn(struct synaptics_rmi4_data *rmi4_data,
+		unsigned char intr_mask)
+{
+	if (!f54)
+		return;
+
+	if (f54->intr_mask & intr_mask)
+		queue_work(f54->test_report_workqueue, &f54->test_report_work);
+
+	return;
+}
+
+static int synaptics_rmi4_test_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+
+	if (f54) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Handle already exists\n",
+				__func__);
+		return 0;
+	}
+
+	f54 = kzalloc(sizeof(*f54), GFP_KERNEL);
+	if (!f54) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for F54\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	f54->rmi4_data = rmi4_data;
+
+	f55 = NULL;
+
+	f21 = NULL;
+
+	retval = test_scan_pdt();
+	if (retval < 0)
+		goto exit_free_mem;
+
+	retval = test_set_queries();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read F54 query registers\n",
+				__func__);
+		goto exit_free_mem;
+	}
+
+	f54->tx_assigned = f54->query.num_of_tx_electrodes;
+	f54->rx_assigned = f54->query.num_of_rx_electrodes;
+
+	retval = test_set_controls();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set up F54 control registers\n",
+				__func__);
+		goto exit_free_control;
+	}
+
+	test_set_data();
+
+	if (f55)
+		test_f55_init(rmi4_data);
+
+	if (f21)
+		test_f21_init(rmi4_data);
+
+	if (rmi4_data->external_afe_buttons)
+		f54->tx_assigned++;
+
+	retval = test_set_sysfs();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs entries\n",
+				__func__);
+		goto exit_sysfs;
+	}
+
+	f54->test_report_workqueue =
+			create_singlethread_workqueue("test_report_workqueue");
+	INIT_WORK(&f54->test_report_work, test_report_work);
+
+	hrtimer_init(&f54->watchdog, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	f54->watchdog.function = test_get_report_timeout;
+	INIT_WORK(&f54->timeout_work, test_timeout_work);
+
+	mutex_init(&f54->status_mutex);
+	f54->status = STATUS_IDLE;
+
+	return 0;
+
+exit_sysfs:
+	if (f21)
+		kfree(f21->force_txrx_assignment);
+
+	if (f55) {
+		kfree(f55->tx_assignment);
+		kfree(f55->rx_assignment);
+		kfree(f55->force_tx_assignment);
+		kfree(f55->force_rx_assignment);
+	}
+
+exit_free_control:
+	test_free_control_mem();
+
+exit_free_mem:
+	kfree(f21);
+	f21 = NULL;
+	kfree(f55);
+	f55 = NULL;
+	kfree(f54);
+	f54 = NULL;
+
+exit:
+	return retval;
+}
+
+static void synaptics_rmi4_test_remove(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!f54)
+		goto exit;
+
+	hrtimer_cancel(&f54->watchdog);
+
+	cancel_work_sync(&f54->test_report_work);
+	flush_workqueue(f54->test_report_workqueue);
+	destroy_workqueue(f54->test_report_workqueue);
+
+	test_remove_sysfs();
+
+	if (f21)
+		kfree(f21->force_txrx_assignment);
+
+	if (f55) {
+		kfree(f55->tx_assignment);
+		kfree(f55->rx_assignment);
+		kfree(f55->force_tx_assignment);
+		kfree(f55->force_rx_assignment);
+	}
+
+	test_free_control_mem();
+
+	if (f54->data_buffer_size)
+		kfree(f54->report_data);
+
+	kfree(f21);
+	f21 = NULL;
+
+	kfree(f55);
+	f55 = NULL;
+
+	kfree(f54);
+	f54 = NULL;
+
+exit:
+	complete(&test_remove_complete);
+}
+
+static void synaptics_rmi4_test_reset(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+
+	if (!f54) {
+		synaptics_rmi4_test_init(rmi4_data);
+		return;
+	}
+
+	if (f21)
+		kfree(f21->force_txrx_assignment);
+
+	if (f55) {
+		kfree(f55->tx_assignment);
+		kfree(f55->rx_assignment);
+		kfree(f55->force_tx_assignment);
+		kfree(f55->force_rx_assignment);
+	}
+
+	test_free_control_mem();
+
+	kfree(f55);
+	f55 = NULL;
+
+	kfree(f21);
+	f21 = NULL;
+
+	retval = test_scan_pdt();
+	if (retval < 0)
+		goto exit_free_mem;
+
+	retval = test_set_queries();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to read F54 query registers\n",
+				__func__);
+		goto exit_free_mem;
+	}
+
+	f54->tx_assigned = f54->query.num_of_tx_electrodes;
+	f54->rx_assigned = f54->query.num_of_rx_electrodes;
+
+	retval = test_set_controls();
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to set up F54 control registers\n",
+				__func__);
+		goto exit_free_control;
+	}
+
+	test_set_data();
+
+	if (f55)
+		test_f55_init(rmi4_data);
+
+	if (f21)
+		test_f21_init(rmi4_data);
+
+	if (rmi4_data->external_afe_buttons)
+		f54->tx_assigned++;
+
+	f54->status = STATUS_IDLE;
+
+	return;
+
+exit_free_control:
+	test_free_control_mem();
+
+exit_free_mem:
+	hrtimer_cancel(&f54->watchdog);
+
+	cancel_work_sync(&f54->test_report_work);
+	flush_workqueue(f54->test_report_workqueue);
+	destroy_workqueue(f54->test_report_workqueue);
+
+	test_remove_sysfs();
+
+	if (f54->data_buffer_size)
+		kfree(f54->report_data);
+
+	kfree(f21);
+	f21 = NULL;
+
+	kfree(f55);
+	f55 = NULL;
+
+	kfree(f54);
+	f54 = NULL;
+
+	return;
+}
+
+static struct synaptics_rmi4_exp_fn test_module = {
+	.fn_type = RMI_TEST_REPORTING,
+	.init = synaptics_rmi4_test_init,
+	.remove = synaptics_rmi4_test_remove,
+	.reset = synaptics_rmi4_test_reset,
+	.reinit = NULL,
+	.early_suspend = NULL,
+	.suspend = NULL,
+	.resume = NULL,
+	.late_resume = NULL,
+	.attn = synaptics_rmi4_test_attn,
+};
+
+static int __init rmi4_test_module_init(void)
+{
+	synaptics_rmi4_new_function(&test_module, true);
+
+	return 0;
+}
+
+static void __exit rmi4_test_module_exit(void)
+{
+	synaptics_rmi4_new_function(&test_module, false);
+
+	wait_for_completion(&test_remove_complete);
+}
+
+module_init(rmi4_test_module_init);
+module_exit(rmi4_test_module_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX Test Reporting Module");
+MODULE_LICENSE("GPL v2");

synaptics_dsx/synaptics_dsx_video.c

@@ -0,0 +1,403 @@
+/*
+ * Synaptics DSX touchscreen driver
+ *
+ * Copyright (C) 2012-2016 Synaptics Incorporated. All rights reserved.
+ *
+ * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2012 Alexandra Chin <[email protected]>
+ * Copyright (C) 2012 Scott Lin <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND SYNAPTICS
+ * EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES, INCLUDING ANY
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
+ * AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY INTELLECTUAL PROPERTY RIGHTS.
+ * IN NO EVENT SHALL SYNAPTICS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, PUNITIVE, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION
+ * WITH THE USE OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED
+ * AND BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS ADVISED OF
+ * THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF COMPETENT JURISDICTION DOES
+ * NOT PERMIT THE DISCLAIMER OF DIRECT DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS'
+ * TOTAL CUMULATIVE LIABILITY TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S.
+ * DOLLARS.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/input/synaptics_dsx.h>
+#include "synaptics_dsx_core.h"
+
+#define SYSFS_FOLDER_NAME "video"
+
+static ssize_t video_sysfs_dcs_write_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static ssize_t video_sysfs_param_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static int video_send_dcs_command(unsigned char command_opcode);
+
+struct f38_command {
+	union {
+		struct {
+			unsigned char command_opcode;
+			unsigned char register_access:1;
+			unsigned char gamma_page:1;
+			unsigned char f38_control1_b2__7:6;
+			unsigned char parameter_field_1;
+			unsigned char parameter_field_2;
+			unsigned char parameter_field_3;
+			unsigned char parameter_field_4;
+			unsigned char send_to_dcs:1;
+			unsigned char f38_command6_b1__7:7;
+		} __packed;
+		unsigned char data[7];
+	};
+};
+
+struct synaptics_rmi4_video_handle {
+	unsigned char param;
+	unsigned short query_base_addr;
+	unsigned short control_base_addr;
+	unsigned short data_base_addr;
+	unsigned short command_base_addr;
+	struct synaptics_rmi4_data *rmi4_data;
+	struct kobject *sysfs_dir;
+};
+
+#ifdef RMI_DCS_SUSPEND_RESUME
+struct dcs_command {
+	unsigned char command;
+	unsigned int wait_time;
+};
+
+static struct dcs_command suspend_sequence[] = {
+	{
+		.command = 0x28,
+		.wait_time = 200,
+	},
+	{
+		.command = 0x10,
+		.wait_time = 200,
+	},
+};
+
+static struct dcs_command resume_sequence[] = {
+	{
+		.command = 0x11,
+		.wait_time = 200,
+	},
+	{
+		.command = 0x29,
+		.wait_time = 200,
+	},
+};
+#endif
+
+static struct device_attribute attrs[] = {
+	__ATTR(dcs_write, 0220,
+			synaptics_rmi4_show_error,
+			video_sysfs_dcs_write_store),
+	__ATTR(param, 0220,
+			synaptics_rmi4_show_error,
+			video_sysfs_param_store),
+};
+
+static struct synaptics_rmi4_video_handle *video;
+
+DECLARE_COMPLETION(video_remove_complete);
+
+static ssize_t video_sysfs_dcs_write_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval;
+	unsigned int input;
+
+	if (kstrtouint(buf, 16, &input) != 1)
+		return -EINVAL;
+
+	retval = video_send_dcs_command((unsigned char)input);
+	if (retval < 0)
+		return retval;
+
+	return count;
+}
+
+static ssize_t video_sysfs_param_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned int input;
+
+	if (kstrtouint(buf, 16, &input) != 1)
+		return -EINVAL;
+
+	video->param = (unsigned char)input;
+
+	return count;
+}
+
+static int video_send_dcs_command(unsigned char command_opcode)
+{
+	int retval;
+	struct f38_command command;
+	struct synaptics_rmi4_data *rmi4_data = video->rmi4_data;
+
+	memset(&command, 0x00, sizeof(command));
+
+	command.command_opcode = command_opcode;
+	command.parameter_field_1 = video->param;
+	command.send_to_dcs = 1;
+
+	video->param = 0;
+
+	retval = synaptics_rmi4_reg_write(rmi4_data,
+			video->command_base_addr,
+			command.data,
+			sizeof(command.data));
+	if (retval < 0) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to send DCS command\n",
+				__func__);
+		return retval;
+	}
+
+	return 0;
+}
+
+static int video_scan_pdt(void)
+{
+	int retval;
+	unsigned char page;
+	unsigned short addr;
+	bool f38_found = false;
+	struct synaptics_rmi4_fn_desc rmi_fd;
+	struct synaptics_rmi4_data *rmi4_data = video->rmi4_data;
+
+	for (page = 0; page < PAGES_TO_SERVICE; page++) {
+		for (addr = PDT_START; addr > PDT_END; addr -= PDT_ENTRY_SIZE) {
+			addr |= (page << 8);
+
+			retval = synaptics_rmi4_reg_read(rmi4_data,
+					addr,
+					(unsigned char *)&rmi_fd,
+					sizeof(rmi_fd));
+			if (retval < 0)
+				return retval;
+
+			addr &= ~(MASK_8BIT << 8);
+
+			if (!rmi_fd.fn_number)
+				break;
+
+			if (rmi_fd.fn_number == SYNAPTICS_RMI4_F38) {
+				f38_found = true;
+				goto f38_found;
+			}
+		}
+	}
+
+	if (!f38_found) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to find F38\n",
+				__func__);
+		return -EINVAL;
+	}
+
+f38_found:
+	video->query_base_addr = rmi_fd.query_base_addr | (page << 8);
+	video->control_base_addr = rmi_fd.ctrl_base_addr | (page << 8);
+	video->data_base_addr = rmi_fd.data_base_addr | (page << 8);
+	video->command_base_addr = rmi_fd.cmd_base_addr | (page << 8);
+
+	return 0;
+}
+
+static int synaptics_rmi4_video_init(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char attr_count;
+
+	if (video) {
+		dev_dbg(rmi4_data->pdev->dev.parent,
+				"%s: Handle already exists\n",
+				__func__);
+		return 0;
+	}
+
+	video = kzalloc(sizeof(*video), GFP_KERNEL);
+	if (!video) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to alloc mem for video\n",
+				__func__);
+		retval = -ENOMEM;
+		goto exit;
+	}
+
+	video->rmi4_data = rmi4_data;
+
+	retval = video_scan_pdt();
+	if (retval < 0) {
+		retval = 0;
+		goto exit_scan_pdt;
+	}
+
+	video->sysfs_dir = kobject_create_and_add(SYSFS_FOLDER_NAME,
+			&rmi4_data->input_dev->dev.kobj);
+	if (!video->sysfs_dir) {
+		dev_err(rmi4_data->pdev->dev.parent,
+				"%s: Failed to create sysfs directory\n",
+				__func__);
+		retval = -ENODEV;
+		goto exit_sysfs_dir;
+	}
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		retval = sysfs_create_file(video->sysfs_dir,
+				&attrs[attr_count].attr);
+		if (retval < 0) {
+			dev_err(rmi4_data->pdev->dev.parent,
+					"%s: Failed to create sysfs attributes\n",
+					__func__);
+			retval = -ENODEV;
+			goto exit_sysfs_attrs;
+		}
+	}
+
+	return 0;
+
+exit_sysfs_attrs:
+	for (attr_count--; attr_count >= 0; attr_count--)
+		sysfs_remove_file(video->sysfs_dir, &attrs[attr_count].attr);
+
+	kobject_put(video->sysfs_dir);
+
+exit_sysfs_dir:
+exit_scan_pdt:
+	kfree(video);
+	video = NULL;
+
+exit:
+	return retval;
+}
+
+static void synaptics_rmi4_video_remove(struct synaptics_rmi4_data *rmi4_data)
+{
+	unsigned char attr_count;
+
+	if (!video)
+		goto exit;
+
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++)
+		sysfs_remove_file(video->sysfs_dir, &attrs[attr_count].attr);
+
+	kobject_put(video->sysfs_dir);
+
+	kfree(video);
+	video = NULL;
+
+exit:
+	complete(&video_remove_complete);
+}
+
+static void synaptics_rmi4_video_reset(struct synaptics_rmi4_data *rmi4_data)
+{
+	if (!video)
+		synaptics_rmi4_video_init(rmi4_data);
+}
+
+#ifdef RMI_DCS_SUSPEND_RESUME
+static void synaptics_rmi4_video_suspend(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char command;
+	unsigned char num_of_cmds;
+
+	if (!video)
+		return;
+
+	num_of_cmds = ARRAY_SIZE(suspend_sequence);
+
+	for (ii = 0; ii < num_of_cmds; ii++) {
+		command = suspend_sequence[ii].command;
+		retval = video_send_dcs_command(command);
+		if (retval < 0)
+			return;
+		msleep(suspend_sequence[ii].wait_time);
+	}
+}
+
+static void synaptics_rmi4_video_resume(struct synaptics_rmi4_data *rmi4_data)
+{
+	int retval;
+	unsigned char ii;
+	unsigned char command;
+	unsigned char num_of_cmds;
+
+	if (!video)
+		return;
+
+	num_of_cmds = ARRAY_SIZE(resume_sequence);
+
+	for (ii = 0; ii < num_of_cmds; ii++) {
+		command = resume_sequence[ii].command;
+		retval = video_send_dcs_command(command);
+		if (retval < 0)
+			return;
+		msleep(resume_sequence[ii].wait_time);
+	}
+}
+#endif
+
+static struct synaptics_rmi4_exp_fn video_module = {
+	.fn_type = RMI_VIDEO,
+	.init = synaptics_rmi4_video_init,
+	.remove = synaptics_rmi4_video_remove,
+	.reset = synaptics_rmi4_video_reset,
+	.reinit = NULL,
+	.early_suspend = NULL,
+#ifdef RMI_DCS_SUSPEND_RESUME
+	.suspend = synaptics_rmi4_video_suspend,
+	.resume = synaptics_rmi4_video_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.late_resume = NULL,
+	.attn = NULL,
+};
+
+static int __init rmi4_video_module_init(void)
+{
+	synaptics_rmi4_new_function(&video_module, true);
+
+	return 0;
+}
+
+static void __exit rmi4_video_module_exit(void)
+{
+	synaptics_rmi4_new_function(&video_module, false);
+
+	wait_for_completion(&video_remove_complete);
+}
+
+module_init(rmi4_video_module_init);
+module_exit(rmi4_video_module_exit);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("Synaptics DSX Video Module");
+MODULE_LICENSE("GPL v2");