add goldfish_sensors: basic support for pipe based accelerometer

This imports a small chunk of the old hw-sensors code to handle the
acceleration code. The old code "supported" a bunch of different sensors
however I've dropped all the boilerplate for that. Any potential
upstream solution will probably want to enumerate things differently.

Apart from only supporting the accelerometer this also interfaces
directly with the AndroidPipe code instead of going through the qemud
layer. It also requires the updated guest code.

Signed-off-by: Alex Bennée <alex.bennee@linaro.org>
[CD: fix time report to ms, initialize state]
Signed-off-by: Christoffer Dall <christoffer.dall@linaro.org>

4 files changed, 357 insertions, 0 deletions

diff --git a/hw/input/Makefile.objs b/hw/input/Makefile.objs
index c188a74b0..843358a08 100644
--- a/hw/input/Makefile.objs
+++ b/hw/input/Makefile.objs
@@ -12,3 +12,4 @@ obj-$(CONFIG_MILKYMIST) += milkymist-softusb.o
 obj-$(CONFIG_PXA2XX) += pxa2xx_keypad.o
 obj-$(CONFIG_TSC210X) += tsc210x.o
 obj-$(CONFIG_GOLDFISH) += goldfish_events.o
+obj-${CONFIG_GOLDFISH} += goldfish_sensors.o
diff --git a/hw/input/goldfish_sensors.c b/hw/input/goldfish_sensors.c
new file mode 100644
index 000000000..f15e69cef
--- /dev/null
+++ b/hw/input/goldfish_sensors.c
@@ -0,0 +1,353 @@
+/*
+ * Goldfish 'sensors' device model
+ *
+ * This provides a android pipe based device to report sensor data to
+ * the android goldfish emulation platform. It is a very much cut-down
+ * version of the original "hw-sensors.c" from the Android Emulator.
+ * The main difference is it only supports the accelerometer and
+ * directly hooks into the AndroidPipe mechanism without the qemud
+ * machinery.
+ *
+ * Copyright (c) 2014 Linaro Limited
+ * Copyright (c) 2009 The Android Open Source Project
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "sysemu/char.h"
+#include "qemu/error-report.h"
+#include "qemu/timer.h"
+#include "trace.h"
+#include "hw/misc/android_pipe.h"
+#include "hw/misc/android_qemud.h"
+
+/* #define DEBUG_SENSORS */
+
+#ifdef DEBUG_SENSORS
+#define DPRINTF(fmt, ...) \
+    do { fprintf(stderr, "%s: " fmt , __func__, ## __VA_ARGS__); } while (0)
+#else
+#define DPRINTF(fmt, ...) do {} while (0)
+#endif
+
+enum {
+    ANDROID_SENSOR_ACCELERATION
+} SensorID_t;
+
+/* The sensor configuration is global to whole driver */
+typedef struct SensorsConfig {
+    /* Timer tick stuff */
+    int         delay_ms;
+
+    /* Sensor data */
+    int         enabled_mask;
+    struct {
+        struct {
+            float   x, y, z;
+        } acceleration;
+    } sensors;
+} SensorsConfig;
+
+struct SensorsConfig sensor_config = {
+    800,
+    0,
+    {
+        .acceleration = { 0.0, 0.0, 0.0 }
+    }
+};
+
+/* There will be one pipe connection per open connection with the
+ * guest. Generally there will be a control path used to interrogate
+ * the list of sensors and a data path which will trigger the periodic
+ * updating of the the data
+ */
+typedef struct SensorsPipe {
+    /* Android Pipe structs */
+    void      *hwpipe;
+    /* Sensor Ticks */
+    QEMUTimer *periodic_tick;
+    /* Arrays containing framed strings, outstanding is used for
+     * staging the frames as they come in/go out.
+     */
+    GPtrArray *send_data;
+    GString   *send_outstanding;
+    GPtrArray *recv_data;
+    GString   *recv_outstanding;
+} SensorsPipe;
+
+
+/* I'll just append to a GString holding our data here, which we
+ * truncate as we page back out....
+ */
+static void sensor_send_data(SensorsPipe *sp, const uint8_t *buf, int len)
+{
+    gchar *msg = g_strndup((gchar *) buf, len);
+    g_ptr_array_add(sp->send_data, msg);
+    android_pipe_wake(sp->hwpipe, PIPE_WAKE_READ);
+}
+
+
+/*
+ * - when the qemu-specific sensors HAL module starts, it sends
+ *   "list-sensors"
+ *
+ * - this code replies with a string containing an integer corresponding
+ *   to a bitmap of available hardware sensors in the current AVD
+ *   configuration (e.g. "1" a.k.a (1 << ANDROID_SENSOR_ACCELERATION))
+ *
+ * - the HAL module sends "set:<sensor>:<flag>" to enable or disable
+ *   the report of a given sensor state. <sensor> must be the name of
+ *   a given sensor (e.g. "accelerometer"), and <flag> must be either
+ *   "1" (to enable) or "0" (to disable).
+ *
+ * - Once at least one sensor is "enabled", this code should periodically
+ *   send information about the corresponding enabled sensors. The default
+ *   period is 200ms.
+ *
+ * - the HAL module sends "set-delay:<delay>", where <delay> is an integer
+ *   corresponding to a time delay in milli-seconds. This corresponds to
+ *   a new interval between sensor events sent by this code to the HAL
+ *   module.
+ *
+ * - the HAL module can also send a "wake" command. This code should simply
+ *   send the "wake" back to the module. This is used internally to wake a
+ *   blocking read that happens in a different thread. This ping-pong makes
+ *   the code in the HAL module very simple.
+ *
+ * - each timer tick, this code sends sensor reports in the following
+ *   format (each line corresponds to a different line sent to the module):
+ *
+ *      acceleration:<x>:<y>:<z>
+ *      sync:<time_us>
+ *
+ *   Where each line before the sync:<time_us> is optional and will only
+ *   appear if the corresponding sensor has been enabled by the HAL module.
+ *
+ *   Note that <time_us> is the VM time in micro-seconds when the report
+ *   was "taken" by this code. This is adjusted by the HAL module to
+ *   emulated system time (using the first sync: to compute an adjustment
+ *   offset).
+ */
+
+/* this function is called periodically to send sensor reports
+ * to the HAL module, and re-arm the timer if necessary
+ */
+static void
+goldfish_sensor_tick(void *opaque)
+{
+    SensorsPipe *sp = opaque;
+    int64_t          delay = sensor_config.delay_ms;
+    int64_t          now_ms;
+    uint32_t         mask  = sensor_config.enabled_mask;
+    char             buffer[128];
+
+    if (sensor_config.enabled_mask & (1<<ANDROID_SENSOR_ACCELERATION)) {
+        snprintf(buffer, sizeof buffer, "acceleration:%g:%g:%g",
+                 sensor_config.sensors.acceleration.x,
+                 sensor_config.sensors.acceleration.y,
+                 sensor_config.sensors.acceleration.z);
+        sensor_send_data(sp, (uint8_t *)buffer, strlen(buffer));
+    }
+
+    now_ms = qemu_clock_get_ms(QEMU_CLOCK_VIRTUAL);
+
+    snprintf(buffer, sizeof buffer, "sync:%" PRId64, now_ms * 1000);
+    sensor_send_data(sp, (uint8_t *)buffer, strlen(buffer));
+
+    /* rearm timer, use a minimum delay of 20 ms, just to
+     * be safe.
+     */
+    if (mask == 0) {
+        return;
+    }
+
+    if (delay < 20) {
+        delay = 20;
+    }
+
+    timer_mod(sp->periodic_tick, now_ms + delay);
+}
+
+/* Incoming command from the guest */
+static ssize_t goldfish_sensors_have_data(SensorsPipe *sp, const gchar *buf)
+{
+    int len = strlen(buf);
+    /* "list-sensors" is used to get an integer bit map of
+     * available emulated sensors. We compute the mask from the
+     * current hardware configuration.
+     */
+    if (len == 12 && g_str_has_prefix(buf, "list-sensors")) {
+        sensor_send_data(sp, (const uint8_t *)"1", 1);
+        return len;
+    }
+
+    /* "wake" is a special message that must be sent back through
+     * the channel. It is used to exit a blocking read.
+     */
+    if (len == 4 && g_str_has_prefix(buf, "wake")) {
+        sensor_send_data(sp, (const uint8_t *)"wake", 4);
+        return len;
+    }
+
+    /* "set-delay:<delay>" is used to set the delay in milliseconds
+     * between sensor events
+     */
+    if (len > 10 && g_str_has_prefix(buf, "set-delay:")) {
+        sensor_config.delay_ms = atoi((const char *)buf+10);
+        if (sensor_config.enabled_mask != 0) {
+            goldfish_sensor_tick(sp);
+        }
+        return len;
+    }
+
+    /* "set:<name>:<state>" is used to enable/disable a given
+     * sensor. <state> must be 0 or 1
+     */
+    if (len > 4 && g_str_has_prefix(buf, "set:")) {
+        int mask = 0;
+        gchar *state = g_strrstr_len(buf, len, ":");
+        if (g_str_has_prefix(buf, "set:acceleration")) {
+            mask = (1<<ANDROID_SENSOR_ACCELERATION);
+        }
+        if (state && mask) {
+            switch (state[1]) {
+            case '0':
+                sensor_config.enabled_mask &= ~mask;
+                break;
+            case '1':
+                sensor_config.enabled_mask |= mask;
+                goldfish_sensor_tick(sp);
+                break;
+            default:
+                DPRINTF("bad set: command (%s)", buf);
+                break;
+            }
+        }
+        return len;
+    }
+
+    return len;
+}
+
+static void *sensors_pipe_init(void *hwpipe, void *opaque, const char *args)
+{
+    SensorsPipe *pipe;
+
+    DPRINTF("hwpipe=%p\n", hwpipe);
+    pipe = g_malloc0(sizeof(SensorsPipe));
+    pipe->hwpipe = hwpipe;
+    pipe->periodic_tick = timer_new_ms(QEMU_CLOCK_VIRTUAL,
+                                       goldfish_sensor_tick, pipe);
+    pipe->send_data = g_ptr_array_new_full(8, g_free);
+    pipe->recv_data = g_ptr_array_new_full(8, g_free);
+    pipe->send_outstanding = g_string_sized_new(128);
+    pipe->recv_outstanding = g_string_sized_new(128);
+    return pipe;
+}
+
+static void sensors_pipe_close(void *opaque)
+{
+    SensorsPipe *pipe = opaque;
+    DPRINTF("pipe %p, hwpipe %p\n", pipe, pipe->hwpipe);
+    timer_del(pipe->periodic_tick);
+    timer_free(pipe->periodic_tick);
+    pipe->periodic_tick = NULL;
+    g_ptr_array_free(pipe->send_data, TRUE);
+    g_ptr_array_free(pipe->recv_data, TRUE);
+    g_string_free(pipe->send_outstanding, TRUE);
+    g_string_free(pipe->recv_outstanding, TRUE);
+    g_free(pipe);
+}
+
+static void sensors_pipe_wake(void *opaque, int flags)
+{
+    SensorsPipe *pipe = opaque;
+
+    /* we have data for the guest to read */
+    if (flags & PIPE_WAKE_READ
+        && (pipe->send_outstanding->len > 0 || pipe->send_data->len > 0)) {
+        DPRINTF("0x%x:PIPE_WAKE_READ we have %d bytes, %d msgs\n", flags,
+                (int) pipe->send_outstanding->len,
+                pipe->send_data->len);
+        android_pipe_wake(pipe->hwpipe, PIPE_WAKE_READ);
+    }
+
+    /* we can always be written to... */
+    if (flags & PIPE_WAKE_WRITE) {
+        DPRINTF("0x%x:PIPE_WAKE_WRITE\n", flags);
+        android_pipe_wake(pipe->hwpipe, PIPE_WAKE_WRITE);
+    }
+}
+
+static int sensors_pipe_recv(void *opaque, AndroidPipeBuffer *buffers,
+                             int cnt)
+{
+    SensorsPipe *pipe = opaque;
+
+    DPRINTF("%d outstanding msgs\n", pipe->send_data->len);
+    if (pipe->send_outstanding->len > 0 || pipe->send_data->len > 0) {
+        return qemud_pipe_write_buffers(pipe->send_data, pipe->send_outstanding,
+                                        buffers, cnt);
+    } else {
+        return PIPE_ERROR_AGAIN;
+    }
+}
+
+/*
+ * We use the qemud helper functions to de-serialise the qemud framed
+ * messages into a series of strings. We then just process each entry
+ * in the array and free the strings when done.
+ */
+static int sensors_pipe_send(void *opaque, const AndroidPipeBuffer* buffers,
+                             int cnt)
+{
+    SensorsPipe *pipe = opaque;
+    int consumed = qemud_pipe_read_buffers(buffers, cnt,
+                                           pipe->recv_data,
+                                           pipe->recv_outstanding);
+
+    DPRINTF("pipe %p, messages: %d\n", pipe, pipe->recv_data->len);
+
+    while (pipe->recv_data->len > 0) {
+        gchar *msg = g_ptr_array_index(pipe->recv_data, 0);
+        goldfish_sensors_have_data(pipe, msg);
+        g_ptr_array_remove(pipe->recv_data, msg);
+    }
+
+    return consumed;
+}
+
+static unsigned sensors_pipe_poll(void *opaque)
+{
+    SensorsPipe *pipe = opaque;
+    unsigned flags = 0;
+
+    if (pipe->send_outstanding->len > 0 || pipe->send_data->len > 0) {
+        flags |= PIPE_POLL_IN;
+    }
+    flags |= PIPE_POLL_OUT;
+    DPRINTF("pipe %p, flags 0x%x", pipe, flags);
+    return flags;
+}
+
+static const AndroidPipeFuncs sensors_pipe_funcs = {
+    sensors_pipe_init,
+    sensors_pipe_close,
+    sensors_pipe_send,
+    sensors_pipe_recv,
+    sensors_pipe_poll,
+    sensors_pipe_wake,
+};
+
+void android_sensors_init(void)
+{
+    goldfish_sensors_set_rotation(0);
+    android_pipe_add_type("qemud:sensors", NULL, &sensors_pipe_funcs);
+}
diff --git a/hw/misc/android_pipe.c b/hw/misc/android_pipe.c
index 7b5dd7422..17a7a47bf 100644
--- a/hw/misc/android_pipe.c
+++ b/hw/misc/android_pipe.c
@@ -789,6 +789,7 @@ static void android_pipe_realize(DeviceState *dev, Error **errp)
     android_zero_pipe_init();
     android_pingpong_init();
     android_throttle_init();
+    android_sensors_init();
 
     /* TODO: This may be a complete hack and there may be beautiful QOM ways
      * to accomplish this.
@@ -796,6 +797,7 @@ static void android_pipe_realize(DeviceState *dev, Error **errp)
      * Initialize android pipe backends
      */
     android_adb_dbg_backend_init();
+
 }
 
 void
diff --git a/include/hw/misc/android_pipe.h b/include/hw/misc/android_pipe.h
index 2d98fa6d8..3fe630faa 100644
--- a/include/hw/misc/android_pipe.h
+++ b/include/hw/misc/android_pipe.h
@@ -228,6 +228,7 @@ extern void android_pingpong_init(void);
 extern void android_throttle_init(void);
 extern void android_adb_dbg_backend_init(void);
 extern void android_adb_backend_init(void);
+extern void android_sensors_init(void);
 
 extern bool adb_server_init(int port);