1// Copyright 2016 The Fuchsia Authors 2// Copyright (c) 2015 Google Inc. All rights reserved 3// 4// Use of this source code is governed by a MIT-style 5// license that can be found in the LICENSE file or at 6// https://opensource.org/licenses/MIT 7 8#include <arch/arm64.h> 9#include <assert.h> 10#include <bits.h> 11#include <kernel/thread.h> 12#include <trace.h> 13 14#define LOCAL_TRACE 0 15 16/* FPEN bits in the cpacr register 17 * 0 means all fpu instructions fault 18 * 3 means no faulting at all EL levels 19 * other values are not useful to us 20 */ 21#define FPU_ENABLE_MASK (3 << 20) 22 23static inline bool is_fpu_enabled(uint32_t cpacr) { 24 return !!(BITS(cpacr, 21, 20) != 0); 25} 26 27static void arm64_fpu_load_state(struct thread* t) { 28 struct fpstate* fpstate = &t->arch.fpstate; 29 30 LTRACEF("cpu %u, thread %s, load fpstate %p\n", arch_curr_cpu_num(), t->name, fpstate); 31 32 static_assert(sizeof(fpstate->regs) == 16 * 32, ""); 33 __asm__ volatile("ldp q0, q1, [%0, #(0 * 32)]\n" 34 "ldp q2, q3, [%0, #(1 * 32)]\n" 35 "ldp q4, q5, [%0, #(2 * 32)]\n" 36 "ldp q6, q7, [%0, #(3 * 32)]\n" 37 "ldp q8, q9, [%0, #(4 * 32)]\n" 38 "ldp q10, q11, [%0, #(5 * 32)]\n" 39 "ldp q12, q13, [%0, #(6 * 32)]\n" 40 "ldp q14, q15, [%0, #(7 * 32)]\n" 41 "ldp q16, q17, [%0, #(8 * 32)]\n" 42 "ldp q18, q19, [%0, #(9 * 32)]\n" 43 "ldp q20, q21, [%0, #(10 * 32)]\n" 44 "ldp q22, q23, [%0, #(11 * 32)]\n" 45 "ldp q24, q25, [%0, #(12 * 32)]\n" 46 "ldp q26, q27, [%0, #(13 * 32)]\n" 47 "ldp q28, q29, [%0, #(14 * 32)]\n" 48 "ldp q30, q31, [%0, #(15 * 32)]\n" 49 "msr fpcr, %1\n" 50 "msr fpsr, %2\n" ::"r"(fpstate->regs), 51 "r"((uint64_t)fpstate->fpcr), 52 "r"((uint64_t)fpstate->fpsr)); 53} 54 55__NO_SAFESTACK static void arm64_fpu_save_state(struct thread* t) { 56 struct fpstate* fpstate = &t->arch.fpstate; 57 58 LTRACEF("cpu %u, thread %s, save fpstate %p\n", arch_curr_cpu_num(), t->name, fpstate); 59 60 __asm__ volatile("stp q0, q1, [%0, #(0 * 32)]\n" 61 "stp q2, q3, [%0, #(1 * 32)]\n" 62 "stp q4, q5, [%0, #(2 * 32)]\n" 63 "stp q6, q7, [%0, #(3 * 32)]\n" 64 "stp q8, q9, [%0, #(4 * 32)]\n" 65 "stp q10, q11, [%0, #(5 * 32)]\n" 66 "stp q12, q13, [%0, #(6 * 32)]\n" 67 "stp q14, q15, [%0, #(7 * 32)]\n" 68 "stp q16, q17, [%0, #(8 * 32)]\n" 69 "stp q18, q19, [%0, #(9 * 32)]\n" 70 "stp q20, q21, [%0, #(10 * 32)]\n" 71 "stp q22, q23, [%0, #(11 * 32)]\n" 72 "stp q24, q25, [%0, #(12 * 32)]\n" 73 "stp q26, q27, [%0, #(13 * 32)]\n" 74 "stp q28, q29, [%0, #(14 * 32)]\n" 75 "stp q30, q31, [%0, #(15 * 32)]\n" ::"r"(fpstate->regs)); 76 77 // These are 32-bit values, but the msr instruction always uses a 78 // 64-bit destination register. 79 uint64_t fpcr, fpsr; 80 __asm__("mrs %0, fpcr\n" 81 : "=r"(fpcr)); 82 __asm__("mrs %0, fpsr\n" 83 : "=r"(fpsr)); 84 fpstate->fpcr = (uint32_t)fpcr; 85 fpstate->fpsr = (uint32_t)fpsr; 86 87 LTRACEF("thread %s, fpcr %x, fpsr %x\n", t->name, fpstate->fpcr, fpstate->fpsr); 88} 89 90/* save fpu state if the thread had dirtied it and disable the fpu */ 91__NO_SAFESTACK void arm64_fpu_context_switch(struct thread* oldthread, 92 struct thread* newthread) { 93 uint64_t cpacr = ARM64_READ_SYSREG(cpacr_el1); 94 if (is_fpu_enabled((uint32_t)cpacr)) { 95 LTRACEF("saving state on thread %s\n", oldthread->name); 96 97 /* save the state */ 98 arm64_fpu_save_state(oldthread); 99 100 /* disable the fpu again */ 101 ARM64_WRITE_SYSREG(cpacr_el1, cpacr & ~FPU_ENABLE_MASK); 102 } 103} 104 105/* called because of a fpu instruction used exception */ 106void arm64_fpu_exception(struct arm64_iframe_long* iframe, uint exception_flags) { 107 LTRACEF("cpu %u, thread %s, flags 0x%x\n", arch_curr_cpu_num(), get_current_thread()->name, exception_flags); 108 109 /* only valid to be called if exception came from lower level */ 110 DEBUG_ASSERT(exception_flags & ARM64_EXCEPTION_FLAG_LOWER_EL); 111 112 uint64_t cpacr = ARM64_READ_SYSREG(cpacr_el1); 113 DEBUG_ASSERT(!is_fpu_enabled((uint32_t)cpacr)); 114 115 /* enable the fpu */ 116 cpacr |= FPU_ENABLE_MASK; 117 ARM64_WRITE_SYSREG(cpacr_el1, cpacr); 118 119 /* load the state from the current cpu */ 120 thread_t* t = get_current_thread(); 121 if (likely(t)) 122 arm64_fpu_load_state(t); 123} 124