sens.c

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