/*
 * Copyright (c) 2011, 2017 embedded brains GmbH.  All rights reserved.
 *
 *  embedded brains GmbH
 *  Dornierstr. 4
 *  82178 Puchheim
 *  Germany
 *  <rtems@embedded-brains.de>
 *
 * The license and distribution terms for this file may be
 * found in the file LICENSE in this distribution or at
 * http://www.rtems.org/license/LICENSE.
 */

#include <bspopts.h>
#include <rtems/score/percpu.h>
#include <bsp/vectors.h>

#define SCRATCH_REGISTER_0 r3
#define SCRATCH_REGISTER_1 r4

	.global	ppc_exc_fatal_normal
	.global	ppc_exc_fatal_critical
	.global	ppc_exc_fatal_machine_check
	.global	ppc_exc_fatal_debug

ppc_exc_fatal_critical:

	PPC_REG_STORE	SCRATCH_REGISTER_1, GPR4_OFFSET(r1)
	mfcsrr0	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR0_FRAME_OFFSET(r1)
	mfcsrr1	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR1_FRAME_OFFSET(r1)
	b	.Lppc_exc_fatal

ppc_exc_fatal_machine_check:

	PPC_REG_STORE	SCRATCH_REGISTER_1, GPR4_OFFSET(r1)
	mfmcsrr0	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR0_FRAME_OFFSET(r1)
	mfmcsrr1	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR1_FRAME_OFFSET(r1)
	b	.Lppc_exc_fatal

ppc_exc_fatal_debug:

	PPC_REG_STORE	SCRATCH_REGISTER_1, GPR4_OFFSET(r1)
	mfspr	SCRATCH_REGISTER_1, BOOKE_DSRR0
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR0_FRAME_OFFSET(r1)
	mfspr	SCRATCH_REGISTER_1, BOOKE_DSRR1
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR1_FRAME_OFFSET(r1)
	b	.Lppc_exc_fatal

ppc_exc_fatal_normal:

	PPC_REG_STORE	SCRATCH_REGISTER_1, GPR4_OFFSET(r1)
	mfsrr0	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR0_FRAME_OFFSET(r1)
	mfsrr1	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, SRR1_FRAME_OFFSET(r1)

.Lppc_exc_fatal:

	stw	r3, EXCEPTION_NUMBER_OFFSET(r1)
	mfcr	SCRATCH_REGISTER_1
	stw	SCRATCH_REGISTER_1, EXC_CR_OFFSET(r1)
	mfxer	SCRATCH_REGISTER_1
	stw	SCRATCH_REGISTER_1, EXC_XER_OFFSET(r1)
	mfctr	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, EXC_CTR_OFFSET(r1)
	mflr	SCRATCH_REGISTER_1
	PPC_REG_STORE	SCRATCH_REGISTER_1, EXC_LR_OFFSET(r1)
	PPC_REG_STORE	r0, GPR0_OFFSET(r1)
	PPC_REG_STORE	r1, GPR1_OFFSET(r1)
	PPC_REG_STORE	r2, GPR2_OFFSET(r1)
	PPC_REG_STORE	r5, GPR5_OFFSET(r1)
	PPC_REG_STORE	r6, GPR6_OFFSET(r1)
	PPC_REG_STORE	r7, GPR7_OFFSET(r1)
	PPC_REG_STORE	r8, GPR8_OFFSET(r1)
	PPC_REG_STORE	r9, GPR9_OFFSET(r1)
	PPC_REG_STORE	r10, GPR10_OFFSET(r1)
	PPC_REG_STORE	r11, GPR11_OFFSET(r1)
	PPC_REG_STORE	r12, GPR12_OFFSET(r1)
	PPC_REG_STORE	r13, GPR13_OFFSET(r1)
	PPC_REG_STORE	r14, GPR14_OFFSET(r1)
	PPC_REG_STORE	r15, GPR15_OFFSET(r1)
	PPC_REG_STORE	r16, GPR16_OFFSET(r1)
	PPC_REG_STORE	r17, GPR17_OFFSET(r1)
	PPC_REG_STORE	r18, GPR18_OFFSET(r1)
	PPC_REG_STORE	r19, GPR19_OFFSET(r1)
	PPC_REG_STORE	r20, GPR20_OFFSET(r1)
	PPC_REG_STORE	r21, GPR21_OFFSET(r1)
	PPC_REG_STORE	r22, GPR22_OFFSET(r1)
	PPC_REG_STORE	r23, GPR23_OFFSET(r1)
	PPC_REG_STORE	r24, GPR24_OFFSET(r1)
	PPC_REG_STORE	r25, GPR25_OFFSET(r1)
	PPC_REG_STORE	r26, GPR26_OFFSET(r1)
	PPC_REG_STORE	r27, GPR27_OFFSET(r1)
	PPC_REG_STORE	r28, GPR28_OFFSET(r1)
	PPC_REG_STORE	r29, GPR29_OFFSET(r1)
	PPC_REG_STORE	r30, GPR30_OFFSET(r1)
	PPC_REG_STORE	r31, GPR31_OFFSET(r1)

	/* Enable FPU and/or AltiVec */
#if defined(PPC_MULTILIB_FPU) || defined(PPC_MULTILIB_ALTIVEC)
	mfmsr	SCRATCH_REGISTER_1
#ifdef PPC_MULTILIB_FPU
	ori	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, MSR_FP
#endif
#ifdef PPC_MULTILIB_ALTIVEC
	oris	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, MSR_VE >> 16
#endif
	mtmsr	SCRATCH_REGISTER_1
	isync
#endif

#ifdef PPC_MULTILIB_ALTIVEC
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(0)
	stvx	v0, r1, SCRATCH_REGISTER_1
	mfvscr	v0
	li	SCRATCH_REGISTER_1, PPC_EXC_VSCR_OFFSET
	stvewx	v0, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(1)
	stvx	v1, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(2)
	stvx	v2, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(3)
	stvx	v3, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(4)
	stvx	v4, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(5)
	stvx	v5, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(6)
	stvx	v6, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(7)
	stvx	v7, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(8)
	stvx	v8, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(9)
	stvx	v9, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(10)
	stvx	v10, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(11)
	stvx	v11, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(12)
	stvx	v12, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(13)
	stvx	v13, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(14)
	stvx	v14, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(15)
	stvx	v15, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(16)
	stvx	v16, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(17)
	stvx	v17, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(18)
	stvx	v18, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(19)
	stvx	v19, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(20)
	stvx	v20, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(21)
	stvx	v21, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(22)
	stvx	v22, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(23)
	stvx	v23, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(24)
	stvx	v24, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(25)
	stvx	v25, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(26)
	stvx	v26, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(27)
	stvx	v27, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(28)
	stvx	v28, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(29)
	stvx	v29, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(30)
	stvx	v30, r1, SCRATCH_REGISTER_1
	li	SCRATCH_REGISTER_1, PPC_EXC_VR_OFFSET(31)
	stvx	v31, r1, SCRATCH_REGISTER_1
	mfvrsave	SCRATCH_REGISTER_1
	stw	SCRATCH_REGISTER_1, PPC_EXC_VRSAVE_OFFSET(r1)
#endif

#ifdef PPC_MULTILIB_FPU
	stfd	f0, PPC_EXC_FR_OFFSET(0)(r1)
	mffs	f0
	stfd	f0, PPC_EXC_FPSCR_OFFSET(r1)
	stfd	f1, PPC_EXC_FR_OFFSET(1)(r1)
	stfd	f2, PPC_EXC_FR_OFFSET(2)(r1)
	stfd	f3, PPC_EXC_FR_OFFSET(3)(r1)
	stfd	f4, PPC_EXC_FR_OFFSET(4)(r1)
	stfd	f5, PPC_EXC_FR_OFFSET(5)(r1)
	stfd	f6, PPC_EXC_FR_OFFSET(6)(r1)
	stfd	f7, PPC_EXC_FR_OFFSET(7)(r1)
	stfd	f8, PPC_EXC_FR_OFFSET(8)(r1)
	stfd	f9, PPC_EXC_FR_OFFSET(9)(r1)
	stfd	f10, PPC_EXC_FR_OFFSET(10)(r1)
	stfd	f11, PPC_EXC_FR_OFFSET(11)(r1)
	stfd	f12, PPC_EXC_FR_OFFSET(12)(r1)
	stfd	f13, PPC_EXC_FR_OFFSET(13)(r1)
	stfd	f14, PPC_EXC_FR_OFFSET(14)(r1)
	stfd	f15, PPC_EXC_FR_OFFSET(15)(r1)
	stfd	f16, PPC_EXC_FR_OFFSET(16)(r1)
	stfd	f17, PPC_EXC_FR_OFFSET(17)(r1)
	stfd	f18, PPC_EXC_FR_OFFSET(18)(r1)
	stfd	f19, PPC_EXC_FR_OFFSET(19)(r1)
	stfd	f20, PPC_EXC_FR_OFFSET(20)(r1)
	stfd	f21, PPC_EXC_FR_OFFSET(21)(r1)
	stfd	f22, PPC_EXC_FR_OFFSET(22)(r1)
	stfd	f23, PPC_EXC_FR_OFFSET(23)(r1)
	stfd	f24, PPC_EXC_FR_OFFSET(24)(r1)
	stfd	f25, PPC_EXC_FR_OFFSET(25)(r1)
	stfd	f26, PPC_EXC_FR_OFFSET(26)(r1)
	stfd	f27, PPC_EXC_FR_OFFSET(27)(r1)
	stfd	f28, PPC_EXC_FR_OFFSET(28)(r1)
	stfd	f29, PPC_EXC_FR_OFFSET(29)(r1)
	stfd	f30, PPC_EXC_FR_OFFSET(30)(r1)
	stfd	f31, PPC_EXC_FR_OFFSET(31)(r1)
#endif

	li	r3, 9
	addi	r4, r1, FRAME_LINK_SPACE
	b	_Terminate
	PPC64_NOP_FOR_LINKER_TOC_POINTER_RESTORE