Don't let the timestamp get too far behind

[android-x86/hardware-intel-libsensors.git] / control.c

diff --git a/control.c b/control.c
index 1124a0f..9565f46 100644 (file)
--- a/control.c
+++ b/control.c
@@ -258,11 +258,11 @@ int adjust_counters (int s, int enabled)
                ALOGI("Enabling sensor %d (iio device %d: %s)\n",
                        s, dev_num, sensor_info[s].friendly_name);
 
-               sensor_info[s].enable_count++;
-
-               if (sensor_info[s].enable_count > 1)
+               if (sensor_info[s].enabled)
                        return 0; /* The sensor was, and remains, in use */
 
+               sensor_info[s].enabled = 1;
+
                switch (sensor_info[s].type) {
                        case SENSOR_TYPE_MAGNETIC_FIELD:
                                compass_read_data(&sensor_info[s]);
@@ -274,16 +274,13 @@ int adjust_counters (int s, int enabled)
                                break;
                }
        } else {
-               if (sensor_info[s].enable_count == 0)
-                       return -1; /* Spurious disable call */
+               if (sensor_info[s].enabled == 0)
+                       return 0; /* Spurious disable call */
 
                ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num,
                      sensor_info[s].friendly_name);
 
-               sensor_info[s].enable_count--;
-
-               if (sensor_info[s].enable_count > 0)
-                       return 0; /* The sensor was, and remains, in use */
+               sensor_info[s].enabled = 0;
 
                /* Sensor disabled, lower report available flag */
                sensor_info[s].report_pending = 0;
@@ -299,7 +296,7 @@ int adjust_counters (int s, int enabled)
 
        /* If uncalibrated type and pair is already active don't adjust counters */
        if (sensor_info[s].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
-               sensor_info[sensor_info[s].pair_idx].enable_count != 0)
+               sensor_info[sensor_info[s].pair_idx].enabled != 0)
                        return 0;
 
        /* We changed the state of a sensor - adjust per iio device counters */
@@ -547,7 +544,7 @@ int sensor_activate(int s, int enabled)
         * Reactivate gyro uncalibrated - Uncalibrated has handler */
 
        if (sensor_info[s].type == SENSOR_TYPE_GYROSCOPE &&
-               sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
+               sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enabled != 0) {
 
                                sensor_activate(sensor_info[s].pair_idx, 0);
                                ret = sensor_activate(s, enabled);
@@ -561,6 +558,8 @@ int sensor_activate(int s, int enabled)
        if (ret <= 0)
                return ret;
 
+       sensor_info[s].event_count = 0;
+       sensor_info[s].meta_data_pending = 0;
 
        if (!is_poll_sensor) {
 
@@ -709,7 +708,7 @@ static void enable_motion_trigger (int dev_num)
 
        for (s=0; s<MAX_SENSORS; s++)
                if (sensor_info[s].dev_num == dev_num &&
-                   sensor_info[s].enable_count &&
+                   sensor_info[s].enabled &&
                    sensor_info[s].num_channels &&
                    (!sensor_info[s].motion_trigger_name[0] ||
                     !sensor_info[s].report_initialized ||
@@ -722,7 +721,7 @@ static void enable_motion_trigger (int dev_num)
 
        for (s=0; s<MAX_SENSORS; s++)
                if (sensor_info[s].dev_num == dev_num &&
-                   sensor_info[s].enable_count &&
+                   sensor_info[s].enabled &&
                    sensor_info[s].num_channels &&
                    sensor_info[s].selected_trigger !=
                        sensor_info[s].motion_trigger_name)
@@ -748,6 +747,7 @@ static void enable_motion_trigger (int dev_num)
  *     FrequencyVerification.java: (0.9)*(expected freq) => (th <= 1.1111)
  */
 #define THRESHOLD 1.10
+#define MAX_DELAY 500000000 /* 500 ms */
 void set_report_ts(int s, int64_t ts)
 {
        int64_t maxTs, period;
@@ -766,13 +766,16 @@ void set_report_ts(int s, int64_t ts)
        {
                period = (int64_t) (1000000000LL / sensor_info[s].sampling_rate);
                maxTs = sensor_info[s].report_ts + (is_accel ? 1 : THRESHOLD) * period;
+               /* If we're too far behind get back on track */
+               if (ts - maxTs >= MAX_DELAY)
+                       maxTs = ts;
                sensor_info[s].report_ts = (ts < maxTs ? ts : maxTs);
        } else {
                sensor_info[s].report_ts = ts;
        }
 }
 
-static int integrate_device_report(int dev_num)
+static int integrate_device_report (int dev_num)
 {
        int len;
        int s,c;
@@ -810,7 +813,7 @@ static int integrate_device_report(int dev_num)
 
        for (s=0; s<MAX_SENSORS; s++)
                if (sensor_info[s].dev_num == dev_num &&
-                   sensor_info[s].enable_count) {
+                   sensor_info[s].enabled) {
 
                        sr_offset = 0;
 
@@ -833,7 +836,7 @@ static int integrate_device_report(int dev_num)
                              sr_offset);
 
                        set_report_ts(s, get_timestamp());
-                       sensor_info[s].report_pending = 1;
+                       sensor_info[s].report_pending = DATA_TRIGGER;
                        sensor_info[s].report_initialized = 1;
                }
 
@@ -859,7 +862,7 @@ static int propagate_sensor_report (int s, struct sensors_event_t  *data)
 
        /* Only return uncalibrated event if also gyro active */
        if (sensor_info[s].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
-               sensor_info[sensor_info[s].pair_idx].enable_count != 0)
+               sensor_info[sensor_info[s].pair_idx].enabled != 0)
                        return 0;
 
        memset(data, 0, sizeof(sensors_event_t));
@@ -917,7 +920,7 @@ static void synthetize_duplicate_samples (void)
        for (s=0; s<sensor_count; s++) {
 
                /* Ignore disabled sensors */
-               if (!sensor_info[s].enable_count)
+               if (!sensor_info[s].enabled)
                        continue;
 
                /* If the sensor is continuously firing, leave it alone */
@@ -945,7 +948,7 @@ static void synthetize_duplicate_samples (void)
                if (target_ts <= current_ts) {
                        /* Mark the sensor for event generation */
                        set_report_ts(s, current_ts);
-                       sensor_info[s].report_pending = 1;
+                       sensor_info[s].report_pending = DATA_DUPLICATE;
                }
        }
 }
@@ -965,7 +968,7 @@ static void integrate_thread_report (uint32_t tag)
 
        if (len == expected_len) {
                set_report_ts(s, get_timestamp());
-               sensor_info[s].report_pending = 1;
+               sensor_info[s].report_pending = DATA_SYSFS;
        }
 }
 
@@ -990,7 +993,7 @@ static int get_poll_wait_timeout (void)
         * duplicate events. Check deadline for the nearest upcoming event.
         */
        for (s=0; s<sensor_count; s++)
-               if (sensor_info[s].enable_count &&
+               if (sensor_info[s].enabled &&
                    sensor_info[s].selected_trigger ==
                    sensor_info[s].motion_trigger_name &&
                    sensor_info[s].sampling_rate) {
@@ -1038,15 +1041,16 @@ return_available_sensor_reports:
        for (s=0; s<sensor_count && returned_events < count; s++) {
                if (sensor_info[s].report_pending) {
                        event_count = 0;
-                       /* Lower flag */
-                       sensor_info[s].report_pending = 0;
 
                        /* Report this event if it looks OK */
                        event_count = propagate_sensor_report(s, &data[returned_events]);
 
+                       /* Lower flag */
+                       sensor_info[s].report_pending = 0;
+
                        /* Duplicate only if both cal & uncal are active */
                        if (sensor_info[s].type == SENSOR_TYPE_GYROSCOPE &&
-                                       sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
+                                       sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enabled != 0) {
                                        struct gyro_cal* gyro_data = (struct gyro_cal*) sensor_info[s].cal_data;
 
                                        memcpy(&data[returned_events + event_count], &data[returned_events],
@@ -1230,7 +1234,7 @@ int sensor_set_delay(int s, int64_t ns)
                for (n=0; n<sensor_count; n++)
                        if (n != s && sensor_info[n].dev_num == dev_num &&
                            sensor_info[n].num_channels &&
-                           sensor_info[n].enable_count &&
+                           sensor_info[n].enabled &&
                            sensor_info[n].sampling_rate > new_sampling_rate)
                                new_sampling_rate= sensor_info[n].sampling_rate;
 
@@ -1304,7 +1308,7 @@ int sensor_flush (int s)
 {
        /* If one shot or not enabled return -EINVAL */
        if (sensor_desc[s].flags & SENSOR_FLAG_ONE_SHOT_MODE ||
-               sensor_info[s].enable_count == 0)
+               sensor_info[s].enabled == 0)
                return -EINVAL;
 
        sensor_info[s].meta_data_pending++;