2 * Copyright (c) 2016 Samsung Electronics Co., Ltd.
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
8 * http://www.apache.org/licenses/LICENSE-2.0
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
20 #include <sys/types.h>
23 #include <linux/input.h>
24 #include <sys/ioctl.h>
29 #include <sensor_common.h>
30 #include <sensor_log.h>
32 #include "geomag_device.h"
34 #define MODEL_NAME "YAS532"
35 #define VENDOR "YAMAHA"
37 #define RAW_DATA_UNIT 0.1
38 #define MIN_INTERVAL 10
39 #define MAX_BATCH_COUNT 0
41 #define SENSOR_NAME "SENSOR_GEOMAGNETIC"
42 #define SENSOR_TYPE_MAGNETIC "MAGNETIC"
44 #define INPUT_NAME "geomagnetic_sensor"
45 #define GEOMAG_SENSORHUB_POLL_NODE_NAME "mag_poll_delay"
47 static sensor_info_t sensor_info = {
50 type: SENSOR_DEVICE_GEOMAGNETIC,
51 event_type: (SENSOR_DEVICE_GEOMAGNETIC << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
52 model_name: MODEL_NAME,
56 resolution: RAW_DATA_UNIT,
59 wakeup_supported: false
62 geomag_device::geomag_device()
68 , m_polling_interval(1000)
70 , m_sensorhub_controlled(false)
72 const std::string sensorhub_interval_node_name = GEOMAG_SENSORHUB_POLL_NODE_NAME;
74 node_info_query query;
77 query.sensorhub_controlled = m_sensorhub_controlled = util::is_sensorhub_controlled(sensorhub_interval_node_name);
78 query.sensor_type = SENSOR_TYPE_MAGNETIC;
79 query.key = INPUT_NAME;
80 query.iio_enable_node_name = "geomagnetic_enable";
81 query.sensorhub_interval_node_name = sensorhub_interval_node_name;
83 if (!util::get_node_info(query, info)) {
84 _E("Failed to get node info");
88 util::show_node_info(info);
90 m_method = info.method;
91 m_data_node = info.data_node_path;
92 m_enable_node = info.enable_node_path;
93 m_interval_node = info.interval_node_path;
95 m_node_handle = open(m_data_node.c_str(), O_RDONLY);
97 if (m_node_handle < 0) {
98 _ERRNO(errno, _E, "Failed to open magnetic sensor");
102 if (m_method == INPUT_EVENT_METHOD) {
103 if (!util::set_monotonic_clock(m_node_handle))
106 update_value = [=]() {
107 return this->update_value_input_event();
110 if (!info.buffer_length_node_path.empty())
111 util::set_node_value(info.buffer_length_node_path, 480);
113 if (!info.buffer_enable_node_path.empty())
114 util::set_node_value(info.buffer_enable_node_path, 1);
116 update_value = [=]() {
117 return this->update_value_iio();
121 _I("geomag_device is created!");
124 geomag_device::~geomag_device()
126 close(m_node_handle);
129 _I("geomag_sensor is destroyed!");
132 int geomag_device::get_poll_fd(void)
134 return m_node_handle;
137 int geomag_device::get_sensors(const sensor_info_t **sensors)
139 *sensors = &sensor_info;
144 bool geomag_device::enable(uint32_t id)
146 util::set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_GEOMAGNETIC_ENABLE_BIT);
147 set_interval(id, m_polling_interval);
150 _I("Enable geomagnetic sensor");
154 bool geomag_device::disable(uint32_t id)
156 util::set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_GEOMAGNETIC_ENABLE_BIT);
158 _I("Disable geomagnetic sensor");
162 bool geomag_device::set_interval(uint32_t id, unsigned long val)
164 unsigned long long polling_interval_ns;
166 polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
168 if (!util::set_node_value(m_interval_node, polling_interval_ns)) {
169 ERR("Failed to set polling resource: %s\n", m_interval_node.c_str());
173 _I("Interval is changed from %lu ms to %lu ms", m_polling_interval, val);
174 m_polling_interval = val;
178 bool geomag_device::update_value_input_event(void)
180 int geo_raw[4] = {0, };
182 int read_input_cnt = 0;
183 const int INPUT_MAX_BEFORE_SYN = 10;
184 unsigned long long fired_time = 0;
187 x = y = z = hdst = false;
189 struct input_event geo_input;
190 _D("geo event detection!");
192 while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
193 int len = read(m_node_handle, &geo_input, sizeof(geo_input));
194 if (len != sizeof(geo_input)) {
195 _E("geo_file read fail, read_len = %d\n", len);
201 if (geo_input.type == EV_REL) {
202 switch (geo_input.code) {
204 geo_raw[0] = (int)geo_input.value;
208 geo_raw[1] = (int)geo_input.value;
212 geo_raw[2] = (int)geo_input.value;
216 geo_raw[3] = (int)geo_input.value;
220 _E("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code);
224 } else if (geo_input.type == EV_SYN) {
226 fired_time = util::get_timestamp(&geo_input.time);
228 _E("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code);
234 _E("EV_SYN didn't come until %d inputs had come", read_input_cnt);
245 m_hdst = geo_raw[3] - 1; /* accuracy bias: -1 */
247 m_fired_time = fired_time;
249 _D("m_x = %d, m_y = %d, m_z = %d, m_hdst = %d, time = %lluus", m_x, m_y, m_z, m_hdst, m_fired_time);
255 bool geomag_device::update_value_iio(void)
263 } __attribute__((packed)) data;
267 pfd.fd = m_node_handle;
268 pfd.events = POLLIN | POLLERR;
271 int ret = poll(&pfd, 1, -1);
274 _ERRNO(errno, _E, "Failed to poll from m_node_handle:%d", m_node_handle);
277 _E("poll timeout m_node_handle:%d", m_node_handle);
281 if (pfd.revents & POLLERR) {
282 _E("poll exception occurred! m_node_handle:%d", m_node_handle);
286 if (!(pfd.revents & POLLIN)) {
287 _E("poll nothing to read! m_node_handle:%d, pfd.revents = %d", m_node_handle, pfd.revents);
291 int len = read(m_node_handle, &data, sizeof(data));
293 if (len != sizeof(data)) {
294 _E("Failed to read data, m_node_handle:%d read_len:%d", m_node_handle, len);
301 m_hdst = data.hdst - 1;
302 m_fired_time = NSEC_TO_MSEC(data.timestamp);
304 _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
309 int geomag_device::read_fd(uint32_t **ids)
311 if (!update_value()) {
312 _D("Failed to update value");
317 event_ids.push_back(sensor_info.id);
319 *ids = &event_ids[0];
321 return event_ids.size();
324 int geomag_device::get_data(uint32_t id, sensor_data_t **data, int *length)
326 sensor_data_t *sensor_data;
327 sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
328 retvm_if(!sensor_data, -ENOMEM, "Memory allocation failed");
330 sensor_data->accuracy = (m_hdst == 1)? 0 : m_hdst;
331 sensor_data->timestamp = m_fired_time;
332 sensor_data->value_count = 4;
333 sensor_data->values[0] = m_x;
334 sensor_data->values[1] = m_y;
335 sensor_data->values[2] = m_z;
336 sensor_data->values[3] = (m_hdst == 1)? 0 : m_hdst;
338 raw_to_base(sensor_data);
341 *length = sizeof(sensor_data_t);
346 void geomag_device::raw_to_base(sensor_data_t *data)
348 data->values[0] = data->values[0] * RAW_DATA_UNIT;
349 data->values[1] = data->values[1] * RAW_DATA_UNIT;
350 data->values[2] = data->values[2] * RAW_DATA_UNIT;