1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * PTP 1588 clock support - User space test program |
4 | * |
5 | * Copyright (C) 2010 OMICRON electronics GmbH |
6 | */ |
7 | #define _GNU_SOURCE |
8 | #define __SANE_USERSPACE_TYPES__ /* For PPC64, to get LL64 types */ |
9 | #include <errno.h> |
10 | #include <fcntl.h> |
11 | #include <inttypes.h> |
12 | #include <math.h> |
13 | #include <signal.h> |
14 | #include <stdio.h> |
15 | #include <stdlib.h> |
16 | #include <string.h> |
17 | #include <sys/ioctl.h> |
18 | #include <sys/mman.h> |
19 | #include <sys/stat.h> |
20 | #include <sys/time.h> |
21 | #include <sys/timex.h> |
22 | #include <sys/types.h> |
23 | #include <time.h> |
24 | #include <unistd.h> |
25 | |
26 | #include <linux/ptp_clock.h> |
27 | |
28 | #define DEVICE "/dev/ptp0" |
29 | |
30 | #ifndef ADJ_SETOFFSET |
31 | #define ADJ_SETOFFSET 0x0100 |
32 | #endif |
33 | |
34 | #ifndef CLOCK_INVALID |
35 | #define CLOCK_INVALID -1 |
36 | #endif |
37 | |
38 | #define NSEC_PER_SEC 1000000000LL |
39 | |
40 | /* clock_adjtime is not available in GLIBC < 2.14 */ |
41 | #if !__GLIBC_PREREQ(2, 14) |
42 | #include <sys/syscall.h> |
43 | static int clock_adjtime(clockid_t id, struct timex *tx) |
44 | { |
45 | return syscall(__NR_clock_adjtime, id, tx); |
46 | } |
47 | #endif |
48 | |
49 | static void show_flag_test(int rq_index, unsigned int flags, int err) |
50 | { |
51 | printf("PTP_EXTTS_REQUEST%c flags 0x%08x : (%d) %s\n" , |
52 | rq_index ? '1' + rq_index : ' ', |
53 | flags, err, strerror(errno)); |
54 | /* sigh, uClibc ... */ |
55 | errno = 0; |
56 | } |
57 | |
58 | static void do_flag_test(int fd, unsigned int index) |
59 | { |
60 | struct ptp_extts_request extts_request; |
61 | unsigned long request[2] = { |
62 | PTP_EXTTS_REQUEST, |
63 | PTP_EXTTS_REQUEST2, |
64 | }; |
65 | unsigned int enable_flags[5] = { |
66 | PTP_ENABLE_FEATURE, |
67 | PTP_ENABLE_FEATURE | PTP_RISING_EDGE, |
68 | PTP_ENABLE_FEATURE | PTP_FALLING_EDGE, |
69 | PTP_ENABLE_FEATURE | PTP_RISING_EDGE | PTP_FALLING_EDGE, |
70 | PTP_ENABLE_FEATURE | (PTP_EXTTS_VALID_FLAGS + 1), |
71 | }; |
72 | int err, i, j; |
73 | |
74 | memset(&extts_request, 0, sizeof(extts_request)); |
75 | extts_request.index = index; |
76 | |
77 | for (i = 0; i < 2; i++) { |
78 | for (j = 0; j < 5; j++) { |
79 | extts_request.flags = enable_flags[j]; |
80 | err = ioctl(fd, request[i], &extts_request); |
81 | show_flag_test(rq_index: i, flags: extts_request.flags, err); |
82 | |
83 | extts_request.flags = 0; |
84 | err = ioctl(fd, request[i], &extts_request); |
85 | } |
86 | } |
87 | } |
88 | |
89 | static clockid_t get_clockid(int fd) |
90 | { |
91 | #define CLOCKFD 3 |
92 | return (((unsigned int) ~fd) << 3) | CLOCKFD; |
93 | } |
94 | |
95 | static long ppb_to_scaled_ppm(int ppb) |
96 | { |
97 | /* |
98 | * The 'freq' field in the 'struct timex' is in parts per |
99 | * million, but with a 16 bit binary fractional field. |
100 | * Instead of calculating either one of |
101 | * |
102 | * scaled_ppm = (ppb / 1000) << 16 [1] |
103 | * scaled_ppm = (ppb << 16) / 1000 [2] |
104 | * |
105 | * we simply use double precision math, in order to avoid the |
106 | * truncation in [1] and the possible overflow in [2]. |
107 | */ |
108 | return (long) (ppb * 65.536); |
109 | } |
110 | |
111 | static int64_t pctns(struct ptp_clock_time *t) |
112 | { |
113 | return t->sec * NSEC_PER_SEC + t->nsec; |
114 | } |
115 | |
116 | static void usage(char *progname) |
117 | { |
118 | fprintf(stderr, |
119 | "usage: %s [options]\n" |
120 | " -c query the ptp clock's capabilities\n" |
121 | " -d name device to open\n" |
122 | " -e val read 'val' external time stamp events\n" |
123 | " -f val adjust the ptp clock frequency by 'val' ppb\n" |
124 | " -F chan Enable single channel mask and keep device open for debugfs verification.\n" |
125 | " -g get the ptp clock time\n" |
126 | " -h prints this message\n" |
127 | " -i val index for event/trigger\n" |
128 | " -k val measure the time offset between system and phc clock\n" |
129 | " for 'val' times (Maximum 25)\n" |
130 | " -l list the current pin configuration\n" |
131 | " -L pin,val configure pin index 'pin' with function 'val'\n" |
132 | " the channel index is taken from the '-i' option\n" |
133 | " 'val' specifies the auxiliary function:\n" |
134 | " 0 - none\n" |
135 | " 1 - external time stamp\n" |
136 | " 2 - periodic output\n" |
137 | " -n val shift the ptp clock time by 'val' nanoseconds\n" |
138 | " -o val phase offset (in nanoseconds) to be provided to the PHC servo\n" |
139 | " -p val enable output with a period of 'val' nanoseconds\n" |
140 | " -H val set output phase to 'val' nanoseconds (requires -p)\n" |
141 | " -w val set output pulse width to 'val' nanoseconds (requires -p)\n" |
142 | " -P val enable or disable (val=1|0) the system clock PPS\n" |
143 | " -s set the ptp clock time from the system time\n" |
144 | " -S set the system time from the ptp clock time\n" |
145 | " -t val shift the ptp clock time by 'val' seconds\n" |
146 | " -T val set the ptp clock time to 'val' seconds\n" |
147 | " -x val get an extended ptp clock time with the desired number of samples (up to %d)\n" |
148 | " -X get a ptp clock cross timestamp\n" |
149 | " -z test combinations of rising/falling external time stamp flags\n" , |
150 | progname, PTP_MAX_SAMPLES); |
151 | } |
152 | |
153 | int main(int argc, char *argv[]) |
154 | { |
155 | struct ptp_clock_caps caps; |
156 | struct ptp_extts_event event; |
157 | struct ptp_extts_request extts_request; |
158 | struct ptp_perout_request perout_request; |
159 | struct ptp_pin_desc desc; |
160 | struct timespec ts; |
161 | struct timex tx; |
162 | struct ptp_clock_time *pct; |
163 | struct ptp_sys_offset *sysoff; |
164 | struct ptp_sys_offset_extended *soe; |
165 | struct ptp_sys_offset_precise *xts; |
166 | |
167 | char *progname; |
168 | unsigned int i; |
169 | int c, cnt, fd; |
170 | |
171 | char *device = DEVICE; |
172 | clockid_t clkid; |
173 | int adjfreq = 0x7fffffff; |
174 | int adjtime = 0; |
175 | int adjns = 0; |
176 | int adjphase = 0; |
177 | int capabilities = 0; |
178 | int extts = 0; |
179 | int flagtest = 0; |
180 | int gettime = 0; |
181 | int index = 0; |
182 | int list_pins = 0; |
183 | int pct_offset = 0; |
184 | int getextended = 0; |
185 | int getcross = 0; |
186 | int n_samples = 0; |
187 | int pin_index = -1, pin_func; |
188 | int pps = -1; |
189 | int seconds = 0; |
190 | int settime = 0; |
191 | int channel = -1; |
192 | |
193 | int64_t t1, t2, tp; |
194 | int64_t interval, offset; |
195 | int64_t perout_phase = -1; |
196 | int64_t pulsewidth = -1; |
197 | int64_t perout = -1; |
198 | |
199 | progname = strrchr(argv[0], '/'); |
200 | progname = progname ? 1+progname : argv[0]; |
201 | while (EOF != (c = getopt(argc, argv, "cd:e:f:F:ghH:i:k:lL:n:o:p:P:sSt:T:w:x:Xz" ))) { |
202 | switch (c) { |
203 | case 'c': |
204 | capabilities = 1; |
205 | break; |
206 | case 'd': |
207 | device = optarg; |
208 | break; |
209 | case 'e': |
210 | extts = atoi(optarg); |
211 | break; |
212 | case 'f': |
213 | adjfreq = atoi(optarg); |
214 | break; |
215 | case 'F': |
216 | channel = atoi(optarg); |
217 | break; |
218 | case 'g': |
219 | gettime = 1; |
220 | break; |
221 | case 'H': |
222 | perout_phase = atoll(optarg); |
223 | break; |
224 | case 'i': |
225 | index = atoi(optarg); |
226 | break; |
227 | case 'k': |
228 | pct_offset = 1; |
229 | n_samples = atoi(optarg); |
230 | break; |
231 | case 'l': |
232 | list_pins = 1; |
233 | break; |
234 | case 'L': |
235 | cnt = sscanf(optarg, "%d,%d" , &pin_index, &pin_func); |
236 | if (cnt != 2) { |
237 | usage(progname); |
238 | return -1; |
239 | } |
240 | break; |
241 | case 'n': |
242 | adjns = atoi(optarg); |
243 | break; |
244 | case 'o': |
245 | adjphase = atoi(optarg); |
246 | break; |
247 | case 'p': |
248 | perout = atoll(optarg); |
249 | break; |
250 | case 'P': |
251 | pps = atoi(optarg); |
252 | break; |
253 | case 's': |
254 | settime = 1; |
255 | break; |
256 | case 'S': |
257 | settime = 2; |
258 | break; |
259 | case 't': |
260 | adjtime = atoi(optarg); |
261 | break; |
262 | case 'T': |
263 | settime = 3; |
264 | seconds = atoi(optarg); |
265 | break; |
266 | case 'w': |
267 | pulsewidth = atoi(optarg); |
268 | break; |
269 | case 'x': |
270 | getextended = atoi(optarg); |
271 | if (getextended < 1 || getextended > PTP_MAX_SAMPLES) { |
272 | fprintf(stderr, |
273 | "number of extended timestamp samples must be between 1 and %d; was asked for %d\n" , |
274 | PTP_MAX_SAMPLES, getextended); |
275 | return -1; |
276 | } |
277 | break; |
278 | case 'X': |
279 | getcross = 1; |
280 | break; |
281 | case 'z': |
282 | flagtest = 1; |
283 | break; |
284 | case 'h': |
285 | usage(progname); |
286 | return 0; |
287 | case '?': |
288 | default: |
289 | usage(progname); |
290 | return -1; |
291 | } |
292 | } |
293 | |
294 | fd = open(device, O_RDWR); |
295 | if (fd < 0) { |
296 | fprintf(stderr, "opening %s: %s\n" , device, strerror(errno)); |
297 | return -1; |
298 | } |
299 | |
300 | clkid = get_clockid(fd); |
301 | if (CLOCK_INVALID == clkid) { |
302 | fprintf(stderr, "failed to read clock id\n" ); |
303 | return -1; |
304 | } |
305 | |
306 | if (capabilities) { |
307 | if (ioctl(fd, PTP_CLOCK_GETCAPS, &caps)) { |
308 | perror("PTP_CLOCK_GETCAPS" ); |
309 | } else { |
310 | printf("capabilities:\n" |
311 | " %d maximum frequency adjustment (ppb)\n" |
312 | " %d programmable alarms\n" |
313 | " %d external time stamp channels\n" |
314 | " %d programmable periodic signals\n" |
315 | " %d pulse per second\n" |
316 | " %d programmable pins\n" |
317 | " %d cross timestamping\n" |
318 | " %d adjust_phase\n" |
319 | " %d maximum phase adjustment (ns)\n" , |
320 | caps.max_adj, |
321 | caps.n_alarm, |
322 | caps.n_ext_ts, |
323 | caps.n_per_out, |
324 | caps.pps, |
325 | caps.n_pins, |
326 | caps.cross_timestamping, |
327 | caps.adjust_phase, |
328 | caps.max_phase_adj); |
329 | } |
330 | } |
331 | |
332 | if (0x7fffffff != adjfreq) { |
333 | memset(&tx, 0, sizeof(tx)); |
334 | tx.modes = ADJ_FREQUENCY; |
335 | tx.freq = ppb_to_scaled_ppm(ppb: adjfreq); |
336 | if (clock_adjtime(clkid, &tx)) { |
337 | perror("clock_adjtime" ); |
338 | } else { |
339 | puts("frequency adjustment okay" ); |
340 | } |
341 | } |
342 | |
343 | if (adjtime || adjns) { |
344 | memset(&tx, 0, sizeof(tx)); |
345 | tx.modes = ADJ_SETOFFSET | ADJ_NANO; |
346 | tx.time.tv_sec = adjtime; |
347 | tx.time.tv_usec = adjns; |
348 | while (tx.time.tv_usec < 0) { |
349 | tx.time.tv_sec -= 1; |
350 | tx.time.tv_usec += NSEC_PER_SEC; |
351 | } |
352 | |
353 | if (clock_adjtime(clkid, &tx) < 0) { |
354 | perror("clock_adjtime" ); |
355 | } else { |
356 | puts("time shift okay" ); |
357 | } |
358 | } |
359 | |
360 | if (adjphase) { |
361 | memset(&tx, 0, sizeof(tx)); |
362 | tx.modes = ADJ_OFFSET | ADJ_NANO; |
363 | tx.offset = adjphase; |
364 | |
365 | if (clock_adjtime(clkid, &tx) < 0) { |
366 | perror("clock_adjtime" ); |
367 | } else { |
368 | puts("phase adjustment okay" ); |
369 | } |
370 | } |
371 | |
372 | if (gettime) { |
373 | if (clock_gettime(clkid, &ts)) { |
374 | perror("clock_gettime" ); |
375 | } else { |
376 | printf("clock time: %ld.%09ld or %s" , |
377 | ts.tv_sec, ts.tv_nsec, ctime(&ts.tv_sec)); |
378 | } |
379 | } |
380 | |
381 | if (settime == 1) { |
382 | clock_gettime(CLOCK_REALTIME, &ts); |
383 | if (clock_settime(clkid, &ts)) { |
384 | perror("clock_settime" ); |
385 | } else { |
386 | puts("set time okay" ); |
387 | } |
388 | } |
389 | |
390 | if (settime == 2) { |
391 | clock_gettime(clkid, &ts); |
392 | if (clock_settime(CLOCK_REALTIME, &ts)) { |
393 | perror("clock_settime" ); |
394 | } else { |
395 | puts("set time okay" ); |
396 | } |
397 | } |
398 | |
399 | if (settime == 3) { |
400 | ts.tv_sec = seconds; |
401 | ts.tv_nsec = 0; |
402 | if (clock_settime(clkid, &ts)) { |
403 | perror("clock_settime" ); |
404 | } else { |
405 | puts("set time okay" ); |
406 | } |
407 | } |
408 | |
409 | if (pin_index >= 0) { |
410 | memset(&desc, 0, sizeof(desc)); |
411 | desc.index = pin_index; |
412 | desc.func = pin_func; |
413 | desc.chan = index; |
414 | if (ioctl(fd, PTP_PIN_SETFUNC, &desc)) { |
415 | perror("PTP_PIN_SETFUNC" ); |
416 | } else { |
417 | puts("set pin function okay" ); |
418 | } |
419 | } |
420 | |
421 | if (extts) { |
422 | memset(&extts_request, 0, sizeof(extts_request)); |
423 | extts_request.index = index; |
424 | extts_request.flags = PTP_ENABLE_FEATURE; |
425 | if (ioctl(fd, PTP_EXTTS_REQUEST, &extts_request)) { |
426 | perror("PTP_EXTTS_REQUEST" ); |
427 | extts = 0; |
428 | } else { |
429 | puts("external time stamp request okay" ); |
430 | } |
431 | for (; extts; extts--) { |
432 | cnt = read(fd, &event, sizeof(event)); |
433 | if (cnt != sizeof(event)) { |
434 | perror("read" ); |
435 | break; |
436 | } |
437 | printf("event index %u at %lld.%09u\n" , event.index, |
438 | event.t.sec, event.t.nsec); |
439 | fflush(stdout); |
440 | } |
441 | /* Disable the feature again. */ |
442 | extts_request.flags = 0; |
443 | if (ioctl(fd, PTP_EXTTS_REQUEST, &extts_request)) { |
444 | perror("PTP_EXTTS_REQUEST" ); |
445 | } |
446 | } |
447 | |
448 | if (flagtest) { |
449 | do_flag_test(fd, index); |
450 | } |
451 | |
452 | if (list_pins) { |
453 | int n_pins = 0; |
454 | if (ioctl(fd, PTP_CLOCK_GETCAPS, &caps)) { |
455 | perror("PTP_CLOCK_GETCAPS" ); |
456 | } else { |
457 | n_pins = caps.n_pins; |
458 | } |
459 | for (i = 0; i < n_pins; i++) { |
460 | desc.index = i; |
461 | if (ioctl(fd, PTP_PIN_GETFUNC, &desc)) { |
462 | perror("PTP_PIN_GETFUNC" ); |
463 | break; |
464 | } |
465 | printf("name %s index %u func %u chan %u\n" , |
466 | desc.name, desc.index, desc.func, desc.chan); |
467 | } |
468 | } |
469 | |
470 | if (pulsewidth >= 0 && perout < 0) { |
471 | puts("-w can only be specified together with -p" ); |
472 | return -1; |
473 | } |
474 | |
475 | if (perout_phase >= 0 && perout < 0) { |
476 | puts("-H can only be specified together with -p" ); |
477 | return -1; |
478 | } |
479 | |
480 | if (perout >= 0) { |
481 | if (clock_gettime(clkid, &ts)) { |
482 | perror("clock_gettime" ); |
483 | return -1; |
484 | } |
485 | memset(&perout_request, 0, sizeof(perout_request)); |
486 | perout_request.index = index; |
487 | perout_request.period.sec = perout / NSEC_PER_SEC; |
488 | perout_request.period.nsec = perout % NSEC_PER_SEC; |
489 | perout_request.flags = 0; |
490 | if (pulsewidth >= 0) { |
491 | perout_request.flags |= PTP_PEROUT_DUTY_CYCLE; |
492 | perout_request.on.sec = pulsewidth / NSEC_PER_SEC; |
493 | perout_request.on.nsec = pulsewidth % NSEC_PER_SEC; |
494 | } |
495 | if (perout_phase >= 0) { |
496 | perout_request.flags |= PTP_PEROUT_PHASE; |
497 | perout_request.phase.sec = perout_phase / NSEC_PER_SEC; |
498 | perout_request.phase.nsec = perout_phase % NSEC_PER_SEC; |
499 | } else { |
500 | perout_request.start.sec = ts.tv_sec + 2; |
501 | perout_request.start.nsec = 0; |
502 | } |
503 | |
504 | if (ioctl(fd, PTP_PEROUT_REQUEST2, &perout_request)) { |
505 | perror("PTP_PEROUT_REQUEST" ); |
506 | } else { |
507 | puts("periodic output request okay" ); |
508 | } |
509 | } |
510 | |
511 | if (pps != -1) { |
512 | int enable = pps ? 1 : 0; |
513 | if (ioctl(fd, PTP_ENABLE_PPS, enable)) { |
514 | perror("PTP_ENABLE_PPS" ); |
515 | } else { |
516 | puts("pps for system time request okay" ); |
517 | } |
518 | } |
519 | |
520 | if (pct_offset) { |
521 | if (n_samples <= 0 || n_samples > 25) { |
522 | puts("n_samples should be between 1 and 25" ); |
523 | usage(progname); |
524 | return -1; |
525 | } |
526 | |
527 | sysoff = calloc(1, sizeof(*sysoff)); |
528 | if (!sysoff) { |
529 | perror("calloc" ); |
530 | return -1; |
531 | } |
532 | sysoff->n_samples = n_samples; |
533 | |
534 | if (ioctl(fd, PTP_SYS_OFFSET, sysoff)) |
535 | perror("PTP_SYS_OFFSET" ); |
536 | else |
537 | puts("system and phc clock time offset request okay" ); |
538 | |
539 | pct = &sysoff->ts[0]; |
540 | for (i = 0; i < sysoff->n_samples; i++) { |
541 | t1 = pctns(t: pct+2*i); |
542 | tp = pctns(t: pct+2*i+1); |
543 | t2 = pctns(t: pct+2*i+2); |
544 | interval = t2 - t1; |
545 | offset = (t2 + t1) / 2 - tp; |
546 | |
547 | printf("system time: %lld.%09u\n" , |
548 | (pct+2*i)->sec, (pct+2*i)->nsec); |
549 | printf("phc time: %lld.%09u\n" , |
550 | (pct+2*i+1)->sec, (pct+2*i+1)->nsec); |
551 | printf("system time: %lld.%09u\n" , |
552 | (pct+2*i+2)->sec, (pct+2*i+2)->nsec); |
553 | printf("system/phc clock time offset is %" PRId64 " ns\n" |
554 | "system clock time delay is %" PRId64 " ns\n" , |
555 | offset, interval); |
556 | } |
557 | |
558 | free(sysoff); |
559 | } |
560 | |
561 | if (getextended) { |
562 | soe = calloc(1, sizeof(*soe)); |
563 | if (!soe) { |
564 | perror("calloc" ); |
565 | return -1; |
566 | } |
567 | |
568 | soe->n_samples = getextended; |
569 | |
570 | if (ioctl(fd, PTP_SYS_OFFSET_EXTENDED, soe)) { |
571 | perror("PTP_SYS_OFFSET_EXTENDED" ); |
572 | } else { |
573 | printf("extended timestamp request returned %d samples\n" , |
574 | getextended); |
575 | |
576 | for (i = 0; i < getextended; i++) { |
577 | printf("sample #%2d: system time before: %lld.%09u\n" , |
578 | i, soe->ts[i][0].sec, soe->ts[i][0].nsec); |
579 | printf(" phc time: %lld.%09u\n" , |
580 | soe->ts[i][1].sec, soe->ts[i][1].nsec); |
581 | printf(" system time after: %lld.%09u\n" , |
582 | soe->ts[i][2].sec, soe->ts[i][2].nsec); |
583 | } |
584 | } |
585 | |
586 | free(soe); |
587 | } |
588 | |
589 | if (getcross) { |
590 | xts = calloc(1, sizeof(*xts)); |
591 | if (!xts) { |
592 | perror("calloc" ); |
593 | return -1; |
594 | } |
595 | |
596 | if (ioctl(fd, PTP_SYS_OFFSET_PRECISE, xts)) { |
597 | perror("PTP_SYS_OFFSET_PRECISE" ); |
598 | } else { |
599 | puts("system and phc crosstimestamping request okay" ); |
600 | |
601 | printf("device time: %lld.%09u\n" , |
602 | xts->device.sec, xts->device.nsec); |
603 | printf("system time: %lld.%09u\n" , |
604 | xts->sys_realtime.sec, xts->sys_realtime.nsec); |
605 | printf("monoraw time: %lld.%09u\n" , |
606 | xts->sys_monoraw.sec, xts->sys_monoraw.nsec); |
607 | } |
608 | |
609 | free(xts); |
610 | } |
611 | |
612 | if (channel >= 0) { |
613 | if (ioctl(fd, PTP_MASK_CLEAR_ALL)) { |
614 | perror("PTP_MASK_CLEAR_ALL" ); |
615 | } else if (ioctl(fd, PTP_MASK_EN_SINGLE, (unsigned int *)&channel)) { |
616 | perror("PTP_MASK_EN_SINGLE" ); |
617 | } else { |
618 | printf("Channel %d exclusively enabled. Check on debugfs.\n" , channel); |
619 | printf("Press any key to continue\n." ); |
620 | getchar(); |
621 | } |
622 | } |
623 | |
624 | close(fd); |
625 | return 0; |
626 | } |
627 | |