GMINL-10383: Allow for prioritization of sensors

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

diff --git a/enumeration.c b/enumeration.c
index c8fa34a..041196d 100644 (file)
--- a/enumeration.c
+++ b/enumeration.c
@@ -975,6 +975,53 @@ static void post_process_sensor_list (char poll_map[catalog_size], char trig_map
 }
 
 
+static void swap_sensors (int s1, int s2)
+{
+       struct sensor_t temp_sensor_desc;
+       sensor_info_t   temp_sensor;
+
+       /* S1 -> temp */
+       memcpy(&temp_sensor, &sensor[s1], sizeof(sensor_info_t));
+       memcpy(&temp_sensor_desc, &sensor_desc[s1], sizeof(struct sensor_t));
+
+       /* S2 -> S1 */
+       memcpy(&sensor[s1], &sensor[s2], sizeof(sensor_info_t));
+       memcpy(&sensor_desc[s1], &sensor_desc[s2], sizeof(struct sensor_t));
+
+       /* temp -> S2 */
+       memcpy(&sensor[s2], &temp_sensor, sizeof(sensor_info_t));
+       memcpy(&sensor_desc[s2], &temp_sensor_desc,  sizeof(struct sensor_t));
+
+       /* Fix-up sensor id mapping, which is stale */
+       sensor_desc[s1].handle  = s1;
+       sensor_desc[s2].handle  = s2;
+
+       /* Fix up name and vendor buffer pointers, which are potentially stale pointers */
+       sensor_desc[s1].name            = sensor_get_name(s1);
+       sensor_desc[s1].vendor          = sensor_get_vendor(s1);
+       sensor_desc[s2].name            = sensor_get_name(s2);
+       sensor_desc[s2].vendor          = sensor_get_vendor(s2);
+}
+
+
+static void reorder_sensors (void)
+{
+       /* Some sensors may be marked as secondary - these need to be listed after other sensors of the same type */
+       int s1, s2;
+
+       for (s1=0; s1<sensor_count-1; s1++)
+               if (sensor[s1].quirks & QUIRK_SECONDARY) {
+                       /* Search for subsequent sensors of same type */
+                       for (s2 = s1+1; s2<sensor_count; s2++)
+                               if (sensor[s2].type == sensor[s1].type && !(sensor[s2].quirks & QUIRK_SECONDARY)) {
+                                       ALOGI("Sensor S%d has higher priority than S%d, swapping\n", s2, s1);
+                                       swap_sensors(s1, s2);
+                                       break;
+                               }
+               }
+}
+
+
 void enumerate_sensors (void)
 {
        /*
@@ -988,6 +1035,7 @@ void enumerate_sensors (void)
        int dev_num;
        unsigned int i;
        int trig_found;
+       int s;
 
        for (dev_num=0; dev_num<MAX_DEVICES; dev_num++) {
                trig_found = 0;
@@ -1019,12 +1067,21 @@ void enumerate_sensors (void)
                        build_sensor_report_maps(dev_num);
        }
 
+       /* Make sure secondary sensors appear after primary ones */
+       reorder_sensors();
+
        ALOGI("Discovered %d sensors\n", sensor_count);
 
        /* Set up default - as well as custom - trigger names */
        setup_trigger_names();
 
+       ALOGI("Discovered %d sensors\n", sensor_count);
+
        virtual_sensors_check();
+
+       for (s=0; s<sensor_count; s++) {
+               ALOGI("S%d: %s\n", s, sensor[s].friendly_name);
+       }
 }