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