From: Jinhyung Choi Date: Mon, 7 Jul 2014 04:58:00 +0000 (+0900) Subject: sensor: added rotation vector sensor driver X-Git-Tag: submit/tizen_common/20140905.094502~9 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=bd1a75ca5a8575ccfb6cbaaa847d6297cc061214;p=sdk%2Femulator%2Femulator-kernel.git sensor: added rotation vector sensor driver also added error handling of sensor init when one of sensors' init is failed. Change-Id: I1fcaa4c454da8270c07c035789ace91225f2993b Signed-off-by: Jinhyung Choi --- diff --git a/drivers/maru/sensors/Makefile b/drivers/maru/sensors/Makefile index ce63afa5a5bd..92d79d5addce 100644 --- a/drivers/maru/sensors/Makefile +++ b/drivers/maru/sensors/Makefile @@ -4,4 +4,5 @@ obj-$(CONFIG_MARU_VIRTIO_SENSOR) += maru_virtio_sensor.o \ maru_gyro.o \ maru_light.o \ maru_proxi.o \ + maru_rotation_vector.o \ maru_haptic.o diff --git a/drivers/maru/sensors/maru_accel.c b/drivers/maru/sensors/maru_accel.c index d7a18bec3ac8..679ffa36f88b 100644 --- a/drivers/maru/sensors/maru_accel.c +++ b/drivers/maru/sensors/maru_accel.c @@ -377,5 +377,6 @@ int maru_accel_exit(struct virtio_sensor *vs) { data = (struct maru_accel_data *)vs->accel_handle; accel_clear(data); + INFO("maru_accel device exit ends."); return 0; } diff --git a/drivers/maru/sensors/maru_geo.c b/drivers/maru/sensors/maru_geo.c index a934fd000a59..b5422e8909a8 100644 --- a/drivers/maru/sensors/maru_geo.c +++ b/drivers/maru/sensors/maru_geo.c @@ -392,5 +392,6 @@ int maru_geo_exit(struct virtio_sensor *vs) { data = (struct maru_geo_data *)vs->geo_handle; geo_clear(data); + INFO("maru_geo device exit ends."); return 0; } diff --git a/drivers/maru/sensors/maru_gyro.c b/drivers/maru/sensors/maru_gyro.c index d96810d671fb..c54268580e19 100644 --- a/drivers/maru/sensors/maru_gyro.c +++ b/drivers/maru/sensors/maru_gyro.c @@ -403,5 +403,6 @@ int maru_gyro_exit(struct virtio_sensor *vs) { data = (struct maru_gyro_data *)vs->gyro_handle; gyro_clear(data); + INFO("maru_gyro device exit ends."); return 0; } diff --git a/drivers/maru/sensors/maru_haptic.c b/drivers/maru/sensors/maru_haptic.c index 4d05cb4aa097..0d8c7fa35c90 100644 --- a/drivers/maru/sensors/maru_haptic.c +++ b/drivers/maru/sensors/maru_haptic.c @@ -155,6 +155,7 @@ int maru_haptic_exit(struct virtio_sensor *vs) { data = (struct maru_haptic_data *)vs->haptic_handle; haptic_clear(data); + INFO("maru_haptic device exit ends."); return 0; } diff --git a/drivers/maru/sensors/maru_light.c b/drivers/maru/sensors/maru_light.c index cd3c5582ddbc..ae77bc95eb45 100644 --- a/drivers/maru/sensors/maru_light.c +++ b/drivers/maru/sensors/maru_light.c @@ -393,5 +393,6 @@ int maru_light_exit(struct virtio_sensor *vs) { data = (struct maru_light_data *)vs->light_handle; light_clear(data); + INFO("maru_light device exit ends."); return 0; } diff --git a/drivers/maru/sensors/maru_proxi.c b/drivers/maru/sensors/maru_proxi.c index 8aa1a5455541..94bbae3d83b0 100644 --- a/drivers/maru/sensors/maru_proxi.c +++ b/drivers/maru/sensors/maru_proxi.c @@ -387,5 +387,6 @@ int maru_proxi_exit(struct virtio_sensor *vs) { data = (struct maru_proxi_data *)vs->proxi_handle; proxi_clear(data); + INFO("maru_proxi device exit ends."); return 0; } diff --git a/drivers/maru/sensors/maru_rotation_vector.c b/drivers/maru/sensors/maru_rotation_vector.c new file mode 100644 index 000000000000..7757c6c0de7a --- /dev/null +++ b/drivers/maru/sensors/maru_rotation_vector.c @@ -0,0 +1,331 @@ +/* + * Maru Virtio Rotation Vector Sensor Device Driver + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. All rights reserved. + * + * Contact: + * Jinhyung Choi + * Sangho Park + * YeongKyoon Lee + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + * Contributors: + * - S-Core Co., Ltd + * + */ + +#include +#include + +#include "maru_virtio_sensor.h" + +struct maru_rotation_vector_data { + struct input_dev *input_data; + struct delayed_work work; + struct mutex data_mutex; + + struct virtio_sensor* vs; + + atomic_t enable; + atomic_t poll_delay; +}; + +static struct device *rotation_vector_sensor_device; + +static void maru_rotation_vector_input_work_func(struct work_struct *work) { + + int poll_time = 200000000; + int enable = 0; + int ret = 0; + int quad_a, quad_b, quad_c, quad_d, accuracy; + char sensor_data[__MAX_BUF_SENSOR]; + struct maru_rotation_vector_data *data = container_of((struct delayed_work *)work, + struct maru_rotation_vector_data, work); + + LOG(1, "maru_rotation_vector_input_work_func starts"); + + memset(sensor_data, 0, __MAX_BUF_SENSOR); + poll_time = atomic_read(&data->poll_delay); + + mutex_lock(&data->data_mutex); + enable = atomic_read(&data->enable); + mutex_unlock(&data->data_mutex); + + if (enable) { + mutex_lock(&data->data_mutex); + ret = get_sensor_data(sensor_type_rotation_vector, sensor_data); + mutex_unlock(&data->data_mutex); + if (!ret) { + sscanf(sensor_data, "%d,%d,%d,%d,%d", &quad_a, &quad_b, &quad_c, &quad_d, &accuracy); + LOG(1, "rotation_vector_set %d,%d,%d,%d,%d", quad_a, quad_b, quad_c, quad_d, accuracy); + + input_report_rel(data->input_data, REL_X, quad_a); + input_report_rel(data->input_data, REL_Y, quad_b); + input_report_rel(data->input_data, REL_Z, quad_c); + input_report_rel(data->input_data, REL_RX, quad_d); + input_report_rel(data->input_data, REL_RY, accuracy); + input_sync(data->input_data); + } + } + + mutex_lock(&data->data_mutex); + enable = atomic_read(&data->enable); + mutex_unlock(&data->data_mutex); + + LOG(1, "enable: %d, poll_time: %d", enable, poll_time); + if (enable) { + if (poll_time > 0) { + schedule_delayed_work(&data->work, nsecs_to_jiffies(poll_time)); + } else { + schedule_delayed_work(&data->work, 0); + } + } + + LOG(1, "maru_rotation_vector_input_work_func ends"); + +} + +static ssize_t maru_name_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s", MARU_ROTATION_DEVICE_NAME); +} + +static ssize_t maru_vendor_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s", MARU_SENSOR_DEVICE_VENDOR); +} + +static ssize_t maru_enable_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + return get_data_for_show(sensor_type_rotation_vector_enable, buf); +} + +static ssize_t maru_enable_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct maru_rotation_vector_data *data = input_get_drvdata(input_data); + int value = simple_strtoul(buf, NULL, 10); + + if (value != 0 && value != 1) + return count; + + set_sensor_data(sensor_type_rotation_vector_enable, buf); + + if (value) { + if (atomic_read(&data->enable) != 1) { + atomic_set(&data->enable, 1); + schedule_delayed_work(&data->work, 0); + + } + } else { + if (atomic_read(&data->enable) != 0) { + atomic_set(&data->enable, 0); + cancel_delayed_work(&data->work); + } + } + + return strnlen(buf, __MAX_BUF_SENSOR); +} + +static ssize_t maru_poll_delay_show(struct device *dev, struct device_attribute *attr, char *buf) +{ + return get_data_for_show(sensor_type_rotation_vector_delay, buf); +} + +static ssize_t maru_poll_delay_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct maru_rotation_vector_data *data = input_get_drvdata(input_data); + int value = simple_strtoul(buf, NULL, 10); + + set_sensor_data(sensor_type_rotation_vector_delay, buf); + atomic_set(&data->poll_delay, value); + + return strnlen(buf, __MAX_BUF_SENSOR); +} + +static struct device_attribute dev_attr_sensor_name = + __ATTR(name, S_IRUGO, maru_name_show, NULL); + +static struct device_attribute dev_attr_sensor_vendor = + __ATTR(vendor, S_IRUGO, maru_vendor_show, NULL); + +static struct device_attribute *rotation_vector_sensor_attrs [] = { + &dev_attr_sensor_name, + &dev_attr_sensor_vendor, + NULL, +}; + +static struct device_attribute attr_rotation_vector [] = +{ + MARU_ATTR_RW(enable), + MARU_ATTR_RW(poll_delay), +}; + +static struct attribute *maru_rotation_vector_attribute[] = { + &attr_rotation_vector[0].attr, + &attr_rotation_vector[1].attr, + NULL +}; + +static struct attribute_group maru_rotation_vector_attribute_group = { + .attrs = maru_rotation_vector_attribute +}; + +static void rotation_vector_clear(struct maru_rotation_vector_data *data) { + if (data == NULL) + return; + + if (data->input_data) { + sysfs_remove_group(&data->input_data->dev.kobj, + &maru_rotation_vector_attribute_group); + input_free_device(data->input_data); + } + + kfree(data); + data = NULL; +} + +static int set_initial_value(struct maru_rotation_vector_data *data) +{ + int delay = 0; + int ret = 0; + char sensor_data [__MAX_BUF_SENSOR]; + + memset(sensor_data, 0, __MAX_BUF_SENSOR); + + ret = get_sensor_data(sensor_type_rotation_vector_delay, sensor_data); + if (ret) { + ERR("failed to get initial delay time"); + return ret; + } + + delay = sensor_atoi(sensor_data); + if (delay < 0) { + ERR("weird value is set initial delay"); + return ret; + } + + atomic_set(&data->poll_delay, delay); + + memset(sensor_data, 0, sizeof(sensor_data)); + sensor_data[0] = '0'; + set_sensor_data(sensor_type_rotation_vector_enable, sensor_data); + atomic_set(&data->enable, 0); + + return ret; +} + +static int create_input_device(struct maru_rotation_vector_data *data) +{ + int ret = 0; + struct input_dev *input_data = NULL; + + input_data = input_allocate_device(); + if (input_data == NULL) { + ERR("failed initialing input handler"); + rotation_vector_clear(data); + return -ENOMEM; + } + + input_data->name = SENSOR_ROTATION_INPUT_NAME; + input_data->id.bustype = BUS_I2C; + + set_bit(EV_REL, input_data->evbit); + set_bit(EV_SYN, input_data->evbit); + input_set_capability(input_data, EV_REL, REL_X); + input_set_capability(input_data, EV_REL, REL_Y); + input_set_capability(input_data, EV_REL, REL_Z); + input_set_capability(input_data, EV_REL, REL_RX); + input_set_capability(input_data, EV_REL, REL_RY); + + data->input_data = input_data; + + ret = input_register_device(input_data); + if (ret) { + ERR("failed to register input data"); + rotation_vector_clear(data); + return ret; + } + + input_set_drvdata(input_data, data); + + ret = sysfs_create_group(&input_data->dev.kobj, + &maru_rotation_vector_attribute_group); + if (ret) { + rotation_vector_clear(data); + ERR("failed initialing devices"); + return ret; + } + + return ret; +} + +int maru_rotation_vector_init(struct virtio_sensor *vs) { + int ret = 0; + struct maru_rotation_vector_data *data = NULL; + + INFO("maru_rotation_vector device init starts."); + + data = kmalloc(sizeof(struct maru_rotation_vector_data), GFP_KERNEL); + if (data == NULL) { + ERR("failed to create rotation_vector data."); + return -ENOMEM; + } + + vs->rotation_vector_handle = data; + data->vs = vs; + + mutex_init(&data->data_mutex); + + INIT_DELAYED_WORK(&data->work, maru_rotation_vector_input_work_func); + + // create name & vendor + ret = register_sensor_device(rotation_vector_sensor_device, vs, + rotation_vector_sensor_attrs, DRIVER_ROTATION_NAME); + if (ret) { + ERR("failed to register rotation_vector device"); + rotation_vector_clear(data); + return -1; + } + + // create input + ret = create_input_device(data); + if (ret) { + ERR("failed to create input device"); + return ret; + } + + // set initial delay & enable + ret = set_initial_value(data); + if (ret) { + ERR("failed to set initial value"); + return ret; + } + + INFO("maru_rotation_vector device init ends."); + + return ret; +} + +int maru_rotation_vector_exit(struct virtio_sensor *vs) { + struct maru_rotation_vector_data *data = NULL; + + data = (struct maru_rotation_vector_data *)vs->rotation_vector_handle; + rotation_vector_clear(data); + INFO("maru_rotation_vector device exit ends."); + return 0; +} diff --git a/drivers/maru/sensors/maru_virtio_sensor.c b/drivers/maru/sensors/maru_virtio_sensor.c index e63cdbfda80f..63c71dfc6b88 100644 --- a/drivers/maru/sensors/maru_virtio_sensor.c +++ b/drivers/maru/sensors/maru_virtio_sensor.c @@ -257,72 +257,101 @@ int get_data_for_show(int type, char* buf) return sprintf(buf, "%s", sensor_data); } -static int device_init(struct virtio_sensor *vs) +static void device_init(struct virtio_sensor *vs) { int ret = 0; if (vs->sensor_capability & sensor_cap_accel) { ret = maru_accel_init(vs); - if (ret) - return ret; + if (ret) { + vs->sensor_fail_init |= sensor_cap_accel; + ERR("failed to init accel with error %d", ret); + } } if (vs->sensor_capability & sensor_cap_geo) { ret = maru_geo_init(vs); - if (ret) - return ret; + if (ret) { + vs->sensor_fail_init |= sensor_cap_geo; + ERR("failed to init geo with error %d", ret); + } } if (vs->sensor_capability & sensor_cap_gyro) { ret = maru_gyro_init(vs); - if (ret) - return ret; + if (ret) { + vs->sensor_fail_init |= sensor_cap_gyro; + ERR("failed to init gyro with error %d", ret); + } } if (vs->sensor_capability & sensor_cap_light) { ret = maru_light_init(vs); - if (ret) - return ret; + if (ret) { + vs->sensor_fail_init |= sensor_cap_light; + ERR("failed to init light with error %d", ret); + } } if (vs->sensor_capability & sensor_cap_proxi) { ret = maru_proxi_init(vs); - if (ret) - return ret; + if (ret) { + vs->sensor_fail_init |= sensor_cap_proxi; + ERR("failed to init proxi with error %d", ret); + } + } + + if (vs->sensor_capability & sensor_cap_rotation_vector) { + ret = maru_rotation_vector_init(vs); + if (ret) { + vs->sensor_fail_init |= sensor_cap_rotation_vector; + ERR("failed to init rotation vector with error %d", ret); + } } if (vs->sensor_capability & sensor_cap_haptic) { ret = maru_haptic_init(vs); - if (ret) - return ret; + if (ret) { + vs->sensor_fail_init |= sensor_cap_haptic; + ERR("failed to init haptic with error %d", ret); + } } - - return ret; } static void device_exit(struct virtio_sensor *vs) { - if (vs->sensor_capability & sensor_cap_accel) { + if (vs->sensor_capability & sensor_cap_accel && + !(vs->sensor_fail_init & sensor_cap_accel)) { maru_accel_exit(vs); } - if (vs->sensor_capability & sensor_cap_geo) { + if (vs->sensor_capability & sensor_cap_geo && + !(vs->sensor_fail_init & sensor_cap_geo)) { maru_geo_exit(vs); } - if (vs->sensor_capability & sensor_cap_gyro) { + if (vs->sensor_capability & sensor_cap_gyro && + !(vs->sensor_fail_init & sensor_cap_gyro)) { maru_gyro_exit(vs); } - if (vs->sensor_capability & sensor_cap_light) { + if (vs->sensor_capability & sensor_cap_light && + !(vs->sensor_fail_init & sensor_cap_light)) { maru_light_exit(vs); } - if (vs->sensor_capability & sensor_cap_proxi) { + if (vs->sensor_capability & sensor_cap_proxi && + !(vs->sensor_fail_init & sensor_cap_proxi)) { maru_proxi_exit(vs); } - if (vs->sensor_capability & sensor_cap_haptic) { + if (vs->sensor_capability & sensor_cap_rotation_vector && + !(vs->sensor_fail_init & sensor_cap_rotation_vector)) { + maru_rotation_vector_exit(vs); + } + + if (vs->sensor_capability & sensor_cap_haptic && + !(vs->sensor_fail_init & sensor_cap_haptic)) { maru_haptic_exit(vs); } } @@ -407,9 +436,16 @@ static int sensor_probe(struct virtio_device* dev) vs->sensor_capability = sensor_atoi(sensor_data); INFO("sensor capability is %02x", vs->sensor_capability); - ret = device_init(vs); - if (ret) { - ERR("failed initialing devices"); + if (vs->sensor_capability == 0) { + ERR("No sensor devices "); + cleanup(dev); + return ret; + } + + device_init(vs); + + if (vs->sensor_capability == vs->sensor_fail_init) { + ERR("failed initialing all devices"); cleanup(dev); return ret; } diff --git a/drivers/maru/sensors/maru_virtio_sensor.h b/drivers/maru/sensors/maru_virtio_sensor.h index b72d88a4cab9..12c75eb42a60 100644 --- a/drivers/maru/sensors/maru_virtio_sensor.h +++ b/drivers/maru/sensors/maru_virtio_sensor.h @@ -64,18 +64,22 @@ enum sensor_types { sensor_type_proxi, sensor_type_proxi_enable, sensor_type_proxi_delay, + sensor_type_rotation_vector, + sensor_type_rotation_vector_enable, + sensor_type_rotation_vector_delay, sensor_type_mag, sensor_type_tilt, sensor_type_max }; enum sensor_capabilities { - sensor_cap_accel = 0x01, - sensor_cap_geo = 0x02, - sensor_cap_gyro = 0x04, - sensor_cap_light = 0x08, - sensor_cap_proxi = 0x10, - sensor_cap_haptic = 0x20 + sensor_cap_accel = 0x01, + sensor_cap_geo = 0x02, + sensor_cap_gyro = 0x04, + sensor_cap_light = 0x08, + sensor_cap_proxi = 0x10, + sensor_cap_rotation_vector = 0x20, + sensor_cap_haptic = 0x40 }; #define __MAX_BUF_SIZE 1024 @@ -109,12 +113,14 @@ struct virtio_sensor { #endif int sensor_capability; + int sensor_fail_init; void* accel_handle; void* geo_handle; void* gyro_handle; void* light_handle; void* proxi_handle; + void* rotation_vector_handle; void* haptic_handle; }; @@ -169,6 +175,10 @@ int get_sensor_data(int type, char* data); #define SENSOR_PROXI_INPUT_NAME "proximity_sensor" #define MARU_PROXI_DEVICE_NAME "maru_sensor_proxi_1" +#define DRIVER_ROTATION_NAME "rotation" +#define SENSOR_ROTATION_INPUT_NAME "rot_sensor" +#define MARU_ROTATION_DEVICE_NAME "maru_sensor_rotation_vector_1" + #define SENSOR_HAPTIC_INPUT_NAME "haptic_sensor" // It locates /sys/module/maru_virtio_sensor/parameters/sensor_driver_debug @@ -217,6 +227,12 @@ int maru_light_exit(struct virtio_sensor *vs); int maru_proxi_init(struct virtio_sensor *vs); int maru_proxi_exit(struct virtio_sensor *vs); +/* + * Rotation Vector device + */ +int maru_rotation_vector_init(struct virtio_sensor *vs); +int maru_rotation_vector_exit(struct virtio_sensor *vs); + /* * Haptic device */