3 files changed, 75 insertions, 20 deletions
diff --git a/include/lid_angle.h b/include/lid_angle.h
index cd13265c93..e45a5683a4 100644
--- a/include/lid_angle.h
+++ b/include/lid_angle.h
@@ -15,7 +15,7 @@
  *
  * @lid_ang Lid angle.
  */
-void lidangle_keyscan_update(float lid_ang);
+void lidangle_keyscan_update(int lid_ang);
 
 /**
  * Getter and setter methods for the keyboard wake angle. In S3, when the
diff --git a/include/math_util.h b/include/math_util.h
index 6ae36a995b..dc0eb431c3 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -8,20 +8,74 @@
 #ifndef __CROS_MATH_UTIL_H
 #define __CROS_MATH_UTIL_H
 
-#ifdef CONFIG_FPU
-typedef float matrix_3x3_t[3][3];
-#else
-typedef int matrix_3x3_t[3][3];
-#endif
+/* Fixed-point type */
+typedef int32_t fp_t;
 
-typedef int vector_3_t[3];
+/* Number of bits left of decimal point for fixed-point */
+#define FP_BITS 16
 
+/* Conversion to/from fixed-point */
+#define INT_TO_FP(x) ((fp_t)(x) << FP_BITS)
+#define FP_TO_INT(x) ((int32_t)((x) >> FP_BITS))
+/* Float to fixed-point, only for compile-time constants and unit tests */
+#define FLOAT_TO_FP(x) ((fp_t)((x) * (float)(1<<FP_BITS)))
+/* Fixed-point to float, for unit tests */
+#define FP_TO_FLOAT(x) ((float)(x) / (float)(1<<FP_BITS))
 
-/* Some useful math functions. */
-#define SQ(x) ((x) * (x))
-#define ABS(x) ((x) >= 0 ? (x) : -(x))
+/*
+ * Fixed-point addition and subtraction can be done directly, because they
+ * work identically.
+ */
+
+/**
+ * Multiplication - return (a * b)
+ */
+static inline fp_t fp_mul(fp_t a, fp_t b)
+{
+	return (fp_t)(((int64_t)a * b) >> FP_BITS);
+}
+
+/**
+ * Division - return (a / b)
+ */
+static inline fp_t fp_div(fp_t a, fp_t b)
+{
+	return (fp_t)(((int64_t)a << FP_BITS) / b);
+}
+
+/**
+ * Square (a * a)
+ */
+static inline fp_t fp_sq(fp_t a)
+{
+	return fp_mul(a, a);
+}
 
-#ifdef CONFIG_FPU
+/**
+ * Absolute value
+ */
+static inline fp_t fp_abs(fp_t a)
+{
+	return (a >= 0 ? a : -a);
+}
+
+/*
+ * Fixed point matrix
+ *
+ * Note that constant matrices MUST be initialized using FLOAT_TO_FP()
+ * or INT_TO_FP() for all non-zero values.
+ */
+typedef fp_t matrix_3x3_t[3][3];
+
+/* Integer vector */
+typedef int vector_3_t[3];
+
+/*
+ * Return absolute value of x.  Note that as a macro expansion, this may have
+ * side effects if x includes function calls, which is why inline functions
+ * like fp_abs() are preferred.
+ */
+#define ABS(x) ((x) >= 0 ? (x) : -(x))
 
 /**
  * Find acos(x) in degrees. Argument is clipped to [-1.0, 1.0].
@@ -30,19 +84,20 @@ typedef int vector_3_t[3];
  *
  * @return acos(x) in degrees.
  */
-float arc_cos(float x);
+fp_t arc_cos(fp_t x);
 
 /**
  * Find the cosine of the angle between two vectors.
  *
+ * The implementation assumes no vector component is greater than
+ * 2^(31 - FP_BITS/2).  For example, 2^23, for FP_BITS=16.
+ *
  * @param v1
  * @param v2
  *
  * @return Cosine of the angle between v1 and v2.
  */
-float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
-
-#endif
+fp_t cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
 
 /**
  * Rotate vector v by rotation matrix R.
@@ -51,8 +106,7 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
  * @param R Rotation matrix.
  * @param res Resultant vector.
  */
-void rotate(const vector_3_t v, const matrix_3x3_t R,
-		vector_3_t res);
+void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res);
 
 
 
diff --git a/include/motion_lid.h b/include/motion_lid.h
index b3348c020e..8ccf27191e 100644
--- a/include/motion_lid.h
+++ b/include/motion_lid.h
@@ -3,13 +3,13 @@
  * found in the LICENSE file.
  */
 
-/* Header for motion_sense.c */
+/* Header for motion_lid.h */
 
 #ifndef __CROS_EC_MOTION_LID_H
 #define __CROS_EC_MOTION_LID_H
 
 /* Anything outside of lid angle range [-180, 180] should work. */
-#define LID_ANGLE_UNRELIABLE 500.0F
+#define LID_ANGLE_UNRELIABLE 500
 
 /**
  * This structure defines all of the data needed to specify the orientation
@@ -37,7 +37,8 @@ extern const struct accel_orientation acc_orient;
  * Get last calculated lid angle. Note, the lid angle calculated by the EC
  * is un-calibrated and is an approximate angle.
  *
- * @return lid angle in degrees in range [0, 360].
+ * @return lid angle in degrees in range [0, 360], or LID_ANGLE_UNRELIABLE
+ * if the lid angle can't be determined.
  */
 int motion_lid_get_angle(void);