#include "Dolphin/os.h"
#include "Dolphin/db.h"

// necessary for some asm functions
extern void __RAS_OSDisableInterrupts_begin();
extern void __RAS_OSDisableInterrupts_end();

/**
 * @note Address: 0x800ECD50
 * @note Size: 0x124
 */
ASM static void __OSLoadFPUContext(register u32, register OSContext* fpuContext)
{
#ifdef __MWERKS__ // clang-format off
	nofralloc
	lhz r5, fpuContext->state;
	clrlwi. r5, r5, 31
	beq _return

	lfd fp0, OS_CONTEXT_FPSCR(fpuContext)
	mtfsf 0xFF, fp0
	mfspr r5, HID2
	rlwinm. r5, r5, 3, 31, 31
	beq _regular_FPRs

	psq_l fp0, OS_CONTEXT_PSF0(fpuContext), 0, 0
	psq_l fp1, OS_CONTEXT_PSF1(fpuContext), 0, 0
	psq_l fp2, OS_CONTEXT_PSF2(fpuContext), 0, 0
	psq_l fp3, OS_CONTEXT_PSF3(fpuContext), 0, 0
	psq_l fp4, OS_CONTEXT_PSF4(fpuContext), 0, 0
	psq_l fp5, OS_CONTEXT_PSF5(fpuContext), 0, 0
	psq_l fp6, OS_CONTEXT_PSF6(fpuContext), 0, 0
	psq_l fp7, OS_CONTEXT_PSF7(fpuContext), 0, 0
	psq_l fp8, OS_CONTEXT_PSF8(fpuContext), 0, 0
	psq_l fp9, OS_CONTEXT_PSF9(fpuContext), 0, 0
	psq_l fp10, OS_CONTEXT_PSF10(fpuContext), 0, 0
	psq_l fp11, OS_CONTEXT_PSF11(fpuContext), 0, 0
	psq_l fp12, OS_CONTEXT_PSF12(fpuContext), 0, 0
	psq_l fp13, OS_CONTEXT_PSF13(fpuContext), 0, 0
	psq_l fp14, OS_CONTEXT_PSF14(fpuContext), 0, 0
	psq_l fp15, OS_CONTEXT_PSF15(fpuContext), 0, 0
	psq_l fp16, OS_CONTEXT_PSF16(fpuContext), 0, 0
	psq_l fp17, OS_CONTEXT_PSF17(fpuContext), 0, 0
	psq_l fp18, OS_CONTEXT_PSF18(fpuContext), 0, 0
	psq_l fp19, OS_CONTEXT_PSF19(fpuContext), 0, 0
	psq_l fp20, OS_CONTEXT_PSF20(fpuContext), 0, 0
	psq_l fp21, OS_CONTEXT_PSF21(fpuContext), 0, 0
	psq_l fp22, OS_CONTEXT_PSF22(fpuContext), 0, 0
	psq_l fp23, OS_CONTEXT_PSF23(fpuContext), 0, 0
	psq_l fp24, OS_CONTEXT_PSF24(fpuContext), 0, 0
	psq_l fp25, OS_CONTEXT_PSF25(fpuContext), 0, 0
	psq_l fp26, OS_CONTEXT_PSF26(fpuContext), 0, 0
	psq_l fp27, OS_CONTEXT_PSF27(fpuContext), 0, 0
	psq_l fp28, OS_CONTEXT_PSF28(fpuContext), 0, 0
	psq_l fp29, OS_CONTEXT_PSF29(fpuContext), 0, 0
	psq_l fp30, OS_CONTEXT_PSF30(fpuContext), 0, 0
	psq_l fp31, OS_CONTEXT_PSF31(fpuContext), 0, 0

_regular_FPRs:
	lfd fp0,  fpuContext->fpr[0]
	lfd fp1,  fpuContext->fpr[1]
	lfd fp2,  fpuContext->fpr[2]
	lfd fp3,  fpuContext->fpr[3]
	lfd fp4,  fpuContext->fpr[4]
	lfd fp5,  fpuContext->fpr[5]
	lfd fp6,  fpuContext->fpr[6]
	lfd fp7,  fpuContext->fpr[7]
	lfd fp8,  fpuContext->fpr[8]
	lfd fp9,  fpuContext->fpr[9]
	lfd fp10, fpuContext->fpr[10]
	lfd fp11, fpuContext->fpr[11]
	lfd fp12, fpuContext->fpr[12]
	lfd fp13, fpuContext->fpr[13]
	lfd fp14, fpuContext->fpr[14]
	lfd fp15, fpuContext->fpr[15]
	lfd fp16, fpuContext->fpr[16]
	lfd fp17, fpuContext->fpr[17]
	lfd fp18, fpuContext->fpr[18]
	lfd fp19, fpuContext->fpr[19]
	lfd fp20, fpuContext->fpr[20]
	lfd fp21, fpuContext->fpr[21]
	lfd fp22, fpuContext->fpr[22]
	lfd fp23, fpuContext->fpr[23]
	lfd fp24, fpuContext->fpr[24]
	lfd fp25, fpuContext->fpr[25]
	lfd fp26, fpuContext->fpr[26]
	lfd fp27, fpuContext->fpr[27]
	lfd fp28, fpuContext->fpr[28]
	lfd fp29, fpuContext->fpr[29]
	lfd fp30, fpuContext->fpr[30]
	lfd fp31, fpuContext->fpr[31]
_return:
	blr
#endif // clang-format on
}

/**
 * @note Address: 0x800ECE74
 * @note Size: 0x128
 */
ASM static void __OSSaveFPUContext(register u32, register u32, register OSContext* fpuContext) {
#ifdef __MWERKS__ // clang-format off
  	nofralloc

	lhz     r3,   fpuContext->state
	ori     r3,   r3, 1
	sth     r3,   fpuContext->state

	stfd    fp0,  fpuContext->fpr[0]
	stfd    fp1,  fpuContext->fpr[1]
	stfd    fp2,  fpuContext->fpr[2]
	stfd    fp3,  fpuContext->fpr[3]
	stfd    fp4,  fpuContext->fpr[4]
	stfd    fp5,  fpuContext->fpr[5]
	stfd    fp6,  fpuContext->fpr[6]
	stfd    fp7,  fpuContext->fpr[7]
	stfd    fp8,  fpuContext->fpr[8]
	stfd    fp9,  fpuContext->fpr[9]
	stfd    fp10, fpuContext->fpr[10]
	stfd    fp11, fpuContext->fpr[11]
	stfd    fp12, fpuContext->fpr[12]
	stfd    fp13, fpuContext->fpr[13]
	stfd    fp14, fpuContext->fpr[14]
	stfd    fp15, fpuContext->fpr[15]
	stfd    fp16, fpuContext->fpr[16]
	stfd    fp17, fpuContext->fpr[17]
	stfd    fp18, fpuContext->fpr[18]
	stfd    fp19, fpuContext->fpr[19]
	stfd    fp20, fpuContext->fpr[20]
	stfd    fp21, fpuContext->fpr[21]
	stfd    fp22, fpuContext->fpr[22]
	stfd    fp23, fpuContext->fpr[23]
	stfd    fp24, fpuContext->fpr[24]
	stfd    fp25, fpuContext->fpr[25]
	stfd    fp26, fpuContext->fpr[26]
	stfd    fp27, fpuContext->fpr[27]
	stfd    fp28, fpuContext->fpr[28]
	stfd    fp29, fpuContext->fpr[29]
	stfd    fp30, fpuContext->fpr[30]
	stfd    fp31, fpuContext->fpr[31]

	mffs    fp0
	stfd    fp0,  OS_CONTEXT_FPSCR(fpuContext)

	lfd     fp0,  fpuContext->fpr[0]

	mfspr   r3, HID2
	rlwinm. r3, r3, 3, 31, 31
	bc      12, 2, _return

	psq_st  fp0, OS_CONTEXT_PSF0(fpuContext), 0, 0
	psq_st  fp1, OS_CONTEXT_PSF1(fpuContext), 0, 0
	psq_st  fp2, OS_CONTEXT_PSF2(fpuContext), 0, 0
	psq_st  fp3, OS_CONTEXT_PSF3(fpuContext), 0, 0
	psq_st  fp4, OS_CONTEXT_PSF4(fpuContext), 0, 0
	psq_st  fp5, OS_CONTEXT_PSF5(fpuContext), 0, 0
	psq_st  fp6, OS_CONTEXT_PSF6(fpuContext), 0, 0
	psq_st  fp7, OS_CONTEXT_PSF7(fpuContext), 0, 0
	psq_st  fp8, OS_CONTEXT_PSF8(fpuContext), 0, 0
	psq_st  fp9, OS_CONTEXT_PSF9(fpuContext), 0, 0
	psq_st  fp10, OS_CONTEXT_PSF10(fpuContext), 0, 0
	psq_st  fp11, OS_CONTEXT_PSF11(fpuContext), 0, 0
	psq_st  fp12, OS_CONTEXT_PSF12(fpuContext), 0, 0
	psq_st  fp13, OS_CONTEXT_PSF13(fpuContext), 0, 0
	psq_st  fp14, OS_CONTEXT_PSF14(fpuContext), 0, 0
	psq_st  fp15, OS_CONTEXT_PSF15(fpuContext), 0, 0
	psq_st  fp16, OS_CONTEXT_PSF16(fpuContext), 0, 0
	psq_st  fp17, OS_CONTEXT_PSF17(fpuContext), 0, 0
	psq_st  fp18, OS_CONTEXT_PSF18(fpuContext), 0, 0
	psq_st  fp19, OS_CONTEXT_PSF19(fpuContext), 0, 0
	psq_st  fp20, OS_CONTEXT_PSF20(fpuContext), 0, 0
	psq_st  fp21, OS_CONTEXT_PSF21(fpuContext), 0, 0
	psq_st  fp22, OS_CONTEXT_PSF22(fpuContext), 0, 0
	psq_st  fp23, OS_CONTEXT_PSF23(fpuContext), 0, 0
	psq_st  fp24, OS_CONTEXT_PSF24(fpuContext), 0, 0
	psq_st  fp25, OS_CONTEXT_PSF25(fpuContext), 0, 0
	psq_st  fp26, OS_CONTEXT_PSF26(fpuContext), 0, 0
	psq_st  fp27, OS_CONTEXT_PSF27(fpuContext), 0, 0
	psq_st  fp28, OS_CONTEXT_PSF28(fpuContext), 0, 0
	psq_st  fp29, OS_CONTEXT_PSF29(fpuContext), 0, 0
	psq_st  fp30, OS_CONTEXT_PSF30(fpuContext), 0, 0
	psq_st  fp31, OS_CONTEXT_PSF31(fpuContext), 0, 0

_return:
		blr
#endif // clang-format on
}

/**
 * @note Address: N/A
 * @note Size: 0x8
 */
ASM void OSLoadFPUContext(register OSContext* fpuContext) {
#ifdef __MWERKS__ // clang-format off
	nofralloc
	addi    r4, fpuContext, 0
	b       __OSLoadFPUContext
#endif // clang-format on
}

/**
 * @note Address: 0x800ECF9C
 * @note Size: 0x8
 */
ASM void OSSaveFPUContext(register OSContext* fpuContext) {
#ifdef __MWERKS__ // clang-format off
	nofralloc
	addi    r5, fpuContext, 0
	b       __OSSaveFPUContext
#endif // clang-format on
}

/**
 * @note Address: 0x800ECFA4
 * @note Size: 0x5C
 */
ASM void OSSetCurrentContext(register OSContext* context) {
#ifdef __MWERKS__ // clang-format off
	nofralloc

	addis   r4, r0, OS_CACHED_REGION_PREFIX

	stw     context, 0x00D4(r4)

	clrlwi  r5, context, 2
	stw     r5, 0x00C0(r4)

	lwz     r5, 0x00D8(r4)
	cmpw    r5, context
	bne     _disableFPU

	lwz     r6, context->srr1
	ori     r6, r6, 0x2000
	stw     r6, context->srr1
	mfmsr   r6
	ori     r6, r6, 2
	mtmsr   r6
	blr

_disableFPU:
	lwz     r6, context->srr1
	rlwinm  r6, r6, 0, 19, 17
	stw     r6, context->srr1
	mfmsr   r6
	rlwinm  r6, r6, 0, 19, 17
	ori     r6, r6, 2
	mtmsr   r6
	isync
	blr
#endif // clang-format on
}

/**
 * @note Address: 0x800ED000
 * @note Size: 0xC
 */
OSContext* OSGetCurrentContext()
{
	return (OSContext*)__OSCurrentContext;
}

/**
 * @note Address: 0x800ED00C
 * @note Size: 0x80
 */
ASM u32 OSSaveContext(register OSContext* context) {
#ifdef __MWERKS__ // clang-format off
	nofralloc
	stmw    r13, context->gpr[13]
	mfspr   r0, GQR1
	stw     r0, context->gqr[1]
	mfspr   r0, GQR2
	stw     r0, context->gqr[2]
	mfspr   r0, GQR3
	stw     r0, context->gqr[3]
	mfspr   r0, GQR4
	stw     r0, context->gqr[4]
	mfspr   r0, GQR5
	stw     r0, context->gqr[5]
	mfspr   r0, GQR6
	stw     r0, context->gqr[6]
	mfspr   r0, GQR7
	stw     r0, context->gqr[7]
	mfcr    r0
	stw     r0, context->cr
	mflr    r0
	stw     r0, context->lr
	stw     r0, context->srr0
	mfmsr   r0
	stw     r0, context->srr1
	mfctr   r0
	stw     r0, context->ctr
	mfxer   r0
	stw     r0, context->xer
	stw     r1, context->gpr[1]
	stw     r2, context->gpr[2]
	li      r0, 0x1
	stw     r0, context->gpr[3]
	li      r3, 0
	blr
#endif // clang-format on
}

/**
 * @note Address: 0x800ED08C
 * @note Size: 0xD8
 */
ASM void OSLoadContext(register OSContext* context)
{
#ifdef __MWERKS__ // clang-format off
	nofralloc

	lis      r4,__RAS_OSDisableInterrupts_begin@ha
	lwz      r6,context->srr0
	addi     r5,r4,__RAS_OSDisableInterrupts_begin@l
	cmplw    r6,r5
	ble      _notInRAS
	lis      r4,__RAS_OSDisableInterrupts_end@ha
	addi     r0,r4,__RAS_OSDisableInterrupts_end@l
	cmplw    r6,r0
	bge      _notInRAS
	stw      r5,context->srr0

_notInRAS:

	lwz     r0, context->gpr[0]
	lwz     r1, context->gpr[1]
	lwz     r2, context->gpr[2]

	lhz     r4, context->state
	rlwinm. r5, r4, 0, 30, 30
	beq     notexc
	rlwinm  r4, r4, 0, 31, 29
	sth     r4, context->state
	lmw     r5, context->gpr[5]
	b       misc
notexc:
	lmw    r13, context->gpr[13]
misc:

	lwz     r4, context->gqr[1]
	mtspr   GQR1, r4
	lwz     r4, context->gqr[2]
	mtspr   GQR2, r4
	lwz     r4, context->gqr[3]
	mtspr   GQR3, r4
	lwz     r4, context->gqr[4]
	mtspr   GQR4, r4
	lwz     r4, context->gqr[5]
	mtspr   GQR5, r4
	lwz     r4, context->gqr[6]
	mtspr   GQR6, r4
	lwz     r4, context->gqr[7]
	mtspr   GQR7, r4

	lwz     r4, context->cr
	mtcr    r4
	lwz     r4, context->lr
	mtlr    r4
	lwz     r4, context->ctr
	mtctr   r4
	lwz     r4, context->xer
	mtxer   r4

	mfmsr   r4
	rlwinm  r4, r4, 0, 17, 15
	rlwinm  r4, r4, 0, 31, 29
	mtmsr   r4

	lwz     r4, context->srr0
	mtsrr0  r4
	lwz     r4, context->srr1
	mtsrr1  r4

	lwz     r4, context->gpr[4]
	lwz     r3, context->gpr[3]

	rfi
#endif // clang-format on
}

/**
 * @note Address: 0x800ED164
 * @note Size: 0x8
 */
ASM u32 OSGetStackPointer() {
#ifdef __MWERKS__ // clang-format off
	nofralloc
	mr r3, r1
	blr
#endif // clang-format on
}

/**
 * @note Address: N/A
 * @note Size: 0x10
 */
ASM u32 OSSwitchStack(register u32 newsp) {
#ifdef __MWERKS__ // clang-format off
	nofralloc
	mr  r5, r1
	mr  r1, newsp
	mr  r3, r5
	blr
#endif // clang-format on
}

/**
 * @note Address: N/A
 * @note Size: 0x30
 */
ASM int OSSwitchFiber(register u32 pc, register u32 newsp)
{
#ifdef __MWERKS__ // clang-format off
	nofralloc
	mflr    r0
	mr      r5, r1
	stwu    r5, -8(newsp)
	mr      r1, newsp
	stw     r0, 4(r5)
	mtlr    pc
	blrl
	lwz     r5, 0(r1)
	lwz     r0, 4(r5)
	mtlr    r0
	mr      r1, r5
	blr
#endif // clang-format on
}

/**
 * @note Address: 0x800ED16C
 * @note Size: 0x24
 */
void OSClearContext(OSContext* context)
{
	context->mode  = 0;
	context->state = 0;
	if (context == __OSFPUContext) {
		__OSFPUContext = nullptr;
	}
}

/**
 * @note Address: 0x800ED190
 * @note Size: 0xBC
 */
ASM void OSInitContext(register OSContext* context, register u32 pc, register u32 newsp)
{
#ifdef __MWERKS__ // clang-format off
	nofralloc

	stw     pc,  OS_CONTEXT_SRR0(context)
	stw     newsp,  OS_CONTEXT_R1(context)
	li      r11, 0
	ori     r11, r11, 0x00008000 | 0x00000020 | 0x00000010 | 0x00000002 | 0x00001000
	stw     r11, OS_CONTEXT_SRR1(context)
	li      r0,  0x0
	stw     r0,  OS_CONTEXT_CR(context)
	stw     r0,  OS_CONTEXT_XER(context)


	stw     r2,  OS_CONTEXT_R2(context)
	stw     r13, OS_CONTEXT_R13(context)

	stw     r0,  OS_CONTEXT_R3(context)
	stw     r0,  OS_CONTEXT_R4(context)
	stw     r0,  OS_CONTEXT_R5(context)
	stw     r0,  OS_CONTEXT_R6(context)
	stw     r0,  OS_CONTEXT_R7(context)
	stw     r0,  OS_CONTEXT_R8(context)
	stw     r0,  OS_CONTEXT_R9(context)
	stw     r0,  OS_CONTEXT_R10(context)
	stw     r0,  OS_CONTEXT_R11(context)
	stw     r0,  OS_CONTEXT_R12(context)

	stw     r0,  OS_CONTEXT_R14(context)
	stw     r0,  OS_CONTEXT_R15(context)
	stw     r0,  OS_CONTEXT_R16(context)
	stw     r0,  OS_CONTEXT_R17(context)
	stw     r0,  OS_CONTEXT_R18(context)
	stw     r0,  OS_CONTEXT_R19(context)
	stw     r0,  OS_CONTEXT_R20(context)
	stw     r0,  OS_CONTEXT_R21(context)
	stw     r0,  OS_CONTEXT_R22(context)
	stw     r0,  OS_CONTEXT_R23(context)
	stw     r0,  OS_CONTEXT_R24(context)
	stw     r0,  OS_CONTEXT_R25(context)
	stw     r0,  OS_CONTEXT_R26(context)
	stw     r0,  OS_CONTEXT_R27(context)
	stw     r0,  OS_CONTEXT_R28(context)
	stw     r0,  OS_CONTEXT_R29(context)
	stw     r0,  OS_CONTEXT_R30(context)
	stw     r0,  OS_CONTEXT_R31(context)

	stw     r0,  OS_CONTEXT_GQR0(context)
	stw     r0,  OS_CONTEXT_GQR1(context)
	stw     r0,  OS_CONTEXT_GQR2(context)
	stw     r0,  OS_CONTEXT_GQR3(context)
	stw     r0,  OS_CONTEXT_GQR4(context)
	stw     r0,  OS_CONTEXT_GQR5(context)
	stw     r0,  OS_CONTEXT_GQR6(context)
	stw     r0,  OS_CONTEXT_GQR7(context)

	b       OSClearContext
#endif // clang-format on
}

/**
 * @note Address: 0x800ED24C
 * @note Size: 0x2A8
 */
void OSDumpContext(OSContext* context)
{
	u32 i;
	u32* p;

	OSReport("------------------------- Context 0x%08x -------------------------\n", context);

	for (i = 0; i < 16; ++i) {
		OSReport("r%-2d  = 0x%08x (%14d)  r%-2d  = 0x%08x (%14d)\n", i, context->gpr[i], context->gpr[i], i + 16, context->gpr[i + 16],
		         context->gpr[i + 16]);
	}

	OSReport("LR   = 0x%08x                   CR   = 0x%08x\n", context->lr, context->cr);
	OSReport("SRR0 = 0x%08x                   SRR1 = 0x%08x\n", context->srr0, context->srr1);

	OSReport("\nGQRs----------\n");
	for (i = 0; i < 4; ++i) {
		OSReport("gqr%d = 0x%08x \t gqr%d = 0x%08x\n", i, context->gqr[i], i + 4, context->gqr[i + 4]);
	}

	if (context->state & OS_CONTEXT_STATE_FPSAVED) {
		OSContext* currentContext;
		OSContext fpuContext;
		BOOL enabled;

		enabled        = OSDisableInterrupts();
		currentContext = OSGetCurrentContext();
		OSClearContext(&fpuContext);
		OSSetCurrentContext(&fpuContext);

		OSReport("\n\nFPRs----------\n");
		for (i = 0; i < 32; i += 2) {
			OSReport("fr%d \t= %d \t fr%d \t= %d\n", i, (u32)context->fpr[i], i + 1, (u32)context->fpr[i + 1]);
		}
		OSReport("\n\nPSFs----------\n");
		for (i = 0; i < 32; i += 2) {
			OSReport("ps%d \t= 0x%x \t ps%d \t= 0x%x\n", i, (u32)context->psf[i], i + 1, (u32)context->psf[i + 1]);
		}

		OSClearContext(&fpuContext);
		OSSetCurrentContext(currentContext);
		OSRestoreInterrupts(enabled);
	}

	OSReport("\nAddress:      Back Chain    LR Save\n");
	for (i = 0, p = (u32*)context->gpr[1]; p && (u32)p != 0xffffffff && i++ < 16; p = (u32*)*p) {
		OSReport("0x%08x:   0x%08x    0x%08x\n", p, p[0], p[1]);
	}
}

/**
 * @note Address: 0x800ED4F4
 * @note Size: 0x84
 */
ASM static void OSSwitchFPUContext(register __OSException exception, register OSContext* context)
{
#ifdef __MWERKS__ // clang-format off
	nofralloc
	mfmsr   r5
	ori     r5, r5, 0x2000
	mtmsr   r5
	isync
	lwz     r5, OS_CONTEXT_SRR1(context)
	ori     r5, r5, 0x2000
	mtsrr1  r5
	addis   r3, r0, OS_CACHED_REGION_PREFIX
	lwz     r5, 0x00D8(r3)
	stw     context, 0x00D8(r3)
	cmpw    r5, r4
	beq     _restoreAndExit
	cmpwi   r5, 0x0
	beq     _loadNewFPUContext
	bl      __OSSaveFPUContext
_loadNewFPUContext:
	bl      __OSLoadFPUContext
_restoreAndExit:
	lwz     r3, OS_CONTEXT_CR(context)
	mtcr    r3
	lwz     r3, OS_CONTEXT_LR(context)
	mtlr    r3
	lwz     r3, OS_CONTEXT_SRR0(context)
	mtsrr0  r3
	lwz     r3, OS_CONTEXT_CTR(context)
	mtctr   r3
	lwz     r3, OS_CONTEXT_XER(context)
	mtxer   r3
	lhz     r3, context->state
	rlwinm  r3, r3, 0, 31, 29
	sth     r3, context->state
	lwz     r5, OS_CONTEXT_R5(context)
	lwz     r3, OS_CONTEXT_R3(context)
	lwz     r4, OS_CONTEXT_R4(context)
	rfi
#endif // clang-format on
}

/**
 * @note Address: 0x800ED578
 * @note Size: 0x48
 */
void __OSContextInit()
{
	__OSSetExceptionHandler(__OS_EXCEPTION_FLOATING_POINT, OSSwitchFPUContext);
	__OSFPUContext = nullptr;
	DBPrintf("FPU-unavailable handler installed\n");
}

/**
 * @note Address: 0x800ED5C0
 * @note Size: 0x12C
 */
ASM void OSFillFPUContext(register OSContext* context)
{
#ifdef __MWERKS__ // clang-format off
	nofralloc
	mfmsr     r5
	ori       r5, r5, 0x2000
	mtmsr     r5

	isync
	stfd      f0, 0x90(r3)
	stfd      f1, 0x98(r3)
	stfd      f2, 0xA0(r3)
	stfd      f3, 0xA8(r3)
	stfd      f4, 0xB0(r3)
	stfd      f5, 0xB8(r3)
	stfd      f6, 0xC0(r3)
	stfd      f7, 0xC8(r3)
	stfd      f8, 0xD0(r3)
	stfd      f9, 0xD8(r3)
	stfd      f10, 0xE0(r3)
	stfd      f11, 0xE8(r3)
	stfd      f12, 0xF0(r3)
	stfd      f13, 0xF8(r3)
	stfd      f14, 0x100(r3)
	stfd      f15, 0x108(r3)
	stfd      f16, 0x110(r3)
	stfd      f17, 0x118(r3)
	stfd      f18, 0x120(r3)
	stfd      f19, 0x128(r3)
	stfd      f20, 0x130(r3)
	stfd      f21, 0x138(r3)
	stfd      f22, 0x140(r3)
	stfd      f23, 0x148(r3)
	stfd      f24, 0x150(r3)
	stfd      f25, 0x158(r3)
	stfd      f26, 0x160(r3)
	stfd      f27, 0x168(r3)
	stfd      f28, 0x170(r3)
	stfd      f29, 0x178(r3)
	stfd      f30, 0x180(r3)
	stfd      f31, 0x188(r3)
	mffs      f0
	stfd      f0, 0x190(r3)
	lfd       f0, 0x90(r3)

	mfspr     r5, 0x398
	rlwinm.   r5,r5,3,31,31
	beq-      _return

	psq_st    f0,0x1C8(r3),0,0
	psq_st    f1,0x1D0(r3),0,0
	psq_st    f2,0x1D8(r3),0,0
	psq_st    f3,0x1E0(r3),0,0
	psq_st    f4,0x1E8(r3),0,0
	psq_st    f5,0x1F0(r3),0,0
	psq_st    f6,0x1F8(r3),0,0
	psq_st    f7,0x200(r3),0,0
	psq_st    f8,0x208(r3),0,0
	psq_st    f9,0x210(r3),0,0
	psq_st    f10,0x218(r3),0,0
	psq_st    f11,0x220(r3),0,0
	psq_st    f12,0x228(r3),0,0
	psq_st    f13,0x230(r3),0,0
	psq_st    f14,0x238(r3),0,0
	psq_st    f15,0x240(r3),0,0
	psq_st    f16,0x248(r3),0,0
	psq_st    f17,0x250(r3),0,0
	psq_st    f18,0x258(r3),0,0
	psq_st    f19,0x260(r3),0,0
	psq_st    f20,0x268(r3),0,0
	psq_st    f21,0x270(r3),0,0
	psq_st    f22,0x278(r3),0,0
	psq_st    f23,0x280(r3),0,0
	psq_st    f24,0x288(r3),0,0
	psq_st    f25,0x290(r3),0,0
	psq_st    f26,0x298(r3),0,0
	psq_st    f27,0x2A0(r3),0,0
	psq_st    f28,0x2A8(r3),0,0
	psq_st    f29,0x2B0(r3),0,0
	psq_st    f30,0x2B8(r3),0,0
	psq_st    f31,0x2C0(r3),0,0

_return:
	blr
#endif // clang-format on
}
