/* Load firmware */
enum ia_css_err ia_css_spctrl_load_fw(sp_ID_t sp_id,
				ia_css_spctrl_cfg *spctrl_cfg)
{
	hrt_vaddress code_addr = mmgr_NULL;
	struct ia_css_sp_init_dmem_cfg *init_dmem_cfg;

	if ((sp_id >= N_SP_ID) || (spctrl_cfg == 0))
		return IA_CSS_ERR_INVALID_ARGUMENTS;

	spctrl_cofig_info[sp_id].code_addr = mmgr_NULL;

#if defined(C_RUN) || defined(HRT_UNSCHED)
	(void)init_dmem_cfg;
	code_addr = mmgr_malloc(1);
	if (code_addr == mmgr_NULL)
		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
#else
	init_dmem_cfg = &spctrl_cofig_info[sp_id].dmem_config;
	init_dmem_cfg->dmem_data_addr = spctrl_cfg->dmem_data_addr;
	init_dmem_cfg->dmem_bss_addr  = spctrl_cfg->dmem_bss_addr;
	init_dmem_cfg->data_size      = spctrl_cfg->data_size;
	init_dmem_cfg->bss_size       = spctrl_cfg->bss_size;
	init_dmem_cfg->sp_id          = sp_id;

	spctrl_cofig_info[sp_id].spctrl_config_dmem_addr = spctrl_cfg->spctrl_config_dmem_addr;
	spctrl_cofig_info[sp_id].spctrl_state_dmem_addr = spctrl_cfg->spctrl_state_dmem_addr;

	/* store code (text + icache) and data to DDR
	 *
	 * Data used to be stored separately, because of access alignment constraints,
	 * fix the FW generation instead
	 */
	code_addr = mmgr_malloc(spctrl_cfg->code_size);
	if (code_addr == mmgr_NULL)
		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
	mmgr_store(code_addr, spctrl_cfg->code, spctrl_cfg->code_size);

	assert(sizeof(hrt_vaddress) <= sizeof(hrt_data));

	init_dmem_cfg->ddr_data_addr  = code_addr + spctrl_cfg->ddr_data_offset;
	assert((init_dmem_cfg->ddr_data_addr % HIVE_ISP_DDR_WORD_BYTES) == 0);
#endif
	spctrl_cofig_info[sp_id].sp_entry = spctrl_cfg->sp_entry;
	spctrl_cofig_info[sp_id].code_addr = code_addr;
	spctrl_cofig_info[sp_id].program_name = spctrl_cfg->program_name;

#ifdef HRT_CSIM
	hrt_cell_set_icache_base_address(SP, spctrl_cofig_info[sp_id].code_addr);
	hrt_cell_invalidate_icache(SP);
	hrt_cell_load_program(SP, spctrl_cofig_info[sp_id].program_name);
#else
	/* now we program the base address into the icache and
	 * invalidate the cache.
	 */
	sp_ctrl_store(sp_id, SP_ICACHE_ADDR_REG, (hrt_data)spctrl_cofig_info[sp_id].code_addr);
	sp_ctrl_setbit(sp_id, SP_ICACHE_INV_REG, SP_ICACHE_INV_BIT);
#endif
	spctrl_loaded[sp_id] = true;
	return IA_CSS_SUCCESS;
}
/* Load firmware */
enum ia_css_err ia_css_spctrl_load_fw(sp_ID_t sp_id,
				ia_css_spctrl_cfg *spctrl_cfg)
{
	hrt_vaddress code_addr = mmgr_NULL;
	struct ia_css_sp_init_dmem_cfg *init_dmem_cfg;

	if ((sp_id >= N_SP_ID) || (spctrl_cfg == 0))
		return IA_CSS_ERR_INVALID_ARGUMENTS;

	spctrl_cofig_info[sp_id].code_addr = mmgr_NULL;

#if defined(C_RUN) || defined(HRT_UNSCHED)
	(void)init_dmem_cfg;
	code_addr = mmgr_malloc(1);
	if (code_addr == mmgr_NULL)
		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
#else
	init_dmem_cfg = &spctrl_cofig_info[sp_id].dmem_config;
	init_dmem_cfg->dmem_data_addr = spctrl_cfg->dmem_data_addr;
	init_dmem_cfg->dmem_bss_addr  = spctrl_cfg->dmem_bss_addr;
	init_dmem_cfg->data_size      = spctrl_cfg->data_size;
	init_dmem_cfg->bss_size       = spctrl_cfg->bss_size;
	init_dmem_cfg->sp_id          = sp_id;

	spctrl_cofig_info[sp_id].spctrl_config_dmem_addr = spctrl_cfg->spctrl_config_dmem_addr;
	spctrl_cofig_info[sp_id].spctrl_state_dmem_addr = spctrl_cfg->spctrl_state_dmem_addr;

	/* store code (text + icache) and data to DDR
	 *
	 * Data used to be stored separately, because of access alignment constraints,
	 * fix the FW generation instead
	 */
	code_addr = mmgr_malloc(spctrl_cfg->code_size);
	if (code_addr == mmgr_NULL)
		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
	mmgr_store(code_addr, spctrl_cfg->code, spctrl_cfg->code_size);

	if (sizeof(hrt_vaddress) > sizeof(hrt_data)) {
		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
				    "size of hrt_vaddress can not be greater than hrt_data\n");
		mmgr_free(spctrl_cfg->code_size);
		spctrl_cfg->code_size = mmgr_NULL;
		return IA_CSS_ERR_INTERNAL_ERROR;
	}

	init_dmem_cfg->ddr_data_addr  = code_addr + spctrl_cfg->ddr_data_offset;
	if ((init_dmem_cfg->ddr_data_addr % HIVE_ISP_DDR_WORD_BYTES) != 0) {
		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
				    "DDR address pointer is not properly aligned for DMA transfer\n");
		mmgr_free(spctrl_cfg->code_size);
		spctrl_cfg->code_size = mmgr_NULL;
		return IA_CSS_ERR_INTERNAL_ERROR;
	}
#endif
	spctrl_cofig_info[sp_id].sp_entry = spctrl_cfg->sp_entry;
	spctrl_cofig_info[sp_id].code_addr = code_addr;
	spctrl_cofig_info[sp_id].program_name = spctrl_cfg->program_name;

#ifdef HRT_CSIM
	/* Secondary SP is named as SP2 in SDK, however we are using secondary
	   SP as SP1 in the HSS and secondary SP Firmware */
	if (sp_id == SP0_ID) {
		hrt_cell_set_icache_base_address(SP, spctrl_cofig_info[sp_id].code_addr);
		hrt_cell_invalidate_icache(SP);
		hrt_cell_load_program(SP, spctrl_cofig_info[sp_id].program_name);
	}
#if defined(HAS_SEC_SP)
	else {
		hrt_cell_set_icache_base_address(SP2, spctrl_cofig_info[sp_id].code_addr);
		hrt_cell_invalidate_icache(SP2);
		hrt_cell_load_program(SP2, spctrl_cofig_info[sp_id].program_name);
	}
#endif /* HAS_SEC_SP */
#else
	/* now we program the base address into the icache and
	 * invalidate the cache.
	 */
	sp_ctrl_store(sp_id, SP_ICACHE_ADDR_REG, (hrt_data)spctrl_cofig_info[sp_id].code_addr);
	sp_ctrl_setbit(sp_id, SP_ICACHE_INV_REG, SP_ICACHE_INV_BIT);
#endif
	spctrl_loaded[sp_id] = true;
	return IA_CSS_SUCCESS;
}