OSDN Git Service

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