1 /* 2 * Copyright (C) 2014 Invensense, Inc. 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 #include <MPLSupport.h> 18 #include <dirent.h> 19 #include <string.h> 20 #include <stdio.h> 21 #include "log.h" 22 #include "SensorBase.h" 23 #include <fcntl.h> 24 25 #include "ml_sysfs_helper.h" 26 #include "ml_load_dmp.h" 27 28 int inv_read_data(char *fname, long *data) 29 { 30 VFUNC_LOG; 31 32 char buf[sizeof(long) * 4]; 33 int count, fd; 34 35 fd = open(fname, O_RDONLY); 36 if(fd < 0) { 37 LOGE("HAL:Error opening %s", fname); 38 return -1; 39 } 40 memset(buf, 0, sizeof(buf)); 41 count = read_attribute_sensor(fd, buf, sizeof(buf)); 42 if(count < 1) { 43 close(fd); 44 return -1; 45 } else { 46 count = sscanf(buf, "%ld", data); 47 if(count) 48 LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data); 49 } 50 close(fd); 51 52 return 0; 53 } 54 55 /* This one DOES NOT close FDs for you */ 56 int read_attribute_sensor(int fd, char* data, unsigned int size) 57 { 58 VFUNC_LOG; 59 60 int count = 0; 61 if (fd > 0) { 62 count = pread(fd, data, size, 0); 63 if(count < 1) { 64 LOGE("HAL:read fails with error code=%d", count); 65 } 66 } 67 return count; 68 } 69 70 /** 71 * @brief Enable a sensor through the sysfs file descriptor 72 * provided. 73 * @note this function one closes FD after the write 74 * @param fd 75 * the file descriptor to write into 76 * @param en 77 * the value to write, typically 1 or 0 78 * @return the errno whenever applicable. 79 */ 80 int enable_sysfs_sensor(int fd, int en) 81 { 82 VFUNC_LOG; 83 84 int nb; 85 int err = 0; 86 87 char c = en ? '1' : '0'; 88 nb = write(fd, &c, 1); 89 90 if (nb <= 0) { 91 err = errno; 92 LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)", 93 c, nb, strerror(err), err); 94 } 95 close(fd); 96 97 98 return -err; 99 } 100 101 /* This one closes FDs for you */ 102 int write_attribute_sensor(int fd, long data) 103 { 104 VFUNC_LOG; 105 106 int num_b = 0; 107 108 if (fd >= 0) { 109 char buf[80]; 110 sprintf(buf, "%ld", data); 111 num_b = write(fd, buf, strlen(buf) + 1); 112 if (num_b <= 0) { 113 int err = errno; 114 LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err); 115 } else { 116 LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data); 117 } 118 close(fd); 119 } 120 121 return num_b; 122 } 123 124 /* This one DOES NOT close FDs for you */ 125 int write_attribute_sensor_continuous(int fd, long data) 126 { 127 VFUNC_LOG; 128 129 int num_b = 0; 130 131 if (fd >= 0) { 132 char buf[80]; 133 sprintf(buf, "%ld", data); 134 num_b = write(fd, buf, strlen(buf) + 1); 135 if (num_b <= 0) { 136 int err = errno; 137 LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err); 138 } else { 139 LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data); 140 } 141 } 142 143 return num_b; 144 } 145 146 int read_sysfs_int(char *filename, int *var) 147 { 148 int res=0; 149 FILE *sysfsfp; 150 151 sysfsfp = fopen(filename, "r"); 152 if (sysfsfp != NULL) { 153 if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) { 154 res = errno; 155 LOGE("HAL:ERR open file %s to read with error %d", filename, res); 156 } 157 } 158 return -res; 159 } 160 161 int read_sysfs_int64(char *filename, int64_t *var) 162 { 163 int res=0; 164 FILE *sysfsfp; 165 166 sysfsfp = fopen(filename, "r"); 167 if (sysfsfp != NULL) { 168 if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) { 169 res = errno; 170 LOGE("HAL:ERR open file %s to read with error %d", filename, res); 171 } 172 } 173 return -res; 174 } 175 176 void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement) 177 { 178 int bytePosition = 0; 179 for (int index = 0; index < numElement; index++) { 180 for (int i = 0; i < 4; i++) { 181 hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF); 182 //LOGI("e%d quat[%d]: %x", index, bytePosition, hex[bytePosition]); 183 bytePosition++; 184 } 185 } 186 return; 187 } 188 189 int write_sysfs_int(char *filename, int var) 190 { 191 int res=0; 192 FILE *sysfsfp; 193 194 sysfsfp = fopen(filename, "w"); 195 if (sysfsfp != NULL) { 196 if (fprintf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) { 197 res = errno; 198 LOGE("HAL:ERR open file %s to write with error %d", filename, res); 199 } 200 } 201 return -res; 202 } 203 204 int write_sysfs_longlong(char *filename, int64_t var) 205 { 206 int res=0; 207 FILE *sysfsfp; 208 209 sysfsfp = fopen(filename, "w"); 210 if (sysfsfp != NULL) { 211 if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) { 212 res = errno; 213 LOGE("HAL:ERR open file %s to write with error %d", filename, res); 214 } 215 } 216 return -res; 217 } 218 219 int fill_dev_full_name_by_prefix(const char* dev_prefix, 220 char *dev_full_name, int len) 221 { 222 char cand_name[20]; 223 int prefix_len = strlen(dev_prefix); 224 strncpy(cand_name, dev_prefix, sizeof(cand_name) / sizeof(cand_name[0])); 225 226 // try adding a number, 0-9 227 for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) { 228 snprintf(&cand_name[prefix_len], 229 sizeof(cand_name) / sizeof(cand_name[0]), 230 "%d", cand_postfix); 231 int dev_num = find_type_by_name(cand_name, "iio:device"); 232 if (dev_num != -ENODEV) { 233 strncpy(dev_full_name, cand_name, len); 234 return 0; 235 } 236 } 237 // try adding a small letter, a-z 238 for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) { 239 snprintf(&cand_name[prefix_len], 240 sizeof(cand_name) / sizeof(cand_name[0]), 241 "%c", cand_postfix); 242 int dev_num = find_type_by_name(cand_name, "iio:device"); 243 if (dev_num != -ENODEV) { 244 strncpy(dev_full_name, cand_name, len); 245 return 0; 246 } 247 } 248 // try adding a capital letter, A-Z 249 for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) { 250 snprintf(&cand_name[prefix_len], 251 sizeof(cand_name) / sizeof(cand_name[0]), 252 "%c", cand_postfix); 253 int dev_num = find_type_by_name(cand_name, "iio:device"); 254 if (dev_num != -ENODEV) { 255 strncpy(dev_full_name, cand_name, len); 256 return 0; 257 } 258 } 259 return 1; 260 } 261 262 void dump_dmp_img(const char *outFile) 263 { 264 FILE *fp; 265 int i; 266 267 char sysfs_path[MAX_SYSFS_NAME_LEN]; 268 char dmp_path[MAX_SYSFS_NAME_LEN]; 269 270 inv_get_sysfs_path(sysfs_path); 271 sprintf(dmp_path, "%s%s", sysfs_path, "/dmp_firmware"); 272 273 LOGI("HAL DEBUG:dump DMP image"); 274 LOGI("HAL DEBUG:open %s\n", dmp_path); 275 LOGI("HAL DEBUG:write to %s", outFile); 276 277 read_dmp_img(dmp_path, (char *)outFile); 278 } 279 280 int read_sysfs_dir(bool fileMode, char *sysfs_path) 281 { 282 VFUNC_LOG; 283 284 int res = 0; 285 char full_path[MAX_SYSFS_NAME_LEN]; 286 int fd; 287 char buf[sizeof(long) *4]; 288 long data; 289 290 DIR *dp; 291 struct dirent *ep; 292 293 dp = opendir (sysfs_path); 294 295 if (dp != NULL) 296 { 297 LOGI("******************** System Sysfs Dump ***************************"); 298 LOGV_IF(0,"HAL DEBUG: opened directory %s", sysfs_path); 299 while ((ep = readdir (dp))) { 300 if(ep != NULL) { 301 LOGV_IF(0,"file name %s", ep->d_name); 302 if(!strcmp(ep->d_name, ".") || !strcmp(ep->d_name, "..") || 303 !strcmp(ep->d_name, "uevent") || !strcmp(ep->d_name, "dev") || 304 !strcmp(ep->d_name, "self_test")) 305 continue; 306 sprintf(full_path, "%s%s%s", sysfs_path, "/", ep->d_name); 307 LOGV_IF(0,"HAL DEBUG: reading %s", full_path); 308 fd = open(full_path, O_RDONLY); 309 if (fd > -1) { 310 memset(buf, 0, sizeof(buf)); 311 res = read_attribute_sensor(fd, buf, sizeof(buf)); 312 close(fd); 313 if (res > 0) { 314 res = sscanf(buf, "%ld", &data); 315 if (res) 316 LOGI("HAL DEBUG:sysfs:cat %s = %ld", full_path, data); 317 } else { 318 LOGV_IF(0,"HAL DEBUG: error reading %s", full_path); 319 } 320 } else { 321 LOGV_IF(0,"HAL DEBUG: error opening %s", full_path); 322 } 323 close(fd); 324 } 325 } 326 closedir(dp); 327 } else{ 328 LOGI("HAL DEBUG: could not open directory %s", sysfs_path); 329 } 330 331 return res; 332 } 333 334 int inv_float_to_q16(float *fdata, long *ldata) 335 { 336 337 if (!fdata || !ldata) 338 return -1; 339 ldata[0] = (long)(fdata[0] * 65536.f); 340 ldata[1] = (long)(fdata[1] * 65536.f); 341 ldata[2] = (long)(fdata[2] * 65536.f); 342 return 0; 343 } 344 345 int inv_long_to_q16(long *fdata, long *ldata) 346 { 347 348 if (!fdata || !ldata) 349 return -1; 350 ldata[0] = (fdata[1] * 65536.f); 351 ldata[1] = (fdata[2] * 65536.f); 352 ldata[2] = (fdata[3] * 65536.f); 353 return 0; 354 } 355 356 int inv_float_to_round(float *fdata, long *ldata) 357 { 358 359 if (!fdata || !ldata) 360 return -1; 361 ldata[0] = (long)fdata[0]; 362 ldata[1] = (long)fdata[1]; 363 ldata[2] = (long)fdata[2]; 364 return 0; 365 } 366 367 int inv_float_to_round2(float *fdata, short *ldata) 368 { 369 370 if (!fdata || !ldata) 371 return -1; 372 ldata[0] = (short)fdata[0]; 373 ldata[1] = (short)fdata[1]; 374 ldata[2] = (short)fdata[2]; 375 return 0; 376 } 377 378 int inv_long_to_float(long *ldata, float *fdata) 379 { 380 381 if (!ldata || !fdata) 382 return -1; 383 fdata[0] = (float)ldata[0]; 384 fdata[1] = (float)ldata[1]; 385 fdata[2] = (float)ldata[2]; 386 return 0; 387 } 388
