src/gyro/gyro_sensor_hal.cpp

   1 /*
   2  * gyro_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 <dirent.h>
  22 #include <linux/input.h>
  23 #include <csensor_config.h>
  24 #include <gyro_sensor_hal.h>
  25 #include <sys/ioctl.h>
  26 #include <fstream>
  27 #include <sys/poll.h>
  28
  29 using std::ifstream;
  30
  31 #define DPS_TO_MDPS 1000
  32 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  33 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  34 #define RAW_DATA_TO_DPS_UNIT(X) ((float)(X)/((float)DPS_TO_MDPS))
  35
  36 #define SENSOR_TYPE_GYRO                "GYRO"
  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 SCALE_AVAILABLE_NODE    "in_anglvel_scale_available"
  45 #define SCAN_EL_DIR                             "scan_elements/"
  46 #define TRIG_SUFFIX                             "-trigger"
  47 #define GYRO_RINGBUF_LEN                32
  48 #define SEC_MSEC                                1000
  49 #define MSEC_TO_FREQ(VAL)               ((SEC_MSEC) / (VAL))
  50 #define NSEC_TO_MUSEC(VAL)              ((VAL) / 1000)
  51
  52 gyro_sensor_hal::gyro_sensor_hal()
  53 : m_x(-1)
  54 , m_y(-1)
  55 , m_z(-1)
  56 , m_node_handle(-1)
  57 , m_polling_interval(POLL_1HZ_MS)
  58 , m_fired_time(0)
  59 {
  60
  61         const string sensorhub_interval_node_name = "gyro_poll_delay";
  62         csensor_config &config = csensor_config::get_instance();
  63
  64         node_path_info_query query;
  65         node_path_info info;
  66         int input_method = IIO_METHOD;
  67
  68         if (!get_model_properties(SENSOR_TYPE_GYRO, m_model_id, input_method)) {
  69                 ERR("Failed to find model_properties");
  70                 throw ENXIO;
  71
  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_GYRO;
  77         query.input_event_key = "gyro_sensor";
  78         query.iio_enable_node_name = "gyro_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_gyro_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_gyro_dir + string(SCALE_AVAILABLE_NODE);
  96
  97         if (!config.get(SENSOR_TYPE_GYRO, 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_GYRO, 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 + TRIG_SUFFIX;
 113                 if (!verify_iio_trigger(m_trigger_name)) {
 114                         ERR("Failed verify trigger");
 115                         throw ENXIO;
 116                 }
 117                 string scan_dir = m_gyro_dir + SCAN_EL_DIR;
 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                         INFO ("generic channel names:");
 122                         for (vector <string>::iterator it = m_generic_channel_names.begin();
 123                                         it != m_generic_channel_names.end(); ++it) {
 124                                 INFO ("%s", it->c_str());
 125                         }
 126                 }
 127         }
 128
 129         long resolution;
 130
 131         if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RESOLUTION, resolution)) {
 132                 ERR("[RESOLUTION] is empty\n");
 133                 throw ENXIO;
 134         }
 135
 136         m_resolution = (int)resolution;
 137
 138         double raw_data_unit;
 139
 140         if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RAW_DATA_UNIT, raw_data_unit)) {
 141                 ERR("[RAW_DATA_UNIT] is empty\n");
 142                 throw ENXIO;
 143         }
 144
 145         m_raw_data_unit = (float)(raw_data_unit);
 146
 147         if ((m_node_handle = open(m_data_node.c_str(), O_RDWR)) < 0) {
 148                 ERR("gyro handle open fail for gyro processor, error:%s\n", strerror(errno));
 149                 throw ENXIO;
 150         }
 151
 152         if (setup_channels() == true)
 153                 INFO("IIO channel setup successful");
 154         else {
 155                 ERR("IIO channel setup failed");
 156                 throw ENXIO;
 157         }
 158
 159         INFO("m_raw_data_unit = %f\n",m_raw_data_unit);
 160         INFO("RAW_DATA_TO_DPS_UNIT(m_raw_data_unit) = [%f]",RAW_DATA_TO_DPS_UNIT(m_raw_data_unit));
 161         INFO("gyro_sensor is created!\n");
 162 }
 163
 164 gyro_sensor_hal::~gyro_sensor_hal()
 165 {
 166         enable_resource(false);
 167         if (m_data != NULL)
 168                 delete []m_data;
 169
 170         close(m_node_handle);
 171         m_node_handle = -1;
 172
 173         INFO("gyro_sensor is destroyed!\n");
 174 }
 175
 176 string gyro_sensor_hal::get_model_id(void)
 177 {
 178         return m_model_id;
 179 }
 180
 181 sensor_type_t gyro_sensor_hal::get_type(void)
 182 {
 183         return GYROSCOPE_SENSOR;
 184 }
 185
 186 bool gyro_sensor_hal::enable(void)
 187 {
 188         AUTOLOCK(m_mutex);
 189
 190         if (!enable_resource(true))
 191                 return false;
 192
 193         set_interval(m_polling_interval);
 194
 195         m_fired_time = 0;
 196         INFO("Gyro sensor real starting");
 197         return true;
 198 }
 199
 200 bool gyro_sensor_hal::disable(void)
 201 {
 202         AUTOLOCK(m_mutex);
 203
 204         if (!enable_resource(false))
 205                 return false;
 206
 207         INFO("Gyro sensor real stopping");
 208         return true;
 209
 210 }
 211
 212 bool gyro_sensor_hal::set_interval(unsigned long ms_interval)
 213 {
 214         int freq, i;
 215
 216         freq = (int)(MSEC_TO_FREQ(ms_interval));
 217
 218         for (i=0; i < m_sample_freq_count; i++) {
 219                 if (freq == m_sample_freq[i]) {
 220                         if (update_sysfs_num(m_interval_node.c_str(), freq, true) == 0) {
 221                                 INFO("Interval is changed from %lums to %lums]", m_polling_interval, ms_interval);
 222                                 m_polling_interval = ms_interval;
 223                                 return true;
 224                         }
 225                         else {
 226                                 ERR("Failed to set data %lu\n", ms_interval);
 227                                 return false;
 228                         }
 229                 }
 230         }
 231
 232         DBG("The interval not supported: %lu\n", ms_interval);
 233         ERR("Failed to set data %lu\n", ms_interval);
 234         return false;
 235 }
 236
 237 bool gyro_sensor_hal::update_value(bool wait)
 238 {
 239         int i;
 240         struct pollfd pfd;
 241         ssize_t read_size;
 242         const int TIMEOUT = 1000;
 243
 244         pfd.fd = m_node_handle;
 245         pfd.events = POLLIN;
 246         if (wait)
 247                 poll(&pfd, 1, TIMEOUT);
 248         else
 249                 poll(&pfd, 1, 0);
 250
 251         read_size = read(m_node_handle, m_data, GYRO_RINGBUF_LEN * m_scan_size);
 252         if (read_size <= 0) {
 253                 ERR("Gyro:No data available\n");
 254                 return false;
 255         }
 256         else {
 257                 for (i = 0; i < (read_size / m_scan_size); i++)
 258                         decode_data();
 259         }
 260         return true;
 261 }
 262
 263 bool gyro_sensor_hal::is_data_ready(bool wait)
 264 {
 265         bool ret;
 266         ret = update_value(wait);
 267         return ret;
 268 }
 269
 270 int gyro_sensor_hal::get_sensor_data(sensor_data_t &data)
 271 {
 272         AUTOLOCK(m_value_mutex);
 273
 274         data.accuracy = SENSOR_ACCURACY_GOOD;
 275         data.timestamp = m_fired_time ;
 276         data.value_count = 3;
 277         data.values[0] = m_x;
 278         data.values[1] = m_y;
 279         data.values[2] = m_z;
 280
 281         return 0;
 282 }
 283
 284 bool gyro_sensor_hal::get_properties(sensor_properties_s &properties)
 285 {
 286         properties.name = m_chip_name;
 287         properties.vendor = m_vendor;
 288         properties.min_range = MIN_RANGE(m_resolution)* RAW_DATA_TO_DPS_UNIT(m_raw_data_unit);
 289         properties.max_range = MAX_RANGE(m_resolution)* RAW_DATA_TO_DPS_UNIT(m_raw_data_unit);
 290         properties.min_interval = 1;
 291         properties.resolution = RAW_DATA_TO_DPS_UNIT(m_raw_data_unit);
 292         properties.fifo_count = 0;
 293         properties.max_batch_count = 0;
 294         return true;
 295
 296 }
 297
 298 bool gyro_sensor_hal::add_gyro_channels_to_array(void)
 299 {
 300         int i = 0;
 301         m_channels = (struct channel_parameters*) malloc(sizeof(struct channel_parameters) * m_generic_channel_names.size());
 302         for (vector <string>::iterator it = m_generic_channel_names.begin();
 303                         it != m_generic_channel_names.end(); ++it) {
 304                 if (add_channel_to_array(m_gyro_dir.c_str(), it->c_str() , &m_channels[i++]) < 0) {
 305                         ERR("Failed to add channel %s to channel array", it->c_str());
 306                         return false;
 307                 }
 308         }
 309         return true;
 310 }
 311
 312 bool gyro_sensor_hal::setup_channels(void)
 313 {
 314         int freq, i;
 315         double sf;
 316
 317         enable_resource(true);
 318
 319         if (!add_gyro_channels_to_array()) {
 320                 ERR("Failed to add channels to array!");
 321                 return false;
 322         }
 323
 324         INFO("Sorting channels by index");
 325         sort_channels_by_index(m_channels, m_generic_channel_names.size());
 326         INFO("Sorting channels by index completed");
 327
 328         m_scan_size = get_channel_array_size(m_channels, m_generic_channel_names.size());
 329         if (m_scan_size == 0) {
 330                 ERR("Channel array size is zero");
 331                 return false;
 332         }
 333
 334         m_data = new (std::nothrow) char[m_scan_size * GYRO_RINGBUF_LEN];
 335         if (m_data == NULL) {
 336                 ERR("Couldn't create data buffer\n");
 337                 return false;
 338         }
 339
 340         FILE *fp = NULL;
 341         fp = fopen(m_available_freq_node_path.c_str(), "r");
 342         if (!fp) {
 343                 ERR("Fail to open available frequencies file:%s\n", m_available_freq_node_path.c_str());
 344                 return false;
 345         }
 346
 347         for (i = 0; i < MAX_FREQ_COUNT; i++)
 348                 m_sample_freq[i] = 0;
 349
 350         i = 0;
 351
 352         while (fscanf(fp, "%d", &freq) > 0)
 353                 m_sample_freq[i++] = freq;
 354
 355         m_sample_freq_count = i;
 356
 357         fp = fopen(m_available_scale_node_path.c_str(), "r");
 358         if (!fp) {
 359                 ERR("Fail to open available scale factors file:%s\n", m_available_scale_node_path.c_str());
 360                 return false;
 361         }
 362
 363         for (i = 0; i < MAX_SCALING_COUNT; i++)
 364                 m_scale_factor[i] = 0;
 365
 366         i = 0;
 367
 368         while (fscanf(fp, "%lf", &sf) > 0)
 369                 m_scale_factor[i++] = sf;
 370
 371         m_scale_factor_count = i;
 372
 373         return true;
 374 }
 375
 376 void gyro_sensor_hal::decode_data(void)
 377 {
 378         AUTOLOCK(m_value_mutex);
 379
 380         m_x = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[0].buf_index), &m_channels[0]);
 381         m_y = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[1].buf_index), &m_channels[1]);
 382         m_z = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[2].buf_index), &m_channels[2]);
 383
 384         long long int val = *(long long int *)(m_data + m_channels[3].buf_index);
 385         if ((val >> m_channels[3].valid_bits) & 1)
 386                 val = (val & m_channels[3].mask) | ~m_channels[3].mask;
 387
 388         m_fired_time = (unsigned long long int)(NSEC_TO_MUSEC(val));
 389         INFO("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 390 }
 391
 392 bool gyro_sensor_hal::setup_trigger(const char* trig_name, bool verify)
 393 {
 394         int ret = 0;
 395
 396         ret = update_sysfs_string(m_trigger_path.c_str(), trig_name);
 397         if (ret < 0) {
 398                 ERR("failed to write to current_trigger,%s,%s\n", m_trigger_path.c_str(), trig_name);
 399                 return false;
 400         }
 401         INFO("current_trigger setup successfully\n");
 402         return true;
 403 }
 404
 405 bool gyro_sensor_hal::setup_buffer(int enable)
 406 {
 407         int ret;
 408         ret = update_sysfs_num(m_buffer_length_node_path.c_str(), GYRO_RINGBUF_LEN, true);
 409         if (ret < 0) {
 410                 ERR("failed to write to buffer/length\n");
 411                 return false;
 412         }
 413         INFO("buffer/length setup successfully\n");
 414
 415         ret = update_sysfs_num(m_buffer_enable_node_path.c_str(), enable, true);
 416         if (ret < 0) {
 417                 ERR("failed to write to buffer/enable\n");
 418                 return false;
 419         }
 420
 421         if (enable)
 422                 INFO("buffer enabled\n");
 423         else
 424                 INFO("buffer disabled\n");
 425         return true;
 426 }
 427
 428 bool gyro_sensor_hal::enable_resource(bool enable)
 429 {
 430         string temp;
 431         if(enable)
 432                 setup_trigger(m_trigger_name.c_str(), enable);
 433         else
 434                 setup_trigger("NULL", enable);
 435
 436         for (vector <string>::iterator it = m_generic_channel_names.begin();
 437                         it != m_generic_channel_names.end(); ++it) {
 438                 temp = m_gyro_dir + string(SCAN_EL_DIR) + *it + string("_en");
 439                 if (update_sysfs_num(temp.c_str(), enable) < 0)
 440                         return false;
 441         }
 442         setup_buffer(enable);
 443         return true;
 444 }
 445
 446 extern "C" void *create(void)
 447 {
 448         gyro_sensor_hal *inst;
 449
 450         try {
 451                 inst = new gyro_sensor_hal();
 452         } catch (int err) {
 453                 ERR("gyro_sensor class create fail , errno : %d , errstr : %s\n", err, strerror(err));
 454                 return NULL;
 455         }
 456
 457         return (void*)inst;
 458 }
 459
 460 extern "C" void destroy(void *inst)
 461 {
 462         delete (gyro_sensor_hal*)inst;
 463 }