/*
 * Copyright (c) 2007, 2008 University of Tsukuba
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * 3. Neither the name of the University of Tsukuba nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/*
 * Copyright (c) 2010-2012 Yuichi Watanabe
 */

#include <core/assert.h>
#include <core/linkage.h>
#include <core/spinlock.h>
#include <core/string.h>
#include <core/cpu.h>
#include <core/time.h>
#include <core/wait.h>
#include "apic.h"
#include "constants.h"
#include "entry.h"
#include "int.h"
#include "mm.h"
#include "panic.h"
#include "seg.h"
#include "smp.h"

#define APINIT_ADDR		((APINIT_SEGMENT << 4) + APINIT_OFFSET)
#define APINIT_SIZE		(cpuinit_end - cpuinit_start)
#define APINIT_POINTER(n)	((void *)(apinit + ((u8 *)&n - cpuinit_start)))

static void ap_start (void);

static int num_of_processors = 1; 
static spinlock_t num_of_processors_lock;
static void (*bsp_main) (void), (*ap_main) (void);
static void *newstack_tmp;
static spinlock_t sync_lock;
volatile static u32 sync_id;
volatile static volatile u32 sync_count;
static spinlock_t *apinitlock;
volatile static bool all_cpu_started = false;
static struct wait wait_all_cpu;

/* this function is called after starting AP and switching a stack */
/* unlock the spinlock because the stack is switched */
static asmlinkage void
apinitproc1 (void)
{
	void *newstack;

	newstack = newstack_tmp;
	spinlock_unlock (apinitlock);
	spinlock_lock (&num_of_processors_lock);
	num_of_processors++;
	spinlock_unlock (&num_of_processors_lock);
	segment_init_ap();
	currentcpu->stackaddr = newstack;
	int_init_ap ();
	for (;;) {
		if (all_cpu_started) {
			break;
		}
		cpu_relax();
	}
	ap_main ();
}

static asmlinkage void
bspinitproc1 (void)
{
	ap_start ();
	bsp_main ();
	panic ("bspinitproc1");
}

/* this function is called on AP by entry.s */
/* other APs are waiting for spinlock */
/* the stack is cpuinit_tmpstack defined in entry.s */
/* this function allocates a stack and calls apinitproc1 with the new stack */
asmlinkage void
apinitproc0 (void)
{
	void *newstack;
	vmmerr_t err;

	/* Switch page tables from vmm_base_cr3 to vmm_main_cr3. */
	asm_wrcr3 (vmm_main_cr3);

	err = alloc_pages (&newstack, NULL, VMM_STACKSIZE / PAGESIZE);
	if (err) {
		panic("Failed to allocate pages for stack.");
	}
	newstack_tmp = newstack;
	asm_wrrsp_and_jmp ((ulong)newstack + VMM_STACKSIZE, apinitproc1);
}

static void
ap_start (void)
{
	volatile u32 *num;
	u8 *apinit;

	spinlock_init (&num_of_processors_lock);
	spinlock_init (&sync_lock);
	sync_id = 0;
	sync_count = 0;

	apinit = mapmem (MAPMEM_HPHYS | MAPMEM_WRITE, APINIT_ADDR,
			 APINIT_SIZE);
	if (apinit == NULL) {
		panic("Failed to map apinit area.");
	}
	memcpy (apinit, cpuinit_start, APINIT_SIZE);
	num = (volatile u32 *)APINIT_POINTER (apinit_procs);
	apinitlock = (spinlock_t *)APINIT_POINTER (apinit_lock);
	*num = 1;
	spinlock_init (apinitlock);
	apic_start_ap(APINIT_ADDR);
	for (;;) {
		if (*num == get_number_of_processors ())
			break;
		cpu_relax();
	}
	all_cpu_started = true;
	unmapmem ((void *)apinit, APINIT_SIZE);
}

static void
bspinitproc0 ()
{
	void *newstack;
	vmmerr_t err;

	err = alloc_pages (&newstack, NULL, VMM_STACKSIZE / PAGESIZE);
	if (err) {
		panic("Failed to allocate pages for stack.");
	}
	currentcpu->stackaddr = newstack;
	asm_wrrsp_and_jmp ((ulong)newstack + VMM_STACKSIZE, bspinitproc1);
}

void
panic_wakeup_all (void)
{
	apic_broadcast_nmi();
}

void
sync_all_processors (void)
{
	wait(&wait_all_cpu, get_number_of_processors());
}

void
start_all_processors (void (*bsp) (void), void (*ap) (void))
{
	wait_init(&wait_all_cpu);
	bsp_main = bsp;
	ap_main = ap;
	bspinitproc0();
}

int
get_number_of_processors ()
{
	int num;
	spinlock_lock (&num_of_processors_lock);
	num = num_of_processors;
	spinlock_unlock (&num_of_processors_lock);
	return num;
}
