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