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)
 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                 client = f;
 301                 pthread_mutex_unlock(&client_mutex);
 302
 303                 return 1;
 304         } else if (!strcmp(argv[0], "stop")) {
 305                 exit(1);
 306         } else {
 307                 CLIENT_ERR(f, "invalid command: %s", cmd);
 308                 return -1;
 309         }
 310
 311 }
 312
 313 #ifdef ANDROID
 314 #define NAME_PREFIX "/dev/socket/"
 315 #else
 316 #define NAME_PREFIX "/tmp/"
 317 #endif
 318
 319 #define SENS_SERVER_NAME NAME_PREFIX "sens-server"
 320
 321 struct sockaddr_un server_addr = {
 322         .sun_family = AF_UNIX,
 323         .sun_path = SENS_SERVER_NAME,
 324 };
 325
 326 static int start_server(void)
 327 {
 328         int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0), conn;
 329         int err;
 330
 331         unlink(SENS_SERVER_NAME);
 332
 333         if (sock < 0) {
 334                 ALOGE("failed to create socket: %s", strerror(errno));
 335                 exit(1);
 336         }
 337
 338         err = bind(sock, (struct sockaddr *)&server_addr, sizeof(server_addr));
 339         if (err) {
 340                 ALOGE("failed to bind socket: %s", strerror(errno));
 341                 exit(1);
 342         }
 343
 344         listen(sock, 1);
 345
 346         while (1) {
 347                 char data_buff[1024], cmsg_buffer[1024];
 348                 struct iovec recv_buff = {
 349                         .iov_base = data_buff,
 350                         .iov_len = sizeof(data_buff),
 351                 };
 352                 struct sockaddr_un from;
 353                 struct msghdr msg = {
 354                         .msg_name = &from,
 355                         .msg_namelen = sizeof(from),
 356                         .msg_iov = &recv_buff,
 357                         .msg_iovlen = 1,
 358                         .msg_control = cmsg_buffer,
 359                         .msg_controllen = sizeof(cmsg_buffer),
 360                 };
 361                 FILE *f =NULL;
 362
 363                 conn = accept(sock, NULL, NULL);
 364                 if (conn < 0) {
 365                         ALOGE("failed to accept connection: %s", strerror(errno));
 366                         continue;
 367                 }
 368
 369                 while (1) {
 370                         struct cmsghdr *cmsg;
 371
 372                         err = recvmsg(conn, &msg, 0);
 373                         if (err < 0) {
 374                                 ALOGE("error in recvmsg: %s", strerror(errno));
 375                                 break;
 376                         }
 377
 378                         if (err == 0)
 379                                 break;
 380
 381                         for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
 382                              cmsg = CMSG_NXTHDR(&msg,cmsg)) {
 383                                 if (cmsg->cmsg_level == SOL_SOCKET
 384                                     && cmsg->cmsg_type == SCM_RIGHTS) {
 385                                         int *fd = (int *)CMSG_DATA(cmsg);
 386                                         f = fdopen(*fd, "w");
 387                                         break;
 388                                 }
 389                         }
 390
 391                         if (data_buff[err - 1] != 0) {
 392                                 ALOGE("command is not NULL terminated\n");
 393                                 break;
 394                         }
 395
 396                         err = dispatch_cmd(data_buff, f);
 397                         if (err < 0) {
 398                                 ALOGE("error dispatching command: %d", err);
 399                                 break;
 400                         }
 401
 402                         /* send ack */
 403                         if (!err)
 404                                 write(conn, data_buff, 1);
 405                 }
 406
 407                 pthread_mutex_lock(&client_mutex);
 408                 client = NULL;
 409                 pthread_mutex_unlock(&client_mutex);
 410
 411                 fclose(f); close(conn);
 412         }
 413 }
 414
 415 static const char *hal_paths[] = {
 416         "/system/lib/hw/sensors.gmin.so",
 417         "sensors.gmin.so",
 418         "/lib/sensors.gmin.so",
 419 };
 420
 421 static int start_hal(int argc, char **argv)
 422 {
 423         void *hal;
 424         pid_t child;
 425         int err;
 426         pthread_t sensors_thread;
 427         const char *hal_path = NULL;
 428
 429         if (argc == 2) {
 430                 unsigned i;
 431
 432                 for(i = 0; i < sizeof(hal_paths)/sizeof(const char*); i++) {
 433                         if (!access(hal_paths[i], R_OK)) {
 434                                 hal_path = hal_paths[i];
 435                                 break;
 436                         }
 437                 }
 438
 439                 if (!hal_path) {
 440                         fprintf(stderr, "unable to find HAL\n");
 441                         exit(1);
 442                 }
 443         } else
 444                 hal_path = argv[2];
 445
 446         hal = dlopen(hal_path, RTLD_NOW);
 447         if (!hal) {
 448                 fprintf(stderr, "unable to load HAL %s: %s\n", hal_path,
 449                         dlerror());
 450                 return 2;
 451         }
 452
 453         hmi = dlsym(hal, HAL_MODULE_INFO_SYM_AS_STR);
 454         if (!hmi) {
 455                 fprintf(stderr, "unable to find %s entry point in HAL\n",
 456                         HAL_MODULE_INFO_SYM_AS_STR);
 457                 return 3;
 458         }
 459
 460         printf("HAL loaded: name %s vendor %s version %d.%d id %s\n",
 461                hmi->common.name, hmi->common.author,
 462                hmi->common.version_major, hmi->common.version_minor,
 463                hmi->common.id);
 464
 465         child = fork();
 466         if (child) {
 467                 usleep(100);
 468                 return 0;
 469         }
 470
 471         if (setsid() == (pid_t)-1) {
 472                 fprintf(stderr, "failed to send process to background\n");
 473                 exit(1);
 474         }
 475
 476         close(0); close(1); close(2);
 477
 478         ALOGI("Initializing HAL");
 479
 480         err = hmi->common.methods->open((struct hw_module_t *)hmi,
 481                                         SENSORS_HARDWARE_POLL, &dev);
 482
 483         if (err) {
 484                 ALOGE("failed to initialize HAL: %d\n", err);
 485                 exit(1);
 486         }
 487
 488         if (pthread_create(&sensors_thread, NULL, run_sensors_thread, NULL)) {
 489                 ALOGE("failed to create sensor thread");
 490                 exit(1);
 491         }
 492
 493         return start_server();
 494 }
 495
 496 int main(int argc, char **argv)
 497 {
 498         char cmd[1024], *tmp;
 499         int sock, i;
 500         struct iovec buff = {
 501                 .iov_base = cmd,
 502         };
 503         struct cmsg_fd {
 504                 struct cmsghdr hdr;
 505                 int fd;
 506         }  cmsg_buff = {
 507                 .hdr = {
 508                         .cmsg_level = SOL_SOCKET,
 509                         .cmsg_type = SCM_RIGHTS,
 510                         .cmsg_len = CMSG_LEN(sizeof(int)),
 511                 },
 512                 .fd = 1,
 513         };
 514         struct msghdr msg = {
 515                 .msg_name = NULL,
 516                 .msg_namelen = 0,
 517                 .msg_iov = &buff,
 518                 .msg_iovlen = 1,
 519                 .msg_control = &cmsg_buff,
 520                 .msg_controllen = sizeof(cmsg_buff),
 521         };
 522
 523
 524         if (argc < 2) {
 525                 usage();
 526                 return 1;
 527         }
 528
 529         if (!strcmp(argv[1], "start")) {
 530                 if (argc < 2)
 531                         return usage();
 532
 533                 return start_hal(argc, argv);
 534         }
 535
 536         if (strlen(argv[1]) >= sizeof(cmd))
 537                 return usage();
 538         strncpy(cmd, argv[1], sizeof(cmd) - 1);
 539         strncat(cmd, " ", sizeof(cmd) - strlen(cmd) - 1);
 540         for(i = 2; i < argc; i++) {
 541                 strncat(cmd, argv[i], sizeof(cmd) - strlen(cmd) - 1);
 542                 strncat(cmd, " ", sizeof(cmd) - strlen(cmd) - 1);
 543         }
 544
 545         sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
 546         if (!sock) {
 547                 fprintf(stderr, "failed to create socket: %s\n", strerror(errno));
 548                 return 3;
 549         }
 550
 551         if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
 552                 fprintf(stderr, "failed to connect to server: %s\n", strerror(errno));
 553                 return 5;
 554         }
 555
 556         buff.iov_len = strlen(cmd) + 1;
 557         if (sendmsg(sock, &msg, 0) < 0) {
 558                 fprintf(stderr, "failed sending command to server: %s\n", strerror(errno));
 559                 return 6;
 560         }
 561
 562         buff.iov_len = sizeof(cmd);
 563         if (read(sock, cmd, 1) < 0) {
 564                 fprintf(stderr, "failed getting ack from server: %s\n", strerror(errno));
 565                 return 7;
 566         }
 567
 568         close(sock);
 569
 570         return 0;
 571 }