Add sensorhub and hrm_batch

Change-Id: Icd6c549ba23417fa29382a845134078958a6cb1c
Signed-off-by: Boram Bae <boram21.bae@samsung.com>

14 files changed, 1359 insertions, 2 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d2b420a..b899200 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,7 +21,7 @@ SET(HUMIDITY "OFF")
 SET(TEMP_HUMIDITY "OFF")
 SET(ULTRAVIOLET "OFF")
 SET(DUST "OFF")
-SET(SENSORHUB "OFF")
+SET(SENSORHUB "ON")
 
 # Common Options
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O2 -omit-frame-pointer -std=gnu++0x")
diff --git a/packaging/99-sensorhub.rules b/packaging/99-sensorhub.rules
index d34f721..6fd55b1 100644
--- a/packaging/99-sensorhub.rules
+++ b/packaging/99-sensorhub.rules
@@ -7,5 +7,5 @@ RUN+="/bin/sh -c '/bin/chsmack -a \* %S/%p/enable %S/%p/set_cal_data %S/%p/*_pol
 SUBSYSTEMS=="misc", DEVPATH=="/devices/virtual/misc/ssp_sensorhub", \
 ENV{DEVNAME}=="/dev/ssp_sensorhub", GROUP="input", SECLABEL{smack}="*"
 
-KERNEL == "ssp_sensorhub", GROUP="sensor", MODE="0660" RUN+="/bin/chown -R sensor:sensor /sys/class/sensors/ssp_sensor/"
+KERNEL == "ssp_sensorhub", GROUP="system_share", MODE="0660" RUN+="/bin/chown -R root:system_share /sys/class/sensors/ssp_sensor/"
 KERNEL == "batch_io", GROUP="sensor", MODE="0660"
diff --git a/src/create.cpp b/src/create.cpp
index 263a2b8..9246692 100644
--- a/src/create.cpp
+++ b/src/create.cpp
@@ -25,6 +25,7 @@
 #include "light/light_device.h"
 #include "hrm_raw/hrm_raw_device.h"
 #include "hrm/hrm_device.h"
+#include "sensorhub/sensorhub.h"
 
 static std::vector<sensor_device_t> devs;
 
@@ -53,6 +54,7 @@ extern "C" int create(sensor_device_t **devices)
 	create_sensor<light_device>("Light");
 	create_sensor<hrm_raw_device>("HRM Raw");
 	create_sensor<hrm_device>("HRM");
+	create_sensor<sensorhub_device>("Sensorhub");
 
 	*devices = &devs[0];
 	return devs.size();
diff --git a/src/sensor_common.h b/src/sensor_common.h
index 5072936..56cf5d4 100755
--- a/src/sensor_common.h
+++ b/src/sensor_common.h
@@ -18,10 +18,15 @@
 #ifndef __SENSOR_COMMON_H__
 #define __SENSOR_COMMON_H__
 
+#define SENSOR_LIB_SHIFT 16
 #define SENSOR_EVENT_SHIFT 16
 #define RAW_DATA_EVENT 0x0001
 
+#define SENSOR_DEVICE_ID(type, index) ((type) << SENSOR_LIB_SHIFT | (index))
+#define SENSOR_EVENT_TYPE(type) ((type) << SENSOR_EVENT_SHIFT | RAW_DATA_EVENT)
 #define SHORT_TO_UINT64(value, shift) (((unsigned long long)(value & 0xFFFF)) << shift)
+#define CONTEXT_SENSOR_ID  0xFFFF0001
+
 #define NS_TO_US(x) ((x) / 1000)
 
 #define UNKNOWN_NAME "UNKNOWN"
diff --git a/src/sensor_log.h b/src/sensor_log.h
index 8ac5abb..d30e210 100644
--- a/src/sensor_log.h
+++ b/src/sensor_log.h
@@ -119,4 +119,8 @@
 
 #endif
 
+#define log_oom() ({ \
+	_E("Out of memory"); \
+	-ENOMEM;})
+
 #endif /* __SENSOR_LOG_H__ */
diff --git a/src/sensorhub/hrm_batch.cpp b/src/sensorhub/hrm_batch.cpp
new file mode 100644
index 0000000..b4d9fa9
--- /dev/null
+++ b/src/sensorhub/hrm_batch.cpp
@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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 <stdlib.h>
+#include <sensor_common.h>
+#include <sensor/sensor_hal.h>
+#include <sensor_log.h>
+#include <deque>
+#include <util.h>
+
+#include "sensorhub_sensor.h"
+#include "sensorhub_manager.h"
+
+#define HEADER_SIZE			8
+#define TUPLE_MAX_COUNT		600
+
+#define DEFAULT_OPR_MODE	1
+
+#define HRM_BATCH_NAME	"SENSOR_HRM_BATCH"
+
+static const sensor_info_t sensor_info = {
+	id: SENSOR_DEVICE_ID(SHUB_LIB_HRM_BATCH, 0x1),
+	name: HRM_BATCH_NAME,
+	type: SENSOR_DEVICE_HRM_BATCH,
+	event_type: SENSOR_EVENT_TYPE(SENSOR_DEVICE_HRM_BATCH),
+	model_name: "HRM Batch Sensor",
+	vendor: "Samsung Electronics",
+	min_range: 0,
+	max_range: 1,
+	resolution: 1,
+	min_interval: 0,
+	max_batch_count: 0,
+	wakeup_supported: true
+};
+
+class sh_hrm_batch_sensor : public sensorhub_sensor {
+public:
+	sh_hrm_batch_sensor();
+
+	bool enable(void);
+	bool disable(void);
+
+	int parse(const char *data, int data_len);
+	int get_data(sensor_data_t **data, int *length);
+	bool set_attribute_int(int32_t attribute, int32_t value);
+
+	bool flush(void);
+
+private:
+	sensor_data_t *m_data;
+	int m_batch_count;
+	int m_opr_mode;
+	bool m_raw_bpm;
+};
+
+sh_hrm_batch_sensor::sh_hrm_batch_sensor()
+: sensorhub_sensor(&sensor_info)
+, m_data(NULL)
+, m_batch_count(0)
+, m_opr_mode(DEFAULT_OPR_MODE)
+, m_raw_bpm(false)
+{
+}
+
+bool sh_hrm_batch_sensor::enable(void)
+{
+	struct cmd {
+		uint8_t inst;
+		uint8_t lib_type;
+		uint8_t mode;
+		uint8_t reserved; /* Alignment */
+	} add_cmd = {(uint8_t)SHUB_INST_LIB_ADD,
+	         (uint8_t)SHUB_LIB_HRM_BATCH,
+	         (uint8_t)m_opr_mode, 0};
+
+	char *cmd = reinterpret_cast<char *>(&add_cmd);
+
+	if (!m_controller) {
+		_E("Controller is not set : %s", HRM_BATCH_NAME);
+		return false;
+	}
+
+	if (m_controller->send_sensorhub_data(cmd, sizeof(struct cmd)) < 0)
+		return false;
+
+	return sensorhub_sensor::enable();
+}
+
+bool sh_hrm_batch_sensor::disable(void)
+{
+	m_raw_bpm = false;
+	return sensorhub_sensor::enable(false, SHUB_LIB_HRM_BATCH);
+}
+
+int sh_hrm_batch_sensor::parse(const char *data, int data_len)
+{
+	/*
+	 * 1, 1, 46, mode(1byte), reserved(2bytes), length(2bytes),
+	 * flag(1byte), hr(1byte), rri(2bytes), flag, hr, rri ...
+	 */
+
+	struct tuple {
+		int8_t flag;
+		uint8_t bpm;
+		int16_t rri;
+	} __attribute__((packed));
+
+	struct hrm_batch {
+		uint8_t inst;
+		uint8_t type;
+		uint8_t lib_type;
+		uint8_t mode;
+		int16_t reserved; /* Alignment */
+		int16_t length;
+		struct tuple tuples[TUPLE_MAX_COUNT];
+	} __attribute__((packed));
+
+	struct hrm_batch *eh = (struct hrm_batch *)data;
+	int count;
+
+	count = eh->length / sizeof(struct tuple);
+	sensor_data_t *d;
+	d = (sensor_data_t *)malloc(sizeof(sensor_data_t) * count);
+	if (!d)
+		return log_oom();
+
+	for (int i = 0; i < count; i++) {
+		d[i].value_count = 3;
+		d[i].values[0] = (int)eh->tuples[i].flag;
+		d[i].values[1] = (int)eh->tuples[i].bpm;
+		d[i].values[2] = (int)eh->tuples[i].rri;
+		d[i].accuracy = SENSOR_ACCURACY_GOOD;
+	}
+
+	m_data = (sensor_data_t *)d;
+	m_batch_count = count;
+
+	return HEADER_SIZE + eh->length;
+}
+
+int sh_hrm_batch_sensor::get_data(sensor_data_t **data, int *length)
+{
+	*data = m_data;
+	*length = (sizeof(sensor_data_t) * m_batch_count);
+	m_data = NULL;
+
+	_I("hrm_batch get");
+	return 0;
+}
+
+bool sh_hrm_batch_sensor::set_attribute_int(int32_t attribute, int32_t value)
+{
+	return false;
+}
+
+bool sh_hrm_batch_sensor::flush(void)
+{
+	struct cmd {
+		uint8_t inst;
+		uint8_t lib;
+		uint8_t prop;
+		uint8_t reserved; /* Alignment */
+	} get_cmd = {(uint8_t)SHUB_INST_LIB_GETVALUE,
+	             (uint8_t)SHUB_LIB_HRM_BATCH,
+	             (uint8_t)SHUB_EXT_CURRENT_INFO, 0};
+
+	char *cmd = reinterpret_cast<char *>(&get_cmd);
+
+	return m_controller->send_sensorhub_data(cmd, sizeof(struct cmd)) >= 0;
+}
+
+REGISTER_SENSORHUB_LIB(sensor_info, sh_hrm_batch_sensor)
diff --git a/src/sensorhub/sensorhub.cpp b/src/sensorhub/sensorhub.cpp
new file mode 100644
index 0000000..409ca03
--- /dev/null
+++ b/src/sensorhub/sensorhub.cpp
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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 <unistd.h>
+#include <algorithm>
+#include <sensor_log.h>
+
+#include "sensorhub.h"
+#include "sensor_common.h"
+#include "sensorhub_controller.h"
+#include "sensorhub_manager.h"
+
+sensorhub_device::sensorhub_device()
+{
+	controller = new(std::nothrow) sensorhub_controller;
+	if (!controller) {
+		_E("Failed to allocated memory");
+		throw;
+	}
+
+	manager = &sensorhub_manager::get_instance();
+	if (!manager) {
+		_E("Failed to allocated memory");
+		throw;
+	}
+	manager->set_controller(controller);
+
+	_I("sensorhub_device is created!");
+}
+
+sensorhub_device::~sensorhub_device()
+{
+	delete controller;
+	_I("sensorhub_device is destroyed!");
+}
+
+int sensorhub_device::get_poll_fd(void)
+{
+	return controller->get_poll_fd();
+}
+
+int sensorhub_device::get_sensors(const sensor_info_t **sensors)
+{
+	int size;
+	size = manager->get_sensor_infos(sensors);
+
+	return size;
+}
+
+bool sensorhub_device::enable(uint32_t id)
+{
+	controller->enable();
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+
+	if (!sensor) {
+		_E("Failed to enable sensor(0x%x)", id);
+		return false;
+	}
+
+	return sensor->enable();
+}
+
+bool sensorhub_device::disable(uint32_t id)
+{
+	controller->disable();
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+
+	if (!sensor) {
+		_E("Failed to disable sensor(0x%x)", id);
+		return false;
+	}
+
+	return sensor->disable();
+}
+
+bool sensorhub_device::set_interval(uint32_t id, unsigned long val)
+{
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+
+	if (!sensor) {
+		_E("Failed to set interval to sensor(0x%x)", id);
+		return false;
+	}
+
+	return sensor->set_interval(val);
+}
+
+bool sensorhub_device::set_batch_latency(uint32_t id, unsigned long val)
+{
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+
+	if (!sensor) {
+		_E("Failed to set batch latency to sensor(0x%x)", id);
+		return false;
+	}
+
+	return sensor->set_batch_latency(val);
+}
+
+bool sensorhub_device::set_attribute_int(uint32_t id, int32_t attribute, int32_t value)
+{
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+
+	if (!sensor) {
+		_E("Failed to set attribute to sensor(0x%x)", id);
+		return false;
+	}
+
+	return sensor->set_attribute_int(attribute, value);
+}
+
+bool sensorhub_device::set_attribute_str(uint32_t id, int32_t attribute, char *value, int len)
+{
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+
+	if (!sensor) {
+		_E("Failed to set attribute to sensor(0x%x)", id);
+		return false;
+	}
+
+	return sensor->set_attribute_str(attribute, value, len);
+}
+
+int sensorhub_device::read_fd(uint32_t **ids)
+{
+	sensorhub_data_t data;
+
+	// step 1
+	if (!controller->read_fd(data))
+		return 0;
+
+	// step 2
+	const char *hub_data = data.values;
+	int data_len = data.value_count;
+
+	// step 3
+	event_ids.clear();
+
+	/* Sensorhub Sensors */
+	while (data_len > 0) {
+		_D("Remaining data length: %d", data_len);
+		int parsed = parse(hub_data, data_len);
+		if (parsed <= 0) {
+			_I("Parsing failed");
+			break;
+		}
+
+		data_len -= parsed;
+		hub_data += parsed;
+	}
+
+	// step 4
+	int size = event_ids.size();
+
+	if (event_ids.empty())
+		return 0;
+
+	*ids = &event_ids[0];
+
+	return size;
+}
+
+int sensorhub_device::get_data(uint32_t id, sensor_data_t **data, int *length)
+{
+	int remains = 1;
+
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+	if (!sensor) {
+		_E("Failed to get data from sensor(0x%x)", id);
+		return -EINVAL;
+	}
+
+	remains = sensor->get_data(data, length);
+
+	return remains;
+}
+
+bool sensorhub_device::flush(uint32_t id)
+{
+	sensorhub_sensor *sensor = manager->get_sensor(id);
+	if (!sensor) {
+		_E("Failed to get data from sensor(0x%x)", id);
+		return false;
+	}
+
+	return sensor->flush();
+}
+
+int sensorhub_device::parse(const char *hub_data, int data_len)
+{
+	return parse_data(hub_data, data_len);
+}
+
+int sensorhub_device::parse_data(const char *hub_data, int data_len)
+{
+	int size = 0;
+	char libtype;
+
+	libtype = hub_data[SHUB_IDX_LIBTYPE];
+	if (!manager->get_sensors(libtype, active_sensors)) {
+		_E("Unknown Sensorhub lib type: %d", libtype);
+		return -EINVAL;
+	}
+
+	for (auto const &sensor : active_sensors) {
+		size = sensor->parse(hub_data, data_len);
+		if (size <= 0)
+			break;
+
+		event_ids.push_back(sensor->get_id());
+	}
+
+	active_sensors.clear();
+
+	return size;
+}
diff --git a/src/sensorhub/sensorhub.h b/src/sensorhub/sensorhub.h
new file mode 100644
index 0000000..eceb5ad
--- /dev/null
+++ b/src/sensorhub/sensorhub.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _SENSORHUB_DEVICE_H_
+#define _SENSORHUB_DEVICE_H_
+
+#include <vector>
+#include <sensor/sensor_hal.h>
+
+#include "sensorhub_controller.h"
+#include "sensorhub_manager.h"
+
+class sensorhub_device : public sensor_device {
+public:
+	sensorhub_device();
+	virtual ~sensorhub_device();
+
+	int get_poll_fd(void);
+	int get_sensors(const sensor_info_t **sensors);
+
+	bool enable(uint32_t id);
+	bool disable(uint32_t id);
+
+	bool set_interval(uint32_t id, unsigned long val);
+	bool set_batch_latency(uint32_t id, unsigned long val);
+	bool set_attribute_int(uint32_t id, int32_t attribute, int32_t value);
+	bool set_attribute_str(uint32_t id, int32_t attribute, char *value, int len);
+
+	int read_fd(uint32_t **ids);
+	int get_data(uint32_t id, sensor_data_t **data, int *length);
+
+	bool flush(uint32_t id);
+
+	std::vector<sensorhub_sensor *> active_sensors;
+
+private:
+	sensorhub_manager *manager;
+	sensorhub_controller *controller;
+	std::vector<uint32_t> event_ids;
+
+	int parse(const char *hub_data, int data_len);
+	int parse_data(const char *hub_data, int data_len);
+};
+
+#endif /* _SENSORHUB_DEVICE_H_ */
diff --git a/src/sensorhub/sensorhub_controller.cpp b/src/sensorhub/sensorhub_controller.cpp
new file mode 100644
index 0000000..d01917f
--- /dev/null
+++ b/src/sensorhub/sensorhub_controller.cpp
@@ -0,0 +1,388 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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 <unistd.h>
+#include <string.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <linux/input.h>
+#include <sys/ioctl.h>
+#include <fstream>
+
+#include <sensor_log.h>
+#include <util.h>
+#include "sensorhub_controller.h"
+
+#define EVENT_TYPE_SENSORHUB_DATA		 REL_RX
+
+#define SSP_INPUT_NODE_NAME			"ssp_context"
+#define SSPSENSORHUB_DEVICE		"/dev/ssp_sensorhub"
+
+#define INJECTION_NODE_PATH "/run/sensord/sensorhub"
+#define INJECTION_ENABLE 1
+#define SENSOR_ATTR_SENSORHUB_INJECT_VALUE CONVERT_TYPE_ATTR(SENSOR_DEVICE_CONTEXT, 0x1)
+
+sensorhub_controller::sensorhub_controller()
+: m_enabled(false)
+, m_poll_node(-1)
+, m_data_node(-1)
+, m_injection_mode(false)
+{
+	/* initialize sensor_data for sensorhub */
+	m_pending_data.accuracy = SENSOR_ACCURACY_UNDEFINED;
+	m_pending_data.timestamp = 0;
+	m_pending_data.value_count = 0;
+	memset(m_pending_data.values, 0, sizeof(m_pending_data.values));
+
+	/* initialize polling node */
+	m_poll_node = open_input_node(SSP_INPUT_NODE_NAME);
+	if (m_poll_node < 0)
+		throw ENXIO;
+
+	/* initialize sensorhub input node */
+	if ((m_data_node = open(SSPSENSORHUB_DEVICE, O_RDWR)) < 0) {
+		_E("Open sensorhub device failed(%d)", m_data_node);
+		throw ENXIO;
+	}
+
+	_I("m_data_node = %s", SSPSENSORHUB_DEVICE);
+}
+
+sensorhub_controller::~sensorhub_controller()
+{
+	if (m_poll_node >= 0)
+		close(m_poll_node);
+
+	if (m_data_node >= 0)
+		close(m_data_node);
+}
+
+int sensorhub_controller::get_poll_fd(void)
+{
+	return m_poll_node;
+}
+
+bool sensorhub_controller::enable(void)
+{
+	if (m_enabled == 0)
+		_I("Enable Sensorhub");
+
+	m_enabled++;
+	return true;
+}
+
+bool sensorhub_controller::disable(void)
+{
+	if (m_enabled == 0) {
+		_E("sensorhub is already disabled");
+		return false;
+	}
+
+	if (m_enabled == 1)
+		_I("Disable Sensorhub");
+
+	m_enabled--;
+	return true;
+}
+
+int sensorhub_controller::open_input_node(const char* input_node)
+{
+	size_t length;
+	int fd = -1;
+	const char *dirname = "/dev/input";
+	char devname[PATH_MAX] = {0};
+	char *filename;
+	DIR *dir;
+	struct dirent *entry;
+
+	_I("======================start open_input_node=============================");
+
+	dir = opendir(dirname);
+
+	if (dir == NULL)
+		return -EINVAL;
+
+	strncpy(devname, dirname, strlen(dirname)+1);
+
+	filename = devname + strlen(devname);
+	*filename++ = '/';
+
+	while (true) {
+		entry = readdir(dir);
+		if (!entry) break;
+
+		if (entry->d_name[0] == '.' &&
+		   (entry->d_name[1] == '\0' ||
+		   (entry->d_name[1] == '.' && entry->d_name[2] == '\0')))
+			continue;
+
+		length = strlen(entry->d_name);
+		strncpy(filename, entry->d_name, length+1);
+
+		/* Need to be terminated with a null character */
+		filename[length] = '\0';
+
+		fd = open(devname, O_RDONLY);
+
+		if (fd < 0)
+			continue;
+
+		char name[80];
+		if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1)
+			name[0] = '\0';
+
+		if (!strcmp(name, input_node)) {
+			_I("m_poll_node = %s", devname);
+			break;
+		}
+
+		close(fd);
+		fd = -1;
+	}
+
+	closedir(dir);
+
+	if (fd < 0)
+		_E("couldn't find '%s' input device", input_node);
+
+	return fd;
+}
+
+bool sensorhub_controller::read_fd(sensorhub_data_t &data)
+{
+	const int INPUT_MAX_BEFORE_SYN = 10;
+	struct input_event sensorhub_input;
+	int read_input_cnt = 0;
+	int sensorhub_len = 0;
+	bool syn = false;
+
+	fd_set readfds, exceptfds;
+
+	FD_ZERO(&readfds);
+	FD_ZERO(&exceptfds);
+
+	FD_SET(m_poll_node, &readfds);
+	FD_SET(m_poll_node, &exceptfds);
+
+	while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
+		int input_len = read(m_poll_node, &sensorhub_input, sizeof(sensorhub_input));
+
+		if (input_len != sizeof(sensorhub_input)) {
+			_E("sensorhub node read fail, read_len = %d", input_len);
+			return false;
+		}
+
+		++read_input_cnt;
+
+		if (sensorhub_input.type == EV_REL) {
+			float value = sensorhub_input.value;
+
+			if (sensorhub_input.code == EVENT_TYPE_SENSORHUB_DATA) {
+				_D("EVENT_TYPE_SENSORHUB_DATA, value_count=%g", value);
+				m_pending_data.value_count = value;
+				sensorhub_len = read_sensorhub_data();
+
+				if (sensorhub_len == 0)
+					_E("No library data");
+				else if (sensorhub_len < 0)
+					_E("read_sensorhub_data() err(%d)", sensorhub_len);
+
+			}
+		} else if (sensorhub_input.type == EV_SYN) {
+			syn = true;
+
+			if (!m_enabled || (sensorhub_len <= 0))
+				break;
+
+			m_data = m_pending_data;
+			data = m_data;
+			_D("sensorhub event is received!");
+		} else {
+			_E("Unknown event (type=%d, code=%d)", sensorhub_input.type, sensorhub_input.code);
+		}
+	}
+
+	if (syn == false) {
+		_E("EV_SYN didn't come until %d inputs had come", read_input_cnt);
+		return false;
+	}
+
+	return true;
+}
+
+int sensorhub_controller::read_sensorhub_data(void)
+{
+	int ret = 0;
+
+	if (m_data_node < 0) {
+		_E("Invalid sensorhub fd(%d)", m_data_node);
+		return -ENODEV;
+	}
+
+read:
+	ret = read(m_data_node, m_pending_data.values, m_pending_data.value_count);
+
+	if (ret > 0) {
+		m_pending_data.value_count = ret;
+		print_sensorhub_data(__FUNCTION__, m_pending_data.values, m_pending_data.value_count);
+	} else if (ret < 0) {
+		if (errno == EINTR) {
+			_E("EINTR! retry read");
+			goto read;
+		}
+
+		_ERRNO(errno, _E, "Failed to read data");
+		return -errno;
+	}
+
+	return ret;
+}
+
+int sensorhub_controller::send_sensorhub_data(const char *data, int data_len)
+{
+	int ret;
+
+	if (data_len <= 0) {
+		_E("Invalid data_len(%d)", data_len);
+		return -EINVAL;
+	}
+
+	if (m_data_node < 0) {
+		_E("Invalid sensorhub fd(%d)", m_data_node);
+		return -ENODEV;
+	}
+
+	if (m_injection_mode)
+		return -EPERM;
+
+	print_sensorhub_data(__FUNCTION__, data, data_len);
+
+write:
+	ret = write(m_data_node, data, data_len);
+	if (ret < 0) {
+		if (errno == EINTR) {
+			_I("EINTR! retry to write");
+			goto write;
+		}
+		_ERRNO(errno, _E, "Failed to write data");
+	}
+
+	return ret < 0 ? -errno : ret;
+}
+
+char *sensorhub_controller::base64_encode(const unsigned char *data, size_t input_length)
+{
+	const char encoding_table[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H',
+				       'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
+				       'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
+				       'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
+				       'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n',
+				       'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
+				       'w', 'x', 'y', 'z', '0', '1', '2', '3',
+				       '4', '5', '6', '7', '8', '9', '+', '/'};
+	const int mod_table[] = {0, 2, 1};
+	int output_length = ((input_length - 1) / 3) * 4 + 5;
+
+	char *encoded_data = (char *)malloc(output_length);
+	if (encoded_data == NULL) return NULL;
+
+	for (size_t i = 0, j = 0; i < input_length;) {
+		uint32_t octet_a = i < input_length ? (unsigned char)data[i++] : 0;
+		uint32_t octet_b = i < input_length ? (unsigned char)data[i++] : 0;
+		uint32_t octet_c = i < input_length ? (unsigned char)data[i++] : 0;
+
+		uint32_t triple = (octet_a << 0x10) + (octet_b << 0x08) + octet_c;
+
+		encoded_data[j++] = encoding_table[(triple >> 3 * 6) & 0x3F];
+		encoded_data[j++] = encoding_table[(triple >> 2 * 6) & 0x3F];
+		encoded_data[j++] = encoding_table[(triple >> 1 * 6) & 0x3F];
+		encoded_data[j++] = encoding_table[(triple >> 0 * 6) & 0x3F];
+	}
+
+	for (int i = 0; i < mod_table[input_length % 3]; i++)
+		encoded_data[output_length - 2 - i] = '=';
+
+	encoded_data[output_length - 1] = '\0';
+
+	return encoded_data;
+}
+
+int sensorhub_controller::print_sensorhub_data(const char* name, const char *data, int length)
+{
+	const int LOG_SIZE_LIMIT = 100;
+
+	char buf[6];
+	char *log_str;
+
+	if ((length > LOG_SIZE_LIMIT) || (length <= 0)) {
+		_W("log size(%d) is exceptional!", length);
+		if (data[0] == 1 && data[1] == 1 && data[2] == 15) {
+			char *encoded_data = base64_encode((unsigned char *)data, length);
+			_W("%s", encoded_data);
+			free(encoded_data);
+		}
+		return -EINVAL;
+	}
+
+	int log_size = strlen(name) + 2 + sizeof(buf) * length + 1;
+
+	log_str = new(std::nothrow) char[log_size];
+	retvm_if(!log_str, -ENOMEM, "Failed to allocate memory");
+
+	memset(log_str, 0, log_size);
+
+	for (int i = 0; i < length; i++ ) {
+		if (i == 0) {
+			strncat(log_str, name, strlen(name)+1);
+			strncat(log_str, ": ", 2);
+		} else {
+			strncat(log_str, ", ", 2);
+		}
+
+		snprintf(buf, sizeof(buf), "%d", (signed char)data[i]);
+		strncat(log_str, buf, sizeof(buf));
+	}
+
+	_I("%s", log_str);
+	delete[] log_str;
+
+	return length;
+}
+
+
+bool sensorhub_controller::set_attribute(int32_t attribute, int32_t value)
+{
+	bool injection_mode = false;
+	std::string path = SSPSENSORHUB_DEVICE;
+	if (attribute != SENSOR_ATTR_SENSORHUB_INJECT_VALUE)
+		return false;
+
+	if (value == INJECTION_ENABLE) {
+		injection_mode = true;
+		path = INJECTION_NODE_PATH;
+	}
+
+	m_injection_mode = injection_mode;
+	m_data_node = open(path.c_str(), O_RDWR);
+	if (m_data_node < 0) {
+		_E("Open sensorhub device failed(%s)", path.c_str());
+		return false;
+	}
+
+	return true;
+}
diff --git a/src/sensorhub/sensorhub_controller.h b/src/sensorhub/sensorhub_controller.h
new file mode 100644
index 0000000..2ca54fd
--- /dev/null
+++ b/src/sensorhub/sensorhub_controller.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _SENSORHUB_CONTROLLER_H_
+#define _SENSORHUB_CONTROLLER_H_
+
+#include <sensor/sensor_hal.h>
+
+class sensorhub_controller {
+public:
+	sensorhub_controller();
+	virtual ~sensorhub_controller();
+
+	int open_input_node(const char* input_node);
+	int get_poll_fd(void);
+
+	bool enable(void);
+	bool disable(void);
+
+	bool read_fd(sensorhub_data_t &data);
+
+	int read_sensorhub_data(void);
+	int send_sensorhub_data(const char *data, int data_len);
+
+	bool set_attribute(int32_t attribute, int32_t value);
+
+private:
+	int m_enabled;
+	int m_poll_node;
+	int m_data_node;
+	bool m_injection_mode;
+
+	sensorhub_data_t m_pending_data;
+	sensorhub_data_t m_data;
+
+	int print_sensorhub_data(const char* name, const char *data, int length);
+	char *base64_encode(const unsigned char *data, size_t input_length);
+};
+
+#endif /* _SENSORHUB_CONTROLLER_H_ */
diff --git a/src/sensorhub/sensorhub_manager.cpp b/src/sensorhub/sensorhub_manager.cpp
new file mode 100644
index 0000000..faa8981
--- /dev/null
+++ b/src/sensorhub/sensorhub_manager.cpp
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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 <sensor_common.h>
+#include <util.h>
+#include "sensorhub_controller.h"
+#include "sensorhub_manager.h"
+#include "sensorhub_sensor.h"
+
+sensorhub_manager::sensorhub_manager()
+{
+}
+
+sensorhub_manager::~sensorhub_manager()
+{
+	for (auto &it : m_id_sensors)
+		delete it.second;
+
+	m_id_sensors.clear();
+	m_infos.clear();
+}
+
+sensorhub_manager& sensorhub_manager::get_instance() {
+	static sensorhub_manager instance;
+	return instance;
+}
+
+bool sensorhub_manager::add_sensor(sensor_info_t info, sensorhub_sensor *sensor)
+{
+	m_infos.push_back(info);
+	m_id_sensors[info.id] = sensor;
+
+	return true;
+}
+
+void sensorhub_manager::set_controller(sensorhub_controller *controller)
+{
+	for (auto const &it : m_id_sensors) {
+		sensorhub_sensor *sensor = it.second;
+		sensor->set_controller(controller);
+	}
+}
+
+sensorhub_sensor *sensorhub_manager::get_sensor(uint32_t id)
+{
+	if (m_id_sensors.find(id) == m_id_sensors.end())
+		return NULL;
+
+	return m_id_sensors[id];
+}
+
+bool sensorhub_manager::get_sensors(char libtype, std::vector<sensorhub_sensor *> &sensors)
+{
+	char type;
+	sensorhub_sensor *sensor;
+
+	for (auto const &it : m_id_sensors) {
+		sensor = it.second;
+		if (!sensor)
+			continue;
+
+		type = (char)(sensor->get_id() >> SENSOR_LIB_SHIFT);
+
+		if (type == libtype)
+			sensors.push_back(sensor);
+	}
+
+	if (sensors.size() == 0)
+		return false;
+
+	return true;
+}
+
+int sensorhub_manager::get_sensor_infos(const sensor_info_t **sensors)
+{
+	int size;
+
+	if (m_infos.empty()) {
+		*sensors = 0;
+		return 0;
+	}
+
+	size = m_infos.size();
+	*sensors = &m_infos[0];
+
+	return size;
+}
+
+void sensorhub_manager::restore_sensors()
+{
+	for (auto &it : m_id_sensors) {
+		it.second->restore();
+		it.second->restore_attributes();
+	}
+}
diff --git a/src/sensorhub/sensorhub_manager.h b/src/sensorhub/sensorhub_manager.h
new file mode 100644
index 0000000..a4bc6f7
--- /dev/null
+++ b/src/sensorhub/sensorhub_manager.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _SENSORHUB_MANAGER_H_
+#define _SENSORHUB_MANAGER_H_
+
+#include <map>
+#include <vector>
+#include <sensor_log.h>
+
+#include "sensorhub_controller.h"
+#include "sensorhub_sensor.h"
+
+
+#define REGISTER_SENSORHUB_LIB(info, sensor_class) \
+	static sensor_initializer<sensor_class> initializer_##sensor_class((info)); \
+
+class sensorhub_manager {
+public:
+	static sensorhub_manager& get_instance();
+	virtual ~sensorhub_manager();
+
+	sensorhub_sensor *get_sensor(uint32_t id);
+	bool get_sensors(char libtype, std::vector<sensorhub_sensor *> &sensors);
+	int get_sensor_infos(const sensor_info_t **sensors);
+
+	void set_controller(sensorhub_controller *controller);
+	bool add_sensor(sensor_info_t info, sensorhub_sensor *sensor);
+
+	void restore_sensors(void);
+
+private:
+	sensorhub_manager();
+
+	std::map<uint32_t, sensorhub_sensor *> m_id_sensors;
+	std::vector<sensor_info_t> m_infos;
+};
+
+template <typename T>
+class sensor_initializer {
+public:
+	sensor_initializer(sensor_info_t info)
+	{
+		T *sensor = new(std::nothrow) T();
+		if (!sensor) {
+			_E("Failed to allocate memory");
+			return;
+		}
+		sensorhub_manager::get_instance().add_sensor(info, sensor);
+	}
+	~sensor_initializer() {}
+};
+
+#endif /* _SENSORHUB_MANAGER_H_ */
diff --git a/src/sensorhub/sensorhub_sensor.cpp b/src/sensorhub/sensorhub_sensor.cpp
new file mode 100644
index 0000000..2f4b694
--- /dev/null
+++ b/src/sensorhub/sensorhub_sensor.cpp
@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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 <sensor_log.h>
+#include <time.h>
+#include "sensorhub_controller.h"
+#include "sensorhub_sensor.h"
+
+sensorhub_sensor::sensorhub_sensor(const sensor_info_t *sensor_info)
+: m_sensor_info(sensor_info)
+, m_enabled(false)
+, m_restore_mode(false)
+{
+}
+
+sensorhub_sensor::~sensorhub_sensor()
+{
+	_I("%s is destroyed!", m_sensor_info->name);
+}
+
+int32_t sensorhub_sensor::get_id(void)
+{
+	return m_sensor_info->id;
+}
+
+void sensorhub_sensor::set_controller(sensorhub_controller *controller)
+{
+	m_controller = controller;
+}
+
+bool sensorhub_sensor::enable(void)
+{
+	m_enabled.store(true);
+	return true;
+}
+
+bool sensorhub_sensor::disable(void)
+{
+	m_enabled.store(false);
+	return true;
+}
+
+bool sensorhub_sensor::is_enabled(void)
+{
+	return m_enabled.load();
+}
+
+bool sensorhub_sensor::is_restore_mode(void)
+{
+	return m_restore_mode.load();
+}
+
+bool sensorhub_sensor::set_interval(unsigned long val)
+{
+	return false;
+}
+
+bool sensorhub_sensor::set_batch_latency(unsigned long val)
+{
+	return false;
+}
+
+bool sensorhub_sensor::set_attribute_int(int32_t attribute, int32_t value)
+{
+	if (is_restore_mode())
+		return false;
+
+	attributes_int.push_back(std::make_pair(attribute, value));
+	return true;
+}
+
+bool sensorhub_sensor::set_attribute_str(int32_t attribute, char *value, int len)
+{
+	attr_info info;
+
+	if (is_restore_mode())
+		return false;
+
+	info.value = value;
+	info.len = len;
+
+	attributes_str.push_back(std::make_pair(attribute, &info));
+	return true;
+}
+
+bool sensorhub_sensor::flush(void)
+{
+	return false;
+}
+
+bool sensorhub_sensor::enable(bool enbl, const char lib_id)
+{
+	struct cmd {
+		uint8_t inst;
+		uint8_t type;
+		uint8_t reserved[2];
+	} cmd = {enbl ? SHUB_INST_LIB_ADD : SHUB_INST_LIB_REMOVE, (uint8_t)lib_id, {0, 0}};
+
+	if (!m_controller) {
+		_E("Controller is not set : %d", lib_id);
+		return false;
+	}
+
+	if (m_controller->send_sensorhub_data(reinterpret_cast<char *>(&cmd), sizeof(struct cmd))) {
+		m_enabled.store(enbl);
+		if (!enbl) {
+			attributes_int.clear();
+			attributes_str.clear();
+		}
+		return true;
+	}
+
+	return false;
+}
+
+void sensorhub_sensor::restore(void)
+{
+	if (!is_enabled())
+		return;
+
+	_I("Restart %s", m_sensor_info->name);
+	enable();
+}
+
+bool sensorhub_sensor::restore_attributes(void)
+{
+	if (!is_enabled())
+		return false;
+
+	_I("Restart attributes %s", m_sensor_info->name);
+
+	m_restore_mode.store(true);
+
+	for (auto it = attributes_int.begin(); it != attributes_int.end(); ++it) {
+		int attr = it->first;
+		int val = it->second;
+		if (!set_attribute_int(attr, val)) {
+			_E("Failed to set_attribute_int(%d, %d) for %s",
+			    attr, val, m_sensor_info->name);
+			return false;
+		}
+	}
+
+	for (auto it = attributes_str.begin(); it != attributes_str.end(); ++it) {
+		int attr = it->first;
+		char *val = it->second->value;
+		int len = it->second->len;
+		if (!set_attribute_str(attr, val, len)) {
+			_E("Failed to set_attribute_str(%d, %s, %d) for %s",
+			    attr, val, len, m_sensor_info->name);
+			return false;
+		}
+	}
+
+	m_restore_mode.store(false);
+	return true;
+}
diff --git a/src/sensorhub/sensorhub_sensor.h b/src/sensorhub/sensorhub_sensor.h
new file mode 100644
index 0000000..2451d5b
--- /dev/null
+++ b/src/sensorhub/sensorhub_sensor.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * 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.
+ *
+ */
+
+#ifndef _SENSORHUB_SENSOR_H_
+#define _SENSORHUB_SENSOR_H_
+
+#include <atomic>
+#include <vector>
+#include "sensorhub_controller.h"
+
+#define SHUB_INST_LIB_GETVALUE	(uint8_t)0x35
+#define SHUB_INST_LIB_ADD		(uint8_t)0x37
+#define SHUB_INST_LIB_REMOVE	(uint8_t)0x38
+
+#define SHUB_IDX_LIBTYPE		2
+#define SHUB_EXT_CURRENT_INFO	1
+
+#define SHUB_LIB_HRM_BATCH		46
+
+struct attr_info {
+	char *value;
+	int len;
+};
+
+typedef std::vector<std::pair<int, int>> sensor_attribute_int_vec;
+typedef std::vector<std::pair<int, attr_info *>> sensor_attribute_str_vec;
+
+class sensorhub_sensor {
+public:
+	sensorhub_sensor(const sensor_info_t *sensor_info);
+	virtual ~sensorhub_sensor();
+
+	int32_t get_id(void);
+
+	virtual bool enable(void);
+	virtual bool disable(void);
+	virtual int parse(const char *hub_data, int data_len) = 0;
+	virtual int get_data(sensor_data_t **data, int *length) = 0;
+
+	void set_controller(sensorhub_controller *controller);
+
+	virtual bool set_interval(unsigned long val);
+	virtual bool set_batch_latency(unsigned long val);
+	virtual bool set_attribute_int(int32_t attribute, int32_t value);
+	virtual bool set_attribute_str(int32_t key, char *value, int len);
+
+	virtual bool flush(void);
+	virtual void restore(void);
+	virtual bool restore_attributes(void);
+
+	sensor_attribute_int_vec attributes_int;
+	sensor_attribute_str_vec attributes_str;
+
+protected:
+	const sensor_info_t *m_sensor_info;
+	sensorhub_controller *m_controller;
+
+	bool enable(bool enbl, const char lib_id);
+	bool is_enabled(void);
+	bool is_restore_mode(void);
+
+private:
+	std::atomic_bool m_enabled;
+	std::atomic_bool m_restore_mode;
+};
+
+#endif /* _SENSORHUB_SENSOR_H_ */