static void surround_update_filter(unsigned int fout)
{
tcoef1 = fp_div(cutoff_l, fout, 31);
tcoef2 = fp_div(cutoff_h, fout, 31);
bcoef = fp_div(cutoff_l / 2, fout, 31);
hcoef = fp_div(cutoff_h * 2, fout, 31);
}
/* c = [a, b] */
void fp_lcm(fp_int *a, fp_int *b, fp_int *c)
{
fp_int t1, t2;
fp_init(&t1);
fp_init(&t2);
fp_gcd(a, b, &t1);
if (fp_cmp_mag(a, b) == FP_GT) {
fp_div(a, &t1, &t2, NULL);
fp_mul(b, &t2, c);
} else {
fp_div(b, &t1, &t2, NULL);
fp_mul(a, &t2, c);
}
}
fp_t arc_cos(fp_t x)
{
int i;
/* Cap x if out of range. */
if (x < FLOAT_TO_FP(-1.0))
x = FLOAT_TO_FP(-1.0);
else if (x > FLOAT_TO_FP(1.0))
x = FLOAT_TO_FP(1.0);
/*
* Increment through lookup table to find index and then linearly
* interpolate for precision.
*/
/* TODO(crosbug.com/p/25600): Optimize with binary search. */
for (i = 0; i < COSINE_LUT_SIZE-1; i++) {
if (x >= cos_lut[i+1]) {
const fp_t interp = fp_div(cos_lut[i] - x,
cos_lut[i] - cos_lut[i + 1]);
return fp_mul(INT_TO_FP(COSINE_LUT_INCR_DEG),
INT_TO_FP(i) + interp);
}
}
/*
* Shouldn't be possible to get here because inputs are clipped to
* [-1, 1] and the cos_lut[] table goes over the same range. If we
* are here, throw an assert.
*/
ASSERT(0);
return 0;
}
void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
struct motion_sensor_t *sensor,
int valid_data)
{
struct ec_response_motion_sensor_data vector;
int i;
data->sensor_num = sensor - motion_sensors;
mutex_lock(&g_sensor_mutex);
if (queue_space(&motion_sense_fifo) == 0) {
queue_remove_unit(&motion_sense_fifo, &vector);
motion_sense_fifo_lost++;
motion_sensors[vector.sensor_num].lost++;
if (vector.flags & MOTIONSENSE_SENSOR_FLAG_FLUSH)
CPRINTS("Lost flush for sensor %d", vector.sensor_num);
}
for (i = 0; i < valid_data; i++)
sensor->xyz[i] = data->data[i];
mutex_unlock(&g_sensor_mutex);
if (valid_data) {
int ap_odr = sensor->config[SENSOR_CONFIG_AP].odr &
~ROUND_UP_FLAG;
int rate = INT_TO_FP(sensor->drv->get_data_rate(sensor));
/* If the AP does not want sensor info, skip */
if (ap_odr == 0)
return;
/* Skip if EC is oversampling */
if (sensor->oversampling < 0) {
sensor->oversampling += fp_div(INT_TO_FP(1000), rate);
return;
}
sensor->oversampling += fp_div(INT_TO_FP(1000), rate) -
fp_div(INT_TO_FP(1000), INT_TO_FP(ap_odr));
}
queue_add_unit(&motion_sense_fifo, data);
}
static void pbe_update_filter(unsigned int fout)
{
tcoef1 = fp_div(160, fout, 31);
tcoef2 = fp_div(500, fout, 31);
tcoef3 = fp_div(1150, fout, 31);
/* Biophonic EQ */
filter_bishelf_coefs(fp_div(20, fout, 32),
fp_div(16000, fout, 32),
0, 53, -5 + pbe_precut,
&pbe_filter[0]);
filter_pk_coefs(fp_div(64, fout, 32), 28, 53,
&pbe_filter[1]);
filter_pk_coefs(fp_div(2000, fout, 32), 28, 58,
&pbe_filter[2]);
filter_pk_coefs(fp_div(7500, fout, 32), 43, -82,
&pbe_filter[3]);
filter_pk_coefs(fp_div(10000, fout, 32), 43, -29,
&pbe_filter[4]);
}
/**
* Calculate first order shelving filter. Filter is not directly usable by the
* filter_process() function.
* @param cutoff shelf midpoint frequency. See eq_pk_coefs for format.
* @param A decibel value multiplied by ten, describing gain/attenuation of
* shelf. Max value is 24 dB.
* @param low true for low-shelf filter, false for high-shelf filter.
* @param c pointer to coefficient storage. Coefficients are s4.27 format.
*/
void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c)
{
long sin, cos;
int32_t b0, b1, a0, a1; /* s3.28 */
const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */
sin = fp_sincos(cutoff/2, &cos);
if (low) {
const int32_t sin_div_g = fp_div(sin, g, 25);
const int32_t sin_g = FRACMUL(sin, g);
cos >>= 3;
b0 = sin_g + cos; /* 0.25 .. 4.10 */
b1 = sin_g - cos; /* -1 .. 3.98 */
a0 = sin_div_g + cos; /* 0.25 .. 4.10 */
a1 = sin_div_g - cos; /* -1 .. 3.98 */
} else {
/* c = a mod b, 0 <= c < b */
int fp_mod(fp_int *a, fp_int *b, fp_int *c)
{
fp_int t;
int err;
fp_zero(&t);
if ((err = fp_div(a, b, NULL, &t)) != FP_OKAY) {
return err;
}
if (t.sign != b->sign) {
fp_add(&t, b, c);
} else {
fp_copy(&t, c);
}
return FP_OKAY;
}
int
main()
{
if (fp_add (1, 1) != 2) fail ("fp_add 1+1");
if (fp_sub (3, 2) != 1) fail ("fp_sub 3-2");
if (fp_mul (2, 3) != 6) fail ("fp_mul 2*3");
if (fp_div (3, 2) != 1.5) fail ("fp_div 3/2");
if (fp_neg (1) != -1) fail ("fp_neg 1");
if (dp_add (1, 1) != 2) fail ("dp_add 1+1");
if (dp_sub (3, 2) != 1) fail ("dp_sub 3-2");
if (dp_mul (2, 3) != 6) fail ("dp_mul 2*3");
if (dp_div (3, 2) != 1.5) fail ("dp_div 3/2");
if (dp_neg (1) != -1) fail ("dp_neg 1");
if (fp_to_dp (1.5) != 1.5) fail ("fp_to_dp 1.5");
if (dp_to_fp (1.5) != 1.5) fail ("dp_to_fp 1.5");
if (floatsisf (1) != 1) fail ("floatsisf 1");
if (floatsidf (1) != 1) fail ("floatsidf 1");
if (fixsfsi (1.42) != 1) fail ("fixsfsi 1.42");
if (fixunssfsi (1.42) != 1) fail ("fixunssfsi 1.42");
if (fixdfsi (1.42) != 1) fail ("fixdfsi 1.42");
if (fixunsdfsi (1.42) != 1) fail ("fixunsdfsi 1.42");
if (eqsf2 (1, 1) == 0) fail ("eqsf2 1==1");
if (eqsf2 (1, 2) != 0) fail ("eqsf2 1==2");
if (nesf2 (1, 2) == 0) fail ("nesf2 1!=1");
if (nesf2 (1, 1) != 0) fail ("nesf2 1!=1");
if (gtsf2 (2, 1) == 0) fail ("gtsf2 2>1");
if (gtsf2 (1, 1) != 0) fail ("gtsf2 1>1");
if (gtsf2 (0, 1) != 0) fail ("gtsf2 0>1");
if (gesf2 (2, 1) == 0) fail ("gesf2 2>=1");
if (gesf2 (1, 1) == 0) fail ("gesf2 1>=1");
if (gesf2 (0, 1) != 0) fail ("gesf2 0>=1");
if (ltsf2 (1, 2) == 0) fail ("ltsf2 1<2");
if (ltsf2 (1, 1) != 0) fail ("ltsf2 1<1");
if (ltsf2 (1, 0) != 0) fail ("ltsf2 1<0");
if (lesf2 (1, 2) == 0) fail ("lesf2 1<=2");
if (lesf2 (1, 1) == 0) fail ("lesf2 1<=1");
if (lesf2 (1, 0) != 0) fail ("lesf2 1<=0");
if (fail_count != 0)
abort ();
exit (0);
}
/* Computes log(x) and stores the result in out.
* The current method of computation is as follows:
* 1) Separate x into the form y * 10^n, where 1 <= y < 10.
* 2) Approximate log(y) using a 5/5 Pade approximant.
* 3) Compute log(10^n) via n * log(10).
* 4) Since log(ab) = log(a) + log(b), compute final result by:
* log(y) + n log(10).
*/
fp_t fp_log(fp_t x) {
fp_t num, div, tmp, p10;
/* Extract the fractional part of x. */
tmp = x;
tmp.expt = 0x7f;
/* Compute the quotient approximation of frac(x) */
num = fp_poly(QUOT_NUMERATOR, sizeof QUOT_NUMERATOR / sizeof QUOT_NUMERATOR[0], tmp);
div = fp_poly(QUOT_DENOMINATOR, sizeof QUOT_DENOMINATOR / sizeof QUOT_DENOMINATOR[0], tmp);
/* Compute the quotient. */
tmp = fp_div(num, div);
p10 = fp_fromint(x.expt - 0x7f);
p10 = fp_mul(p10, FP_LOG10);
tmp = fp_add(tmp, p10);
return tmp;
}
int main() {
int count = numbers_cnt * numbers_cnt;
int errors = 0;
for (int i = 0; i < numbers_cnt; i++) {
for (int j = 0; j < numbers_cnt; j++) {
uint32_t v = v_div(numbers[i], numbers[j]);
uint32_t fp = fp_div(numbers[i], numbers[j]);
if (v != fp && errors < 10) {
printf("div: %08x %08x => v %08x | fp %08x\n", numbers[i], numbers[j], v, fp);
}
errors += fp != v;
}
if ((i % 500) == 0) {
int p = count * 100 / numbers_cnt / numbers_cnt;
printf("div: %d%% (%d errors)\n", p, errors);
}
}
printf("div: errors: %d tests: %d\n", errors, count);
return errors != 0;
}
{
long sin, cos;
int32_t b0, b1, a0, a1; /* s3.28 */
const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */
sin = fp_sincos(cutoff/2, &cos);
if (low) {
const int32_t sin_div_g = fp_div(sin, g, 25);
const int32_t sin_g = FRACMUL(sin, g);
cos >>= 3;
b0 = sin_g + cos; /* 0.25 .. 4.10 */
b1 = sin_g - cos; /* -1 .. 3.98 */
a0 = sin_div_g + cos; /* 0.25 .. 4.10 */
a1 = sin_div_g - cos; /* -1 .. 3.98 */
} else {
const int32_t cos_div_g = fp_div(cos, g, 25);
const int32_t cos_g = FRACMUL(cos, g);
sin >>= 3;
b0 = sin + cos_g; /* 0.25 .. 4.10 */
b1 = sin - cos_g; /* -3.98 .. 1 */
a0 = sin + cos_div_g; /* 0.25 .. 4.10 */
a1 = sin - cos_div_g; /* -3.98 .. 1 */
}
const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */
*c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */
*c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */
*c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */
}
/**
* Calculate the lid angle using two acceleration vectors, one recorded in
* the base and one in the lid.
*
* @param base Base accel vector
* @param lid Lid accel vector
* @param lid_angle Pointer to location to store lid angle result
*
* @return flag representing if resulting lid angle calculation is reliable.
*/
static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
int *lid_angle)
{
vector_3_t v;
fp_t ang_lid_to_base, cos_lid_90, cos_lid_270;
fp_t lid_to_base, base_to_hinge;
fp_t denominator;
int reliable = 1;
/*
* The angle between lid and base is:
* acos((cad(base, lid) - cad(base, hinge)^2) /(1 - cad(base, hinge)^2))
* where cad() is the cosine_of_angle_diff() function.
*
* Make sure to check for divide by 0.
*/
lid_to_base = cosine_of_angle_diff(base, lid);
base_to_hinge = cosine_of_angle_diff(base, p_acc_orient->hinge_axis);
/*
* If hinge aligns too closely with gravity, then result may be
* unreliable.
*/
if (fp_abs(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
reliable = 0;
base_to_hinge = fp_sq(base_to_hinge);
/* Check divide by 0. */
denominator = FLOAT_TO_FP(1.0) - base_to_hinge;
if (fp_abs(denominator) < FLOAT_TO_FP(0.01)) {
*lid_angle = 0;
return 0;
}
ang_lid_to_base = arc_cos(fp_div(lid_to_base - base_to_hinge,
denominator));
/*
* The previous calculation actually has two solutions, a positive and
* a negative solution. To figure out the sign of the answer, calculate
* the cosine of the angle between the actual lid angle and the
* estimated vector if the lid were open to 90 deg, cos_lid_90. Also
* calculate the cosine of the angle between the actual lid angle and
* the estimated vector if the lid were open to 270 deg,
* cos_lid_270. The smaller of the two angles represents which one is
* closer. If the lid is closer to the estimated 270 degree vector then
* the result is negative, otherwise it is positive.
*/
rotate(base, p_acc_orient->rot_hinge_90, v);
cos_lid_90 = cosine_of_angle_diff(v, lid);
rotate(v, p_acc_orient->rot_hinge_180, v);
cos_lid_270 = cosine_of_angle_diff(v, lid);
/*
* Note that cos_lid_90 and cos_lid_270 are not in degrees, because
* the arc_cos() was never performed. But, since arc_cos() is
* monotonically decreasing, we can do this comparison without ever
* taking arc_cos(). But, since the function is monotonically
* decreasing, the logic of this comparison is reversed.
*/
if (cos_lid_270 > cos_lid_90)
ang_lid_to_base = -ang_lid_to_base;
/* Place lid angle between 0 and 360 degrees. */
if (ang_lid_to_base < 0)
ang_lid_to_base += FLOAT_TO_FP(360);
/*
* Round to nearest int by adding 0.5. Note, only works because lid
* angle is known to be positive.
*/
*lid_angle = FP_TO_INT(ang_lid_to_base + FLOAT_TO_FP(0.5));
return reliable;
}
/* div */
static int divide(void *a, void *b, void *c, void *d)
{
LTC_ARGCHK(a != NULL);
LTC_ARGCHK(b != NULL);
return tfm_to_ltc_error(fp_div(a, b, c, d));
}