Add math library support for trunc functions. From Brennan Ashton.

6 files changed, 256 insertions, 33 deletions

diff --git a/apps/interpreters/micropython/micropython_main.c b/apps/interpreters/micropython/micropython_main.c
index 24bb12edf..0d68e815b 100644
--- a/apps/interpreters/micropython/micropython_main.c
+++ b/apps/interpreters/micropython/micropython_main.c
@@ -149,39 +149,6 @@ float nanf(FAR const char *tagp)
   return 0;
 }
 
-float truncf(float x)
-{
-  union
-    {
-      float f;
-      uint32_t i;
-    } u =
-  {
-  x};
-  int e = (int)(u.i >> 23 & 0xff) - 0x7f + 9;
-  uint32_t m;
-
-  if (e >= 23 + 9)
-    {
-      return x;
-    }
-
-  if (e < 9)
-    {
-      e = 1;
-    }
-
-  m = -1U >> e;
-  if ((u.i & m) == 0)
-    {
-      return x;
-    }
-
-  FORCE_EVAL(x + 0x1p120f);
-  u.i &= ~m;
-  return u.f;
-}
-
 /****************************************************************************
  * mp_import_stat
  ****************************************************************************/
diff --git a/nuttx/include/nuttx/math.h b/nuttx/include/nuttx/math.h
index b2ddcc4bb..89223531e 100644
--- a/nuttx/include/nuttx/math.h
+++ b/nuttx/include/nuttx/math.h
@@ -377,6 +377,14 @@ double      copysign  (double x, double y);
 long double copysignl (long double x, long double y);
 #endif
 
+float       truncf (float x);
+#if CONFIG_HAVE_DOUBLE
+double      trunc (double x);
+#endif
+#ifdef CONFIG_HAVE_LONG_DOUBLE
+long double truncl (long double x);
+#endif
+
 #if defined(__cplusplus)
 }
 #endif
diff --git a/nuttx/libc/math/Make.defs b/nuttx/libc/math/Make.defs
index 5f386f0ac..cf25a1ec7 100644
--- a/nuttx/libc/math/Make.defs
+++ b/nuttx/libc/math/Make.defs
@@ -42,18 +42,21 @@ CSRCS += lib_coshf.c  lib_expf.c lib_fabsf.c lib_floorf.c lib_fmodf.c lib_frexpf
 CSRCS += lib_ldexpf.c lib_logf.c lib_log10f.c lib_log2f.c lib_modff.c lib_powf.c
 CSRCS += lib_rintf.c lib_roundf.c lib_sinf.c lib_sinhf.c lib_sqrtf.c lib_tanf.c
 CSRCS += lib_tanhf.c lib_asinhf.c lib_acoshf.c lib_atanhf.c lib_erff.c lib_copysignf.c
+CSRCS += lib_truncf.c
 
 CSRCS += lib_acos.c lib_asin.c lib_atan.c lib_atan2.c lib_ceil.c lib_cos.c
 CSRCS += lib_cosh.c lib_exp.c lib_fabs.c lib_floor.c lib_fmod.c lib_frexp.c
 CSRCS += lib_ldexp.c lib_log.c lib_log10.c lib_log2.c lib_modf.c lib_pow.c
 CSRCS += lib_rint.c lib_round.c lib_sin.c lib_sinh.c lib_sqrt.c lib_tan.c
 CSRCS += lib_tanh.c lib_asinh.c lib_acosh.c lib_atanh.c lib_erf.c lib_copysign.c
+CSRCS += lib_trunc.c
 
 CSRCS += lib_acosl.c lib_asinl.c lib_atan2l.c lib_atanl.c lib_ceill.c lib_cosl.c
 CSRCS += lib_coshl.c lib_expl.c lib_fabsl.c lib_floorl.c lib_fmodl.c lib_frexpl.c
 CSRCS += lib_ldexpl.c lib_logl.c lib_log10l.c lib_log2l.c lib_modfl.c lib_powl.c
 CSRCS += lib_rintl.c lib_roundl.c lib_sinl.c lib_sinhl.c lib_sqrtl.c lib_tanl.c
 CSRCS += lib_tanhl.c lib_asinhl.c lib_acoshl.c lib_atanhl.c lib_erfl.c lib_copysignl.c
+CSRCS += lib_truncl.c
 
 CSRCS += lib_libexpi.c lib_libsqrtapprox.c
 
diff --git a/nuttx/libc/math/lib_trunc.c b/nuttx/libc/math/lib_trunc.c
new file mode 100644
index 000000000..1de0f4b52
--- /dev/null
+++ b/nuttx/libc/math/lib_trunc.c
@@ -0,0 +1,75 @@
+/****************************************************************************
+ * libc/math/lib_trunc.c
+ * 
+ * This implementation is derived from the musl library under the MIT License
+ *
+ * Copyright © 2005-2014 Rich Felker, et al.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ ****************************************************************************/
+
+/************************************************************************
+ * Included Files
+ ************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+
+#include <stdint.h>
+#include <math.h>
+
+/************************************************************************
+ * Public Functions
+ ************************************************************************/
+
+#ifdef CONFIG_HAVE_DOUBLE
+double trunc(double x)
+{
+  union {double f; uint64_t i;} u = {x};
+  int e = (int)(u.i >> 52 & 0x7ff) - 0x3ff + 12;
+  uint64_t m;
+  volatile float __x;
+
+  if (e >= 52 + 12)
+    {
+      return x;
+    }
+
+  if (e < 12)
+    {
+      e = 1;
+    }
+
+  m = -1ull >> e;
+  if ((u.i & m) == 0)
+    {
+      return x;
+    }
+
+  /* Force Evaluation */
+
+  __x = (x + 0x1p120f);
+  (void)__x;
+
+  u.i &= ~m;
+  return u.f;
+}
+#endif
diff --git a/nuttx/libc/math/lib_truncf.c b/nuttx/libc/math/lib_truncf.c
new file mode 100644
index 000000000..6093de733
--- /dev/null
+++ b/nuttx/libc/math/lib_truncf.c
@@ -0,0 +1,73 @@
+/****************************************************************************
+ * libc/math/lib_truncf.c
+ * 
+ * This implementation is derived from the musl library under the MIT License
+ *
+ * Copyright © 2005-2014 Rich Felker, et al.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ ****************************************************************************/
+
+/************************************************************************
+ * Included Files
+ ************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+
+#include <stdint.h>
+#include <math.h>
+
+/************************************************************************
+ * Public Functions
+ ************************************************************************/
+
+float truncf(float x)
+{
+  union {float f; uint32_t i;} u = {x};
+  int e = (int)(u.i >> 23 & 0xff) - 0x7f + 9;
+  uint32_t m;
+  volatile float __x;
+
+  if (e >= 23 + 9)
+    {
+      return x;
+    }
+
+  if (e < 9)
+    {
+      e = 1;
+    }
+
+  m = -1u >> e;
+  if ((u.i & m) == 0)
+    {
+      return x;
+    }
+
+  /* Force Eval */
+
+  __x = (x + 0x1p120f);
+  (void)__x;
+
+  u.i &= ~m;
+  return u.f;
+}
diff --git a/nuttx/libc/math/lib_truncl.c b/nuttx/libc/math/lib_truncl.c
new file mode 100644
index 000000000..29f940206
--- /dev/null
+++ b/nuttx/libc/math/lib_truncl.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ * libc/math/lib_truncl.c
+ * 
+ * This implementation is derived from the musl library under the MIT License
+ *
+ * Copyright © 2005-2014 Rich Felker, et al.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ ****************************************************************************/
+
+/************************************************************************
+ * Included Files
+ ************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+
+#include <stdint.h>
+#include <float.h>
+#include <math.h>
+
+/************************************************************************
+ * Public Functions
+ ************************************************************************/
+
+#ifdef CONFIG_HAVE_LONG_DOUBLE
+static const long double toint = 1 / LDBL_EPSILON;
+
+/* FIXME This will only work if long double is 64 bit and little endian */
+
+union ldshape
+{
+  long double f;
+  struct
+    {
+      uint64_t m;
+      uint16_t se;
+    } i;
+};
+
+long double truncl(long double x)
+{
+  union ldshape u = {x};
+  int e = u.i.se & 0x7fff;
+  int s = u.i.se >> 15;
+  long double y;
+  volatile long double __x;
+
+  if (e >= 0x3fff + LDBL_MANT_DIG - 1)
+    {
+      return x;
+    }
+
+  if (e <= 0x3fff - 1)
+    {
+      /* Force Eval */
+
+      __x = (x + 0x1p120f);
+      (void)__x;
+      return x*0;
+    }
+
+  /* y = int(|x|) - |x|, where int(|x|) is an integer neighbor of |x| */
+
+  if (s)
+    {
+      x = -x;
+    }
+
+  y = x + toint - toint - x;
+  if (y > 0)
+    {
+      y -= 1;
+    }
+
+  x += y;
+  return s ? -x : x;
+}
+#endif