1/*
2 * Copyright 2013, winocm. <winocm@icloud.com>
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without modification,
6 * are permitted provided that the following conditions are met:
7 *
8 *   Redistributions of source code must retain the above copyright notice, this
9 *   list of conditions and the following disclaimer.
10 *
11 *   Redistributions in binary form must reproduce the above copyright notice, this
12 *   list of conditions and the following disclaimer in the documentation and/or
13 *   other materials provided with the distribution.
14 *
15 *   If you are going to use this software in any form that does not involve
16 *   releasing the source to this project or improving it, let me know beforehand.
17 *
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
22 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
25 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
27 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 */
29/*
30 * VFP initialization
31 */
32
33#include <arm/arch.h>
34#include <arm/asm_help.h>
35
36#define VFP_ENABLE   (1 << 30)
37/**
38 * init_vfp
39 *
40 * Enable the use of VFP/NEON. This just enables cp10/cp11.
41 */
42EnterARM(init_vfp)
43    mrc     p15, 0, r0, c1, c0, 2
44    mov     r1, #0xf00000
45    orr     r0, r0, r1
46    mcr     p15, 0, r0, c1, c0, 2
47    bx      lr
48
49/**
50 * vfp_context_save
51 *
52 * Save the current VFP state.
53 */
54EnterARM(vfp_context_save)
55    /* Set enable state so things don't move */
56    vmrs    r2, fpexc
57    mov     r1, #VFP_ENABLE
58    vmsr    fpexc, r1
59
60    /* Store registers */
61    vstmia  r0, {d0-d15}
62    add     r0, r0, #0x80
63    vstmia  r0, {d16-d31}
64    add     r0, r0, #0x80
65
66    /* Restore state */
67    vmrs    r1, fpscr
68    str     r1, [r0]
69    vmsr    fpexc, r2
70    bx      lr
71
72/**
73 * vfp_context_load
74 *
75 * Load the saved VFP state into the current registers.
76 */
77EnterARM(vfp_context_load)
78    /* Set enable state so things don't move */
79    vmrs    r2, fpexc
80    mov     r1, #VFP_ENABLE
81    vmsr    fpexc, r1
82
83    /* Load registers */
84    vldmia  r0, {d0-d15}
85    add     r0, r0, #0x80
86    vldmia  r0, {d16-d31}
87    add     r0, r0, #0x80
88
89    /* Restore state */
90    ldr     r1, [r0]
91    vmsr    fpscr, r1
92    vmsr    fpexc, r2
93
94    /* end. */
95    bx      lr
96
97/**
98 * vfp_enable_exception
99 *
100 * Enable/disable the exception state based on input boolean
101 * value,
102 */
103EnterARM(vfp_enable_exception)
104    /* Get current state */
105    vmrs    r2, fpexc
106
107    /* Check the value */
108    cmp     r0, #0
109    mov     r1, #0
110
111    /* It's one, set the enable state */
112    orrne   r1, r1, #VFP_ENABLE
113
114    /* Set the value in FPEXC */
115    vmsr    fpexc, r1
116
117    /* Return the original state. */
118    mov     r0, r2
119    bx      lr
120