utils.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <stdlib.h>
   6 #include <fcntl.h>
   7 #include <utils/Log.h>
   8 #include <hardware/sensors.h>
   9 #include "utils.h"
  10
  11
  12 /* Note:
  13  *
  14  * Some of these calls are going to fail, because not all sensors expose all
  15  * possible sysfs attributes. As an optimization we may want to cache which
  16  * ones are valid and immediately return in error for inexistent entries.
  17  */
  18
  19 int sysfs_write_int(const char path[PATH_MAX], int value)
  20 {
  21         int ret;
  22         int fd;
  23         int len;
  24         char buf[20];
  25
  26         if (!path[0]) {
  27                 return -1;
  28         }
  29
  30         fd = open(path, O_WRONLY);
  31
  32         if (fd == -1) {
  33                 ALOGV("Cannot open %s (%s)\n", path, strerror(errno));
  34                 return -1;
  35         }
  36
  37         len = sprintf(buf, "%d", value) + 1;
  38
  39         ret = write(fd, buf, len);
  40
  41         if (ret != len) {
  42                 ALOGW("Cannot write %s (%d bytes) to %s (%s)\n", buf, len, path,
  43                       strerror(errno));
  44         }
  45
  46         close(fd);
  47
  48         return ret;
  49 }
  50
  51
  52 int sysfs_read_int(const char path[PATH_MAX], int *value)
  53 {
  54         int fd;
  55         int len;
  56         char buf[20] = {0};
  57
  58         if (!path[0] || !value) {
  59                 return -1;
  60         }
  61
  62         fd = open(path, O_RDONLY);
  63
  64         if (fd == -1) {
  65                 ALOGV("Cannot open %s (%s)\n", path, strerror(errno));
  66                 return -1;
  67         }
  68
  69         len = read(fd, buf, sizeof(buf));
  70
  71         close(fd);
  72
  73         if (len <= 0) {
  74                 ALOGW("Cannot read integer from %s (%s)\n", path,
  75                       strerror(errno));
  76                 return -1;
  77         }
  78
  79         *value = atoi(buf);
  80
  81         ALOGV("Read %d from %s\n", *value, path);
  82
  83         return 0;
  84 }
  85
  86
  87 int sysfs_write_str(const char path[PATH_MAX], const char *str)
  88 {
  89         int ret;
  90         int fd;
  91         int len;
  92
  93         if (!path[0] || !str || !str[0]) {
  94                 return -1;
  95         }
  96
  97         fd = open(path, O_WRONLY);
  98
  99         if (fd == -1) {
 100                 ALOGV("Cannot open %s (%s)\n", path, strerror(errno));
 101                 return -1;
 102         }
 103
 104         len = strlen(str);
 105
 106         ret = write(fd, str, len);
 107
 108         if (ret != len) {
 109                 ALOGW("Cannot write %s (%d bytes) to %s (%s)\n", str, len, path,
 110                       strerror(errno));
 111         }
 112         else
 113                 ALOGV("Wrote %s to %s\n", str, path);
 114
 115         close(fd);
 116
 117         return ret;
 118 }
 119
 120
 121 int sysfs_read_str(const char path[PATH_MAX], char *buf, int buf_len)
 122 {
 123         int fd;
 124         int len;
 125
 126         if (!path[0] || !buf || buf_len < 1)
 127                 return -1;
 128
 129         fd = open(path, O_RDONLY);
 130
 131         if (fd == -1) {
 132                 ALOGV("Cannot open %s (%s)\n", path, strerror(errno));
 133                 return -1;
 134         }
 135
 136         len = read(fd, buf, buf_len);
 137
 138         close(fd);
 139
 140         if (len == -1) {
 141                 ALOGW("Cannot read string from %s (%s)\n", path,
 142                       strerror(errno));
 143                 return -1;
 144         }
 145
 146         buf[len == 0 ? 0 : len-1] = '\0';
 147
 148         ALOGV("Read %s from %s\n", buf, path);
 149
 150         return len;
 151 }
 152
 153
 154 int sysfs_read_float(const char path[PATH_MAX], float *value)
 155 {
 156         int fd;
 157         int len;
 158         char buf[20] = {0};
 159
 160         if (!path[0] || !value) {
 161                 return -1;
 162         }
 163
 164         fd = open(path, O_RDONLY);
 165
 166         if (fd == -1) {
 167                 ALOGV("Cannot open %s (%s)\n", path, strerror(errno));
 168                 return -1;
 169         }
 170
 171         len = read(fd, buf, sizeof(buf));
 172
 173         close(fd);
 174
 175         if (len <= 0) {
 176                 ALOGW("Cannot read float from %s (%s)\n", path,
 177                       strerror(errno));
 178                 return -1;
 179         }
 180
 181         *value = (float) strtod(buf, NULL);
 182
 183         ALOGV("Read %f from %s\n", *value, path);
 184
 185         return 0;
 186 }
 187
 188
 189 int decode_type_spec(   const char type_buf[MAX_TYPE_SPEC_LEN],
 190                         struct datum_info_t *type_info)
 191 {
 192         /* Return size in bytes for this type specification, or -1 in error */
 193         char sign;
 194         char endianness;
 195         unsigned int realbits, storagebits, shift;
 196         int tokens;
 197
 198         /* Valid specs: "le:u10/16>>0", "le:s16/32>>0" or "le:s32/32>>0" */
 199
 200         tokens = sscanf(type_buf, "%ce:%c%u/%u>>%u",
 201                         &endianness, &sign, &realbits, &storagebits, &shift);
 202
 203         if     (tokens != 5 ||
 204                 (endianness != 'b' && endianness != 'l') ||
 205                 (sign != 'u' && sign != 's') ||
 206                 realbits > storagebits ||
 207                 (storagebits != 16 && storagebits != 32 && storagebits != 64)) {
 208                         ALOGE("Invalid iio channel type spec: %s\n", type_buf);
 209                         return -1;
 210                 }
 211
 212         type_info->endianness   =               endianness;
 213         type_info->sign         =               sign;
 214         type_info->realbits     =       (short) realbits;
 215         type_info->storagebits  =       (short) storagebits;
 216         type_info->shift        =       (short) shift;
 217
 218         return storagebits / 8;
 219 }
 220
 221
 222 int64_t sample_as_int64(unsigned char* sample, struct datum_info_t* type)
 223 {
 224         uint16_t u16;
 225         uint32_t u32;
 226         uint64_t u64;
 227         int i;
 228
 229         switch (type->storagebits) {
 230                 case 64:
 231                         u64 = 0;
 232
 233                         if (type->endianness == 'b')
 234                                 for (i=0; i<8; i++)
 235                                         u64 = (u64 << 8) | sample[i];
 236                         else
 237                                 for (i=7; i>=0; i--)
 238                                         u64 = (u64 << 8) | sample[i];
 239
 240                         if (type->sign == 'u')
 241                                 return (int64_t) (u64 >> type->shift);
 242
 243                         return ((int64_t) u64) >> type->shift;
 244
 245                 case 32:
 246                         if (type->endianness == 'b')
 247                                 u32 = (sample[0] << 24) | (sample[1] << 16) |
 248                                         (sample[2] << 8) | sample[3];
 249                         else
 250                                 u32 = (sample[3] << 24) | (sample[2] << 16) |
 251                                         (sample[1] << 8) | sample[0];
 252
 253                         if (type->sign == 'u')
 254                                 return u32 >> type->shift;
 255
 256                         return ((int32_t) u32) >> type->shift;
 257
 258                 case 16:
 259                         if (type->endianness == 'b')
 260                                 u16 = (sample[0] << 8) | sample[1];
 261                         else
 262                                 u16 = (sample[1] << 8) | sample[0];
 263
 264                         if (type->sign == 'u')
 265                                 return u16 >> type->shift;
 266
 267                         return  ((int16_t) u16) >> type->shift;
 268         }
 269
 270         ALOGE("Unhandled sample storage size\n");
 271         return 0;
 272 }
 273
 274
 275 float transform_sample (int sensor_type, int channel, float val)
 276 {
 277         /* Last opportunity to alter sample data before it goes to Android */
 278         switch (sensor_type) {
 279                 case SENSOR_TYPE_ACCELEROMETER:
 280                         /*
 281                          * Invert x axis orientation from SI units - see
 282                          * /hardware/libhardware/include/hardware/sensors.h
 283                          * for a discussion of what Android expects
 284                          */
 285                         if (channel == 0)
 286                                 return -val;
 287                         break;
 288
 289                 case SENSOR_TYPE_GYROSCOPE:
 290                         /* Limit drift */
 291                         if (val > -0.05 && val < 0.05)
 292                                 return 0;
 293                         break;
 294         }
 295
 296         return val;
 297 }
 298
 299
 300 int64_t get_timestamp(void)
 301 {
 302         struct timespec ts = {0};
 303
 304         clock_gettime(CLOCK_MONOTONIC, &ts);
 305
 306         return 1000000000LL * ts.tv_sec + ts.tv_nsec;
 307 }
 308
 309