Sensor Command Line Interface implementation.

Implement an unified sensor command line interface(CLI) on emulator
to check all available sensors, and get/set specified sensor data.

Change-Id: Ibb3c3522dc6c88e42fa1c7dffa32fbb675596b08

3 files changed, 344 insertions, 39 deletions

diff --git a/android/console.c b/android/console.c
index f4273f4..ed02db5 100644
--- a/android/console.c
+++ b/android/console.c
@@ -48,6 +48,7 @@
 #include <fcntl.h>
 #include "android/hw-events.h"
 #include "user-events.h"
+#include "android/hw-sensors.h"
 #include "android/keycode-array.h"
 #include "android/charmap.h"
 #include "android/display-core.h"
@@ -2410,6 +2411,200 @@ static const CommandDefRec  geo_commands[] =
 /********************************************************************************************/
 /********************************************************************************************/
 /*****                                                                                 ******/
+/*****                        S E N S O R S  C O M M A N D S                           ******/
+/*****                                                                                 ******/
+/********************************************************************************************/
+/********************************************************************************************/
+
+/* For sensors user prompt string size.*/
+#define SENSORS_INFO_SIZE 150
+
+/* Get sensor data - (a,b,c) from sensor name */
+static int
+do_sensors_get( ControlClient client, char* args )
+{
+    if (! args) {
+        control_write( client, "KO: Usage: \"get <sensorname>\"\n" );
+        return -1;
+    }
+
+    int status = SENSOR_STATUS_UNKNOWN;
+    char sensor[strlen(args) + 1];
+    if (1 != sscanf( args, "%s", &sensor[0] ))
+        goto SENSOR_STATUS_ERROR;
+
+    int sensor_id = android_sensors_get_id_from_name( sensor );
+    char buffer[SENSORS_INFO_SIZE] = { 0 };
+    float a, b, c;
+
+    if (sensor_id < 0) {
+        status = sensor_id;
+        goto SENSOR_STATUS_ERROR;
+    } else {
+        status = android_sensors_get( sensor_id, &a, &b, &c );
+        if (status != SENSOR_STATUS_OK)
+            goto SENSOR_STATUS_ERROR;
+        snprintf( buffer, sizeof(buffer),
+                "%s = %g:%g:%g\r\n", sensor, a, b, c );
+        do_control_write( client, buffer );
+        return 0;
+    }
+
+SENSOR_STATUS_ERROR:
+    switch(status) {
+    case SENSOR_STATUS_NO_SERVICE:
+        snprintf( buffer, sizeof(buffer), "KO: No sensor service found!\r\n" );
+        break;
+    case SENSOR_STATUS_DISABLED:
+        snprintf( buffer, sizeof(buffer), "KO: '%s' sensor is disabled.\r\n", sensor );
+        break;
+    case SENSOR_STATUS_UNKNOWN:
+        snprintf( buffer, sizeof(buffer),
+                "KO: unknown sensor name: %s, run 'sensor status' to get available sensors.\r\n", sensor );
+        break;
+    default:
+        snprintf( buffer, sizeof(buffer), "KO: '%s' sensor: exception happens.\r\n", sensor );
+    }
+    do_control_write( client, buffer );
+    return -1;
+}
+
+/* set sensor data - (a,b,c) from sensor name */
+static int
+do_sensors_set( ControlClient client, char* args )
+{
+    if (! args) {
+        control_write( client, "KO: Usage: \"set <sensorname> <value-a>[:<value-b>[:<value-c>]]\"\n" );
+        return -1;
+    }
+
+    int status;
+    char* sensor;
+    char* value;
+    char* args_dup = strdup( args );
+    if (args_dup == NULL) {
+        control_write( client, "KO: Memory allocation failed.\n" );
+        return -1;
+    }
+    char* p = args_dup;
+
+    /* Parsing the args to get sensor name string */
+    while (*p && isspace(*p)) p++;
+    if (*p == 0)
+        goto INPUT_ERROR;
+    sensor = p;
+
+    /* Parsing the args to get value string */
+    while (*p && (! isspace(*p))) p++;
+    if (*p == 0 || *(p + 1) == 0/* make sure value isn't NULL */)
+        goto INPUT_ERROR;
+    *p = 0;
+    value = p + 1;
+
+    if (! (strlen(sensor) && strlen(value)))
+        goto INPUT_ERROR;
+
+    int sensor_id = android_sensors_get_id_from_name( sensor );
+    char buffer[SENSORS_INFO_SIZE] = { 0 };
+
+    if (sensor_id < 0) {
+        status = sensor_id;
+        goto SENSOR_STATUS_ERROR;
+    } else {
+        float fvalues[3];
+        status = android_sensors_get( sensor_id, &fvalues[0], &fvalues[1], &fvalues[2] );
+        if (status != SENSOR_STATUS_OK)
+            goto SENSOR_STATUS_ERROR;
+
+        /* Parsing the value part to get the sensor values(a, b, c) */
+        int i;
+        char* pnext;
+        char* pend = value + strlen(value);
+        for (i = 0; i < 3; i++, value = pnext + 1) {
+            pnext=strchr( value, ':' );
+            if (pnext) {
+                *pnext = 0;
+            } else {
+                pnext = pend;
+            }
+
+            if (pnext > value) {
+                if (1 != sscanf( value,"%g", &fvalues[i] ))
+                    goto INPUT_ERROR;
+            }
+        }
+
+        status = android_sensors_set( sensor_id, fvalues[0], fvalues[1], fvalues[2] );
+        if (status != SENSOR_STATUS_OK)
+            goto SENSOR_STATUS_ERROR;
+
+        free( args_dup );
+        return 0;
+    }
+
+SENSOR_STATUS_ERROR:
+    switch(status) {
+    case SENSOR_STATUS_NO_SERVICE:
+        snprintf( buffer, sizeof(buffer), "KO: No sensor service found!\r\n" );
+        break;
+    case SENSOR_STATUS_DISABLED:
+        snprintf( buffer, sizeof(buffer), "KO: '%s' sensor is disabled.\r\n", sensor );
+        break;
+    case SENSOR_STATUS_UNKNOWN:
+        snprintf( buffer, sizeof(buffer),
+                "KO: unknown sensor name: %s, run 'sensor status' to get available sensors.\r\n", sensor );
+        break;
+    default:
+        snprintf( buffer, sizeof(buffer), "KO: '%s' sensor: exception happens.\r\n", sensor );
+    }
+    do_control_write( client, buffer );
+    free( args_dup );
+    return -1;
+
+INPUT_ERROR:
+    control_write( client, "KO: Usage: \"set <sensorname> <value-a>[:<value-b>[:<value-c>]]\"\n" );
+    free( args_dup );
+    return -1;
+}
+
+/* get all available sensor names and enable status respectively. */
+static int
+do_sensors_status( ControlClient client, char* args )
+{
+    uint8_t id, status;
+    char buffer[SENSORS_INFO_SIZE] = { 0 };
+
+    for(id = 0; id < MAX_SENSORS; id++) {
+        status = android_sensors_get_sensor_status( id );
+        snprintf( buffer, sizeof(buffer), "%s: %s\n",
+                android_sensors_get_name_from_id(id), (status ? "enabled.":"disabled.") );
+        control_write( client, buffer );
+    }
+
+    return 0;
+}
+
+/* Sensor commands for get/set sensor values and get available sensor names. */
+static const CommandDefRec sensor_commands[] =
+{
+    { "status", "list all sensors and their status.",
+      "'status': list all sensors and their status.\r\n",
+      NULL, do_sensors_status, NULL },
+
+    { "get", "get sensor values",
+      "'get <sensorname>' returns the values of a given sensor.\r\n",
+      NULL, do_sensors_get, NULL },
+
+    { "set", "set sensor values",
+      "'set <sensorname> <value-a>[:<value-b>[:<value-c>]]' set the values of a given sensor.\r\n",
+      NULL, do_sensors_set, NULL },
+
+    { NULL, NULL, NULL, NULL, NULL, NULL }
+};
+
+/********************************************************************************************/
+/********************************************************************************************/
+/*****                                                                                 ******/
 /*****                           M A I N   C O M M A N D S                             ******/
 /*****                                                                                 ******/
 /********************************************************************************************/
@@ -2784,6 +2979,10 @@ static const CommandDefRec   main_commands[] =
     "allows to connect to the QEMU virtual machine monitor\r\n", NULL,
     NULL, qemu_commands },
 
+    { "sensor", "manage emulator sensors",
+      "allows you to request the emulator sensors\r\n", NULL,
+      NULL, sensor_commands },
+
     { NULL, NULL, NULL, NULL, NULL, NULL }
 };
 
diff --git a/android/hw-sensors.c b/android/hw-sensors.c
index 69447a2..7d77b62 100644
--- a/android/hw-sensors.c
+++ b/android/hw-sensors.c
@@ -62,6 +62,20 @@ _sensorIdFromName( const char*  name )
     return -1;
 }
 
+static const char*
+_sensorNameFromId( int  id )
+{
+    int  nn;
+    for (nn = 0; nn < MAX_SENSORS; nn++)
+        if (id == _sSensors[nn].id)
+            return _sSensors[nn].name;
+    return NULL;
+}
+
+/* For common Sensor Value struct */
+typedef struct {
+    float a, b, c;
+} SensorValues;
 
 typedef struct {
     float   x, y, z;
@@ -92,6 +106,7 @@ typedef struct {
 typedef struct {
     char       enabled;
     union {
+        SensorValues   value;
         Acceleration   acceleration;
         MagneticField  magnetic;
         Orientation    orientation;
@@ -462,14 +477,15 @@ _hwSensors_connect( void*  opaque, QemudService*  service, int  channel )
     return client;
 }
 
-/* change the value of the emulated acceleration vector */
+/* change the value of the emulated sensor vector */
 static void
-_hwSensors_setAcceleration( HwSensors*  h, float x, float y, float z )
+_hwSensors_setSensorValue( HwSensors*  h, int sensor_id, float a, float b, float c )
 {
-    Sensor*  s = &h->sensors[ANDROID_SENSOR_ACCELERATION];
-    s->u.acceleration.x = x;
-    s->u.acceleration.y = y;
-    s->u.acceleration.z = z;
+    Sensor* s = &h->sensors[sensor_id];
+
+    s->u.value.a = a;
+    s->u.value.b = b;
+    s->u.value.c = c;
 }
 
 /* Saves available sensors to allow checking availability when loaded.
@@ -579,36 +595,6 @@ _hwSensors_load( QEMUFile*  f, QemudService*  s, void*  opaque)
 }
 
 
-#if 0  /* not used yet */
-/* change the value of the emulated magnetic vector */
-static void
-_hwSensors_setMagneticField( HwSensors*  h, float x, float y, float z )
-{
-    Sensor*  s = &h->sensors[ANDROID_SENSOR_MAGNETIC_FIELD];
-    s->u.magnetic.x = x;
-    s->u.magnetic.y = y;
-    s->u.magnetic.z = z;
-}
-
-/* change the values of the emulated orientation */
-static void
-_hwSensors_setOrientation( HwSensors*  h, float azimuth, float pitch, float roll )
-{
-    Sensor*  s = &h->sensors[ANDROID_SENSOR_ORIENTATION];
-    s->u.orientation.azimuth = azimuth;
-    s->u.orientation.pitch   = pitch;
-    s->u.orientation.roll    = roll;
-}
-
-/* change the emulated temperature */
-static void
-_hwSensors_setTemperature( HwSensors*  h, float celsius )
-{
-    Sensor*  s = &h->sensors[ANDROID_SENSOR_TEMPERATURE];
-    s->u.temperature.celsius = celsius;
-}
-#endif
-
 /* change the emulated proximity */
 static void
 _hwSensors_setProximity( HwSensors*  h, float value )
@@ -640,11 +626,11 @@ _hwSensors_setCoarseOrientation( HwSensors*  h, AndroidCoarseOrientation  orient
 
     switch (orient) {
     case ANDROID_COARSE_PORTRAIT:
-        _hwSensors_setAcceleration( h, 0., g*cos_angle, g*sin_angle );
+        _hwSensors_setSensorValue( h, ANDROID_SENSOR_ACCELERATION, 0., g*cos_angle, g*sin_angle );
         break;
 
     case ANDROID_COARSE_LANDSCAPE:
-        _hwSensors_setAcceleration( h, g*cos_angle, 0., g*sin_angle );
+        _hwSensors_setSensorValue( h, ANDROID_SENSOR_ACCELERATION, g*cos_angle, 0., g*sin_angle );
         break;
     default:
         ;
@@ -693,3 +679,94 @@ android_sensors_set_coarse_orientation( AndroidCoarseOrientation  orient )
     _hwSensors_setCoarseOrientation(_sensorsState, orient);
 }
 
+/* Get sensor name from sensor id */
+extern const char*
+android_sensors_get_name_from_id( int sensor_id )
+{
+    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
+        return NULL;
+
+    return _sensorNameFromId(sensor_id);
+}
+
+/* Get sensor id from sensor name */
+extern int
+android_sensors_get_id_from_name( char* sensorname )
+{
+    HwSensors* hw = _sensorsState;
+
+    if (sensorname == NULL)
+        return SENSOR_STATUS_UNKNOWN;
+
+    int id = _sensorIdFromName(sensorname);
+
+    if (id < 0 || id >= MAX_SENSORS)
+        return SENSOR_STATUS_UNKNOWN;
+
+    if (hw->service != NULL) {
+        if (! hw->sensors[id].enabled)
+            return SENSOR_STATUS_DISABLED;
+    } else
+        return SENSOR_STATUS_NO_SERVICE;
+
+    return id;
+}
+
+/* Interface of reading the data for all sensors */
+extern int
+android_sensors_get( int sensor_id, float* a, float* b, float* c )
+{
+    HwSensors* hw = _sensorsState;
+
+    *a = 0;
+    *b = 0;
+    *c = 0;
+
+    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
+        return SENSOR_STATUS_UNKNOWN;
+
+    Sensor* sensor = &hw->sensors[sensor_id];
+    if (hw->service != NULL) {
+        if (! sensor->enabled)
+            return SENSOR_STATUS_DISABLED;
+    } else
+        return SENSOR_STATUS_NO_SERVICE;
+
+    *a = sensor->u.value.a;
+    *b = sensor->u.value.b;
+    *c = sensor->u.value.c;
+
+    return SENSOR_STATUS_OK;
+}
+
+/* Interface of setting the data for all sensors */
+extern int
+android_sensors_set( int sensor_id, float a, float b, float c )
+{
+    HwSensors* hw = _sensorsState;
+
+    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
+        return SENSOR_STATUS_UNKNOWN;
+
+    if (hw->service != NULL) {
+        if (! hw->sensors[sensor_id].enabled)
+            return SENSOR_STATUS_DISABLED;
+    } else
+        return SENSOR_STATUS_NO_SERVICE;
+
+    _hwSensors_setSensorValue(hw, sensor_id, a, b, c);
+
+    return SENSOR_STATUS_OK;
+}
+
+/* Get Sensor from sensor id */
+extern uint8_t
+android_sensors_get_sensor_status( int sensor_id )
+{
+    HwSensors* hw = _sensorsState;
+
+    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
+        return SENSOR_STATUS_UNKNOWN;
+
+    return hw->sensors[sensor_id].enabled;
+}
diff --git a/android/hw-sensors.h b/android/hw-sensors.h
index 665cc6d..78c6a26 100644
--- a/android/hw-sensors.h
+++ b/android/hw-sensors.h
@@ -17,6 +17,20 @@
 /* initialize sensor emulation */
 extern void  android_hw_sensors_init( void );
 
+/* NOTE: Sensor status Error definition, It will be used in the Sensors Command
+ *       part in android/console.c. Details:
+ *       SENSOR_STATUS_NO_SERVICE: "sensors" qemud service is not available/initiated.
+ *       SENSOR_STATUS_DISABLED: sensor is disabled.
+ *       SENSOR_STATUS_UNKNOWN: wrong sensor name.
+ *       SENSOR_STATUS_OK: Everything is OK to the current sensor.
+ * */
+typedef enum{
+    SENSOR_STATUS_NO_SERVICE = -3,
+    SENSOR_STATUS_DISABLED   = -2,
+    SENSOR_STATUS_UNKNOWN    = -1,
+    SENSOR_STATUS_OK         = 0,
+} SensorStatus;
+
 /* NOTE: this list must be the same that the one defined in
  *       the sensors_qemu.c source of the libsensors.goldfish.so
  *       library.
@@ -47,6 +61,21 @@ typedef enum {
 } AndroidCoarseOrientation;
 
 /* change the coarse orientation value */
-extern void  android_sensors_set_coarse_orientation( AndroidCoarseOrientation  orient );
+extern void android_sensors_set_coarse_orientation( AndroidCoarseOrientation  orient );
+
+/* get sensor values */
+extern int android_sensors_get( int sensor_id, float* a, float* b, float* c );
+
+/* set sensor values */
+extern int android_sensors_set( int sensor_id, float a, float b, float c );
+
+/* Get sensor id from sensor name */
+extern int android_sensors_get_id_from_name( char* sensorname );
+
+/* Get sensor name from sensor id */
+extern const char* android_sensors_get_name_from_id( int sensor_id );
+
+/* Get sensor from sensor id */
+extern uint8_t android_sensors_get_sensor_status( int sensor_id );
 
 #endif /* _android_gps_h */