C++ read_mpidr函数代码示例

/*******************************************************************************
 * This function performs any remaining book keeping in the test secure payload
 * after this cpu's architectural state has been setup in response to an earlier
 * psci cpu_on request.
 ******************************************************************************/
tsp_args_t *tsp_cpu_on_main(void)
{
	uint32_t linear_id = plat_my_core_pos();

	/* Initialize secure/applications state here */
	tsp_generic_timer_start();

	/* Update this cpu's statistics */
	tsp_stats[linear_id].smc_count++;
	tsp_stats[linear_id].eret_count++;
	tsp_stats[linear_id].cpu_on_count++;

#if LOG_LEVEL >= LOG_LEVEL_INFO
	spin_lock(&console_lock);
	INFO("TSP: cpu 0x%lx turned on\n", read_mpidr());
	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
		read_mpidr(),
		tsp_stats[linear_id].smc_count,
		tsp_stats[linear_id].eret_count,
		tsp_stats[linear_id].cpu_on_count);
	spin_unlock(&console_lock);
#endif
	/* Indicate to the SPD that we have completed turned ourselves on */
	return set_smc_args(TSP_ON_DONE, 0, 0, 0, 0, 0, 0, 0);
}

int ari_online_core(uint32_t ari_base, uint32_t core)
{
	int cpu = read_mpidr() & MPIDR_CPU_MASK;
	int cluster = (read_mpidr() & MPIDR_CLUSTER_MASK) >>
			MPIDR_AFFINITY_BITS;
	int impl = (read_midr() >> MIDR_IMPL_SHIFT) & MIDR_IMPL_MASK;

	/* construct the current CPU # */
	cpu |= (cluster << 2);

	/* sanity check target core id */
	if ((core >= MCE_CORE_ID_MAX) || (cpu == core)) {
		ERROR("%s: unsupported core id (%d)\n", __func__, core);
		return EINVAL;
	}

	/*
	 * The Denver cluster has 2 CPUs only - 0, 1.
	 */
	if (impl == DENVER_IMPL && ((core == 2) || (core == 3))) {
		ERROR("%s: unknown core id (%d)\n", __func__, core);
		return EINVAL;
	}

	/* clean the previous response state */
	ari_clobber_response(ari_base);

	return ari_request_wait(ari_base, 0, TEGRA_ARI_ONLINE_CORE, core, 0);
}

int32_t tsp_irq_received(void)
{
	uint32_t linear_id = plat_my_core_pos();

	tsp_stats[linear_id].irq_count++;
#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
	spin_lock(&console_lock);
	VERBOSE("TSP: cpu 0x%lx received irq\n", read_mpidr());
	VERBOSE("TSP: cpu 0x%lx: %d irq requests\n",
		read_mpidr(), tsp_stats[linear_id].irq_count);
	spin_unlock(&console_lock);
#endif
	return TSP_PREEMPTED;
}

static void hikey_pwr_domain_on_finish(const psci_power_state_t *target_state)
{
	unsigned long mpidr;
	int cpu, cluster;

	mpidr = read_mpidr();
	cluster = MPIDR_AFFLVL1_VAL(mpidr);
	cpu = MPIDR_AFFLVL0_VAL(mpidr);


	/*
	 * Enable CCI coherency for this cluster.
	 * No need for locks as no other cpu is active at the moment.
	 */
	if (CLUSTER_PWR_STATE(target_state) == PLAT_MAX_OFF_STATE)
		cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(mpidr));

	/* Zero the jump address in the mailbox for this cpu */
	hisi_pwrc_set_core_bx_addr(cpu, cluster, 0);

	/* Program the GIC per-cpu distributor or re-distributor interface */
	gicv2_pcpu_distif_init();
	/* Enable the GIC cpu interface */
	gicv2_cpuif_enable();
}

int psci_cpu_suspend(unsigned int power_state,
		     unsigned long entrypoint,
		     unsigned long context_id)
{
	int rc;
	unsigned long mpidr;
	unsigned int target_afflvl, pstate_type;

	/* Sanity check the requested state */
	target_afflvl = psci_get_pstate_afflvl(power_state);
	if (target_afflvl > MPIDR_MAX_AFFLVL)
		return PSCI_E_INVALID_PARAMS;

	pstate_type = psci_get_pstate_type(power_state);
	if (pstate_type == PSTATE_TYPE_STANDBY) {
		if  (psci_plat_pm_ops->affinst_standby)
			rc = psci_plat_pm_ops->affinst_standby(power_state);
		else
			return PSCI_E_INVALID_PARAMS;
	} else {
		mpidr = read_mpidr();
		rc = psci_afflvl_suspend(mpidr,
					 entrypoint,
					 context_id,
					 power_state,
					 MPIDR_AFFLVL0,
					 target_afflvl);
	}

	assert(rc == PSCI_E_INVALID_PARAMS || rc == PSCI_E_SUCCESS);
	return rc;
}

/*******************************************************************************
 * This function passes control to the OPTEE image (BL32) for the first time
 * on the primary cpu after a cold boot. It assumes that a valid secure
 * context has already been created by opteed_setup() which can be directly
 * used.  It also assumes that a valid non-secure context has been
 * initialised by PSCI so it does not need to save and restore any
 * non-secure state. This function performs a synchronous entry into
 * OPTEE. OPTEE passes control back to this routine through a SMC.
 ******************************************************************************/
static int32_t opteed_init(void)
{
	uint64_t mpidr = read_mpidr();
	uint32_t linear_id = platform_get_core_pos(mpidr);
	optee_context_t *optee_ctx = &opteed_sp_context[linear_id];
	entry_point_info_t *optee_entry_point;
	uint64_t rc;

	/*
	 * Get information about the OPTEE (BL32) image. Its
	 * absence is a critical failure.
	 */
	optee_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
	assert(optee_entry_point);

	cm_init_context(mpidr, optee_entry_point);

	/*
	 * Arrange for an entry into OPTEE. It will be returned via
	 * OPTEE_ENTRY_DONE case
	 */
	rc = opteed_synchronous_sp_entry(optee_ctx);
	assert(rc != 0);

	return rc;
}

int psci_cpu_suspend(unsigned int power_state,
		     unsigned long entrypoint,
		     unsigned long context_id)
{
	int rc;
	unsigned long mpidr;
	unsigned int target_afflvl, pstate_type;

	/* TODO: Standby states are not supported at the moment */
	pstate_type = psci_get_pstate_type(power_state);
	if (pstate_type == 0) {
		rc = PSCI_E_INVALID_PARAMS;
		goto exit;
	}

	/* Sanity check the requested state */
	target_afflvl = psci_get_pstate_afflvl(power_state);
	if (target_afflvl > MPIDR_MAX_AFFLVL) {
		rc = PSCI_E_INVALID_PARAMS;
		goto exit;
	}

	mpidr = read_mpidr();
	rc = psci_afflvl_suspend(mpidr,
				 entrypoint,
				 context_id,
				 power_state,
				 MPIDR_AFFLVL0,
				 target_afflvl);

exit:
	if (rc != PSCI_E_SUCCESS)
		assert(rc == PSCI_E_INVALID_PARAMS);
	return rc;
}

void bl1_plat_set_ep_info(unsigned int image_id,
		entry_point_info_t *ep_info)
{
	unsigned int data = 0;
	uintptr_t tmp = HIKEY960_NS_TMP_OFFSET;

	if (image_id != NS_BL1U_IMAGE_ID)
		panic();
	/* Copy NS BL1U from 0x1AC1_8000 to 0x1AC9_8000 */
	memcpy((void *)tmp, (void *)HIKEY960_NS_IMAGE_OFFSET,
		NS_BL1U_SIZE);
	memcpy((void *)NS_BL1U_BASE, (void *)tmp, NS_BL1U_SIZE);
	inv_dcache_range(NS_BL1U_BASE, NS_BL1U_SIZE);
	/* Initialize the GIC driver, cpu and distributor interfaces */
	gicv2_driver_init(&hikey960_gic_data);
	gicv2_distif_init();
	gicv2_pcpu_distif_init();
	gicv2_cpuif_enable();
	/* CNTFRQ is read-only in EL1 */
	write_cntfrq_el0(plat_get_syscnt_freq2());
	data = read_cpacr_el1();
	do {
		data |= 3 << 20;
		write_cpacr_el1(data);
		data = read_cpacr_el1();
	} while ((data & (3 << 20)) != (3 << 20));
	INFO("cpacr_el1:0x%x\n", data);

	ep_info->args.arg0 = 0xffff & read_mpidr();
	ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
				DISABLE_ALL_EXCEPTIONS);
}

/*******************************************************************************
 * Perform any BL1 specific platform actions.
 ******************************************************************************/
void bl1_early_platform_setup(void)
{
	const size_t bl1_size = BL1_RAM_LIMIT - BL1_RAM_BASE;

	/* Initialize the console to provide early debug support */
	console_init(PL011_UART2_BASE, PL011_UART2_CLK_IN_HZ, PL011_BAUDRATE);

	/*
	 * Enable CCI-400 for this cluster. No need for locks as no other cpu is
	 * active at the moment
	 */
	cci_init(CCI400_BASE,
		 CCI400_SL_IFACE3_CLUSTER_IX,
		 CCI400_SL_IFACE4_CLUSTER_IX);
	cci_enable_cluster_coherency(read_mpidr());

	/* Allow BL1 to see the whole Trusted RAM */
	bl1_tzram_layout.total_base = TZRAM_BASE;
	bl1_tzram_layout.total_size = TZRAM_SIZE;

	/* Calculate how much RAM BL1 is using and how much remains free */
	bl1_tzram_layout.free_base = TZRAM_BASE;
	bl1_tzram_layout.free_size = TZRAM_SIZE;
	reserve_mem(&bl1_tzram_layout.free_base,
		    &bl1_tzram_layout.free_size,
		    BL1_RAM_BASE,
		    bl1_size);

	INFO("BL1: 0x%lx - 0x%lx [size = %u]\n", BL1_RAM_BASE, BL1_RAM_LIMIT,
	     bl1_size);
}

/*******************************************************************************
 * This cpu is being suspended. S-EL1 state must have been saved in the
 * resident cpu (mpidr format) if it is a UP/UP migratable TSP.
 ******************************************************************************/
static void tspd_cpu_suspend_handler(uint64_t power_state)
{
	int32_t rc = 0;
	uint64_t mpidr = read_mpidr();
	uint32_t linear_id = platform_get_core_pos(mpidr);
	tsp_context *tsp_ctx = &tspd_sp_context[linear_id];

	assert(tsp_entry_info);
	assert(tsp_ctx->state == TSP_STATE_ON);

	/* Program the entry point, power_state parameter and enter the TSP */
	write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx),
		      CTX_GPREG_X0,
		      power_state);
	cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_suspend_entry);
	rc = tspd_synchronous_sp_entry(tsp_ctx);

	/*
	 * Read the response from the TSP. A non-zero return means that
	 * something went wrong while communicating with the TSP.
	 */
	if (rc != 0)
		panic();

	/* Update its context to reflect the state the TSP is in */
	tsp_ctx->state = TSP_STATE_SUSPEND;
}

/*******************************************************************************
 * This function performs any book keeping in the test secure payload after this
 * cpu's architectural state has been restored after wakeup from an earlier psci
 * cpu_suspend request.
 ******************************************************************************/
tsp_args *tsp_cpu_resume_main(uint64_t suspend_level,
			      uint64_t arg1,
			      uint64_t arg2,
			      uint64_t arg3,
			      uint64_t arg4,
			      uint64_t arg5,
			      uint64_t arg6,
			      uint64_t arg7)
{
	uint64_t mpidr = read_mpidr();
	uint32_t linear_id = platform_get_core_pos(mpidr);

	/* Update this cpu's statistics */
	tsp_stats[linear_id].smc_count++;
	tsp_stats[linear_id].eret_count++;
	tsp_stats[linear_id].cpu_resume_count++;

	spin_lock(&console_lock);
	printf("SP: cpu 0x%x resumed. suspend level %d \n\r",
	       mpidr, suspend_level);
	INFO("cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n", mpidr,
	     tsp_stats[linear_id].smc_count,
	     tsp_stats[linear_id].eret_count,
	     tsp_stats[linear_id].cpu_suspend_count);
	spin_unlock(&console_lock);

	/* Indicate to the SPD that we have completed this request */
	return set_smc_args(TSP_RESUME_DONE, 0, 0, 0, 0, 0, 0, 0);
}

/*******************************************************************************
 * This cpu is being turned off. Allow the TSPD/TSP to perform any actions
 * needed
 ******************************************************************************/
static int32_t tspd_cpu_off_handler(uint64_t cookie)
{
	int32_t rc = 0;
	uint64_t mpidr = read_mpidr();
	uint32_t linear_id = platform_get_core_pos(mpidr);
	tsp_context *tsp_ctx = &tspd_sp_context[linear_id];

	assert(tsp_entry_info);
	assert(tsp_ctx->state == TSP_STATE_ON);

	/* Program the entry point and enter the TSP */
	cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_off_entry);
	rc = tspd_synchronous_sp_entry(tsp_ctx);

	/*
	 * Read the response from the TSP. A non-zero return means that
	 * something went wrong while communicating with the TSP.
	 */
	if (rc != 0)
		panic();

	/*
	 * Reset TSP's context for a fresh start when this cpu is turned on
	 * subsequently.
	 */
	 tsp_ctx->state = TSP_STATE_OFF;

	 return 0;
}

/*******************************************************************************
 * This cpu has been turned on. Enter the TSP to initialise S-EL1 and other bits
 * before passing control back to the Secure Monitor. Entry in S-El1 is done
 * after initialising minimal architectural state that guarantees safe
 * execution.
 ******************************************************************************/
static void tspd_cpu_on_finish_handler(uint64_t cookie)
{
	int32_t rc = 0;
	uint64_t mpidr = read_mpidr();
	uint32_t linear_id = platform_get_core_pos(mpidr);
	tsp_context *tsp_ctx = &tspd_sp_context[linear_id];

	assert(tsp_entry_info);
	assert(tsp_ctx->state == TSP_STATE_OFF);

	/* Initialise this cpu's secure context */
	tspd_init_secure_context((uint64_t) tsp_entry_info->cpu_on_entry,
				TSP_AARCH64,
				mpidr,
				tsp_ctx);

	/* Enter the TSP */
	rc = tspd_synchronous_sp_entry(tsp_ctx);

	/*
	 * Read the response from the TSP. A non-zero return means that
	 * something went wrong while communicating with the SP.
	 */
	if (rc != 0)
		panic();

	/* Update its context to reflect the state the SP is in */
	tsp_ctx->state = TSP_STATE_ON;
}

/*******************************************************************************
 * TSP main entry point where it gets the opportunity to initialize its secure
 * state/applications. Once the state is initialized, it must return to the
 * SPD with a pointer to the 'tsp_vector_table' jump table.
 ******************************************************************************/
uint64_t tsp_main(void)
{
	NOTICE("TSP: %s\n", version_string);
	NOTICE("TSP: %s\n", build_message);
	INFO("TSP: Total memory base : 0x%lx\n", BL32_TOTAL_BASE);
	INFO("TSP: Total memory size : 0x%lx bytes\n",
			 BL32_TOTAL_LIMIT - BL32_TOTAL_BASE);

	uint32_t linear_id = plat_my_core_pos();

	/* Initialize the platform */
	tsp_platform_setup();

	/* Initialize secure/applications state here */
	tsp_generic_timer_start();

	/* Update this cpu's statistics */
	tsp_stats[linear_id].smc_count++;
	tsp_stats[linear_id].eret_count++;
	tsp_stats[linear_id].cpu_on_count++;

#if LOG_LEVEL >= LOG_LEVEL_INFO
	spin_lock(&console_lock);
	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
	     read_mpidr(),
	     tsp_stats[linear_id].smc_count,
	     tsp_stats[linear_id].eret_count,
	     tsp_stats[linear_id].cpu_on_count);
	spin_unlock(&console_lock);
#endif
	return (uint64_t) &tsp_vector_table;
}

/*******************************************************************************
 * BL31 is responsible for setting up the runtime services for the primary cpu
 * before passing control to the bootloader or an Operating System. This
 * function calls runtime_svc_init() which initializes all registered runtime
 * services. The run time services would setup enough context for the core to
 * swtich to the next exception level. When this function returns, the core will
 * switch to the programmed exception level via. an ERET.
 ******************************************************************************/
void bl31_main(void)
{
#if DEBUG
	unsigned long mpidr = read_mpidr();
#endif

	/* Perform remaining generic architectural setup from EL3 */
	bl31_arch_setup();

	/* Perform platform setup in BL1 */
	bl31_platform_setup();

	printf("BL31 %s\n\r", build_message);

	/* Initialise helper libraries */
	bl31_lib_init();

	/* Initialize the runtime services e.g. psci */
	runtime_svc_init();

	/* Clean caches before re-entering normal world */
	dcsw_op_all(DCCSW);

	/*
	 * Use the more complex exception vectors now that context
	 * management is setup. SP_EL3 should point to a 'cpu_context'
	 * structure which has an exception stack allocated.  The PSCI
	 * service should have set the context.
	 */
	assert(cm_get_context(mpidr, NON_SECURE));
	cm_set_next_eret_context(NON_SECURE);
	cm_init_pcpu_ptr_cache();
	write_vbar_el3((uint64_t) runtime_exceptions);
	isb();
	next_image_type = NON_SECURE;

	/*
	 * All the cold boot actions on the primary cpu are done. We now need to
	 * decide which is the next image (BL32 or BL33) and how to execute it.
	 * If the SPD runtime service is present, it would want to pass control
	 * to BL32 first in S-EL1. In that case, SPD would have registered a
	 * function to intialize bl32 where it takes responsibility of entering
	 * S-EL1 and returning control back to bl31_main. Once this is done we
	 * can prepare entry into BL33 as normal.
	 */

	/*
	 * If SPD had registerd an init hook, invoke it. Pass it the information
	 * about memory extents
	 */
	if (bl32_init)
		(*bl32_init)(bl31_plat_get_bl32_mem_layout());

	/*
	 * We are ready to enter the next EL. Prepare entry into the image
	 * corresponding to the desired security state after the next ERET.
	 */
	bl31_prepare_next_image_entry();
}