nds32: Add fabsf and sqrtf function

The magnetometer online calibration requires these two
functions and taken from newlib.

Signed-off-by: Dino Li <dino.li@ite.com.tw>

BRANCH=none
BUG=none
TEST=fabsf():
fabsf(1.23) = 1.23
fabsf(-1.23) = 1.23

sqrtf():
sqrtf(4.56) = 2.135
sqrtf(0.123) = 0.350
sqrtf(-0.123) = an exception is triggered.

Change-Id: I808ca7f1bd03c6d6c1b32861ede4ecbfeeaa3da6
Reviewed-on: https://chromium-review.googlesource.com/429730
Commit-Ready: Dino Li <Dino.Li@ite.com.tw>
Tested-by: Dino Li <Dino.Li@ite.com.tw>
Reviewed-by: Randall Spangler <rspangler@chromium.org>

3 files changed, 131 insertions, 1 deletions

diff --git a/core/nds32/build.mk b/core/nds32/build.mk
index d35f7d455e..52009a6eff 100644
--- a/core/nds32/build.mk
+++ b/core/nds32/build.mk
@@ -12,4 +12,4 @@ CROSS_COMPILE?=nds32le-cros-elf-
 # CPU specific compilation flags
 CFLAGS_CPU+=-march=v3m -Os
 
-core-y=cpu.o init.o panic.o task.o switch.o __muldi3.o
+core-y=cpu.o init.o panic.o task.o switch.o __muldi3.o math.o
diff --git a/core/nds32/include/math.h b/core/nds32/include/math.h
new file mode 100644
index 0000000000..49d5585657
--- /dev/null
+++ b/core/nds32/include/math.h
@@ -0,0 +1,14 @@
+/* Copyright 2017 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Math utility functions for N8 */
+
+#ifndef __CROS_EC_MATH_H
+#define __CROS_EC_MATH_H
+
+float sqrtf(float x);
+float fabsf(float x);
+
+#endif  /* __CROS_EC_MATH_H */
diff --git a/core/nds32/math.c b/core/nds32/math.c
new file mode 100644
index 0000000000..496fcc0e5d
--- /dev/null
+++ b/core/nds32/math.c
@@ -0,0 +1,116 @@
+/* Copyright 2017 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "common.h"
+
+#ifdef CONFIG_FPU
+union ieee_float_shape_type {
+	float value;
+	uint32_t word;
+};
+
+/* Get a 32 bit int from a float.  */
+#define GET_FLOAT_WORD(i, d) \
+	do { \
+		union ieee_float_shape_type gf_u; \
+		gf_u.value = (d); \
+		(i) = gf_u.word; \
+	} while (0)
+
+/* Set a float from a 32 bit int. */
+#define SET_FLOAT_WORD(d, i) \
+	do { \
+		union ieee_float_shape_type sf_u; \
+		sf_u.word = (i); \
+		(d) = sf_u.value; \
+	} while (0)
+
+float fabsf(float x)
+{
+	uint32_t ix;
+
+	GET_FLOAT_WORD(ix, x);
+	SET_FLOAT_WORD(x, (ix & 0x7fffffff));
+
+	return x;
+}
+
+#define FLT_UWORD_IS_ZERO(x) ((x) == 0)
+#define FLT_UWORD_IS_SUBNORMAL(x) ((x) < 0x00800000L)
+#define FLT_UWORD_IS_FINITE(x) ((x) < 0x7f800000L)
+
+static const float one = 1.0f, tiny = 1.0e-30f;
+static float __ieee754_sqrtf(float x)
+{
+	float z;
+	uint32_t r, hx;
+	int32_t ix, s, q, m, t, i;
+
+	GET_FLOAT_WORD(ix, x);
+	hx = ix & 0x7fffffff;
+
+	/*
+	 * take care of Inf and NaN
+	 * sqrt(NaN)=NaN, sqrt(+inf)=+inf, sqrt(-inf)=sNaN
+	 */
+	if (!FLT_UWORD_IS_FINITE(hx))
+		return x * x + x;
+	/* take care of zero and -ves */
+	if (FLT_UWORD_IS_ZERO(hx))
+		return x;
+	if (ix < 0)
+		return (x - x) / (x - x);
+
+	m = (ix >> 23);
+	if (FLT_UWORD_IS_SUBNORMAL(hx)) {
+		for (i = 0; (ix & 0x00800000L) == 0; i++)
+			ix <<= 1;
+		m -= i - 1;
+	}
+
+	m -= 127;
+	ix = (ix & 0x007fffffL) | 0x00800000L;
+	if (m & 1)
+		ix += ix;
+
+	m >>= 1;
+	ix += ix;
+	q = s = 0;
+	r = 0x01000000L;
+
+	while (r != 0) {
+		t = s + r;
+		if (t <= ix) {
+			s = t + r;
+			ix -= t;
+			q += r;
+		}
+		ix += ix;
+		r >>= 1;
+	}
+
+	if (ix != 0) {
+		z = one - tiny;
+		if (z >= one) {
+			z = one + tiny;
+			if (z > one)
+				q += 2;
+			else
+				q += (q & 1);
+		}
+	}
+
+	ix = (q >> 1) + 0x3f000000L;
+	ix += (m << 23);
+	SET_FLOAT_WORD(z, ix);
+
+	return z;
+}
+
+float sqrtf(float x)
+{
+	return __ieee754_sqrtf(x);
+}
+#endif