/*
 * Copyright 2016-2022 NXP
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of NXP Semiconductor, Inc. nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * @file    LPC55S06_Project.c
 * @brief   Application entry point.
 */
#include <stdio.h>
#include "board.h"
#include "pin_mux.h"
#include "clock_config.h"
#include "LPC55S69_cm33_core0.h"
#include "fsl_debug_console.h"


#include "fsl_iap.h"
#include "fsl_iap_ffr.h"
#include "fsl_usart.h"


////////////////////////////////////////////////////////////////////////////////
// Definitions
///////////////////////////////////////////////////////////////////////////////
#define APP_USART_RX_ERROR               kUSART_RxError

#define APP_READ_ONLY_UUID 			      0
#define APP_INCREASE_VERSION_ONLY         1
#define APP_INCREASE_VENDOR_USAGE_ONLY	  2
#define APP_INCREASE_IMAGEKEY_REVOKE_ONLY 3
#define APP_TRY_ROLLBACK                  4
#define APP_INCREASE_NSFW_ONLY            5
#define APP_TRY_ROLLBACK_NSFW             6



////////////////////////////////////////////////////////////////////////////////
// Variables
////////////////////////////////////////////////////////////////////////////////
volatile uint32_t g_UUID[4];
uint32_t g_CFPAData[FLASH_FFR_MAX_PAGE_SIZE / sizeof(uint32_t)];
flash_config_t flashInstance;

uint16_t vendorUsage_right;
uint16_t vendorUsage_left;


////////////////////////////////////////////////////////////////////////////////
// Prototypes
////////////////////////////////////////////////////////////////////////////////
void verify_status(status_t status);

void error_trap(void);

static uint32_t APP_GetUserSelection();

uint32_t ProtectFlashCfg( uint32_t ch);

/*
 * @brief   Application entry point.
 */

int main(void) {

	/* Init board hardware. */

	/* attach 12 MHz clock to FLEXCOMM0 (debug console) */
	CLOCK_AttachClk(BOARD_DEBUG_UART_CLK_ATTACH);

	BOARD_InitBootPins();
	BOARD_BootClockFROHF96M();
	//BOARD_InitBootPeripherals();
#ifndef BOARD_INIT_DEBUG_CONSOLE_PERIPHERAL
	/* Init FSL debug console. */

	BOARD_InitDebugConsole();
#endif


	/* Print basic information for Flash Driver API.*/
	PRINTF("\r\nEnable/Disable LPC55S6x Flash Read example...\r\n");

	PRINTF("\r\nWrite in the console one of the following options...\r\n");
	PRINTF("\r\n 0 to READ  UUID and CFPA page ...\r\n");
	PRINTF("\r\n 1 to Increase Version Only ...\r\n");
	PRINTF("\r\n 2 to Increase Vendor Usage...\r\n");
	PRINTF("\r\n 3 to Increase ImageKey Revoke...\r\n");
	PRINTF("\r\n 4 to Try to RollBack...\r\n");
	PRINTF("\r\n 5 to Increase NSFW Version...\r\n");
	PRINTF("\r\n 6 to Try to RollBack NSFW...\r\n");
	PRINTF("\r\n\r\n");


	volatile uint32_t usr_cmd ;
	while(1) {
		/* wait for commands from the console*/
		usr_cmd = APP_GetUserSelection();
		/* process the user command**/
		ProtectFlashCfg( usr_cmd);
	}

	/** program should never reach this line*/
	return 0 ;
}


void verify_status(status_t status)
{
	char *tipString = "Unknown status";
	switch (status)
	{
	case kStatus_Success:
		tipString = "Done.";
		break;
	case kStatus_InvalidArgument:
		tipString = "Invalid argument.";
		break;
	case kStatus_FLASH_AlignmentError:
		tipString = "Alignment Error.";
		break;
	case kStatus_FLASH_AccessError:
		tipString = "Flash Access Error.";
		break;
	case kStatus_FLASH_CommandNotSupported:
		tipString = "This API is not supported in current target.";
		break;
	default:
		break;
	}
	PRINTF("%s\r\n\r\n", tipString);
}

/*
 * @brief Gets called when an error occurs.
 */
void error_trap(void)
{
	PRINTF("\r\n\r\n\r\n\t---- HALTED DUE TO FLASH ERROR! ----");
	while (1)
	{
	}
}

static uint32_t APP_GetUserSelection()
{
	uint32_t ch;

	/* Clear rx overflow error once it happens during low power mode. */
	if (APP_USART_RX_ERROR == (APP_USART_RX_ERROR & USART_GetStatusFlags((USART_Type *)BOARD_DEBUG_UART_BASEADDR)))
	{
		USART_ClearStatusFlags((USART_Type *)BOARD_DEBUG_UART_BASEADDR, APP_USART_RX_ERROR);
	}


	/**  usr can not exit this get char loop until he gives a correct option.*/
	while(1)
	{
		ch = GETCHAR();
		if ((ch < '0') || (ch > '7')) /* Only '0', '1', '2'. */

		{
			continue;
		}
		else
		{
			ch = ch - '0'; /* Only '0', '1', '2'. */
			break;
		}
	}
	switch (ch)
	{
	case 0:
		ch = APP_READ_ONLY_UUID;
		PRINTF("\r\n %c \r\n", ch + '0');
		break;
	case 1:
		ch = APP_INCREASE_VERSION_ONLY;
		PRINTF("\r\n %c \r\n", ch+ '0');
		break;
	case 2:
		ch = APP_INCREASE_VENDOR_USAGE_ONLY;
		PRINTF("\r\n %c \r\n", ch+ '0');
		break;
	case 3:
		ch = APP_INCREASE_IMAGEKEY_REVOKE_ONLY;
		PRINTF("\r\n %c \r\n", ch+ '0');
		break;
	case 4:
		ch = APP_TRY_ROLLBACK;
		PRINTF("\r\n %c \r\n", ch+ '0');
		break;
	case 5:
		ch = APP_INCREASE_NSFW_ONLY;
		PRINTF("\r\n %c \r\n", ch+ '0');
		break;
	case 6:
		ch = APP_TRY_ROLLBACK_NSFW;
		PRINTF("\r\n %c \r\n", ch+ '0');
		break;


	}
	return ch;

}

uint32_t ProtectFlashCfg( uint32_t ch)
{
	int32_t kCommandIndex = ch;

	static uint32_t status;

	switch (kCommandIndex)
	{
	/* 0, Read chip UUID */
	case APP_READ_ONLY_UUID:
		PRINTF("Read CHIP UUID and read CFPA...\r\n");
		/* Initialize flash driver */
		FLASH_Init(&flashInstance);

		if (FFR_Init(&flashInstance) == kStatus_Success)
		{
			PRINTF("Flash init successfull!!. Halting...\r\n");
		}
		else
		{
			error_trap();
		}

		status =  FFR_GetUUID(&flashInstance, (uint8_t *)g_UUID);
		PRINTF("UUID 0x%8x 0x%8x 0x%8x 0x%8x\r\n", g_UUID[0], g_UUID[1], g_UUID[2], g_UUID[3]);

		status = FFR_GetCustomerInfieldData(&flashInstance, (uint8_t *)g_CFPAData, 0x0, FLASH_FFR_MAX_PAGE_SIZE);

		PRINTF("\r\n");
		PRINTF("Header 0x%08x ,            Version  0x%08x ,   SecureFW Version 0x%08x , NonSecureFW Version    0x%08x\r\n", g_CFPAData[0], g_CFPAData[1], g_CFPAData[2], g_CFPAData[3]);
		PRINTF("ImageKey Revoke 0x%08x ,   Reserved  0x%08x ,  RothRevoke 0x%08x ,       Vendor Usage   0x%08x\r\n", g_CFPAData[4], g_CFPAData[5], g_CFPAData[6], g_CFPAData[7]);
		PRINTF("NS PIN 0x%08x ,            NS DFLT  0x%08x ,   Enable FA Mode 0x%08x ,   Reserved1 0x%08x\r\n", g_CFPAData[8], g_CFPAData[9], g_CFPAData[10], g_CFPAData[11]);
		PRINTF("\r\n");

		break;

		/* 1, disable flash protect */
	case APP_INCREASE_VERSION_ONLY:
		PRINTF("Disable Flash Protect...\r\n");
		/* Initialize flash driver */
		FLASH_Init(&flashInstance);

		if (FFR_Init(&flashInstance) == kStatus_Success)
		{
			PRINTF("Flash init successfull!!. Halting...\r\n");
		}
		else
		{
			error_trap();
		}
		status = FFR_GetCustomerInfieldData(&flashInstance, (uint8_t *)g_CFPAData, 0x0, FLASH_FFR_MAX_PAGE_SIZE);
		PRINTF("\r\n");
		PRINTF("Header 0x%08x ,            Version  0x%08x ,   SecureFW Version 0x%08x , NonSecureFW Version    0x%08x\r\n", g_CFPAData[0], g_CFPAData[1], g_CFPAData[2], g_CFPAData[3]);
		PRINTF("ImageKey Revoke 0x%08x ,   Reserved  0x%08x ,  RothRevoke 0x%08x ,       Vendor Usage   0x%08x\r\n", g_CFPAData[4], g_CFPAData[5], g_CFPAData[6], g_CFPAData[7]);
		PRINTF("NS PIN 0x%08x ,            NS DFLT  0x%08x ,   Enable FA Mode 0x%08x ,   Reserved1 0x%08x\r\n", g_CFPAData[8], g_CFPAData[9], g_CFPAData[10], g_CFPAData[11]);
		PRINTF("\r\n");


		/* Clean-up CFPA area */
		g_CFPAData[8]  = 0;
		g_CFPAData[9] = 0;

		/*Increase Monotonic counter*/
		uint32_t *p32    = (uint32_t *)(uint32_t)g_CFPAData;
		uint32_t version = p32[1];


		if (version == 0xFFFFFFFFu)
		{
			return kStatus_Fail;
		}
		version++;
		p32[1] = version;

		PRINTF("Version to write: 0x%08x \r\n", version);

		status_t Status = FFR_InfieldPageWrite(&flashInstance, (uint8_t *)g_CFPAData, FLASH_FFR_MAX_PAGE_SIZE);
		if (kStatus_FLASH_Success == Status)
		{
			status = kStatus_Success;
			PRINTF("CFPA Write Done!\r\n");
		}
		else
		{
			status = kStatus_Fail;
		}

		NVIC_SystemReset();

		break;
	case APP_INCREASE_VENDOR_USAGE_ONLY:

		PRINTF("Disable Flash Protect...\r\n");
		/* Initialize flash driver */
		FLASH_Init(&flashInstance);

		if (FFR_Init(&flashInstance) == kStatus_Success)
		{
			PRINTF("Flash init successfull!!. Halting...\r\n");
		}
		else
		{
			error_trap();
		}

		status = FFR_GetCustomerInfieldData(&flashInstance, (uint8_t *)g_CFPAData, 0x0, FLASH_FFR_MAX_PAGE_SIZE);
		PRINTF("\r\n");
		PRINTF("Header 0x%08x ,            Version  0x%08x ,   SecureFW Version 0x%08x , NonSecureFW Version    0x%08x\r\n", g_CFPAData[0], g_CFPAData[1], g_CFPAData[2], g_CFPAData[3]);
		PRINTF("ImageKey Revoke 0x%08x ,   Reserved  0x%08x ,  RothRevoke 0x%08x ,       Vendor Usage   0x%08x\r\n", g_CFPAData[4], g_CFPAData[5], g_CFPAData[6], g_CFPAData[7]);
		PRINTF("NS PIN 0x%08x ,            NS DFLT  0x%08x ,   Enable FA Mode 0x%08x ,   Reserved1 0x%08x\r\n", g_CFPAData[8], g_CFPAData[9], g_CFPAData[10], g_CFPAData[11]);
		PRINTF("\r\n");


		/* Clean-up CFPA area */
			g_CFPAData[8]  = 0;
			g_CFPAData[9] = 0;

		/*Increase Monotonic counter*/
		p32    = (uint32_t *)(uint32_t)g_CFPAData;
		version = p32[1];


		if (version == 0xFFFFFFFFu)
		{
			return kStatus_Fail;
		}
		version++;
		p32[1] = version;

		PRINTF("Version to write: 0x%08x \r\n", version);

		/*Increase Vendor Usage*/
		uint32_t vendor_usage = p32[7];
		if(vendor_usage == 0x0)
		{
			vendorUsage_right = 0xFFFF;
			vendorUsage_left  = 0x0000;
		}
		else
		{
			vendorUsage_right = vendor_usage & 0xFFFF;
			vendorUsage_left = vendor_usage >> 16;
		}

		vendorUsage_left += 0x1;
		vendorUsage_right -= 0x1;
		vendor_usage = (vendorUsage_left << 16) | vendorUsage_right;
		p32[7] = vendor_usage;

		PRINTF("Vendor_Usage to write: 0x%08x \r\n", vendor_usage);

		Status = FFR_InfieldPageWrite(&flashInstance, (uint8_t *)g_CFPAData, FLASH_FFR_MAX_PAGE_SIZE);
		if (kStatus_FLASH_Success == Status)
		{
			status = kStatus_Success;
			PRINTF("CFPA Write Done!\r\n");
		}
		else
		{
			status = kStatus_Fail;
		}

		NVIC_SystemReset();
		break;
	case APP_INCREASE_IMAGEKEY_REVOKE_ONLY:

		PRINTF("Disable Flash Protect...\r\n");
		/* Initialize flash driver */
		FLASH_Init(&flashInstance);

		if (FFR_Init(&flashInstance) == kStatus_Success)
		{
			PRINTF("Flash init successfull!!. Halting...\r\n");
		}
		else
		{
			error_trap();
		}

		status = FFR_GetCustomerInfieldData(&flashInstance, (uint8_t *)g_CFPAData, 0x0, FLASH_FFR_MAX_PAGE_SIZE);
		PRINTF("\r\n");
		PRINTF("Header 0x%08x ,            Version  0x%08x ,   SecureFW Version 0x%08x , NonSecureFW Version    0x%08x\r\n", g_CFPAData[0], g_CFPAData[1], g_CFPAData[2], g_CFPAData[3]);
		PRINTF("ImageKey Revoke 0x%08x ,   Reserved  0x%08x ,  RothRevoke 0x%08x ,       Vendor Usage   0x%08x\r\n", g_CFPAData[4], g_CFPAData[5], g_CFPAData[6], g_CFPAData[7]);
		PRINTF("NS PIN 0x%08x ,            NS DFLT  0x%08x ,   Enable FA Mode 0x%08x ,   Reserved1 0x%08x\r\n", g_CFPAData[8], g_CFPAData[9], g_CFPAData[10], g_CFPAData[11]);
		PRINTF("\r\n");


		/* Clean-up CFPA area */
			g_CFPAData[8]  = 0;
			g_CFPAData[9] = 0;
		/*Increase Monotonic counter*/
		p32    = (uint32_t *)(uint32_t)g_CFPAData;
		version = p32[1];


		if (version == 0xFFFFFFFFu)
		{
			return kStatus_Fail;
		}
		version++;
		p32[1] = version;

		PRINTF("Version to write: 0x%08x \r\n", version);

		/*Increase Vendor Usage*/
		uint32_t imagekey_revoke = p32[4];

		imagekey_revoke = (imagekey_revoke << 1) | 0x1;

		p32[4] = imagekey_revoke;

		PRINTF("ImageKey_Revoke to write: 0x%08x \r\n", imagekey_revoke);

		Status = FFR_InfieldPageWrite(&flashInstance, (uint8_t *)g_CFPAData, FLASH_FFR_MAX_PAGE_SIZE);
		if (kStatus_FLASH_Success == Status)
		{
			status = kStatus_Success;
			PRINTF("CFPA Write Done!\r\n");
		}
		else
		{
			status = kStatus_Fail;
		}

		NVIC_SystemReset();
		break;
	case APP_TRY_ROLLBACK:

		PRINTF("Disable Flash Protect...\r\n");
		/* Initialize flash driver */
		FLASH_Init(&flashInstance);

		if (FFR_Init(&flashInstance) == kStatus_Success)
		{
			PRINTF("Flash init successfull!!. Halting...\r\n");
		}
		else
		{
			error_trap();
		}

		status = FFR_GetCustomerInfieldData(&flashInstance, (uint8_t *)g_CFPAData, 0x0, FLASH_FFR_MAX_PAGE_SIZE);
		PRINTF("\r\n");
		PRINTF("Header 0x%08x ,            Version  0x%08x ,   SecureFW Version 0x%08x , NonSecureFW Version    0x%08x\r\n", g_CFPAData[0], g_CFPAData[1], g_CFPAData[2], g_CFPAData[3]);
		PRINTF("ImageKey Revoke 0x%08x ,   Reserved  0x%08x ,  RothRevoke 0x%08x ,       Vendor Usage   0x%08x\r\n", g_CFPAData[4], g_CFPAData[5], g_CFPAData[6], g_CFPAData[7]);
		PRINTF("NS PIN 0x%08x ,            NS DFLT  0x%08x ,   Enable FA Mode 0x%08x ,   Reserved1 0x%08x\r\n", g_CFPAData[8], g_CFPAData[9], g_CFPAData[10], g_CFPAData[11]);
		PRINTF("\r\n");


		/* Clean-up CFPA area */
			g_CFPAData[8]  = 0;
			g_CFPAData[9] = 0;

		/*Increase Monotonic counter*/
		p32    = (uint32_t *)(uint32_t)g_CFPAData;
		version = p32[1];


		if (version == 0xFFFFFFFFu)
		{
			return kStatus_Fail;
		}
		version++;
		p32[1] = version;

		PRINTF("Version to write: 0x%08x \r\n", version);

		/*Increase Vendor Usage*/
		imagekey_revoke = p32[4];

		imagekey_revoke = imagekey_revoke >> 1;

		p32[4] = imagekey_revoke;

		PRINTF("ImageKey_Revoke to write: 0x%08x \r\n", imagekey_revoke);

		Status = FFR_InfieldPageWrite(&flashInstance, (uint8_t *)g_CFPAData, FLASH_FFR_MAX_PAGE_SIZE);
		if (kStatus_FLASH_Success == Status)
		{
			status = kStatus_Success;
			PRINTF("CFPA Write Done!\r\n");
		}
		else
		{
			status = kStatus_Fail;
		}

		NVIC_SystemReset();
		break;
	case APP_INCREASE_NSFW_ONLY:

		PRINTF("Disable Flash Protect...\r\n");
		/* Initialize flash driver */
		FLASH_Init(&flashInstance);

		if (FFR_Init(&flashInstance) == kStatus_Success)
		{
			PRINTF("Flash init successfull!!. Halting...\r\n");
		}
		else
		{
			error_trap();
		}

		status = FFR_GetCustomerInfieldData(&flashInstance, (uint8_t *)g_CFPAData, 0x0, FLASH_FFR_MAX_PAGE_SIZE);
		PRINTF("\r\n");
		PRINTF("Header 0x%08x ,            Version  0x%08x ,   SecureFW Version 0x%08x , NonSecureFW Version    0x%08x\r\n", g_CFPAData[0], g_CFPAData[1], g_CFPAData[2], g_CFPAData[3]);
		PRINTF("ImageKey Revoke 0x%08x ,   Reserved  0x%08x ,  RothRevoke 0x%08x ,       Vendor Usage   0x%08x\r\n", g_CFPAData[4], g_CFPAData[5], g_CFPAData[6], g_CFPAData[7]);
		PRINTF("NS PIN 0x%08x ,            NS DFLT  0x%08x ,   Enable FA Mode 0x%08x ,   Reserved1 0x%08x\r\n", g_CFPAData[8], g_CFPAData[9], g_CFPAData[10], g_CFPAData[11]);
		PRINTF("\r\n");


		/* Clean-up CFPA area */
			g_CFPAData[8]  = 0;
			g_CFPAData[9] = 0;

		/*Increase Monotonic counter*/
		p32    = (uint32_t *)(uint32_t)g_CFPAData;
		version = p32[1];


		if (version == 0xFFFFFFFFu)
		{
			return kStatus_Fail;
		}
		version++;
		p32[1] = version;

		PRINTF("Version to write: 0x%08x \r\n", version);

		/*Increase NS FW Version*/
		uint32_t NSFW_Version = p32[3];

		NSFW_Version = NSFW_Version + 0x1;

		p32[3] = NSFW_Version;

		PRINTF("NSFW_Version to write: 0x%08x \r\n", NSFW_Version);

		Status = FFR_InfieldPageWrite(&flashInstance, (uint8_t *)g_CFPAData, FLASH_FFR_MAX_PAGE_SIZE);
		if (kStatus_FLASH_Success == Status)
		{
			status = kStatus_Success;
			PRINTF("CFPA Write Done!\r\n");
		}
		else
		{
			status = kStatus_Fail;
		}

		NVIC_SystemReset();
		break;
	case APP_TRY_ROLLBACK_NSFW:

		PRINTF("Disable Flash Protect...\r\n");
		/* Initialize flash driver */
		FLASH_Init(&flashInstance);

		if (FFR_Init(&flashInstance) == kStatus_Success)
		{
			PRINTF("Flash init successfull!!. Halting...\r\n");
		}
		else
		{
			error_trap();
		}

		status = FFR_GetCustomerInfieldData(&flashInstance, (uint8_t *)g_CFPAData, 0x0, FLASH_FFR_MAX_PAGE_SIZE);
		PRINTF("\r\n");
		PRINTF("Header 0x%08x ,            Version  0x%08x ,   SecureFW Version 0x%08x , NonSecureFW Version    0x%08x\r\n", g_CFPAData[0], g_CFPAData[1], g_CFPAData[2], g_CFPAData[3]);
		PRINTF("ImageKey Revoke 0x%08x ,   Reserved  0x%08x ,  RothRevoke 0x%08x ,       Vendor Usage   0x%08x\r\n", g_CFPAData[4], g_CFPAData[5], g_CFPAData[6], g_CFPAData[7]);
		PRINTF("NS PIN 0x%08x ,            NS DFLT  0x%08x ,   Enable FA Mode 0x%08x ,   Reserved1 0x%08x\r\n", g_CFPAData[8], g_CFPAData[9], g_CFPAData[10], g_CFPAData[11]);
		PRINTF("\r\n");


		/* Clean-up CFPA area */
			g_CFPAData[8]  = 0;
			g_CFPAData[9] = 0;

		/*Increase Monotonic counter*/
		p32    = (uint32_t *)(uint32_t)g_CFPAData;
		version = p32[1];


		if (version == 0xFFFFFFFFu)
		{
			return kStatus_Fail;
		}
		version++;
		p32[1] = version;

		PRINTF("Version to write: 0x%08x \r\n", version);

		/*Decrease NS FW Version*/
		NSFW_Version = p32[3];

		NSFW_Version = NSFW_Version - 0x1;

		p32[3] = NSFW_Version;

		PRINTF("NSFW_Version to write: 0x%08x \r\n", NSFW_Version);

		Status = FFR_InfieldPageWrite(&flashInstance, (uint8_t *)g_CFPAData, FLASH_FFR_MAX_PAGE_SIZE);
		if (kStatus_FLASH_Success == Status)
		{
			status = kStatus_Success;
			PRINTF("CFPA Write Done!\r\n");
		}
		else
		{
			status = kStatus_Fail;
		}

		NVIC_SystemReset();
		break;
	default:
		PRINTF("Command index is wrong\r\n");
		break;
	}
	PRINTF(" \r\n");
	return 0 ;

}