1 #include <xen/sched.h>
2 #include <xen/init.h>
3 #include <asm/processor.h>
4 #include <asm/vfp.h>
5
vfp_save_state(struct vcpu * v)6 void vfp_save_state(struct vcpu *v)
7 {
8 v->arch.vfp.fpexc = READ_CP32(FPEXC);
9
10 WRITE_CP32(v->arch.vfp.fpexc | FPEXC_EN, FPEXC);
11
12 v->arch.vfp.fpscr = READ_CP32(FPSCR);
13
14 if ( v->arch.vfp.fpexc & FPEXC_EX ) /* Check for sub-architecture */
15 {
16 v->arch.vfp.fpinst = READ_CP32(FPINST);
17
18 if ( v->arch.vfp.fpexc & FPEXC_FP2V )
19 v->arch.vfp.fpinst2 = READ_CP32(FPINST2);
20 /* Disable FPEXC_EX */
21 WRITE_CP32((v->arch.vfp.fpexc | FPEXC_EN) & ~FPEXC_EX, FPEXC);
22 }
23
24 /* Save {d0-d15} */
25 asm volatile("stc p11, cr0, [%1], #32*4"
26 : "=Q" (*v->arch.vfp.fpregs1) : "r" (v->arch.vfp.fpregs1));
27
28 /* 32 x 64 bits registers? */
29 if ( (READ_CP32(MVFR0) & MVFR0_A_SIMD_MASK) == 2 )
30 {
31 /* Save {d16-d31} */
32 asm volatile("stcl p11, cr0, [%1], #32*4"
33 : "=Q" (*v->arch.vfp.fpregs2) : "r" (v->arch.vfp.fpregs2));
34 }
35
36 WRITE_CP32(v->arch.vfp.fpexc & ~(FPEXC_EN), FPEXC);
37 }
38
vfp_restore_state(struct vcpu * v)39 void vfp_restore_state(struct vcpu *v)
40 {
41 //uint64_t test[16];
42 WRITE_CP32(READ_CP32(FPEXC) | FPEXC_EN, FPEXC);
43
44 /* Restore {d0-d15} */
45 asm volatile("ldc p11, cr0, [%1], #32*4"
46 : : "Q" (*v->arch.vfp.fpregs1), "r" (v->arch.vfp.fpregs1));
47
48 /* 32 x 64 bits registers? */
49 if ( (READ_CP32(MVFR0) & MVFR0_A_SIMD_MASK) == 2 ) /* 32 x 64 bits registers */
50 /* Restore {d16-d31} */
51 asm volatile("ldcl p11, cr0, [%1], #32*4"
52 : : "Q" (*v->arch.vfp.fpregs2), "r" (v->arch.vfp.fpregs2));
53
54 if ( v->arch.vfp.fpexc & FPEXC_EX )
55 {
56 WRITE_CP32(v->arch.vfp.fpinst, FPINST);
57 if ( v->arch.vfp.fpexc & FPEXC_FP2V )
58 WRITE_CP32(v->arch.vfp.fpinst2, FPINST2);
59 }
60
61 WRITE_CP32(v->arch.vfp.fpscr, FPSCR);
62
63 WRITE_CP32(v->arch.vfp.fpexc, FPEXC);
64 }
65
vfp_init(void)66 static __init int vfp_init(void)
67 {
68 unsigned int vfpsid;
69 unsigned int vfparch;
70
71 vfpsid = READ_CP32(FPSID);
72
73 printk("VFP implementer 0x%02x architecture %d part 0x%02x variant 0x%x "
74 "rev 0x%x\n",
75 (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
76 (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT,
77 (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
78 (vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
79 (vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);
80
81 vfparch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;
82 if ( vfparch < 2 )
83 panic("Xen only support VFP 3");
84
85 return 0;
86 }
87 presmp_initcall(vfp_init);
88
89 /*
90 * Local variables:
91 * mode: C
92 * c-file-style: "BSD"
93 * c-basic-offset: 4
94 * indent-tabs-mode: nil
95 * End:
96 */
97