Merge branch partner-drops-b5s5 into r-dev-msm-pixel-4.19

Bug: 138816997
Change-Id: I043714e646112b7e71c657755496a69e71b55065

9 files changed, 294 insertions, 86 deletions

diff --git a/fts.h b/fts.h
index 178bf07..51f0e03 100644
--- a/fts.h
+++ b/fts.h
@@ -52,9 +52,9 @@
   */
 /* **** CODE CONFIGURATION **** */
 #define FTS_TS_DRV_NAME		"fts"	/* driver name */
-#define FTS_TS_DRV_VERSION	"5.2.16.8"	/* driver version string
+#define FTS_TS_DRV_VERSION	"5.2.16.11"	/* driver version string
 							 * */
-#define FTS_TS_DRV_VER		0x05021008	/* driver version u32 format */
+#define FTS_TS_DRV_VER		0x0502100B	/* driver version u32 format */
 
 /* #define DEBUG */	/* /< define to print more logs in the kernel log
 			 * and better follow the code flow */
@@ -70,16 +70,16 @@
 /* If both COMPUTE_INIT_METHOD and PRE_SAVED_METHOD are not defined,
  * driver will be automatically configured as GOLDEN_VALUE_METHOD
  */
-#define COMPUTE_INIT_METHOD  /* Allow to compute init data on phone during
+/*#define COMPUTE_INIT_METHOD   Allow to compute init data on phone during
 			      * production
 			      */
 #define SKIP_PRODUCTION_TEST /* Allow to skip Production test */
 
-#ifndef COMPUTE_INIT_METHOD
-#define PRE_SAVED_METHOD /* Pre-Saved Method used
+//#ifndef COMPUTE_INIT_METHOD
+/*#define PRE_SAVED_METHOD  Pre-Saved Method used
 			  * during production
 			  */
-#endif
+//#endif
 
 /*#define FW_H_FILE*/			/* include the FW data as header file */
 #ifdef FW_H_FILE
diff --git a/fts_lib/ftsCore.c b/fts_lib/ftsCore.c
index b8d0af1..81582a9 100644
--- a/fts_lib/ftsCore.c
+++ b/fts_lib/ftsCore.c
@@ -307,10 +307,10 @@ int checkEcho(u8 *cmd, int size)
 		pr_err("checkEcho: Error Size = %d not valid!\n", size);
 		return ERROR_OP_NOT_ALLOW;
 	} else {
-		if ((size + 3) > FIFO_EVENT_SIZE)
-			size = FIFO_EVENT_SIZE - 3;
+		if ((size + 4) > FIFO_EVENT_SIZE)
+			size = FIFO_EVENT_SIZE - 4;
 		/* Echo event 0x43 0x01 xx xx xx xx xx fifo_status
-		 * therefore command with more than 5 bytes will be trunked */
+		 * therefore command with more than 4 bytes will be trunked */
 
 		event_to_search[0] = EVT_ID_STATUS_UPDATE;
 		event_to_search[1] = EVT_TYPE_STATUS_ECHO;
diff --git a/fts_lib/ftsFlash.c b/fts_lib/ftsFlash.c
index 270b9aa..f57de07 100644
--- a/fts_lib/ftsFlash.c
+++ b/fts_lib/ftsFlash.c
@@ -294,6 +294,16 @@ int hold_m3(void)
 			return ret;
 		}
 
+#ifdef ALIX
+		cmd[0] = 0x70;
+		ret = fts_writeU8UX(FTS_CMD_HW_REG_W, ADDR_SIZE_HW_REG,
+				    ADDR_GPIO_CONFIG_REG3, cmd, 1);
+		if (ret < OK) {
+			pr_err("%s: can not set gpio config ERROR %08X\n",
+				__func__, ret);
+			return ret;
+		}
+#else
 		cmd[0] = 0x07;
 		ret = fts_writeU8UX(FTS_CMD_HW_REG_W, ADDR_SIZE_HW_REG,
 				    ADDR_GPIO_CONFIG_REG2, cmd, 1);
@@ -302,6 +312,7 @@ int hold_m3(void)
 				__func__, ret);
 			return ret;
 		}
+#endif
 
 		cmd[0] = 0x30;
 		ret = fts_writeU8UX(FTS_CMD_HW_REG_W, ADDR_SIZE_HW_REG,
@@ -488,6 +499,43 @@ int parseBinFile(u8 *fw_data, int fw_size, Firmware *fwData, int keep_cx)
 		pr_info("parseBinFile: CX Version = %04X\n", fwData->cx_ver);
 
 		fwData->data_size = dimension;
+		index = FLASH_ORG_INFO_INDEX;
+		fwData->fw_code_size = fw_data[index++];
+		fwData->panel_config_size = fw_data[index++];
+		fwData->cx_area_size = fw_data[index++];
+		fwData->fw_config_size = fw_data[index];
+
+		pr_info("parseBinFile: Code Pages: %d panel area Pages: %d"
+			" cx area Pages: %d fw config Pages: %d !\n",
+			fwData->fw_code_size, fwData->panel_config_size,
+			fwData->cx_area_size, fwData->fw_config_size);
+
+		if ((fwData->fw_code_size == 0) ||
+			(fwData->panel_config_size == 0) ||
+			(fwData->cx_area_size == 0) ||
+			(fwData->fw_config_size == 0)) {
+			pr_info("parseBinFile: Using default flash Address\n");
+			fwData->code_start_addr = FLASH_ADDR_CODE;
+			fwData->cx_start_addr = FLASH_ADDR_CX;
+			fwData->config_start_addr = FLASH_ADDR_CONFIG;
+		} else {
+			fwData->code_start_addr = FLASH_ADDR_CODE;
+			fwData->cx_start_addr = (FLASH_ADDR_CODE +
+						(((fwData->fw_code_size +
+						fwData->panel_config_size) *
+						FLASH_PAGE_SIZE) / 4));
+			fwData->config_start_addr = (FLASH_ADDR_CODE +
+						(((fwData->fw_code_size +
+						fwData->panel_config_size +
+						fwData->cx_area_size) *
+						FLASH_PAGE_SIZE) / 4));
+		}
+
+		pr_info("parseBinFile: Code start addr: 0x%08X"
+			" cx start addr: 0x%08X"
+			" fw start addr: 0x%08X !\n",
+			fwData->code_start_addr, fwData->cx_start_addr,
+			fwData->config_start_addr);
 
 		pr_info("READ FW DONE %d bytes!\n", fwData->data_size);
 		res = OK;
@@ -623,9 +671,15 @@ int flash_full_erase(void)
   * be deleted
   * @return OK if success or an error code which specify the type of error
   */
-int flash_erase_page_by_page(ErasePage keep_cx)
+int flash_erase_page_by_page(ErasePage keep_cx, Firmware *fw)
 {
 	u8 status, i = 0;
+
+	u8 flash_cx_start_page = FLASH_CX_PAGE_START;
+	u8 flash_cx_end_page = FLASH_CX_PAGE_END;
+	u8 flash_panel_start_page = FLASH_PANEL_PAGE_START;
+	u8 flash_panel_end_page = FLASH_PANEL_PAGE_END;
+
 	u8 cmd1[6] = { FTS_CMD_HW_REG_W,      0x20,	 0x00,	    0x00,
 		       FLASH_ERASE_CODE0 + 1, 0x00 };
 	u8 cmd[6] = { FTS_CMD_HW_REG_W, 0x20, 0x00, 0x00, FLASH_ERASE_CODE0,
@@ -639,14 +693,39 @@ int flash_erase_page_by_page(ErasePage keep_cx)
 	int buff_len = sizeof(buff);
 	int index = 0;
 
-	for (i = FLASH_CX_PAGE_START; i <= FLASH_CX_PAGE_END && keep_cx >=
+	if ((fw->fw_code_size == 0) ||
+	(fw->panel_config_size == 0) ||
+	(fw->cx_area_size == 0) || (fw->fw_config_size == 0)) {
+		pr_info(" using default page address!\n");
+	} else {
+		flash_panel_start_page = fw->fw_code_size;
+		if (fw->panel_config_size > 1)
+			flash_panel_end_page = flash_panel_start_page +
+				(fw->panel_config_size - 1);
+		else
+			flash_panel_end_page = flash_panel_start_page;
+
+		flash_cx_start_page = flash_panel_end_page + 1;
+		if (fw->cx_area_size > 1)
+			flash_cx_end_page = flash_cx_start_page +
+				(fw->cx_area_size - 1);
+		else
+			flash_cx_end_page = flash_cx_start_page;
+	}
+
+	pr_info(" CX Start page: %d CX end page: %d Panel Start Page: %d"
+		"Panel End page: %d!\n", flash_cx_start_page,
+		flash_cx_end_page, flash_panel_start_page,
+		flash_panel_end_page);
+
+	for (i = flash_cx_start_page; i <= flash_cx_end_page && keep_cx >=
 	     SKIP_PANEL_CX_INIT; i++) {
 		pr_info("Skipping erase CX page %d!\n", i);
 		fromIDtoMask(i, mask, 4);
 	}
 
 
-	for (i = FLASH_PANEL_PAGE_START; i <= FLASH_PANEL_PAGE_END && keep_cx >=
+	for (i = flash_panel_start_page; i <= flash_panel_end_page && keep_cx >=
 	     SKIP_PANEL_INIT; i++) {
 		pr_info("Skipping erase Panel Init page %d!\n", i);
 		fromIDtoMask(i, mask, 4);
@@ -963,11 +1042,11 @@ start:
 	pr_info(" 6) FLASH ERASE:\n");
 	if (keep_cx > 0) {
 		if (fw.sec2_size != 0 && force_burn == CRC_CX)
-			res = flash_erase_page_by_page(SKIP_PANEL_INIT);
+			res = flash_erase_page_by_page(SKIP_PANEL_INIT, &fw);
 		else
-			res = flash_erase_page_by_page(SKIP_PANEL_CX_INIT);
+			res = flash_erase_page_by_page(SKIP_PANEL_CX_INIT, &fw);
 	} else {
-		res = flash_erase_page_by_page(SKIP_PANEL_INIT);
+		res = flash_erase_page_by_page(SKIP_PANEL_INIT, &fw);
 		if (fw.sec2_size == 0)
 			pr_err("WARNING!!! Erasing CX memory but no CX in fw file! touch will not work right after fw update!\n");
 	}
@@ -980,7 +1059,7 @@ start:
 	pr_info("   flash erase COMPLETED!\n\n");
 
 	pr_info(" 7) LOAD PROGRAM:\n");
-	res = fillFlash(FLASH_ADDR_CODE, &fw.data[0], fw.sec0_size);
+	res = fillFlash(fw.code_start_addr, &fw.data[0], fw.sec0_size);
 	if (res < OK) {
 		pr_err("   load program ERROR %08X\n",
 			 ERROR_FLASH_BURN_FAILED);
@@ -989,7 +1068,7 @@ start:
 	pr_info("   load program DONE!\n");
 
 	pr_info(" 8) LOAD CONFIG:\n");
-	res = fillFlash(FLASH_ADDR_CONFIG, &(fw.data[fw.sec0_size]),
+	res = fillFlash(fw.config_start_addr, &(fw.data[fw.sec0_size]),
 			fw.sec1_size);
 	if (res < OK) {
 		pr_err("   load config ERROR %08X\n",
@@ -1000,7 +1079,7 @@ start:
 
 	if (fw.sec2_size != 0 && (force_burn == CRC_CX || keep_cx <= 0)) {
 		pr_info(" 8.1) LOAD CX:\n");
-		res = fillFlash(FLASH_ADDR_CX,
+		res = fillFlash(fw.cx_start_addr,
 				&(fw.data[fw.sec0_size + fw.sec1_size]),
 				fw.sec2_size);
 		if (res < OK) {
diff --git a/fts_lib/ftsFlash.h b/fts_lib/ftsFlash.h
index 6bdc99d..2a52c53 100644
--- a/fts_lib/ftsFlash.h
+++ b/fts_lib/ftsFlash.h
@@ -50,12 +50,25 @@
 #define PATH_FILE_FW		"ftm5_fw.ftb"	/* new FW bin file name */
 #endif
 
+#ifdef ALIX
+#define FLASH_CHUNK		(32 * 1024)	/* /< Max number of bytes that
+						 * the
+						  * DMA can burn on flash in
+						  *one shot in FTI */
+#else
 #define FLASH_CHUNK		(64 * 1024)	/* /< Max number of bytes that
-						 * the DMA can burn on the flash
-						 * in one shot in FTI */
+						 * the
+						  * DMA can burn on flash in
+						  *one shot in FTI */
+#endif
+
 #define DMA_CHUNK		32	/* /< Max number of bytes that can be
 					 * written in I2C to the DMA */
 
+#define FLASH_ORG_INFO_INDEX	280
+
+#define FLASH_PAGE_SIZE		(4 * 1024) //page size of 4KB
+
 /**
   * Define which kind of erase page by page should be performed
   */
@@ -87,6 +100,17 @@ typedef struct {
 	u32 sec1_size;	/* /< dimension of section 1 (Config) in .ftb file */
 	u32 sec2_size;	/* /< dimension of section 2 (Cx) in .ftb file */
 	u32 sec3_size;	/* /< dimension of section 3 (TBD) in .ftb file */
+	u8 fw_code_size; /* /< size of fw code in pages in
+			.ftb file */
+	u8 panel_config_size;/* /< size of panel area in pages in
+			.ftb file */
+	u8 cx_area_size;/* /< size of cx area in pages in
+			 .ftb file */
+	u8 fw_config_size;/* /< size of fw config in pages in
+			.ftb file */
+	u32 code_start_addr;  /* start addr for fw code */
+	u32 cx_start_addr; /* start addr for cx area */
+	u32 config_start_addr; /* start addr for config area */
 } Firmware;
 
 /** @}*/
@@ -99,7 +123,7 @@ int wait_for_flash_ready(u8 type);
 int hold_m3(void);
 int flash_erase_unlock(void);
 int flash_full_erase(void);
-int flash_erase_page_by_page(ErasePage keep_cx);
+int flash_erase_page_by_page(ErasePage keep_cx, Firmware *fw);
 int start_flash_dma(void);
 int fillFlash(u32 address, u8 *data, int size);
 
diff --git a/fts_lib/ftsHardware.h b/fts_lib/ftsHardware.h
index 1edfed9..7e2b6cd 100644
--- a/fts_lib/ftsHardware.h
+++ b/fts_lib/ftsHardware.h
@@ -23,8 +23,15 @@
 #define FTS_HARDWARE_H
 
 /* DIGITAL CHIP INFO */
+#define ALIX
+
+#ifdef ALIX
+#define DCHIP_ID_0			0x36	/* /< LSB chip ID for FTM5 */
+#define DCHIP_ID_1			0x48	/* /< MSB chip ID for FTM5 */
+#else
 #define DCHIP_ID_0			0x36	/* /< LSB chip ID for FTM5 */
 #define DCHIP_ID_1			0x39	/* /< MSB chip ID for FTM5 */
+#endif
 
 #define DCHIP_FW_VER_BYTE		2	/* /< number of bytes of the fw
 						 * versions */
@@ -298,6 +305,10 @@
 								 * GPIO config
 								 * register */
 
+#define ADDR_GPIO_CONFIG_REG3	((u64)0x000000002000003E)	/* /< address of
+								 * GPIO config
+								 * register */
+
 /**@}*/
 
 
diff --git a/fts_lib/ftsTest.c b/fts_lib/ftsTest.c
index a12f3cb..b0a26ea 100644
--- a/fts_lib/ftsTest.c
+++ b/fts_lib/ftsTest.c
@@ -68,9 +68,10 @@ int initTestToDo(void)
 
 #ifndef COMPUTE_INIT_METHOD
 	tests.MutualRawAdjITO = 1;
+	tests.MutualRawMapITO = 1;
 
-	tests.MutualRaw = 1;
-	tests.MutualRawMap = 0;
+	tests.MutualRaw = 0;
+	tests.MutualRawMap = 1;
 	tests.MutualRawGap = 0;
 	tests.MutualRawAdj = 0;
 	tests.MutualRawAdjGap = 0;
@@ -78,7 +79,7 @@ int initTestToDo(void)
 
 	tests.MutualRawLP = 0;
 	tests.MutualRawGapLP = 0;
-	tests.MutualRawMapLP = 0;
+	tests.MutualRawMapLP = 1;
 	tests.MutualRawAdjLP = 0;
 
 	tests.MutualCx1 = 0;
@@ -88,8 +89,8 @@ int initTestToDo(void)
 	tests.MutualCxTotalAdj = 0;
 
 	tests.MutualCx1LP = 0;
-	tests.MutualCx2LP = 1;
-	tests.MutualCx2AdjLP = 1;
+	tests.MutualCx2LP = 0;
+	tests.MutualCx2AdjLP = 0;
 	tests.MutualCxTotalLP = 0;
 	tests.MutualCxTotalAdjLP = 0;
 
@@ -106,16 +107,18 @@ int initTestToDo(void)
 	tests.MutualKeyCxTotal = 0;
 #endif
 
-	tests.SelfForceRaw = 1;
+	tests.SelfForceRaw = 0;
 	tests.SelfForceRawGap = 0;
+	tests.SelfForceRawMap = 1;
 
-	tests.SelfForceRawLP = 1;
+	tests.SelfForceRawLP = 0;
 	tests.SelfForceRawGapLP = 0;
+	tests.SelfForceRawMapLP = 1;
 
 	tests.SelfForceIx1 = 0;
 	tests.SelfForceIx2 = 0;
 	tests.SelfForceIx2Adj = 0;
-	tests.SelfForceIxTotal = 1;
+	tests.SelfForceIxTotal = 0;
 	tests.SelfForceIxTotalAdj = 0;
 	tests.SelfForceCx1 = 0;
 	tests.SelfForceCx2 = 0;
@@ -125,7 +128,7 @@ int initTestToDo(void)
 	tests.SelfForceIx1LP = 0;
 	tests.SelfForceIx2LP = 0;
 	tests.SelfForceIx2AdjLP = 0;
-	tests.SelfForceIxTotalLP = 1;
+	tests.SelfForceIxTotalLP = 0;
 	tests.SelfForceIxTotalAdjLP = 0;
 	tests.SelfForceCx1LP = 0;
 	tests.SelfForceCx2LP = 0;
@@ -133,16 +136,18 @@ int initTestToDo(void)
 	tests.SelfForceCxTotalLP = 0;
 	tests.SelfForceCxTotalAdjLP = 0;
 
-	tests.SelfSenseRaw = 1;
+	tests.SelfSenseRaw = 0;
 	tests.SelfSenseRawGap = 0;
+	tests.SelfSenseRawMap = 1;
 
 	tests.SelfSenseRawLP = 0;
 	tests.SelfSenseRawGapLP = 0;
+	tests.SelfSenseRawMapLP = 1;
 
 	tests.SelfSenseIx1 = 0;
 	tests.SelfSenseIx2 = 0;
 	tests.SelfSenseIx2Adj = 0;
-	tests.SelfSenseIxTotal = 1;
+	tests.SelfSenseIxTotal = 0;
 	tests.SelfSenseIxTotalAdj = 0;
 	tests.SelfSenseCx1 = 0;
 	tests.SelfSenseCx2 = 0;
@@ -161,6 +166,7 @@ int initTestToDo(void)
 	tests.SelfSenseCxTotalAdjLP = 0;
 #else
 	tests.MutualRawAdjITO = 1;
+	tests.MutualRawMapITO = 1;
 
 	tests.MutualRaw = 1;  /* in case of YOCTA please use Map */
 	tests.MutualRawMap = 0;
@@ -958,7 +964,7 @@ int production_test_ito(const char *path_limits, TestToDo *todo,
 	pr_info("ITO Command = OK!\n");
 
 	pr_info("MS RAW ITO ADJ TEST:\n");
-	if (todo->MutualRawAdjITO == 1) {
+	if (todo->MutualRawAdjITO == 1 || todo->MutualRawMapITO == 1) {
 		pr_info("Collecting MS Raw data...\n");
 
 		if (frame != NULL) {
@@ -1064,8 +1070,44 @@ int production_test_ito(const char *path_limits, TestToDo *todo,
 
 		kfree(adj);
 		adj = NULL;
+
+		pr_info("MS RAW ITO MIN MAX TEST:\n");
+		if (todo->MutualRawMapITO == 1) {
+			res = parseProductionTestLimits(path_limits,
+			&limit_file,
+			MS_RAW_ITO_MIN_MAX,
+			&thresholds, &trows,
+			&tcolumns);
+		if (res < OK || (trows != 1 || tcolumns != 2)) {
+			pr_err("production_test_data: parseProduction"
+			"TestLimits MS_RAW_ITO_MIN_MAX failed... ERROR %08X\n",
+			ERROR_PROD_TEST_DATA);
+			res |= ERROR_PROD_TEST_DATA;
+			goto ERROR;
+		}
+
+
+		res = checkLimitsMinMax((*ptr_frame).node_data,
+			(*ptr_frame).header.force_node,
+			(*ptr_frame).header.sense_node,
+			thresholds[0],
+			thresholds[1]);
+		if (res != OK) {
+			pr_err("production_test_data: checkLimitsMinMax"
+			" MS RAW ITO failed... ERROR COUNT = %d\n", res);
+			pr_err("MS RAW ITO MIN MAX TEST:................."
+			"FAIL\n\n");
+			res |= ERROR_PROD_TEST_DATA;
+			goto ERROR;
+		} else
+			pr_info("MS RAW ITO MIN MAX TEST:................OK\n");
+			kfree(thresholds);
+			thresholds = NULL;
+		} else
+			pr_info("MS RAW ITO MIN MAX TEST:................."
+			"SKIPPED\n");
 	} else
-		pr_info("MS RAW ITO ADJ TEST:.................SKIPPED\n");
+		pr_info("MS RAW ITO TEST:.................SKIPPED\n");
 
 ERROR:
 	if (thresholds != NULL)
@@ -1767,7 +1809,7 @@ int production_test_ms_raw_lp(const char *path_limits, int stop_on_fail,
 	/************** Mutual Sense Test **************/
 	pr_info("MS RAW LP DATA TEST:\n");
 	if (todo->MutualRawLP == 1 || todo->MutualRawGapLP == 1 ||
-	    todo->MutualRawAdjLP == 1) {
+	    todo->MutualRawAdjLP == 1 || todo->MutualRawMapLP) {
 		ret = setScanMode(SCAN_MODE_LOCKED, LOCKED_LP_ACTIVE);
 		msleep(WAIT_FOR_FRESH_FRAMES);
 		ret |= setScanMode(SCAN_MODE_ACTIVE, 0x00);
diff --git a/fts_lib/ftsTest.h b/fts_lib/ftsTest.h
index a50bb3c..c17f646 100644
--- a/fts_lib/ftsTest.h
+++ b/fts_lib/ftsTest.h
@@ -97,6 +97,7 @@
 #define MS_RAW_ADJV			"MS_RAW_DATA_ADJ_VERTICAL"
 #define MS_RAW_ITO_ADJH			"MS_RAW_ITO_DATA_ADJ_HORIZONTAL"
 #define MS_RAW_ITO_ADJV			"MS_RAW_ITO_DATA_ADJ_VERTICAL"
+#define MS_RAW_ITO_MIN_MAX		"MS_RAW_ITO_MIN_MAX"
 #define MS_RAW_LP_MIN_MAX		"MS_RAW_LOWPOWER_DATA_MIN_MAX"
 #define MS_RAW_LP_EACH_NODE_MIN		"MS_RAW_LOWPOWER_DATA_EACH_MIN"
 #define MS_RAW_LP_EACH_NODE_MAX		"MS_RAW_LOWPOWER_DATA_EACH_MAX"
@@ -248,6 +249,7 @@ typedef struct {
 	int MutualRawGapLP;	/* /< MS Low Power Raw Gap(max-min) test */
 	int MutualRawAdjLP;	/* /< MS Low Power Raw Adjacent test */
 	int MutualRawAdjITO;	/* /< MS Raw Adjacent test during ITO test */
+	int MutualRawMapITO; 	/* /< MS Raw ITO min/Max test */
 
 	int MutualCx1;	/* /< MS Cx1 min/Max test */
 	int MutualCx2;	/* /< MS Cx2 min/Max (for each node) test */
diff --git a/fts_lib/ftsTool.c b/fts_lib/ftsTool.c
index de51a8f..6a3f7c5 100644
--- a/fts_lib/ftsTool.c
+++ b/fts_lib/ftsTool.c
@@ -355,22 +355,22 @@ short **array1dTo2d_short(short *data, int size, int columns)
 	int i;
 	short **matrix = NULL;
 
-	if (size == 0) {
-		matrix = (short **)kmalloc_array(1,
-				sizeof(short *), GFP_KERNEL);
-		matrix[0] = (short *)kmalloc_array(0,
-				sizeof(short), GFP_KERNEL);
-	} else {
+	if (size != 0)
 		matrix = (short **)kmalloc_array(((int)(size / columns)),
 				sizeof(short *), GFP_KERNEL);
 
-		if (matrix != NULL) {
-			for (i = 0; i < (int)(size / columns); i++)
-				matrix[i] = (short *)kmalloc_array(columns,
-						sizeof(short), GFP_KERNEL);
+	if (matrix != NULL) {
+		for (i = 0; i < (int)(size / columns); i++) {
+			matrix[i] = (short *)kmalloc_array(columns,
+					sizeof(short), GFP_KERNEL);
+			if (!matrix[i])
+				break;
+		}
 
-			for (i = 0; i < size; i++)
-				matrix[i / columns][i % columns] = data[i];
+		for (i = 0; i < size; i++) {
+			if (!matrix[i / columns])
+				break;
+			matrix[i / columns][i % columns] = data[i];
 		}
 	}
 
@@ -391,20 +391,22 @@ u16 **array1dTo2d_u16(u16 *data, int size, int columns)
 	int i;
 	u16 **matrix = NULL;
 
-	if (size == 0) {
-		matrix = (u16 **)kmalloc_array(1, sizeof(u16 *), GFP_KERNEL);
-		matrix[0] = (u16 *)kmalloc_array(0, sizeof(u16), GFP_KERNEL);
-	} else {
+	if (size != 0)
 		matrix = (u16 **)kmalloc_array(((int)(size / columns)),
 				sizeof(u16 *), GFP_KERNEL);
 
-		if (matrix != NULL) {
-			for (i = 0; i < (int)(size / columns); i++)
-				matrix[i] = (u16 *)kmalloc_array(columns,
-						sizeof(u16), GFP_KERNEL);
+	if (matrix != NULL) {
+		for (i = 0; i < (int)(size / columns); i++) {
+			matrix[i] = (u16 *)kmalloc_array(columns,
+					sizeof(u16), GFP_KERNEL);
+			if (!matrix[i])
+				break;
+		}
 
-			for (i = 0; i < size; i++)
-				matrix[i / columns][i % columns] = data[i];
+		for (i = 0; i < size; i++) {
+			if (!matrix[i / columns])
+				break;
+			matrix[i / columns][i % columns] = data[i];
 		}
 	}
 
@@ -425,22 +427,23 @@ u8 **array1dTo2d_u8(u8 *data, int size, int columns)
 	int i;
 	u8 **matrix = NULL;
 
-	if (size == 0) {
-		matrix = (u8 **)kmalloc_array(1, sizeof(u8 *), GFP_KERNEL);
-
-		matrix[0] = (u8 *)kmalloc_array(0, sizeof(u8), GFP_KERNEL);
-	} else {
-
+	if (size != 0) {
 		matrix = (u8 **)kmalloc_array(((int)(size / columns)),
 				sizeof(u8 *), GFP_KERNEL);
+	}
 
-		if (matrix != NULL) {
-			for (i = 0; i < (int)(size / columns); i++)
-				matrix[i] = (u8 *)kmalloc_array(columns,
-						sizeof(u8), GFP_KERNEL);
+	if (matrix != NULL) {
+		for (i = 0; i < (int)(size / columns); i++) {
+			matrix[i] = (u8 *)kmalloc_array(columns,
+					sizeof(u8), GFP_KERNEL);
+			if (!matrix[i])
+				break;
+		}
 
-			for (i = 0; i < size; i++)
-				matrix[i / columns][i % columns] = data[i];
+		for (i = 0; i < size; i++) {
+			if (!matrix[i / columns])
+				break;
+			matrix[i / columns][i % columns] = data[i];
 		}
 	}
 
@@ -461,20 +464,22 @@ i8 **array1dTo2d_i8(i8 *data, int size, int columns)
 	int i;
 	i8 **matrix = NULL;
 
-	if (size == 0) {
-		matrix = (i8 **)kmalloc_array(1, sizeof(i8 *), GFP_KERNEL);
-		matrix[0] = (i8 *)kmalloc_array(0, sizeof(i8), GFP_KERNEL);
-	} else {
+	if (size != 0)
 		matrix = (i8 **)kmalloc_array(((int)(size / columns)),
 				sizeof(i8 *), GFP_KERNEL);
 
-		if (matrix != NULL) {
-			for (i = 0; i < (int)(size / columns); i++)
-				matrix[i] = (i8 *)kmalloc_array(columns,
-						sizeof(i8), GFP_KERNEL);
+	if (matrix != NULL) {
+		for (i = 0; i < (int)(size / columns); i++) {
+			matrix[i] = (i8 *)kmalloc_array(columns,
+					sizeof(i8), GFP_KERNEL);
+			if (!matrix[i])
+				break;
+		}
 
-			for (i = 0; i < size; i++)
-				matrix[i / columns][i % columns] = data[i];
+		for (i = 0; i < size; i++) {
+			if (!matrix[i / columns])
+				break;
+			matrix[i / columns][i % columns] = data[i];
 		}
 	}
 
@@ -495,6 +500,11 @@ void print_frame_short(char *label, short **matrix, int row, int column)
 	int buff_len, index;
 	char *buff;
 
+	pr_info("%s\n", label);
+
+	if (matrix == NULL)
+		return;
+
 	buff_len = (6 + 1) * column + 1; /* -32768 str len: 6 */
 	buff = kzalloc(buff_len, GFP_KERNEL);
 	if (buff == NULL) {
@@ -502,8 +512,9 @@ void print_frame_short(char *label, short **matrix, int row, int column)
 		return;
 	}
 
-	pr_info("%s\n", label);
 	for (i = 0; i < row; i++) {
+		if (!matrix[i])
+			break;
 		index = 0;
 		for (j = 0; j < column; j++)
 			index += scnprintf(buff + index, buff_len - index,
@@ -529,6 +540,11 @@ void print_frame_u16(char *label, u16 **matrix, int row, int column)
 	int buff_len, index;
 	char *buff;
 
+	pr_info("%s\n", label);
+
+	if (matrix == NULL)
+		return;
+
 	buff_len = (5 + 1) * column + 1; /* 65535 str len: 5 */
 	buff = kzalloc(buff_len, GFP_KERNEL);
 	if (buff == NULL) {
@@ -536,8 +552,9 @@ void print_frame_u16(char *label, u16 **matrix, int row, int column)
 		return;
 	}
 
-	pr_info("%s\n", label);
 	for (i = 0; i < row; i++) {
+		if (!matrix[i])
+			break;
 		index = 0;
 		for (j = 0; j < column; j++)
 			index += scnprintf(buff + index, buff_len - index,
@@ -563,6 +580,11 @@ void print_frame_u8(char *label, u8 **matrix, int row, int column)
 	int buff_len, index;
 	char *buff;
 
+	pr_info("%s\n", label);
+
+	if (matrix == NULL)
+		return;
+
 	buff_len = (3 + 1) * column + 1; /* 255 str len: 3 */
 	buff = kzalloc(buff_len, GFP_KERNEL);
 	if (buff == NULL) {
@@ -570,8 +592,9 @@ void print_frame_u8(char *label, u8 **matrix, int row, int column)
 		return;
 	}
 
-	pr_info("%s\n", label);
 	for (i = 0; i < row; i++) {
+		if (!matrix[i])
+			break;
 		index = 0;
 		for (j = 0; j < column; j++)
 			index += scnprintf(buff + index, buff_len - index,
@@ -597,6 +620,11 @@ void print_frame_i8(char *label, i8 **matrix, int row, int column)
 	int buff_len, index;
 	char *buff;
 
+	pr_info("%s\n", label);
+
+	if (matrix == NULL)
+		return;
+
 	buff_len = (4 + 1) * column + 1; /* -128 str len: 4 */
 	buff = kzalloc(buff_len, GFP_KERNEL);
 	if (buff == NULL) {
@@ -604,8 +632,9 @@ void print_frame_i8(char *label, i8 **matrix, int row, int column)
 		return;
 	}
 
-	pr_info("%s\n", label);
 	for (i = 0; i < row; i++) {
+		if (!matrix[i])
+			break;
 		index = 0;
 		for (j = 0; j < column; j++)
 			index += scnprintf(buff + index, buff_len - index,
@@ -631,6 +660,11 @@ void print_frame_u32(char *label, u32 **matrix, int row, int column)
 	int buff_len, index;
 	char *buff;
 
+	pr_info("%s\n", label);
+
+	if (matrix == NULL)
+		return;
+
 	buff_len = (10 + 1) * column + 1; /* 4294967295 str len: 10 */
 	buff = kzalloc(buff_len, GFP_KERNEL);
 	if (buff == NULL) {
@@ -638,8 +672,9 @@ void print_frame_u32(char *label, u32 **matrix, int row, int column)
 		return;
 	}
 
-	pr_info("%s\n", label);
 	for (i = 0; i < row; i++) {
+		if (!matrix[i])
+			break;
 		index = 0;
 		for (j = 0; j < column; j++)
 			index += scnprintf(buff + index, buff_len - index,
@@ -665,6 +700,11 @@ void print_frame_int(char *label, int **matrix, int row, int column)
 	int buff_len, index;
 	char *buff;
 
+	pr_info("%s\n", label);
+
+	if (matrix == NULL)
+		return;
+
 	buff_len = (11 + 1) * column + 1; /* -2147483648 str len: 11 */
 	buff = kzalloc(buff_len, GFP_KERNEL);
 	if (buff == NULL) {
@@ -672,8 +712,9 @@ void print_frame_int(char *label, int **matrix, int row, int column)
 		return;
 	}
 
-	pr_info("%s\n", label);
 	for (i = 0; i < row; i++) {
+		if (!matrix[i])
+			break;
 		index = 0;
 		for (j = 0; j < column; j++)
 			index += scnprintf(buff + index, buff_len - index,
diff --git a/fts_proc.c b/fts_proc.c
index d00677e..b6cbd4e 100644
--- a/fts_proc.c
+++ b/fts_proc.c
@@ -1967,13 +1967,22 @@ static ssize_t fts_driver_test_write(struct file *file, const char __user *buf,
 
 		case CMD_FLASHERASEPAGE:
 			if (numberParam == 2) {	/* need to pass: keep_cx */
+				pr_info("Reading FW File...\n");
+				res = readFwFile(info->board->fw_name, &fw,
+						funcToTest[1]);
+				if (res < OK)
+					pr_err("Error reading FW File ERROR"
+						"%08X\n", res);
+				else
+					pr_info("Read FW File Finished!\n");
 				pr_info("Starting Flashing Page Erase...\n");
-				res = flash_erase_page_by_page(cmd[1]);
+				res = flash_erase_page_by_page(cmd[1], &fw);
 				if (res < OK)
 					pr_err("Error during flash page erase... ERROR %08X\n",
 						res);
 				else
 					pr_info("Flash Page Erase Finished!\n");
+				kfree(fw.data);
 			} else {
 				pr_err("Wrong number of parameters!\n");
 				res = ERROR_OP_NOT_ALLOW;