OSDN Git Service

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