OSDN Git Service

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