1 /*
2  * Copyright (c) 2018 Vikrant More
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 
7 #include <math.h>
8 #include <stdint.h>
9 #include <stdio.h>
10 #include <stdlib.h>
11 #include <zephyr/sys/util.h>
12 
13 #define MAX_F_ITTERATIONS	6  /* usually converges in 4 loops */
14 				   /* this ensures we break out of the loop */
15 #define MAX_F_ERROR_COUNT	3  /* when result almost converges, stop */
16 #define EXP_MASK32	GENMASK(30, 23)
17 
18 typedef union {
19 	float f;
20 	int32_t i;
21 } intfloat_t;
22 
sqrtf(float square)23 float sqrtf(float square)
24 {
25 	int i;
26 	int32_t exponent;
27 	intfloat_t root;
28 	intfloat_t last;
29 	intfloat_t p_square;
30 
31 	p_square.f = square;
32 
33 	if (square == 0.0f) {
34 		return square;
35 	}
36 	if (square < 0.0f) {
37 		return (square - square) / (square - square);
38 	}
39 
40 	/* we need a good starting guess so that this will converge quickly,
41 	 * we can do this by dividing the exponent part of the float by 2
42 	 * this assumes IEEE-754 format doubles
43 	 */
44 	exponent = ((p_square.i & EXP_MASK32) >> 23) - 127;
45 	if (exponent == 0xFF - 127) {
46 		/* the number is a NAN or inf, return NaN or inf */
47 		return square + square;
48 	}
49 	exponent /= 2;
50 	root.i = (p_square.i & ~EXP_MASK32) | (exponent + 127) << 23;
51 
52 	for (i = 0; i < MAX_F_ITTERATIONS; i++) {
53 		last = root;
54 		root.f = (root.f + square / root.f) * 0.5f;
55 		/* if (labs(*p_root - *p_last) < MAX_F_ERROR_COUNT) */
56 		if ((root.i ^ last.i) < MAX_F_ERROR_COUNT) {
57 			break;
58 		}
59 	}
60 	return root.f;
61 }
62