diff --git a/code/espurna/libs/fs_math.c b/code/espurna/libs/fs_math.c deleted file mode 100644 index 8f45eb260b..0000000000 --- a/code/espurna/libs/fs_math.c +++ /dev/null @@ -1,636 +0,0 @@ -/** - * This code is available at - * http://www.mindspring.com/~pfilandr/C/fs_math/ - * and it is believed to be public domain. - */ - -/* BEGIN fs_math.c */ - -#include "fs_math.h" - -#include -/* -** pi == (atan(1.0 / 3) + atan(1.0 / 2)) * 4 -*/ -static double fs_pi(void); -static long double fs_pil(void); - -double fs_sqrt(double x) -{ - int n; - double a, b; - - if (x > 0 && DBL_MAX >= x) { - for (n = 0; x > 2; x /= 4) { - ++n; - } - while (0.5 > x) { - --n; - x *= 4; - } - a = x; - b = (1 + x) / 2; - do { - x = b; - b = (a / x + x) / 2; - } while (x > b); - while (n > 0) { - x *= 2; - --n; - } - while (0 > n) { - x /= 2; - ++n; - } - } else { - if (x != 0) { - x = DBL_MAX; - } - } - return x; -} - -double fs_log(double x) -{ - int n; - double a, b, c, epsilon; - static double A, B, C; - static int initialized; - - if (x > 0 && DBL_MAX >= x) { - if (!initialized) { - initialized = 1; - A = fs_sqrt(2); - B = A / 2; - C = fs_log(A); - } - for (n = 0; x > A; x /= 2) { - ++n; - } - while (B > x) { - --n; - x *= 2; - } - a = (x - 1) / (x + 1); - x = C * n + a; - c = a * a; - n = 1; - epsilon = DBL_EPSILON * x; - if (0 > a) { - if (epsilon > 0) { - epsilon = -epsilon; - } - do { - n += 2; - a *= c; - b = a / n; - x += b; - } while (epsilon > b); - } else { - if (0 > epsilon) { - epsilon = -epsilon; - } - do { - n += 2; - a *= c; - b = a / n; - x += b; - } while (b > epsilon); - } - x *= 2; - } else { - x = -DBL_MAX; - } - return x; -} - -double fs_log10(double x) -{ - static double log_10; - static int initialized; - - if (!initialized) { - initialized = 1; - log_10 = fs_log(10); - } - return x > 0 && DBL_MAX >= x ? fs_log(x) / log_10 : fs_log(x); -} - -double fs_exp(double x) -{ - unsigned n, square; - double b, e; - static double x_max, x_min, epsilon; - static int initialized; - - if (!initialized) { - initialized = 1; - x_max = fs_log(DBL_MAX); - x_min = fs_log(DBL_MIN); - epsilon = DBL_EPSILON / 4; - } - if (x_max >= x && x >= x_min) { - for (square = 0; x > 1; x /= 2) { - ++square; - } - while (-1 > x) { - ++square; - x /= 2; - } - e = b = n = 1; - do { - b /= n++; - b *= x; - e += b; - b /= n++; - b *= x; - e += b; - } while (b > epsilon); - while (square-- != 0) { - e *= e; - } - } else { - e = x > 0 ? DBL_MAX : 0; - } - return e; -} - -double fs_modf(double value, double *iptr) -{ - double a, b; - const double c = value; - - if (0 > c) { - value = -value; - } - if (DBL_MAX >= value) { - for (*iptr = 0; value >= 1; value -= b) { - a = value / 2; - b = 1; - while (a >= b) { - b *= 2; - } - *iptr += b; - } - } else { - *iptr = value; - value = 0; - } - if (0 > c) { - *iptr = -*iptr; - value = -value; - } - return value; -} - -double fs_fmod(double x, double y) -{ - double a, b; - const double c = x; - - if (0 > c) { - x = -x; - } - if (0 > y) { - y = -y; - } - if (y != 0 && DBL_MAX >= y && DBL_MAX >= x) { - while (x >= y) { - a = x / 2; - b = y; - while (a >= b) { - b *= 2; - } - x -= b; - } - } else { - x = 0; - } - return 0 > c ? -x : x; -} - -double fs_pow(double x, double y) -{ - double p = 0; - - if (0 > x && fs_fmod(y, 1) == 0) { - if (fs_fmod(y, 2) == 0) { - p = fs_exp(fs_log(-x) * y); - } else { - p = -fs_exp(fs_log(-x) * y); - } - } else { - if (x != 0 || 0 >= y) { - p = fs_exp(fs_log( x) * y); - } - } - return p; -} - -static double fs_pi(void) -{ - unsigned n; - double a, b, epsilon; - static double p; - static int initialized; - - if (!initialized) { - initialized = 1; - epsilon = DBL_EPSILON / 4; - n = 1; - a = 3; - do { - a /= 9; - b = a / n; - n += 2; - a /= 9; - b -= a / n; - n += 2; - p += b; - } while (b > epsilon); - epsilon = DBL_EPSILON / 2; - n = 1; - a = 2; - do { - a /= 4; - b = a / n; - n += 2; - a /= 4; - b -= a / n; - n += 2; - p += b; - } while (b > epsilon); - p *= 4; - } - return p; -} - -double fs_cos(double x) -{ - unsigned n; - int negative, sine; - double a, b, c; - static double pi, two_pi, half_pi, third_pi, epsilon; - static int initialized; - - if (0 > x) { - x = -x; - } - if (DBL_MAX >= x) { - if (!initialized) { - initialized = 1; - pi = fs_pi(); - two_pi = 2 * pi; - half_pi = pi / 2; - third_pi = pi / 3; - epsilon = DBL_EPSILON / 2; - } - if (x > two_pi) { - x = fs_fmod(x, two_pi); - } - if (x > pi) { - x = two_pi - x; - } - if (x > half_pi) { - x = pi - x; - negative = 1; - } else { - negative = 0; - } - if (x > third_pi) { - x = half_pi - x; - sine = 1; - } else { - sine = 0; - } - c = x * x; - x = n = 0; - a = 1; - do { - b = a; - a *= c; - a /= ++n; - a /= ++n; - b -= a; - a *= c; - a /= ++n; - a /= ++n; - x += b; - } while (b > epsilon); - if (sine) { - x = fs_sqrt((1 - x) * (1 + x)); - } - if (negative) { - x = -x; - } - } else { - x = -DBL_MAX; - } - return x; -} - -double fs_log2(double x) -{ - static double log_2; - static int initialized; - - if (!initialized) { - initialized = 1; - log_2 = fs_log(2); - } - return x > 0 && DBL_MAX >= x ? fs_log(x) / log_2 : fs_log(x); -} - -double fs_exp2(double x) -{ - static double log_2; - static int initialized; - - if (!initialized) { - initialized = 1; - log_2 = fs_log(2); - } - return fs_exp(x * log_2); -} - -long double fs_powl(long double x, long double y) -{ - long double p; - - if (0 > x && fs_fmodl(y, 1) == 0) { - if (fs_fmodl(y, 2) == 0) { - p = fs_expl(fs_logl(-x) * y); - } else { - p = -fs_expl(fs_logl(-x) * y); - } - } else { - if (x != 0 || 0 >= y) { - p = fs_expl(fs_logl( x) * y); - } else { - p = 0; - } - } - return p; -} - -long double fs_sqrtl(long double x) -{ - long int n; - long double a, b; - - if (x > 0 && LDBL_MAX >= x) { - for (n = 0; x > 2; x /= 4) { - ++n; - } - while (0.5 > x) { - --n; - x *= 4; - } - a = x; - b = (1 + x) / 2; - do { - x = b; - b = (a / x + x) / 2; - } while (x > b); - while (n > 0) { - x *= 2; - --n; - } - while (0 > n) { - x /= 2; - ++n; - } - } else { - if (x != 0) { - x = LDBL_MAX; - } - } - return x; -} - -long double fs_logl(long double x) -{ - long int n; - long double a, b, c, epsilon; - static long double A, B, C; - static int initialized; - - if (x > 0 && LDBL_MAX >= x) { - if (!initialized) { - initialized = 1; - B = 1.5; - do { - A = B; - B = 1 / A + A / 2; - } while (A > B); - B /= 2; - C = fs_logl(A); - } - for (n = 0; x > A; x /= 2) { - ++n; - } - while (B > x) { - --n; - x *= 2; - } - a = (x - 1) / (x + 1); - x = C * n + a; - c = a * a; - n = 1; - epsilon = LDBL_EPSILON * x; - if (0 > a) { - if (epsilon > 0) { - epsilon = -epsilon; - } - do { - n += 2; - a *= c; - b = a / n; - x += b; - } while (epsilon > b); - } else { - if (0 > epsilon) { - epsilon = -epsilon; - } - do { - n += 2; - a *= c; - b = a / n; - x += b; - } while (b > epsilon); - } - x *= 2; - } else { - x = -LDBL_MAX; - } - return x; -} - -long double fs_expl(long double x) -{ - long unsigned n, square; - long double b, e; - static long double x_max, x_min, epsilon; - static int initialized; - - if (!initialized) { - initialized = 1; - x_max = fs_logl(LDBL_MAX); - x_min = fs_logl(LDBL_MIN); - epsilon = LDBL_EPSILON / 4; - } - if (x_max >= x && x >= x_min) { - for (square = 0; x > 1; x /= 2) { - ++square; - } - while (-1 > x) { - ++square; - x /= 2; - } - e = b = n = 1; - do { - b /= n++; - b *= x; - e += b; - b /= n++; - b *= x; - e += b; - } while (b > epsilon); - while (square-- != 0) { - e *= e; - } - } else { - e = x > 0 ? LDBL_MAX : 0; - } - return e; -} - -static long double fs_pil(void) -{ - long unsigned n; - long double a, b, epsilon; - static long double p; - static int initialized; - - if (!initialized) { - initialized = 1; - epsilon = LDBL_EPSILON / 4; - n = 1; - a = 3; - do { - a /= 9; - b = a / n; - n += 2; - a /= 9; - b -= a / n; - n += 2; - p += b; - } while (b > epsilon); - epsilon = LDBL_EPSILON / 2; - n = 1; - a = 2; - do { - a /= 4; - b = a / n; - n += 2; - a /= 4; - b -= a / n; - n += 2; - p += b; - } while (b > epsilon); - p *= 4; - } - return p; -} - -long double fs_cosl(long double x) -{ - long unsigned n; - int negative, sine; - long double a, b, c; - static long double pi, two_pi, half_pi, third_pi, epsilon; - static int initialized; - - if (0 > x) { - x = -x; - } - if (LDBL_MAX >= x) { - if (!initialized) { - initialized = 1; - pi = fs_pil(); - two_pi = 2 * pi; - half_pi = pi / 2; - third_pi = pi / 3; - epsilon = LDBL_EPSILON / 2; - } - if (x > two_pi) { - x = fs_fmodl(x, two_pi); - } - if (x > pi) { - x = two_pi - x; - } - if (x > half_pi) { - x = pi - x; - negative = 1; - } else { - negative = 0; - } - if (x > third_pi) { - x = half_pi - x; - sine = 1; - } else { - sine = 0; - } - c = x * x; - x = n = 0; - a = 1; - do { - b = a; - a *= c; - a /= ++n; - a /= ++n; - b -= a; - a *= c; - a /= ++n; - a /= ++n; - x += b; - } while (b > epsilon); - if (sine) { - x = fs_sqrtl((1 - x) * (1 + x)); - } - if (negative) { - x = -x; - } - } else { - x = -LDBL_MAX; - } - return x; -} - -long double fs_fmodl(long double x, long double y) -{ - long double a, b; - const long double c = x; - - if (0 > c) { - x = -x; - } - if (0 > y) { - y = -y; - } - if (y != 0 && LDBL_MAX >= y && LDBL_MAX >= x) { - while (x >= y) { - a = x / 2; - b = y; - while (a >= b) { - b *= 2; - } - x -= b; - } - } else { - x = 0; - } - return 0 > c ? -x : x; -} - -/* END fs_math.c */ diff --git a/code/espurna/libs/fs_math.h b/code/espurna/libs/fs_math.h index 1884fcddff..c3eb7cbd97 100644 --- a/code/espurna/libs/fs_math.h +++ b/code/espurna/libs/fs_math.h @@ -4,12 +4,12 @@ * and it is believed to be public domain. */ -/* BEGIN fs_math.h */ -/* -** Portable freestanding code. -*/ -#ifndef H_FS_MATH_H -#define H_FS_MATH_H + #ifndef H_FS_MATH_H + #define H_FS_MATH_H + + // ----------------------------------------------------------------------------- + // fs_math.h + // ----------------------------------------------------------------------------- double fs_sqrt(double x); double fs_log(double x); @@ -34,83 +34,633 @@ long double fs_expl(long double x); long double fs_cosl(long double x); long double fs_fmodl(long double x, long double y); -#endif +// ----------------------------------------------------------------------------- +// fs_math.c +// ----------------------------------------------------------------------------- -/* END fs_math.h */ +#include +/* +** pi == (atan(1.0 / 3) + atan(1.0 / 2)) * 4 +*/ +static double fs_pi(void); +static long double fs_pil(void); -#if 0 +double fs_sqrt(double x) +{ + int n; + double a, b; -/* -> > Anybody know where I can get some source code for a -> > reasonably fast double -> > precision square root algorithm in C. -> > I'm looking for one that is not IEEE -> > compliant as I am running on a Z/OS mainframe. -> > -> > I would love to use the standard library but -> > unfortunatly I'm using a -> > stripped down version of C that looses the the runtime library -> > (we have to write our own). -> -> long double Ssqrt(long double x) -> { -> long double a, b; -> size_t c; - -size_t is a bug here. -c needs to be a signed type: - long c; - -> if (x > 0) { -> c = 0; -> while (x > 4) { -> x /= 4; -> ++c; -> } -> while (1.0 / 4 > x) { -> x *= 4; -> --c; -> } -> a = x; -> b = ((4 > a) + a) / 2; - -Not a bug, but should be: - b = (1 + a) / 2; - -> do { -> x = b; -> b = (a / x + x) / 2; -> } while (x > b); -> if (c > 0) { - -The above line is why c needs to be a signed type, -otherwise the decremented values of c, are greater than zero, -and the function won't work if the initial value of x -is less than 0.25 - -> while (c--) { -> x *= 2; -> } -> } else { -> while (c++) { -> x /= 2; -> } -> } -> } -> return x; -> } - -> -> > -> > That algorithm was actually 4 times slower -> > then the one below, and more -> > code. It was accurate though. -> > -> -> Sorry Pete, I wasn't looking very carefully. -> When I converted your function -> to double precision it's was much quicker, the best I've seen yet. + if (x > 0 && DBL_MAX >= x) { + for (n = 0; x > 2; x /= 4) { + ++n; + } + while (0.5 > x) { + --n; + x *= 4; + } + a = x; + b = (1 + x) / 2; + do { + x = b; + b = (a / x + x) / 2; + } while (x > b); + while (n > 0) { + x *= 2; + --n; + } + while (0 > n) { + x /= 2; + ++n; + } + } else { + if (x != 0) { + x = DBL_MAX; + } + } + return x; +} -*/ +double fs_log(double x) +{ + int n; + double a, b, c, epsilon; + static double A, B, C; + static int initialized; + + if (x > 0 && DBL_MAX >= x) { + if (!initialized) { + initialized = 1; + A = fs_sqrt(2); + B = A / 2; + C = fs_log(A); + } + for (n = 0; x > A; x /= 2) { + ++n; + } + while (B > x) { + --n; + x *= 2; + } + a = (x - 1) / (x + 1); + x = C * n + a; + c = a * a; + n = 1; + epsilon = DBL_EPSILON * x; + if (0 > a) { + if (epsilon > 0) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (epsilon > b); + } else { + if (0 > epsilon) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (b > epsilon); + } + x *= 2; + } else { + x = -DBL_MAX; + } + return x; +} + +double fs_log10(double x) +{ + static double log_10; + static int initialized; + + if (!initialized) { + initialized = 1; + log_10 = fs_log(10); + } + return x > 0 && DBL_MAX >= x ? fs_log(x) / log_10 : fs_log(x); +} + +double fs_exp(double x) +{ + unsigned n, square; + double b, e; + static double x_max, x_min, epsilon; + static int initialized; + + if (!initialized) { + initialized = 1; + x_max = fs_log(DBL_MAX); + x_min = fs_log(DBL_MIN); + epsilon = DBL_EPSILON / 4; + } + if (x_max >= x && x >= x_min) { + for (square = 0; x > 1; x /= 2) { + ++square; + } + while (-1 > x) { + ++square; + x /= 2; + } + e = b = n = 1; + do { + b /= n++; + b *= x; + e += b; + b /= n++; + b *= x; + e += b; + } while (b > epsilon); + while (square-- != 0) { + e *= e; + } + } else { + e = x > 0 ? DBL_MAX : 0; + } + return e; +} + +double fs_modf(double value, double *iptr) +{ + double a, b; + const double c = value; + + if (0 > c) { + value = -value; + } + if (DBL_MAX >= value) { + for (*iptr = 0; value >= 1; value -= b) { + a = value / 2; + b = 1; + while (a >= b) { + b *= 2; + } + *iptr += b; + } + } else { + *iptr = value; + value = 0; + } + if (0 > c) { + *iptr = -*iptr; + value = -value; + } + return value; +} + +double fs_fmod(double x, double y) +{ + double a, b; + const double c = x; + + if (0 > c) { + x = -x; + } + if (0 > y) { + y = -y; + } + if (y != 0 && DBL_MAX >= y && DBL_MAX >= x) { + while (x >= y) { + a = x / 2; + b = y; + while (a >= b) { + b *= 2; + } + x -= b; + } + } else { + x = 0; + } + return 0 > c ? -x : x; +} + +double fs_pow(double x, double y) +{ + double p = 0; + + if (0 > x && fs_fmod(y, 1) == 0) { + if (fs_fmod(y, 2) == 0) { + p = fs_exp(fs_log(-x) * y); + } else { + p = -fs_exp(fs_log(-x) * y); + } + } else { + if (x != 0 || 0 >= y) { + p = fs_exp(fs_log( x) * y); + } + } + return p; +} + +static double fs_pi(void) +{ + unsigned n; + double a, b, epsilon; + static double p; + static int initialized; + + if (!initialized) { + initialized = 1; + epsilon = DBL_EPSILON / 4; + n = 1; + a = 3; + do { + a /= 9; + b = a / n; + n += 2; + a /= 9; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + epsilon = DBL_EPSILON / 2; + n = 1; + a = 2; + do { + a /= 4; + b = a / n; + n += 2; + a /= 4; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + p *= 4; + } + return p; +} + +double fs_cos(double x) +{ + unsigned n; + int negative, sine; + double a, b, c; + static double pi, two_pi, half_pi, third_pi, epsilon; + static int initialized; + + if (0 > x) { + x = -x; + } + if (DBL_MAX >= x) { + if (!initialized) { + initialized = 1; + pi = fs_pi(); + two_pi = 2 * pi; + half_pi = pi / 2; + third_pi = pi / 3; + epsilon = DBL_EPSILON / 2; + } + if (x > two_pi) { + x = fs_fmod(x, two_pi); + } + if (x > pi) { + x = two_pi - x; + } + if (x > half_pi) { + x = pi - x; + negative = 1; + } else { + negative = 0; + } + if (x > third_pi) { + x = half_pi - x; + sine = 1; + } else { + sine = 0; + } + c = x * x; + x = n = 0; + a = 1; + do { + b = a; + a *= c; + a /= ++n; + a /= ++n; + b -= a; + a *= c; + a /= ++n; + a /= ++n; + x += b; + } while (b > epsilon); + if (sine) { + x = fs_sqrt((1 - x) * (1 + x)); + } + if (negative) { + x = -x; + } + } else { + x = -DBL_MAX; + } + return x; +} + +double fs_log2(double x) +{ + static double log_2; + static int initialized; + + if (!initialized) { + initialized = 1; + log_2 = fs_log(2); + } + return x > 0 && DBL_MAX >= x ? fs_log(x) / log_2 : fs_log(x); +} + +double fs_exp2(double x) +{ + static double log_2; + static int initialized; + + if (!initialized) { + initialized = 1; + log_2 = fs_log(2); + } + return fs_exp(x * log_2); +} + +long double fs_powl(long double x, long double y) +{ + long double p; + + if (0 > x && fs_fmodl(y, 1) == 0) { + if (fs_fmodl(y, 2) == 0) { + p = fs_expl(fs_logl(-x) * y); + } else { + p = -fs_expl(fs_logl(-x) * y); + } + } else { + if (x != 0 || 0 >= y) { + p = fs_expl(fs_logl( x) * y); + } else { + p = 0; + } + } + return p; +} + +long double fs_sqrtl(long double x) +{ + long int n; + long double a, b; + + if (x > 0 && LDBL_MAX >= x) { + for (n = 0; x > 2; x /= 4) { + ++n; + } + while (0.5 > x) { + --n; + x *= 4; + } + a = x; + b = (1 + x) / 2; + do { + x = b; + b = (a / x + x) / 2; + } while (x > b); + while (n > 0) { + x *= 2; + --n; + } + while (0 > n) { + x /= 2; + ++n; + } + } else { + if (x != 0) { + x = LDBL_MAX; + } + } + return x; +} + +long double fs_logl(long double x) +{ + long int n; + long double a, b, c, epsilon; + static long double A, B, C; + static int initialized; + + if (x > 0 && LDBL_MAX >= x) { + if (!initialized) { + initialized = 1; + B = 1.5; + do { + A = B; + B = 1 / A + A / 2; + } while (A > B); + B /= 2; + C = fs_logl(A); + } + for (n = 0; x > A; x /= 2) { + ++n; + } + while (B > x) { + --n; + x *= 2; + } + a = (x - 1) / (x + 1); + x = C * n + a; + c = a * a; + n = 1; + epsilon = LDBL_EPSILON * x; + if (0 > a) { + if (epsilon > 0) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (epsilon > b); + } else { + if (0 > epsilon) { + epsilon = -epsilon; + } + do { + n += 2; + a *= c; + b = a / n; + x += b; + } while (b > epsilon); + } + x *= 2; + } else { + x = -LDBL_MAX; + } + return x; +} + +long double fs_expl(long double x) +{ + long unsigned n, square; + long double b, e; + static long double x_max, x_min, epsilon; + static int initialized; + + if (!initialized) { + initialized = 1; + x_max = fs_logl(LDBL_MAX); + x_min = fs_logl(LDBL_MIN); + epsilon = LDBL_EPSILON / 4; + } + if (x_max >= x && x >= x_min) { + for (square = 0; x > 1; x /= 2) { + ++square; + } + while (-1 > x) { + ++square; + x /= 2; + } + e = b = n = 1; + do { + b /= n++; + b *= x; + e += b; + b /= n++; + b *= x; + e += b; + } while (b > epsilon); + while (square-- != 0) { + e *= e; + } + } else { + e = x > 0 ? LDBL_MAX : 0; + } + return e; +} + +static long double fs_pil(void) +{ + long unsigned n; + long double a, b, epsilon; + static long double p; + static int initialized; + + if (!initialized) { + initialized = 1; + epsilon = LDBL_EPSILON / 4; + n = 1; + a = 3; + do { + a /= 9; + b = a / n; + n += 2; + a /= 9; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + epsilon = LDBL_EPSILON / 2; + n = 1; + a = 2; + do { + a /= 4; + b = a / n; + n += 2; + a /= 4; + b -= a / n; + n += 2; + p += b; + } while (b > epsilon); + p *= 4; + } + return p; +} + +long double fs_cosl(long double x) +{ + long unsigned n; + int negative, sine; + long double a, b, c; + static long double pi, two_pi, half_pi, third_pi, epsilon; + static int initialized; + + if (0 > x) { + x = -x; + } + if (LDBL_MAX >= x) { + if (!initialized) { + initialized = 1; + pi = fs_pil(); + two_pi = 2 * pi; + half_pi = pi / 2; + third_pi = pi / 3; + epsilon = LDBL_EPSILON / 2; + } + if (x > two_pi) { + x = fs_fmodl(x, two_pi); + } + if (x > pi) { + x = two_pi - x; + } + if (x > half_pi) { + x = pi - x; + negative = 1; + } else { + negative = 0; + } + if (x > third_pi) { + x = half_pi - x; + sine = 1; + } else { + sine = 0; + } + c = x * x; + x = n = 0; + a = 1; + do { + b = a; + a *= c; + a /= ++n; + a /= ++n; + b -= a; + a *= c; + a /= ++n; + a /= ++n; + x += b; + } while (b > epsilon); + if (sine) { + x = fs_sqrtl((1 - x) * (1 + x)); + } + if (negative) { + x = -x; + } + } else { + x = -LDBL_MAX; + } + return x; +} + +long double fs_fmodl(long double x, long double y) +{ + long double a, b; + const long double c = x; + + if (0 > c) { + x = -x; + } + if (0 > y) { + y = -y; + } + if (y != 0 && LDBL_MAX >= y && LDBL_MAX >= x) { + while (x >= y) { + a = x / 2; + b = y; + while (a >= b) { + b *= 2; + } + x -= b; + } + } else { + x = 0; + } + return 0 > c ? -x : x; +} #endif diff --git a/code/espurna/libs/pwm.c b/code/espurna/libs/pwm.c deleted file mode 100644 index 0e3794c18d..0000000000 --- a/code/espurna/libs/pwm.c +++ /dev/null @@ -1,447 +0,0 @@ -/* - * Copyright (C) 2016 Stefan Brüns - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - */ - -/* Set the following three defines to your needs */ - -#ifndef SDK_PWM_PERIOD_COMPAT_MODE - #define SDK_PWM_PERIOD_COMPAT_MODE 0 -#endif -#ifndef PWM_MAX_CHANNELS - #define PWM_MAX_CHANNELS 8 -#endif -#define PWM_DEBUG 0 -#define PWM_USE_NMI 1 - -/* no user servicable parts beyond this point */ - -#define PWM_MAX_TICKS 0x7fffff -#if SDK_PWM_PERIOD_COMPAT_MODE -#define PWM_PERIOD_TO_TICKS(x) (x * 0.2) -#define PWM_DUTY_TO_TICKS(x) (x * 5) -#define PWM_MAX_DUTY (PWM_MAX_TICKS * 0.2) -#define PWM_MAX_PERIOD (PWM_MAX_TICKS * 5) -#else -#define PWM_PERIOD_TO_TICKS(x) (x) -#define PWM_DUTY_TO_TICKS(x) (x) -#define PWM_MAX_DUTY PWM_MAX_TICKS -#define PWM_MAX_PERIOD PWM_MAX_TICKS -#endif - -#include -#include -#include -#include - -// from SDK hw_timer.c -#define TIMER1_DIVIDE_BY_16 0x0004 -#define TIMER1_ENABLE_TIMER 0x0080 - -struct pwm_phase { - uint32_t ticks; ///< delay until next phase, in 200ns units - uint16_t on_mask; ///< GPIO mask to switch on - uint16_t off_mask; ///< GPIO mask to switch off -}; - -/* Three sets of PWM phases, the active one, the one used - * starting with the next cycle, and the one updated - * by pwm_start. After the update pwm_next_set - * is set to the last updated set. pwm_current_set is set to - * pwm_next_set from the interrupt routine during the first - * pwm phase - */ -typedef struct pwm_phase (pwm_phase_array)[PWM_MAX_CHANNELS + 2]; -static pwm_phase_array pwm_phases[3]; -static struct { - struct pwm_phase* next_set; - struct pwm_phase* current_set; - uint8_t current_phase; -} pwm_state; - -static uint32_t pwm_period; -static uint32_t pwm_period_ticks; -static uint32_t pwm_duty[PWM_MAX_CHANNELS]; -static uint16_t gpio_mask[PWM_MAX_CHANNELS]; -static uint8_t pwm_channels; - -// 3-tuples of MUX_REGISTER, MUX_VALUE and GPIO number -typedef uint32_t (pin_info_type)[3]; - -struct gpio_regs { - uint32_t out; /* 0x60000300 */ - uint32_t out_w1ts; /* 0x60000304 */ - uint32_t out_w1tc; /* 0x60000308 */ - uint32_t enable; /* 0x6000030C */ - uint32_t enable_w1ts; /* 0x60000310 */ - uint32_t enable_w1tc; /* 0x60000314 */ - uint32_t in; /* 0x60000318 */ - uint32_t status; /* 0x6000031C */ - uint32_t status_w1ts; /* 0x60000320 */ - uint32_t status_w1tc; /* 0x60000324 */ -}; -static struct gpio_regs* gpio = (struct gpio_regs*)(0x60000300); - -struct timer_regs { - uint32_t frc1_load; /* 0x60000600 */ - uint32_t frc1_count; /* 0x60000604 */ - uint32_t frc1_ctrl; /* 0x60000608 */ - uint32_t frc1_int; /* 0x6000060C */ - uint8_t pad[16]; - uint32_t frc2_load; /* 0x60000620 */ - uint32_t frc2_count; /* 0x60000624 */ - uint32_t frc2_ctrl; /* 0x60000628 */ - uint32_t frc2_int; /* 0x6000062C */ - uint32_t frc2_alarm; /* 0x60000630 */ -}; -static struct timer_regs* timer = (struct timer_regs*)(0x60000600); - -static void ICACHE_RAM_ATTR -pwm_intr_handler(void) -{ - if ((pwm_state.current_set[pwm_state.current_phase].off_mask == 0) && - (pwm_state.current_set[pwm_state.current_phase].on_mask == 0)) { - pwm_state.current_set = pwm_state.next_set; - pwm_state.current_phase = 0; - } - - do { - // force write to GPIO registers on each loop - asm volatile ("" : : : "memory"); - - gpio->out_w1ts = (uint32_t)(pwm_state.current_set[pwm_state.current_phase].on_mask); - gpio->out_w1tc = (uint32_t)(pwm_state.current_set[pwm_state.current_phase].off_mask); - - uint32_t ticks = pwm_state.current_set[pwm_state.current_phase].ticks; - - pwm_state.current_phase++; - - if (ticks) { - if (ticks >= 16) { - // constant interrupt overhead - ticks -= 9; - timer->frc1_int &= ~FRC1_INT_CLR_MASK; - WRITE_PERI_REG(&timer->frc1_load, ticks); - return; - } - - ticks *= 4; - do { - ticks -= 1; - // stop compiler from optimizing delay loop to noop - asm volatile ("" : : : "memory"); - } while (ticks > 0); - } - - } while (1); -} - -/** - * period: initial period (base unit 1us OR 200ns) - * duty: array of initial duty values, may be NULL, may be freed after pwm_init - * pwm_channel_num: number of channels to use - * pin_info_list: array of pin_info - */ -void ICACHE_FLASH_ATTR -pwm_init(uint32_t period, uint32_t *duty, uint32_t pwm_channel_num, - uint32_t (*pin_info_list)[3]) -{ - int i, j, n; - - pwm_channels = pwm_channel_num; - if (pwm_channels > PWM_MAX_CHANNELS) - pwm_channels = PWM_MAX_CHANNELS; - - for (i = 0; i < 3; i++) { - for (j = 0; j < (PWM_MAX_CHANNELS + 2); j++) { - pwm_phases[i][j].ticks = 0; - pwm_phases[i][j].on_mask = 0; - pwm_phases[i][j].off_mask = 0; - } - } - pwm_state.current_set = pwm_state.next_set = 0; - pwm_state.current_phase = 0; - - uint32_t all = 0; - // PIN info: MUX-Register, Mux-Setting, PIN-Nr - for (n = 0; n < pwm_channels; n++) { - pin_info_type* pin_info = &pin_info_list[n]; - PIN_FUNC_SELECT((*pin_info)[0], (*pin_info)[1]); - gpio_mask[n] = 1 << (*pin_info)[2]; - all |= 1 << (*pin_info)[2]; - if (duty) - pwm_set_duty(duty[n], n); - } - GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, all); - GPIO_REG_WRITE(GPIO_ENABLE_W1TS_ADDRESS, all); - - pwm_set_period(period); - -#if PWM_USE_NMI - ETS_FRC_TIMER1_NMI_INTR_ATTACH(pwm_intr_handler); -#else - ETS_FRC_TIMER1_INTR_ATTACH(pwm_intr_handler, NULL); -#endif - TM1_EDGE_INT_ENABLE(); - - timer->frc1_int &= ~FRC1_INT_CLR_MASK; - timer->frc1_ctrl = 0; - - pwm_start(); -} - -__attribute__ ((noinline)) -static uint8_t ICACHE_FLASH_ATTR -_pwm_phases_prep(struct pwm_phase* pwm) -{ - uint8_t n, phases; - - for (n = 0; n < pwm_channels + 2; n++) { - pwm[n].ticks = 0; - pwm[n].on_mask = 0; - pwm[n].off_mask = 0; - } - phases = 1; - for (n = 0; n < pwm_channels; n++) { - uint32_t ticks = PWM_DUTY_TO_TICKS(pwm_duty[n]); - if (ticks == 0) { - pwm[0].off_mask |= gpio_mask[n]; - } else if (ticks >= pwm_period_ticks) { - pwm[0].on_mask |= gpio_mask[n]; - } else { - if (ticks < (pwm_period_ticks/2)) { - pwm[phases].ticks = ticks; - pwm[0].on_mask |= gpio_mask[n]; - pwm[phases].off_mask = gpio_mask[n]; - } else { - pwm[phases].ticks = pwm_period_ticks - ticks; - pwm[phases].on_mask = gpio_mask[n]; - pwm[0].off_mask |= gpio_mask[n]; - } - phases++; - } - } - pwm[phases].ticks = pwm_period_ticks; - - // bubble sort, lowest to hightest duty - n = 2; - while (n < phases) { - if (pwm[n].ticks < pwm[n - 1].ticks) { - struct pwm_phase t = pwm[n]; - pwm[n] = pwm[n - 1]; - pwm[n - 1] = t; - if (n > 2) - n--; - } else { - n++; - } - } - -#if PWM_DEBUG - int t = 0; - for (t = 0; t <= phases; t++) { - ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); - } -#endif - - // shift left to align right edge; - uint8_t l = 0, r = 1; - while (r <= phases) { - uint32_t diff = pwm[r].ticks - pwm[l].ticks; - if (diff && (diff <= 16)) { - uint16_t mask = pwm[r].on_mask | pwm[r].off_mask; - pwm[l].off_mask ^= pwm[r].off_mask; - pwm[l].on_mask ^= pwm[r].on_mask; - pwm[0].off_mask ^= pwm[r].on_mask; - pwm[0].on_mask ^= pwm[r].off_mask; - pwm[r].ticks = pwm_period_ticks - diff; - pwm[r].on_mask ^= mask; - pwm[r].off_mask ^= mask; - } else { - l = r; - } - r++; - } - -#if PWM_DEBUG - for (t = 0; t <= phases; t++) { - ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); - } -#endif - - // sort again - n = 2; - while (n <= phases) { - if (pwm[n].ticks < pwm[n - 1].ticks) { - struct pwm_phase t = pwm[n]; - pwm[n] = pwm[n - 1]; - pwm[n - 1] = t; - if (n > 2) - n--; - } else { - n++; - } - } - - // merge same duty - l = 0, r = 1; - while (r <= phases) { - if (pwm[r].ticks == pwm[l].ticks) { - pwm[l].off_mask |= pwm[r].off_mask; - pwm[l].on_mask |= pwm[r].on_mask; - pwm[r].on_mask = 0; - pwm[r].off_mask = 0; - } else { - l++; - if (l != r) { - struct pwm_phase t = pwm[l]; - pwm[l] = pwm[r]; - pwm[r] = t; - } - } - r++; - } - phases = l; - -#if PWM_DEBUG - for (t = 0; t <= phases; t++) { - ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); - } -#endif - - // transform absolute end time to phase durations - for (n = 0; n < phases; n++) { - pwm[n].ticks = - pwm[n + 1].ticks - pwm[n].ticks; - // subtract common overhead - pwm[n].ticks--; - } - pwm[phases].ticks = 0; - - // do a cyclic shift if last phase is short - if (pwm[phases - 1].ticks < 16) { - for (n = 0; n < phases - 1; n++) { - struct pwm_phase t = pwm[n]; - pwm[n] = pwm[n + 1]; - pwm[n + 1] = t; - } - } - -#if PWM_DEBUG - for (t = 0; t <= phases; t++) { - ets_printf("%d +%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); - } - ets_printf("\n"); -#endif - - return phases; -} - -void ICACHE_FLASH_ATTR -pwm_start(void) -{ - pwm_phase_array* pwm = &pwm_phases[0]; - - if ((*pwm == pwm_state.next_set) || - (*pwm == pwm_state.current_set)) - pwm++; - if ((*pwm == pwm_state.next_set) || - (*pwm == pwm_state.current_set)) - pwm++; - - uint8_t phases = _pwm_phases_prep(*pwm); - - // all with 0% / 100% duty - stop timer - if (phases == 1) { - if (pwm_state.next_set) { -#if PWM_DEBUG - ets_printf("PWM stop\n"); -#endif - timer->frc1_ctrl = 0; - ETS_FRC1_INTR_DISABLE(); - } - pwm_state.next_set = NULL; - - GPIO_REG_WRITE(GPIO_OUT_W1TS_ADDRESS, (*pwm)[0].on_mask); - GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, (*pwm)[0].off_mask); - - return; - } - - // start if not running - if (!pwm_state.next_set) { -#if PWM_DEBUG - ets_printf("PWM start\n"); -#endif - pwm_state.current_set = pwm_state.next_set = *pwm; - pwm_state.current_phase = phases - 1; - ETS_FRC1_INTR_ENABLE(); - RTC_REG_WRITE(FRC1_LOAD_ADDRESS, 0); - timer->frc1_ctrl = TIMER1_DIVIDE_BY_16 | TIMER1_ENABLE_TIMER; - return; - } - - pwm_state.next_set = *pwm; -} - -void ICACHE_FLASH_ATTR -pwm_set_duty(uint32_t duty, uint8_t channel) -{ - if (channel > PWM_MAX_CHANNELS) - return; - - if (duty > PWM_MAX_DUTY) - duty = PWM_MAX_DUTY; - - pwm_duty[channel] = duty; -} - -uint32_t ICACHE_FLASH_ATTR -pwm_get_duty(uint8_t channel) -{ - if (channel > PWM_MAX_CHANNELS) - return 0; - return pwm_duty[channel]; -} - -void ICACHE_FLASH_ATTR -pwm_set_period(uint32_t period) -{ - pwm_period = period; - - if (pwm_period > PWM_MAX_PERIOD) - pwm_period = PWM_MAX_PERIOD; - - pwm_period_ticks = PWM_PERIOD_TO_TICKS(period); -} - -uint32_t ICACHE_FLASH_ATTR -pwm_get_period(void) -{ - return pwm_period; -} - -uint32_t ICACHE_FLASH_ATTR -get_pwm_version(void) -{ - return 1; -} - -void ICACHE_FLASH_ATTR -set_pwm_debug_en(uint8_t print_en) -{ - (void) print_en; -} diff --git a/code/espurna/libs/pwm.h b/code/espurna/libs/pwm.h index bf5605fb1e..b8bed253ae 100755 --- a/code/espurna/libs/pwm.h +++ b/code/espurna/libs/pwm.h @@ -1,15 +1,32 @@ -#ifndef __PWM_H__ -#define __PWM_H__ - -/*pwm.h: function and macro definition of PWM API , driver level */ -/*user_light.h: user interface for light API, user level*/ -/*user_light_adj: API for color changing and lighting effects, user level*/ +/* + * Copyright (C) 2016 Stefan Brüns + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ +/* Set the following three defines to your needs */ - /*NOTE!! : DO NOT CHANGE THIS FILE*/ +#ifndef __PWM_H__ +#define __PWM_H__ /*SUPPORT UP TO 8 PWM CHANNEL*/ -//#define PWM_CHANNEL_NUM_MAX 8 +//#define PWM_CHANNEL_NUM_MAX 8 + +// ----------------------------------------------------------------------------- +// pwm.h +// ----------------------------------------------------------------------------- struct pwm_param { uint32 period; @@ -17,7 +34,6 @@ struct pwm_param { uint32 duty[PWM_CHANNEL_NUM_MAX]; //PWM_CHANNEL<=8 }; - /* pwm_init should be called only once, for now */ void pwm_init(uint32 period, uint32 *duty,uint32 pwm_channel_num,uint32 (*pin_info_list)[3]); void pwm_start(void); @@ -30,4 +46,436 @@ uint32 pwm_get_period(void); uint32 get_pwm_version(void); void set_pwm_debug_en(uint8 print_en); +// ----------------------------------------------------------------------------- +// pwm.c +// ----------------------------------------------------------------------------- + +#ifndef SDK_PWM_PERIOD_COMPAT_MODE + #define SDK_PWM_PERIOD_COMPAT_MODE 0 +#endif +#ifndef PWM_MAX_CHANNELS + #define PWM_MAX_CHANNELS 8 +#endif +#define PWM_DEBUG 0 +#define PWM_USE_NMI 1 + +/* no user servicable parts beyond this point */ + +#define PWM_MAX_TICKS 0x7fffff +#if SDK_PWM_PERIOD_COMPAT_MODE +#define PWM_PERIOD_TO_TICKS(x) (x * 0.2) +#define PWM_DUTY_TO_TICKS(x) (x * 5) +#define PWM_MAX_DUTY (PWM_MAX_TICKS * 0.2) +#define PWM_MAX_PERIOD (PWM_MAX_TICKS * 5) +#else +#define PWM_PERIOD_TO_TICKS(x) (x) +#define PWM_DUTY_TO_TICKS(x) (x) +#define PWM_MAX_DUTY PWM_MAX_TICKS +#define PWM_MAX_PERIOD PWM_MAX_TICKS +#endif + +#include +#include +#include +#include + +// from SDK hw_timer.c +#define TIMER1_DIVIDE_BY_16 0x0004 +#define TIMER1_ENABLE_TIMER 0x0080 + +struct pwm_phase { + uint32_t ticks; ///< delay until next phase, in 200ns units + uint16_t on_mask; ///< GPIO mask to switch on + uint16_t off_mask; ///< GPIO mask to switch off +}; + +/* Three sets of PWM phases, the active one, the one used + * starting with the next cycle, and the one updated + * by pwm_start. After the update pwm_next_set + * is set to the last updated set. pwm_current_set is set to + * pwm_next_set from the interrupt routine during the first + * pwm phase + */ +typedef struct pwm_phase (pwm_phase_array)[PWM_MAX_CHANNELS + 2]; +static pwm_phase_array pwm_phases[3]; +static struct { + struct pwm_phase* next_set; + struct pwm_phase* current_set; + uint8_t current_phase; +} pwm_state; + +static uint32_t pwm_period; +static uint32_t pwm_period_ticks; +static uint32_t pwm_duty[PWM_MAX_CHANNELS]; +static uint16_t gpio_mask[PWM_MAX_CHANNELS]; +static uint8_t pwm_channels; + +// 3-tuples of MUX_REGISTER, MUX_VALUE and GPIO number +typedef uint32_t (pin_info_type)[3]; + +struct gpio_regs { + uint32_t out; /* 0x60000300 */ + uint32_t out_w1ts; /* 0x60000304 */ + uint32_t out_w1tc; /* 0x60000308 */ + uint32_t enable; /* 0x6000030C */ + uint32_t enable_w1ts; /* 0x60000310 */ + uint32_t enable_w1tc; /* 0x60000314 */ + uint32_t in; /* 0x60000318 */ + uint32_t status; /* 0x6000031C */ + uint32_t status_w1ts; /* 0x60000320 */ + uint32_t status_w1tc; /* 0x60000324 */ +}; +static struct gpio_regs* gpio = (struct gpio_regs*)(0x60000300); + +struct timer_regs { + uint32_t frc1_load; /* 0x60000600 */ + uint32_t frc1_count; /* 0x60000604 */ + uint32_t frc1_ctrl; /* 0x60000608 */ + uint32_t frc1_int; /* 0x6000060C */ + uint8_t pad[16]; + uint32_t frc2_load; /* 0x60000620 */ + uint32_t frc2_count; /* 0x60000624 */ + uint32_t frc2_ctrl; /* 0x60000628 */ + uint32_t frc2_int; /* 0x6000062C */ + uint32_t frc2_alarm; /* 0x60000630 */ +}; +static struct timer_regs* timer = (struct timer_regs*)(0x60000600); + +static void ICACHE_RAM_ATTR +pwm_intr_handler(void) +{ + if ((pwm_state.current_set[pwm_state.current_phase].off_mask == 0) && + (pwm_state.current_set[pwm_state.current_phase].on_mask == 0)) { + pwm_state.current_set = pwm_state.next_set; + pwm_state.current_phase = 0; + } + + do { + // force write to GPIO registers on each loop + asm volatile ("" : : : "memory"); + + gpio->out_w1ts = (uint32_t)(pwm_state.current_set[pwm_state.current_phase].on_mask); + gpio->out_w1tc = (uint32_t)(pwm_state.current_set[pwm_state.current_phase].off_mask); + + uint32_t ticks = pwm_state.current_set[pwm_state.current_phase].ticks; + + pwm_state.current_phase++; + + if (ticks) { + if (ticks >= 16) { + // constant interrupt overhead + ticks -= 9; + timer->frc1_int &= ~FRC1_INT_CLR_MASK; + WRITE_PERI_REG(&timer->frc1_load, ticks); + return; + } + + ticks *= 4; + do { + ticks -= 1; + // stop compiler from optimizing delay loop to noop + asm volatile ("" : : : "memory"); + } while (ticks > 0); + } + + } while (1); +} + +/** + * period: initial period (base unit 1us OR 200ns) + * duty: array of initial duty values, may be NULL, may be freed after pwm_init + * pwm_channel_num: number of channels to use + * pin_info_list: array of pin_info + */ +void ICACHE_FLASH_ATTR +pwm_init(uint32_t period, uint32_t *duty, uint32_t pwm_channel_num, + uint32_t (*pin_info_list)[3]) +{ + int i, j, n; + + pwm_channels = pwm_channel_num; + if (pwm_channels > PWM_MAX_CHANNELS) + pwm_channels = PWM_MAX_CHANNELS; + + for (i = 0; i < 3; i++) { + for (j = 0; j < (PWM_MAX_CHANNELS + 2); j++) { + pwm_phases[i][j].ticks = 0; + pwm_phases[i][j].on_mask = 0; + pwm_phases[i][j].off_mask = 0; + } + } + pwm_state.current_set = pwm_state.next_set = 0; + pwm_state.current_phase = 0; + + uint32_t all = 0; + // PIN info: MUX-Register, Mux-Setting, PIN-Nr + for (n = 0; n < pwm_channels; n++) { + pin_info_type* pin_info = &pin_info_list[n]; + PIN_FUNC_SELECT((*pin_info)[0], (*pin_info)[1]); + gpio_mask[n] = 1 << (*pin_info)[2]; + all |= 1 << (*pin_info)[2]; + if (duty) + pwm_set_duty(duty[n], n); + } + GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, all); + GPIO_REG_WRITE(GPIO_ENABLE_W1TS_ADDRESS, all); + + pwm_set_period(period); + +#if PWM_USE_NMI + ETS_FRC_TIMER1_NMI_INTR_ATTACH(pwm_intr_handler); +#else + ETS_FRC_TIMER1_INTR_ATTACH(pwm_intr_handler, NULL); +#endif + TM1_EDGE_INT_ENABLE(); + + timer->frc1_int &= ~FRC1_INT_CLR_MASK; + timer->frc1_ctrl = 0; + + pwm_start(); +} + +__attribute__ ((noinline)) +static uint8_t ICACHE_FLASH_ATTR +_pwm_phases_prep(struct pwm_phase* pwm) +{ + uint8_t n, phases; + + for (n = 0; n < pwm_channels + 2; n++) { + pwm[n].ticks = 0; + pwm[n].on_mask = 0; + pwm[n].off_mask = 0; + } + phases = 1; + for (n = 0; n < pwm_channels; n++) { + uint32_t ticks = PWM_DUTY_TO_TICKS(pwm_duty[n]); + if (ticks == 0) { + pwm[0].off_mask |= gpio_mask[n]; + } else if (ticks >= pwm_period_ticks) { + pwm[0].on_mask |= gpio_mask[n]; + } else { + if (ticks < (pwm_period_ticks/2)) { + pwm[phases].ticks = ticks; + pwm[0].on_mask |= gpio_mask[n]; + pwm[phases].off_mask = gpio_mask[n]; + } else { + pwm[phases].ticks = pwm_period_ticks - ticks; + pwm[phases].on_mask = gpio_mask[n]; + pwm[0].off_mask |= gpio_mask[n]; + } + phases++; + } + } + pwm[phases].ticks = pwm_period_ticks; + + // bubble sort, lowest to hightest duty + n = 2; + while (n < phases) { + if (pwm[n].ticks < pwm[n - 1].ticks) { + struct pwm_phase t = pwm[n]; + pwm[n] = pwm[n - 1]; + pwm[n - 1] = t; + if (n > 2) + n--; + } else { + n++; + } + } + +#if PWM_DEBUG + int t = 0; + for (t = 0; t <= phases; t++) { + ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); + } +#endif + + // shift left to align right edge; + uint8_t l = 0, r = 1; + while (r <= phases) { + uint32_t diff = pwm[r].ticks - pwm[l].ticks; + if (diff && (diff <= 16)) { + uint16_t mask = pwm[r].on_mask | pwm[r].off_mask; + pwm[l].off_mask ^= pwm[r].off_mask; + pwm[l].on_mask ^= pwm[r].on_mask; + pwm[0].off_mask ^= pwm[r].on_mask; + pwm[0].on_mask ^= pwm[r].off_mask; + pwm[r].ticks = pwm_period_ticks - diff; + pwm[r].on_mask ^= mask; + pwm[r].off_mask ^= mask; + } else { + l = r; + } + r++; + } + +#if PWM_DEBUG + for (t = 0; t <= phases; t++) { + ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); + } +#endif + + // sort again + n = 2; + while (n <= phases) { + if (pwm[n].ticks < pwm[n - 1].ticks) { + struct pwm_phase t = pwm[n]; + pwm[n] = pwm[n - 1]; + pwm[n - 1] = t; + if (n > 2) + n--; + } else { + n++; + } + } + + // merge same duty + l = 0, r = 1; + while (r <= phases) { + if (pwm[r].ticks == pwm[l].ticks) { + pwm[l].off_mask |= pwm[r].off_mask; + pwm[l].on_mask |= pwm[r].on_mask; + pwm[r].on_mask = 0; + pwm[r].off_mask = 0; + } else { + l++; + if (l != r) { + struct pwm_phase t = pwm[l]; + pwm[l] = pwm[r]; + pwm[r] = t; + } + } + r++; + } + phases = l; + +#if PWM_DEBUG + for (t = 0; t <= phases; t++) { + ets_printf("%d @%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); + } +#endif + + // transform absolute end time to phase durations + for (n = 0; n < phases; n++) { + pwm[n].ticks = + pwm[n + 1].ticks - pwm[n].ticks; + // subtract common overhead + pwm[n].ticks--; + } + pwm[phases].ticks = 0; + + // do a cyclic shift if last phase is short + if (pwm[phases - 1].ticks < 16) { + for (n = 0; n < phases - 1; n++) { + struct pwm_phase t = pwm[n]; + pwm[n] = pwm[n + 1]; + pwm[n + 1] = t; + } + } + +#if PWM_DEBUG + for (t = 0; t <= phases; t++) { + ets_printf("%d +%d: %04x %04x\n", t, pwm[t].ticks, pwm[t].on_mask, pwm[t].off_mask); + } + ets_printf("\n"); +#endif + + return phases; +} + +void ICACHE_FLASH_ATTR +pwm_start(void) +{ + pwm_phase_array* pwm = &pwm_phases[0]; + + if ((*pwm == pwm_state.next_set) || + (*pwm == pwm_state.current_set)) + pwm++; + if ((*pwm == pwm_state.next_set) || + (*pwm == pwm_state.current_set)) + pwm++; + + uint8_t phases = _pwm_phases_prep(*pwm); + + // all with 0% / 100% duty - stop timer + if (phases == 1) { + if (pwm_state.next_set) { +#if PWM_DEBUG + ets_printf("PWM stop\n"); +#endif + timer->frc1_ctrl = 0; + ETS_FRC1_INTR_DISABLE(); + } + pwm_state.next_set = NULL; + + GPIO_REG_WRITE(GPIO_OUT_W1TS_ADDRESS, (*pwm)[0].on_mask); + GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, (*pwm)[0].off_mask); + + return; + } + + // start if not running + if (!pwm_state.next_set) { +#if PWM_DEBUG + ets_printf("PWM start\n"); +#endif + pwm_state.current_set = pwm_state.next_set = *pwm; + pwm_state.current_phase = phases - 1; + ETS_FRC1_INTR_ENABLE(); + RTC_REG_WRITE(FRC1_LOAD_ADDRESS, 0); + timer->frc1_ctrl = TIMER1_DIVIDE_BY_16 | TIMER1_ENABLE_TIMER; + return; + } + + pwm_state.next_set = *pwm; +} + +void ICACHE_FLASH_ATTR +pwm_set_duty(uint32_t duty, uint8_t channel) +{ + if (channel > PWM_MAX_CHANNELS) + return; + + if (duty > PWM_MAX_DUTY) + duty = PWM_MAX_DUTY; + + pwm_duty[channel] = duty; +} + +uint32_t ICACHE_FLASH_ATTR +pwm_get_duty(uint8_t channel) +{ + if (channel > PWM_MAX_CHANNELS) + return 0; + return pwm_duty[channel]; +} + +void ICACHE_FLASH_ATTR +pwm_set_period(uint32_t period) +{ + pwm_period = period; + + if (pwm_period > PWM_MAX_PERIOD) + pwm_period = PWM_MAX_PERIOD; + + pwm_period_ticks = PWM_PERIOD_TO_TICKS(period); +} + +uint32_t ICACHE_FLASH_ATTR +pwm_get_period(void) +{ + return pwm_period; +} + +uint32_t ICACHE_FLASH_ATTR +get_pwm_version(void) +{ + return 1; +} + +void ICACHE_FLASH_ATTR +set_pwm_debug_en(uint8_t print_en) +{ + (void) print_en; +} + #endif diff --git a/code/espurna/web.ino b/code/espurna/web.ino index 8e7855a718..08dabfab65 100644 --- a/code/espurna/web.ino +++ b/code/espurna/web.ino @@ -21,7 +21,7 @@ Copyright (C) 2016-2018 by Xose Pérez #include "static/index.light.html.gz.h" #elif SENSOR_SUPPORT #include "static/index.sensor.html.gz.h" -#elif ITEAD_SONOFF_RFBRIDGE +#elif defined(ITEAD_SONOFF_RFBRIDGE) #include "static/index.rfbridge.html.gz.h" #else #include "static/index.small.html.gz.h"