#include <psw.h>
#include <die.h>

#include "svcint.h"

void setcontext(struct psw *psw, u64 *regs)
{
	__builtin_memcpy(PSA_PSW_TMP, psw, sizeof(struct psw));

	asm volatile(
		"lmg	%%r0,%%r15,%0\n"	/* load gpr */
		"lpswe	%1\n"			/* load new psw */
	: /* output */
	: /* input */
	  "m" (*regs),
	  "m" (*PSA_PSW_TMP)
	);

	/* unreachable */
	die();
}

void swapcontext(struct psw *oldpsw, u64 *oldregs,
		 struct psw *newpsw, u64 *newregs)
{
	__builtin_memcpy(PSA_PSW_TMP, newpsw, sizeof(struct psw));

	__builtin_memset(oldpsw, 0, sizeof(struct psw));
	oldpsw->ea = 1;
	oldpsw->ba = 1;

	asm volatile(
		"stmg	%%r0,%%r15,%0\n"	/* store gpr */
		"larl	%%r1,0f\n"
		"stg	%%r1,%1\n"		/* store the IA */

		"lmg	%%r0,%%r15,%2\n"	/* load gpr */
		"lpswe	%3\n"			/* load new psw */

		"0:\n"
	: /* output */
	  "=m" (*oldregs),
	  "=m" (oldpsw->ptr)
	: /* input */
	  "m" (*newregs),
	  "m" (*PSA_PSW_TMP)
	);
}

void savecontext(struct psw *psw, u64 *regs)
{
	__builtin_memcpy(psw, PSA_OLD_SVC_PSW, sizeof(struct psw));
	__builtin_memcpy(regs, PSA_GPRS, 16*sizeof(u64));
}

extern void *SVCINT;
u64 _SVC_HANDLER;
u64 _SVC_STACK;

void set_svc_handler(u64(*f)(u64, u64, u64, u64, u64), void *stack)
{
	struct psw psw;

	if (((u64)stack) & 7)
		die();

	_SVC_HANDLER = (u64) f;
	_SVC_STACK   = (u64) stack;

	__builtin_memset(&psw, 0, sizeof(struct psw));
	psw.ea = 1;
	psw.ba = 1;
	psw.ptr = (u64) &SVCINT;
	__builtin_memcpy(PSA_NEW_SVC_PSW, &psw, sizeof(struct psw));
}