/*
 * Copyright (C) 2025 Intel Corporation.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 * Authors:
 *   Haicheng Li <haicheng.li@intel.com>
 */

#include <types.h>
#include <cpu.h>
#include <per_cpu.h>
#include <logmsg.h>
#include <mmu.h>
#include <timer.h>
#include <console.h>
#include <shell.h>
#include <uart16550.h>
#ifdef STACK_PROTECTOR
#include <asm/security.h>
#endif

static void init_pcpu_comm_post(void);

/* Push sp magic to top of stack for call trace */
#define SWITCH_TO(sp, to)							\
{										\
	asm volatile ( 								\
		"mv sp, %0\n"           /* Set stack pointer */			\
		"addi sp, sp, -8\n"     /* Make room for magic value */		\
		"sd %1, 0(sp)\n"        /* Store magic value */			\
		"jalr %2\n"             /* Jump to function pointer */		\
		:								\
		: "r"(sp), "r"(SP_BOTTOM_MAGIC), "r"(to)			\
	);									\
}

static void init_debug_pre(void)
{
	console_init();
}

static void init_debug_post(uint16_t pcpu_id)
{
	if (pcpu_id == BSP_CPU_ID) {
		/* Initialize the shell */
		shell_init();
	}

	if (pcpu_id == VUART_TIMER_CPU) {
		console_setup_timer();
	}
}

/* C entry point for boot CPU */
void init_primary_pcpu(uint64_t hart_id, uint64_t fdt_paddr)
{
	uint16_t pcpu_id = BSP_CPU_ID;
	uint64_t pcpu_sp;

	(void)fdt_paddr;

	init_percpu_hart_id(hart_id);
	set_pcpu_active(pcpu_id);
#ifdef STACK_PROTECTOR
	init_stack_canary();
#endif

	/*
	 * Set state for this CPU to initializing, the current pcpu_id
	 * is set to a per-cpu register tp from now on.
	 */
	pcpu_set_current_state(pcpu_id, PCPU_STATE_INITIALIZING);

	init_debug_pre();
	init_paging();
	/*
	 * Need update uart_base_address here for vaddr2paddr mapping may changed
	 * WARNNING: DO NOT CALL PRINTF BETWEEN ENABLE PAGING IN init_paging AND HERE!
	 */
        uart16550_init(false);

	if (!start_pcpus(AP_MASK)) {
		panic("Failed to start all secondary cores!");
	}

	/* Switch to run-time stack */
	pcpu_sp = (uint64_t)(&get_cpu_var(stack)[CONFIG_STACK_SIZE - 1]);
	pcpu_sp &= ~(CPU_STACK_ALIGN - 1UL);
	SWITCH_TO(pcpu_sp, init_pcpu_comm_post);
}

/* C entry point for AP */
void init_secondary_pcpu(uint64_t hart_id)
{
	uint16_t pcpu_id;

	pcpu_id = get_pcpu_id_from_hart_id(hart_id);
	if (pcpu_id >= MAX_PCPU_NUM) {
		panic("Invalid pCPU ID!");
	}

	set_pcpu_active(pcpu_id);
	enable_paging();

	/*
	 * Set state for this CPU to initializing, the current pcpu_id
	 * is set to a per-cpu register tp from now on.
	 */
	pcpu_set_current_state(pcpu_id, PCPU_STATE_INITIALIZING);

	/* This function shall never return */
	init_pcpu_comm_post();
}

static void init_pcpu_comm_post(void)
{
	uint16_t pcpu_id;

	pcpu_id = get_pcpu_id();

	if (pcpu_id == BSP_CPU_ID) {
		/* Print Hypervisor Banner */
		print_hv_banner();
	}

	init_interrupt(pcpu_id);
	timer_init();

	init_sched(pcpu_id);

	init_debug_post(pcpu_id);
	run_idle_thread();
}