1c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro/* Industrialio buffer test code. 2c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * 3c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Copyright (c) 2012 Invensense Inc. 4c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * 5c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * This program is free software; you can redistribute it and/or modify it 6c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * under the terms of the GNU General Public License version 2 as published by 7c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * the Free Software Foundation. 8c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * 9c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Command line parameters 10c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * stress_iio -d time1 -e time2 11c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 12c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 13c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <unistd.h> 14c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <dirent.h> 15c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <fcntl.h> 16c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <stdio.h> 17c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <errno.h> 18c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <sys/stat.h> 19c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <dirent.h> 20c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <linux/types.h> 21c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <string.h> 22c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <poll.h> 23c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include <pthread.h> 24c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "iio_utils.h" 25c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "ml_load_dmp.h" 26c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "ml_sysfs_helper.h" 27c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#include "authenticate.h" 28c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 29c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaropthread_mutex_t data_switch_lock = PTHREAD_MUTEX_INITIALIZER; 30c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 31c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int has_compass = 0; 32c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int has_pressure = 0; 33c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_random_delay = 0; 34c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_delay = 10; 35c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int disable_delay = 10; 36c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_motion_on = 0; 37c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int final_output_rate; 38c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int first_flag; 39c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic char dmp_path[200]; 40c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 41c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int dev_num; 42c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic char *dev_dir_name; 43c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic char *buf_dir_name; 44c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic char *scan_el_dir; 45c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int gyro_data_is_enabled, accel_data_is_enabled, compass_data_is_enabled, quaternion_data_is_enabled; 46c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int accel_engine_is_on; 47c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 48c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostruct dmp_struct { 49c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char fname[100]; 50c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro void (*action)(struct dmp_struct *, int); 51c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro}; 52c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 53c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void HandleTap(struct dmp_struct *dmp, int tap); 54c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void sipmle_print(struct dmp_struct *dmp, int d){ 55c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("%s:%d\n", dmp->fname, d); 56c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 57c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 58c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void handle_smd() { 59c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write wake lock for SMD\n"); 60c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //write_sysfs_string_and_verify("wake_lock", "/sys/power/", "hack"); 61c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 62c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 63c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void pedo_print() 64c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 65dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro struct timespec aa; 66dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro unsigned long long t; 67dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro 68dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro clock_gettime(CLOCK_REALTIME, &aa); 69dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro t = (unsigned long long)aa.tv_sec * 1000000000 + aa.tv_nsec; 70dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro printf("steps=%lld, time=%lld, system=%lld\n", 71c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro read_sysfs_poslonglong("pedometer_steps", dev_dir_name), 72dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro read_sysfs_poslonglong("pedometer_time", dev_dir_name), 73dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro t); 74c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 75c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 76c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostruct dmp_struct event_file[] = { 77c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#if 1 78c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 79c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .fname = "event_tap", 80c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .action = HandleTap, 81c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro }, 82c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 83c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 84c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .fname = "event_smd", 85c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .action = handle_smd, 86c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro }, 87c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 88c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .fname = "event_accel_motion", 89c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .action = sipmle_print, 90c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro }, 91c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro { 92c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .fname = "event_pedometer", 93c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .action = pedo_print, 94c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro }, 95c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro}; 96c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 97c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void HandleTap(struct dmp_struct *dmp, int tap) 98c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 99c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int tap_dir = tap/8; 100c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int tap_num = tap%8 + 1; 101c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 102c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro switch (tap_dir) { 103c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 1: 104c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("INV_TAP_AXIS_X_POS\n"); 105c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 106c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 2: 107c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("INV_TAP_AXIS_X_NEG\n"); 108c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 109c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 3: 110c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("INV_TAP_AXIS_Y_POS\n"); 111c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 112c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 4: 113c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("INV_TAP_AXIS_Y_NEG\n"); 114c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 115c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 5: 116c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("INV_TAP_AXIS_Z_POS\n"); 117c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 118c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 6: 119c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("INV_TAP_AXIS_Z_NEG\n"); 120c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 121c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro default: 122c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 123c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 124c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("Tap number: %d\n", tap_num); 125c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 126c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define DMP_CODE_SIZE 2799 127c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic char dmp_img[DMP_CODE_SIZE]; 128c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void verify_img(){ 129c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro FILE *fp; 130c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 131c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char dmp_path[] = "/sys/bus/iio/devices/iio:device0/dmp_firmware"; 132c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 133c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("saving image\n"); 134c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((fp = fopen(dmp_path, "rb")) < 0 ) { 135c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro perror("dmp fail"); 136c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 137c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 138c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro i = fread(dmp_img, 1, DMP_CODE_SIZE, fp); 139c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("Result=%d\n", i); 140c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fclose(fp); 141c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fp = fopen("/dev/read_img.h", "wt"); 142c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fprintf(fp, "unsigned char rec[]={\n"); 143c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for(i=0; i<DMP_CODE_SIZE; i++) { 144c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fprintf(fp, "0x%02x, ", dmp_img[i]); 145c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //printf( "0x%02x, ", dmp_img[i]); 146c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(((i+1)%16) == 0) { 147c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fprintf(fp, "\n"); 148c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //printf("\n"); 149c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 150c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 151c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fprintf(fp, "};\n "); 152c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fclose(fp); 153c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("saving image Done\n"); 154c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 155c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 156c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void inv_set_rate() 157c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 158c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 159c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 160c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("set rate \n"); 161c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("accel_rate", dev_dir_name, 5); 162c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("gyro_rate", dev_dir_name, 5); 163c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (has_compass) 164dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 10); 165c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (has_pressure) 166dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 30); 167dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 5); 168dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 5); 169c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("three_axes_q_rate", dev_dir_name, 5); 170c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 171c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 172c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 173c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int setup_offset_and_bias() 174c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 175c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 176c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 177c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_accel_x_offset", dev_dir_name, 0); 178c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 179c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel x offset failed.\n"); 180c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_accel_y_offset", dev_dir_name, 0); 181c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 182c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel y offset failed.\n"); 183c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_accel_z_offset", dev_dir_name, 0); 184c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 185c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel z offset failed.\n"); 186c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 187c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_anglvel_x_offset", dev_dir_name, 0); 188c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 189c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel x offset failed.\n"); 190c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_anglvel_y_offset", dev_dir_name, 0); 191c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 192c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel y offset failed.\n"); 193c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_anglvel_z_offset", dev_dir_name, 0); 194c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 195dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro printf("write accel z offset failed.\n"); 196c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 197c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_accel_x_dmp_bias", dev_dir_name, 0); 198c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 199c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel x offset failed.\n"); 200c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_accel_y_dmp_bias", dev_dir_name, 0); 201c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 202c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel y offset failed.\n"); 203c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_accel_z_dmp_bias", dev_dir_name, 0); 204c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 205c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel z offset failed.\n"); 206c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 207c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_anglvel_x_dmp_bias", dev_dir_name, 0); 208c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 209c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write gyro x offset failed.\n"); 210c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_anglvel_y_dmp_bias", dev_dir_name, 0); 211c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 212c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write gyro y offset failed.\n"); 213c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("in_anglvel_z_dmp_bias", dev_dir_name, 0); 214c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 215c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write gyro z offset failed.\n"); 216c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 217c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 218c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 219c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 220c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void setup_dmp(char *dev_path){ 221c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char sysfs_path[200]; 222c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 223c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro FILE *fd; 224c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sprintf(sysfs_path, "%s", dev_path); 225c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("sysfs: %s\n", sysfs_path); 226c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1); 227c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 228c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 229c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 230c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int("in_accel_scale", dev_path, 0); 231c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 232c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 233c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int("in_anglvel_scale", dev_path, 3); 234c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 235dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro return; 236c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int("sampling_frequency", sysfs_path, 200); 237c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 238dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro return; 239c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0); 240c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 241c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 242c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sprintf(dmp_path, "%s/dmp_firmware", dev_path); 243c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((fd = fopen(dmp_path, "wb")) < 0 ) { 244c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro perror("dmp fail"); 245dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro } 246c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_load_dmp(fd); 247c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fclose(fd); 248c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path)); 249c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1); 250c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 251c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 252c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1); 253c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 254c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 255c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* selelct which event to enable and interrupt on/off here */ 256f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro //enable_glu(sysfs_path, 0); 257f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro// ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 0); 258f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro// if (ret < 0) 259f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro// return; 260c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("pedometer_int_on", sysfs_path, 1); 261c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("pedometer_on", sysfs_path, 1); 262c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 263c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, 1); 264dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro write_sysfs_int64("pedometer_steps", sysfs_path, 0); 265dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro write_sysfs_int64("pedometer_time", sysfs_path, 0); 266c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 267c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 268c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 269c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = setup_offset_and_bias(); 270c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 271c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 272c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 273c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 274c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#if 0 275c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic char reg_dump_arr[2000]; 276c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int inv_do_reg_dump(void) 277c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 278c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char reg_dump_name[100]; 279c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int fd, i; 280c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 281c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sprintf(reg_dump_name, "%s/reg_dump", dev_dir_name); 282c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("%s\n", reg_dump_name); 283c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fd = open(reg_dump_name, O_RDONLY); 284c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pread(fd, reg_dump_arr, 2000, 0); 285c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro close(fd); 286c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for ( i = 0; i < 2000; i++) { 287c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("%c", reg_dump_arr[i]); 288c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //if((i+1)%16 == 0) 289c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //printf("\n"); 290c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 291c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 292c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 293c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#endif 294c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 295c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void *get_dmp_event(void *param) { 296c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char file_name[100]; 297c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 298c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int data; 299c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char d[4]; 300c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro FILE *fp; 301c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct pollfd pfd[ARRAY_SIZE(event_file)]; 302c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 303c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("get DMP event: %s\n", dev_dir_name); 304c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro while(1) { 305c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < ARRAY_SIZE(event_file); i++) { 306c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname); 307c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pfd[i].fd = open(file_name, O_RDONLY | O_NONBLOCK); 308c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pfd[i].events = POLLPRI|POLLERR; 309c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pfd[i].revents = 0; 310c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro read(pfd[i].fd, d, 4); 311c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 312c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 313c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro poll(pfd, ARRAY_SIZE(event_file), -1); 314c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < ARRAY_SIZE(event_file); i++) { 315c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro close(pfd[i].fd); 316c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 317c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 318c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i=0; i< ARRAY_SIZE(pfd); i++) { 319c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(pfd[i].revents != 0) { 320c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname); 321c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fp = fopen(file_name, "rt"); 322c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fscanf(fp, "%d\n", &data); 323c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro event_file[i].action(&event_file[i], data); 324c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 325dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro } 326c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 327dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro 328c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 329c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 330c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 331c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_gyro(int on){ 332c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 333c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, on); 334c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 335c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write gyro_enable failed\n"); 336c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 337c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 338c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 339c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 340c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_gyro_output(int on){ 341c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 342c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro gyro_data_is_enabled = on; 343c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("gyro_fifo_enable", dev_dir_name, on); 344c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 345c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write gyro_fifo_enable failed\n"); 346c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 347c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 348c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 349c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 350c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_compass(int on){ 351c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 352c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 353c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro compass_data_is_enabled = on; 354c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, on); 355c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 356c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write gyro_enable failed\n"); 357c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 358c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 359c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 360c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 361c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_pressure(int on){ 362c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 363c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 364c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("pressure_enable", dev_dir_name, on); 365c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 366c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write pressure_enable failed\n"); 367c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 368c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 369c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 370c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 371c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_quaternion(int on) { 372c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 373c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("ped_q_on", dev_dir_name, on); 374c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 375c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write quaternion_on failed\n"); 376c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("six_axes_q_on", dev_dir_name, on); 377c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 378c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write quaternion_on failed\n"); 379c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("three_axes_q_on", dev_dir_name, on); 380c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 381c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write quaternion_on failed\n"); 382c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 383c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 384c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 385c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_step_detector(int on) { 386c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 387c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 388c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("step_detector_on", dev_dir_name, on); 389c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 390c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write step detector on failed\n"); 391c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 392c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_step_indicator(int on) { 393c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 394c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 395c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("step_indicator_on", dev_dir_name, on); 396c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 397c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write step indicator on failed\n"); 398c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 399c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 400c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_accel(int on){ 401c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 402c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro accel_data_is_enabled = on; 403c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro accel_engine_is_on = on; 404c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("accel_enable", dev_dir_name, on); 405c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 406c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel_enable failed\n"); 407c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on); 408c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 409c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel_fifo_enable failed\n"); 410c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 411c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 412c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 413c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_accel_output(int on) { 414c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 415c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro accel_data_is_enabled = on; 416c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 417c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on); 418c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 419c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write accel_fifo_enable failed\n"); 420c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 421c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 422c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 423c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 424c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_enable(int on){ 425c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 426c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 427c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (0 == on) { 428dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro //pthread_mutex_lock(&data_switch_lock); 429c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 430c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, on); 431c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 432c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write enable failed\n"); 433c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 434c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (on) { 435dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro //pthread_mutex_unlock(&data_switch_lock); 436c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 437c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 438c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 439c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 440c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int write_dmp_event(int on) { 441c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 442c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("dmp_event_int_on", dev_dir_name, on); 443c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 444c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write dmp_event_int_on failed\n"); 445c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 446c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 447c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 448c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void random_delay(){ 449c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 450c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro float bb; 451c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 452c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro i = rand(); 453c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro bb = i * 200.0; 454c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro bb = i * 10.0; 455c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro i = 1 + (unsigned int)(bb/(RAND_MAX + 1.0)); 456c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro i *= 2; 457c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (i%2) { 458c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("sleep %d ms\n", i); 459c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro usleep(i*1000); 460c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 461c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("sleep %d s\n", i); 462c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sleep(i); 463c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 464c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 465c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 466c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void dmp_event_control(on){ 467c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 468c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 469c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = 0; 470c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 471c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro// ret = write_sysfs_int_and_verify("tap_on", dev_dir_name, on); 472c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("smd_enable", dev_dir_name, 1); 473c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 474c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 475c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_set_rate(); 476c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 477c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //ret = write_sysfs_int_and_verify("batchmode_wake_fifo_full_on", dev_dir_name, 1); 478dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 10000); 479dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0); 480c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //ret = write_sysfs_int_and_verify("smd_delay_threshold", dev_dir_name, 10); 481c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 482c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 483c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //ret = write_sysfs_int_and_verify("smd_threshold", dev_dir_name, 5000); 484c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 485c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return; 486c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //write_sysfs_int_and_verify("motion_lpa_duration", dev_dir_name, 1000); 487c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //write_sysfs_int_and_verify("motion_lpa_threshold", dev_dir_name, 200); 488c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro write_sysfs_int_and_verify("dmp_on", dev_dir_name, 1); 489c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("sampling_frequency", dev_dir_name, 200); 490c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro //write_sysfs_int_and_verify("motion_lpa_freq", dev_dir_name, 3); 491c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 492c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 493c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid enable_motion(int on) { 494c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 495c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 496c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, on); 497c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (on) { 498c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro gyro_data_is_enabled = 0; 499c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro compass_data_is_enabled = 0; 500c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro quaternion_data_is_enabled = 0; 501c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 502c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 503c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarobool g, a; 504c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int counter = 0; 505c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic unsigned char data_rate[] = {5, 10, 15, 50, 100, 200}; 506c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int run_enable_sequence() 507c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 508c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro bool g, a, out; 509c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 510c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro counter++; 511c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g = rand()%2; 512c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro a = rand()%2; 513c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (!g && !a) 514c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro a = true; 515c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 516dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro g = true; 517f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro// g = false; 518dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro a = true; 519f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro// a = false; 520c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /*disable the master enable */ 521c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_enable(0); 522c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(g) { 523c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro(1); 524c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (rand()%2) { 525c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro out = rand()%2; 526c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_quaternion(out); 527c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro_output(!out); 528c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 529c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_quaternion(1); 530c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro_output(1); 531c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 532dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_quaternion(1); 533dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_gyro_output(0); 534c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 535c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 536c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro(0); 537c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro_output(0); 538c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_quaternion(0); 539c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 540c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(a) { 541c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel(1); 542dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_accel_output(0); 543c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 544c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel(0); 545c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel_output(0); 546c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 547c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (has_compass) { 548c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(rand()%2) 549c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_compass(1); 550c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 551c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_compass(0); 552c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_compass(counter%2); 553dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_compass(0); 554c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 555c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (has_pressure) { 556c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(rand()%2) 557c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_pressure(1); 558c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 559c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_pressure(0); 560c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_pressure(counter%3); 561dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_pressure(0); 562c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 563c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_step_detector(1); 564c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_step_indicator(1); 565dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_step_detector(0); 566dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_step_indicator(0); 567c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 568c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro write_dmp_event(0); 569c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 570c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_motion(0); 571c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (accel_engine_is_on) 572c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dmp_event_control(1); 573c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 574c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dmp_event_control(0); 575c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro first_flag = 1; 576c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /*enable the master enable */ 577c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_enable(1); 578f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro //enable_enable(0); 579f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro //verify_img(); 580f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro //while(1); 581f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro //write_sysfs_string_and_verify("wake_unlock", "/sys/power/", "hack"); 582c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (enable_random_delay) 583c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro random_delay(); 584c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 585c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("sleep %ds\n", enable_delay); 586c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sleep(enable_delay); 587c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 588c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 589c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 590c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 591c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 592c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int run_disable_sequence() { 593c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_enable(0); 594c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 595c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro(0); 596c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel(1); 597c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_quaternion(0); 598c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel_output(0); 599c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro write_dmp_event(1); 600c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_motion(enable_motion_on); 601c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (accel_engine_is_on) 602c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dmp_event_control(1); 603c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 604c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dmp_event_control(0); 605c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 606c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_enable(1); 607c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (enable_random_delay) 608c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro random_delay(); 609c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else { 610c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("sleep %ds\n", disable_delay); 611c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sleep(disable_delay); 612c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 613c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 614dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro if (has_pressure) { 615dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro if(rand()%2) 616dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_pressure(1); 617dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro else 618dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_pressure(0); 619dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_pressure(counter%3); 620dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_pressure(1); 621dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro } 622dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro 623c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 624c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 625c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int run_dmp_off() { 626c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro bool g, a, out; 627c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 628c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro counter++; 629c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g = rand()%2; 630c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro a = rand()%2; 631c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (!g && !a) 632c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro a = true; 633c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 634c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro g = true; 635c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro a = true; 636dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro a = false; 637f51dc826bb5e75a074a14c1227e750160b3fc694Nick Vaccaro// g = false; 638c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /*disable the master enable */ 639c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_enable(0); 640c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(g) { 641c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro(1); 642c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (rand()%2) { 643c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro_output(!out); 644c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 645c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro_output(1); 646c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 647c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro_output(1); 648c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 649c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 650c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro(0); 651c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_gyro_output(0); 652c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 653c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(a) { 654c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel(1); 655c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel_output(1); 656c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro// enable_accel_output(0); 657c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 658c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel(0); 659c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_accel_output(0); 660c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 661c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (has_compass) { 662c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(rand()%2) 663c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_compass(1); 664c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 665c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_compass(0); 666c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_compass(counter%2); 667dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_compass(0); 668c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 669c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (has_pressure) { 670c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if(rand()%2) 671c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_pressure(1); 672c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 673c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_pressure(0); 674c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_pressure(counter%3); 675c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_pressure(1); 676c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 677dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro printf("111111111111111\n"); 678c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 679dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,15); 680dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); 681c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro first_flag = 1; 682c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /*enable the master enable */ 683dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro enable_enable(1); 684dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro //sleep(2); 685c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 686c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 687dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro} 688c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void *control_switch(void *param) 689c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 690c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro while(1) { 691c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro run_enable_sequence(); 692dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro //run_dmp_off(); 693c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("sleeping\n"); 694dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro //sleep(1000); 695c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro run_disable_sequence(); 696c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 697c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 698c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 699c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 700c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarovoid get_sensor_data(char *d, short *sensor) 701c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 702c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int i; 703c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 704c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i = 0; i < 3; i++) 705c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro sensor[i] = *(short *)(d + 2 + i * 2); 706c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 707c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 708c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void *read_data(void *param) 709c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 710c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char *buffer_access; 711dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro char data[2048], *dptr, tmp[24]; 712c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro short sensor[3]; 713c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int q[3], i, ind, left_over_size, buf_size; 714c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret, fp,read_size; 715c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned short hdr; 716c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro bool done_flag; 717dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro struct timespec ts_1; 718dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro unsigned long long t0, t1; 719dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro int g_count, sq_count; 720c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 721c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define PRESSURE_HDR 0x8000 722c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define ACCEL_HDR 0x4000 723c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define GYRO_HDR 0x2000 724c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define COMPASS_HDR 0x1000 725dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro#define COMPASS_HDR_2 0x1800 726c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define LPQUAT_HDR 0x0800 727c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define SIXQUAT_HDR 0x0400 728c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define PEDQUAT_HDR 0x0200 729c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define STEP_DETECTOR_HDR 0x0100 730c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define STEP_INDICATOR_HDR 0x0001 731c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define END_MARKER 0x0010 732c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro#define EMPTY_MARKER 0x0020 733c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 734c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("read_data Thread: %s\n", dev_dir_name); 735c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, dev_dir_name); 736c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 737c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_alloc_scan_el_dir; 738c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num); 739c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 740c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_alloc_buffer_access; 741c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 742c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro fp = open(buffer_access, O_RDONLY | O_NONBLOCK); 743c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (fp == -1) { /*If it isn't there make the node */ 744c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("Failed to open %s\n", buffer_access); 745c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = -errno; 746c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_open_buffer_access; 747c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 748c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ind = 0; 749dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro 750dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro clock_gettime(CLOCK_REALTIME, &ts_1); 751dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro t0 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec; 752c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro while(1) { 753dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro 754dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro clock_gettime(CLOCK_REALTIME, &ts_1); 755dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro t1 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec; 756dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro //printf("diff=%lld, a_count=%d, sq_count=%d\n", (t1-t0), g_count, sq_count); 757dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro g_count = 0; 758dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro sq_count = 0; 759dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro t0 = t1; 760dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro 761c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro struct pollfd pfd = { 762c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .fd = fp, 763c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro .events = POLLIN, 764c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro }; 765c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro poll(&pfd, 1, -1); 766c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 767c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (left_over_size > 0) 768c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(data, tmp, left_over_size); 769c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr = data + left_over_size; 770dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro read_size = read(fp, dptr, 2000); 771c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("readsize=%d, left_over_size=%d\n", read_size, left_over_size); 772c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (read_size <= 0) { 773c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("Wrong size=%d\n", read_size); 774c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_mutex_unlock(&data_switch_lock); 775c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro continue; 776c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 777c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ind = read_size + left_over_size; 778c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr = data; 779dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro //printf("ind=%d\n", ind); 780c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro buf_size = ind - (dptr - data); 781c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = false; 782c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 783c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro while ((buf_size > 0) && (!done_flag)) { 784c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro hdr = *((short *)(dptr)); 785c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if ((hdr & 0xf) && (hdr != STEP_INDICATOR_HDR)) 786c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("STEP$$$$$$$$$$$$$$$=%x ", hdr); 787c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro switch (hdr & (~0xf)) { 788c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PRESSURE_HDR: 789c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 16) { 790c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro get_sensor_data(dptr, sensor); 791c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 792c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("PRESSURE:%d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2], *(long long *)dptr); 793c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 794c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 795c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 796c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case ACCEL_HDR: 797c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 16) { 798c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro get_sensor_data(dptr, sensor); 799c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 800c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("A:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 801c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 802c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 803c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 804c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case GYRO_HDR: 805c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 16) { 806c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro get_sensor_data(dptr, sensor); 807c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 808dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro g_count++; 809c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("G:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 810c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 811c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 812c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 813c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case COMPASS_HDR: 814c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 16) { 815c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro get_sensor_data(dptr, sensor); 816c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 817c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("M:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 818c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 819c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 820c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 821c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case PEDQUAT_HDR: 822c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 16) { 823c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro get_sensor_data(dptr, sensor); 824c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 825c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("PED:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 826c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 827c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 828c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 829c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case LPQUAT_HDR: 830c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 24) { 831c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro q[0] = *(int *)(dptr + 4); 832c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 833c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro q[1] = *(int *)(dptr); 834c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro q[2] = *(int *)(dptr + 4); 835c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 836c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("LPQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); 837c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 838c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 839c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 840c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case SIXQUAT_HDR: 841c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 24) { 842c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro q[0] = *(int *)(dptr + 4); 843c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 844c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro q[1] = *(int *)(dptr); 845c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro q[2] = *(int *)(dptr + 4); 846c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 847dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro sq_count++; 848c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("SIXQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); 849c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 850c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 851c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 852c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case STEP_DETECTOR_HDR: 853c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (buf_size >= 16) { 854c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("STEP DETECTOR "); 855c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 856c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf(" %lld\n", *(long long *)dptr); 857c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else 858c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro done_flag = true; 859c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 860c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 861c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro default: 862c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (hdr == EMPTY_MARKER) { 863c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("emptry marker !!!!!!!!!!!\n"); 864c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else if (hdr == END_MARKER) { 865c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("end marker !!!!!\n"); 866dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro } else if (hdr == COMPASS_HDR_2) { 867dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro printf("bad compass data\n"); 868c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } else { 869c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr +=8; 870c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("%lld\n", *(long long *)dptr); 871c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 872c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 873c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 874c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (!done_flag) 875c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dptr += 8; 876c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro buf_size = ind - (dptr - data); 877c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 878c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ind - (dptr - data) > 0) 879c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro memcpy(tmp, dptr, ind - (dptr - data)); 880c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro left_over_size = ind - (dptr - data); 881c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 882c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro close(fp); 883c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 884c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroerror_open_buffer_access: 885c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro free(buffer_access); 886c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroerror_alloc_buffer_access: 887c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro free(scan_el_dir); 888c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroerror_alloc_scan_el_dir: 889c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 890c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 891c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 892c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 893c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic void inv_create_thread() { 894c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_t thread_dmp_event, thread_read_data, thread_control; 895c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 896c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_create(&thread_dmp_event, NULL, &get_dmp_event, (void *)dev_dir_name); 897c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_create(&thread_read_data, NULL, &read_data, (void *)dev_dir_name); 898c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_create(&thread_control, NULL, &control_switch, (void *)dev_dir_name); 899c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 900c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_join(thread_dmp_event, NULL); 901c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_join(thread_read_data, NULL); 902c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro pthread_join(thread_control, NULL); 903c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 904c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 905c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccarostatic int enable_enable_main(int on){ 906c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret; 907c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 908c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("enable_enable: %s=%d\n", dev_dir_name, on); 909c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("enable", buf_dir_name, on); 910c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 911c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("write enable failed\n"); 912c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 913c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return 0; 914c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 915c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 916c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroint main(int argc, char **argv) 917c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro{ 918c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro unsigned long buf_len = 240; 919c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 920c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int ret, c, i; 921c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 922c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char *trigger_name = NULL; 923c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 924c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int datardytrigger = 1; 925c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro int trig_num; 926c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char *dummy; 927c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char chip_name[10]; 928c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char device_name[10]; 929c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro char sysfs[100]; 930c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 931c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro gyro_data_is_enabled = 0; 932c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro accel_data_is_enabled = 0; 933c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro compass_data_is_enabled = 0; 934c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro quaternion_data_is_enabled = 0; 935c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 936c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro while ((c = getopt(argc, argv, "lcd:e:rmp")) != -1) { 937c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro switch (c) { 938c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 'c': 939c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro has_compass = 1; 940c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 941c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 'p': 942c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro has_pressure = 1; 943c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 944c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 'd': 945c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro disable_delay = strtoul(optarg, &dummy, 10); 946c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 947c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 'e': 948c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_delay = strtoul(optarg, &dummy, 10); 949c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 950c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 'r': 951c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_random_delay = 1; 952c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 953c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case 'm': 954c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_motion_on = 1; 955c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro break; 956c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro case '?': 957c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return -1; 958c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 959c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 960c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 961c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_get_sysfs_path(sysfs); 962c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("sss:::%s\n", sysfs); 963c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (inv_get_chip_name(chip_name) != INV_SUCCESS) { 964c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("get chip name fail\n"); 965c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro exit(0); 966c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 967c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("chip_name=%s\n", chip_name); 968c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (INV_SUCCESS != inv_check_key()) 969c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("key check fail\n"); 970c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro else 971c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("key authenticated\n"); 972c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 973c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro for (i=0; i<strlen(chip_name); i++) { 974c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro device_name[i] = tolower(chip_name[i]); 975c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 976c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro device_name[strlen(chip_name)] = '\0'; 977c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("device name: %s\n", device_name); 978c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 979c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Find the device requested */ 980c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dev_num = find_type_by_name(device_name, "iio:device"); 981c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (dev_num < 0) { 982c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("Failed to find the %s\n", device_name); 983c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = -ENODEV; 984c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_ret; 985c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 986c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("iio device number being used is %d\n", dev_num); 987c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num); 988c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("allco=%x\n", (int)dev_dir_name); 989c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (trigger_name == NULL) { 990c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* 991c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Build the trigger name. If it is device associated it's 992c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * name is <device_name>_dev[n] where n matches the device 993c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * number found above 994c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 995c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = asprintf(&trigger_name, 996c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro "%s-dev%d", device_name, dev_num); 997c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) { 998c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = -ENOMEM; 999c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_ret; 1000c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 1001c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 1002c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Verify the trigger exists */ 1003c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro trig_num = find_type_by_name(trigger_name, "trigger"); 1004c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (trig_num < 0) { 1005c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("Failed to find the trigger %s\n", trigger_name); 1006c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = -ENODEV; 1007c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_free_triggername; 1008c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 1009c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("iio trigger number being used is %d\n", trig_num); 1010c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num); 1011c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) { 1012c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = -ENOMEM; 1013c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_free_triggername; 1014c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 1015c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_enable_main(0); 1016c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1); 1017c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* 1018c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * Parse the files in scan_elements to identify what channels are 1019c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro * present 1020c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro */ 1021c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = 0; 1022c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro setup_dmp(dev_dir_name); 1023c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1024c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("%s %s\n", dev_dir_name, trigger_name); 1025c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro 1026c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Set the device trigger to be the data rdy trigger found above */ 1027c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_string_and_verify("trigger/current_trigger", 1028c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro dev_dir_name, 1029c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro trigger_name); 1030c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) { 1031c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro printf("Failed to write current_trigger file\n"); 1032c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto error_free_buf_dir_name; 1033c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro } 1034c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro /* Setup ring buffer parameters */ 1035dc17e951c1ea1633fb80a9a59931e073f154c924Nick Vaccaro /* length must be even number because iio_store_to_sw_ring is expecting 1036c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro half pointer to be equal to the read pointer, which is impossible 1037c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro when buflen is odd number. This is actually a bug in the code */ 1038c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro ret = write_sysfs_int("length", buf_dir_name, buf_len*2); 1039c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (ret < 0) 1040c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro goto exit_here; 1041c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro enable_enable_main(1); 1042c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro inv_create_thread(); 1043c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroexit_here: 1044c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroerror_free_buf_dir_name: 1045c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro free(buf_dir_name); 1046c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroerror_free_triggername: 1047c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro if (datardytrigger) 1048c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro free(trigger_name); 1049c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaroerror_ret: 1050c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro return ret; 1051c3d4ca9f2df5ddf9894b36d1554fdfc95d625d3fNick Vaccaro} 1052