/**
 * This function initializes the processor and updates the cluster id
 * which indicates CPU on which fsbl is running
 *
 * @param	FsblInstancePtr is pointer to the XFsbl Instance
 *
 * @return	returns the error codes described in xfsbl_error.h on any error
 * 			returns XFSBL_SUCCESS on success
 *
 ******************************************************************************/
static u32 XFsbl_ProcessorInit(XFsblPs * FsblInstancePtr)
{
	u32 Status = XFSBL_SUCCESS;
	//u64 ClusterId=0U;
	PTRSIZE ClusterId=0U;
	u32 RegValue;
	u32 Index=0U;

	/**
	 * Read the cluster ID and Update the Processor ID
	 * Initialize the processor settings that are not done in
	 * BSP startup code
	 */
#ifdef XFSBL_A53
	ClusterId = mfcp(MPIDR_EL1);
#else
	ClusterId = mfcp(XREG_CP15_MULTI_PROC_AFFINITY);
#endif

	XFsbl_Printf(DEBUG_INFO,"Cluster ID 0x%0lx\n\r", ClusterId);

	if (XFSBL_PLATFORM == XFSBL_PLATFORM_QEMU) {
		/**
		 * Remmaping for R5 in QEMU
		 */
		if (ClusterId == 0x80000004U) {
			ClusterId = 0xC0000100U;
		} else if (ClusterId == 0x80000005U) {
			/* this corresponds to R5-1 */
			Status = XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID;
			XFsbl_Printf(DEBUG_GENERAL,
					"XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID\n\r");
			goto END;
		} else {
			/* For MISRA C compliance */
		}
	}

	/* store the processor ID based on the cluster ID */
	if ((ClusterId & XFSBL_CLUSTER_ID_MASK) == XFSBL_A53_PROCESSOR) {
		XFsbl_Printf(DEBUG_GENERAL,"Running on A53-0 ");
		FsblInstancePtr->ProcessorID =
				XIH_PH_ATTRB_DEST_CPU_A53_0;
#ifdef __aarch64__
		/* Running on A53 64-bit */
		XFsbl_Printf(DEBUG_GENERAL,"(64-bit) Processor \n\r");
		FsblInstancePtr->A53ExecState = XIH_PH_ATTRB_A53_EXEC_ST_AA64;
#else
		/* Running on A53 32-bit */
		XFsbl_Printf(DEBUG_GENERAL,"(32-bit) Processor \n\r");
		FsblInstancePtr->A53ExecState = XIH_PH_ATTRB_A53_EXEC_ST_AA32;
#endif

	} else if ((ClusterId & XFSBL_CLUSTER_ID_MASK) == XFSBL_R5_PROCESSOR) {
		/* A53ExecState is not valid for R5 */
		FsblInstancePtr->A53ExecState = XIH_INVALID_EXEC_ST;

		RegValue = XFsbl_In32(RPU_RPU_GLBL_CNTL);
		if ((RegValue & RPU_RPU_GLBL_CNTL_SLSPLIT_MASK) == 0U) {
			XFsbl_Printf(DEBUG_GENERAL,
				"Running on R5 Processor in Lockstep \n\r");
			FsblInstancePtr->ProcessorID =
				XIH_PH_ATTRB_DEST_CPU_R5_L;
		} else {
			XFsbl_Printf(DEBUG_GENERAL,
				"Running on R5-0 Processor \n\r");
			FsblInstancePtr->ProcessorID =
				XIH_PH_ATTRB_DEST_CPU_R5_0;
		}

		/**
		 * Update the Vector locations in R5 TCM
		 */
		while (Index<32U) {
			XFsbl_Out32(Index, 0U);
			XFsbl_Out32(Index, XFSBL_R5_VECTOR_VALUE);
			Index += 4;
		}

	} else {
		Status = XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID;
		XFsbl_Printf(DEBUG_GENERAL,
				"XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID\n\r");
		goto END;
	}

	/**
	 * Register the exception handlers
	 */
	XFsbl_RegisterHandlers();

END:
	return Status;
}
/**
 * This function initializes the processor and updates the cluster id
 * which indicates CPU on which fsbl is running
 *
 * @param	FsblInstancePtr is pointer to the XFsbl Instance
 *
 * @return	returns the error codes described in xfsbl_error.h on any error
 * 			returns XFSBL_SUCCESS on success
 *
 ******************************************************************************/
static u32 XFsbl_ProcessorInit(XFsblPs * FsblInstancePtr)
{
	u32 Status = XFSBL_SUCCESS;
	//u64 ClusterId=0U;
	PTRSIZE ClusterId=0U;
	u32 RegValue;
	u32 Index=0U;

	/**
	 * Read the cluster ID and Update the Processor ID
	 * Initialize the processor settings that are not done in
	 * BSP startup code
	 */
#ifdef ARMA53_64
	ClusterId = mfcp(MPIDR_EL1);
#else
	ClusterId = mfcp(XREG_CP15_MULTI_PROC_AFFINITY);
#endif

	XFsbl_Printf(DEBUG_INFO,"Cluster ID 0x%0lx\n\r", ClusterId);

	if (XGet_Zynq_UltraMp_Platform_info() == XPLAT_ZYNQ_ULTRA_MPQEMU) {
		/**
		 * Remmaping for R5 in QEMU
		 */
		if (ClusterId == 0x80000004U) {
			ClusterId = 0xC0000100U;
		} else if (ClusterId == 0x80000005U) {
			/* this corresponds to R5-1 */
			Status = XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID;
			XFsbl_Printf(DEBUG_GENERAL,
					"XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID\n\r");
			goto END;
		} else {
			/* For MISRA C compliance */
		}
	}

	/* store the processor ID based on the cluster ID */
	if ((ClusterId & XFSBL_CLUSTER_ID_MASK) == XFSBL_A53_PROCESSOR) {
		XFsbl_Printf(DEBUG_GENERAL,"Running on A53-0 ");
		FsblInstancePtr->ProcessorID =
				XIH_PH_ATTRB_DEST_CPU_A53_0;
#ifdef __aarch64__
		/* Running on A53 64-bit */
		XFsbl_Printf(DEBUG_GENERAL,"(64-bit) Processor \n\r");
		FsblInstancePtr->A53ExecState = XIH_PH_ATTRB_A53_EXEC_ST_AA64;
#else
		/* Running on A53 32-bit */
		XFsbl_Printf(DEBUG_GENERAL,"(32-bit) Processor \n\r");
		FsblInstancePtr->A53ExecState = XIH_PH_ATTRB_A53_EXEC_ST_AA32;
#endif

	} else if ((ClusterId & XFSBL_CLUSTER_ID_MASK) == XFSBL_R5_PROCESSOR) {
		/* A53ExecState is not valid for R5 */
		FsblInstancePtr->A53ExecState = XIH_INVALID_EXEC_ST;

		RegValue = XFsbl_In32(RPU_RPU_GLBL_CNTL);
		if ((RegValue & RPU_RPU_GLBL_CNTL_SLSPLIT_MASK) == 0U) {
			XFsbl_Printf(DEBUG_GENERAL,
				"Running on R5 Processor in Lockstep \n\r");
			FsblInstancePtr->ProcessorID =
				XIH_PH_ATTRB_DEST_CPU_R5_L;
		} else {
			XFsbl_Printf(DEBUG_GENERAL,
				"Running on R5-0 Processor \n\r");
			FsblInstancePtr->ProcessorID =
				XIH_PH_ATTRB_DEST_CPU_R5_0;
		}

		/**
		 * Update the Vector locations in R5 TCM
		 */
		while (Index<32U) {
			XFsbl_Out32(Index, XFSBL_R5_VECTOR_VALUE);
			Index += 4;
		}

	} else {
		Status = XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID;
		XFsbl_Printf(DEBUG_GENERAL,
				"XFSBL_ERROR_UNSUPPORTED_CLUSTER_ID\n\r");
		goto END;
	}

	/**
	 * Register the exception handlers
	 */
	XFsbl_RegisterHandlers();

	/* Prints for the perf measurement */
#ifdef XFSBL_PERF

#if !defined(ARMR5)
	if (FsblInstancePtr->ProcessorID == XIH_PH_ATTRB_DEST_CPU_A53_0) {
		XFsbl_Printf(DEBUG_PRINT_ALWAYS, "Proc: A53-0 Freq: %d Hz",
				XPAR_CPU_CORTEXA53_0_CPU_CLK_FREQ_HZ);

		if (FsblInstancePtr->A53ExecState == XIH_PH_ATTRB_A53_EXEC_ST_AA32) {
			XFsbl_Printf(DEBUG_PRINT_ALWAYS, " Arch: 32 \r\n");
		}
		else if (FsblInstancePtr->A53ExecState ==
				XIH_PH_ATTRB_A53_EXEC_ST_AA64) {
			XFsbl_Printf(DEBUG_PRINT_ALWAYS, " Arch: 64 \r\n");
		}
	}
#else
	if (FsblInstancePtr->ProcessorID == XIH_PH_ATTRB_DEST_CPU_R5_0) {
		XFsbl_Printf(DEBUG_PRINT_ALWAYS, "Proc: R5-0 Freq: %d Hz \r\n",
				XPAR_PSU_CORTEXR5_0_CPU_CLK_FREQ_HZ)
	}
	else if (FsblInstancePtr->ProcessorID == XIH_PH_ATTRB_DEST_CPU_R5_L) {
		XFsbl_Printf(DEBUG_PRINT_ALWAYS, "Proc: R5-Lockstep "
			"Freq: %d Hz \r\n", XPAR_PSU_CORTEXR5_0_CPU_CLK_FREQ_HZ);
	}
#endif

#endif

END:
	return Status;
}