mag_cal: Support fixed-point calculation.

Modified from floating point version.  This includes changes to
vec3, vec4, mat33, mat44, and mag_cal.
Now fixed-point type (fp_*) functions is a function wrapper for both
fixed-point and floating point version operations:

* define CONFIG_FPU to use floating version mag_cal
* undef CONFIG_FPU to use fixed-point version mag_cal

Also, add tests for both float and fp types operations.

TEST=define CONFIG_FPU; flash on reef; See ARC++ magnetmeter app moving.
TEST=undef CONFIG_FPU; flash on reef; See ARC++ magnetmeter app moving.
TEST=make runtests -j
TEST=make buildalltests -j
BUG=b:113364863
BRANCH=None

Change-Id: Ie695945acb666912babb2a603e09c602a0624d44
Signed-off-by: Yilun Lin <yllin@google.com>
Reviewed-on: https://chromium-review.googlesource.com/1260704
Commit-Ready: Yilun Lin <yllin@chromium.org>
Tested-by: Yilun Lin <yllin@chromium.org>
Reviewed-by: Nicolas Boichat <drinkcat@chromium.org>

4 files changed, 156 insertions, 137 deletions

diff --git a/common/mag_cal.c b/common/mag_cal.c
index 6e5a48b6b1..1e71059921 100644
--- a/common/mag_cal.c
+++ b/common/mag_cal.c
@@ -16,9 +16,9 @@
 /* Data from sensor is in 16th of uT */
 #define MAG_CAL_RAW_UT      16
 
-#define MAX_EIGEN_RATIO     25.0f
-#define MAX_EIGEN_MAG       (80.0f * MAG_CAL_RAW_UT)
-#define MIN_EIGEN_MAG       (10.0f * MAG_CAL_RAW_UT)
+#define MAX_EIGEN_RATIO     FLOAT_TO_FP(25.0f)
+#define MAX_EIGEN_MAG       FLOAT_TO_FP(80.0f * MAG_CAL_RAW_UT)
+#define MIN_EIGEN_MAG       FLOAT_TO_FP(10.0f * MAG_CAL_RAW_UT)
 
 #define MAX_FIT_MAG         MAX_EIGEN_MAG
 #define MIN_FIT_MAG         MIN_EIGEN_MAG
@@ -34,21 +34,24 @@
  */
 static int moc_eigen_test(struct mag_cal_t *moc)
 {
-	mat33_float_t S;
-	floatv3_t eigenvals;
-	mat33_float_t eigenvecs;
-	float evmax, evmin, evmag;
+	mat33_fp_t S;
+	fpv3_t eigenvals;
+	mat33_fp_t eigenvecs;
+	fp_t evmax, evmin, evmag;
 	int eigen_pass;
 
 	/* covariance matrix */
-	S[0][0] = moc->acc[0][0] - moc->acc[0][3] * moc->acc[0][3];
-	S[0][1] = S[1][0] = moc->acc[0][1] - moc->acc[0][3] * moc->acc[1][3];
-	S[0][2] = S[2][0] = moc->acc[0][2] - moc->acc[0][3] * moc->acc[2][3];
-	S[1][1] = moc->acc[1][1] - moc->acc[1][3] * moc->acc[1][3];
-	S[1][2] = S[2][1] = moc->acc[1][2] - moc->acc[1][3] * moc->acc[2][3];
-	S[2][2] = moc->acc[2][2] - moc->acc[2][3] * moc->acc[2][3];
-
-	mat33_float_get_eigenbasis(S, eigenvals, eigenvecs);
+	S[0][0] = moc->acc[0][0] - fp_sq(moc->acc[0][3]);
+	S[0][1] = S[1][0] =
+		moc->acc[0][1] - fp_mul(moc->acc[0][3], moc->acc[1][3]);
+	S[0][2] = S[2][0] =
+		moc->acc[0][2] - fp_mul(moc->acc[0][3], moc->acc[2][3]);
+	S[1][1] = moc->acc[1][1] - fp_sq(moc->acc[1][3]);
+	S[1][2] = S[2][1] =
+		moc->acc[1][2] - fp_mul(moc->acc[1][3], moc->acc[2][3]);
+	S[2][2] = moc->acc[2][2] - fp_sq(moc->acc[2][3]);
+
+	mat33_fp_get_eigenbasis(S, eigenvals, eigenvecs);
 
 	evmax = (eigenvals[X] > eigenvals[Y]) ? eigenvals[X] : eigenvals[Y];
 	evmax = (eigenvals[Z] > evmax) ? eigenvals[Z] : evmax;
@@ -56,9 +59,9 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 	evmin = (eigenvals[X] < eigenvals[Y]) ? eigenvals[X] : eigenvals[Y];
 	evmin = (eigenvals[Z] < evmin) ? eigenvals[Z] : evmin;
 
-	evmag = sqrtf(eigenvals[X] + eigenvals[Y] + eigenvals[Z]);
+	evmag = fp_sqrtf(eigenvals[X] + eigenvals[Y] + eigenvals[Z]);
 
-	eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax)
+	eigen_pass = (fp_mul(evmin, MAX_EIGEN_RATIO) > evmax)
 		&& (evmag > MIN_EIGEN_MAG)
 		&& (evmag < MAX_EIGEN_MAG);
 
@@ -80,10 +83,10 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 /*
  * Kasa sphere fitting with normal equation
  */
-static int moc_fit(struct mag_cal_t *moc, floatv3_t bias, float *radius)
+static int moc_fit(struct mag_cal_t *moc, fpv3_t bias, fp_t *radius)
 {
 	sizev4_t pivot;
-	floatv4_t out;
+	fpv4_t out;
 	int success = 0;
 
 	/*
@@ -100,16 +103,16 @@ static int moc_fit(struct mag_cal_t *moc, floatv3_t bias, float *radius)
 	moc->acc[3][0] = moc->acc[0][3];
 	moc->acc[3][1] = moc->acc[1][3];
 	moc->acc[3][2] = moc->acc[2][3];
-	moc->acc[3][3] = 1.0f;
+	moc->acc[3][3] = FLOAT_TO_FP(1.0f);
 
-	moc->acc_w[X] *= -1;
-	moc->acc_w[Y] *= -1;
-	moc->acc_w[Z] *= -1;
-	moc->acc_w[W] *= -1;
+	moc->acc_w[X] = fp_mul(moc->acc_w[X], FLOAT_TO_FP(-1));
+	moc->acc_w[Y] = fp_mul(moc->acc_w[Y], FLOAT_TO_FP(-1));
+	moc->acc_w[Z] = fp_mul(moc->acc_w[Z], FLOAT_TO_FP(-1));
+	moc->acc_w[W] = fp_mul(moc->acc_w[W], FLOAT_TO_FP(-1));
 
-	mat44_float_decompose_lup(moc->acc, pivot);
+	mat44_fp_decompose_lup(moc->acc, pivot);
 
-	mat44_float_solve(moc->acc, out, moc->acc_w, pivot);
+	mat44_fp_solve(moc->acc, out, moc->acc_w, pivot);
 
 	/*
 	 * spherei is defined by:
@@ -120,10 +123,10 @@ static int moc_fit(struct mag_cal_t *moc, floatv3_t bias, float *radius)
 	 * r = sqrt(xc^2 + yc^2 + zc^2 - out[W])
 	 */
 
-	memcpy(bias, out, sizeof(floatv3_t));
-	floatv3_scalar_mul(bias, -0.5f);
+	memcpy(bias, out, sizeof(fpv3_t));
+	fpv3_scalar_mul(bias, FLOAT_TO_FP(-0.5f));
 
-	*radius = sqrtf(floatv3_dot(bias, bias) - out[W]);
+	*radius = fp_sqrtf(fpv3_dot(bias, bias) - out[W]);
 
 #if 0
 	CPRINTF("mag cal: bias (%d, %d, %d), R %d uT\n",
@@ -152,60 +155,61 @@ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
 	int new_bias = 0;
 
 	/* 1. run accumulators */
-	float w = v[X] * v[X] + v[Y] * v[Y] + v[Z] * v[Z];
+	fp_t w = fp_sq(v[X]) + fp_sq(v[Y]) + fp_sq(v[Z]);
 
 	moc->acc[0][3] += v[X];
 	moc->acc[1][3] += v[Y];
 	moc->acc[2][3] += v[Z];
 	moc->acc_w[W] += w;
 
-	moc->acc[0][0] += v[X] * v[X];
-	moc->acc[0][1] += v[X] * v[Y];
-	moc->acc[0][2] += v[X] * v[Z];
-	moc->acc_w[X] += v[X] * w;
+	moc->acc[0][0] += fp_sq(v[X]);
+	moc->acc[0][1] += fp_mul(v[X], v[Y]);
+	moc->acc[0][2] += fp_mul(v[X], v[Z]);
+	moc->acc_w[X] += fp_mul(v[X], w);
 
-	moc->acc[1][1] += v[Y] * v[Y];
-	moc->acc[1][2] += v[Y] * v[Z];
-	moc->acc_w[Y] += v[Y] * w;
+	moc->acc[1][1] += fp_sq(v[Y]);
+	moc->acc[1][2] += fp_mul(v[Y], v[Z]);
+	moc->acc_w[Y] += fp_mul(v[Y], w);
 
-	moc->acc[2][2] += v[Z] * v[Z];
-	moc->acc_w[Z] += v[Z] * w;
+	moc->acc[2][2] += fp_sq(v[Z]);
+	moc->acc_w[Z] += fp_mul(v[Z], w);
 
 	if (moc->nsamples < MAG_CAL_MAX_SAMPLES)
 		moc->nsamples++;
 
 	/* 2. batch has enough samples? */
 	if (moc->batch_size > 0 && moc->nsamples >= moc->batch_size) {
-		float inv = 1.0f / moc->nsamples;
+		fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
+				  INT_TO_FP((int)moc->nsamples));
 
-		moc->acc[0][3] *= inv;
-		moc->acc[1][3] *= inv;
-		moc->acc[2][3] *= inv;
-		moc->acc_w[W] *= inv;
+		moc->acc[0][3] = fp_mul(moc->acc[0][3], inv);
+		moc->acc[1][3] = fp_mul(moc->acc[1][3], inv);
+		moc->acc[2][3] = fp_mul(moc->acc[2][3], inv);
+		moc->acc_w[W] = fp_mul(moc->acc_w[W], inv);
 
-		moc->acc[0][0] *= inv;
-		moc->acc[0][1] *= inv;
-		moc->acc[0][2] *= inv;
-		moc->acc_w[X] *= inv;
+		moc->acc[0][0] = fp_mul(moc->acc[0][0], inv);
+		moc->acc[0][1] = fp_mul(moc->acc[0][1], inv);
+		moc->acc[0][2] = fp_mul(moc->acc[0][2], inv);
+		moc->acc_w[X] = fp_mul(moc->acc_w[X], inv);
 
-		moc->acc[1][1] *= inv;
-		moc->acc[1][2] *= inv;
-		moc->acc_w[Y] *= inv;
+		moc->acc[1][1] = fp_mul(moc->acc[1][1], inv);
+		moc->acc[1][2] = fp_mul(moc->acc[1][2], inv);
+		moc->acc_w[Y] = fp_mul(moc->acc_w[Y], inv);
 
-		moc->acc[2][2] *= inv;
-		moc->acc_w[Z] *= inv;
+		moc->acc[2][2] = fp_mul(moc->acc[2][2], inv);
+		moc->acc_w[Z] = fp_mul(moc->acc_w[Z], inv);
 
 		/* 3. eigen test */
 		if (moc_eigen_test(moc)) {
-			floatv3_t bias;
-			float radius;
+			fpv3_t bias;
+			fp_t radius;
 
 			/* 4. Kasa sphere fitting */
 			if (moc_fit(moc, bias, &radius)) {
 
-				moc->bias[X] = bias[X] * -1;
-				moc->bias[Y] = bias[Y] * -1;
-				moc->bias[Z] = bias[Z] * -1;
+				moc->bias[X] = fp_mul(bias[X], FLOAT_TO_FP(-1));
+				moc->bias[Y] = fp_mul(bias[Y], FLOAT_TO_FP(-1));
+				moc->bias[Z] = fp_mul(bias[Z], FLOAT_TO_FP(-1));
 
 				moc->radius = radius;
 
@@ -218,4 +222,3 @@ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
 
 	return new_bias;
 }
-
diff --git a/common/mat33.c b/common/mat33.c
index 793173ee3a..87e335db26 100644
--- a/common/mat33.c
+++ b/common/mat33.c
@@ -10,31 +10,35 @@
 
 #define K_EPSILON 1E-5f
 
-void init_zero_matrix(mat33_float_t A)
+void mat33_fp_init_zero(mat33_fp_t A)
 {
-	memset(A, 0, sizeof(mat33_float_t));
+	memset(A, 0, sizeof(mat33_fp_t));
 }
 
-void init_diagonal_matrix(mat33_float_t A, float x)
+void mat33_fp_init_diagonal(mat33_fp_t A, fp_t x)
 {
+	const size_t N = 3;
 	size_t i;
-	init_zero_matrix(A);
 
-	for (i = 0; i < 3; ++i)
+	mat33_fp_init_zero(A);
+
+	for (i = 0; i < N; ++i)
 		A[i][i] = x;
 }
 
-void mat33_float_scalar_mul(mat33_float_t A, float c)
+void mat33_fp_scalar_mul(mat33_fp_t A, fp_t c)
 {
+	const size_t N = 3;
 	size_t i;
-	for (i = 0; i < 3; ++i) {
+
+	for (i = 0; i < N; ++i) {
 		size_t j;
-		for (j = 0; j < 3; ++j)
-			A[i][j] *= c;
+		for (j = 0; j < N; ++j)
+			A[i][j] = fp_mul(A[i][j], c);
 	}
 }
 
-void mat33_float_swap_rows(mat33_float_t A, const size_t i, const size_t j)
+void mat33_fp_swap_rows(mat33_fp_t A, const size_t i, const size_t j)
 {
 	const size_t N = 3;
 	size_t k;
@@ -43,7 +47,7 @@ void mat33_float_swap_rows(mat33_float_t A, const size_t i, const size_t j)
 		return;
 
 	for (k = 0; k < N; ++k) {
-		float tmp = A[i][k];
+		fp_t tmp = A[i][k];
 		A[i][k] = A[j][k];
 		A[j][k] = tmp;
 	}
@@ -55,79 +59,91 @@ void mat33_float_swap_rows(mat33_float_t A, const size_t i, const size_t j)
  * The i-th eigenvalue corresponds to the eigenvector in the i-th _row_ of
  * "eigenvecs".
  */
-void mat33_float_get_eigenbasis(mat33_float_t S, floatv3_t e_vals,
-				mat33_float_t e_vecs)
+void mat33_fp_get_eigenbasis(mat33_fp_t S, fpv3_t e_vals,
+			     mat33_fp_t e_vecs)
 {
 	const size_t N = 3;
 	sizev3_t ind;
 	size_t i, j, k, l, m;
 
 	for (k = 0; k < N; ++k) {
-		ind[k] = mat33_float_maxind(S, k);
+		ind[k] = mat33_fp_maxind(S, k);
 		e_vals[k] = S[k][k];
 	}
 
-	init_diagonal_matrix(e_vecs, 1.0f);
+	mat33_fp_init_diagonal(e_vecs, FLOAT_TO_FP(1.0f));
 
 	for (;;) {
-		float y, t, s, c, p, sum;
+		fp_t y, t, s, c, p, sum;
+
 		m = 0;
-		for (k = 1; k + 1 < N; ++k) {
-			if (fabsf(S[k][ind[k]]) >
-			    fabsf(S[m][ind[m]])) {
+		for (k = 1; k + 1 < N; ++k)
+			if (fp_abs(S[k][ind[k]]) > fp_abs(S[m][ind[m]]))
 				m = k;
-			}
-		}
 
 		k = m;
 		l = ind[m];
 		p = S[k][l];
 
-		if (fabsf(p) < K_EPSILON)
+		/*
+		 * Note: K_EPSILON(1E-5) is too small to fit into 32-bit
+		 * fixed-point(with 16 fp bits). The minimum positive value is
+		 * 1 which is approximately 1.52E-5, so the
+		 * FLOAT_TO_FP(K_EPSILON) becomes zero.
+		 */
+		if (fp_abs(p) <= FLOAT_TO_FP(K_EPSILON))
 			break;
 
-		y = (e_vals[l] - e_vals[k]) * 0.5f;
+		y = fp_mul(e_vals[l] - e_vals[k], FLOAT_TO_FP(0.5f));
 
-		t = fabsf(y) + sqrtf(p * p + y * y);
-		s = sqrtf(p * p + t * t);
-		c = t / s;
-		s = p / s;
-		t = p * p / t;
+		t = fp_abs(y) + fp_sqrtf(fp_sq(p) + fp_sq(y));
+		s = fp_sqrtf(fp_sq(p) + fp_sq(t));
+		c = fp_div_dbz(t, s);
+		s = fp_div_dbz(p, s);
+		t = fp_div_dbz(fp_sq(p), t);
 
-		if (y < 0.0f) {
+		if (y < FLOAT_TO_FP(0.0f)) {
 			s = -s;
 			t = -t;
 		}
 
-		S[k][l] = 0.0f;
+		S[k][l] = FLOAT_TO_FP(0.0f);
 
 		e_vals[k] -= t;
 		e_vals[l] += t;
 
 		for (i = 0; i < k; ++i)
-			mat33_float_rotate(S, c, s, i, k, i, l);
+			mat33_fp_rotate(S, c, s, i, k, i, l);
 
 		for (i = k + 1; i < l; ++i)
-			mat33_float_rotate(S, c, s, k, i, i, l);
+			mat33_fp_rotate(S, c, s, k, i, i, l);
 
 		for (i = l + 1; i < N; ++i)
-			mat33_float_rotate(S, c, s, k, i, l, i);
+			mat33_fp_rotate(S, c, s, k, i, l, i);
 
 		for (i = 0; i < N; ++i) {
-			float tmp = c * e_vecs[k][i] - s * e_vecs[l][i];
-			e_vecs[l][i] = s * e_vecs[k][i] + c * e_vecs[l][i];
+			fp_t tmp = fp_mul(c, e_vecs[k][i]) -
+				   fp_mul(s, e_vecs[l][i]);
+			e_vecs[l][i] = fp_mul(s, e_vecs[k][i]) +
+				       fp_mul(c, e_vecs[l][i]);
 			e_vecs[k][i] = tmp;
 		}
 
-		ind[k] = mat33_float_maxind(S, k);
-		ind[l] = mat33_float_maxind(S, l);
+		ind[k] = mat33_fp_maxind(S, k);
+		ind[l] = mat33_fp_maxind(S, l);
 
-		sum = 0.0f;
+		sum = FLOAT_TO_FP(0.0f);
 		for (i = 0; i < N; ++i)
 			for (j = i + 1; j < N; ++j)
-				sum += fabsf(S[i][j]);
-
-		if (sum < K_EPSILON)
+				sum += fp_abs(S[i][j]);
+
+		/*
+		 * Note: K_EPSILON(1E-5) is too small to fit into 32-bit
+		 * fixed-point(with 16 fp bits). The minimum positive value is
+		 * 1 which is approximately 1.52E-5, so the
+		 * FLOAT_TO_FP(K_EPSILON) becomes zero.
+		 */
+		if (sum <= FLOAT_TO_FP(K_EPSILON))
 			break;
 	}
 
@@ -138,32 +154,32 @@ void mat33_float_get_eigenbasis(mat33_float_t S, floatv3_t e_vals,
 				m = l;
 
 		if (k != m) {
-			float tmp = e_vals[k];
+			fp_t tmp = e_vals[k];
 			e_vals[k] = e_vals[m];
 			e_vals[m] = tmp;
 
-			mat33_float_swap_rows(e_vecs, k, m);
+			mat33_fp_swap_rows(e_vecs, k, m);
 		}
 	}
 }
 
 /* index of largest off-diagonal element in row k */
-size_t mat33_float_maxind(mat33_float_t A, size_t k)
+size_t mat33_fp_maxind(mat33_fp_t A, size_t k)
 {
 	const size_t N = 3;
 	size_t i, m = k + 1;
 
 	for (i = k + 2; i < N; ++i)
-		if (fabsf(A[k][i]) > fabsf(A[k][m]))
+		if (fp_abs(A[k][i]) > fp_abs(A[k][m]))
 			m = i;
 
 	return m;
 }
 
-void mat33_float_rotate(mat33_float_t A, float c, float s,
-		  size_t k, size_t l, size_t i, size_t j)
+void mat33_fp_rotate(mat33_fp_t A, fp_t c, fp_t s,
+		     size_t k, size_t l, size_t i, size_t j)
 {
-	float tmp = c * A[k][l] - s * A[i][j];
-	A[i][j] = s * A[k][l] + c * A[i][j];
+	fp_t tmp = fp_mul(c, A[k][l]) - fp_mul(s, A[i][j]);
+	A[i][j] = fp_mul(s, A[k][l]) + fp_mul(c, A[i][j]);
 	A[k][l] = tmp;
 }
diff --git a/common/mat44.c b/common/mat44.c
index 2ca3b69c32..a4232bf8d4 100644
--- a/common/mat44.c
+++ b/common/mat44.c
@@ -10,37 +10,38 @@
 
 #define K_EPSILON 1E-5f
 
-void mat44_float_decompose_lup(mat44_float_t LU, sizev4_t pivot)
+void mat44_fp_decompose_lup(mat44_fp_t LU, sizev4_t pivot)
 {
 	const size_t N = 4;
 	size_t i, j, k;
 
 	for (k = 0; k < N; ++k) {
-		float max = fabsf(LU[k][k]);
+		fp_t max = fp_abs(LU[k][k]);
 		pivot[k] = k;
 		for (j = k + 1; j < N; ++j) {
-			if (max < fabsf(LU[j][k])) {
-				max = fabsf(LU[j][k]);
+			const fp_t lu_jk = fp_abs(LU[j][k]);
+			if (max < lu_jk) {
+				max = lu_jk;
 				pivot[k] = j;
 			}
 		}
 
 		if (pivot[k] != k)
-			mat44_float_swap_rows(LU, k, pivot[k]);
+			mat44_fp_swap_rows(LU, k, pivot[k]);
 
-		if (fabsf(LU[k][k]) < K_EPSILON)
+		if (fp_abs(LU[k][k]) < FLOAT_TO_FP(K_EPSILON))
 			continue;
 
 		for (j = k + 1; j < N; ++j)
-			LU[k][j] /= LU[k][k];
+			LU[k][j] = fp_div_dbz(LU[k][j], LU[k][k]);
 
 		for (i = k + 1; i < N; ++i)
 			for (j = k + 1; j < N; ++j)
-				LU[i][j] -= LU[i][k] * LU[k][j];
+				LU[i][j] -= fp_mul(LU[i][k], LU[k][j]);
 	}
 }
 
-void mat44_float_swap_rows(mat44_float_t A, const size_t i, const size_t j)
+void mat44_fp_swap_rows(mat44_fp_t A, const size_t i, const size_t j)
 {
 	const size_t N = 4;
 	size_t k;
@@ -49,35 +50,35 @@ void mat44_float_swap_rows(mat44_float_t A, const size_t i, const size_t j)
 		return;
 
 	for (k = 0; k < N; ++k) {
-		float tmp = A[i][k];
+		fp_t tmp = A[i][k];
 		A[i][k] = A[j][k];
 		A[j][k] = tmp;
 	}
 }
 
-void mat44_float_solve(mat44_float_t A, floatv4_t x, const floatv4_t b,
-		 const sizev4_t pivot)
+void mat44_fp_solve(mat44_fp_t A, fpv4_t x, const fpv4_t b,
+		    const sizev4_t pivot)
 {
 	const size_t N = 4;
-	floatv4_t b_copy;
+	fpv4_t b_copy;
 	size_t i, k;
 
-	memcpy(b_copy, b, sizeof(floatv4_t));
+	memcpy(b_copy, b, sizeof(fpv4_t));
 
 	for (k = 0; k < N; ++k) {
 		if (pivot[k] != k) {
-			float tmp = b_copy[k];
+			fp_t tmp = b_copy[k];
 			b_copy[k] = b_copy[pivot[k]];
 			b_copy[pivot[k]] = tmp;
 		}
 
 		x[k] = b_copy[k];
 		for (i = 0; i < k; ++i)
-			x[k] -= x[i] * A[k][i];
-		x[k] /= A[k][k];
+			x[k] -= fp_mul(x[i], A[k][i]);
+		x[k] = fp_div_dbz(x[k], A[k][k]);
 	}
 
 	for (k = N; k-- > 0;)
 		for (i = k + 1; i < N; ++i)
-			x[k] -= x[i] * A[k][i];
+			x[k] -= fp_mul(x[i], A[k][i]);
 }
diff --git a/common/vec3.c b/common/vec3.c
index 4c157467c1..9a3561365a 100644
--- a/common/vec3.c
+++ b/common/vec3.c
@@ -9,25 +9,24 @@
 #include "vec3.h"
 #include "util.h"
 
-void floatv3_scalar_mul(floatv3_t v, float c)
+void fpv3_scalar_mul(fpv3_t v, fp_t c)
 {
-	v[X] *= c;
-	v[Y] *= c;
-	v[Z] *= c;
+	v[X] = fp_mul(v[X], c);
+	v[Y] = fp_mul(v[Y], c);
+	v[Z] = fp_mul(v[Z], c);
 }
 
-float floatv3_dot(const floatv3_t v, const floatv3_t w)
+fp_t fpv3_dot(const fpv3_t v, const fpv3_t w)
 {
-	return v[X] * w[X] + v[Y] * w[Y] + v[Z] * w[Z];
+	return fp_mul(v[X], w[X]) + fp_mul(v[Y], w[Y]) + fp_mul(v[Z], w[Z]);
 }
 
-float floatv3_norm_squared(const floatv3_t v)
+fp_t fpv3_norm_squared(const fpv3_t v)
 {
-	return floatv3_dot(v, v);
+	return fpv3_dot(v, v);
 }
 
-float floatv3_norm(const floatv3_t v)
+fp_t fpv3_norm(const fpv3_t v)
 {
-	return sqrtf(floatv3_norm_squared(v));
+	return fp_sqrtf(fpv3_norm_squared(v));
 }
-