/*
 * Copyright (C) 2014, Samsung Electronics Co., Ltd.
 *
 * Custom SMC Handler main program for Exynos 5433
 */

#include "handler.h"
#include "kernel_api.h"
#include "sha.h"
#include "rsa.h"
#include "fcTimaHashCopy.h"
#include "gpio-fp.h"

#define DMVERITY_MODE_SUPPORT		//dmverity addtion

#if defined(CONFIG_EXYNOS) || defined(CONFIG_MEDIATEK)
#define TIMA_ICCC
#define ICCC_SECURE_BASE DRAM_HASH_TABLE_PA
#define ICCC_DATA_OFFSET (ICCC_SECURE_BASE+0x400)
#define ICCC_DEBUG 0
#endif

#if defined(TIMA_FEATURE_FOR_64BIT)
#define TIMA_MAX_SIZE 0x200000
#else
#define TIMA_MAX_SIZE 0x100000
#endif

/**************dmverity**********/
#include "fcDMVERITY.h"
#define NORMAL          10
#define RECOVERY        11

#define FAILED          0xcafebabe
#define SUCCEEDED       0xdeadbeef

#define CMD_UPDATE_BOOT_MODE                    0
#define CMD_UPDATE_SYSTEM_IMAGE_CHECK_STATUS    1
#define CMD_RESTORE_SYSTEM_IMAGE_CHECK_STATUS   2
#define CMD_READ_SYSTEM_IMAGE_CHECK_STATUS      3

#define ERR_DMVERITY_NOT_IN_RECOVERY -1
#define ERR_DMVERITY_UPDATES_LOCKED -2
#define ERR_DMVERITY_INVALID_CMD_ID -3

static uint32_t lock_updates = 0;

/*******************************/

const char test_str[] = "Kernel API printf: %d\n";

#define CMD_ID_KAP_STATUS          0x50
#define CMD_ID_DISABLE_KAP         0x51
#define CMD_ID_DISABLE_KAP_BL      0x52
#define CMD_ID_BOOT_STS_KAP_ON     0x53
#define CMD_ID_BOOT_KAP_VIO        0x54
#define KAP_MAGIC              0x5afe0000
#define FC_KAP_ON       0x11
#define FC_KAP_VIOL     0x01
#define FC_KAP_OFF      0x00

volatile uint32_t KAP_STATUS_FUSE = 0; //This fuse defines the status of RP mode 1: OFF 0:ON
volatile uint32_t KAP_VIOLATION_FUSE = 0; //This fuse records RP violation 1: at lease 1 violation occured 0: None
volatile uint32_t KAP_BOOT_STATUS_FUSE = 0; //This fuse defines the RP mode status on booting up 1: it was ON while booting 0:it was OFF while booting

#define PAGE_SIZE       4096
#define PAGE_SIZE_SHFT	12

uint8_t g_check_call_iccc_dmv=0;
uint8_t g_check_call_iccc_hdm=0;

inline uint32_t fcKapStatus(void)
{
	uint32_t kap_status;
        if (KAP_VIOLATION_FUSE)
	{
                kap_status = 3; //RKP and/or DMVerity says device is tampered
        }
	else if (KAP_STATUS_FUSE)
	{
		kap_status = 0; //KAP OFF
	}
	else
	{
		kap_status = 1; //KAP ON - No tamper
        }
	kap_status |= KAP_MAGIC; //Add the magic to verify the return value
	return kap_status;
}

static int fcICCCKernelInfoSave(iccc_secure_pamameters_info_t *iccc_struct,uint32_t val)
{
	iccc_struct->ta_secure_info.selinux_status=val;
	return 0;
}

static int fcICCCKernelInfoRead(iccc_secure_pamameters_info_t *iccc_struct)
{
	return iccc_struct->ta_secure_info.selinux_status;
}

static int fcICCCHdmSet(iccc_secure_pamameters_info_t *iccc_struct,uint32_t val,uint32_t temp)
{
	int temp1;
	temp1=temp;

	if(temp1==0)
	{
		if(g_check_call_iccc_hdm!=0)
		{
			printf("ICCC : only one time write call is allowed in hdm area");
			return -1;
		}
		iccc_struct->ta_secure_info.hdm_status=val;
		g_check_call_iccc_hdm=1;
	}
	else
	{
		temp1=iccc_struct->ta_secure_info.hdm_status;
		return temp1;
	}
	return 0;
}

static int fcICCCDmvSet(iccc_secure_pamameters_info_t *iccc_struct,uint32_t val,uint32_t temp)
{
	int temp1;
	temp1=temp;

	if(temp1==0)
	{
		if(g_check_call_iccc_dmv!=0)
		{
			printf("ICCC : only one time write call is allowed in dmv area");
			return -1;
		}
		iccc_struct->kern_secure_info.dmv_status=val;
		g_check_call_iccc_dmv=1;
	}
	else
	{
		temp1=iccc_struct->kern_secure_info.dmv_status;
		return temp1;
	}
	return 0;
}

static int IcccSvbResult(iccc_secure_pamameters_info_t *iccc_struct, uint32_t val)
{
	iccc_struct->sys_secure_info.trustboot_flag = val;
	printf("CSMC: IcccSvbResult : 0x%x\n", iccc_struct->sys_secure_info.trustboot_flag);
	return 1;
}

static int IcccDataLock(iccc_secure_pamameters_info_t *iccc_struct)
{
	uint32_t *lock_status;

	lock_status = (uint32_t *)((void *)iccc_struct + ICCC_SECURE_PARAMETERS_LENGTH);
	*lock_status = ICCC_TRUSTBOOT_LOCK;
	printf("CSMC: IcccDataLock : 0x%x\n", *lock_status);

	return 1;
}

static int fcICCCSaveSecureParam(iccc_secure_pamameters_info_t *iccc_dst, smc_secure_info_t *iccc_src)
{
	uint32_t i;

	memset(iccc_dst, 0x0, ICCC_SECURE_PARAMETERS_LENGTH + ICCC_LOCK_FLAG_LENGTH);

	iccc_dst->bl_secure_info.header.magic_str = (uint16_t)BL_MAGIC_STR;
	iccc_dst->bl_secure_info.header.used_size = (uint16_t)(sizeof(bl_secure_info_t));
	iccc_dst->bl_secure_info.rp_ver = iccc_src->rp_ver;
	iccc_dst->bl_secure_info.kernel_rp = iccc_src->kernel_rp;
	iccc_dst->bl_secure_info.system_rp = iccc_src->system_rp;
	iccc_dst->bl_secure_info.test_bit = iccc_src->test_bit;
	iccc_dst->bl_secure_info.sec_boot = iccc_src->sec_boot;
	iccc_dst->bl_secure_info.react_lock = iccc_src->react_lock;
	iccc_dst->bl_secure_info.kiwi_lock = iccc_src->kiwi_lock;
	iccc_dst->bl_secure_info.frp_lock = iccc_src->frp_lock;
	iccc_dst->bl_secure_info.cc_mode = iccc_src->cc_mode;
	iccc_dst->bl_secure_info.mdm_mode = iccc_src->mdm_mode;
	iccc_dst->bl_secure_info.curr_bin_status = iccc_src->curr_bin_status;
	iccc_dst->bl_secure_info.afw_value = iccc_src->afw_value;
	iccc_dst->bl_secure_info.warranty_bit = iccc_src->warranty_bit;
#if defined(CONFIG_PARAMEXPANSION_ICCC)
	iccc_dst->bl_secure_info.image_status_bl = iccc_src->image_status_bl;
	iccc_dst->bl_secure_info.WbHistory = iccc_src->WbHistory;
#endif
#ifdef ICCC_ADD_CPUID
	/* PHASE 3 */
	iccc_dst->bl_secure_info.ap_serial_0 = iccc_src->ap_serial_0;
	iccc_dst->bl_secure_info.ap_serial_1 = iccc_src->ap_serial_1;
#endif
	iccc_dst->bl_secure_info.em_status = iccc_src->em_status;
	iccc_dst->bl_secure_info.em_token = iccc_src->em_token;

	if(iccc_src->kap_status != (uint32_t)0xFFFFFFFF)
	{
		iccc_dst->bl_secure_info.kap_status = (uint32_t)((fcKapStatus()) & 0xF);
	}
	else
	{
		iccc_dst->bl_secure_info.kap_status = iccc_src->kap_status;
	}

	for(i = 0; i < MAX_IMAGES ; i++) {
		if(iccc_src->image_status[i] != 0xFF)
			iccc_dst->bl_secure_info.image_status[i] = (uint32_t)(iccc_src->image_status[i]);
		else
			iccc_dst->bl_secure_info.image_status[i] = 0xFFFFFFFF;
	}

	iccc_dst->ta_secure_info.header.magic_str = (uint16_t)TA_MAGIC_STR;
	iccc_dst->ta_secure_info.header.used_size = (uint16_t)(sizeof(ta_secure_info_t));
	iccc_dst->ta_secure_info.pkm_text = 0xFFFFFFFF;
	iccc_dst->ta_secure_info.pkm_ro = 0xFFFFFFFF;
	iccc_dst->ta_secure_info.selinux_status = 0xFFFFFFFF;

	iccc_dst->ta_secure_info.sectimer_base = 0xFFFFFFFF;
	iccc_dst->ta_secure_info.sectimer_flag = 0xFFFFFFFF;
	iccc_dst->ta_secure_info.sectimer_status = 0xFFFFFFFF;
	iccc_dst->ta_secure_info.hdm_status = 0xFFFFFFFF;

	for (i = 0; i < sizeof(iccc_dst->ta_secure_info.atn_blob_hash)/sizeof(uint32_t); i++)
		iccc_dst->ta_secure_info.atn_blob_hash[i] = 0xFFFFFFFF;

	iccc_dst->kern_secure_info.header.magic_str = (uint16_t)KERN_MAGIC_STR;
	iccc_dst->kern_secure_info.header.used_size = (uint16_t)(sizeof(kern_secure_info_t));
	iccc_dst->kern_secure_info.dmv_status = iccc_src->dmv_status;
	for(i = 0; i < sizeof(iccc_dst->kern_secure_info.verified_boot_hash)/sizeof(uint32_t) ; i++) {
		iccc_dst->kern_secure_info.verified_boot_hash[i] = iccc_src->verified_boot_hash[i];
	}

	iccc_dst->sys_secure_info.header.magic_str = (uint16_t)SYS_MAGIC_STR;
	iccc_dst->sys_secure_info.header.used_size = (uint16_t)(sizeof(sys_secure_info_t));
	iccc_dst->sys_secure_info.sysscope_flag = iccc_src->sysscope_flag;
	iccc_dst->sys_secure_info.trustboot_flag = 0xFFFFFFFF;

#ifdef CONFIG_ROT_IN_ICCC
	iccc_dst->rot_secure_info.header.magic_str = (uint16_t)ROT_MAGIC_STR;
	iccc_dst->rot_secure_info.header.used_size = (uint16_t)(sizeof(rot_secure_info_t));
	iccc_dst->rot_secure_info.verified_boot_state = iccc_src->verified_boot_state;
	iccc_dst->rot_secure_info.device_locked = iccc_src->device_locked;
	iccc_dst->rot_secure_info.os_version = iccc_src->os_version;
	iccc_dst->rot_secure_info.patch_month_year = iccc_src->patch_month_year;
	for(i = 0; i < ROT_KEY_SIZE/sizeof(uint32_t) ; i++) {
		iccc_dst->rot_secure_info.verified_boot_key[i] = *(uint32_t *)(iccc_src->verified_boot_key+(i * 4));
	}
#endif
	iccc_dst->rot_secure_info.boot_patch_level = iccc_src->boot_patch_level;
	iccc_dst->rot_secure_info.vendor_patch_level = iccc_src->vendor_patch_level;

	return 0;
}

int str_to_int(char *str)
{
	int int_val=0;
	while(*str){
		int_val = int_val*10 + *str - '0';
		str++;
	}
	return int_val;
}

uint32_t fcBuildinfo(void)
{
	int build_info;
	build_info = str_to_int(_BUILD_INFO);
	printf("build_info = 0x%x\n", build_info);
	return build_info;
}

DECLARE_HANDLER_MAIN(unsigned int fid, struct custom_handler_params *params)
{
	//uint32_t ret_r0 = 0, ret_r1 = 0, ret_r2 = 0, ret_r3 = 0;
	uint32_t p1 = params->p0;
	uint32_t p2 = params->p1;
	uint32_t p3 = params->p2;
	//int ret = 0;

	unsigned long pfn;
 	uint32_t i =0;
	//uint32_t j =0;
	params->ret_r0 = 0x121212;
#if defined(CONFIG_EXYNOS)
	tima_ctx_t *s_hash_vaddr = NULL;
#else
	TB_HASH_T *s_hash_vaddr = NULL;
#endif
	unsigned long s_hash_paddr_page;
	uint8_t *s_hash_vaddr_pagealigned = NULL;
	unsigned long p2_paddr_page;
	uint8_t *p2_vaddr_pagealigned = NULL;
	uint8_t *p2_vaddr = NULL;


#if TB_DEBUG
	unsigned long p3_paddr_page;
	uint8_t *p3_vaddr_pagealigned = NULL;
	uint8_t *p3_vaddr = NULL;
#endif

#ifdef DMVERITY_MODE_SUPPORT
	uint32_t cmd_id = params->p1;
	uint32_t arg1 = params->p2;
	uint32_t *boot_mode;
	uint32_t *system_image_check;
	uint32_t *kap_status_flag;
	uint8_t *boot_mode_page_aligned;
	uint32_t dmverity_paddr = DMVERITY_PA;
#endif
#if defined(TIMA_HAVE_GOOD_MEASUREMENT)
	uint8_t *good_meas_addr_vaddr_pagealigned;
	uint8_t *good_meas_addr;
	uint32_t golden_measurement_pa;

	golden_measurement_pa = (DRAM_HASH_TABLE_PA + GOOD_MEASUREMENT_OFFSET);
	pfn = (golden_measurement_pa / PAGE_SIZE); /* calc page number from phys.addr */
#endif

#ifdef TIMA_ICCC
	unsigned long iccc_p2_paddr_page;
	uint8_t *iccc_p2_vaddr_pagealigned = NULL;
	smc_secure_info_t *iccc_p2_vaddr = NULL;
#if ICCC_DEBUG
	unsigned long iccc_p3_paddr_page;
	uint8_t *iccc_p3_vaddr_pagealigned = NULL;
	uint8_t *iccc_p3_vaddr = NULL;
#endif
	unsigned long iccc_secure_param_paddr_page;
	uint8_t *iccc_secure_param_vaddr_pagealigned = NULL;
	iccc_secure_pamameters_info_t *iccc_secure_param_vaddr = NULL;
	unsigned long iccc_ta_info_page;
	uint8_t *iccc_ta_pagealigned=NULL;
	iccc_secure_pamameters_info_t *iccc_ta=NULL;
#endif

	(void)fid;

	printf("CSMC: BEGIN\n");
    /*
     * Select subfunction to execute
     */
	switch(p1)
	{
		/* TIMA Hash Copy and Magic String/Warranty string copy into secure DRAM Area */
		case SUBFUN_TRUSTBOOT_SUPPORT:
		/* Address mappings from p2 */
			if(!p2)
			{
				printf("CSMC: p2 is invalid\n");
				params->ret_r0 = (uint32_t)-1;
				break;
			}
			p2_paddr_page = (p2 >> PAGE_SIZE_SHFT); /* calc page number from phys.addr */
			p2_vaddr_pagealigned = vm_map_phys_buffer(&p2_paddr_page, 1, VMA_NON_CACHED);
			if(!p2_vaddr_pagealigned)
			{
				printf("CSMC: p2_vaddr is FAILED\n");
				params->ret_r0 = (uint32_t)-2;
				break;
			}
			p2_vaddr = p2_vaddr_pagealigned + (p2 % PAGE_SIZE);
#if TB_DEBUG
		/* Address mappings from p3 */
			p3_paddr_page = (p3 >> PAGE_SIZE_SHFT);
			p3_vaddr_pagealigned = vm_map_phys_buffer(&p3_paddr_page, 1, VMA_NON_CACHED);
			if(!p3_vaddr_pagealigned)
			{
				printf("CSMC: p3_vaddr is FAILED\n");
				vm_unmap_phys_buffer((unsigned long)p2_vaddr_pagealigned, 1);
				params->ret_r0 = (uint32_t)-3;
				break;
			}
			p3_vaddr = p3_vaddr_pagealigned + (p3 % PAGE_SIZE);
#endif
		/* Address mappings from Secure Memory including Boot HASH */
			s_hash_paddr_page = (DRAM_HASH_TABLE_PA >> PAGE_SIZE_SHFT);
			s_hash_vaddr_pagealigned =  vm_map_phys_buffer(&s_hash_paddr_page, 1, VMA_NON_CACHED);
			if(!s_hash_vaddr_pagealigned)
			{
				printf("CSMC: s_hash_vaddr is FAILED\n");
#if TB_DEBUG
				vm_unmap_phys_buffer((unsigned long)p3_vaddr_pagealigned, 1);
#endif
				vm_unmap_phys_buffer((unsigned long)p2_vaddr_pagealigned, 1);
				params->ret_r0 = (uint32_t)-4;
				break;
			}

#if defined(TIMA_HAVE_GOOD_MEASUREMENT)
			good_meas_addr_vaddr_pagealigned = vm_map_phys_buffer(&pfn, 1, VMA_NON_CACHED);
			if(!good_meas_addr_vaddr_pagealigned)
			{
				printf("CSMC: good_meas_addr_vaddr_pagealigned is FAILED\n");
				vm_unmap_phys_buffer((unsigned long)s_hash_vaddr_pagealigned, 1);
#if TB_DEBUG
				vm_unmap_phys_buffer((unsigned long)p3_vaddr_pagealigned, 1);
#endif
				vm_unmap_phys_buffer((unsigned long)p2_vaddr_pagealigned, 1);
				params->ret_r0 = (uint32_t)-8;
				break;
			}
			good_meas_addr = good_meas_addr_vaddr_pagealigned + (golden_measurement_pa % PAGE_SIZE);
#endif

#if defined(CONFIG_MEDIATEK)	//Copy from LK
			s_hash_vaddr = (TB_HASH_T *)s_hash_vaddr_pagealigned + (DRAM_HASH_TABLE_PA % PAGE_SIZE);
			if(!strncmp((const char *)s_hash_vaddr->magic, TB_TIMA_MAGIC, TB_TIMA_MAGIC_LEN))
			{
				if(s_hash_vaddr->hash_num == TB_TIMA_BOOT_HASH_STORED)
				{
#if defined(TIMA_HAVE_GOOD_MEASUREMENT)
				/* Clear hash values to store golden measurement */
					memset(good_meas_addr, 0x0, TB_TIMA_BOOT_HASH_NUM * SHA_DIGEST_LEN);
#endif
				/* Set Kernel hash to the Boot Measurement area in Secure Memory */
					memcpy((uint8_t *)&s_hash_vaddr->hash[4][0], p2_vaddr, SHA_DIGEST_LEN);
					s_hash_vaddr->hash_num++;
#if TB_DEBUG
				/* Get all hash values from Secure Memory for checking in LK*/
					memcpy(p3_vaddr, s_hash_vaddr, sizeof(TB_HASH_T));
#endif
					params->ret_r0 = 1; /* ok */
				}
				else
				{
					printf("CSMC: HASH num is wrong\n");
					params->ret_r0 = (uint32_t)-6;
				}
			}
			else
			{
				printf("CSMC: no TIMA MAGIC\n");
				params->ret_r0 = (uint32_t)-5;
			}
#else
			/* add offset in the page to virtual address */
			s_hash_vaddr = (tima_ctx_t *)s_hash_vaddr_pagealigned + (DRAM_HASH_TABLE_PA % PAGE_SIZE);
			/* Copy Magic string */
			for (i = 0; i < sizeof(s_hash_vaddr->magic_string); i++)
			{
				s_hash_vaddr->magic_string[i] = p2_vaddr[i];
			}

			if (!strncmp((const char *)(p2_vaddr + 48), "SVB", strlen("SVB")))
			{
				/* Copy vbmeta hash values(SHA256) instead of copying binary hashs in iRAM */
				memcpy((uint8_t *)s_hash_vaddr->vpcr1, p2_vaddr + 64, SHA256_DIGEST_LEN);
			}

			/* Copy Warranty string */
			memcpy(&(s_hash_vaddr->warranty_string), p2_vaddr + 16, 32);

			/* Copy SVB magic */
			memcpy(&(s_hash_vaddr->svb_magic), p2_vaddr + 48, 16);

			printf("s_hash_vaddr->magic_string : %s\n", s_hash_vaddr->magic_string);
			if (!strncmp((const char *)(s_hash_vaddr->svb_magic), "SVB", strlen("SVB")))
				printf("s_hash_vaddr->svb_magic : %s\n", s_hash_vaddr->svb_magic);
			else
				printf("s_hash_vaddr->svb_magic : NONE\n");
			printf("s_hash_vaddr->vpcr1 : %x %x %x %x %x %x %x %x\n",
				s_hash_vaddr->vpcr1[0],s_hash_vaddr->vpcr1[1],s_hash_vaddr->vpcr1[2],s_hash_vaddr->vpcr1[3],s_hash_vaddr->vpcr1[4],s_hash_vaddr->vpcr1[5],s_hash_vaddr->vpcr1[6],s_hash_vaddr->vpcr1[7]);

			printf("s_hash_vaddr->warranty_string : %s\n", s_hash_vaddr->warranty_string);
#if defined(TIMA_HAVE_GOOD_MEASUREMENT)
			/* Clear hash values to store golden measurement */
			memset(good_meas_addr, 0x0, TB_TIMA_BOOT_HASH_NUM * SHA_DIGEST_LEN);
#endif
#endif
			vm_unmap_phys_buffer((unsigned long)s_hash_vaddr_pagealigned, 1);
#if TB_DEBUG
			vm_unmap_phys_buffer((unsigned long)p3_vaddr_pagealigned, 1);
#endif
			vm_unmap_phys_buffer((unsigned long)p2_vaddr_pagealigned, 1);
#if defined(TIMA_HAVE_GOOD_MEASUREMENT)
			vm_unmap_phys_buffer((unsigned long)good_meas_addr_vaddr_pagealigned, 1);
#endif
			params->ret_r0 = 1;
			break;

#ifdef TIMA_ICCC
		case SUBFUN_ICCC_SAVE:
			iccc_ta_info_page=(ICCC_DATA_OFFSET>>PAGE_SIZE_SHFT);
			iccc_ta_pagealigned=vm_map_phys_buffer(&iccc_ta_info_page, 1, 0);
			if(!iccc_ta_pagealigned)
			{
				printf("CSMC: iccc_ta_pagealigned is FAILED\n");
				params->ret_r0=(uint32_t)-2;
 				break;
			}

			iccc_ta=(iccc_secure_pamameters_info_t*)(iccc_ta_pagealigned+(ICCC_DATA_OFFSET%PAGE_SIZE));
			if(p2!=0xFF000002)
			{
				printf("CSMC:Invalid type\n");
				vm_unmap_phys_buffer((unsigned long)iccc_ta_pagealigned, 1);
				params->ret_r0=(uint32_t)-1;
				break;
			}
			params->ret_r0=fcICCCKernelInfoSave(iccc_ta,p3);
			vm_unmap_phys_buffer((unsigned long)iccc_ta_pagealigned, 1);
			break;

		case SUBFUN_ICCC_READ:
			iccc_ta_info_page=(ICCC_DATA_OFFSET>>PAGE_SIZE_SHFT);
			iccc_ta_pagealigned=vm_map_phys_buffer(&iccc_ta_info_page, 1, 0);
			if(!iccc_ta_pagealigned)
			{
				printf("CSMC: iccc_ta_pagealigned is FAILED\n");
				params->ret_r0=(uint32_t)-2;
 				break;
			}

			iccc_ta=(iccc_secure_pamameters_info_t*)(iccc_ta_pagealigned+(ICCC_DATA_OFFSET%PAGE_SIZE));
			if(p2!=0xFF000002)
			{
				printf("CSMC:Invalid type\n");
				vm_unmap_phys_buffer((unsigned long)iccc_ta_pagealigned, 1);
				params->ret_r0=(uint32_t)-1;
				break;
			}
			params->ret_r0=fcICCCKernelInfoRead(iccc_ta);
			vm_unmap_phys_buffer((unsigned long)iccc_ta_pagealigned,1);
			break;

        case SUBFUN_ICCC_HDM_POLICY:
			iccc_ta_info_page=(ICCC_DATA_OFFSET>>PAGE_SIZE_SHFT);
			iccc_ta_pagealigned=vm_map_phys_buffer(&iccc_ta_info_page,1,0);
			if(!iccc_ta_pagealigned)
			{
				printf("CSMC: iccc_ta_pagealigned is FAILED\n");
  				break;
			}

			iccc_ta=(iccc_secure_pamameters_info_t*)(iccc_ta_pagealigned+(ICCC_DATA_OFFSET%PAGE_SIZE));
			params->ret_r0=fcICCCHdmSet(iccc_ta,p2,p3);
			vm_unmap_phys_buffer((unsigned long)iccc_ta_pagealigned,1);
			break;

		case SUBFUN_DMV_WRITE:
			iccc_ta_info_page=(ICCC_DATA_OFFSET>>PAGE_SIZE_SHFT);
			iccc_ta_pagealigned=vm_map_phys_buffer(&iccc_ta_info_page,1,0);
			if(!iccc_ta_pagealigned)
			{
				printf("CSMC: iccc_ta_pagealigned is FAILED\n");
  				break;
			}

			iccc_ta=(iccc_secure_pamameters_info_t*)(iccc_ta_pagealigned+(ICCC_DATA_OFFSET%PAGE_SIZE));
			params->ret_r0=fcICCCDmvSet(iccc_ta,p2,p3);
			vm_unmap_phys_buffer((unsigned long)iccc_ta_pagealigned,1);
			break;

		case SUBFUN_ICCC_SVB_RESULT:
			iccc_secure_param_paddr_page = (ICCC_DATA_OFFSET >> PAGE_SIZE_SHFT);
			iccc_secure_param_vaddr_pagealigned = vm_map_phys_buffer(&iccc_secure_param_paddr_page, 1, VMA_NON_CACHED);
			if(!iccc_secure_param_vaddr_pagealigned)
			{
				printf("CSMC: iccc_secure_param_vaddr_pagealigned is FAILED\n");
  				break;
			}

			iccc_secure_param_vaddr = (iccc_secure_pamameters_info_t *)(iccc_secure_param_vaddr_pagealigned + (ICCC_DATA_OFFSET % PAGE_SIZE));
			params->ret_r0 = IcccSvbResult(iccc_secure_param_vaddr, p2);
			vm_unmap_phys_buffer((unsigned long)iccc_secure_param_vaddr_pagealigned, 1);
			break;

		case SUBFUN_ICCC_DATA_LOCK:
			iccc_secure_param_paddr_page = (ICCC_DATA_OFFSET >> PAGE_SIZE_SHFT);
			iccc_secure_param_vaddr_pagealigned = vm_map_phys_buffer(&iccc_secure_param_paddr_page, 1, VMA_NON_CACHED);
			if(!iccc_secure_param_vaddr_pagealigned)
			{
				printf("CSMC: iccc_secure_param_vaddr_pagealigned is FAILED\n");
  				break;
			}

			iccc_secure_param_vaddr = (iccc_secure_pamameters_info_t *)(iccc_secure_param_vaddr_pagealigned + (ICCC_DATA_OFFSET % PAGE_SIZE));
			params->ret_r0 = IcccDataLock(iccc_secure_param_vaddr);
			vm_unmap_phys_buffer((unsigned long)iccc_secure_param_vaddr_pagealigned, 1);
			break;

		case SUBFUN_ICCC:
			/* Address mappings from p2 */
			if(!p2)
			{
				printf("CSMC: iccc p2 is invalid (%x)\n", p2);
				params->ret_r0 = (uint32_t)-1;
				break;
			}
#if defined(SECURE_MEM_BOUNDARY)
//			printf("CSMC: boundary config from %x to %x\n", DRAM_HASH_TABLE_PA, SECURE_MEM_BOUNDARY);
			if((DRAM_HASH_TABLE_PA <= p2) && (p2 <= SECURE_MEM_BOUNDARY))
			{
				printf("CSMC: secure memory delivered\n");
				break;
			}
#endif
			iccc_p2_paddr_page = (p2 >> PAGE_SIZE_SHFT);
			iccc_p2_vaddr_pagealigned = vm_map_phys_buffer(&iccc_p2_paddr_page, 1, VMA_NON_CACHED);
			if(!iccc_p2_vaddr_pagealigned)
			{
				printf("CSMC: iccc_p2_vaddr is FAILED (%p)\n", (void *)iccc_p2_vaddr_pagealigned);
				params->ret_r0 = (uint32_t)-2;
				break;
			}
			iccc_p2_vaddr = (smc_secure_info_t *)(iccc_p2_vaddr_pagealigned + (p2 % PAGE_SIZE));
#if ICCC_DEBUG
			/* Address mappings from p3 */
			iccc_p3_paddr_page = (p3 >> PAGE_SIZE_SHFT);
			iccc_p3_vaddr_pagealigned = vm_map_phys_buffer(&iccc_p3_paddr_page, 1, VMA_NON_CACHED);
			if(!iccc_p3_vaddr_pagealigned)
			{
				printf("CSMC: iccc_p3_vaddr is FAILED (%x)\n", (uint32_t)iccc_p3_vaddr_pagealigned);
				vm_unmap_phys_buffer((unsigned long)iccc_p2_vaddr_pagealigned, 1);
				params->ret_r0 = (uint32_t)-3;
				break;
			}
			iccc_p3_vaddr = iccc_p3_vaddr_pagealigned + (p3 % PAGE_SIZE);
#endif
		/* Address mappings from Secure Memory including Boot HASH */
			iccc_secure_param_paddr_page = (ICCC_DATA_OFFSET >> PAGE_SIZE_SHFT);
			iccc_secure_param_vaddr_pagealigned = vm_map_phys_buffer(&iccc_secure_param_paddr_page, 1, VMA_NON_CACHED);
			if(!iccc_secure_param_vaddr_pagealigned)
			{
				printf("CSMC: iccc_secure_param_vaddr is FAILED\n");
#if ICCC_DEBUG
				vm_unmap_phys_buffer((unsigned long)iccc_p3_vaddr_pagealigned, 1);
#endif
				vm_unmap_phys_buffer((unsigned long)iccc_p2_vaddr_pagealigned, 1);
				params->ret_r0 = (uint32_t)-4;
				break;
			}
			iccc_secure_param_vaddr = (iccc_secure_pamameters_info_t *)(iccc_secure_param_vaddr_pagealigned + (ICCC_DATA_OFFSET % PAGE_SIZE));

			fcICCCSaveSecureParam(iccc_secure_param_vaddr, iccc_p2_vaddr);

#if defined(BF_DEBUG)
			printf("CSMC: p2 value (%x)\n", p2);
			printf("CSMC: iccc_p2_vaddr : %p\n", iccc_p2_vaddr);
			printf("CSMC: ICCC_DATA_OFFSET (%x)\n", ICCC_DATA_OFFSET);
			printf("CSMC: iccc_secure_param_vaddr (%p)\n", iccc_secure_param_vaddr);
			printf("CSMC: bl_secure_info.header.magic_str (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.header.magic_str);
			printf("CSMC: bl_secure_info addr (%p)\n", &(((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info));
			printf("CSMC: bl_secure_info.header.used_size (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.header.used_size);
#endif
			printf("CSMC: bl_secure_info.rp_ver (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.rp_ver);
			printf("CSMC: bl_secure_info.kernel_rp (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.kernel_rp);
			printf("CSMC: bl_secure_info.system_rp (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.system_rp);
			printf("CSMC: bl_secure_info.test_bit (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.test_bit);
			printf("CSMC: bl_secure_info.sec_boot (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.sec_boot);
			printf("CSMC: bl_secure_info.react_lock (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.react_lock);
			printf("CSMC: bl_secure_info.kiwi_lock (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.kiwi_lock);
			printf("CSMC: bl_secure_info.frp_lock (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.frp_lock);
			printf("CSMC: bl_secure_info.cc_mode (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.cc_mode);
			printf("CSMC: bl_secure_info.mdm_mode (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.mdm_mode);
			printf("CSMC: bl_secure_info.curr_bin_status (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.curr_bin_status);
			printf("CSMC: bl_secure_info.afw_value (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.afw_value);
			printf("CSMC: bl_secure_info.warranty_bit (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.warranty_bit);
			printf("CSMC: bl_secure_info.kap_status  (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.kap_status);
			for(i = 0; i < MAX_IMAGES ; i++)
				printf("CSMC: bl_secure_info.image_status[%d]  (%x)\n", i, ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.image_status[i]);
			printf("CSMC: bl_secure_info.image_status_bl  (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.image_status_bl);
			printf("CSMC: bl_secure_info.WbHistory  (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.WbHistory);
//			printf("CSMC: bl_secure_info.ap_serial_0 (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.ap_serial_0);
//			printf("CSMC: bl_secure_info.ap_serial_1 (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.ap_serial_1);
			printf("CSMC: bl_secure_info.em_status (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.em_status);
			printf("CSMC: bl_secure_info.em_token (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->bl_secure_info.em_token);

#if defined(BF_DEBUG)
			printf("CSMC: ta_secure_info.header.magic_str (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.header.magic_str);
			printf("CSMC: ta_secure_info addr (%p)\n", &(((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info));
			printf("CSMC: ta_secure_info.header.used_size (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.header.used_size);
#endif
			printf("CSMC: ta_secure_info.pkm_text (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.pkm_text);
			printf("CSMC: ta_secure_info.pkm_ro (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.pkm_ro);
			printf("CSMC: ta_secure_info.selinux_status (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.selinux_status);
			printf("CSMC: ta_secure_info.sectimer_base (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.sectimer_base);
			printf("CSMC: ta_secure_info.sectimer_flag (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.sectimer_flag);
			printf("CSMC: ta_secure_info.sectimer_status (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->ta_secure_info.sectimer_status);

#if defined(BF_DEBUG)
			printf("CSMC: kern_secure_info.header.magic_str (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->kern_secure_info.header.magic_str);
			printf("CSMC: kern_secure_info addr (%p)\n", &(((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->kern_secure_info));
			printf("CSMC: kern_secure_info.header.used_size (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->kern_secure_info.header.used_size);
#endif
			printf("CSMC: kern_secure_info.dmv_status (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->kern_secure_info.dmv_status);
			for(i = 0; i < sizeof(((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->kern_secure_info.verified_boot_hash)/sizeof(uint32_t) ; i++) {
				printf("CSMC: kern_secure_info.verified_boot_hash[%d] (%x)\n", i, ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->kern_secure_info.verified_boot_hash[i]);
			}

#if defined(BF_DEBUG)
			printf("CSMC: sys_secure_info.header.magic_str (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->sys_secure_info.header.magic_str);
			printf("CSMC: sys_secure_info addr (%p)\n", &(((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->sys_secure_info));
			printf("CSMC: sys_secure_info.header.used_size (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->sys_secure_info.header.used_size);
#endif
			printf("CSMC: sys_secure_info.sysscope_flag (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->sys_secure_info.sysscope_flag);
			printf("CSMC: sys_secure_info.trustboot_flag (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->sys_secure_info.trustboot_flag);

#if defined(BF_DEBUG)
			printf("CSMC: rot_secure_info.header.magic_str (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.header.magic_str);
			printf("CSMC: rot_secure_info addr (%p)\n", &(((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info));
			printf("CSMC: rot_secure_info.header.used_size (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.header.used_size);
#endif
			printf("CSMC: rot_secure_info.verified_boot_state (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.verified_boot_state);
			printf("CSMC: rot_secure_info.device_locked (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.device_locked);
			printf("CSMC: rot_secure_info.os_version (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.os_version);
			printf("CSMC: rot_secure_info.patch_month_year (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.patch_month_year);
			for(i = 0; i < ROT_KEY_SIZE/sizeof(uint32_t) ; i++) {
				printf("CSMC: rot_secure_info.verified_boot_key[%d] (%x)\n", i,  ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.verified_boot_key[i]);
			}
			printf("CSMC: rot_secure_info.boot_patch_level (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.boot_patch_level);
			printf("CSMC: rot_secure_info.vendor_patch_level (%x)\n", ((iccc_secure_pamameters_info_t *)iccc_secure_param_vaddr)->rot_secure_info.vendor_patch_level);

			params->ret_r0 = (uint32_t)1;

			vm_unmap_phys_buffer((unsigned long)iccc_secure_param_vaddr_pagealigned, 1);
#if ICCC_DEBUG
			vm_unmap_phys_buffer((unsigned long)iccc_p3_vaddr_pagealigned, 1);
#endif
			vm_unmap_phys_buffer((unsigned long)iccc_p2_vaddr_pagealigned, 1);

			break;
#endif

#ifdef DMVERITY_MODE_SUPPORT
		case SUBFUN_DMVERITY_RECOVERY:
			//Mapping system_image_check and boot-up
			pfn = dmverity_paddr>>PAGE_SIZE_SHFT;
			boot_mode_page_aligned = vm_map_phys_buffer(&pfn, 1, 0);
			if(!boot_mode_page_aligned)
			{
				printf("CSMC: boot_mode_page_aligned is FAILED\n");
  				break;
			}

			boot_mode = (uint32_t *)(boot_mode_page_aligned + (dmverity_paddr & (PAGE_SIZE - 1)));
			system_image_check = (uint32_t *)(boot_mode + 1);

			printf("cmd_id: %d, arg1: %d\n", cmd_id, arg1);	//delete
			printf("boot_mode: %d, system_image_check: %d\n", *boot_mode, *system_image_check);	//delete
			*boot_mode = RECOVERY;
			*system_image_check = 123;
			printf("boot_mode: %d, system_image_check: %d\n", *boot_mode, *system_image_check);	//delete
			// First will be all read-only command-ids. They will always be accessible
			if (cmd_id == CMD_READ_SYSTEM_IMAGE_CHECK_STATUS)
			{
				// TODO: Check value of rsp
				if (*boot_mode == RECOVERY)
				{
					printf("checkpoint1\n");  //delete
					params->ret_r0 = *system_image_check;
					goto unmap_dmverity;
				}
				else
				{
					printf("checkpoint2\n");  //delete
					// not in recovery mode
					params->ret_r0 = ERR_DMVERITY_NOT_IN_RECOVERY;
					goto unmap_dmverity;
				}
			}

			if (cmd_id == 5)
			{
				printf("checkpoint3\n");  //delete
				params->ret_r0 = *boot_mode;
				goto unmap_dmverity;
			}

			if (cmd_id == 6)
			{
				printf("checkpoint4\n");  //delete
				params->ret_r0 = *system_image_check;
				goto unmap_dmverity;
			}

			// Now that read-only command-ids are done. Check for update lock
			if (lock_updates == 1)
			{
				printf("checkpoint5\n");  //delete
				params->ret_r0 = ERR_DMVERITY_UPDATES_LOCKED;
				goto unmap_dmverity;
			}

			// Write cmd ids
			if (cmd_id == CMD_UPDATE_SYSTEM_IMAGE_CHECK_STATUS)
			{
				printf("checkpoint6\n");  //delete
				if (arg1)
				{
						*system_image_check = SUCCEEDED;
				}
				else
				{
						*system_image_check = FAILED;
				}
				params->ret_r0 = 0;
				goto unmap_dmverity;
			}
			else if (cmd_id == CMD_RESTORE_SYSTEM_IMAGE_CHECK_STATUS)
			{
				printf("checkpoint7\n");  //delete
				*system_image_check = arg1;
				params->ret_r0 = 0;
				goto unmap_dmverity;
			}
			else if (cmd_id == CMD_UPDATE_BOOT_MODE)
			{
				// Update to this value is allowed only once during boot-up, it is then i
				 // locked
				 //
				 printf("checkpoint8\n");  //delete
				lock_updates = 1;

				if (arg1)
				{
					*boot_mode = RECOVERY;
				}
				else
				{
					*boot_mode = NORMAL;
					// Reset system_image_check to a known value
					*system_image_check = FAILED;
				}
				params->ret_r0 = 0;
				goto unmap_dmverity;
			}
			else
			{
				printf("checkpoint9\n");  //delete
				params->ret_r0 = ERR_DMVERITY_INVALID_CMD_ID;
				goto unmap_dmverity;
			}
			printf("checkpoint#\n");  //delete
			unmap_dmverity:
			//unmapping pages
			vm_unmap_phys_buffer((unsigned long)boot_mode_page_aligned,1);
			break;
		case SUBFUN_KAP_VAL:
			pfn = dmverity_paddr>>PAGE_SIZE_SHFT;
			boot_mode_page_aligned = vm_map_phys_buffer(&pfn, 1, 0);
			if(!boot_mode_page_aligned)
			{
				printf("CSMC: boot_mode_page_aligned is FAILED\n");
  				break;
			}

			boot_mode = (uint32_t *)(boot_mode_page_aligned + (dmverity_paddr & (PAGE_SIZE - 1)));
			kap_status_flag = (uint32_t *)(boot_mode + 2) ;

			printf("cmd_id: %d, arg1: %d\n", cmd_id, arg1);	//delete
			printf("boot_mode: %d, kap_status_flag : %d\n", *boot_mode, *kap_status_flag);	//delete
			switch (cmd_id)
			{
				case CMD_ID_KAP_STATUS:
					params->ret_r0 = fcKapStatus();
					break;
				case CMD_ID_DISABLE_KAP:
					KAP_STATUS_FUSE = 1;
					*kap_status_flag = KAP_MAGIC|FC_KAP_OFF;
					params->ret_r0 = 0;
					break;
				case CMD_ID_DISABLE_KAP_BL:
					/* This cmd_id is to disable KAP mode from the bootloader */
					KAP_STATUS_FUSE = 1;
					*kap_status_flag = KAP_MAGIC|FC_KAP_OFF;
					params->ret_r0 = 0;
                			break;
				case CMD_ID_BOOT_STS_KAP_ON:
                			//tima_util_log("KAP is on at BL", 0, 0, 0);
                			KAP_BOOT_STATUS_FUSE = 1;
                			*kap_status_flag = KAP_MAGIC|FC_KAP_ON;
					params->ret_r0 = 0;
	        		        break;
				case CMD_ID_BOOT_KAP_VIO:
					//tima_util_log("BL detected warranty bit blown", 0, 0, 0);
					KAP_VIOLATION_FUSE = 1;
					*kap_status_flag = KAP_MAGIC|FC_KAP_VIOL;
					params->ret_r0 = 0;
					break;
				default:
					params->ret_r0 = 1;  // wrong cmd_id
			}
			//unmapping pages
			vm_unmap_phys_buffer((unsigned long)boot_mode_page_aligned,1);
			break;
		case SUBFUN_KAP_STATUS:
			params->ret_r0 = fcKapStatus();
			break;
#endif
#if defined(CONFIG_FP_SUPPORT)
		case SUBFUN_SECURE_PIN:
			if (p2 == FP_MODE)
			{
				gpio_set_fp_init();
			}
			break;

		case SUBFUN_FP_PM_SUSPEND:
			save_fp_gpio_regs();
			break;

		case SUBFUN_FP_PM_RESUME:
			restore_fp_gpio_regs();
			break;

		case SUBFUN_FP_BTP_OCP_HIGH:
			gpio_set_fp_btp_ocp(GPIO_HIGH);
			break;

		case SUBFUN_FP_BTP_OCP_LOW:
			gpio_set_fp_btp_ocp(GPIO_LOW);
			break;
#endif
		case SUBFUN_JENKINS_INFO:
			params->ret_r0 = fcBuildinfo();
			break;

		default:
			params->ret_r0 = (uint32_t)-1;

	}
    	printf("CSMC: END\n");
    /*
     * Result is returned to the caller in registers R0-R3
     */
//    RET_SMC_ARG4(CUSTOM_SMC_EXEC_DONE, ret_r0, ret_r1, ret_r2, ret_r3);
//    __builtin_unreachable();
}