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 
sqrtf(float square)18 float sqrtf(float square)
19 {
20 	int i;
21 	float root;
22 	float last;
23 	int32_t	exponent;
24 	int32_t *p_square = (int32_t *)&square;
25 	int32_t *p_root = (int32_t *)&root;
26 	int32_t *p_last = (int32_t *)&last;
27 
28 	if (square == 0.0f) {
29 		return square;
30 	}
31 	if (square < 0.0f) {
32 		return (square - square) / (square - square);
33 	}
34 
35 	/* we need a good starting guess so that this will converge quickly,
36 	 * we can do this by dividing the exponent part of the float by 2
37 	 * this assumes IEEE-754 format doubles
38 	 */
39 	exponent = ((*p_square & EXP_MASK32)>>23)-127;
40 	if (exponent == 0xFF-127) {
41 		/* the number is a NAN or inf, return NaN or inf */
42 		return square + square;
43 	}
44 	exponent /= 2;
45 	*p_root = (*p_square & ~EXP_MASK32) | (exponent+127) << 23;
46 
47 	for (i = 0; i < MAX_F_ITTERATIONS; i++) {
48 		last = root;
49 		root = (root + square / root) * 0.5f;
50 		/* if (labs(*p_root - *p_last) < MAX_F_ERROR_COUNT) */
51 		if ((*p_root ^ *p_last) < MAX_F_ERROR_COUNT) {
52 			break;
53 		}
54 	}
55 	return root;
56 }
57