sensors/sensorhub.c - device/moto/shamu - Git at Google

 /*
  * Copyright (C) 2011-2012 Motorola Mobility, Inc.
  * Copyright (C) 2008 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.
  */

 #define LOG_TAG "sensorhub"

 #include <cutils/log.h>

 #include <dirent.h>
 #include <endian.h>
 #include <errno.h>
 #include <fcntl.h>
 #include <pthread.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>

 #include <linux/android_alarm.h>
 #include <linux/input.h>
 #include <linux/stm401.h>

 #include <sys/ioctl.h>
 #include <sys/poll.h>
 #include <sys/time.h>
 #include <sys/types.h>

 #include <hardware/mot_sensorhub_stm401.h>

 /* paths to the driver fds */
 #define DRIVER_CONTROL_PATH "/dev/stm401"
 #define DRIVER_DATA_NAME "/dev/stm401_ms"

 /* Constants and helper macro for accessing sensor hub event data. */
 #define MOVE_VALUE 0

 #define ALGO_PAST       0
 #define ALGO_CONFIDENCE 0
 #define ALGO_ID         0
 #define ALGO_TIME       1
 #define ALGO_OLDSTATE   1
 #define ALGO_NEWSTATE   3
 #define ALGO_DISTANCE   3
 #define ALGO_MS         5
 #define ALGO_ALGO       7

 #define GENERIC_INT_OFFSET 0

 #define STMLE16TOH(p) (int16_t) le16toh(*((uint16_t *) (p)))

 static pthread_mutex_t g_lock = PTHREAD_MUTEX_INITIALIZER;

 static uint8_t num_parts[SENSORHUB_NUM_ALGOS] = {
     SENSORHUB_NUM_MODALITIES, SENSORHUB_NUM_ORIENTATIONS, SENSORHUB_NUM_STOWED,
     SENSORHUB_NUM_ACCUM_REQS, 0, 0 };

 static uint16_t algo_bits[SENSORHUB_NUM_ALGOS] = {
     M_ALGO_MODALITY, M_ALGO_ORIENTATION, M_ALGO_STOWED,
     M_ALGO_ACCUM_MODALITY, M_ALGO_ACCUM_MVMT, 0 };

 struct sensorhub_context_t {
     struct sensorhub_device_t device;
     int control_fd;
     struct pollfd data_pollfd;
     uint16_t active_algos;
     uint32_t active_parts[SENSORHUB_NUM_ALGOS];
 };

 static int64_t get_wall_clock()
 {
     struct timeval time;
     gettimeofday(&time, NULL);
     return (int64_t)((time.tv_sec*(int64_t)1000) + (time.tv_usec/(int64_t)1000));
 }

 static int64_t get_elapsed_realtime()
 {
     struct timespec ts;
     int fd, result;

     fd = open("/dev/alarm", O_RDONLY);
     if (fd < 0)
         return fd;

    result = ioctl(fd, ANDROID_ALARM_GET_TIME(ANDROID_ALARM_ELAPSED_REALTIME), &ts);
    close(fd);

     if (result == 0)
         return (int64_t)((ts.tv_sec*(int64_t)1000) + (ts.tv_nsec/(int64_t)1000000));
     return -1;
 }

 static int sensorhub_enable(struct sensorhub_device_t* device, struct sensorhub_algo_t* algo)
 {
     struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
     int error = 0;
     unsigned int data;

     pthread_mutex_lock(&g_lock);

     switch (algo->type) {
     case SENSORHUB_ALGO_MOVEMENT:
         if (algo->enable) {
             data = algo->parameter[0];
             if (ioctl(context->control_fd, STM401_IOCTL_SET_MOTION_DUR, &data) < 0) {
                 ALOGE("STM401_IOCTL_SET_MOTION_DUR error (%s)", strerror(errno));
                 error = -errno;
             }
             data = algo->parameter[1];
             if (ioctl(context->control_fd, STM401_IOCTL_SET_ZRMOTION_DUR, &data) < 0) {
                 ALOGE("STM401_IOCTL_SET_ZRMOTION_DUR error (%s)", strerror(errno));
                 error = -errno;
             }
             data = context->active_algos | (M_MMOVEME | M_NOMMOVE);
         } else {
             data = context->active_algos & ~(M_MMOVEME | M_NOMMOVE);
         }
         break;
     }

     if (!error) {
         if (ioctl(context->control_fd, STM401_IOCTL_SET_ALGOS, &data) < 0) {
             ALOGE("STM401_IOCTL_SET_ALGOS error (%s)", strerror(errno));
             error = -errno;
         }
     }
     if (!error) {
         context->active_algos = data;
     }

     pthread_mutex_unlock(&g_lock);
     return error;
 }

 static int sensorhub_algo_req(struct sensorhub_device_t* device, uint16_t algo,
         uint32_t active_parts, struct sensorhub_req_t* req_info)
 {
     struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
     int i, error = 0;
     uint8_t req_len;
     uint16_t algos;

     if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
         req_len = sizeof(sensorhub_accum_mvmt_req_t);
     } else
     if (algo == SENSORHUB_ALGO_ACCUM_MODALITY) {
         req_len = (sizeof(sensorhub_accum_state_req_t) * num_parts[algo]);
     } else {
         req_len = sizeof(active_parts) + (sizeof(req_info[0].durations) * num_parts[algo]);
     }

     unsigned char bytes[sizeof(algo) + sizeof(req_len) + req_len];

     pthread_mutex_lock(&g_lock);
     ALOGD("sensorhub_algo_req(): algo: %d, active_parts: %d, num_parts: %d, req_len: %d, bytes: %d",
         algo, active_parts, num_parts[algo], req_len, sizeof(bytes));

     algos = context->active_algos;
     if (!context->active_parts[algo] && active_parts) {
         algos |= algo_bits[algo];
     } else
     if (context->active_parts[algo] && !active_parts) {
         algos &= ~algo_bits[algo];
     }

     ALOGD("sensorhub_algo_req(): algos: %d", algos);
     // ioctl set algo req = active_parts + req_info for each part
     unsigned char* p = bytes;
     memcpy(p, &algo, sizeof(algo));
     p += sizeof(algo);
     memcpy(p, &req_len, sizeof(req_len));
     p += sizeof(req_len);

     if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
         memcpy(p, &req_info->am_req, sizeof(req_info->am_req));
     } else
     if (algo == SENSORHUB_ALGO_ACCUM_MODALITY) {
         ALOGD("sensorhub_algo_accum_modality(): about to copy req_info size: %d", req_len);
         for (i = 0; i < num_parts[algo]; i++) {
             memcpy(p, &req_info[i].as_req, sizeof(req_info[i].as_req));
             p += sizeof(req_info[i].as_req);
         }
     } else {
         memcpy(p, &active_parts, sizeof(active_parts));
         p += sizeof(active_parts);
         for (i = 0; i < num_parts[algo]; i++) {
             memcpy(p, req_info[i].durations, sizeof(req_info[i].durations));
             p += sizeof(req_info[i].durations);
         }
     }

     if (ioctl(context->control_fd, STM401_IOCTL_SET_ALGO_REQ, bytes) < 0) {
         ALOGE("STM401_IOCTL_SET_ALGO_REQ error (%s)", strerror(errno));
         error = -errno;
     } else {
         context->active_parts[algo] = active_parts;

         // ioctl set algos
         if (ioctl(context->control_fd, STM401_IOCTL_SET_ALGOS, &algos) < 0) {
             ALOGE("STM401_IOCTL_SET_ALGOS error (%s)", strerror(errno));
             error = -errno;
         } else {
             context->active_algos = algos;
         }
     }
     pthread_mutex_unlock(&g_lock);
     return error;
 }

 static int sensorhub_algo_query(struct sensorhub_device_t* device, uint16_t algo,
     struct sensorhub_event_t* output)
 {
     struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
     int i, evt_reg_size, error = 0;

     if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
         evt_reg_size = STM401_EVT_SZ_ACCUM_MVMT;
     } else {
         evt_reg_size = STM401_EVT_SZ_TRANSITION;
     }

     unsigned char bytes[sizeof(algo) + evt_reg_size];

     pthread_mutex_lock(&g_lock);
     ALOGD("sensorhub_algo_query(): algo: %d", algo);

     memcpy(bytes, &algo, sizeof(algo));

     if (ioctl(context->control_fd, STM401_IOCTL_GET_ALGO_EVT, bytes) < 0) {
         ALOGE("STM401_IOCTL_GET_ALGO_EVT error (%s)", strerror(errno));
         error = -errno;
     } else {
         // just clear time for query output
         output->time = 0;
         output->ertime = 0;
         unsigned char* p_evt = bytes + sizeof(algo);

         if (algo == SENSORHUB_ALGO_ACCUM_MVMT)
         {
             output->type = SENSORHUB_EVENT_ACCUM_MVMT;
             output->time_s = (p_evt[1] << 8) | p_evt[0];
             output->distance = (p_evt[3] << 8) | p_evt[2];
         } else {
             output->type = SENSORHUB_EVENT_TRANSITION;
             output->algo = algo;
             output->past = (p_evt[0] & 0x80) > 0;
             output->confidence = p_evt[0] & 0x7F;
             output->old_state = (p_evt[2] << 8) | p_evt[1];
             output->new_state = (p_evt[4] << 8) | p_evt[3];
         }
     }
     pthread_mutex_unlock(&g_lock);
     return error;
 }

 static int sensorhub_poll(struct sensorhub_device_t* device, struct sensorhub_event_t* event)
 {
     struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
     struct stm401_moto_sensor_data buff;
     int64_t elapsed_ms;
     uint16_t algo;
     int ret;

     ALOGD("sensorhub_poll() polling...");
     ret = poll(&(context->data_pollfd), 1, -1);
     if (ret < 0) {
         ALOGE("poll() failed (%s)", strerror(errno));
         return -errno;
     }

     ret = read(context->data_pollfd.fd, &buff, sizeof(struct stm401_moto_sensor_data));
     if (ret == 0)
         return 0;

     switch (buff.type) {
         case DT_MMMOVE:
             event->type = SENSORHUB_EVENT_START_MOVEMENT;
             event->value[2] = buff.data[MOVE_VALUE] >> 4;
             break;
         case DT_NOMOVE:
             event->type = SENSORHUB_EVENT_END_MOVEMENT;
             event->value[2] = buff.data[MOVE_VALUE] >> 4;
             break;
         case DT_ALGO_EVT:
             // These events are sent little endian and are different from
             // all other sensorhub data which are sent big endian.
             algo = buff.data[ALGO_ALGO];
             event->time = get_wall_clock();
             elapsed_ms = get_elapsed_realtime();
             event->ertime = elapsed_ms > 0 ? elapsed_ms : 0;
             if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
                 event->type = SENSORHUB_EVENT_ACCUM_MVMT;
                 event->time_s = STMLE16TOH(buff.data + ALGO_TIME);
                 event->distance = STMLE16TOH(buff.data + ALGO_DISTANCE);
                 //ALOGD("sensorhub_poll(): accum mvmt: time_s: %d, distance: %d",
                 //    event->time_s, event->distance);
             } else
             if (algo == SENSORHUB_ALGO_ACCUM_MODALITY) {
                 event->type = SENSORHUB_EVENT_ACCUM_STATE;
                 event->accum_algo = algo;
                 event->id = STMLE16TOH(buff.data + ALGO_ID);
             } else {
                 event->type = SENSORHUB_EVENT_TRANSITION;
                 elapsed_ms = STMLE16TOH(buff.data + ALGO_MS) * 1000;
                 event->time -= elapsed_ms;
                 if (event->ertime > 0)
                     event->ertime -= elapsed_ms;
                 event->algo = algo;
                 event->past = (buff.data[ALGO_PAST] & 0x80) > 0;
                 event->confidence = buff.data[ALGO_CONFIDENCE] & 0x7F;
                 event->old_state = STMLE16TOH(buff.data + ALGO_OLDSTATE);
                 event->new_state = STMLE16TOH(buff.data + ALGO_NEWSTATE);
                 //ALOGD("sensorhub_poll(): tran: algo: %d, elapsed: %lld, t: %lld, ert: %lld",
                 //    event->algo, elapsed_ms, event->time, event->ertime);
             }
             break;
         case DT_GENERIC_INT:
 #if 0
             // packaging irq3_status into ertime field
             // of the event
             event->type = SENSORHUB_EVENT_GENERIC_CB;
             event->time = get_wall_clock();
             event->ertime = buff.data[GENERIC_INT_OFFSET];
 #endif
             context->data_pollfd.revents = 0;
             return 0;
             break;
         case DT_RESET:
             event->type = SENSORHUB_EVENT_RESET;
             event->time = get_wall_clock();
             break;
     }
     context->data_pollfd.revents = 0;
     return 1;
 }

 static int sensorhub_close(struct hw_device_t* device)
 {
     struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
     close(context->control_fd);
     close(context->data_pollfd.fd);
     free(context);
     pthread_mutex_destroy(&g_lock);
     return 0;
 }

 static int sensorhub_open(const struct hw_module_t* module, char const* name, struct hw_device_t** device)
 {
     struct sensorhub_context_t* context = calloc(1, sizeof(struct sensorhub_context_t));
     int fd;

     if (!context) {
         ALOGE("%s: Couldn't allocate context.", __func__);
         return -ENOMEM;
     }

     pthread_mutex_init(&g_lock, NULL);

     context->device.common.tag = HARDWARE_DEVICE_TAG;
     context->device.common.version = 0;
     context->device.common.module = (struct hw_module_t*)module;
     context->device.common.close = sensorhub_close;
     context->device.enable = sensorhub_enable;
     context->device.algo_req = sensorhub_algo_req;
     context->device.algo_query = sensorhub_algo_query;
     context->device.poll = sensorhub_poll;

     fd = open(DRIVER_CONTROL_PATH, O_RDWR);
     if (fd < 0) {
         ALOGE("%s: Couldn't open control '%s' (%s)",
                 __func__, DRIVER_CONTROL_PATH, strerror(errno));
         free(context);
         return -errno;
     }
     context->control_fd = fd;

     fd = open(DRIVER_DATA_NAME, O_RDONLY);
     if (fd < 0) {
         ALOGE("%s: Couldn't open data '%s' (%s)",
                 __func__, DRIVER_DATA_NAME, strerror(errno));
         close(context->control_fd);
         free(context);
         return -errno;
     }
     context->data_pollfd.fd = fd;
     context->data_pollfd.events = POLLIN;

     context->active_algos = 0;

     *device = (struct hw_device_t*)context;
     return 0;
 }

 static struct hw_module_methods_t sensorhub_module_methods = {
     .open = sensorhub_open,
 };

 struct hw_module_t HAL_MODULE_INFO_SYM = {
     .tag = HARDWARE_MODULE_TAG,
     .version_major = 3,
     .version_minor = 0,
     .id = SENSORHUB_HARDWARE_MODULE_ID,
     .name = "Motorola Mobility Smart Fusion module",
     .author = "Motorola Mobility, Inc.",
     .methods = &sensorhub_module_methods,
 };
	/*
	* Copyright (C) 2011-2012 Motorola Mobility, Inc.
	* Copyright (C) 2008 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.
	*/

	#define LOG_TAG "sensorhub"

	#include <cutils/log.h>

	#include <dirent.h>
	#include <endian.h>
	#include <errno.h>
	#include <fcntl.h>
	#include <pthread.h>
	#include <stdlib.h>
	#include <string.h>
	#include <unistd.h>

	#include <linux/android_alarm.h>
	#include <linux/input.h>
	#include <linux/stm401.h>

	#include <sys/ioctl.h>
	#include <sys/poll.h>
	#include <sys/time.h>
	#include <sys/types.h>

	#include <hardware/mot_sensorhub_stm401.h>

	/* paths to the driver fds */
	#define DRIVER_CONTROL_PATH "/dev/stm401"
	#define DRIVER_DATA_NAME "/dev/stm401_ms"

	/* Constants and helper macro for accessing sensor hub event data. */
	#define MOVE_VALUE 0

	#define ALGO_PAST 0
	#define ALGO_CONFIDENCE 0
	#define ALGO_ID 0
	#define ALGO_TIME 1
	#define ALGO_OLDSTATE 1
	#define ALGO_NEWSTATE 3
	#define ALGO_DISTANCE 3
	#define ALGO_MS 5
	#define ALGO_ALGO 7

	#define GENERIC_INT_OFFSET 0

	#define STMLE16TOH(p) (int16_t) le16toh(((uint16_t ) (p)))

	static pthread_mutex_t g_lock = PTHREAD_MUTEX_INITIALIZER;

	static uint8_t num_parts[SENSORHUB_NUM_ALGOS] = {
	SENSORHUB_NUM_MODALITIES, SENSORHUB_NUM_ORIENTATIONS, SENSORHUB_NUM_STOWED,
	SENSORHUB_NUM_ACCUM_REQS, 0, 0 };

	static uint16_t algo_bits[SENSORHUB_NUM_ALGOS] = {
	M_ALGO_MODALITY, M_ALGO_ORIENTATION, M_ALGO_STOWED,
	M_ALGO_ACCUM_MODALITY, M_ALGO_ACCUM_MVMT, 0 };

	struct sensorhub_context_t {
	struct sensorhub_device_t device;
	int control_fd;
	struct pollfd data_pollfd;
	uint16_t active_algos;
	uint32_t active_parts[SENSORHUB_NUM_ALGOS];
	};

	static int64_t get_wall_clock()
	{
	struct timeval time;
	gettimeofday(&time, NULL);
	return (int64_t)((time.tv_sec*(int64_t)1000) + (time.tv_usec/(int64_t)1000));
	}

	static int64_t get_elapsed_realtime()
	{
	struct timespec ts;
	int fd, result;

	fd = open("/dev/alarm", O_RDONLY);
	if (fd < 0)
	return fd;

	result = ioctl(fd, ANDROID_ALARM_GET_TIME(ANDROID_ALARM_ELAPSED_REALTIME), &ts);
	close(fd);

	if (result == 0)
	return (int64_t)((ts.tv_sec*(int64_t)1000) + (ts.tv_nsec/(int64_t)1000000));
	return -1;
	}

	static int sensorhub_enable(struct sensorhub_device_t* device, struct sensorhub_algo_t* algo)
	{
	struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
	int error = 0;
	unsigned int data;

	pthread_mutex_lock(&g_lock);

	switch (algo->type) {
	case SENSORHUB_ALGO_MOVEMENT:
	if (algo->enable) {
	data = algo->parameter[0];
	if (ioctl(context->control_fd, STM401_IOCTL_SET_MOTION_DUR, &data) < 0) {
	ALOGE("STM401_IOCTL_SET_MOTION_DUR error (%s)", strerror(errno));
	error = -errno;
	}
	data = algo->parameter[1];
	if (ioctl(context->control_fd, STM401_IOCTL_SET_ZRMOTION_DUR, &data) < 0) {
	ALOGE("STM401_IOCTL_SET_ZRMOTION_DUR error (%s)", strerror(errno));
	error = -errno;
	}
	data = context->active_algos \| (M_MMOVEME \| M_NOMMOVE);
	} else {
	data = context->active_algos & ~(M_MMOVEME \| M_NOMMOVE);
	}
	break;
	}

	if (!error) {
	if (ioctl(context->control_fd, STM401_IOCTL_SET_ALGOS, &data) < 0) {
	ALOGE("STM401_IOCTL_SET_ALGOS error (%s)", strerror(errno));
	error = -errno;
	}
	}
	if (!error) {
	context->active_algos = data;
	}

	pthread_mutex_unlock(&g_lock);
	return error;
	}

	static int sensorhub_algo_req(struct sensorhub_device_t* device, uint16_t algo,
	uint32_t active_parts, struct sensorhub_req_t* req_info)
	{
	struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
	int i, error = 0;
	uint8_t req_len;
	uint16_t algos;

	if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
	req_len = sizeof(sensorhub_accum_mvmt_req_t);
	} else
	if (algo == SENSORHUB_ALGO_ACCUM_MODALITY) {
	req_len = (sizeof(sensorhub_accum_state_req_t) * num_parts[algo]);
	} else {
	req_len = sizeof(active_parts) + (sizeof(req_info[0].durations) * num_parts[algo]);
	}

	unsigned char bytes[sizeof(algo) + sizeof(req_len) + req_len];

	pthread_mutex_lock(&g_lock);
	ALOGD("sensorhub_algo_req(): algo: %d, active_parts: %d, num_parts: %d, req_len: %d, bytes: %d",
	algo, active_parts, num_parts[algo], req_len, sizeof(bytes));

	algos = context->active_algos;
	if (!context->active_parts[algo] && active_parts) {
	algos \|= algo_bits[algo];
	} else
	if (context->active_parts[algo] && !active_parts) {
	algos &= ~algo_bits[algo];
	}

	ALOGD("sensorhub_algo_req(): algos: %d", algos);
	// ioctl set algo req = active_parts + req_info for each part
	unsigned char* p = bytes;
	memcpy(p, &algo, sizeof(algo));
	p += sizeof(algo);
	memcpy(p, &req_len, sizeof(req_len));
	p += sizeof(req_len);

	if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
	memcpy(p, &req_info->am_req, sizeof(req_info->am_req));
	} else
	if (algo == SENSORHUB_ALGO_ACCUM_MODALITY) {
	ALOGD("sensorhub_algo_accum_modality(): about to copy req_info size: %d", req_len);
	for (i = 0; i < num_parts[algo]; i++) {
	memcpy(p, &req_info[i].as_req, sizeof(req_info[i].as_req));
	p += sizeof(req_info[i].as_req);
	}
	} else {
	memcpy(p, &active_parts, sizeof(active_parts));
	p += sizeof(active_parts);
	for (i = 0; i < num_parts[algo]; i++) {
	memcpy(p, req_info[i].durations, sizeof(req_info[i].durations));
	p += sizeof(req_info[i].durations);
	}
	}

	if (ioctl(context->control_fd, STM401_IOCTL_SET_ALGO_REQ, bytes) < 0) {
	ALOGE("STM401_IOCTL_SET_ALGO_REQ error (%s)", strerror(errno));
	error = -errno;
	} else {
	context->active_parts[algo] = active_parts;

	// ioctl set algos
	if (ioctl(context->control_fd, STM401_IOCTL_SET_ALGOS, &algos) < 0) {
	ALOGE("STM401_IOCTL_SET_ALGOS error (%s)", strerror(errno));
	error = -errno;
	} else {
	context->active_algos = algos;
	}
	}
	pthread_mutex_unlock(&g_lock);
	return error;
	}

	static int sensorhub_algo_query(struct sensorhub_device_t* device, uint16_t algo,
	struct sensorhub_event_t* output)
	{
	struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
	int i, evt_reg_size, error = 0;

	if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
	evt_reg_size = STM401_EVT_SZ_ACCUM_MVMT;
	} else {
	evt_reg_size = STM401_EVT_SZ_TRANSITION;
	}

	unsigned char bytes[sizeof(algo) + evt_reg_size];

	pthread_mutex_lock(&g_lock);
	ALOGD("sensorhub_algo_query(): algo: %d", algo);

	memcpy(bytes, &algo, sizeof(algo));

	if (ioctl(context->control_fd, STM401_IOCTL_GET_ALGO_EVT, bytes) < 0) {
	ALOGE("STM401_IOCTL_GET_ALGO_EVT error (%s)", strerror(errno));
	error = -errno;
	} else {
	// just clear time for query output
	output->time = 0;
	output->ertime = 0;
	unsigned char* p_evt = bytes + sizeof(algo);

	if (algo == SENSORHUB_ALGO_ACCUM_MVMT)
	{
	output->type = SENSORHUB_EVENT_ACCUM_MVMT;
	output->time_s = (p_evt[1] << 8) \| p_evt[0];
	output->distance = (p_evt[3] << 8) \| p_evt[2];
	} else {
	output->type = SENSORHUB_EVENT_TRANSITION;
	output->algo = algo;
	output->past = (p_evt[0] & 0x80) > 0;
	output->confidence = p_evt[0] & 0x7F;
	output->old_state = (p_evt[2] << 8) \| p_evt[1];
	output->new_state = (p_evt[4] << 8) \| p_evt[3];
	}
	}
	pthread_mutex_unlock(&g_lock);
	return error;
	}

	static int sensorhub_poll(struct sensorhub_device_t* device, struct sensorhub_event_t* event)
	{
	struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
	struct stm401_moto_sensor_data buff;
	int64_t elapsed_ms;
	uint16_t algo;
	int ret;

	ALOGD("sensorhub_poll() polling...");
	ret = poll(&(context->data_pollfd), 1, -1);
	if (ret < 0) {
	ALOGE("poll() failed (%s)", strerror(errno));
	return -errno;
	}

	ret = read(context->data_pollfd.fd, &buff, sizeof(struct stm401_moto_sensor_data));
	if (ret == 0)
	return 0;

	switch (buff.type) {
	case DT_MMMOVE:
	event->type = SENSORHUB_EVENT_START_MOVEMENT;
	event->value[2] = buff.data[MOVE_VALUE] >> 4;
	break;
	case DT_NOMOVE:
	event->type = SENSORHUB_EVENT_END_MOVEMENT;
	event->value[2] = buff.data[MOVE_VALUE] >> 4;
	break;
	case DT_ALGO_EVT:
	// These events are sent little endian and are different from
	// all other sensorhub data which are sent big endian.
	algo = buff.data[ALGO_ALGO];
	event->time = get_wall_clock();
	elapsed_ms = get_elapsed_realtime();
	event->ertime = elapsed_ms > 0 ? elapsed_ms : 0;
	if (algo == SENSORHUB_ALGO_ACCUM_MVMT) {
	event->type = SENSORHUB_EVENT_ACCUM_MVMT;
	event->time_s = STMLE16TOH(buff.data + ALGO_TIME);
	event->distance = STMLE16TOH(buff.data + ALGO_DISTANCE);
	//ALOGD("sensorhub_poll(): accum mvmt: time_s: %d, distance: %d",
	// event->time_s, event->distance);
	} else
	if (algo == SENSORHUB_ALGO_ACCUM_MODALITY) {
	event->type = SENSORHUB_EVENT_ACCUM_STATE;
	event->accum_algo = algo;
	event->id = STMLE16TOH(buff.data + ALGO_ID);
	} else {
	event->type = SENSORHUB_EVENT_TRANSITION;
	elapsed_ms = STMLE16TOH(buff.data + ALGO_MS) * 1000;
	event->time -= elapsed_ms;
	if (event->ertime > 0)
	event->ertime -= elapsed_ms;
	event->algo = algo;
	event->past = (buff.data[ALGO_PAST] & 0x80) > 0;
	event->confidence = buff.data[ALGO_CONFIDENCE] & 0x7F;
	event->old_state = STMLE16TOH(buff.data + ALGO_OLDSTATE);
	event->new_state = STMLE16TOH(buff.data + ALGO_NEWSTATE);
	//ALOGD("sensorhub_poll(): tran: algo: %d, elapsed: %lld, t: %lld, ert: %lld",
	// event->algo, elapsed_ms, event->time, event->ertime);
	}
	break;
	case DT_GENERIC_INT:
	#if 0
	// packaging irq3_status into ertime field
	// of the event
	event->type = SENSORHUB_EVENT_GENERIC_CB;
	event->time = get_wall_clock();
	event->ertime = buff.data[GENERIC_INT_OFFSET];
	#endif
	context->data_pollfd.revents = 0;
	return 0;
	break;
	case DT_RESET:
	event->type = SENSORHUB_EVENT_RESET;
	event->time = get_wall_clock();
	break;
	}
	context->data_pollfd.revents = 0;
	return 1;
	}

	static int sensorhub_close(struct hw_device_t* device)
	{
	struct sensorhub_context_t* context = (struct sensorhub_context_t*)device;
	close(context->control_fd);
	close(context->data_pollfd.fd);
	free(context);
	pthread_mutex_destroy(&g_lock);
	return 0;
	}

	static int sensorhub_open(const struct hw_module_t* module, char const* name, struct hw_device_t** device)
	{
	struct sensorhub_context_t* context = calloc(1, sizeof(struct sensorhub_context_t));
	int fd;

	if (!context) {
	ALOGE("%s: Couldn't allocate context.", __func__);
	return -ENOMEM;
	}

	pthread_mutex_init(&g_lock, NULL);

	context->device.common.tag = HARDWARE_DEVICE_TAG;
	context->device.common.version = 0;
	context->device.common.module = (struct hw_module_t*)module;
	context->device.common.close = sensorhub_close;
	context->device.enable = sensorhub_enable;
	context->device.algo_req = sensorhub_algo_req;
	context->device.algo_query = sensorhub_algo_query;
	context->device.poll = sensorhub_poll;

	fd = open(DRIVER_CONTROL_PATH, O_RDWR);
	if (fd < 0) {
	ALOGE("%s: Couldn't open control '%s' (%s)",
	__func__, DRIVER_CONTROL_PATH, strerror(errno));
	free(context);
	return -errno;
	}
	context->control_fd = fd;

	fd = open(DRIVER_DATA_NAME, O_RDONLY);
	if (fd < 0) {
	ALOGE("%s: Couldn't open data '%s' (%s)",
	__func__, DRIVER_DATA_NAME, strerror(errno));
	close(context->control_fd);
	free(context);
	return -errno;
	}
	context->data_pollfd.fd = fd;
	context->data_pollfd.events = POLLIN;

	context->active_algos = 0;

	device = (struct hw_device_t)context;
	return 0;
	}

	static struct hw_module_methods_t sensorhub_module_methods = {
	.open = sensorhub_open,
	};

	struct hw_module_t HAL_MODULE_INFO_SYM = {
	.tag = HARDWARE_MODULE_TAG,
	.version_major = 3,
	.version_minor = 0,
	.id = SENSORHUB_HARDWARE_MODULE_ID,
	.name = "Motorola Mobility Smart Fusion module",
	.author = "Motorola Mobility, Inc.",
	.methods = &sensorhub_module_methods,
	};