Add hrm_led_green_batch sensor

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

5 files changed, 240 insertions, 2 deletions

diff --git a/src/sensorhub/hrm_led_green_batch.cpp b/src/sensorhub/hrm_led_green_batch.cpp
new file mode 100644
index 0000000..41d63aa
--- /dev/null
+++ b/src/sensorhub/hrm_led_green_batch.cpp
@@ -0,0 +1,192 @@
+/*
+ * Copyright (c) 2018 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 <util.h>
+
+#include "sensorhub_manager.h"
+
+#define HEADER_SIZE			8
+#define TUPLE_MAX_COUNT		300
+#define DEFAULT_POLLING		40
+
+#define HRM_LED_GREEN_BATCH_NAME	"SENSOR_HRM_LED_GREEN_BATCH"
+#define PROP_TYPE_BATCH_PERIOD	1
+
+static const sensor_info_t sensor_info = {
+	id: SENSOR_DEVICE_ID(SHUB_LIB_HRM_LED_GREEN_BATCH, 0x1),
+	name: HRM_LED_GREEN_BATCH_NAME,
+	type: SENSOR_DEVICE_HRM_LED_GREEN_BATCH,
+	event_type: SENSOR_EVENT_TYPE(SENSOR_DEVICE_HRM_LED_GREEN_BATCH),
+	model_name: "HRM Led Green 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_led_green_batch_sensor : public sensorhub_sensor {
+public:
+	sh_hrm_led_green_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);
+
+private:
+	sensor_data_t *m_data;
+	int m_batch_count;
+	int m_polling_interval;
+
+	bool flush(void);
+};
+
+sh_hrm_led_green_batch_sensor::sh_hrm_led_green_batch_sensor()
+: sensorhub_sensor(&sensor_info)
+, m_data(NULL)
+, m_batch_count(0)
+, m_polling_interval(DEFAULT_POLLING)
+{
+}
+
+bool sh_hrm_led_green_batch_sensor::enable(void)
+{
+	struct cmd {
+		uint8_t inst;
+		uint8_t lib_type;
+		uint16_t polling_interval;
+	} add_cmd = { (uint8_t)SHUB_INST_LIB_ADD,
+				  (uint8_t)SHUB_LIB_HRM_LED_GREEN_BATCH,
+				  (uint16_t)m_polling_interval };
+
+	char *cmd = reinterpret_cast<char *>(&add_cmd);
+
+	if (!m_controller) {
+		_E("Controller is not set : %s", HRM_LED_GREEN_BATCH_NAME);
+		return false;
+	}
+
+	if (m_controller->send_sensorhub_data(cmd, sizeof(struct cmd)) < 0)
+		return false;
+
+	return sensorhub_sensor::enable();
+}
+
+bool sh_hrm_led_green_batch_sensor::disable(void)
+{
+	m_polling_interval = DEFAULT_POLLING;
+	return sensorhub_sensor::enable(false, SHUB_LIB_HRM_LED_GREEN_BATCH);
+}
+
+int sh_hrm_led_green_batch_sensor::parse(const char *data, int data_len)
+{
+	/*
+	  GPS-batch packet: 1, 1, 28, reserved(1), ts(4), tuple(12) x TUPLE_MAX_COUNT
+	  A tuple has LED Green(4), Acc X(2), Acc Y(2), Acc Z(2), Index(2) ......
+	 */
+
+	struct tuple {
+		int32_t green;
+		int16_t acc_x;
+		int16_t acc_y;
+		int16_t acc_z;
+		uint16_t index;
+	} __attribute__((packed));
+
+	struct hrm_led_green_batch {
+		uint8_t inst;
+		uint8_t type;
+		uint8_t lib_type;
+		uint8_t reserved; /* Alignment */
+		uint32_t ts;
+		struct tuple tuples[TUPLE_MAX_COUNT];
+	} __attribute__((packed));
+
+	struct hrm_led_green_batch *pb = (struct hrm_led_green_batch *)data;
+	sensor_data_t *d;
+	int count = 0;
+	unsigned long long timestamp;
+
+	for (int i = 0; i < TUPLE_MAX_COUNT; ++i) {
+		if (!pb->tuples[i].index && !pb->tuples[i].acc_x && !pb->tuples[i].green)
+			break;
+		count++;
+	}
+
+	d = (sensor_data_t *)malloc(sizeof(sensor_data_t) * count);
+	if (!d)
+		return log_oom();
+
+	timestamp = pb->ts - (40 * (count - 1));
+
+	for (int i = 0; i < count; ++i) {
+		d[i].value_count = 5;
+		d[i].values[0] = pb->tuples[i].green;
+		d[i].values[1] = pb->tuples[i].acc_x;
+		d[i].values[2] = pb->tuples[i].acc_y;
+		d[i].values[3] = pb->tuples[i].acc_z;
+		d[i].values[4] = pb->tuples[i].index;
+		d[i].accuracy = SENSOR_ACCURACY_GOOD;
+		d[i].timestamp = MSEC_TO_USEC(timestamp + (i * 40));
+	}
+
+	m_data = (sensor_data_t *)d;
+	m_batch_count = TUPLE_MAX_COUNT;
+
+	return HEADER_SIZE + (sizeof(struct tuple) * TUPLE_MAX_COUNT);
+}
+
+int sh_hrm_led_green_batch_sensor::get_data(sensor_data_t **data, int *length)
+{
+	*data = m_data;
+	*length = (sizeof(sensor_data_t) * m_batch_count);
+	m_data = NULL;
+
+	return 0;
+}
+
+bool sh_hrm_led_green_batch_sensor::set_attribute_int(int32_t attribute, int32_t value)
+{
+	return false;
+}
+
+bool sh_hrm_led_green_batch_sensor::flush(void)
+{
+	char cmd[] = {SHUB_INST_LIB_GETVALUE, SHUB_LIB_HRM_LED_GREEN_BATCH, SHUB_EXT_CURRENT_INFO};
+
+	if (!m_controller) {
+		_E("Controller is not set : %s", HRM_LED_GREEN_BATCH_NAME);
+		return false;
+	}
+
+	if (m_controller->send_sensorhub_data(cmd, sizeof(cmd)) < 0)
+		return false;
+
+	return true;
+}
+
+
+REGISTER_SENSORHUB_LIB(sensor_info, sh_hrm_led_green_batch_sensor)
diff --git a/src/sensorhub/sensorhub_controller.cpp b/src/sensorhub/sensorhub_controller.cpp
index d01917f..1b8e4bc 100644
--- a/src/sensorhub/sensorhub_controller.cpp
+++ b/src/sensorhub/sensorhub_controller.cpp
@@ -29,9 +29,13 @@
 #include "sensorhub_controller.h"
 
 #define EVENT_TYPE_SENSORHUB_DATA		 REL_RX
+#define EVENT_TYPE_LARGE_SENSORHUB_DATA   REL_RY
+#define EVENT_TYPE_SENSORHUB_NOTI		 REL_RZ
 
 #define SSP_INPUT_NODE_NAME			"ssp_context"
 #define SSPSENSORHUB_DEVICE		"/dev/ssp_sensorhub"
+#define SENSORHUB_IOCTL_MAGIC		'S'
+#define IOCTL_READ_LARGE_SENSORHUB_DATA	_IOR(SENSORHUB_IOCTL_MAGIC, 3, unsigned int)
 
 #define INJECTION_NODE_PATH "/run/sensord/sensorhub"
 #define INJECTION_ENABLE 1
@@ -202,6 +206,16 @@ bool sensorhub_controller::read_fd(sensorhub_data_t &data)
 				else if (sensorhub_len < 0)
 					_E("read_sensorhub_data() err(%d)", sensorhub_len);
 
+			} else if (sensorhub_input.code == EVENT_TYPE_LARGE_SENSORHUB_DATA) {
+				_D("EVENT_TYPE_LARGE_SENSORHUB_DATA, value_count=%g", value);
+				m_pending_data.value_count = value;
+				sensorhub_len = read_large_sensorhub_data();
+
+				if (sensorhub_len == 0)
+					_E("No large library data");
+				else if (sensorhub_len < 0)
+					_E("read_large_sensorhub_data() err(%d)", sensorhub_len);
+
 			}
 		} else if (sensorhub_input.type == EV_SYN) {
 			syn = true;
@@ -253,6 +267,34 @@ read:
 	return ret;
 }
 
+int sensorhub_controller::read_large_sensorhub_data(void)
+{
+	int ret = 0;
+
+	if (m_data_node < 0) {
+		_E("Invalid sensorhub fd(%d)", m_data_node);
+		return -ENODEV;
+	}
+
+ioctl:
+	ret = ioctl(m_data_node, IOCTL_READ_LARGE_SENSORHUB_DATA, m_pending_data.values);
+
+	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) {
+			_I("EINTR! retry ioctl");
+			goto ioctl;
+		}
+
+		_ERRNO(errno, _E, "Failed to ioctl");
+		return -errno;
+	}
+
+	return ret;
+}
+
 int sensorhub_controller::send_sensorhub_data(const char *data, int data_len)
 {
 	int ret;
diff --git a/src/sensorhub/sensorhub_controller.h b/src/sensorhub/sensorhub_controller.h
index 2ca54fd..40bf786 100644
--- a/src/sensorhub/sensorhub_controller.h
+++ b/src/sensorhub/sensorhub_controller.h
@@ -34,6 +34,7 @@ public:
 	bool read_fd(sensorhub_data_t &data);
 
 	int read_sensorhub_data(void);
+	int read_large_sensorhub_data(void);
 	int send_sensorhub_data(const char *data, int data_len);
 
 	bool set_attribute(int32_t attribute, int32_t value);
diff --git a/src/sensorhub/sensorhub_sensor.h b/src/sensorhub/sensorhub_sensor.h
index 2451d5b..edc1a0d 100644
--- a/src/sensorhub/sensorhub_sensor.h
+++ b/src/sensorhub/sensorhub_sensor.h
@@ -23,12 +23,13 @@
 #include "sensorhub_controller.h"
 
 #define SHUB_INST_LIB_GETVALUE	(uint8_t)0x35
-#define SHUB_INST_LIB_ADD		(uint8_t)0x37
+#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_EXT_CURRENT_INFO		1
 
+#define SHUB_LIB_HRM_LED_GREEN_BATCH	28
 #define SHUB_LIB_HRM_BATCH		46
 
 struct attr_info {
diff --git a/src/util.h b/src/util.h
index 2358119..9a497a5 100644
--- a/src/util.h
+++ b/src/util.h
@@ -21,6 +21,8 @@
 #include <sys/time.h>
 #include <string>
 
+#define MSEC_TO_USEC(X)	((uint64_t)(X) * 1000ull)
+
 typedef struct {
 	int method;
 	std::string data_node_path;