sens.c

   1 /*
   2  * Copyright (C) 2014-2015 Intel Corporation.
   3  */
   4
   5 #include <stdio.h>
   6 #include <stdlib.h>
   7 #include <sys/socket.h>
   8 #include <sys/un.h>
   9 #include <dlfcn.h>
  10 #include <pthread.h>
  11 #include <errno.h>
  12 #include <signal.h>
  13
  14 #include <hardware/sensors.h>
  15 #include <utils/Log.h>
  16
  17 int usage(void)
  18 {
  19         fprintf(stderr, "sens start [sensors.gmin.so]\n");
  20         fprintf(stderr, "sens [activate | deactivate] sensor_id\n");
  21         fprintf(stderr, "sens set_delay sensor_id delay\n");
  22         fprintf(stderr, "sens poll\n");
  23         fprintf(stderr, "sens poll [duration] [number_of_events] \n");
  24         fprintf(stderr, "sens poll_stop\n");
  25         return 1;
  26 }
  27
  28 static struct sensors_module_t *hmi;
  29
  30 static const char* types[] = {
  31         "metadata",
  32         "accelerometer",
  33         "magnetometer",
  34         "orientation",
  35         "gyroscope",
  36         "light",
  37         "pressure",
  38         "temperature",
  39         "proximity",
  40         "gravity",
  41         "linear acceleration",
  42         "rotation vector",
  43         "relative humitidy",
  44         "ambient temperature",
  45         "uncalibrated magnetometer",
  46         "game rotation vector",
  47         "uncalibrated gyrocope",
  48         "significant motion",
  49         "step detector",
  50         "step counter",
  51         "geomagnetic rotation vector",
  52 };
  53
  54 static const char *type_str(int type)
  55 {
  56         int type_count = sizeof(types)/sizeof(char *);
  57
  58         if (type < 0 || type >= type_count)
  59                 return "unknown";
  60         return types[type];
  61 }
  62
  63
  64 static struct sensors_module_t *hmi;
  65 static struct hw_device_t *dev;
  66 static FILE *client;
  67 static pthread_mutex_t client_mutex = PTHREAD_MUTEX_INITIALIZER;
  68 static pthread_cond_t cond = PTHREAD_COND_INITIALIZER;
  69 static int ready_to_close = 0;
  70 static int number_of_events = 0;
  71 static int non_param_poll = 1;
  72 static int event_no = 0;
  73 static int init_events = 0;
  74 static long long timestamp = 0;
  75 static long long event_init_poll_time = 0;
  76 static long long poll_duration = 0;
  77
  78 static void print_event(struct sensors_event_t *e)
  79 {
  80         FILE *f;
  81
  82         pthread_mutex_lock(&client_mutex);
  83         if (!client) {
  84                 pthread_mutex_unlock(&client_mutex);
  85                 return;
  86         }
  87         f = client;
  88
  89         fprintf(f, "event %d: version=%d sensor=%d type=%s timestamp=%lld\n",event_no,
  90                 e->version, e->sensor, type_str(e->type), (long long)e->timestamp);
  91         if (poll_duration != 0)
  92                 fprintf(f,"Time remaining:%lld \n",poll_duration - ((long long)e->timestamp
  93                         - event_init_poll_time));
  94         switch (e->type) {
  95         case SENSOR_TYPE_META_DATA:
  96                 break;
  97         case SENSOR_TYPE_ACCELEROMETER:
  98         case SENSOR_TYPE_LINEAR_ACCELERATION:
  99         case SENSOR_TYPE_GRAVITY:
 100                 fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f status=%d\n",
 101                         e->acceleration.x, e->acceleration.y, e->acceleration.z,
 102                         e->acceleration.status);
 103                 break;
 104         case SENSOR_TYPE_MAGNETIC_FIELD:
 105                 fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f status=%d\n",
 106                         e->magnetic.x, e->magnetic.y, e->magnetic.z,
 107                         e->magnetic.status);
 108                 break;
 109         case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
 110                 fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f bias_x=%10.2f bias_y=%10.2f bias_z=%10.2f \n",
 111                         e->uncalibrated_magnetic.x_uncalib,
 112                         e->uncalibrated_magnetic.y_uncalib,
 113                         e->uncalibrated_magnetic.z_uncalib,
 114                         e->uncalibrated_magnetic.x_bias,
 115                         e->uncalibrated_magnetic.y_bias,
 116                         e->uncalibrated_magnetic.z_bias);
 117                 break;
 118         case SENSOR_TYPE_ORIENTATION:
 119                 fprintf(f, "event: azimuth=%10.2f pitch=%10.2f roll=%10.2f status=%d\n",
 120                         e->orientation.azimuth, e->orientation.pitch, e->orientation.roll,
 121                         e->orientation.status);
 122                 break;
 123         case SENSOR_TYPE_GYROSCOPE:
 124                 fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f status=%d\n",
 125                         e->gyro.x, e->gyro.y, e->gyro.z, e->gyro.status);
 126                 break;
 127         case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 128                 fprintf(f, "event: x=%10.2f y=%10.2f z=%10.2f bias_x=%10.2f bias_y=%10.2f bias_z=%10.2f \n",
 129                         e->uncalibrated_gyro.x_uncalib,
 130                         e->uncalibrated_gyro.y_uncalib,
 131                         e->uncalibrated_gyro.z_uncalib,
 132                         e->uncalibrated_gyro.x_bias,
 133                         e->uncalibrated_gyro.y_bias,
 134                         e->uncalibrated_gyro.z_bias);
 135                 break;
 136         case SENSOR_TYPE_LIGHT:
 137                 fprintf(f, "event: light=%10.2f\n", e->light);
 138                 break;
 139         case SENSOR_TYPE_PRESSURE:
 140                 fprintf(f, "event: pressure=%10.2f\n", e->pressure);
 141                 break;
 142         case SENSOR_TYPE_TEMPERATURE:
 143         case SENSOR_TYPE_AMBIENT_TEMPERATURE:
 144                 fprintf(f, "event: temperature=%10.2f\n", e->temperature);
 145                 break;
 146         case SENSOR_TYPE_PROXIMITY:
 147                 fprintf(f, "event: distance=%10.2f\n", e->distance);
 148                 break;
 149         case SENSOR_TYPE_ROTATION_VECTOR:
 150         case SENSOR_TYPE_GAME_ROTATION_VECTOR:
 151         case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR:
 152                 fprintf(f, "event: rot_x=%10.2f rot_y=%10.2f rot_z=%10.2f cos=%10.2f estimated_accuracy=%10.2f\n",
 153                         e->data[0], e->data[1], e->data[2], e->data[3], e->data[4]);
 154                 break;
 155         case SENSOR_TYPE_RELATIVE_HUMIDITY:
 156                 fprintf(f, "event: humidity=%10.2f\n", e->relative_humidity);
 157                 break;
 158         case SENSOR_TYPE_SIGNIFICANT_MOTION:
 159                 fprintf(f, "event: significant_motion=%10.2f\n", e->data[0]);
 160                 break;
 161         case SENSOR_TYPE_STEP_DETECTOR:
 162                 fprintf(f, "event: step_detector=%10.2f\n", e->data[0]);
 163                 break;
 164         case SENSOR_TYPE_STEP_COUNTER:
 165                 fprintf(f, "event: step_counter=%llu\n",
 166                         (unsigned long long)e->u64.step_counter);
 167                 break;
 168         }
 169         fprintf(f, "\n");
 170         fflush(f);
 171
 172         pthread_mutex_unlock(&client_mutex);
 173 }
 174
 175 static void print_result()
 176 {
 177         FILE *f;
 178         pthread_mutex_lock(&client_mutex);
 179         if (!client) {
 180                 pthread_mutex_unlock(&client_mutex);
 181                 return;
 182         }
 183         f = client;
 184         fprintf(f, "Number of events: %d \n", event_no - init_events);
 185         fprintf(f, "Time: %lld \n\n", (long long) timestamp - event_init_poll_time);
 186         fflush(f);
 187         pthread_mutex_unlock(&client_mutex);
 188
 189 }
 190
 191 static void process_event(struct sensors_event_t *e)
 192 {
 193         int is_poll_duration_over = 0;
 194         int is_event_number_reached = 0;
 195
 196         if (event_init_poll_time == 0) {
 197                 event_init_poll_time = (long long) e->timestamp;
 198                 init_events = event_no;
 199         }
 200         is_poll_duration_over = (long long) e->timestamp - event_init_poll_time <= poll_duration ? 0 : 1;
 201         is_event_number_reached = (event_no - init_events) < number_of_events ? 0 : 1;
 202
 203         if ((!is_poll_duration_over && !is_event_number_reached) || non_param_poll)
 204         {
 205                 timestamp = e -> timestamp;
 206                 event_no++;
 207                 print_event(e);
 208         } else {
 209                 ready_to_close = 1;
 210                 print_result();
 211                 pthread_cond_signal(&cond);
 212         }
 213 }
 214
 215 static void run_sensors_poll_v0(void)
 216 {
 217         struct sensors_poll_device_t *poll_dev = (struct sensors_poll_device_t *)dev;
 218
 219         while (1) {
 220                 sensors_event_t events[256];
 221                 int i, count;
 222
 223                 count = poll_dev->poll(poll_dev, events, sizeof(events)/sizeof(sensors_event_t));
 224
 225                 for(i = 0; i < count; i++)
 226                         process_event(&events[i]);
 227         }
 228 }
 229
 230 static void sig_pipe(int sig)
 231 {
 232         client = NULL;
 233 }
 234
 235 static void *run_sensors_thread(void *arg __attribute((unused)))
 236 {
 237
 238         signal(SIGPIPE, sig_pipe);
 239
 240         switch (dev->version) {
 241         case SENSORS_DEVICE_API_VERSION_0_1:
 242         default:
 243                 run_sensors_poll_v0();
 244                 break;
 245         }
 246
 247         return NULL;
 248 }
 249
 250 void print_sensor(const struct sensor_t *s, FILE *f)
 251 {
 252         if (!f)
 253                 return;
 254
 255         fprintf(f, "sensor%d: name=%s vendor=%s version=%d type=%s\n",
 256                 s->handle, s->name, s->vendor, s->version, type_str(s->type));
 257         fprintf(f, "sensor%d: maxRange=%10.2f resolution=%10.2f power=%10.2f\n",
 258                 s->handle, s->maxRange, s->resolution, s->power);
 259         fprintf(f, "sensor%d: minDelay=%d fifoReservedEventCount=%d fifoMaxEventCount=%d\n",
 260                 s->handle, s->minDelay, s->fifoReservedEventCount,
 261                 s->fifoMaxEventCount);
 262
 263 }
 264
 265 static int sensor_set_delay(int handle, int64_t delay)
 266 {
 267         switch (dev->version) {
 268         default:
 269         case SENSORS_DEVICE_API_VERSION_0_1:
 270         {
 271                 struct sensors_poll_device_t *poll_dev = (struct sensors_poll_device_t *)dev;
 272
 273                 return poll_dev->setDelay(poll_dev, handle, delay);
 274         }
 275         }
 276 }
 277
 278
 279 static int sensor_activate(int handle, int enable)
 280 {
 281         switch (dev->version) {
 282         default:
 283         case SENSORS_DEVICE_API_VERSION_0_1:
 284         {
 285                 struct sensors_poll_device_t *poll_dev = (struct sensors_poll_device_t *)dev;
 286
 287                 return poll_dev->activate(poll_dev, handle, enable);
 288         }
 289         }
 290 }
 291
 292 #define CLIENT_ERR(f, fmt...)                   \
 293         { if (f) { fprintf(f, fmt); fprintf(f, "\n"); } ALOGE(fmt); }
 294
 295 static int dispatch_cmd(char *cmd, FILE *f)
 296 {
 297         char *argv[16], *tmp;
 298         int argc = 0, handle;
 299
 300         tmp = strtok(cmd, " ");
 301         while (tmp) {
 302                 argv[argc++] = tmp;
 303                 tmp = strtok(NULL, " ");
 304         }
 305         if (!argc)
 306                 argv[argc++] = tmp;
 307
 308         if (argc < 1) {
 309                 CLIENT_ERR(f, "invalid cmd: %s", cmd);
 310                 return -1;
 311         }
 312
 313         if (!strcmp(argv[0], "ls")) {
 314                 struct sensor_t const* list;
 315                 int i, count = hmi->get_sensors_list(hmi, &list);
 316
 317                 for(i = 0; i < count; i++)
 318                         print_sensor(&list[i], f);;
 319
 320                 return 0;
 321         } else if (!strcmp(argv[0], "activate")) {
 322
 323                 if (argc < 2) {
 324                         CLIENT_ERR(f, "activate: no sensor handle");
 325                         return -1;
 326                 }
 327
 328                 handle = atoi(argv[1]);
 329
 330                 return sensor_activate(handle, 1);
 331
 332         } else if (!strcmp(argv[0], "deactivate")) {
 333
 334                 if (argc < 2) {
 335                         CLIENT_ERR(f, "activate: no sensor handle");
 336                         return -1;
 337                 }
 338
 339                 handle = atoi(argv[1]);
 340
 341                 return sensor_activate(handle, 0);
 342
 343         } else if (!strcmp(argv[0], "set_delay")) {
 344                 int64_t delay;
 345
 346                 if (argc < 3) {
 347                         CLIENT_ERR(f, "setDelay: no sensor handle and/or delay");
 348                         return -1;
 349                 }
 350
 351                 handle=atoi(argv[1]);
 352                 delay=atoll(argv[2]);
 353
 354                 return sensor_set_delay(handle, delay);
 355
 356         } else if (!strcmp(argv[0], "poll")) {
 357                 if (argc == 1) {
 358                         non_param_poll = 1;
 359                 } else if (argc == 3) {
 360                         non_param_poll = 0;
 361                         poll_duration = atoll(argv[1]);
 362                         number_of_events = atoi(argv[2]);
 363                         event_init_poll_time = 0;
 364                         ready_to_close = 0;
 365                 } else {
 366                         CLIENT_ERR(f, "poll: no poll duration or number of events set");
 367                         return -1;
 368                 }
 369                 pthread_mutex_lock(&client_mutex);
 370                 if (client)
 371                         fclose(client);
 372                 client = f;
 373
 374                 if (!non_param_poll) {
 375                         pthread_cond_wait(&cond, &client_mutex);
 376                         fclose(client);
 377                         client = NULL;
 378                 }
 379
 380                 pthread_mutex_unlock(&client_mutex);
 381
 382                 return 1;
 383         } else if (!strcmp(argv[0], "poll_stop")) {
 384                 pthread_mutex_lock(&client_mutex);
 385                 if (client){
 386                         fclose(client);
 387                         client = NULL;
 388                 }
 389                 pthread_mutex_unlock(&client_mutex);
 390
 391                 return 1;
 392         } else if (!strcmp(argv[0], "stop")) {
 393                 exit(1);
 394         } else {
 395                 CLIENT_ERR(f, "invalid command: %s", cmd);
 396                 return -1;
 397         }
 398
 399 }
 400
 401 #ifdef ANDROID
 402 #define NAME_PREFIX "/dev/socket/"
 403 #else
 404 #define NAME_PREFIX "/tmp/"
 405 #endif
 406
 407 #define SENS_SERVER_NAME NAME_PREFIX "sens-server"
 408
 409 struct sockaddr_un server_addr = {
 410         .sun_family = AF_UNIX,
 411         .sun_path = SENS_SERVER_NAME,
 412 };
 413
 414 static int start_server(void)
 415 {
 416         int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0), conn;
 417         int err;
 418
 419         unlink(SENS_SERVER_NAME);
 420
 421         if (sock < 0) {
 422                 ALOGE("failed to create socket: %s", strerror(errno));
 423                 exit(1);
 424         }
 425
 426         err = bind(sock, (struct sockaddr *)&server_addr, sizeof(server_addr));
 427         if (err) {
 428                 ALOGE("failed to bind socket: %s", strerror(errno));
 429                 exit(1);
 430         }
 431
 432         listen(sock, 1);
 433
 434         while (1) {
 435                 char data_buff[1024], cmsg_buffer[1024];
 436                 struct iovec recv_buff = {
 437                         .iov_base = data_buff,
 438                         .iov_len = sizeof(data_buff),
 439                 };
 440                 struct sockaddr_un from;
 441                 struct msghdr msg = {
 442                         .msg_name = &from,
 443                         .msg_namelen = sizeof(from),
 444                         .msg_iov = &recv_buff,
 445                         .msg_iovlen = 1,
 446                         .msg_control = cmsg_buffer,
 447                         .msg_controllen = sizeof(cmsg_buffer),
 448                 };
 449                 FILE *f =NULL;
 450                 struct cmsghdr *cmsg;
 451
 452                 conn = accept(sock, NULL, NULL);
 453                 if (conn < 0) {
 454                         ALOGE("failed to accept connection: %s", strerror(errno));
 455                         continue;
 456                 }
 457
 458                 err = recvmsg(conn, &msg, 0);
 459                 if (err < 0) {
 460                         ALOGE("error in recvmsg: %s", strerror(errno));
 461                         close(conn);
 462                         continue;
 463                 }
 464
 465                 if (err == 0)
 466                         continue;
 467
 468                 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
 469                      cmsg = CMSG_NXTHDR(&msg,cmsg)) {
 470                         if (cmsg->cmsg_level == SOL_SOCKET
 471                             && cmsg->cmsg_type == SCM_RIGHTS) {
 472                                 int *fd = (int *)CMSG_DATA(cmsg);
 473                                 f = fdopen(*fd, "w");
 474                                 break;
 475                         }
 476                 }
 477
 478                 if (data_buff[err - 1] != 0) {
 479                         ALOGE("command is not NULL terminated\n");
 480                         close(conn);
 481                         continue;
 482                 }
 483
 484                 err = dispatch_cmd(data_buff, f);
 485                 if (err < 0) {
 486                         ALOGE("error dispatching command: %d", err);
 487                         close(conn);
 488                         continue;
 489                 }
 490
 491                 /* send ack */
 492                 if (!err) {
 493                         write(conn, data_buff, 1);
 494                         fclose(f);
 495                 }
 496
 497                 close(conn);
 498         }
 499 }
 500
 501 static const char *hal_paths[] = {
 502         "/system/lib/hw/sensors.gmin.so",
 503         "sensors.gmin.so",
 504         "/lib/sensors.gmin.so",
 505 };
 506
 507 static int start_hal(int argc, char **argv)
 508 {
 509         void *hal;
 510         pid_t child;
 511         int err;
 512         pthread_t sensors_thread;
 513         const char *hal_path = NULL;
 514
 515         if (argc == 2) {
 516                 unsigned i;
 517
 518                 for(i = 0; i < sizeof(hal_paths)/sizeof(const char*); i++) {
 519                         if (!access(hal_paths[i], R_OK)) {
 520                                 hal_path = hal_paths[i];
 521                                 break;
 522                         }
 523                 }
 524
 525                 if (!hal_path) {
 526                         fprintf(stderr, "unable to find HAL\n");
 527                         exit(1);
 528                 }
 529         } else
 530                 hal_path = argv[2];
 531
 532         hal = dlopen(hal_path, RTLD_NOW);
 533         if (!hal) {
 534                 fprintf(stderr, "unable to load HAL %s: %s\n", hal_path,
 535                         dlerror());
 536                 return 2;
 537         }
 538
 539         hmi = dlsym(hal, HAL_MODULE_INFO_SYM_AS_STR);
 540         if (!hmi) {
 541                 fprintf(stderr, "unable to find %s entry point in HAL\n",
 542                         HAL_MODULE_INFO_SYM_AS_STR);
 543                 return 3;
 544         }
 545
 546         printf("HAL loaded: name %s vendor %s version %d.%d id %s\n",
 547                hmi->common.name, hmi->common.author,
 548                hmi->common.version_major, hmi->common.version_minor,
 549                hmi->common.id);
 550
 551         child = fork();
 552         if (child) {
 553                 usleep(100);
 554                 return 0;
 555         }
 556
 557         if (setsid() == (pid_t)-1) {
 558                 fprintf(stderr, "failed to send process to background\n");
 559                 exit(1);
 560         }
 561
 562         close(0); close(1); close(2);
 563
 564         ALOGI("Initializing HAL");
 565
 566         err = hmi->common.methods->open((struct hw_module_t *)hmi,
 567                                         SENSORS_HARDWARE_POLL, &dev);
 568
 569         if (err) {
 570                 ALOGE("failed to initialize HAL: %d\n", err);
 571                 exit(1);
 572         }
 573
 574         if (pthread_create(&sensors_thread, NULL, run_sensors_thread, NULL)) {
 575                 ALOGE("failed to create sensor thread");
 576                 exit(1);
 577         }
 578
 579         return start_server();
 580 }
 581
 582 int main(int argc, char **argv)
 583 {
 584         char cmd[1024];
 585         int sock, i;
 586         struct iovec buff = {
 587                 .iov_base = cmd,
 588         };
 589         struct cmsg_fd {
 590                 struct cmsghdr hdr;
 591                 int fd;
 592         }  cmsg_buff = {
 593                 .hdr = {
 594                         .cmsg_level = SOL_SOCKET,
 595                         .cmsg_type = SCM_RIGHTS,
 596                         .cmsg_len = CMSG_LEN(sizeof(int)),
 597                 },
 598                 .fd = 1,
 599         };
 600         struct msghdr msg = {
 601                 .msg_name = NULL,
 602                 .msg_namelen = 0,
 603                 .msg_iov = &buff,
 604                 .msg_iovlen = 1,
 605                 .msg_control = &cmsg_buff,
 606                 .msg_controllen = sizeof(cmsg_buff),
 607         };
 608
 609
 610         if (argc < 2) {
 611                 usage();
 612                 return 1;
 613         }
 614
 615         if (!strcmp(argv[1], "start")) {
 616                 if (argc < 2)
 617                         return usage();
 618
 619                 return start_hal(argc, argv);
 620         }
 621
 622         if (strlen(argv[1]) >= sizeof(cmd))
 623                 return usage();
 624         strncpy(cmd, argv[1], sizeof(cmd) - 1);
 625         strncat(cmd, " ", sizeof(cmd) - strlen(cmd) - 1);
 626         for(i = 2; i < argc; i++) {
 627                 strncat(cmd, argv[i], sizeof(cmd) - strlen(cmd) - 1);
 628                 strncat(cmd, " ", sizeof(cmd) - strlen(cmd) - 1);
 629         }
 630
 631         sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
 632         if (!sock) {
 633                 fprintf(stderr, "failed to create socket: %s\n", strerror(errno));
 634                 return 3;
 635         }
 636
 637         if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
 638                 fprintf(stderr, "failed to connect to server: %s\n", strerror(errno));
 639                 return 5;
 640         }
 641
 642         buff.iov_len = strlen(cmd) + 1;
 643         if (sendmsg(sock, &msg, 0) < 0) {
 644                 fprintf(stderr, "failed sending command to server: %s\n", strerror(errno));
 645                 return 6;
 646         }
 647
 648         buff.iov_len = sizeof(cmd);
 649         if (read(sock, cmd, 1) < 0) {
 650                 fprintf(stderr, "failed getting ack from server: %s\n", strerror(errno));
 651                 return 7;
 652         }
 653
 654         close(sock);
 655
 656         return 0;
 657 }