src/geomag/geomag_device.cpp

   1 /*
   2  * Copyright (c) 2016 Samsung Electronics Co., Ltd.
   3  *
   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
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  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.
  15  *
  16  */
  17
  18 #include <fcntl.h>
  19 #include <unistd.h>
  20 #include <sys/types.h>
  21 #include <sys/stat.h>
  22
  23 #include <linux/input.h>
  24 #include <sys/ioctl.h>
  25 #include <poll.h>
  26
  27 #include <macro.h>
  28 #include <util.h>
  29 #include <sensor_common.h>
  30 #include <sensor_log.h>
  31
  32 #include "geomag_device.h"
  33
  34 #define MODEL_NAME "YAS532"
  35 #define VENDOR "YAMAHA"
  36 #define RESOLUTION 16
  37 #define RAW_DATA_UNIT 0.1
  38 #define MIN_INTERVAL 10
  39 #define MAX_BATCH_COUNT 0
  40
  41 #define SENSOR_NAME "SENSOR_GEOMAGNETIC"
  42 #define SENSOR_TYPE_MAGNETIC    "MAGNETIC"
  43
  44 #define INPUT_NAME      "geomagnetic_sensor"
  45 #define GEOMAG_SENSORHUB_POLL_NODE_NAME "mag_poll_delay"
  46
  47 static sensor_info_t sensor_info = {
  48         id: 0x1,
  49         name: SENSOR_NAME,
  50         type: SENSOR_DEVICE_GEOMAGNETIC,
  51         event_type: (SENSOR_DEVICE_GEOMAGNETIC << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
  52         model_name: MODEL_NAME,
  53         vendor: VENDOR,
  54         min_range: -1200,
  55         max_range: 1200,
  56         resolution: RAW_DATA_UNIT,
  57         min_interval: 10,
  58         max_batch_count: 0,
  59         wakeup_supported: false
  60 };
  61
  62 geomag_device::geomag_device()
  63 : m_node_handle(-1)
  64 , m_x(-1)
  65 , m_y(-1)
  66 , m_z(-1)
  67 , m_hdst(0)
  68 , m_polling_interval(1000)
  69 , m_fired_time(0)
  70 , m_sensorhub_controlled(false)
  71 {
  72         const std::string sensorhub_interval_node_name = GEOMAG_SENSORHUB_POLL_NODE_NAME;
  73
  74         node_info_query query;
  75         node_info info;
  76
  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;
  82
  83         if (!util::get_node_info(query, info)) {
  84                 _E("Failed to get node info");
  85                 throw ENXIO;
  86         }
  87
  88         util::show_node_info(info);
  89
  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;
  94
  95         m_node_handle = open(m_data_node.c_str(), O_RDONLY);
  96
  97         if (m_node_handle < 0) {
  98                 _ERRNO(errno, _E, "Failed to open magnetic sensor");
  99                 throw ENXIO;
 100         }
 101
 102         if (m_method == INPUT_EVENT_METHOD) {
 103                 if (!util::set_monotonic_clock(m_node_handle))
 104                         throw ENXIO;
 105
 106                 update_value = [=]() {
 107                         return this->update_value_input_event();
 108                 };
 109         } else {
 110                 if (!info.buffer_length_node_path.empty())
 111                         util::set_node_value(info.buffer_length_node_path, 480);
 112
 113                 if (!info.buffer_enable_node_path.empty())
 114                         util::set_node_value(info.buffer_enable_node_path, 1);
 115
 116                 update_value = [=]() {
 117                         return this->update_value_iio();
 118                 };
 119         }
 120
 121         _I("geomag_device is created!");
 122 }
 123
 124 geomag_device::~geomag_device()
 125 {
 126         close(m_node_handle);
 127         m_node_handle = -1;
 128
 129         _I("geomag_sensor is destroyed!");
 130 }
 131
 132 int geomag_device::get_poll_fd(void)
 133 {
 134         return m_node_handle;
 135 }
 136
 137 int geomag_device::get_sensors(const sensor_info_t **sensors)
 138 {
 139         *sensors = &sensor_info;
 140
 141         return 1;
 142 }
 143
 144 bool geomag_device::enable(uint32_t id)
 145 {
 146         util::set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_GEOMAGNETIC_ENABLE_BIT);
 147         set_interval(id, m_polling_interval);
 148
 149         m_fired_time = 0;
 150         _I("Enable geomagnetic sensor");
 151         return true;
 152 }
 153
 154 bool geomag_device::disable(uint32_t id)
 155 {
 156         util::set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_GEOMAGNETIC_ENABLE_BIT);
 157
 158         _I("Disable geomagnetic sensor");
 159         return true;
 160 }
 161
 162 bool geomag_device::set_interval(uint32_t id, unsigned long val)
 163 {
 164         unsigned long long polling_interval_ns;
 165
 166         polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
 167
 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());
 170                 return false;
 171         }
 172
 173         _I("Interval is changed from %dms to %dms", m_polling_interval, val);
 174         m_polling_interval = val;
 175         return true;
 176 }
 177
 178 bool geomag_device::update_value_input_event(void)
 179 {
 180         int geo_raw[4] = {0, };
 181         bool x, y, z, hdst;
 182         int read_input_cnt = 0;
 183         const int INPUT_MAX_BEFORE_SYN = 10;
 184         unsigned long long fired_time = 0;
 185         bool syn = false;
 186
 187         x = y = z = hdst = false;
 188
 189         struct input_event geo_input;
 190         _D("geo event detection!");
 191
 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);
 196                         return false;
 197                 }
 198
 199                 ++read_input_cnt;
 200
 201                 if (geo_input.type == EV_REL) {
 202                         switch (geo_input.code) {
 203                                 case REL_RX:
 204                                         geo_raw[0] = (int)geo_input.value;
 205                                         x = true;
 206                                         break;
 207                                 case REL_RY:
 208                                         geo_raw[1] = (int)geo_input.value;
 209                                         y = true;
 210                                         break;
 211                                 case REL_RZ:
 212                                         geo_raw[2] = (int)geo_input.value;
 213                                         z = true;
 214                                         break;
 215                                 case REL_HWHEEL:
 216                                         geo_raw[3] = (int)geo_input.value;
 217                                         hdst = true;
 218                                         break;
 219                                 default:
 220                                         _E("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code);
 221                                         return false;
 222                                         break;
 223                         }
 224                 } else if (geo_input.type == EV_SYN) {
 225                         syn = true;
 226                         fired_time = util::get_timestamp(&geo_input.time);
 227                 } else {
 228                         _E("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code);
 229                         return false;
 230                 }
 231         }
 232
 233         if (syn == false) {
 234                 _E("EV_SYN didn't come until %d inputs had come", read_input_cnt);
 235                 return false;
 236         }
 237
 238         if (x)
 239                 m_x = geo_raw[0];
 240         if (y)
 241                 m_y = geo_raw[1];
 242         if (z)
 243                 m_z = geo_raw[2];
 244         if (hdst)
 245                 m_hdst = geo_raw[3] - 1; /* accuracy bias: -1 */
 246
 247         m_fired_time = fired_time;
 248
 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);
 250
 251         return true;
 252 }
 253
 254
 255 bool geomag_device::update_value_iio(void)
 256 {
 257         struct {
 258                 int16_t x;
 259                 int16_t y;
 260                 int16_t z;
 261                 int8_t hdst;
 262                 int64_t timestamp;
 263         } __attribute__((packed)) data;
 264
 265         struct pollfd pfd;
 266
 267         pfd.fd = m_node_handle;
 268         pfd.events = POLLIN | POLLERR;
 269         pfd.revents = 0;
 270
 271         int ret = poll(&pfd, 1, -1);
 272
 273         if (ret == -1) {
 274                 _ERRNO(errno, _E, "Failed to poll from m_node_handle:%d", m_node_handle);
 275                 return false;
 276         } else if (!ret) {
 277                 _E("poll timeout m_node_handle:%d", m_node_handle);
 278                 return false;
 279         }
 280
 281         if (pfd.revents & POLLERR) {
 282                 _E("poll exception occurred! m_node_handle:%d", m_node_handle);
 283                 return false;
 284         }
 285
 286         if (!(pfd.revents & POLLIN)) {
 287                 _E("poll nothing to read! m_node_handle:%d, pfd.revents = %d", m_node_handle, pfd.revents);
 288                 return false;
 289         }
 290
 291         int len = read(m_node_handle, &data, sizeof(data));
 292
 293         if (len != sizeof(data)) {
 294                 _E("Failed to read data, m_node_handle:%d read_len:%d", m_node_handle, len);
 295                 return false;
 296         }
 297
 298         m_x = data.x;
 299         m_y = data.y;
 300         m_z = data.z;
 301         m_hdst = data.hdst - 1;
 302         m_fired_time = NSEC_TO_MSEC(data.timestamp);
 303
 304         _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 305
 306         return true;
 307 }
 308
 309 int geomag_device::read_fd(uint32_t **ids)
 310 {
 311         if (!update_value()) {
 312                 _D("Failed to update value");
 313                 return false;
 314         }
 315
 316         event_ids.clear();
 317         event_ids.push_back(sensor_info.id);
 318
 319         *ids = &event_ids[0];
 320
 321         return event_ids.size();
 322 }
 323
 324 int geomag_device::get_data(uint32_t id, sensor_data_t **data, int *length)
 325 {
 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");
 329
 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;
 337
 338         raw_to_base(sensor_data);
 339
 340         *data = sensor_data;
 341         *length = sizeof(sensor_data_t);
 342
 343         return 0;
 344 }
 345
 346 void geomag_device::raw_to_base(sensor_data_t *data)
 347 {
 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;
 351 }