OSDN Git Service

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