This fixes the hardware emulation to support several clients listening

to the "sensors" service at the same time. Each client can decide to
listen to different sensors, with different delays too.

The previous implementation only allowed a single client, which blocked
anything application that tried to read the sensors (since the framework
was already the only allowed client)

Signed-off-by: David 'Digit' Turner <digit@google.com>

1 files changed, 238 insertions, 153 deletions

diff --git a/android/hw-sensors.c b/android/hw-sensors.c
index 37d4af7..1fa12dc 100644
--- a/android/hw-sensors.c
+++ b/android/hw-sensors.c
@@ -13,6 +13,7 @@
 #include "android/hw-sensors.h"
 #include "android/utils/debug.h"
 #include "android/utils/misc.h"
+#include "android/utils/system.h"
 #include "android/hw-qemud.h"
 #include "android/globals.h"
 #include "qemu-char.h"
@@ -139,189 +140,172 @@ typedef struct {
 #define  HEADER_SIZE  4
 #define  BUFFER_SIZE  512
 
+typedef struct HwSensorClient   HwSensorClient;
+
 typedef struct {
-    QemudService*   service;
-    int32_t         delay_ms;
-    uint32_t        enabledMask;
-    QEMUTimer*      timer;
-    Sensor          sensors[MAX_SENSORS];
+    QemudService*    service;
+    Sensor           sensors[MAX_SENSORS];
+    HwSensorClient*  clients;
 } HwSensors;
 
-/* forward */
-
-static void  hw_sensors_receive( HwSensors*  h,
-                                 uint8_t*    query,
-                                 int         querylen );
-
-static void  hw_sensors_timer_tick(void*  opaque);
-
-/* Qemud service management */
+struct HwSensorClient {
+    HwSensorClient*  next;
+    HwSensors*       sensors;
+    QemudClient*     client;
+    QEMUTimer*       timer;
+    uint32_t         enabledMask;
+    int32_t          delay_ms;
+};
 
 static void
-_hw_sensors_qemud_client_recv( void*  opaque, uint8_t*  msg, int  msglen,
-                               QemudClient*  client )
+_hwSensorClient_free( HwSensorClient*  cl )
 {
-    hw_sensors_receive(opaque, msg, msglen);
-}
+    /* remove from sensors's list */
+    if (cl->sensors) {
+        HwSensorClient**  pnode = &cl->sensors->clients;
+        for (;;) {
+            HwSensorClient*  node = *pnode;
+            if (node == NULL)
+                break;
+            if (node == cl) {
+                *pnode = cl->next;
+                break;
+            }
+            pnode = &node->next;
+        }
+        cl->next    = NULL;
+        cl->sensors = NULL;
+    }
 
-static QemudClient*
-_hw_sensors_service_connect( void*  opaque, QemudService*  service, int  channel )
-{
-    HwSensors*    sensors = opaque;
-    QemudClient*  client  = qemud_client_new(service, channel,
-                                             sensors,
-                                             _hw_sensors_qemud_client_recv,
-                                             NULL);
-    qemud_client_set_framing(client, 1);
-    return client;
+    /* close QEMUD client, if any */
+    if (cl->client) {
+        qemud_client_close(cl->client);
+        cl->client = NULL;
+    }
+    /* remove timer, if any */
+    if (cl->timer) {
+        qemu_del_timer(cl->timer);
+        qemu_free_timer(cl->timer);
+        cl->timer = NULL;
+    }
+    AFREE(cl);
 }
 
-/* change the value of the emulated acceleration vector */
-static void
-hw_sensors_set_acceleration( HwSensors*  h, float x, float y, float z )
-{
-    Sensor*  s = &h->sensors[ANDROID_SENSOR_ACCELERATION];
-    s->u.acceleration.x = x;
-    s->u.acceleration.y = y;
-    s->u.acceleration.z = z;
-}
+/* forward */
+static void  _hwSensorClient_tick(void*  opaque);
 
-#if 0  /* not used yet */
-/* change the value of the emulated magnetic vector */
-static void
-hw_sensors_set_magnetic_field( 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
-hw_sensors_set_orientation( HwSensors*  h, float azimuth, float pitch, float roll )
+static HwSensorClient*
+_hwSensorClient_new( HwSensors*  sensors )
 {
-    Sensor*  s = &h->sensors[ANDROID_SENSOR_MAGNETIC_FIELD];
-    s->u.orientation.azimuth = azimuth;
-    s->u.orientation.pitch   = pitch;
-    s->u.orientation.roll    = roll;
-}
+    HwSensorClient*  cl;
 
-/* change the emulated temperature */
-static void
-hw_sensors_set_temperature( HwSensors*  h, float celsius )
-{
-    Sensor*  s = &h->sensors[ANDROID_SENSOR_MAGNETIC_FIELD];
-    s->u.temperature.celsius = celsius;
+    ANEW0(cl);
+
+    cl->sensors     = sensors;
+    cl->enabledMask = 0;
+    cl->delay_ms    = 1000;
+    cl->timer       = qemu_new_timer(vm_clock, _hwSensorClient_tick, cl);
+
+    cl->next         = sensors->clients;
+    sensors->clients = cl;
+
+    return cl;
 }
-#endif
 
-/* change the coarse orientation (landscape/portrait) of the emulated device */
+/* forward */
+
+static void  _hwSensorClient_receive( HwSensorClient*  cl,
+                                      uint8_t*         query,
+                                      int              querylen );
+
+/* Qemud service management */
+
 static void
-hw_sensors_set_coarse_orientation( HwSensors*  h, AndroidCoarseOrientation  orient )
+_hwSensorClient_recv( void*  opaque, uint8_t*  msg, int  msglen,
+                      QemudClient*  client )
 {
-    /* The Android framework computes the orientation by looking at
-     * the accelerometer sensor (*not* the orientation sensor !)
-     *
-     * That's because the gravity is a constant 9.81 vector that
-     * can be determined quite easily.
-     *
-     * Also, for some reason, the framework code considers that the phone should
-     * be inclined by 30 degrees along the phone's X axis to be considered
-     * in its ideal "vertical" position
-     *
-     * If the phone is completely vertical, rotating it will not do anything !
-     */
-    const double  g      = 9.81;
-    const double  cos_30 = 0.866025403784;
-    const double  sin_30 = 0.5;
+    HwSensorClient*  cl = opaque;
 
-    switch (orient) {
-    case ANDROID_COARSE_PORTRAIT:
-        hw_sensors_set_acceleration( h, 0., g*cos_30, g*sin_30 );
-        break;
-
-    case ANDROID_COARSE_LANDSCAPE:
-        hw_sensors_set_acceleration( h, g*cos_30, 0., g*sin_30 );
-        break;
-    default:
-        ;
-    }
+    _hwSensorClient_receive(cl, msg, msglen);
 }
 
-
-/* initialize the sensors state */
 static void
-hw_sensors_init( HwSensors*  h )
+_hwSensorClient_close( void*  opaque )
 {
-    h->service = qemud_service_register("sensors", 1, h,
-                                        _hw_sensors_service_connect );
-    h->enabledMask = 0;
-    h->delay_ms    = 1000;
-    h->timer       = qemu_new_timer(vm_clock, hw_sensors_timer_tick, h);
+    HwSensorClient*  cl = opaque;
 
-    hw_sensors_set_coarse_orientation(h, ANDROID_COARSE_PORTRAIT);
+    /* the client is already closed here */
+    cl->client = NULL;
+    _hwSensorClient_free(cl);
 }
 
 /* send a one-line message to the HAL module through a qemud channel */
 static void
-hw_sensors_send( HwSensors*  hw, const uint8_t*  msg, int  msglen )
+_hwSensorClient_send( HwSensorClient*  cl, const uint8_t*  msg, int  msglen )
 {
     D("%s: '%s'", __FUNCTION__, quote_bytes((const void*)msg, msglen));
-    qemud_service_broadcast(hw->service, msg, msglen);
+    qemud_client_send(cl->client, msg, msglen);
+}
+
+static int
+_hwSensorClient_enabled( HwSensorClient*  cl, int  sensorId )
+{
+    return (cl->enabledMask & (1 << sensorId)) != 0;
 }
 
 /* this function is called periodically to send sensor reports
  * to the HAL module, and re-arm the timer if necessary
  */
 static void
-hw_sensors_timer_tick( void*  opaque )
+_hwSensorClient_tick( void*  opaque )
 {
-    HwSensors*  h = opaque;
-    int64_t     delay = h->delay_ms;
-    int64_t     now_ns;
-    uint32_t    mask  = h->enabledMask;
-    Sensor*     sensor;
-    char        buffer[128];
-
-    sensor = &h->sensors[ANDROID_SENSOR_ACCELERATION];
-    if (sensor->enabled) {
+    HwSensorClient*  cl = opaque;
+    HwSensors*       hw  = cl->sensors;
+    int64_t          delay = cl->delay_ms;
+    int64_t          now_ns;
+    uint32_t         mask  = cl->enabledMask;
+    Sensor*          sensor;
+    char             buffer[128];
+
+    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_ACCELERATION)) {
+        sensor = &hw->sensors[ANDROID_SENSOR_ACCELERATION];
         snprintf(buffer, sizeof buffer, "acceleration:%g:%g:%g",
                  sensor->u.acceleration.x,
                  sensor->u.acceleration.y,
                  sensor->u.acceleration.z);
-        hw_sensors_send(h, (uint8_t*)buffer, strlen(buffer));
+        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
     }
 
-    sensor = &h->sensors[ANDROID_SENSOR_MAGNETIC_FIELD];
-    if (sensor->enabled) {
+    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_MAGNETIC_FIELD)) {
+        sensor = &hw->sensors[ANDROID_SENSOR_MAGNETIC_FIELD];
         snprintf(buffer, sizeof buffer, "magnetic-field:%g:%g:%g",
                  sensor->u.magnetic.x,
                  sensor->u.magnetic.y,
                  sensor->u.magnetic.z);
-        hw_sensors_send(h, (uint8_t*)buffer, strlen(buffer));
+        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
     }
 
-    sensor = &h->sensors[ANDROID_SENSOR_ORIENTATION];
-    if (sensor->enabled) {
+    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_ORIENTATION)) {
+        sensor = &hw->sensors[ANDROID_SENSOR_ORIENTATION];
         snprintf(buffer, sizeof buffer, "orientation:%g:%g:%g",
                  sensor->u.orientation.azimuth,
                  sensor->u.orientation.pitch,
                  sensor->u.orientation.roll);
-        hw_sensors_send(h, (uint8_t*)buffer, strlen(buffer));
+        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
     }
 
-    sensor = &h->sensors[ANDROID_SENSOR_TEMPERATURE];
-    if (sensor->enabled) {
+    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_TEMPERATURE)) {
+        sensor = &hw->sensors[ANDROID_SENSOR_TEMPERATURE];
         snprintf(buffer, sizeof buffer, "temperature:%g",
                  sensor->u.temperature.celsius);
-        hw_sensors_send(h, (uint8_t*)buffer, strlen(buffer));
+        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
     }
 
     now_ns = qemu_get_clock(vm_clock);
 
     snprintf(buffer, sizeof buffer, "sync:%lld", now_ns/1000);
-    hw_sensors_send(h, (uint8_t*)buffer, strlen(buffer));
+    _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
 
     /* rearm timer, use a minimum delay of 20 ms, just to
      * be safe.
@@ -333,13 +317,15 @@ hw_sensors_timer_tick( void*  opaque )
         delay = 20;
 
     delay *= 1000000LL;  /* convert to nanoseconds */
-    qemu_mod_timer(h->timer, now_ns + delay);
+    qemu_mod_timer(cl->timer, now_ns + delay);
 }
 
 /* handle incoming messages from the HAL module */
 static void
-hw_sensors_receive( HwSensors*  hw, uint8_t*  msg, int  msglen )
+_hwSensorClient_receive( HwSensorClient*  cl, uint8_t*  msg, int  msglen )
 {
+    HwSensors*  hw = cl->sensors;
+
     D("%s: '%.*s'", __FUNCTION__, msglen, msg);
 
     /* "list-sensors" is used to get an integer bit map of
@@ -349,15 +335,15 @@ hw_sensors_receive( HwSensors*  hw, uint8_t*  msg, int  msglen )
     if (msglen == 12 && !memcmp(msg, "list-sensors", 12)) {
         char  buff[12];
         int   mask = 0;
+        int   nn;
 
-        if (android_hw->hw_accelerometer)
-            mask |= (1 << ANDROID_SENSOR_ACCELERATION);
-
-        /* XXX: TODO: Add other tests when we add the corresponding
-         * properties to hardware-properties.ini et al. */
+        for (nn = 0; nn < MAX_SENSORS; nn++) {
+            if (hw->sensors[nn].enabled)
+                mask |= (1 << nn);
+        }
 
         snprintf(buff, sizeof buff, "%d", mask);
-        hw_sensors_send(hw, (const uint8_t*)buff, strlen(buff));
+        _hwSensorClient_send(cl, (const uint8_t*)buff, strlen(buff));
         return;
     }
 
@@ -365,7 +351,7 @@ hw_sensors_receive( HwSensors*  hw, uint8_t*  msg, int  msglen )
      * the channel. It is used to exit a blocking read.
      */
     if (msglen == 4 && !memcmp(msg, "wake", 4)) {
-        hw_sensors_send(hw, (const uint8_t*)"wake", 4);
+        _hwSensorClient_send(cl, (const uint8_t*)"wake", 4);
         return;
     }
 
@@ -373,9 +359,9 @@ hw_sensors_receive( HwSensors*  hw, uint8_t*  msg, int  msglen )
      * between sensor events
      */
     if (msglen > 10 && !memcmp(msg, "set-delay:", 10)) {
-        hw->delay_ms = atoi((const char*)msg+10);
-        if (hw->enabledMask != 0)
-            hw_sensors_timer_tick(hw);
+        cl->delay_ms = atoi((const char*)msg+10);
+        if (cl->enabledMask != 0)
+            _hwSensorClient_tick(cl);
 
         return;
     }
@@ -385,7 +371,7 @@ hw_sensors_receive( HwSensors*  hw, uint8_t*  msg, int  msglen )
      */
     if (msglen > 4 && !memcmp(msg, "set:", 4)) {
         char*  q;
-        int    id, enabled, oldEnabledMask = hw->enabledMask;
+        int    id, enabled, oldEnabledMask = cl->enabledMask;
         msg += 4;
         q    = strchr((char*)msg, ':');
         if (q == NULL) {  /* should not happen */
@@ -395,32 +381,27 @@ hw_sensors_receive( HwSensors*  hw, uint8_t*  msg, int  msglen )
         *q++ = 0;
 
         id = _sensorIdFromName((const char*)msg);
-        if (id < 0) {
+        if (id < 0 || id >= MAX_SENSORS) {
             D("%s: ignore unknown sensor name '%s'", __FUNCTION__, msg);
             return;
         }
 
+        if (!hw->sensors[id].enabled) {
+            D("%s: trying to set disabled %s sensor", __FUNCTION__, msg);
+            return;
+        }
         enabled = (q[0] == '1');
 
-        hw->sensors[id].enabled = (char) enabled;
         if (enabled)
-            hw->enabledMask |= (1 << id);
+            cl->enabledMask |= (1 << id);
         else
-            hw->enabledMask &= ~(1 << id);
-
-        D("%s: %s %s sensor", __FUNCTION__,
-          hw->sensors[id].enabled ? "enabling" : "disabling",  msg);
+            cl->enabledMask &= ~(1 << id);
 
-        if (oldEnabledMask == 0 && enabled) {
-            /* we enabled our first sensor, start event reporting */
-            D("%s: starting event reporting (mask=%04x)", __FUNCTION__,
-              hw->enabledMask);
-        }
-        else if (hw->enabledMask == 0 && !enabled) {
-            /* we disabled our last sensor, stop event reporting */
-            D("%s: stopping event reporting", __FUNCTION__);
+        if (cl->enabledMask != oldEnabledMask) {
+            D("%s: %s %s sensor", __FUNCTION__,
+                (cl->enabledMask & (1 << id))  ? "enabling" : "disabling",  msg);
         }
-        hw_sensors_timer_tick(hw);
+        _hwSensorClient_tick(cl);
         return;
     }
 
@@ -428,6 +409,110 @@ hw_sensors_receive( HwSensors*  hw, uint8_t*  msg, int  msglen )
 }
 
 
+static QemudClient*
+_hwSensors_connect( void*  opaque, QemudService*  service, int  channel )
+{
+    HwSensors*       sensors = opaque;
+    HwSensorClient*  cl      = _hwSensorClient_new(sensors);
+    QemudClient*     client  = qemud_client_new(service, channel, cl,
+                                                _hwSensorClient_recv,
+                                                _hwSensorClient_close);
+    qemud_client_set_framing(client, 1);
+    cl->client = client;
+
+    return client;
+}
+
+/* change the value of the emulated acceleration vector */
+static void
+_hwSensors_setAcceleration( HwSensors*  h, float x, float y, float z )
+{
+    Sensor*  s = &h->sensors[ANDROID_SENSOR_ACCELERATION];
+    s->u.acceleration.x = x;
+    s->u.acceleration.y = y;
+    s->u.acceleration.z = z;
+}
+
+#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 coarse orientation (landscape/portrait) of the emulated device */
+static void
+_hwSensors_setCoarseOrientation( HwSensors*  h, AndroidCoarseOrientation  orient )
+{
+    /* The Android framework computes the orientation by looking at
+     * the accelerometer sensor (*not* the orientation sensor !)
+     *
+     * That's because the gravity is a constant 9.81 vector that
+     * can be determined quite easily.
+     *
+     * Also, for some reason, the framework code considers that the phone should
+     * be inclined by 30 degrees along the phone's X axis to be considered
+     * in its ideal "vertical" position
+     *
+     * If the phone is completely vertical, rotating it will not do anything !
+     */
+    const double  g      = 9.81;
+    const double  cos_30 = 0.866025403784;
+    const double  sin_30 = 0.5;
+
+    switch (orient) {
+    case ANDROID_COARSE_PORTRAIT:
+        _hwSensors_setAcceleration( h, 0., g*cos_30, g*sin_30 );
+        break;
+
+    case ANDROID_COARSE_LANDSCAPE:
+        _hwSensors_setAcceleration( h, g*cos_30, 0., g*sin_30 );
+        break;
+    default:
+        ;
+    }
+}
+
+
+/* initialize the sensors state */
+static void
+_hwSensors_init( HwSensors*  h )
+{
+    h->service = qemud_service_register("sensors", 0, h,
+                                        _hwSensors_connect );
+
+    if (android_hw->hw_accelerometer)
+        h->sensors[ANDROID_SENSOR_ACCELERATION].enabled = 1;
+
+    /* XXX: TODO: Add other tests when we add the corresponding
+        * properties to hardware-properties.ini et al. */
+
+    _hwSensors_setCoarseOrientation(h, ANDROID_COARSE_PORTRAIT);
+}
+
 static HwSensors    _sensorsState[1];
 
 void
@@ -436,7 +521,7 @@ android_hw_sensors_init( void )
     HwSensors*  hw = _sensorsState;
 
     if (hw->service == NULL) {
-        hw_sensors_init(hw);
+        _hwSensors_init(hw);
         D("%s: sensors qemud service initialized", __FUNCTION__);
     }
 }
@@ -446,6 +531,6 @@ extern void
 android_sensors_set_coarse_orientation( AndroidCoarseOrientation  orient )
 {
     android_hw_sensors_init();
-    hw_sensors_set_coarse_orientation(_sensorsState, orient);
+    _hwSensors_setCoarseOrientation(_sensorsState, orient);
 }