android/sensors-port.c - platform/external/qemu - Git at Google

 /*
  * Copyright (C) 2011 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 #include "android/sensors-port.h"
 #include "android/hw-sensors.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

 /* Maximum length of a sensor message. */
 #define ASP_MAX_SENSOR_MSG      1024

 /* Maximum length of a sensor event. */
 #define ASP_MAX_SENSOR_EVENT    256

 /* Query timeout in milliseconds. */
 #define ASP_QUERY_TIMEOUT       3000

 /* 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];
 };

 /* Destroys and frees the descriptor. */
 static void
 _sensors_port_free(AndroidSensorsPort* asp)
 {
     if (asp != NULL) {
         if (asp->device != NULL) {
             android_device_destroy(asp->device);
         }
         AFREE(asp);
     }
 }

 /********************************************************************************
  *                          Sensors port callbacks
  *******************************************************************************/

 /* A callback that invoked on sensor events.
  * Param:
  *  opaque - AndroidSensorsPort instance.
  *  ad - Android device used by this sensors port.
  *  msg, msgsize - Sensor event message
  *  failure - Message receiving status.
  */
 static void
 _on_sensor_received(void* opaque, AndroidDevice* ad, char* msg, int msgsize)
 {
     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;
     }

     /* 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);
         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;
                 }
             }
         }
         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);
 }

 /* 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)
 {
     if (!failure) {
         AndroidSensorsPort* asp = (AndroidSensorsPort*)opaque;
         asp->is_connected = 1;
         D("Sensor emulation has started");
         /* Initialize sensors on device. */
         sensors_port_init_sensors(asp);
     }
 }

 /* Invoked when an I/O failure occurs on a socket.
  * Note that this callback will not be invoked on connection failures.
  * Param:
  *  opaque - AndroidSensorsPort instance.
  *  ad - Android device instance
  *  ads - Connection socket where failure has occured.
  *  failure - Contains 'errno' indicating the reason for failure.
  */
 static void
 _on_io_failure(void* opaque, AndroidDevice* ad, int failure)
 {
     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);
 }

 /********************************************************************************
  *                          Sensors port API
  *******************************************************************************/

 AndroidSensorsPort*
 sensors_port_create(void* opaque)
 {
     AndroidSensorsPort* asp;
     char* wrk;
     int res;

     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);
     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)
 {
     _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);
         }
     }
     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);
         }
     }

     return res;
 }
	/*
	* Copyright (C) 2011 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	#include "android/sensors-port.h"
	#include "android/hw-sensors.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

	/* Maximum length of a sensor message. */
	#define ASP_MAX_SENSOR_MSG 1024

	/* Maximum length of a sensor event. */
	#define ASP_MAX_SENSOR_EVENT 256

	/* Query timeout in milliseconds. */
	#define ASP_QUERY_TIMEOUT 3000

	/* 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];
	};

	/* Destroys and frees the descriptor. */
	static void
	_sensors_port_free(AndroidSensorsPort* asp)
	{
	if (asp != NULL) {
	if (asp->device != NULL) {
	android_device_destroy(asp->device);
	}
	AFREE(asp);
	}
	}

	/********************************************************************************
	* Sensors port callbacks
	*******************************************************************************/

	/* A callback that invoked on sensor events.
	* Param:
	* opaque - AndroidSensorsPort instance.
	* ad - Android device used by this sensors port.
	* msg, msgsize - Sensor event message
	* failure - Message receiving status.
	*/
	static void
	_on_sensor_received(void* opaque, AndroidDevice* ad, char* msg, int msgsize)
	{
	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;
	}

	/* 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);
	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;
	}
	}
	}
	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);
	}

	/* 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)
	{
	if (!failure) {
	AndroidSensorsPort* asp = (AndroidSensorsPort*)opaque;
	asp->is_connected = 1;
	D("Sensor emulation has started");
	/* Initialize sensors on device. */
	sensors_port_init_sensors(asp);
	}
	}

	/* Invoked when an I/O failure occurs on a socket.
	* Note that this callback will not be invoked on connection failures.
	* Param:
	* opaque - AndroidSensorsPort instance.
	* ad - Android device instance
	* ads - Connection socket where failure has occured.
	* failure - Contains 'errno' indicating the reason for failure.
	*/
	static void
	_on_io_failure(void* opaque, AndroidDevice* ad, int failure)
	{
	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);
	}

	/********************************************************************************
	* Sensors port API
	*******************************************************************************/

	AndroidSensorsPort*
	sensors_port_create(void* opaque)
	{
	AndroidSensorsPort* asp;
	char* wrk;
	int res;

	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);
	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)
	{
	_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);
	}
	}
	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);
	}
	}

	return res;
	}