Added state snapshot support for QemuD.

With this patch, both modem and sensor functionality are correctly
restored when a state snapshot is loaded. This was not the case
previously because communication with either of these services is
done using the qemud daemon, which did not support snapshots.

The boot-properties and charpipe services have no specific save/load
functionality yet, since the framework itself should be reviewed
first. Adding support for bootproperties should not be difficult
though, and charpipe may not need it.

For a description of the high-level process for saving and loading,
consult section IV "State snapshots" in docs/ANDROID-QEMUD.TXT.

Change-Id: I5b06d88b911ca096e78060163174904c48a01c66

9 files changed, 719 insertions, 49 deletions

diff --git a/android/boot-properties.c b/android/boot-properties.c
index 7810b0d..c094202 100644
--- a/android/boot-properties.c
+++ b/android/boot-properties.c
@@ -155,7 +155,7 @@ boot_property_service_connect( void*          opaque,
 
     client = qemud_client_new( serv, channel, NULL,
                                boot_property_client_recv,
-                               NULL );
+                               NULL, NULL, NULL );
 
     qemud_client_set_framing(client, 1);
     return client;
@@ -168,7 +168,8 @@ boot_property_init_service( void )
     if (!_inited) {
         QemudService*  serv = qemud_service_register( SERVICE_NAME,
                                                     1, NULL,
-                                                    boot_property_service_connect );
+                                                    boot_property_service_connect,
+                                                    NULL, NULL);
         if (serv == NULL) {
             derror("could not register '%s' service", SERVICE_NAME);
             return;
diff --git a/android/hw-control.c b/android/hw-control.c
index d99f7e3..6dac8c2 100644
--- a/android/hw-control.c
+++ b/android/hw-control.c
@@ -68,7 +68,7 @@ _hw_control_qemud_connect( void*  opaque, QemudService*  service, int  channel )
     client = qemud_client_new( service, channel,
                                opaque,
                                _hw_control_qemud_client_recv,
-                               NULL );
+                               NULL, NULL, NULL );
 
     qemud_client_set_framing(client, 1);
     return client;
@@ -123,7 +123,8 @@ hw_control_init( HwControl*                    control,
     control->client_funcs = client_funcs[0];
     control->service      = qemud_service_register( "hw-control", 0,
                                                     control,
-                                                    _hw_control_qemud_connect );
+                                                    _hw_control_qemud_connect,
+                                                    NULL, NULL);
 }
 
 void
diff --git a/android/hw-qemud.c b/android/hw-qemud.c
index 2100ce6..8f92bcd 100644
--- a/android/hw-qemud.c
+++ b/android/hw-qemud.c
@@ -14,6 +14,7 @@
 #include "android/utils/misc.h"
 #include "android/utils/system.h"
 #include "android/utils/bufprint.h"
+#include "hw/hw.h"
 #include "qemu-char.h"
 #include "charpipe.h"
 #include "cbuffer.h"
@@ -39,6 +40,11 @@
  */
 #define  MAX_FRAME_PAYLOAD  65535
 
+/* Version number of snapshots code. Increment whenever the data saved
+ * or the layout in which it is saved is changed.
+ */
+#define QEMUD_SAVE_VERSION 1
+
 
 /* define SUPPORT_LEGACY_QEMUD to 1 if you want to support
  * talking to a legacy qemud daemon. See docs/ANDROID-QEMUD.TXT
@@ -89,18 +95,43 @@
  * read a fixed amount of bytes into a buffer
  */
 typedef struct QemudSink {
-    int       len;
-    int       size;
+    int       used;  /* number of bytes already used */
+    int       size;  /* total number of bytes in buff */
     uint8_t*  buff;
 } QemudSink;
 
+/* save the state of a QemudSink to a snapshot.
+ *
+ * The buffer pointer is not saved, since it usually points to buffer
+ * fields in other structs, which have save functions themselves. It
+ * is up to the caller to make sure the buffer is correctly saved and
+ * restored.
+ */
+static void
+qemud_sink_save(QEMUFile* f, QemudSink* s)
+{
+    qemu_put_be32(f, s->used);
+    qemu_put_be32(f, s->size);
+}
+
+/* load the state of a QemudSink from a snapshot.
+ */
+static int
+qemud_sink_load(QEMUFile* f, QemudSink* s)
+{
+    s->used = qemu_get_be32(f);
+    s->size = qemu_get_be32(f);
+    return 0;
+}
+
+
 /* reset a QemudSink, i.e. provide a new destination buffer address
  * and its size in bytes.
  */
 static void
 qemud_sink_reset( QemudSink*  ss, int  size, uint8_t*  buffer )
 {
-    ss->len  = 0;
+    ss->used = 0;
     ss->size = size;
     ss->buff = buffer;
 }
@@ -114,7 +145,7 @@ qemud_sink_reset( QemudSink*  ss, int  size, uint8_t*  buffer )
 static int
 qemud_sink_fill( QemudSink*  ss, const uint8_t* *pmsg, int  *plen)
 {
-    int  avail = ss->size - ss->len;
+    int  avail = ss->size - ss->used;
 
     if (avail <= 0)
         return 1;
@@ -122,12 +153,12 @@ qemud_sink_fill( QemudSink*  ss, const uint8_t* *pmsg, int  *plen)
     if (avail > *plen)
         avail = *plen;
 
-    memcpy(ss->buff + ss->len, *pmsg, avail);
+    memcpy(ss->buff + ss->used, *pmsg, avail);
     *pmsg += avail;
     *plen -= avail;
-    ss->len += avail;
+    ss->used += avail;
 
-    return (ss->len == ss->size);
+    return (ss->used == ss->size);
 }
 
 /* returns the number of bytes needed to fill a sink's destination
@@ -136,7 +167,7 @@ qemud_sink_fill( QemudSink*  ss, const uint8_t* *pmsg, int  *plen)
 static int
 qemud_sink_needed( QemudSink*  ss )
 {
-    return ss->size - ss->len;
+    return ss->size - ss->used;
 }
 
 /** HANDLING SERIAL PORT CONNECTION
@@ -203,6 +234,66 @@ typedef struct QemudSerial {
 } QemudSerial;
 
 
+/* Save the state of a QemudSerial to a snapshot file.
+ */
+static void
+qemud_serial_save(QEMUFile* f, QemudSerial* s)
+{
+    /* cs, recv_func and recv_opaque are not saved, as these are assigned only
+     * during emulator init. A load within a session can re-use the values
+     * already assigned, a newly launched emulator has freshly assigned values.
+     */
+
+    /* state of incoming packets from the serial port */
+    qemu_put_be32(f, s->need_header);
+    qemu_put_be32(f, s->overflow);
+    qemu_put_be32(f, s->in_size);
+    qemu_put_be32(f, s->in_channel);
+#if SUPPORT_LEGACY_QEMUD
+    qemu_put_be32(f, s->version);
+#endif
+    qemud_sink_save(f, s->header);
+    qemud_sink_save(f, s->payload);
+    qemu_put_be32(f, MAX_SERIAL_PAYLOAD+1);
+    qemu_put_buffer(f, s->data0, MAX_SERIAL_PAYLOAD+1);
+}
+
+/* Load the state of a QemudSerial from a snapshot file.
+ */
+static int
+qemud_serial_load(QEMUFile* f, QemudSerial* s)
+{
+    /* state of incoming packets from the serial port */
+    s->need_header = qemu_get_be32(f);
+    s->overflow    = qemu_get_be32(f);
+    s->in_size     = qemu_get_be32(f);
+    s->in_channel  = qemu_get_be32(f);
+#if SUPPORT_LEGACY_QEMUD
+    s->version = qemu_get_be32(f);
+#endif
+    qemud_sink_load(f, s->header);
+    qemud_sink_load(f, s->payload);
+
+    /* s->header and s->payload are only ever connected to s->data0 */
+    s->header->buff = s->payload->buff = s->data0;
+
+    int len = qemu_get_be32(f);
+    if (len - 1 > MAX_SERIAL_PAYLOAD) {
+        D("%s: load failed: size of saved payload buffer (%d) exceeds "
+          "current maximum (%d)\n",
+          __FUNCTION__, len - 1, MAX_SERIAL_PAYLOAD);
+        return -EIO;
+    }
+    int ret;
+    if ((ret = qemu_get_buffer(f, s->data0, len)) != len) {
+        D("%s: failed to load serial buffer contents (tried reading %d bytes, got %d)\n",
+          __FUNCTION__, len, ret);
+        return -EIO;
+    }
+
+    return 0;
+}
+
 /* called by the charpipe to see how much bytes can be
  * read from the serial port.
  */
@@ -282,7 +373,7 @@ qemud_serial_read( void*  opaque, const uint8_t*  from, int  len )
             s->in_size     = hex2int( s->data0 + LENGTH_OFFSET,  LENGTH_SIZE );
             s->in_channel  = hex2int( s->data0 + CHANNEL_OFFSET, CHANNEL_SIZE );
 #endif
-            s->header->len = 0;
+            s->header->used = 0;
 
             if (s->in_size <= 0 || s->in_channel < 0) {
                 D("%s: bad header: '%.*s'", __FUNCTION__, HEADER_SIZE, s->data0);
@@ -495,6 +586,8 @@ struct QemudClient {
     void*             clie_opaque;
     QemudClientRecv   clie_recv;
     QemudClientClose  clie_close;
+    QemudClientSave   clie_save;
+    QemudClientLoad   clie_load;
     QemudService*     service;
     QemudClient*      next_serv; /* next in same service */
     QemudClient*      next;
@@ -597,7 +690,7 @@ qemud_client_recv( void*  opaque, uint8_t*  msg, int  msglen )
             AARRAY_NEW(data, frame_size+1);  /* +1 for terminating zero */
             qemud_sink_reset(c->payload, frame_size, data);
             c->need_header = 0;
-            c->header->len = 0;
+            c->header->used = 0;
         }
 
         /* read the payload */
@@ -659,6 +752,8 @@ qemud_client_alloc( int               channel_id,
                     void*             clie_opaque,
                     QemudClientRecv   clie_recv,
                     QemudClientClose  clie_close,
+                    QemudClientSave   clie_save,
+                    QemudClientLoad   clie_load,
                     QemudSerial*      serial,
                     QemudClient**     pclients )
 {
@@ -671,6 +766,8 @@ qemud_client_alloc( int               channel_id,
     c->clie_opaque = clie_opaque;
     c->clie_recv   = clie_recv;
     c->clie_close  = clie_close;
+    c->clie_save   = clie_save;
+    c->clie_load   = clie_load;
 
     c->framing     = 0;
     c->need_header = 1;
@@ -681,6 +778,117 @@ qemud_client_alloc( int               channel_id,
     return c;
 }
 
+/* forward */
+static void  qemud_service_save_name( QEMUFile* f, QemudService* s );
+static char* qemud_service_load_name( QEMUFile* f );
+static QemudService* qemud_service_find(  QemudService*  service_list,
+                                          const char*    service_name );
+static QemudClient*  qemud_service_connect_client(  QemudService  *sv,
+                                                    int           channel_id );
+
+/* Saves the client state needed to re-establish connections on load.
+ */
+static void
+qemud_client_save(QEMUFile* f, QemudClient* c)
+{
+    /* save generic information */
+    qemud_service_save_name(f, c->service);
+    qemu_put_be32(f, c->channel);
+
+    /* save client-specific state */
+    if (c->clie_save)
+        c->clie_save(f, c, c->clie_opaque);
+
+    /* save framing configuration */
+    qemu_put_be32(f, c->framing);
+    if (c->framing) {
+        qemu_put_be32(f, c->need_header);
+        /* header sink always connected to c->header0, no need to save */
+        qemu_put_be32(f, FRAME_HEADER_SIZE);
+        qemu_put_buffer(f, c->header0, FRAME_HEADER_SIZE);
+        /* payload sink */
+        qemud_sink_save(f, c->payload);
+        qemu_put_buffer(f, c->payload->buff, c->payload->size);
+    }
+}
+
+/* Loads client state from file, then starts a new client connected to the
+ * corresponding service.
+ */
+static int
+qemud_client_load(QEMUFile* f, QemudService* current_services )
+{
+    char *service_name = qemud_service_load_name(f);
+    if (service_name == NULL)
+        return -EIO;
+
+    /* get current service instance */
+    QemudService *sv = qemud_service_find(current_services, service_name);
+    if (sv == NULL) {
+        D("%s: load failed: unknown service \"%s\"\n",
+          __FUNCTION__, service_name);
+        return -EIO;
+    }
+
+    /* get channel id */
+    int channel = qemu_get_be32(f);
+    if (channel == 0) {
+        D("%s: illegal snapshot: client for control channel must no be saved\n",
+          __FUNCTION__);
+        return -EIO;
+    }
+
+    /* re-connect client */
+    QemudClient* c = qemud_service_connect_client(sv, channel);
+    if(c == NULL)
+        return -EIO;
+
+    /* load client-specific state */
+    int ret;
+    if (c->clie_load)
+        if ((ret = c->clie_load(f, c, c->clie_opaque)))
+            return ret;  /* load failure */
+
+    /* load framing configuration */
+    c->framing = qemu_get_be32(f);
+    if (c->framing) {
+
+        /* header buffer */
+        c->need_header = qemu_get_be32(f);
+        int header_size = qemu_get_be32(f);
+        if (header_size > FRAME_HEADER_SIZE) {
+            D("%s: load failed: payload buffer requires %d bytes, %d available\n",
+              __FUNCTION__, header_size, FRAME_HEADER_SIZE);
+            return -EIO;
+        }
+        int ret;
+        if ((ret = qemu_get_buffer(f, c->header0, header_size)) != header_size) {
+            D("%s: frame header buffer load failed: expected %d bytes, got %d\n",
+              __FUNCTION__, header_size, ret);
+            return -EIO;
+        }
+
+        /* payload sink */
+        if ((ret = qemud_sink_load(f, c->payload)))
+            return ret;
+
+        /* replace payload buffer by saved data */
+        if (c->payload->buff) {
+            AFREE(c->payload->buff);
+        }
+        AARRAY_NEW(c->payload->buff, c->payload->size+1);  /* +1 for terminating zero */
+        if ((ret = qemu_get_buffer(f, c->payload->buff, c->payload->size)) != c->payload->size) {
+            D("%s: frame payload buffer load failed: expected %d bytes, got %d\n",
+              __FUNCTION__, c->payload->size, ret);
+            AFREE(c->payload->buff);
+            return -EIO;
+        }
+    }
+
+    return 0;
+}
+
+
 /** SERVICES
  **/
 
@@ -701,6 +909,8 @@ struct QemudService {
     int                  num_clients;
     QemudClient*         clients;
     QemudServiceConnect  serv_connect;
+    QemudServiceSave     serv_save;
+    QemudServiceLoad     serv_load;
     void*                serv_opaque;
     QemudService*        next;
 };
@@ -711,6 +921,8 @@ qemud_service_new( const char*          name,
                    int                  max_clients,
                    void*                serv_opaque,
                    QemudServiceConnect  serv_connect,
+                   QemudServiceSave     serv_save,
+                   QemudServiceLoad     serv_load,
                    QemudService**       pservices )
 {
     QemudService*  s;
@@ -723,6 +935,8 @@ qemud_service_new( const char*          name,
 
     s->serv_opaque  = serv_opaque;
     s->serv_connect = serv_connect;
+    s->serv_save = serv_save;
+    s->serv_load = serv_load;
 
     s->next    = *pservices;
     *pservices = s;
@@ -765,6 +979,127 @@ qemud_service_remove_client( QemudService*  s, QemudClient*  c )
     s->num_clients -= 1;
 }
 
+/* ask the service to create a new QemudClient. Note that we
+ * assume that this calls qemud_client_new() which will add
+ * the client to the service's list automatically.
+ *
+ * returns the client or NULL if an error occurred
+ */
+static QemudClient*
+qemud_service_connect_client(QemudService *sv, int channel_id)
+{
+    QemudClient* client = sv->serv_connect( sv->serv_opaque, sv, channel_id );
+    if (client == NULL) {
+        D("%s: registration failed for '%s' service",
+          __FUNCTION__, sv->name);
+        return NULL;
+    }
+
+    D("%s: registered client channel %d for '%s' service",
+      __FUNCTION__, channel_id, sv->name);
+    return client;
+}
+
+/* find a registered service by name.
+ */
+static QemudService*
+qemud_service_find( QemudService*  service_list, const char*  service_name)
+{
+    QemudService*  sv = NULL;
+    for (sv = service_list; sv != NULL; sv = sv->next) {
+        if (!strcmp(sv->name, service_name)) {
+            break;
+        }
+    }
+    return sv;
+}
+
+/* Save the name of the given service.
+ */
+static void
+qemud_service_save_name(QEMUFile* f, QemudService* s)
+{
+    int len = strlen(s->name) + 1;  // include '\0' terminator
+    qemu_put_be32(f, len);
+    qemu_put_buffer(f, (const uint8_t *) s->name, len);
+}
+
+/* Load the name of a service. Returns a pointer to the loaded name, or NULL
+ * on failure.
+ */
+static char*
+qemud_service_load_name( QEMUFile*  f )
+{
+    int ret;
+    int name_len = qemu_get_be32(f);
+    char *service_name = android_alloc(name_len);
+    if ((ret = qemu_get_buffer(f, (uint8_t*)service_name, name_len) != name_len)) {
+        D("%s: service name load failed: expected %d bytes, got %d\n",
+          __FUNCTION__, name_len, ret);
+        AFREE(service_name);
+        return NULL;
+    }
+    if (service_name[name_len - 1] != '\0') {
+        char last = service_name[name_len - 1];
+        service_name[name_len - 1] = '\0';  /* make buffer contents printable */
+        D("%s: service name load failed: expecting NULL-terminated string, but "
+          "last char is '%c' (buffer contents: '%s%c')\n",
+          __FUNCTION__, name_len, last, service_name, last);
+        AFREE(service_name);
+        return NULL;
+    }
+
+    return service_name;
+}
+
+/* Saves state of a service.
+ */
+static void
+qemud_service_save(QEMUFile* f, QemudService* s)
+{
+    qemud_service_save_name(f, s);
+    qemu_put_be32(f, s->max_clients);
+    qemu_put_be32(f, s->num_clients);
+
+    if (s->serv_save)
+        s->serv_save(f, s, s->serv_opaque);
+}
+
+/* Loads service state from file, then updates the currently running instance
+ * of that service to mirror the loaded state. If the service is not running,
+ * the load process is aborted.
+ *
+ * Parameter 'current_services' should be the list of active services.
+ */
+static int
+qemud_service_load(  QEMUFile*  f, QemudService*  current_services  )
+{
+    char* service_name = qemud_service_load_name(f);
+    if (service_name == NULL)
+        return -EIO;
+
+    /* get current service instance */
+    QemudService *sv = qemud_service_find(current_services, service_name);
+    if (sv == NULL) {
+        D("%s: loading failed: service \"%s\" not available\n",
+          __FUNCTION__, service_name);
+        return -EIO;
+    }
+
+    /* reconfigure service as required */
+    sv->max_clients = qemu_get_be32(f);
+    sv->num_clients = qemu_get_be32(f);
+
+    /* load service specific data */
+    int ret;
+    if (sv->serv_load)
+        if ((ret = sv->serv_load(f, sv, sv->serv_opaque)))
+            return ret;  /* load failure */
+
+    return 0;
+}
+
+
 /** MULTIPLEXER
  **/
 
@@ -828,16 +1163,8 @@ qemud_multiplexer_connect( QemudMultiplexer*  m,
                            const char*        service_name,
                            int                channel_id )
 {
-    QemudService*  sv;
-    QemudClient*   client;
-
     /* find the corresponding registered service by name */
-    for (sv = m->services; sv != NULL; sv = sv->next) {
-        if (!strcmp(sv->name, service_name)) {
-            break;
-        }
-    }
-
+    QemudService*  sv = qemud_service_find(m->services, service_name);
     if (sv == NULL) {
         D("%s: no registered '%s' service", __FUNCTION__, service_name);
         return -1;
@@ -850,19 +1177,10 @@ qemud_multiplexer_connect( QemudMultiplexer*  m,
         return -2;
     }
 
-    /* ask the service to create a new QemudClient. Note that we
-     * assume that this calls qemud_client_new() which will add
-     * the client to the service's list automatically.
-     */
-    client = sv->serv_connect( sv->serv_opaque, sv, channel_id );
-    if (client == NULL) {
-        D("%s: registration failed for '%s' service",
-          __FUNCTION__, service_name);
+    /* connect a new client to the service on the given channel */
+    if (qemud_service_connect_client(sv, channel_id) == NULL)
         return -1;
-    }
 
-    D("%s: registered client channel %d for '%s' service",
-      __FUNCTION__, channel_id, service_name);
     return 0;
 }
 
@@ -890,6 +1208,32 @@ qemud_multiplexer_disconnect( QemudMultiplexer*  m,
       __FUNCTION__, channel);
 }
 
+/* disconnects all channels, except for the control channel, without informing
+ * the daemon in the guest that disconnection has occurred.
+ *
+ * Used to silently kill clients when restoring emulator state snapshots.
+ */
+static void
+qemud_multiplexer_disconnect_noncontrol( QemudMultiplexer*  m )
+{
+    QemudClient* c;
+    QemudClient* next = m->clients;
+
+    while (next) {
+        c = next;
+        next = c->next;  /* disconnect frees c, remember next in advance */
+
+        if (c->channel > 0) { /* skip control channel */
+            D("%s: disconnecting client %d",
+              __FUNCTION__, c->channel);
+            D("%s: disconnecting client %d\n",
+              __FUNCTION__, c->channel);
+            c->channel = -1; /* do not send disconnect:<id> */
+            qemud_client_disconnect(c);
+        }
+    }
+}
+
 /* handle control messages. This is used as the receive
  * callback for the special QemudClient setup to manage
  * channel 0.
@@ -1044,7 +1388,7 @@ qemud_multiplexer_init( QemudMultiplexer*  mult,
     control = qemud_client_alloc( 0,
                                   mult,
                                   qemud_multiplexer_control_recv,
-                                  NULL,
+                                  NULL, NULL, NULL,
                                   mult->serial,
                                   &mult->clients );
 }
@@ -1070,13 +1414,17 @@ qemud_client_new( QemudService*     service,
                   int               channelId,
                   void*             clie_opaque,
                   QemudClientRecv   clie_recv,
-                  QemudClientClose  clie_close )
+                  QemudClientClose  clie_close,
+                  QemudClientSave   clie_save,
+                  QemudClientLoad   clie_load )
 {
     QemudMultiplexer*  m = _multiplexer;
     QemudClient*       c = qemud_client_alloc( channelId,
                                                clie_opaque,
                                                clie_recv,
                                                clie_close,
+                                               clie_save,
+                                               clie_load,
                                                m->serial,
                                                &m->clients );
 
@@ -1120,6 +1468,132 @@ qemud_client_close( QemudClient*  client )
 }
 
 
+/** SNAPSHOT SUPPORT
+ **/
+
+/* Saves the number of clients.
+ */
+static void
+qemud_client_save_count(QEMUFile* f, QemudClient* c)
+{
+    unsigned int client_count = 0;
+    for( ; c; c = c->next)   // walk over linked list
+        if (c->channel > 0)  // skip control channel, which is not saved
+            client_count++;
+
+    qemu_put_be32(f, client_count);
+}
+
+/* Saves the number of services currently available.
+ */
+static void
+qemud_service_save_count(QEMUFile* f, QemudService* s)
+{
+    unsigned int service_count = 0;
+    for( ; s; s = s->next )  // walk over linked list
+        service_count++;
+
+    qemu_put_be32(f, service_count);
+}
+
+/* Save QemuD state to snapshot.
+ *
+ * The control channel has no state of its own, other than the local variables
+ * in qemud_multiplexer_control_recv. We can therefore safely skip saving it,
+ * which spares us dealing with the exception of a client not connected to a
+ * service.
+ */
+static void
+qemud_save(QEMUFile* f, void* opaque)
+{
+    QemudMultiplexer *m = opaque;
+
+    qemud_serial_save(f, m->serial);
+
+    /* save service states */
+    qemud_service_save_count(f, m->services);
+    QemudService *s;
+    for (s = m->services; s; s = s->next)
+        qemud_service_save(f, s);
+
+    /* save client channels */
+    qemud_client_save_count(f, m->clients);
+    QemudClient *c;
+    for (c = m->clients; c; c = c->next) {
+        if (c->channel > 0) {         /* skip control channel client */
+            qemud_client_save(f, c);
+        }
+    }
+
+}
+
+
+/* Checks whether the same services are available at this point as when the
+ * snapshot was made.
+ */
+static int
+qemud_load_services( QEMUFile*  f, QemudService*  current_services )
+{
+    int i, ret;
+    int service_count = qemu_get_be32(f);
+    for (i = 0; i < service_count; i++) {
+        if ((ret = qemud_service_load(f, current_services)))
+            return ret;
+    }
+
+    return 0;
+}
+
+/* Removes all active non-control clients, then creates new ones with state
+ * taken from the snapshot.
+ *
+ * We do not send "disconnect" commands, over the channel. If we did, we might
+ * stop clients in the restored guest, resulting in an incorrect restore.
+ *
+ * Instead, we silently replace the clients that were running before the
+ * restore with new clients, whose state we copy from the snapshot. Since
+ * everything is multiplexed over one link, only the multiplexer notices the
+ * changes, there is no communication with the guest.
+ */
+static int
+qemud_load_clients(QEMUFile* f, QemudMultiplexer* m )
+{
+    /* Remove all clients, except on the control channel.*/
+    qemud_multiplexer_disconnect_noncontrol(m);
+
+    /* Load clients from snapshot */
+    int client_count = qemu_get_be32(f);
+    int i, ret;
+    for (i = 0; i < client_count; i++) {
+        if ((ret = qemud_client_load(f, m->services))) {
+            return ret;
+        }
+    }
+
+    return 0;
+}
+
+/* Load QemuD state from file.
+ */
+static int
+qemud_load(QEMUFile *f, void* opaque, int version)
+{
+    QemudMultiplexer *m = opaque;
+
+    int ret;
+    if (version != QEMUD_SAVE_VERSION)
+        return -1;
+
+    if ((ret = qemud_serial_load(f, m->serial)))
+        return ret;
+    if ((ret = qemud_load_services(f, m->services)))
+        return ret;
+    if ((ret = qemud_load_clients(f, m)))
+        return ret;
+
+    return 0;
+}
+
 
 /* this is the end of the serial charpipe that must be passed
  * to the emulated tty implementation. The other end of the
@@ -1142,6 +1616,9 @@ android_qemud_init( void )
     }
 
     qemud_multiplexer_init(_multiplexer, cs);
+
+    register_savevm( "qemud", 0, QEMUD_SAVE_VERSION,
+                      qemud_save, qemud_load, _multiplexer);
 }
 
 /* return the serial charpipe endpoint that must be used
@@ -1172,7 +1649,9 @@ QemudService*
 qemud_service_register( const char*          service_name,
                         int                  max_clients,
                         void*                serv_opaque,
-                        QemudServiceConnect  serv_connect )
+                        QemudServiceConnect  serv_connect,
+                        QemudServiceSave     serv_save,
+                        QemudServiceLoad     serv_load )
 {
     QemudMultiplexer*  m  = _multiplexer;
     QemudService*      sv;
@@ -1184,6 +1663,8 @@ qemud_service_register( const char*          service_name,
                              max_clients,
                              serv_opaque,
                              serv_connect,
+                             serv_save,
+                             serv_load,
                              &m->services);
 
     return sv;
@@ -1282,7 +1763,8 @@ _qemud_char_service_connect( void*  opaque, QemudService*  sv, int  channel )
     QemudClient*       c  = qemud_client_new( sv, channel,
                                               cs,
                                               _qemud_char_client_recv,
-                                              _qemud_char_client_close);
+                                              _qemud_char_client_close,
+                                              NULL, NULL );
 
     /* now we can open the gates :-) */
     qemu_chr_add_handlers( cs,
@@ -1306,7 +1788,7 @@ android_qemud_get_channel( const char*  name, CharDriverState* *pcs )
         derror("can't open charpipe for '%s' qemud service", name);
         exit(2);
     }
-    qemud_service_register(name, 1, cs, _qemud_char_service_connect);
+    qemud_service_register(name, 1, cs, _qemud_char_service_connect, NULL, NULL);
     return 0;
 }
 
@@ -1322,6 +1804,7 @@ android_qemud_set_channel( const char*  name, CharDriverState*  peer_cs )
     if (char_buffer == NULL)
         return -1;
 
-    qemud_service_register(name, 1, char_buffer, _qemud_char_service_connect);
+    qemud_service_register(name, 1, char_buffer, _qemud_char_service_connect,
+                           NULL, NULL);
     return 0;
 }
diff --git a/android/hw-qemud.h b/android/hw-qemud.h
index 8aae74d..18eec6f 100644
--- a/android/hw-qemud.h
+++ b/android/hw-qemud.h
@@ -72,6 +72,16 @@ typedef void (*QemudClientClose)( void*  opaque );
  */
 typedef void (*QemudClientRecv) ( void*  opaque, uint8_t*  msg, int  msglen, QemudClient*  client );
 
+/* A function that will be called when the state of the client should be
+ * saved to a snapshot.
+ */
+typedef void (*QemudClientSave) ( QEMUFile*  f, QemudClient*  client, void*  opaque );
+
+/* A function that will be called when the state of the client should be
+ * restored from a snapshot.
+ */
+typedef int (*QemudClientLoad) ( QEMUFile*  f, QemudClient*  client, void*  opaque );
+
 /* Register a new client for a given service.
  * 'clie_opaque' will be sent as the first argument to 'clie_recv' and 'clie_close'
  * 'clie_recv' and 'clie_close' are both optional and may be NULL.
@@ -83,7 +93,9 @@ extern QemudClient*  qemud_client_new( QemudService*     service,
                                        int               channel_id,
                                        void*             clie_opaque,
                                        QemudClientRecv   clie_recv,
-                                       QemudClientClose  clie_close );
+                                       QemudClientClose  clie_close,
+                                       QemudClientSave   clie_save,
+                                       QemudClientLoad   clie_load );
 
 /* Enable framing on a given client channel.
  */
@@ -104,13 +116,25 @@ extern void   qemud_client_close( QemudClient*  client );
  */
 typedef QemudClient*  (*QemudServiceConnect)( void*   opaque, QemudService*  service, int  channel );
 
+/* A function that will be called when the state of the service should be
+ * saved to a snapshot.
+ */
+typedef void (*QemudServiceSave) ( QEMUFile*  f, QemudService*  service, void*  opaque );
+
+/* A function that will be called when the state of the service should be
+ * restored from a snapshot.
+ */
+typedef int (*QemudServiceLoad) ( QEMUFile*  f, QemudService*  service, void*  opaque );
+
 /* Register a new qemud service.
  * 'serv_opaque' is the first parameter to 'serv_connect'
  */
 extern QemudService*  qemud_service_register( const char*          serviceName,
                                               int                  max_clients,
                                               void*                serv_opaque,
-                                              QemudServiceConnect  serv_connect );
+                                              QemudServiceConnect  serv_connect,
+                                              QemudServiceSave     serv_save,
+                                              QemudServiceLoad     serv_load);
 
 /* Sends a message to all clients of a given service.
  */
diff --git a/android/hw-sensors.c b/android/hw-sensors.c
index 1fa12dc..690bb49 100644
--- a/android/hw-sensors.c
+++ b/android/hw-sensors.c
@@ -16,6 +16,7 @@
 #include "android/utils/system.h"
 #include "android/hw-qemud.h"
 #include "android/globals.h"
+#include "hw/hw.h"
 #include "qemu-char.h"
 #include "qemu-timer.h"
 
@@ -84,7 +85,7 @@ typedef struct {
 
 
 typedef struct {
-    char       enabled;
+    uint8_t  enabled;
     union {
         Acceleration   acceleration;
         MagneticField  magnetic;
@@ -408,6 +409,29 @@ _hwSensorClient_receive( HwSensorClient*  cl, uint8_t*  msg, int  msglen )
     D("%s: ignoring unknown query", __FUNCTION__);
 }
 
+/* Saves sensor-specific client data to snapshot */
+static void
+_hwSensorClient_save( QEMUFile*  f, QemudClient*  client, void*  opaque  )
+{
+    HwSensorClient* sc = opaque;
+
+    qemu_put_be32(f, sc->delay_ms);
+    qemu_put_be32(f, sc->enabledMask);
+    qemu_put_timer(f, sc->timer);
+}
+
+/* Loads sensor-specific client data from snapshot */
+static int
+_hwSensorClient_load( QEMUFile*  f, QemudClient*  client, void*  opaque  )
+{
+    HwSensorClient* sc = opaque;
+
+    sc->delay_ms = qemu_get_be32(f);
+    sc->enabledMask = qemu_get_be32(f);
+    qemu_get_timer(f, sc->timer);
+
+    return 0;
+}
 
 static QemudClient*
 _hwSensors_connect( void*  opaque, QemudService*  service, int  channel )
@@ -416,7 +440,9 @@ _hwSensors_connect( void*  opaque, QemudService*  service, int  channel )
     HwSensorClient*  cl      = _hwSensorClient_new(sensors);
     QemudClient*     client  = qemud_client_new(service, channel, cl,
                                                 _hwSensorClient_recv,
-                                                _hwSensorClient_close);
+                                                _hwSensorClient_close,
+                                                _hwSensorClient_save,
+                                                _hwSensorClient_load );
     qemud_client_set_framing(client, 1);
     cl->client = client;
 
@@ -433,6 +459,99 @@ _hwSensors_setAcceleration( HwSensors*  h, float x, float y, float z )
     s->u.acceleration.z = z;
 }
 
+/* Saves available sensors to allow checking availability when loaded.
+ */
+static void
+_hwSensors_save( QEMUFile*  f, QemudService*  sv, void*  opaque)
+{
+    HwSensors* h = opaque;
+
+    // number of sensors
+    qemu_put_be32(f, MAX_SENSORS);
+    AndroidSensor i;
+    for (i = 0 ; i < MAX_SENSORS; i++) {
+        Sensor* s = &h->sensors[i];
+        qemu_put_be32(f, s->enabled);
+
+        /* this switch ensures that a warning is raised when a new sensor is
+         * added and is not added here as well.
+         */
+        switch (i) {
+        case ANDROID_SENSOR_ACCELERATION:
+            qemu_put_float(f, s->u.acceleration.x);
+            qemu_put_float(f, s->u.acceleration.y);
+            qemu_put_float(f, s->u.acceleration.z);
+            break;
+        case ANDROID_SENSOR_MAGNETIC_FIELD:
+            qemu_put_float(f, s->u.magnetic.x);
+            qemu_put_float(f, s->u.magnetic.y);
+            qemu_put_float(f, s->u.magnetic.z);
+            break;
+        case ANDROID_SENSOR_ORIENTATION:
+            qemu_put_float(f, s->u.orientation.azimuth);
+            qemu_put_float(f, s->u.orientation.pitch);
+            qemu_put_float(f, s->u.orientation.roll);
+            break;
+        case ANDROID_SENSOR_TEMPERATURE:
+            qemu_put_float(f, s->u.temperature.celsius);
+            break;
+        case MAX_SENSORS:
+            break;
+        }
+    }
+}
+
+
+static int
+_hwSensors_load( QEMUFile*  f, QemudService*  s, void*  opaque)
+{
+    HwSensors* h = opaque;
+
+    /* check number of sensors */
+    int32_t num_sensors = qemu_get_be32(f);
+    if (num_sensors != MAX_SENSORS) {
+        D("%s: cannot load: snapshot requires %d sensors, %d available\n",
+          __FUNCTION__, num_sensors, MAX_SENSORS);
+        return -EIO;
+    }
+
+    /* load sensor state */
+    AndroidSensor i;
+    for (i = 0 ; i < MAX_SENSORS; i++) {
+        Sensor* s = &h->sensors[i];
+        s->enabled = qemu_get_be32(f);
+
+        /* this switch ensures that a warning is raised when a new sensor is
+         * added and is not added here as well.
+         */
+        switch (i) {
+        case ANDROID_SENSOR_ACCELERATION:
+            s->u.acceleration.x = qemu_get_float(f);
+            s->u.acceleration.y = qemu_get_float(f);
+            s->u.acceleration.z = qemu_get_float(f);
+            break;
+        case ANDROID_SENSOR_MAGNETIC_FIELD:
+            s->u.magnetic.x = qemu_get_float(f);
+            s->u.magnetic.y = qemu_get_float(f);
+            s->u.magnetic.z = qemu_get_float(f);
+            break;
+        case ANDROID_SENSOR_ORIENTATION:
+            s->u.orientation.azimuth = qemu_get_float(f);
+            s->u.orientation.pitch   = qemu_get_float(f);
+            s->u.orientation.roll    = qemu_get_float(f);
+            break;
+        case ANDROID_SENSOR_TEMPERATURE:
+            s->u.temperature.celsius = qemu_get_float(f);
+            break;
+        case MAX_SENSORS:
+            break;
+        }
+    }
+
+    return 0;
+}
+
+
 #if 0  /* not used yet */
 /* change the value of the emulated magnetic vector */
 static void
@@ -501,8 +620,8 @@ _hwSensors_setCoarseOrientation( HwSensors*  h, AndroidCoarseOrientation  orient
 static void
 _hwSensors_init( HwSensors*  h )
 {
-    h->service = qemud_service_register("sensors", 0, h,
-                                        _hwSensors_connect );
+    h->service = qemud_service_register("sensors", 0, h, _hwSensors_connect,
+                                        _hwSensors_save, _hwSensors_load);
 
     if (android_hw->hw_accelerometer)
         h->sensors[ANDROID_SENSOR_ACCELERATION].enabled = 1;
diff --git a/docs/ANDROID-QEMUD.TXT b/docs/ANDROID-QEMUD.TXT
index 1364553..7841399 100644
--- a/docs/ANDROID-QEMUD.TXT
+++ b/docs/ANDROID-QEMUD.TXT
@@ -249,3 +249,27 @@ but lacked a lot of flexibility:
 
 The current implementation moves any service-specific code to the emulator,
 only uses a single socket and allows concurrent clients for a all services.
+
+
+IV. State snapshots:
+--------------------
+
+Support for snapshots relies on the symmetric qemud_*_save and qemud_*_load 
+functions which save the state of the various Qemud* structs defined in
+android/hw-qemud.c. The high-level process is as follows.
+
+When a snapshot is made, the names and configurations of all services are
+saved. Services can register a custom callback, which is invoked at this point
+to allow saving of service-specific state. Next, clients are saved following
+the same pattern. We save the channel id and the name of service they are
+registered to, then invoke a client-specific callback.
+
+When a snapshot is restored, the first step is to check whether all services
+that were present when the snapshot was made are available. There is currently
+no functionality to start start missing services, so loading fails if a service
+is not present. If all services are present, callbacks are used to restore
+service-specific state.
+
+Next, all active clients are shut down. Information from the snapshot is used
+to start new clients for the services and channels as they were when the
+snapshot was made. This completes the restore process.
diff --git a/hw/hw.h b/hw/hw.h
index a826a72..4f9b650 100644
--- a/hw/hw.h
+++ b/hw/hw.h
@@ -63,6 +63,7 @@ static inline void qemu_put_ubyte(QEMUFile *f, unsigned int v)
 void qemu_put_be16(QEMUFile *f, unsigned int v);
 void qemu_put_be32(QEMUFile *f, unsigned int v);
 void qemu_put_be64(QEMUFile *f, uint64_t v);
+void qemu_put_float(QEMUFile *f, float v);
 int qemu_get_buffer(QEMUFile *f, uint8_t *buf, int size);
 int qemu_get_byte(QEMUFile *f);
 
@@ -76,6 +77,7 @@ static inline unsigned int qemu_get_ubyte(QEMUFile *f)
 unsigned int qemu_get_be16(QEMUFile *f);
 unsigned int qemu_get_be32(QEMUFile *f);
 uint64_t qemu_get_be64(QEMUFile *f);
+float qemu_get_float(QEMUFile *f);
 int qemu_file_rate_limit(QEMUFile *f);
 size_t qemu_file_set_rate_limit(QEMUFile *f, size_t new_rate);
 size_t qemu_file_get_rate_limit(QEMUFile *f);
diff --git a/savevm.c b/savevm.c
index 458e31d..8f0d8f1 100644
--- a/savevm.c
+++ b/savevm.c
@@ -724,6 +724,22 @@ int   qemu_get_struct(QEMUFile*  f, const QField*  fields, void*  s)
     return 0;
 }
 
+/* write a float to file */
+void qemu_put_float(QEMUFile *f, float v)
+{
+    uint8_t *bytes = (uint8_t*) &v;
+    qemu_put_buffer(f, bytes, sizeof(float));
+}
+
+/* read a float from file */
+float qemu_get_float(QEMUFile *f)
+{
+    uint8_t bytes[sizeof(float)];
+    qemu_get_buffer(f, bytes, sizeof(float));
+
+    return *((float*) bytes);
+}
+
 typedef struct SaveStateEntry {
     char idstr[256];
     int instance_id;
diff --git a/vl-android.c b/vl-android.c
index a946a35..fce438e 100644
--- a/vl-android.c
+++ b/vl-android.c
@@ -5419,12 +5419,12 @@ int main(int argc, char **argv, char **envp)
                 gdbstub_dev);
     }
 
-    if (loadvm)
-        do_loadvm(cur_mon, loadvm);
-
     /* call android-specific setup function */
     android_emulation_setup();
 
+    if (loadvm)
+        do_loadvm(cur_mon, loadvm);
+
     if (incoming) {
         autostart = 0; /* fixme how to deal with -daemonize */
         qemu_start_incoming_migration(incoming);