/*
 * Copyright (c) 2017-2018, The Linux Foundation. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 and
 * only version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include <common.h>
#include <malloc.h>
#include <asm/psci.h>
#include <cli.h>
#include <console.h>
#include <linux/linkage.h>

DECLARE_GLOBAL_DATA_PTR;

#define SECONDARY_CORE_STACKSZ (8 * 1024)
#define CPU_POWER_DOWN (1 << 16)

extern void *globl_core_array;

struct cpu_entry_arg {
	void *stack_ptr;
	volatile void *gd_ptr;
	void *arg_ptr;
	int  cpu_up;
	int cmd_complete;
	int cmd_result;
	void *stack_top_ptr;
};

extern void secondary_cpu_init(void);
extern void bring_secondary_core_down(int);

struct cpu_entry_arg core[NR_CPUS - 1];

asmlinkage void secondary_core_entry(char *argv, int *cmd_complete,
					int *cmd_result)
{
	unsigned int state = 0;

	if (!mmu_enabled()) {
		mmu_setup();
		cp_delay();
	}

	/* Update here as ncessary - secondary entry point */
	*cmd_result = cli_simple_run_command(argv, CMD_FLAG_SEC_CORE);
	*cmd_complete = 1;

	state = CPU_POWER_DOWN;
	bring_secondary_core_down(state);
}

static void disable_console(void)
{
	gd->flags |= GD_FLG_SILENT | GD_FLG_DISABLE_CONSOLE;
}

static void enable_console(void)
{
	gd->flags &= ~(GD_FLG_SILENT | GD_FLG_DISABLE_CONSOLE);
}

int do_runmulticore(cmd_tbl_t *cmdtp,
			   int flag, int argc, char *const argv[])
{
	int j;
	int i;
	int ret;
	int delay = 0;
	int core_status = 0;
	int core_on_status = 0;
	char *ptr = NULL;

	if ((argc <= 1) || (argc > NR_CPUS + 1))
		return CMD_RET_USAGE;

	dcache_disable();

	/* Setting up stack for secondary cores */
	memset(core, 0, sizeof(core));

	globl_core_array = core;
	for (i = 1; i < argc; i++) {
		ptr = malloc(SECONDARY_CORE_STACKSZ);
		if (NULL == ptr) {
			j = i - 1;
			while (j >= 0) {
				free(core[i - 1].stack_ptr);
				j--;
			}
			printf("Memory allocation failure\n");
			return CMD_RET_FAILURE;
		}
		/* 0xf0 is the padding length */
		core[i - 1].stack_top_ptr = ptr;
		core[i - 1].stack_ptr = (ptr + (SECONDARY_CORE_STACKSZ) - 0xf0);

		core[i - 1].cpu_up = 0;
		core[i - 1].cmd_complete = 0;
		core[i - 1].cmd_result = -1;
		core[i - 1].gd_ptr = gd;
		core[i - 1].arg_ptr = argv[i];
	}

	if (!mmu_enabled()) {
		mmu_setup();
		cp_delay();
	}

	/* Bringing up the secondary cores */
	for (i = 1; i < argc; i++) {
		printf("Scheduling Core %d\n", i);
		delay = 0;
		disable_console();
		ret = bring_sec_core_up(i, (unsigned int)secondary_cpu_init,
				(unsigned int)&(core[i - 1]));
		if (ret) {
			panic("Some problem to getting core %d up\n", i);
		}
		while ((delay < 5) && (!(core[i - 1].cpu_up))) {
			mdelay(1000);
			delay++;
		}
		if (!(core[i - 1].cpu_up)) {
			panic("Can't bringup core %d\n",i);
		}
		enable_console();

		core_status |= (BIT(i - 1));
		core_on_status |= (BIT(i - 1));
	}

	/* Waiting for secondary cores to complete the task */
	while (core_status) {
		for (i = 1; i < argc; i++) {
			if ((core_status & (BIT(i - 1))) &&
					(core[i - 1].cmd_complete)) {
				printf("Command on core %d is %s\n", i,
					core[i - 1].cmd_complete ?
					((core[i - 1].cmd_result == -1) ?
					"FAIL" : "PASS"):
					"INCOMPLETE");
				core_status &= (~BIT((i - 1)));
			}
		}
		if (ctrlc()) {
			run_command("reset", 0);
		}
	}

	for (i = 1; (core_status && (i < argc)); i++) {
		if (core_status & (BIT(i - 1))) {
			printf("Command on core %d is %s\n", i,
				core[i - 1].cmd_complete ?
				((core[i - 1].cmd_result == -1) ?
				 "FAIL" : "PASS"): "INCOMPLETE");
		}
	}

	/* Waiting for cores to powerdown */
	delay = 0;
	while (core_on_status) {
		for (i = 1; i < argc; i++) {
			if (core_on_status & (BIT(i - 1))) {
				if (is_secondary_core_off(i) == 1) {
					printf("core %d powered off\n", i);
					core_on_status &= (~BIT((i - 1)));
				}
			}
		}
		mdelay(1000);
		delay++;
		if (delay > 5)
			panic("Some cores can't be powered off\n");
	}

	/* Free up all the stack */
	for (i = 1; i < argc; i++) {
		free(core[i - 1].stack_top_ptr);
	}

	printf("Status:\n");
	for (i = 1; i < argc; i++) {
		printf("Core %d: %s\n", i,
				core[i - 1].cmd_complete ?
				((core[i - 1].cmd_result == -1) ?
				 "FAIL" : "PASS"): "INCOMPLETE");
	}

	invalidate_dcache_all();
	dcache_enable();

	return CMD_RET_SUCCESS;
}

U_BOOT_CMD(runmulticore, 4, 0, do_runmulticore,
	   "Enable and schedule secondary cores",
	   "runmulticore <\"command to core1\"> [core2 core3 ...]");