src/accel/accel_sensor_hal.cpp

   1 /*
   2  * accel_sensor_hal
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19 #include <fcntl.h>
  20 #include <sys/stat.h>
  21 #include <linux/input.h>
  22 #include <csensor_config.h>
  23 #include <accel_sensor_hal.h>
  24 #include <sys/poll.h>
  25
  26 using std::ifstream;
  27
  28 #define GRAVITY 9.80665
  29 #define G_TO_MG 1000
  30 #define RAW_DATA_TO_G_UNIT(X) (((float)(X))/((float)G_TO_MG))
  31 #define RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(X) (GRAVITY * (RAW_DATA_TO_G_UNIT(X)))
  32
  33 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  34 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  35
  36 #define SENSOR_TYPE_ACCEL               "ACCEL"
  37 #define ELEMENT_NAME                    "NAME"
  38 #define ELEMENT_VENDOR                  "VENDOR"
  39 #define ELEMENT_RAW_DATA_UNIT   "RAW_DATA_UNIT"
  40 #define ELEMENT_RESOLUTION              "RESOLUTION"
  41
  42 #define ATTR_VALUE                              "value"
  43
  44 #define INPUT_NAME      "accelerometer_sensor"
  45 #define ACCEL_SENSORHUB_POLL_NODE_NAME "accel_poll_delay"
  46
  47 #define SCAN_EL_DIR                             "scan_elements/"
  48 #define SCALE_AVAILABLE_NODE    "in_accel_scale_available"
  49 #define ACCEL_RINGBUF_LEN       32
  50 #define SEC_MSEC                        1000
  51 #define MSEC_TO_FREQ(VAL)       ((SEC_MSEC) / (VAL))
  52 #define NSEC_TO_MUSEC(VAL)      ((VAL) / 1000)
  53
  54 accel_sensor_hal::accel_sensor_hal()
  55 : m_x(-1)
  56 , m_y(-1)
  57 , m_z(-1)
  58 , m_node_handle(-1)
  59 , m_polling_interval(POLL_1HZ_MS)
  60 , m_fired_time(0)
  61 {
  62         const string sensorhub_interval_node_name = "accel_poll_delay";
  63         csensor_config &config = csensor_config::get_instance();
  64
  65         node_path_info_query query;
  66         node_path_info info;
  67         int input_method = IIO_METHOD;
  68
  69         if (!get_model_properties(SENSOR_TYPE_ACCEL, m_model_id, input_method)) {
  70                 ERR("Failed to find model_properties");
  71                 throw ENXIO;
  72         }
  73
  74         query.input_method = input_method;
  75         query.sensorhub_controlled = m_sensorhub_controlled = is_sensorhub_controlled(sensorhub_interval_node_name);
  76         query.sensor_type = SENSOR_TYPE_ACCEL;
  77         query.input_event_key = "accelerometer_sensor";
  78         query.iio_enable_node_name = "accel_enable";
  79         query.sensorhub_interval_node_name = sensorhub_interval_node_name;
  80
  81         if (!get_node_path_info(query, info)) {
  82                 ERR("Failed to get node info");
  83                 throw ENXIO;
  84         }
  85
  86         show_node_path_info(info);
  87
  88         m_data_node = info.data_node_path;
  89         m_interval_node = info.interval_node_path;
  90         m_accel_dir = info.base_dir;
  91         m_trigger_path = info.trigger_node_path;
  92         m_buffer_enable_node_path = info.buffer_enable_node_path;
  93         m_buffer_length_node_path = info.buffer_length_node_path;
  94         m_available_freq_node_path = info.available_freq_node_path;
  95         m_available_scale_node_path = m_accel_dir + string(SCALE_AVAILABLE_NODE);
  96
  97         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_VENDOR, m_vendor)) {
  98                 ERR("[VENDOR] is empty\n");
  99                 throw ENXIO;
 100         }
 101
 102         INFO("m_vendor = %s", m_vendor.c_str());
 103
 104         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_NAME, m_chip_name)) {
 105                 ERR("[NAME] is empty\n");
 106                 throw ENXIO;
 107         }
 108
 109         INFO("m_chip_name = %s\n",m_chip_name.c_str());
 110
 111         if (input_method == IIO_METHOD) {
 112                 m_trigger_name = m_model_id + "-trigger";
 113                 if (!verify_iio_trigger(m_trigger_name)) {
 114                         ERR("Failed verify trigger");
 115                         throw ENXIO;
 116                 }
 117                 string scan_dir = m_accel_dir + "scan_elements/";
 118                 if (!get_generic_channel_names(scan_dir, string("_type"), m_generic_channel_names))
 119                         ERR ("Failed to find any input channels");
 120                 else
 121                 {
 122                         INFO ("generic channel names:");
 123                         for (vector <string>::iterator it = m_generic_channel_names.begin();
 124                                         it != m_generic_channel_names.end(); ++it) {
 125                                 INFO ("%s", it->c_str());
 126                         }
 127                 }
 128         }
 129
 130         long resolution;
 131
 132         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_RESOLUTION, resolution)) {
 133                 ERR("[RESOLUTION] is empty\n");
 134                 throw ENXIO;
 135         }
 136
 137         m_resolution = (int)resolution;
 138
 139         INFO("m_resolution = %d\n",m_resolution);
 140
 141         double raw_data_unit;
 142
 143         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_RAW_DATA_UNIT, raw_data_unit)) {
 144                 ERR("[RAW_DATA_UNIT] is empty\n");
 145                 throw ENXIO;
 146         }
 147
 148         m_raw_data_unit = (float)(raw_data_unit);
 149
 150         m_node_handle = open(m_data_node.c_str(), O_RDONLY | O_NONBLOCK);
 151         if (m_node_handle < 0) {
 152                 ERR("accel handle open fail for accel processor, error:%s\n", strerror(errno));
 153                 throw ENXIO;
 154         }
 155
 156         if (setup_channels() == true)
 157                 INFO("IIO channel setup successful");
 158         else {
 159                 ERR("IIO channel setup failed");
 160                 throw ENXIO;
 161         }
 162
 163 //      int clockId = CLOCK_MONOTONIC;
 164 //      if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0) {
 165 //              ERR("Fail to set monotonic timestamp for %s", m_data_node.c_str());
 166 //              throw ENXIO;
 167 //      }
 168
 169         INFO("m_raw_data_unit = %f\n", m_raw_data_unit);
 170         INFO("accel_sensor is created!\n");
 171 }
 172
 173 accel_sensor_hal::~accel_sensor_hal()
 174 {
 175         enable_resource(false);
 176         if (m_data != NULL)
 177                 delete []m_data;
 178
 179         close(m_node_handle);
 180         m_node_handle = -1;
 181
 182         INFO("accel_sensor is destroyed!\n");
 183 }
 184
 185 string accel_sensor_hal::get_model_id(void)
 186 {
 187         return m_model_id;
 188 }
 189
 190 sensor_type_t accel_sensor_hal::get_type(void)
 191 {
 192         return ACCELEROMETER_SENSOR;
 193 }
 194
 195 bool accel_sensor_hal::add_accel_channels_to_array(void)
 196 {
 197         int i = 0;
 198         m_channels = (struct channel_parameters*) malloc(sizeof(struct channel_parameters) * m_generic_channel_names.size());
 199         for (vector <string>::iterator it = m_generic_channel_names.begin();
 200                         it != m_generic_channel_names.end(); ++it) {
 201                 if (add_channel_to_array(m_accel_dir.c_str(), it->c_str() , &m_channels[i++]) < 0) {
 202                         ERR("Failed to add channel %s to channel array", it->c_str());
 203                         return false;
 204                 }
 205         }
 206         return true;
 207 }
 208
 209 bool accel_sensor_hal::setup_channels(void)
 210 {
 211         int freq, i;
 212         double sf;
 213
 214         enable_resource(true);
 215
 216         if (!add_accel_channels_to_array()) {
 217                 ERR("Failed to add channels to array!");
 218                 return false;
 219         }
 220
 221         INFO("Sorting channels by index");
 222         sort_channels_by_index(m_channels, m_generic_channel_names.size());
 223         INFO("Sorting channels by index completed");
 224
 225         m_scan_size = get_channel_array_size(m_channels, m_generic_channel_names.size());
 226         if (m_scan_size == 0) {
 227                 ERR("Channel array size is zero");
 228                 return false;
 229         }
 230
 231         m_data = new (std::nothrow) char[m_scan_size * ACCEL_RINGBUF_LEN];
 232         if (m_data == NULL) {
 233                 ERR("Couldn't create data buffer\n");
 234                 return false;
 235         }
 236
 237         FILE *fp = NULL;
 238         fp = fopen(m_available_freq_node_path.c_str(), "r");
 239         if (!fp) {
 240                 ERR("Fail to open available frequencies file:%s\n", m_available_freq_node_path.c_str());
 241                 return false;
 242         }
 243
 244         for (i = 0; i < MAX_FREQ_COUNT; i++)
 245                 m_sample_freq[i] = 0;
 246
 247         i = 0;
 248
 249         while (fscanf(fp, "%d", &freq) > 0)
 250                 m_sample_freq[i++] = freq;
 251
 252         m_sample_freq_count = i;
 253
 254         fp = fopen(m_available_scale_node_path.c_str(), "r");
 255         if (!fp) {
 256                 ERR("Fail to open available scale factors file:%s\n", m_available_scale_node_path.c_str());
 257                 return false;
 258         }
 259
 260         for (i = 0; i < MAX_SCALING_COUNT; i++)
 261                 m_scale_factor[i] = 0;
 262
 263         i = 0;
 264
 265         while (fscanf(fp, "%lf", &sf) > 0)
 266                 m_scale_factor[i++] = sf;
 267
 268         m_scale_factor_count = i;
 269
 270         return true;
 271 }
 272
 273 void accel_sensor_hal::decode_data(void)
 274 {
 275         AUTOLOCK(m_value_mutex);
 276
 277         m_x = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[0].buf_index), &m_channels[0]);
 278         m_y = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[1].buf_index), &m_channels[1]);
 279         m_z = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[2].buf_index), &m_channels[2]);
 280
 281         long long int val = *(long long int *)(m_data + m_channels[3].buf_index);
 282         if ((val >> m_channels[3].valid_bits) & 1)
 283                 val = (val & m_channels[3].mask) | ~m_channels[3].mask;
 284
 285         m_fired_time = (unsigned long long int)(NSEC_TO_MUSEC(val));
 286         DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 287 }
 288 bool accel_sensor_hal::setup_trigger(const char* trig_name, bool verify)
 289 {
 290         int ret = 0;
 291
 292         ret = update_sysfs_string(m_trigger_path.c_str(), trig_name);
 293         if (ret < 0) {
 294                 ERR("failed to write to current_trigger,%s,%s\n", m_trigger_path.c_str(), trig_name);
 295                 return false;
 296         }
 297         INFO("current_trigger setup successfully\n");
 298         return true;
 299 }
 300
 301 bool accel_sensor_hal::setup_buffer(int enable)
 302 {
 303         int ret;
 304         ret = update_sysfs_num(m_buffer_length_node_path.c_str(), ACCEL_RINGBUF_LEN, true);
 305         if (ret < 0) {
 306                 ERR("failed to write to buffer/length\n");
 307                 return false;
 308         }
 309         INFO("buffer/length setup successfully\n");
 310
 311         ret = update_sysfs_num(m_buffer_enable_node_path.c_str(), enable, true);
 312         if (ret < 0) {
 313                 ERR("failed to write to buffer/enable\n");
 314                 return false;
 315         }
 316
 317         if (enable)
 318                 INFO("buffer enabled\n");
 319         else
 320                 INFO("buffer disabled\n");
 321         return true;
 322 }
 323
 324 bool accel_sensor_hal::enable_resource(bool enable)
 325 {
 326         string temp;
 327         if(enable)
 328                 setup_trigger(m_trigger_name.c_str(), enable);
 329         else
 330                 setup_trigger("NULL", enable);
 331
 332         for (vector <string>::iterator it = m_generic_channel_names.begin();
 333                         it != m_generic_channel_names.end(); ++it) {
 334                 temp = m_accel_dir + string(SCAN_EL_DIR) + *it + string("_en");
 335                 if (update_sysfs_num(temp.c_str(), enable) < 0)
 336                         return false;
 337         }
 338         setup_buffer(enable);
 339         return true;
 340 }
 341
 342 bool accel_sensor_hal::enable(void)
 343 {
 344         AUTOLOCK(m_mutex);
 345
 346         if (!enable_resource(true))
 347                         return false;
 348
 349         set_interval(m_polling_interval);
 350
 351         m_fired_time = 0;
 352         INFO("Accel sensor real starting");
 353         return true;
 354 }
 355
 356 bool accel_sensor_hal::disable(void)
 357 {
 358         AUTOLOCK(m_mutex);
 359
 360         if (!enable_resource(false))
 361                 return false;
 362
 363         INFO("Accel sensor real stopping");
 364         return true;
 365 }
 366
 367 bool accel_sensor_hal::set_interval(unsigned long ms_interval)
 368 {
 369         int freq, i;
 370
 371         freq = (int)(MSEC_TO_FREQ(ms_interval));
 372
 373         for (i=0; i < m_sample_freq_count; i++) {
 374                 if (freq == m_sample_freq[i]) {
 375                         if (update_sysfs_num(m_interval_node.c_str(), freq, true) == 0) {
 376                                 INFO("Interval is changed from %lums to %lums]", m_polling_interval, ms_interval);
 377                                 m_polling_interval = ms_interval;
 378                                 return true;
 379                         }
 380                         else {
 381                                 ERR("Failed to set data %lu\n", ms_interval);
 382                                 return false;
 383                         }
 384                 }
 385         }
 386
 387         DBG("The interval not supported: %lu\n", ms_interval);
 388         ERR("Failed to set data %lu\n", ms_interval);
 389         return false;
 390 }
 391
 392 bool accel_sensor_hal::update_value(bool wait)
 393 {
 394         int i;
 395         struct pollfd pfd;
 396         ssize_t read_size;
 397         const int TIMEOUT = 1000;
 398
 399         pfd.fd = m_node_handle;
 400         pfd.events = POLLIN;
 401         if (wait)
 402                 poll(&pfd, 1, TIMEOUT);
 403         else
 404                 poll(&pfd, 1, 0);
 405
 406         read_size = read(m_node_handle, m_data, ACCEL_RINGBUF_LEN * m_scan_size);
 407         if (read_size <= 0) {
 408                 ERR("Accel:No data available\n");
 409                 return false;
 410         }
 411         else {
 412                 for (i = 0; i < (read_size / m_scan_size); i++)
 413                         decode_data();
 414         }
 415         return true;
 416 }
 417
 418 bool accel_sensor_hal::is_data_ready(bool wait)
 419 {
 420         bool ret;
 421         ret = update_value(wait);
 422         return ret;
 423 }
 424
 425 int accel_sensor_hal::get_sensor_data(sensor_data_t &data)
 426 {
 427         AUTOLOCK(m_value_mutex);
 428
 429         data.accuracy = SENSOR_ACCURACY_GOOD;
 430         data.timestamp = m_fired_time;
 431         data.value_count = 3;
 432         data.values[0] = m_x;
 433         data.values[1] = m_y;
 434         data.values[2] = m_z;
 435
 436         return 0;
 437 }
 438
 439 bool accel_sensor_hal::get_properties(sensor_properties_s &properties)
 440 {
 441         properties.name = m_chip_name;
 442         properties.vendor = m_vendor;
 443         properties.min_range = MIN_RANGE(m_resolution)* RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(m_raw_data_unit);
 444         properties.max_range = MAX_RANGE(m_resolution)* RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(m_raw_data_unit);
 445         properties.min_interval = 1;
 446         properties.resolution = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(m_raw_data_unit);
 447         properties.fifo_count = 0;
 448         properties.max_batch_count = 0;
 449         return true;
 450 }
 451
 452 extern "C" void *create(void)
 453 {
 454         accel_sensor_hal *inst;
 455
 456         try {
 457                 inst = new accel_sensor_hal();
 458         } catch (int err) {
 459                 ERR("accel_sensor class create fail , errno : %d , errstr : %s\n", err, strerror(err));
 460                 return NULL;
 461         }
 462
 463         return (void*)inst;
 464 }
 465
 466 extern "C" void destroy(void *inst)
 467 {
 468         delete (accel_sensor_hal*)inst;
 469 }