diff options
Diffstat (limited to 'android/sensors-port.c')
| -rw-r--r-- | android/sensors-port.c | 654 |
1 files changed, 379 insertions, 275 deletions
diff --git a/android/sensors-port.c b/android/sensors-port.c index 8620783..6831fe3 100644 --- a/android/sensors-port.c +++ b/android/sensors-port.c @@ -14,370 +14,474 @@ * limitations under the License. */ +#include "android/sdk-controller-socket.h" #include "android/sensors-port.h" #include "android/hw-sensors.h" +#include "android/utils/debug.h" #define E(...) derror(__VA_ARGS__) #define W(...) dwarning(__VA_ARGS__) #define D(...) VERBOSE_PRINT(sensors_port,__VA_ARGS__) #define D_ACTIVE VERBOSE_CHECK(sensors_port) -/* Maximum number of sensors supported. */ -#define ASP_MAX_SENSOR 12 +#define TRACE_ON 1 + +#if TRACE_ON +#define T(...) VERBOSE_PRINT(sensors_port,__VA_ARGS__) +#else +#define T(...) +#endif + +/* Timeout (millisec) to use when communicating with SDK controller. */ +#define SDKCTL_SENSORS_TIMEOUT 3000 -/* Maximum length of a sensor message. */ -#define ASP_MAX_SENSOR_MSG 1024 +/* + * Queries sent to sensors port of the SDK controller. + */ -/* Maximum length of a sensor event. */ -#define ASP_MAX_SENSOR_EVENT 256 +/* Queries the port for list of available sensors. */ +#define SDKCTL_SENSORS_QUERY_LIST 1 -/* Query timeout in milliseconds. */ -#define ASP_QUERY_TIMEOUT 3000 +/* + * Messages sent between the emuator, and sensors port of the SDK controller. + */ + +/* Starts sensor emulation. */ +#define SDKCTL_SENSORS_START 1 +/* Stops sensor emulation. */ +#define SENSOR_SENSORS_STOP 2 +/* Enables emulation for a sensor. */ +#define SDKCTL_SENSORS_ENABLE 3 +/* Disables emulation for a sensor. */ +#define SDKCTL_SENSORS_DISABLE 4 +/* This message delivers sensor values. */ +#define SDKCTL_SENSORS_SENSOR_EVENT 5 + + +/* Describes a sensor on the device. + * When SDK controller sensors port replies to a "list" query, it replies with + * a flat buffer containing entries of this type following each other. End of + * each entry is a zero-terminator for its 'sensor_name' field. The end of the + * entire list is marked with an entry, containing -1 at its 'sensor_id' field. + */ +typedef struct SensorEntry { + /* Identifies sensor on the device. Value -1 indicates list terminator, + * rather than a valid sensor descriptor. */ + int sensor_id; + /* Beginning of zero-terminated sensor name. */ + char sensor_name[1]; +} SensorEntry; + +/* Describes a sensor in the array of emulated sensors. */ +typedef struct SensorDescriptor { + /* Identifies sensor on the device. */ + int sensor_id; + /* Identifies sensor in emulator. */ + int emulator_id; + /* Sensor name. */ + char* sensor_name; +} SensorDescriptor; + +/* Sensor event message descriptor. + * Entries of this type are sent along with SDKCTL_SENSORS_SENSOR_EVENT message + */ +typedef struct SensorEvent { + /* Identifies a device sensor for which values have been delivered. */ + int sensor_id; + /* Sensor values. */ + float fvalues[3]; +} SensorEvent; /* Sensors port descriptor. */ struct AndroidSensorsPort { /* Caller identifier. */ - void* opaque; - /* Connected android device. */ - AndroidDevice* device; - /* String containing list of all available sensors. */ - char sensors[ASP_MAX_SENSOR * 64]; - /* Array of available sensor names. Note that each string in this array - * points inside the 'sensors' buffer. */ - const char* sensor_list[ASP_MAX_SENSOR]; - /* Number of available sensors. */ - int sensors_num; - /* Connection status: 1 connected, 0 - disconnected. */ - int is_connected; - /* Buffer where to receive sensor messages. */ - char sensor_msg[ASP_MAX_SENSOR_MSG]; - /* Buffer where to receive sensor events. */ - char events[ASP_MAX_SENSOR_EVENT]; + void* opaque; + /* Communication socket. */ + SDKCtlSocket* sdkctl; + /* Lists sensors available for emulation. */ + SensorDescriptor** sensors; + /* Number of sensors in 'sensors' list. */ + int sensors_count; }; +/******************************************************************************** + * Sensors port internals + *******************************************************************************/ + +/* Checks if sensor descriptor is the terminator. + * Return: + * Boolean, 1 if it is a terminator, 0 if it is not. + */ +static int +_sensor_entry_is_terminator(const SensorEntry* entry) +{ + return entry == NULL || entry->sensor_id == -1; +} + +/* Gets next sensor descriptor. + * Return: + * Next sensor desciptor, or NULL if there are no more descriptors in the list. + */ +static const SensorEntry* +_sensor_entry_next(const SensorEntry* entry) +{ + if (!_sensor_entry_is_terminator(entry)) { + /* Next descriptor begins right after zero-terminator for the sensor_name + * field of this descriptor. */ + entry = (const SensorEntry*)(entry->sensor_name + strlen(entry->sensor_name) + 1); + if (!_sensor_entry_is_terminator(entry)) { + return entry; + } + } + return NULL; +} + +/* Gets number of entries in the list. */ +static int +_sensor_entry_list_size(const SensorEntry* entry) { + int ret = 0; + while (!_sensor_entry_is_terminator(entry)) { + ret++; + entry = _sensor_entry_next(entry); + } + return ret; +} + +/* Discards sensors saved in AndroidSensorsPort's array. */ +static void +_sensors_port_discard_sensors(AndroidSensorsPort* asp) +{ + if (asp->sensors != NULL) { + int n; + for (n = 0; n < asp->sensors_count; n++) { + if (asp->sensors[n] != NULL) { + free(asp->sensors[n]->sensor_name); + AFREE(asp->sensors[n]); + } + } + free(asp->sensors); + asp->sensors = NULL; + } + asp->sensors_count = 0; +} + + /* Destroys and frees the descriptor. */ static void _sensors_port_free(AndroidSensorsPort* asp) { if (asp != NULL) { - if (asp->device != NULL) { - android_device_destroy(asp->device); + _sensors_port_discard_sensors(asp); + if (asp->sdkctl != NULL) { + sdkctl_socket_release(asp->sdkctl); } AFREE(asp); } } -/******************************************************************************** - * Sensors port callbacks - *******************************************************************************/ +/* Parses flat sensor list, and saves its entries into 'sensors' array filed of + * the AndroidSensorsPort descriptor. */ +static void +_sensors_port_save_sensors(AndroidSensorsPort* asp, const SensorEntry* list) +{ + const int count = _sensor_entry_list_size(list); + if (count != 0) { + int n; + /* Allocate array for sensor descriptors. */ + asp->sensors = malloc(sizeof(SensorDescriptor*) * count); + + /* Iterate through the flat sensor list, filling up array of emulated + * sensors. */ + const SensorEntry* entry = _sensor_entry_is_terminator(list) ? NULL : list; + for (n = 0; n < count && entry != NULL; n++) { + /* Get emulator-side ID for the sensor. < 0 value indicates that + * sensor is not supported by the emulator. */ + const int emulator_id = + android_sensors_get_id_from_name((char*)entry->sensor_name); + if (emulator_id >= 0) { + SensorDescriptor* desc; + ANEW0(desc); + desc->emulator_id = emulator_id; + desc->sensor_id = entry->sensor_id; + desc->sensor_name = ASTRDUP(entry->sensor_name); + + asp->sensors[asp->sensors_count++] = desc; + D("Sensors: Emulated sensor '%s': Device id = %d, Emulator id = %d", + desc->sensor_name, desc->sensor_id, desc->emulator_id); + } else { + D("Sensors: Sensor '%s' is not support by emulator", + entry->sensor_name); + } + entry = _sensor_entry_next(entry); + } + D("Sensors: Emulating %d sensors", asp->sensors_count); + } +} -/* A callback that invoked on sensor events. +/* Finds sensor descriptor for an SDK controller-side ID. */ +static const SensorDescriptor* +_sensor_from_sdkctl_id(AndroidSensorsPort* asp, int id) +{ + int n; + for (n = 0; n < asp->sensors_count; n++) { + if (asp->sensors[n]->sensor_id == id) { + return asp->sensors[n]; + } + } + return NULL; +} + +/* Initiates sensor emulation. * Param: - * opaque - AndroidSensorsPort instance. - * ad - Android device used by this sensors port. - * msg, msgsize - Sensor event message - * failure - Message receiving status. + * asp - Android sensors port instance returned from sensors_port_create. + * Return: + * Zero on success, failure otherwise. */ static void -_on_sensor_received(void* opaque, AndroidDevice* ad, char* msg, int msgsize) +_sensors_port_start(AndroidSensorsPort* asp) { - float fvalues[3] = {0, 0, 0}; - char sensor[ASP_MAX_SENSOR_MSG]; - char* value; - int id; - AndroidSensorsPort* asp = (AndroidSensorsPort*)opaque; - - if (errno) { - D("Sensors notification has failed on sensors port: %s", strerror(errno)); - return; - } + int n; - /* Parse notification, separating sensor name from parameters. */ - memcpy(sensor, msg, msgsize); - value = strchr(sensor, ':'); - if (value == NULL) { - W("Bad format for sensor notification: %s", msg); + if (!sdkctl_socket_is_port_ready(asp->sdkctl)) { + /* SDK controller side is not ready for emulation. Retreat... */ + D("Sensors: SDK controller side is not ready for emulation."); return; } - sensor[value-sensor] = '\0'; - value++; - - id = android_sensors_get_id_from_name(sensor); - if (id >= 0) { - /* Parse 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] )) { - W("Bad parameters in sensor notification %s", msg); - return; - } - } + /* Disable all sensors, and reenable only those that are emulated by + * hardware. */ + sensors_port_disable_sensor(asp, "all"); + + /* Walk throuh the list of enabled sensors enabling them on the device. */ + for (n = 0; n < asp->sensors_count; n++) { + if (android_sensors_get_sensor_status(asp->sensors[n]->emulator_id) == 1) { + /* Reenable emulation for this sensor. */ + sensors_port_enable_sensor(asp, asp->sensors[n]->sensor_name); + D("Sensors: Sensor '%s' is enabled on SDK controller.", + asp->sensors[n]->sensor_name); } - android_sensors_set(id, fvalues[0], fvalues[1], fvalues[2]); - } else { - W("Unknown sensor name '%s' in '%s'", sensor, msg); } - /* Listen to the next event. */ - android_device_listen(ad, asp->events, sizeof(asp->events), _on_sensor_received); + /* Start the emulation. */ + SDKCtlMessage* const msg = + sdkctl_message_send(asp->sdkctl, SDKCTL_SENSORS_START, NULL, 0); + sdkctl_message_release(msg); + + D("Sensors: Emulation has been started."); } -/* A callback that is invoked when android device is connected (i.e. both, command - * and event channels have been stablished. - * Param: - * opaque - AndroidSensorsPort instance. - * ad - Android device used by this sensors port. - * failure - Connections status. - */ -static void -_on_device_connected(void* opaque, AndroidDevice* ad, int failure) +/******************************************************************************** + * Sensors port callbacks + *******************************************************************************/ + +/* Completion for the "list" query. */ +static AsyncIOAction +_on_sensor_list_query(void* query_opaque, + SDKCtlQuery* query, + AsyncIOState status) { - if (!failure) { - AndroidSensorsPort* asp = (AndroidSensorsPort*)opaque; - asp->is_connected = 1; - D("Sensor emulation has started"); - /* Initialize sensors on device. */ - sensors_port_init_sensors(asp); + AndroidSensorsPort* const asp = (AndroidSensorsPort*)(query_opaque); + if (status != ASIO_STATE_SUCCEEDED) { + /* We don't really care about failures at this point. They will + * eventually surface up in another place. */ + return ASIO_ACTION_DONE; } + + /* Parse query response which is a flat list of SensorEntry entries. */ + const SensorEntry* const list = + (const SensorEntry*)sdkctl_query_get_buffer_out(query); + D("Sensors: Sensor list received with %d sensors.", + _sensor_entry_list_size(list)); + _sensors_port_save_sensors(asp, list); + + /* At this point we are ready to statr sensor emulation. */ + _sensors_port_start(asp); + + return ASIO_ACTION_DONE; } -/* Invoked when an I/O failure occurs on a socket. - * Note that this callback will not be invoked on connection failures. +/* A callback that is invoked on sensor events. * Param: - * opaque - AndroidSensorsPort instance. - * ad - Android device instance - * ads - Connection socket where failure has occured. - * failure - Contains 'errno' indicating the reason for failure. + * asp - AndroidSensorsPort instance. + * event - Sensor event. */ static void -_on_io_failure(void* opaque, AndroidDevice* ad, int failure) +_on_sensor_event(AndroidSensorsPort* asp, const SensorEvent* event) { - AndroidSensorsPort* asp = (AndroidSensorsPort*)opaque; - E("Sensors port got disconnected: %s", strerror(failure)); - asp->is_connected = false; - android_device_disconnect(ad); - android_device_connect_async(ad, _on_device_connected); + /* Find corresponding server descriptor. */ + const SensorDescriptor* const desc = + _sensor_from_sdkctl_id(asp, event->sensor_id); + if (desc != NULL) { + T("Sensors: %s -> %f, %f, %f", desc->sensor_name, + event->fvalues[0], event->fvalues[1], + event->fvalues[2]); + /* Fire up sensor change in the guest. */ + android_sensors_set(desc->emulator_id, event->fvalues[0], + event->fvalues[1], event->fvalues[2]); + } else { + W("Sensors: No descriptor for sensor %d", event->sensor_id); + } } -/******************************************************************************** - * Sensors port API - *******************************************************************************/ - -#include "android/sdk-controller-socket.h" - +/* A callback that is invoked on SDK controller socket connection events. */ static AsyncIOAction -_on_sdkctl_connection(void* client_opaque, SDKCtlSocket* sdkctl, AsyncIOState status) +_on_sensors_socket_connection(void* client_opaque, + SDKCtlSocket* sdkctl, + AsyncIOState status) { + AndroidSensorsPort* const asp = (AndroidSensorsPort*)client_opaque; if (status == ASIO_STATE_FAILED) { - sdkctl_socket_reconnect(sdkctl, 1970, 20); + /* Disconnection could mean that user is swapping devices. New device may + * have different set of sensors, so we need to re-query sensor list on + * reconnection. */ + _sensors_port_discard_sensors(asp); + + /* Reconnect (after timeout delay) on failures */ + if (sdkctl_socket_is_handshake_ok(sdkctl)) { + sdkctl_socket_reconnect(sdkctl, SDKCTL_DEFAULT_TCP_PORT, + SDKCTL_SENSORS_TIMEOUT); + } } return ASIO_ACTION_DONE; } -void on_sdkctl_handshake(void* client_opaque, - SDKCtlSocket* sdkctl, - void* handshake, - uint32_t handshake_size, - AsyncIOState status) +/* A callback that is invoked on SDK controller port connection events. */ +static void +_on_sensors_port_connection(void* client_opaque, + SDKCtlSocket* sdkctl, + SdkCtlPortStatus status) { - if (status == ASIO_STATE_SUCCEEDED) { - printf("---------- Handshake %d bytes received.\n", handshake_size); - } else { - printf("!!!!!!!!!! Handshake failed with status %d: %d -> %s\n", - status, errno, strerror(errno)); - sdkctl_socket_reconnect(sdkctl, 1970, 20); + AndroidSensorsPort* const asp = (AndroidSensorsPort*)client_opaque; + switch (status) { + case SDKCTL_PORT_CONNECTED: { + D("Sensors: SDK Controller is connected."); + /* Query list of available sensors. */ + SDKCtlQuery* const query = + sdkctl_query_build_and_send(asp->sdkctl, SDKCTL_SENSORS_QUERY_LIST, + 0, NULL, NULL, NULL, + _on_sensor_list_query, asp, + SDKCTL_SENSORS_TIMEOUT); + sdkctl_query_release(query); + break; + } + + case SDKCTL_PORT_DISCONNECTED: + _sensors_port_discard_sensors(asp); + D("Sensors: SDK Controller is disconnected."); + break; + + case SDKCTL_PORT_ENABLED: + _sensors_port_start(asp); + D("Sensors: SDK Controller is enabled."); + break; + + case SDKCTL_PORT_DISABLED: + D("Sensors: SDK Controller is disabled."); + break; + + case SDKCTL_HANDSHAKE_CONNECTED: + D("Sensors: SDK Controller has succeeded handshake, and port is connected."); + break; + + case SDKCTL_HANDSHAKE_NO_PORT: + D("Sensors: SDK Controller has succeeded handshake, and port is not connected."); + break; + + case SDKCTL_HANDSHAKE_DUP: + E("Sensors: SDK Controller has failed the handshake due to port duplication."); + sdkctl_socket_disconnect(sdkctl); + break; + + case SDKCTL_HANDSHAKE_UNKNOWN_QUERY: + E("Sensors: SDK Controller has failed the handshake due to unknown query."); + sdkctl_socket_disconnect(sdkctl); + break; + + case SDKCTL_HANDSHAKE_UNKNOWN_RESPONSE: + default: + E("Sensors: Handshake has failed due to unknown reasons."); + sdkctl_socket_disconnect(sdkctl); + break; } } -void on_sdkctl_message(void* client_opaque, - SDKCtlSocket* sdkctl, - SDKCtlPacket* message, - int msg_type, - void* msg_data, - int msg_size) +/* A callback that is invoked when a message is received from SDK controller. */ +static void +_on_sensors_message(void* client_opaque, + SDKCtlSocket* sdkctl, + SDKCtlMessage* message, + int msg_type, + void* msg_data, + int msg_size) { - printf("########################################################\n"); + AndroidSensorsPort* const asp = (AndroidSensorsPort*)client_opaque; + switch (msg_type) { + case SDKCTL_SENSORS_SENSOR_EVENT: + _on_sensor_event(asp, (const SensorEvent*)msg_data); + break; + + default: + E("Sensors: Unknown message type %d", msg_type); + break; + } } +/******************************************************************************** + * Sensors port API + *******************************************************************************/ + AndroidSensorsPort* sensors_port_create(void* opaque) { AndroidSensorsPort* asp; - char* wrk; - int res; - - SDKCtlSocket* sdkctl = sdkctl_socket_new(20, "test", _on_sdkctl_connection, - on_sdkctl_handshake, on_sdkctl_message, - NULL); -// sdkctl_init_recycler(sdkctl, 64, 8); - sdkctl_socket_connect(sdkctl, 1970, 20); ANEW0(asp); asp->opaque = opaque; - asp->is_connected = 0; - - asp->device = android_device_init(asp, AD_SENSOR_PORT, _on_io_failure); - if (asp->device == NULL) { - _sensors_port_free(asp); - return NULL; - } - - res = android_device_connect_sync(asp->device, ASP_QUERY_TIMEOUT); - if (res != 0) { - sensors_port_destroy(asp); - return NULL; - } - - res = android_device_query(asp->device, "list", - asp->sensors, sizeof(asp->sensors), - ASP_QUERY_TIMEOUT); - if (res != 0) { - sensors_port_destroy(asp); - return NULL; - } - - /* Parse sensor list. */ - asp->sensors_num = 0; - wrk = asp->sensors; - - while (wrk != NULL && *wrk != '\0' && *wrk != '\n') { - asp->sensor_list[asp->sensors_num] = wrk; - asp->sensors_num++; - wrk = strchr(wrk, '\n'); - if (wrk != NULL) { - *wrk = '\0'; wrk++; - } - } - - android_device_listen(asp->device, asp->events, sizeof(asp->events), - _on_sensor_received); + asp->sensors = NULL; + asp->sensors_count = 0; + asp->sdkctl = sdkctl_socket_new(SDKCTL_SENSORS_TIMEOUT, "sensors", + _on_sensors_socket_connection, + _on_sensors_port_connection, + _on_sensors_message, asp); + sdkctl_init_recycler(asp->sdkctl, 76, 8); + sdkctl_socket_connect(asp->sdkctl, SDKCTL_DEFAULT_TCP_PORT, + SDKCTL_SENSORS_TIMEOUT); return asp; } -int -sensors_port_init_sensors(AndroidSensorsPort* asp) -{ - int res, id; - - /* Disable all sensors for now. Reenable only those that are emulated. */ - res = sensors_port_disable_sensor(asp, "all"); - if (res) { - return res; - } - - /* Start listening on sensor events. */ - res = android_device_listen(asp->device, asp->events, sizeof(asp->events), - _on_sensor_received); - if (res) { - return res; - } - - /* Walk throuh the list of enabled sensors enabling them on the device. */ - for (id = 0; id < MAX_SENSORS; id++) { - if (android_sensors_get_sensor_status(id) == 1) { - res = sensors_port_enable_sensor(asp, android_sensors_get_name_from_id(id)); - if (res == 0) { - D("Sensor '%s' is enabled on the device.", - android_sensors_get_name_from_id(id)); - } - } - } - - /* Start sensor events. */ - return sensors_port_start(asp); -} - void sensors_port_destroy(AndroidSensorsPort* asp) { + if (asp->sdkctl != NULL) { + sdkctl_socket_disconnect(asp->sdkctl); + } _sensors_port_free(asp); } int -sensors_port_is_connected(AndroidSensorsPort* asp) -{ - return asp->is_connected; -} - -int sensors_port_enable_sensor(AndroidSensorsPort* asp, const char* name) { - char query[1024]; - char qresp[1024]; - snprintf(query, sizeof(query), "enable:%s", name); - const int res = - android_device_query(asp->device, query, qresp, sizeof(qresp), - ASP_QUERY_TIMEOUT); - if (res) { - if (errno) { - D("Query '%s' failed on I/O: %s", query, strerror(errno)); - } else { - D("Query '%s' failed on device: %s", query, qresp); - } + if (asp->sdkctl != NULL && sdkctl_socket_is_port_ready(asp->sdkctl)) { + SDKCtlMessage* const msg = sdkctl_message_send(asp->sdkctl, + SDKCTL_SENSORS_ENABLE, + name, strlen(name)); + sdkctl_message_release(msg); + return 0; + } else { + return -1; } - return res; } int sensors_port_disable_sensor(AndroidSensorsPort* asp, const char* name) { - char query[1024]; - char qresp[1024]; - snprintf(query, sizeof(query), "disable:%s", name); - const int res = - android_device_query(asp->device, query, qresp, sizeof(qresp), - ASP_QUERY_TIMEOUT); - if (res) { - if (errno) { - D("Query '%s' failed on I/O: %s", query, strerror(errno)); - } else { - D("Query '%s' failed on device: %s", query, qresp); - } - } - return res; -} - -int -sensors_port_start(AndroidSensorsPort* asp) -{ - char qresp[ASP_MAX_SENSOR_MSG]; - const int res = - android_device_query(asp->device, "start", qresp, sizeof(qresp), - ASP_QUERY_TIMEOUT); - if (res) { - if (errno) { - D("Query 'start' failed on I/O: %s", strerror(errno)); - } else { - D("Query 'start' failed on device: %s", qresp); - } - } - return res; -} - -int -sensors_port_stop(AndroidSensorsPort* asp) -{ - char qresp[ASP_MAX_SENSOR_MSG]; - const int res = - android_device_query(asp->device, "stop", qresp, sizeof(qresp), - ASP_QUERY_TIMEOUT); - if (res) { - if (errno) { - D("Query 'stop' failed on I/O: %s", strerror(errno)); - } else { - D("Query 'stop' failed on device: %s", qresp); - } + if (asp->sdkctl != NULL && sdkctl_socket_is_port_ready(asp->sdkctl)) { + SDKCtlMessage* const msg = sdkctl_message_send(asp->sdkctl, + SDKCTL_SENSORS_DISABLE, + name, strlen(name)); + sdkctl_message_release(msg); + return 0; + } else { + return -1; } - - return res; } |