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