Add support for missing libm functions (#109)

overdrivenpotato · Jun 12, 2022 · de9ee9d · de9ee9d
1 parent f446065
commit de9ee9d
Show file tree

Hide file tree

Showing 8 changed files with 279 additions and 49 deletions.
diff --git a/ci/tests/src/math_test.rs b/ci/tests/src/math_test.rs
@@ -1,14 +1,43 @@
 use psp::math;
+use core::f32::{EPSILON, NAN, consts::PI};
 use psp::test_runner::TestRunner;
 
+
 pub fn test_main(test_runner: &mut TestRunner) {
     test_runner.check_list(&[
         ("cos_2.5", test_cos(2.5), -0.8011436),
         ("cos_0", test_cos(0.0), 1.0),
-        ("cos_pi", test_cos(psp::sys::GU_PI), -1.0),
+        ("cos_pi", test_cos(PI), -1.0),
+        ("sin_0", test_sin(0.0), 0.0),
+        ("sin_2.5", test_sin(2.5), 0.5984721),
+        ("fmodf", test_fmodf(-10.0, 3.0), -1.0),
+        ("fminf", test_fminf(-10.0, 3.0), -10.0),
+        ("fminf_NAN", test_fminf(-10.0, NAN), -10.0),
+        ("fmaxf", test_fmaxf(-10.0, 3.0), 3.0),
+        ("fmaxf_NAN", test_fmaxf(NAN, 3.0), 3.0),
     ]);
+    let almost_zero = test_sin(PI);
+    test_runner.check_true("sin_pi", almost_zero < EPSILON && almost_zero > -EPSILON);
+    let fmodf_0 = test_fmodf(1.0, 0.0);
+    test_runner.check_true("fmodf_0", fmodf_0.is_nan());
 }
 
 fn test_cos(num: f32) -> f32 {
-    unsafe { math::cosf32(num) }
+    unsafe { math::cosf(num) }
+}
+
+fn test_sin(num: f32) -> f32 {
+    unsafe { math::sinf(num) }
+}
+
+fn test_fminf(x: f32, y: f32) -> f32 {
+    unsafe { math::fminf(x, y) }
+}
+
+fn test_fmaxf(x: f32, y: f32) -> f32 {
+    unsafe { math::fmaxf(x, y) }
+}
+
+fn test_fmodf(x: f32, y: f32) -> f32 {
+    math::fmodf(x, y)
 }
diff --git a/examples/vfpu-addition/src/main.rs b/examples/vfpu-addition/src/main.rs
@@ -50,13 +50,6 @@ fn vfpu_add(a: i32, b: i32) -> i32 {
 
 fn psp_main() {
     psp::enable_home_button();
-
-    // Enable the VFPU
-    unsafe {
-        use psp::sys::{self, ThreadAttributes};
-        sys::sceKernelChangeCurrentThreadAttr(0, ThreadAttributes::VFPU);
-    }
-
     psp::dprintln!("Testing VFPU...");
     psp::dprintln!("VFPU 123 + 4 = {}", vfpu_add(123, 4));
 }
diff --git a/psp/Cargo.toml b/psp/Cargo.toml
@@ -25,6 +25,7 @@ stub-only = []
 [dependencies]
 paste = "1.0.1"
 bitflags = "1.2.1"
+libm = "0.2.1"
 embedded-graphics = { version = "0.7.1", optional = true, features = ["fixed_point"] }
 unstringify = "0.1.4"
 

diff --git a/psp/src/lib.rs b/psp/src/lib.rs
@@ -4,9 +4,9 @@
     alloc_error_handler,
     global_asm,
     untagged_unions,
-    core_intrinsics,
     const_loop,
     const_if_match,
+    core_intrinsics,
     c_variadic,
     lang_items
 )]
@@ -217,7 +217,7 @@ macro_rules! module {
                         32,
                         // 256kb stack
                         256 * 1024,
-                        ThreadAttributes::USER,
+                        ThreadAttributes::USER | ThreadAttributes::VFPU,
                         core::ptr::null_mut(),
                     );
 

diff --git a/psp/src/math/mod.rs b/psp/src/math/mod.rs
@@ -1,2 +1,232 @@
-mod trig;
-pub use trig::*;
+#[no_mangle]
+pub unsafe extern "C" fn fminf(
+    x: f32,
+    y: f32,
+) -> f32 {
+    let out: f32;
+    if x.is_nan() && !y.is_nan() {
+        out = y;
+    }
+    else if y.is_nan() && !x.is_nan() {
+        out = x;
+    }
+    else if x.is_nan() && y.is_nan() {
+        out = core::f32::NAN;
+    }
+    else {
+        vfpu_asm! (
+            "mfc1 {tmp1}, {x}",
+            "mfc1 {tmp2}, {y}",
+            "mtv {tmp1}, S000",
+            "mtv {tmp2}, S001",
+            "vmin.s S000, S000, S001",
+            "mfv {tmp1}, S000",
+            "mtc1 {tmp1}, {out}",
+            "nop",
+            x = in(freg) x,
+            y = in(freg) y,
+            tmp1 = out(reg) _,
+            tmp2 = out(reg) _,
+            out = out(freg) out,
+            options(nostack, nomem),
+        );
+    }
+    out
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn fmaxf(
+    x: f32,
+    y: f32,
+) -> f32 {
+    let out: f32;
+    if x.is_nan() && !y.is_nan() {
+        out = y;
+    }
+    else if y.is_nan() && !x.is_nan() {
+        out = x;
+    }
+    else if x.is_nan() && y.is_nan() {
+        out = core::f32::NAN;
+    }
+    else {
+        vfpu_asm! (
+            "mfc1 {tmp1}, {x}",
+            "mfc1 {tmp2}, {y}",
+            "mtv {tmp1}, S000",
+            "mtv {tmp2}, S001",
+            "vmax.s S000, S000, S001",
+            "mfv {tmp1}, S000",
+            "mtc1 {tmp1}, {out}",
+            "nop",
+            x = in(freg) x,
+            y = in(freg) y,
+            tmp1 = out(reg) _,
+            tmp2 = out(reg) _,
+            out = out(freg) out,
+            options(nostack, nomem),
+        );
+    }
+    out
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn cosf(
+    scalar: f32
+) -> f32 {
+    let out: f32;
+    vfpu_asm! (
+        "mfc1 {tmp}, {scalar}",
+        "mtv {tmp}, S000",
+        "nop",
+        "vcst.s S001, VFPU_2_PI",
+        "vmul.s S000, S000, S001",
+        "vcos.s S000, S000",
+        "mfv {tmp}, S000",
+        "mtc1 {tmp}, {scalar}",
+        "nop",
+        scalar = inlateout(freg) scalar => out,
+        tmp = out(reg) _,
+        options(nostack, nomem),
+    );
+    out
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn sinf(
+    scalar: f32
+) -> f32 {
+    let out: f32;
+    vfpu_asm! (
+        "mfc1 {tmp}, {scalar}",
+        "mtv {tmp}, S000",
+        "nop",
+        "vcst.s S001, VFPU_2_PI",
+        "vmul.s S000, S000, S001",
+        "vsin.s S000, S000",
+        "mfv {tmp}, S000",
+        "mtc1 {tmp}, {scalar}",
+        "nop",
+        scalar = inlateout(freg) scalar => out,
+        tmp = out(reg) _,
+        options(nostack, nomem),
+    );
+    out
+}
+// borrowed from https://github.com/samcrow/cmsis_dsp.rs/blob/master/src/libm_c.rs
+macro_rules! forward {
+    // One argument, argument and return types are the same
+    { $( $name:ident($value_type:ty) ,)+ } => {
+        $(
+            #[no_mangle]
+            pub extern "C" fn $name(value: $value_type) -> $value_type {
+                libm::$name(value)
+            }
+        )+
+    };
+    // Two arguments, argument and return types may be different
+    { $( $name:ident($arg1_type:ty, $arg2_type:ty) -> $return_type:ty ,)+ }
+    => {
+        $(
+            #[no_mangle]
+            pub extern "C" fn $name(arg1: $arg1_type, arg2: $arg2_type) -> $return_type {
+                libm::$name(arg1, arg2)
+            }
+        )+
+    };
+    // Three arguments, argument and return types may be different
+    { $( $name:ident($arg1_type:ty, $arg2_type:ty, $arg3_type:ty) -> $return_type:ty ,)+ }
+    => {
+        $(
+            #[no_mangle]
+            pub extern "C" fn $name(arg1: $arg1_type, arg2: $arg2_type, arg3: $arg3_type) -> $return_type {
+                libm::$name(arg1, arg2, arg3)
+            }
+        )+
+    };
+}
+
+// One-argument functions
+forward! {
+    fabsf(f32),
+    fabs(f64),
+    expf(f32),
+    exp(f64),
+    exp2(f64),
+    exp2f(f32),
+    expm1(f64),
+    expm1f(f32),
+    log(f64),
+    logf(f32),
+    log10(f64),
+    log10f(f32),
+    log2(f64),
+    log2f(f32),
+    log1p(f64),
+    log1pf(f32),
+    sqrtf(f32),
+    sqrt(f64),
+    cbrtf(f32),
+    cbrt(f64),
+    sin(f64),
+    cos(f64),
+    tan(f64),
+    tanf(f32),
+    asin(f64),
+    asinf(f32),
+    acos(f64),
+    acosf(f32),
+    atan(f64),
+    atanf(f32),
+    sinh(f64),
+    sinhf(f32),
+    cosh(f64),
+    coshf(f32),
+    tanh(f64),
+    tanhf(f32),
+    asinh(f64),
+    asinhf(f32),
+    acosh(f64),
+    acoshf(f32),
+    atanh(f64),
+    atanhf(f32),
+    erf(f64),
+    erff(f32),
+    erfc(f64),
+    erfcf(f32),
+    tgamma(f64),
+    tgammaf(f32),
+    lgamma(f64),
+    lgammaf(f32),
+    ceil(f64),
+    ceilf(f32),
+    floor(f64),
+    floorf(f32),
+    trunc(f64),
+    round(f64),
+    roundf(f32),
+}
+
+// Two-argument functions
+forward! {
+    fmod(f64, f64) -> f64,
+    fmodf(f32, f32) -> f32,
+    remainder(f64, f64) -> f64,
+    remainderf(f32, f32) -> f32,
+    fmax(f64, f64) -> f64,
+    fmin(f64, f64) -> f64,
+    fdim(f64, f64) -> f64,
+    fdimf(f32, f32) -> f32,
+    pow(f64, f64) -> f64,
+    powf(f32, f32) -> f32,
+    hypot(f64, f64) -> f64,
+    hypotf(f32, f32) -> f32,
+    atan2(f64, f64) -> f64,
+    atan2f(f32, f32) -> f32,
+}
+
+// Three-argument functions
+forward! {
+    fma(f64, f64, f64) -> f64,
+    fmaf(f32, f32, f32) -> f32,
+}
diff --git a/psp/src/math/trig.rs b/psp/src/math/trig.rs
diff --git a/psp/src/sys/vfpu_context.rs b/psp/src/sys/vfpu_context.rs
@@ -27,14 +27,8 @@ pub struct Context {
 }
 
 impl Context {
+    /// Threads which call this must have ThreadAttributes::VFPU set
     pub fn new() -> Self {
-        unsafe {
-            use crate::sys::{self, ThreadAttributes};
-
-            // TODO: Handle errors.
-            sys::sceKernelChangeCurrentThreadAttr(0, ThreadAttributes::VFPU);
-        }
-
         let zero_vector = ScePspFVector4 { x: 0.0, y: 0.0, z: 0.0, w: 0.0 };
         let zero_matrix = ScePspFMatrix4 {
             x: zero_vector,