/* Copyright 2014 The ChromiumOS Authors * Use of this source code is governed by a BSD-style license that can be * found in the LICENSE file. */ /* Motion sense module to read from various motion sensors. */ #include "accelgyro.h" #include "acpi.h" #include "chipset.h" #include "common.h" #include "console.h" #include "gesture.h" #include "hooks.h" #include "host_command.h" #include "lid_angle.h" #include "lid_switch.h" #include "math_util.h" #include "motion_lid.h" #include "motion_sense.h" #include "power.h" #include "tablet_mode.h" #include "task.h" #include "timer.h" #include "util.h" /* Console output macros */ #define CPUTS(outstr) cputs(CC_MOTION_LID, outstr) #define CPRINTS(format, args...) cprints(CC_MOTION_LID, format, ##args) #define CPRINTF(format, args...) cprintf(CC_MOTION_LID, format, ##args) #ifdef CONFIG_TABLET_MODE /* Previous lid_angle. */ static fp_t last_lid_angle_fp = FLOAT_TO_FP(-1); /* * This defines the range from 0 to SMALL_LID_ANGLE_RANGE of possible lid angle * measurements when the lid is physically closed. This will be used in * reliability calculations. */ #define SMALL_LID_ANGLE_RANGE (FLOAT_TO_FP(15)) #endif /* Current acceleration vectors and current lid angle. */ static int lid_angle_deg; static int lid_angle_is_reliable; /* Smoothed vectors to increase accurency. */ static intv3_t smoothed_base, smoothed_lid; /* 8.7 m/s^2 is the the maximum acceleration parallel to the hinge */ #define SCALED_HINGE_VERTICAL_MAXIMUM \ ((int)((8.7f * MOTION_SCALING_FACTOR) / MOTION_ONE_G)) #define SCALED_HINGE_VERTICAL_SMOOTHING_START \ ((int)((7.0f * MOTION_SCALING_FACTOR) / MOTION_ONE_G)) /* * Constant to debounce lid angle changes around 360 - 0: * If we have a rotation through the angle 0, ignore. */ #define DEBOUNCE_ANGLE_DELTA FLOAT_TO_FP(45) /* * Since the accelerometers are on the same physical device, they should be * under the same acceleration. This constant, which mirrors * kNoisyMagnitudeDeviation used in Chromium, is an integer which defines the * maximum deviation in magnitude between the base and lid vectors. The units * are in g. Currently set at 1m/s^2. */ #define NOISY_MAGNITUDE_DEVIATION ((int)(MOTION_SCALING_FACTOR / MOTION_ONE_G)) /* * Even with noise, any measurement greater than 1g on any axis is not suitable * for lid calculation. It means the device is moving. * To avoid using 64bits arithmetic, we need to be sure that square of magnitude * is less than 1<<31, so magnitude is less sqrt(2)*(1<<15), less than ~40% over * 1g. This is way above any usable noise. Assume noise is less than 10%. */ #define MOTION_SCALING_AXIS_MAX ((MOTION_SCALING_FACTOR * 110) / 100) #define MOTION_SCALING_FACTOR2 (MOTION_SCALING_FACTOR * MOTION_SCALING_FACTOR) /* * Define the accelerometer orientation matrices based on the standard * reference frame in use (note: accel data is converted to standard ref * frame before calculating lid angle). */ #ifdef CONFIG_ACCEL_STD_REF_FRAME_OLD static const intv3_t hinge_axis = { 0, 1, 0 }; #define HINGE_AXIS Y #else static const intv3_t hinge_axis = { 1, 0, 0 }; #define HINGE_AXIS X #endif static const struct motion_sensor_t *const accel_base = &motion_sensors[CONFIG_LID_ANGLE_SENSOR_BASE]; static const struct motion_sensor_t *const accel_lid = &motion_sensors[CONFIG_LID_ANGLE_SENSOR_LID]; STATIC_IF(CONFIG_TABLET_MODE) void motion_lid_set_tablet_mode(int reliable); STATIC_IF(CONFIG_TABLET_MODE) int lid_angle_set_tablet_mode_threshold(int angle, int hys); STATIC_IF(CONFIG_TABLET_MODE) fp_t tablet_zone_lid_angle; STATIC_IF(CONFIG_TABLET_MODE) fp_t laptop_zone_lid_angle; STATIC_IF(CONFIG_TABLET_MODE) int tablet_mode_lid_angle; STATIC_IF(CONFIG_TABLET_MODE) int tablet_mode_hys_degree; #ifdef CONFIG_TABLET_MODE __attribute__((weak)) int board_is_lid_angle_tablet_mode(void) { return 1; } /* * We are in tablet mode when the lid angle has been calculated * to be large. * * By default, at boot, we are in tablet mode. * Once a lid angle is calculated, we will get out of this fake state and enter * tablet mode only if a high angle has been calculated. * * There might be false positives: * - when the EC enters RO or RW mode. * - when lid is closed while the hinge is perpendicular to the floor, we will * stay in tablet mode. * * Tablet mode is defined as the lid angle being greater than 180 degree(by * default). We use 2 threshold to calculate tablet mode: * tablet_mode: * 1 | +-----<----+---------- * | \/ /\ * | | | * 0 |------------------>----+ * +------------+----------+----------+ lid angle * 0 160 200 360 * * Host can configure the threshold to be different than default of 180 +/- 20 * by using MOTIONSENSE_CMD_TABLET_MODE_LID_ANGLE. */ #define DEFAULT_TABLET_MODE_ANGLE (180) #define DEFAULT_TABLET_MODE_HYS (20) #define TABLET_ZONE_ANGLE(a, h) ((a) + (h)) #define LAPTOP_ZONE_ANGLE(a, h) ((a) - (h)) static fp_t tablet_zone_lid_angle = FLOAT_TO_FP( TABLET_ZONE_ANGLE(DEFAULT_TABLET_MODE_ANGLE, DEFAULT_TABLET_MODE_HYS)); static fp_t laptop_zone_lid_angle = FLOAT_TO_FP( LAPTOP_ZONE_ANGLE(DEFAULT_TABLET_MODE_ANGLE, DEFAULT_TABLET_MODE_HYS)); static int tablet_mode_lid_angle = DEFAULT_TABLET_MODE_ANGLE; static int tablet_mode_hys_degree = DEFAULT_TABLET_MODE_HYS; static void motion_lid_set_tablet_mode(int reliable) { static int tablet_mode_debounce_cnt = TABLET_MODE_DEBOUNCE_COUNT; const int current_mode = tablet_get_mode(); int new_mode = current_mode; if (reliable) { if (last_lid_angle_fp > tablet_zone_lid_angle) new_mode = 1; else if (last_lid_angle_fp < laptop_zone_lid_angle) new_mode = 0; /* Only change tablet mode if we're sure. */ if (current_mode != new_mode) { if (tablet_mode_debounce_cnt == 0) { /* Alright, we're convinced. */ tablet_mode_debounce_cnt = TABLET_MODE_DEBOUNCE_COUNT; tablet_set_mode(new_mode, TABLET_TRIGGER_LID); return; } tablet_mode_debounce_cnt--; return; } } /* * If we got a reliable measurement that agrees with our current tablet * mode, then reset the debounce counter. Also, make it harder to leave * tablet mode by resetting the debounce count when we encounter an * unreliable angle when we're already in tablet mode. */ if (((reliable == 0) && current_mode == 1) || ((reliable == 1) && (current_mode == new_mode))) tablet_mode_debounce_cnt = TABLET_MODE_DEBOUNCE_COUNT; } static int lid_angle_set_tablet_mode_threshold(int angle, int hys) { if ((angle == EC_MOTION_SENSE_NO_VALUE) || (hys == EC_MOTION_SENSE_NO_VALUE)) return EC_RES_SUCCESS; if ((angle < 0) || (hys < 0) || (angle < hys) || ((angle + hys) > 360)) return EC_RES_INVALID_PARAM; tablet_mode_lid_angle = angle; tablet_mode_hys_degree = hys; tablet_zone_lid_angle = INT_TO_FP(TABLET_ZONE_ANGLE(angle, hys)); laptop_zone_lid_angle = INT_TO_FP(LAPTOP_ZONE_ANGLE(angle, hys)); return EC_RES_SUCCESS; } #endif /* CONFIG_TABLET_MODE */ #if defined(CONFIG_DPTF_MULTI_PROFILE) && \ defined(CONFIG_DPTF_MOTION_LID_NO_GMR_SENSOR) #define MOTION_LID_SET_DPTF_PROFILE #endif STATIC_IF(MOTION_LID_SET_DPTF_PROFILE) void motion_lid_set_dptf_profile(int reliable); #ifdef MOTION_LID_SET_DPTF_PROFILE /* * If CONFIG_DPTF_MULTI_PROFILE is defined by a board, then lid motion driver * sets different profile numbers depending upon the current lid * angle. Following profiles are currently supported by this driver: * 1. Clamshell mode - DPTF_PROFILE_CLAMSHELL * 2. 360-degree flipped mode - DPTF_PROFILE_FLIPPED_360_MODE * * 360-degree flipped mode is defined as the mode with base being behind the * lid. We use 2 threshold to calculate this: * * 360-degree mode * 1 | +-----<----+---------- * | \/ /\ * | | | * 0 |------------------------>----+ * +------------------+----------+----------+ lid angle * 0 240 300 360 */ #define FLIPPED_360_ZONE_LID_ANGLE FLOAT_TO_FP(300) #define CLAMSHELL_ZONE_LID_ANGLE FLOAT_TO_FP(240) /* * Detection of DPTF profile is very similar to tablet mode detection using * debounce counter. This is done to avoid any spurious changes in setting DPTF * profile numbers. */ #define DPTF_MODE_DEBOUNCE_COUNT 3 static void motion_lid_set_dptf_profile(int reliable) { static int debounce_cnt = DPTF_MODE_DEBOUNCE_COUNT; int current_prof = acpi_dptf_get_profile_num(); int new_prof = current_prof; if (reliable) { if (last_lid_angle_fp > FLIPPED_360_ZONE_LID_ANGLE) new_prof = DPTF_PROFILE_FLIPPED_360_MODE; else if (last_lid_angle_fp < CLAMSHELL_ZONE_LID_ANGLE) new_prof = DPTF_PROFILE_CLAMSHELL; if (current_prof != new_prof) { if (debounce_cnt != 0) { debounce_cnt--; return; } debounce_cnt = DPTF_MODE_DEBOUNCE_COUNT; acpi_dptf_set_profile_num(new_prof); return; } } debounce_cnt = DPTF_MODE_DEBOUNCE_COUNT; } #endif /* MOTION_LID_SET_DPTF_PROFILE */ /** * 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 intv3_t base, const intv3_t lid, int *lid_angle) { intv3_t cross, proj_lid, proj_base, scaled_base, scaled_lid; fp_t lid_to_base_fp, smoothed_ratio; int base_magnitude2, lid_magnitude2, largest_hinge_accel; int reliable = 1, i; /* * Scale the vectors by their range, to be able to compare them. * If a single measurement is greated than 1g, we may overflow fixed * point calculation. However, we can exclude such a measurement, it * means the device is in movement and lid angle calculation is not * possible. */ for (i = X; i <= Z; i++) { scaled_base[i] = base[i] * accel_base->current_range; scaled_lid[i] = lid[i] * accel_lid->current_range; if (ABS(scaled_base[i]) > MOTION_SCALING_AXIS_MAX || ABS(scaled_lid[i]) > MOTION_SCALING_AXIS_MAX) { reliable = 0; goto end_calculate_lid_angle; } } /* * Calculate square of vector magnitude in g. * Each entry is guaranteed to be up to +/- 1<<15, so the square will be * less than 1<<30. */ base_magnitude2 = scaled_base[X] * scaled_base[X] + scaled_base[Y] * scaled_base[Y] + scaled_base[Z] * scaled_base[Z]; lid_magnitude2 = scaled_lid[X] * scaled_lid[X] + scaled_lid[Y] * scaled_lid[Y] + scaled_lid[Z] * scaled_lid[Z]; /* * Check to see if they differ than more than NOISY_MAGNITUDE_DEVIATION. * If the vectors do, then the measured angle is unreliable. * * Note, that we don't actually have to take the square root to get the * magnitude, but we can work with the magnitudes squared directly as * shown below: * * If A is a magnitudes, and x is the noisy magnitude deviation: * * 0 < 1g - A < x * 0 < 1g^2 - A^2 < x * (A + B) * 0 < 1g^2 - A^2 < 2 * x * avg(A, B) * * If we assume that the average acceleration should be about 1g, then * we have: * * 0 < 1g^2 - A^2 < 2 * 1g * NOISY_MAGNITUDE_DEVIATION */ if (MOTION_SCALING_FACTOR2 - base_magnitude2 > 2 * MOTION_SCALING_FACTOR * NOISY_MAGNITUDE_DEVIATION) { reliable = 0; goto end_calculate_lid_angle; } if (MOTION_SCALING_FACTOR2 - lid_magnitude2 > 2 * MOTION_SCALING_FACTOR * NOISY_MAGNITUDE_DEVIATION) { reliable = 0; goto end_calculate_lid_angle; } largest_hinge_accel = MAX(ABS(scaled_base[HINGE_AXIS]), ABS(scaled_lid[HINGE_AXIS])); smoothed_ratio = MAX( INT_TO_FP(0), MIN(INT_TO_FP(1), fp_div(INT_TO_FP(largest_hinge_accel - SCALED_HINGE_VERTICAL_SMOOTHING_START), INT_TO_FP(SCALED_HINGE_VERTICAL_MAXIMUM - SCALED_HINGE_VERTICAL_SMOOTHING_START)))); /* Check hinge is not too vertical */ if (largest_hinge_accel > SCALED_HINGE_VERTICAL_MAXIMUM) { reliable = 0; goto end_calculate_lid_angle; } /* Smooth input to reduce calculation error due to noise. */ vector_scale(smoothed_base, smoothed_ratio); vector_scale(smoothed_lid, smoothed_ratio); vector_scale(scaled_base, INT_TO_FP(1) - smoothed_ratio); vector_scale(scaled_lid, INT_TO_FP(1) - smoothed_ratio); for (i = X; i <= Z; i++) { smoothed_base[i] += scaled_base[i]; smoothed_lid[i] += scaled_lid[i]; } /* Project vectors on the hinge hyperplan, putting smooth ones aside. */ memcpy(proj_base, smoothed_base, sizeof(intv3_t)); memcpy(proj_lid, smoothed_lid, sizeof(intv3_t)); proj_base[HINGE_AXIS] = 0; proj_lid[HINGE_AXIS] = 0; /* Calculate the clockwise angle */ lid_to_base_fp = arc_cos(cosine_of_angle_diff(proj_base, proj_lid)); cross_product(proj_base, proj_lid, cross); /* * If the dot product of this cross product is normal, it means that * the shortest angle between |base| and |lid| was counterclockwise * with respect to the surface represented by |hinge_axis| and this * angle must be reversed. */ if (dot_product(cross, hinge_axis) > 0) lid_to_base_fp = FLOAT_TO_FP(360) - lid_to_base_fp; /* * Angle is between the keyboard and the front of screen: we need to * anlge between keyboard and back of screen: * 180 instead of 0 when lid and base are flat on surface. * 0 instead of 180 when lid is closed on keyboard. */ if (!IS_ENABLED(CONFIG_ACCEL_STD_REF_FRAME_OLD)) lid_to_base_fp = FLOAT_TO_FP(180) - lid_to_base_fp; /* Place lid angle between 0 and 360 degrees. */ if (lid_to_base_fp < 0) lid_to_base_fp += FLOAT_TO_FP(360); #ifdef CONFIG_TABLET_MODE /* Ignore large angles when the lid is closed. */ if (!lid_is_open() && (lid_to_base_fp > SMALL_LID_ANGLE_RANGE)) { reliable = 0; goto end_calculate_lid_angle; } /* * Ignore small angles when the lid is open. * * Note that we're not correcting the angle, but just marking it as * unreliable. Attempting to correct the angle would cause bad angles * when closing the lid. However, there is one edge case. If the * device is suspended in laptop mode, but then is physically placed in * tablet mode, but ALL the angles are read as unreliable, a keypress * may wake us up. This is because we require at least 4 consecutive * reliable readings over a threshold to disable key scanning. */ if (lid_is_open() && (lid_to_base_fp <= SMALL_LID_ANGLE_RANGE)) { reliable = 0; goto end_calculate_lid_angle; } /* Seed the lid angle now that we have a reliable measurement. */ if (last_lid_angle_fp == FLOAT_TO_FP(-1)) last_lid_angle_fp = lid_to_base_fp; /* * If the angle was last seen as really large and now it's quite * small, we may be rotating around from 360->0 so correct it to * be large. But in case that the lid switch is closed, we can * prove the small angle we see is correct so we take the angle * as is. */ if ((last_lid_angle_fp >= FLOAT_TO_FP(360) - DEBOUNCE_ANGLE_DELTA) && (lid_to_base_fp <= DEBOUNCE_ANGLE_DELTA) && (lid_is_open())) last_lid_angle_fp = FLOAT_TO_FP(360) - lid_to_base_fp; else last_lid_angle_fp = lid_to_base_fp; end_calculate_lid_angle: /* * Round to nearest int by adding 0.5. Note, only works because lid * angle is known to be positive. */ *lid_angle = FP_TO_INT(last_lid_angle_fp + FLOAT_TO_FP(0.5)); if (board_is_lid_angle_tablet_mode()) motion_lid_set_tablet_mode(reliable); if (IS_ENABLED(MOTION_LID_SET_DPTF_PROFILE)) motion_lid_set_dptf_profile(reliable); #else /* CONFIG_TABLET_MODE */ end_calculate_lid_angle: if (reliable) *lid_angle = FP_TO_INT(lid_to_base_fp + FLOAT_TO_FP(0.5)); #endif return reliable; } int motion_lid_get_angle(void) { if (lid_angle_is_reliable) return lid_angle_deg; else return LID_ANGLE_UNRELIABLE; } /* * Calculate lid angle and massage the results */ void motion_lid_calc(void) { /* Calculate angle of lid accel. */ lid_angle_is_reliable = calculate_lid_angle( accel_base->xyz, accel_lid->xyz, &lid_angle_deg); if (IS_ENABLED(CONFIG_LID_ANGLE_UPDATE)) lid_angle_update(motion_lid_get_angle()); } /*****************************************************************************/ /* Host commands */ /** * @brief This is a "sub"-host command accessed through EC_CMD_MOTION_SENSE_CMD, * defined in `common/motion_sense.c` * * @param args Hot command args * @return enum ec_status Exit status */ enum ec_status host_cmd_motion_lid(struct host_cmd_handler_args *args) { const struct ec_params_motion_sense *in = args->params; struct ec_response_motion_sense *out = args->response; switch (in->cmd) { case MOTIONSENSE_CMD_KB_WAKE_ANGLE: if (IS_ENABLED(CONFIG_LID_ANGLE_UPDATE)) { /* * Set new keyboard wake lid angle if data arg has * value. */ if (in->kb_wake_angle.data != EC_MOTION_SENSE_NO_VALUE) lid_angle_set_wake_angle( in->kb_wake_angle.data); out->kb_wake_angle.ret = lid_angle_get_wake_angle(); } else { out->kb_wake_angle.ret = 0; } args->response_size = sizeof(out->kb_wake_angle); break; case MOTIONSENSE_CMD_LID_ANGLE: if (IS_ENABLED(CONFIG_LID_ANGLE)) { out->lid_angle.value = motion_lid_get_angle(); args->response_size = sizeof(out->lid_angle); } else { return EC_RES_INVALID_PARAM; } break; case MOTIONSENSE_CMD_TABLET_MODE_LID_ANGLE: if (IS_ENABLED(CONFIG_TABLET_MODE)) { int ret; ret = lid_angle_set_tablet_mode_threshold( in->tablet_mode_threshold.lid_angle, in->tablet_mode_threshold.hys_degree); if (ret != EC_RES_SUCCESS) return ret; out->tablet_mode_threshold.lid_angle = tablet_mode_lid_angle; out->tablet_mode_threshold.hys_degree = tablet_mode_hys_degree; args->response_size = sizeof(out->tablet_mode_threshold); } else { return EC_RES_INVALID_PARAM; } break; default: return EC_RES_INVALID_PARAM; } return EC_RES_SUCCESS; }