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, int cmd_len, 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                         err = dispatch_cmd(data_buff, err, f);
 392                         if (err < 0) {
 393                                 ALOGE("error dispatching command: %d", err);
 394                                 break;
 395                         }
 396
 397                         /* send ack */
 398                         if (!err)
 399                                 write(conn, data_buff, 1);
 400                 }
 401
 402                 pthread_mutex_lock(&client_mutex);
 403                 client = NULL;
 404                 pthread_mutex_unlock(&client_mutex);
 405
 406                 fclose(f); close(conn);
 407         }
 408 }
 409
 410 static const char *hal_paths[] = {
 411         "/system/lib/hw/sensors.gmin.so",
 412         "sensors.gmin.so",
 413         "/lib/sensors.gmin.so",
 414 };
 415
 416 static int start_hal(int argc, char **argv)
 417 {
 418         void *hal;
 419         pid_t child;
 420         int err;
 421         pthread_t sensors_thread;
 422         const char *hal_path = NULL;
 423
 424         if (argc == 2) {
 425                 unsigned i;
 426
 427                 for(i = 0; i < sizeof(hal_paths)/sizeof(const char*); i++) {
 428                         if (!access(hal_paths[i], R_OK)) {
 429                                 hal_path = hal_paths[i];
 430                                 break;
 431                         }
 432                 }
 433
 434                 if (!hal_path) {
 435                         fprintf(stderr, "unable to find HAL\n");
 436                         exit(1);
 437                 }
 438         } else
 439                 hal_path = argv[2];
 440
 441         hal = dlopen(hal_path, RTLD_NOW);
 442         if (!hal) {
 443                 fprintf(stderr, "unable to load HAL %s: %s\n", hal_path,
 444                         dlerror());
 445                 return 2;
 446         }
 447
 448         hmi = dlsym(hal, HAL_MODULE_INFO_SYM_AS_STR);
 449         if (!hmi) {
 450                 fprintf(stderr, "unable to find %s entry point in HAL\n",
 451                         HAL_MODULE_INFO_SYM_AS_STR);
 452                 return 3;
 453         }
 454
 455         printf("HAL loaded: name %s vendor %s version %d.%d id %s\n",
 456                hmi->common.name, hmi->common.author,
 457                hmi->common.version_major, hmi->common.version_minor,
 458                hmi->common.id);
 459
 460         child = fork();
 461         if (child) {
 462                 usleep(100);
 463                 return 0;
 464         }
 465
 466         if (setsid() == (pid_t)-1) {
 467                 fprintf(stderr, "failed to send process to background\n");
 468                 exit(1);
 469         }
 470
 471         close(0); close(1); close(2);
 472
 473         ALOGI("Initializing HAL");
 474
 475         err = hmi->common.methods->open((struct hw_module_t *)hmi,
 476                                         SENSORS_HARDWARE_POLL, &dev);
 477
 478         if (err) {
 479                 ALOGE("failed to initialize HAL: %d\n", err);
 480                 exit(1);
 481         }
 482
 483         if (pthread_create(&sensors_thread, NULL, run_sensors_thread, NULL)) {
 484                 ALOGE("failed to create sensor thread");
 485                 exit(1);
 486         }
 487
 488         return start_server();
 489 }
 490
 491 int main(int argc, char **argv)
 492 {
 493         char cmd[1024];
 494         int sock, i;
 495         struct iovec recv_buff = {
 496                 .iov_base = cmd,
 497                 .iov_len = sizeof(cmd),
 498         };
 499         struct cmsg_fd {
 500                 struct cmsghdr hdr;
 501                 int fd;
 502         }  cmsg_buff = {
 503                 .hdr = {
 504                         .cmsg_level = SOL_SOCKET,
 505                         .cmsg_type = SCM_RIGHTS,
 506                         .cmsg_len = CMSG_LEN(sizeof(int)),
 507                 },
 508                 .fd = 1,
 509         };
 510         struct msghdr msg = {
 511                 .msg_name = NULL,
 512                 .msg_namelen = 0,
 513                 .msg_iov = &recv_buff,
 514                 .msg_iovlen = 1,
 515                 .msg_control = &cmsg_buff,
 516                 .msg_controllen = sizeof(cmsg_buff),
 517         };
 518
 519
 520         if (argc < 2) {
 521                 usage();
 522                 return 1;
 523         }
 524
 525         if (!strcmp(argv[1], "start")) {
 526                 if (argc < 2)
 527                         return usage();
 528
 529                 return start_hal(argc, argv);
 530         }
 531
 532         strcpy(cmd, argv[1]); strcat(cmd, " ");
 533         for(i = 2; i < argc; i++) {
 534                 strcat(cmd, argv[i]); strcat(cmd, " ");
 535         }
 536
 537         sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
 538         if (!sock) {
 539                 fprintf(stderr, "failed to create socket: %s\n", strerror(errno));
 540                 return 3;
 541         }
 542
 543         if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
 544                 fprintf(stderr, "failed to connect to server: %s\n", strerror(errno));
 545                 return 5;
 546         }
 547
 548         if (sendmsg(sock, &msg, 0) < 0) {
 549                 fprintf(stderr, "failed sending command to server: %s\n", strerror(errno));
 550                 return 6;
 551         }
 552
 553         if (read(sock, cmd, 1) < 0) {
 554                 fprintf(stderr, "failed getting ack from server: %s\n", strerror(errno));
 555                 return 7;
 556         }
 557
 558         close(sock);
 559
 560         return 0;
 561 }