1/*
2 * Copyright 2015, Cyril Bur, IBM Corp.
3 *
4 * This program is free software; you can redistribute it and/or
5 * modify it under the terms of the GNU General Public License
6 * as published by the Free Software Foundation; either version
7 * 2 of the License, or (at your option) any later version.
8 */
9
10#include "basic_asm.h"
11#include "vmx_asm.h"
12
13# Should be safe from C, only touches r4, r5 and v0,v1,v2
14FUNC_START(check_vmx)
15	PUSH_BASIC_STACK(32)
16	mr r4,r3
17	li	r3,1 # assume a bad result
18	li	r5,0
19	lvx	v0,r5,r4
20	vcmpequd.	v1,v0,v20
21	vmr	v2,v1
22
23	addi	r5,r5,16
24	lvx	v0,r5,r4
25	vcmpequd.	v1,v0,v21
26	vand	v2,v2,v1
27
28	addi	r5,r5,16
29	lvx	v0,r5,r4
30	vcmpequd.	v1,v0,v22
31	vand	v2,v2,v1
32
33	addi	r5,r5,16
34	lvx	v0,r5,r4
35	vcmpequd.	v1,v0,v23
36	vand	v2,v2,v1
37
38	addi	r5,r5,16
39	lvx	v0,r5,r4
40	vcmpequd.	v1,v0,v24
41	vand	v2,v2,v1
42
43	addi	r5,r5,16
44	lvx	v0,r5,r4
45	vcmpequd.	v1,v0,v25
46	vand	v2,v2,v1
47
48	addi	r5,r5,16
49	lvx	v0,r5,r4
50	vcmpequd.	v1,v0,v26
51	vand	v2,v2,v1
52
53	addi	r5,r5,16
54	lvx	v0,r5,r4
55	vcmpequd.	v1,v0,v27
56	vand	v2,v2,v1
57
58	addi	r5,r5,16
59	lvx	v0,r5,r4
60	vcmpequd.	v1,v0,v28
61	vand	v2,v2,v1
62
63	addi	r5,r5,16
64	lvx	v0,r5,r4
65	vcmpequd.	v1,v0,v29
66	vand	v2,v2,v1
67
68	addi	r5,r5,16
69	lvx	v0,r5,r4
70	vcmpequd.	v1,v0,v30
71	vand	v2,v2,v1
72
73	addi	r5,r5,16
74	lvx	v0,r5,r4
75	vcmpequd.	v1,v0,v31
76	vand	v2,v2,v1
77
78	li	r5,STACK_FRAME_LOCAL(0,0)
79	stvx	v2,r5,sp
80	ldx	r0,r5,sp
81	cmpdi	r0,0xffffffffffffffff
82	bne	1f
83	li	r3,0
841:	POP_BASIC_STACK(32)
85	blr
86FUNC_END(check_vmx)
87
88# Safe from C
89FUNC_START(test_vmx)
90	# r3 holds pointer to where to put the result of fork
91	# r4 holds pointer to the pid
92	# v20-v31 are non-volatile
93	PUSH_BASIC_STACK(512)
94	std	r3,STACK_FRAME_PARAM(0)(sp) # Address of varray
95	std r4,STACK_FRAME_PARAM(1)(sp) # address of pid
96	PUSH_VMX(STACK_FRAME_LOCAL(2,0),r4)
97
98	bl load_vmx
99	nop
100
101	li	r0,__NR_fork
102	sc
103	# Pass the result of fork back to the caller
104	ld	r9,STACK_FRAME_PARAM(1)(sp)
105	std	r3,0(r9)
106
107	ld r3,STACK_FRAME_PARAM(0)(sp)
108	bl check_vmx
109	nop
110
111	POP_VMX(STACK_FRAME_LOCAL(2,0),r4)
112	POP_BASIC_STACK(512)
113	blr
114FUNC_END(test_vmx)
115
116# int preempt_vmx(vector int *varray, int *threads_starting, int *running)
117# On starting will (atomically) decrement threads_starting as a signal that
118# the VMX have been loaded with varray. Will proceed to check the validity of
119# the VMX registers while running is not zero.
120FUNC_START(preempt_vmx)
121	PUSH_BASIC_STACK(512)
122	std r3,STACK_FRAME_PARAM(0)(sp) # vector int *varray
123	std r4,STACK_FRAME_PARAM(1)(sp) # int *threads_starting
124	std r5,STACK_FRAME_PARAM(2)(sp) # int *running
125	# VMX need to write to 16 byte aligned addresses, skip STACK_FRAME_LOCAL(3,0)
126	PUSH_VMX(STACK_FRAME_LOCAL(4,0),r4)
127
128	bl load_vmx
129	nop
130
131	sync
132	# Atomic DEC
133	ld r3,STACK_FRAME_PARAM(1)(sp)
1341:	lwarx r4,0,r3
135	addi r4,r4,-1
136	stwcx. r4,0,r3
137	bne- 1b
138
1392:	ld r3,STACK_FRAME_PARAM(0)(sp)
140	bl check_vmx
141	nop
142	cmpdi r3,0
143	bne 3f
144	ld r4,STACK_FRAME_PARAM(2)(sp)
145	ld r5,0(r4)
146	cmpwi r5,0
147	bne 2b
148
1493:	POP_VMX(STACK_FRAME_LOCAL(4,0),r4)
150	POP_BASIC_STACK(512)
151	blr
152FUNC_END(preempt_vmx)
153