From f9004dbfff8a3fbbd7e2a88c8a4327c7f2f8e5b2 Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Wed, 31 Jan 2024 01:04:47 +0000
Subject: [PATCH] add driver 5G
---
kernel/arch/arm/vfp/vfpmodule.c | 88 +++++++++++++++++++++++++++++++++++---------
1 files changed, 70 insertions(+), 18 deletions(-)
diff --git a/kernel/arch/arm/vfp/vfpmodule.c b/kernel/arch/arm/vfp/vfpmodule.c
index 66c5e69..2cb355c 100644
--- a/kernel/arch/arm/vfp/vfpmodule.c
+++ b/kernel/arch/arm/vfp/vfpmodule.c
@@ -1,12 +1,9 @@
+// SPDX-License-Identifier: GPL-2.0-only
/*
* linux/arch/arm/vfp/vfpmodule.c
*
* Copyright (C) 2004 ARM Limited.
* Written by Deep Blue Solutions Limited.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
*/
#include <linux/types.h>
#include <linux/cpu.h>
@@ -26,6 +23,7 @@
#include <asm/cputype.h>
#include <asm/system_info.h>
#include <asm/thread_notify.h>
+#include <asm/traps.h>
#include <asm/vfp.h>
#include "vfpinstr.h"
@@ -34,7 +32,6 @@
/*
* Our undef handlers (in entry.S)
*/
-asmlinkage void vfp_testing_entry(void);
asmlinkage void vfp_support_entry(void);
asmlinkage void vfp_null_entry(void);
@@ -45,7 +42,7 @@
* Used in startup: set to non-zero if VFP checks fail
* After startup, holds VFP architecture
*/
-unsigned int VFP_arch;
+static unsigned int __initdata VFP_arch;
/*
* The pointer to the vfpstate structure of the thread which currently
@@ -216,13 +213,6 @@
*/
static void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
{
- siginfo_t info;
-
- clear_siginfo(&info);
- info.si_signo = SIGFPE;
- info.si_code = sicode;
- info.si_addr = (void __user *)(instruction_pointer(regs) - 4);
-
/*
* This is the same as NWFPE, because it's not clear what
* this is used for
@@ -230,7 +220,9 @@
current->thread.error_code = 0;
current->thread.trap_no = 6;
- send_sig_info(SIGFPE, &info, current);
+ send_sig_fault(SIGFPE, sicode,
+ (void __user *)(instruction_pointer(regs) - 4),
+ current);
}
static void vfp_panic(char *reason, u32 inst)
@@ -444,7 +436,7 @@
* present on all CPUs within a SMP complex. Needs to be called prior to
* vfp_init().
*/
-void vfp_disable(void)
+void __init vfp_disable(void)
{
if (VFP_arch) {
pr_debug("%s: should be called prior to vfp_init\n", __func__);
@@ -650,7 +642,9 @@
return 0;
}
-void vfp_kmode_exception(void)
+#ifdef CONFIG_KERNEL_MODE_NEON
+
+static int vfp_kmode_exception(struct pt_regs *regs, unsigned int instr)
{
/*
* If we reach this point, a floating point exception has been raised
@@ -668,9 +662,51 @@
pr_crit("BUG: unsupported FP instruction in kernel mode\n");
else
pr_crit("BUG: FP instruction issued in kernel mode with FP unit disabled\n");
+ pr_crit("FPEXC == 0x%08x\n", fmrx(FPEXC));
+ return 1;
}
-#ifdef CONFIG_KERNEL_MODE_NEON
+static struct undef_hook vfp_kmode_exception_hook[] = {{
+ .instr_mask = 0xfe000000,
+ .instr_val = 0xf2000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0xff100000,
+ .instr_val = 0xf4000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0xef000000,
+ .instr_val = 0xef000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE | PSR_T_BIT,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0xff100000,
+ .instr_val = 0xf9000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE | PSR_T_BIT,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0x0c000e00,
+ .instr_val = 0x0c000a00,
+ .cpsr_mask = MODE_MASK,
+ .cpsr_val = SVC_MODE,
+ .fn = vfp_kmode_exception,
+}};
+
+static int __init vfp_kmode_exception_hook_init(void)
+{
+ int i;
+
+ for (i = 0; i < ARRAY_SIZE(vfp_kmode_exception_hook); i++)
+ register_undef_hook(&vfp_kmode_exception_hook[i]);
+ return 0;
+}
+subsys_initcall(vfp_kmode_exception_hook_init);
/*
* Kernel-side NEON support functions
@@ -716,6 +752,21 @@
#endif /* CONFIG_KERNEL_MODE_NEON */
+static int __init vfp_detect(struct pt_regs *regs, unsigned int instr)
+{
+ VFP_arch = UINT_MAX; /* mark as not present */
+ regs->ARM_pc += 4;
+ return 0;
+}
+
+static struct undef_hook vfp_detect_hook __initdata = {
+ .instr_mask = 0x0c000e00,
+ .instr_val = 0x0c000a00,
+ .cpsr_mask = MODE_MASK,
+ .cpsr_val = SVC_MODE,
+ .fn = vfp_detect,
+};
+
/*
* VFP support code initialisation.
*/
@@ -736,10 +787,11 @@
* The handler is already setup to just log calls, so
* we just need to read the VFPSID register.
*/
- vfp_vector = vfp_testing_entry;
+ register_undef_hook(&vfp_detect_hook);
barrier();
vfpsid = fmrx(FPSID);
barrier();
+ unregister_undef_hook(&vfp_detect_hook);
vfp_vector = vfp_null_entry;
pr_info("VFP support v0.3: ");
--
Gitblit v1.6.2