/*
 * Copyright 2015, Cyril Bur, IBM Corp.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version
 * 2 of the License, or (at your option) any later version.
 */

#include "../basic_asm.h"

# POS MUST BE 16 ALIGNED!
#define PUSH_VMX(pos,reg) \
	li	reg,pos; \
	stvx	v20,reg,sp; \
	addi	reg,reg,16; \
	stvx	v21,reg,sp; \
	addi	reg,reg,16; \
	stvx	v22,reg,sp; \
	addi	reg,reg,16; \
	stvx	v23,reg,sp; \
	addi	reg,reg,16; \
	stvx	v24,reg,sp; \
	addi	reg,reg,16; \
	stvx	v25,reg,sp; \
	addi	reg,reg,16; \
	stvx	v26,reg,sp; \
	addi	reg,reg,16; \
	stvx	v27,reg,sp; \
	addi	reg,reg,16; \
	stvx	v28,reg,sp; \
	addi	reg,reg,16; \
	stvx	v29,reg,sp; \
	addi	reg,reg,16; \
	stvx	v30,reg,sp; \
	addi	reg,reg,16; \
	stvx	v31,reg,sp;

# POS MUST BE 16 ALIGNED!
#define POP_VMX(pos,reg) \
	li	reg,pos; \
	lvx	v20,reg,sp; \
	addi	reg,reg,16; \
	lvx	v21,reg,sp; \
	addi	reg,reg,16; \
	lvx	v22,reg,sp; \
	addi	reg,reg,16; \
	lvx	v23,reg,sp; \
	addi	reg,reg,16; \
	lvx	v24,reg,sp; \
	addi	reg,reg,16; \
	lvx	v25,reg,sp; \
	addi	reg,reg,16; \
	lvx	v26,reg,sp; \
	addi	reg,reg,16; \
	lvx	v27,reg,sp; \
	addi	reg,reg,16; \
	lvx	v28,reg,sp; \
	addi	reg,reg,16; \
	lvx	v29,reg,sp; \
	addi	reg,reg,16; \
	lvx	v30,reg,sp; \
	addi	reg,reg,16; \
	lvx	v31,reg,sp;

# Carefull this will 'clobber' vmx (by design)
# Don't call this from C
FUNC_START(load_vmx)
	li	r5,0
	lvx	v20,r5,r3
	addi	r5,r5,16
	lvx	v21,r5,r3
	addi	r5,r5,16
	lvx	v22,r5,r3
	addi	r5,r5,16
	lvx	v23,r5,r3
	addi	r5,r5,16
	lvx	v24,r5,r3
	addi	r5,r5,16
	lvx	v25,r5,r3
	addi	r5,r5,16
	lvx	v26,r5,r3
	addi	r5,r5,16
	lvx	v27,r5,r3
	addi	r5,r5,16
	lvx	v28,r5,r3
	addi	r5,r5,16
	lvx	v29,r5,r3
	addi	r5,r5,16
	lvx	v30,r5,r3
	addi	r5,r5,16
	lvx	v31,r5,r3
	blr
FUNC_END(load_vmx)

# Should be safe from C, only touches r4, r5 and v0,v1,v2
FUNC_START(check_vmx)
	PUSH_BASIC_STACK(32)
	mr r4,r3
	li	r3,1 # assume a bad result
	li	r5,0
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v20
	vmr	v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v21
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v22
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v23
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v24
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v25
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v26
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v27
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v28
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v29
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v30
	vand	v2,v2,v1

	addi	r5,r5,16
	lvx	v0,r5,r4
	vcmpequd.	v1,v0,v31
	vand	v2,v2,v1

	li	r5,STACK_FRAME_LOCAL(0,0)
	stvx	v2,r5,sp
	ldx	r0,r5,sp
	cmpdi	r0,0xffffffffffffffff
	bne	1f
	li	r3,0
1:	POP_BASIC_STACK(32)
	blr
FUNC_END(check_vmx)

# Safe from C
FUNC_START(test_vmx)
	# r3 holds pointer to where to put the result of fork
	# r4 holds pointer to the pid
	# v20-v31 are non-volatile
	PUSH_BASIC_STACK(512)
	std	r3,STACK_FRAME_PARAM(0)(sp) # Address of varray
	std r4,STACK_FRAME_PARAM(1)(sp) # address of pid
	PUSH_VMX(STACK_FRAME_LOCAL(2,0),r4)

	bl load_vmx
	nop

	li	r0,__NR_fork
	sc
	# Pass the result of fork back to the caller
	ld	r9,STACK_FRAME_PARAM(1)(sp)
	std	r3,0(r9)

	ld r3,STACK_FRAME_PARAM(0)(sp)
	bl check_vmx
	nop

	POP_VMX(STACK_FRAME_LOCAL(2,0),r4)
	POP_BASIC_STACK(512)
	blr
FUNC_END(test_vmx)

# int preempt_vmx(vector int *varray, int *threads_starting, int *running)
# On starting will (atomically) decrement threads_starting as a signal that
# the VMX have been loaded with varray. Will proceed to check the validity of
# the VMX registers while running is not zero.
FUNC_START(preempt_vmx)
	PUSH_BASIC_STACK(512)
	std r3,STACK_FRAME_PARAM(0)(sp) # vector int *varray
	std r4,STACK_FRAME_PARAM(1)(sp) # int *threads_starting
	std r5,STACK_FRAME_PARAM(2)(sp) # int *running
	# VMX need to write to 16 byte aligned addresses, skip STACK_FRAME_LOCAL(3,0)
	PUSH_VMX(STACK_FRAME_LOCAL(4,0),r4)

	bl load_vmx
	nop

	sync
	# Atomic DEC
	ld r3,STACK_FRAME_PARAM(1)(sp)
1:	lwarx r4,0,r3
	addi r4,r4,-1
	stwcx. r4,0,r3
	bne- 1b

2:	ld r3,STACK_FRAME_PARAM(0)(sp)
	bl check_vmx
	nop
	cmpdi r3,0
	bne 3f
	ld r4,STACK_FRAME_PARAM(2)(sp)
	ld r5,0(r4)
	cmpwi r5,0
	bne 2b

3:	POP_VMX(STACK_FRAME_LOCAL(4,0),r4)
	POP_BASIC_STACK(512)
	blr
FUNC_END(preempt_vmx)