src/accel/accel_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 <util.h>
  28 #include <sensor_common.h>
  29 #include <sensor_log.h>
  30
  31 #include "accel_device.h"
  32
  33 #define GRAVITY 9.80665
  34 #define G_TO_MG 1000
  35 #define RAW_DATA_TO_G_UNIT(X) (((float)(X))/((float)G_TO_MG))
  36 #define RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(X) (GRAVITY * (RAW_DATA_TO_G_UNIT(X)))
  37
  38 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  39 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  40
  41 #define MODEL_NAME "K2HH"
  42 #define VENDOR "ST Microelectronics"
  43 #define RESOLUTION 16
  44 #define RAW_DATA_UNIT 0.122
  45 #define MIN_INTERVAL 1
  46 #define MAX_BATCH_COUNT 0
  47
  48 #define SENSOR_NAME "SENSOR_ACCELEROMETER"
  49 #define SENSOR_TYPE_ACCEL               "ACCEL"
  50
  51 #define INPUT_NAME      "accelerometer_sensor"
  52 #define ACCEL_SENSORHUB_POLL_NODE_NAME "accel_poll_delay"
  53
  54 static sensor_info_t sensor_info = {
  55         id: 0x1,
  56         name: SENSOR_NAME,
  57         type: SENSOR_DEVICE_ACCELEROMETER,
  58         event_type: (SENSOR_DEVICE_ACCELEROMETER << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
  59         model_name: MODEL_NAME,
  60         vendor: VENDOR,
  61         min_range: MIN_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  62         max_range: MAX_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  63         resolution: RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  64         min_interval: MIN_INTERVAL,
  65         max_batch_count: MAX_BATCH_COUNT,
  66         wakeup_supported: false
  67 };
  68
  69 accel_device::accel_device()
  70 : m_node_handle(-1)
  71 , m_x(-1)
  72 , m_y(-1)
  73 , m_z(-1)
  74 , m_polling_interval(1000)
  75 , m_fired_time(0)
  76 , m_sensorhub_controlled(false)
  77 {
  78         const std::string sensorhub_interval_node_name = ACCEL_SENSORHUB_POLL_NODE_NAME;
  79
  80         node_info_query query;
  81         node_info info;
  82
  83         query.sensorhub_controlled = m_sensorhub_controlled = util::is_sensorhub_controlled(sensorhub_interval_node_name);
  84         query.sensor_type = SENSOR_TYPE_ACCEL;
  85         query.key = INPUT_NAME;
  86         query.iio_enable_node_name = "accel_enable";
  87         query.sensorhub_interval_node_name = sensorhub_interval_node_name;
  88
  89         if (!util::get_node_info(query, info)) {
  90                 _E("Failed to get node info");
  91                 throw ENXIO;
  92         }
  93
  94         util::show_node_info(info);
  95
  96         m_method = info.method;
  97         m_data_node = info.data_node_path;
  98         m_enable_node = info.enable_node_path;
  99         m_interval_node = info.interval_node_path;
 100
 101         m_node_handle = open(m_data_node.c_str(), O_RDONLY);
 102
 103         if (m_node_handle < 0) {
 104                 _ERRNO(errno, _E, "accel handle open fail for accel processor");
 105                 throw ENXIO;
 106         }
 107
 108         if (m_method == INPUT_EVENT_METHOD) {
 109                 if (!util::set_monotonic_clock(m_node_handle))
 110                         throw ENXIO;
 111
 112                 update_value = [=]() {
 113                         return this->update_value_input_event();
 114                 };
 115         } else {
 116                 if (!info.buffer_length_node_path.empty())
 117                         util::set_node_value(info.buffer_length_node_path, 480);
 118
 119                 if (!info.buffer_enable_node_path.empty())
 120                         util::set_node_value(info.buffer_enable_node_path, 1);
 121
 122                 update_value = [=]() {
 123                         return this->update_value_iio();
 124                 };
 125         }
 126
 127         _I("accel_sensor is created!");
 128 }
 129
 130 accel_device::~accel_device()
 131 {
 132         close(m_node_handle);
 133         m_node_handle = -1;
 134
 135         _I("accel_sensor is destroyed!");
 136 }
 137
 138 int accel_device::get_poll_fd()
 139 {
 140         return m_node_handle;
 141 }
 142
 143 int accel_device::get_sensors(const sensor_info_t **sensors)
 144 {
 145         *sensors = &sensor_info;
 146
 147         return 1;
 148 }
 149
 150 bool accel_device::enable(uint32_t id)
 151 {
 152         util::set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 153         set_interval(id, m_polling_interval);
 154
 155         m_fired_time = 0;
 156         _I("Enable accelerometer sensor");
 157         return true;
 158 }
 159
 160 bool accel_device::disable(uint32_t id)
 161 {
 162         util::set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 163
 164         _I("Disable accelerometer sensor");
 165         return true;
 166 }
 167
 168 bool accel_device::set_interval(uint32_t id, unsigned long val)
 169 {
 170         unsigned long long polling_interval_ns;
 171
 172         polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
 173
 174         if (!util::set_node_value(m_interval_node, polling_interval_ns)) {
 175                 _E("Failed to set polling resource: %s", m_interval_node.c_str());
 176                 return false;
 177         }
 178
 179         _I("Interval is changed from %dms to %dms", m_polling_interval, val);
 180         m_polling_interval = val;
 181         return true;
 182 }
 183
 184 bool accel_device::update_value_input_event(void)
 185 {
 186         int accel_raw[3] = {0,};
 187         bool x,y,z;
 188         int read_input_cnt = 0;
 189         const int INPUT_MAX_BEFORE_SYN = 10;
 190         unsigned long long fired_time = 0;
 191         bool syn = false;
 192
 193         x = y = z = false;
 194
 195         struct input_event accel_input;
 196         _D("accel event detection!");
 197
 198         while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
 199                 int len = read(m_node_handle, &accel_input, sizeof(accel_input));
 200                 if (len != sizeof(accel_input)) {
 201                         _E("accel_file read fail, read_len = %d",len);
 202                         return false;
 203                 }
 204
 205                 ++read_input_cnt;
 206
 207                 if (accel_input.type == EV_REL) {
 208                         switch (accel_input.code) {
 209                         case REL_X:
 210                                 accel_raw[0] = (int)accel_input.value;
 211                                 x = true;
 212                                 break;
 213                         case REL_Y:
 214                                 accel_raw[1] = (int)accel_input.value;
 215                                 y = true;
 216                                 break;
 217                         case REL_Z:
 218                                 accel_raw[2] = (int)accel_input.value;
 219                                 z = true;
 220                                 break;
 221                         default:
 222                                 _E("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 223                                 return false;
 224                                 break;
 225                         }
 226                 } else if (accel_input.type == EV_SYN) {
 227                         syn = true;
 228                         fired_time = util::get_timestamp(&accel_input.time);
 229                 } else {
 230                         _E("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 231                         return false;
 232                 }
 233         }
 234
 235         if (syn == false) {
 236                 _E("EV_SYN didn't come until %d inputs had come", read_input_cnt);
 237                 return false;
 238         }
 239
 240         if (x)
 241                 m_x =  accel_raw[0];
 242         if (y)
 243                 m_y =  accel_raw[1];
 244         if (z)
 245                 m_z =  accel_raw[2];
 246
 247         m_fired_time = fired_time;
 248
 249         _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 250
 251         return true;
 252 }
 253
 254 bool accel_device::update_value_iio(void)
 255 {
 256         struct {
 257                 int16_t x;
 258                 int16_t y;
 259                 int16_t z;
 260                 int64_t timestamp;
 261         } __attribute__((packed)) data;
 262
 263         struct pollfd pfd;
 264
 265         pfd.fd = m_node_handle;
 266         pfd.events = POLLIN | POLLERR;
 267         pfd.revents = 0;
 268
 269         int ret = poll(&pfd, 1, -1);
 270
 271         if (ret == -1) {
 272                 _ERRNO(errno, _E, "Failed to poll from m_node_handle:%d", m_node_handle);
 273                 return false;
 274         } else if (!ret) {
 275                 _E("poll timeout m_node_handle:%d", m_node_handle);
 276                 return false;
 277         }
 278
 279         if (pfd.revents & POLLERR) {
 280                 _E("poll exception occurred! m_node_handle:%d", m_node_handle);
 281                 return false;
 282         }
 283
 284         if (!(pfd.revents & POLLIN)) {
 285                 _E("poll nothing to read! m_node_handle:%d, pfd.revents = %d", m_node_handle, pfd.revents);
 286                 return false;
 287         }
 288
 289         int len = read(m_node_handle, &data, sizeof(data));
 290
 291         if (len != sizeof(data)) {
 292                 _E("Failed to read data, m_node_handle:%d read_len:%d", m_node_handle, len);
 293                 return false;
 294         }
 295
 296         m_x = data.x;
 297         m_y = data.y;
 298         m_z = data.z;
 299         m_fired_time = data.timestamp;
 300
 301         _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 302
 303         return true;
 304 }
 305
 306 int accel_device::read_fd(uint32_t **ids)
 307 {
 308         if (!update_value()) {
 309                 _D("Failed to update value");
 310                 return false;
 311         }
 312
 313         event_ids.clear();
 314         event_ids.push_back(sensor_info.id);
 315
 316         *ids = &event_ids[0];
 317
 318         return event_ids.size();
 319 }
 320
 321 int accel_device::get_data(uint32_t id, sensor_data_t **data, int *length)
 322 {
 323         int remains = 1;
 324         sensor_data_t *sensor_data;
 325         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 326         retvm_if(!sensor_data, -ENOMEM, "Memory allocation failed");
 327
 328         sensor_data->accuracy = SENSOR_ACCURACY_GOOD;
 329         sensor_data->timestamp = m_fired_time;
 330         sensor_data->value_count = 3;
 331         sensor_data->values[0] = m_x;
 332         sensor_data->values[1] = m_y;
 333         sensor_data->values[2] = m_z;
 334
 335         raw_to_base(sensor_data);
 336
 337         *data = sensor_data;
 338         *length = sizeof(sensor_data_t);
 339
 340         return --remains;
 341 }
 342
 343 void accel_device::raw_to_base(sensor_data_t *data)
 344 {
 345         data->value_count = 3;
 346         data->values[0] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[0] * RAW_DATA_UNIT);
 347         data->values[1] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[1] * RAW_DATA_UNIT);
 348         data->values[2] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[2] * RAW_DATA_UNIT);
 349 }