]>
Commit | Line | Data |
---|---|---|
d94ba80e RC |
1 | /* |
2 | * PTP 1588 clock support - User space test program | |
3 | * | |
4 | * Copyright (C) 2010 OMICRON electronics GmbH | |
5 | * | |
6 | * This program is free software; you can redistribute it and/or modify | |
7 | * it under the terms of the GNU General Public License as published by | |
8 | * the Free Software Foundation; either version 2 of the License, or | |
9 | * (at your option) any later version. | |
10 | * | |
11 | * This program is distributed in the hope that it will be useful, | |
12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
14 | * GNU General Public License for more details. | |
15 | * | |
16 | * You should have received a copy of the GNU General Public License | |
17 | * along with this program; if not, write to the Free Software | |
18 | * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. | |
19 | */ | |
20 | #include <errno.h> | |
21 | #include <fcntl.h> | |
22 | #include <math.h> | |
23 | #include <signal.h> | |
24 | #include <stdio.h> | |
25 | #include <stdlib.h> | |
26 | #include <string.h> | |
27 | #include <sys/ioctl.h> | |
28 | #include <sys/mman.h> | |
29 | #include <sys/stat.h> | |
30 | #include <sys/time.h> | |
31 | #include <sys/timex.h> | |
32 | #include <sys/types.h> | |
33 | #include <time.h> | |
34 | #include <unistd.h> | |
35 | ||
36 | #include <linux/ptp_clock.h> | |
37 | ||
38 | #define DEVICE "/dev/ptp0" | |
39 | ||
40 | #ifndef ADJ_SETOFFSET | |
41 | #define ADJ_SETOFFSET 0x0100 | |
42 | #endif | |
43 | ||
44 | #ifndef CLOCK_INVALID | |
45 | #define CLOCK_INVALID -1 | |
46 | #endif | |
47 | ||
48 | /* When glibc offers the syscall, this will go away. */ | |
49 | #include <sys/syscall.h> | |
50 | static int clock_adjtime(clockid_t id, struct timex *tx) | |
51 | { | |
52 | return syscall(__NR_clock_adjtime, id, tx); | |
53 | } | |
54 | ||
55 | static clockid_t get_clockid(int fd) | |
56 | { | |
57 | #define CLOCKFD 3 | |
58 | #define FD_TO_CLOCKID(fd) ((~(clockid_t) (fd) << 3) | CLOCKFD) | |
59 | ||
60 | return FD_TO_CLOCKID(fd); | |
61 | } | |
62 | ||
63 | static void handle_alarm(int s) | |
64 | { | |
65 | printf("received signal %d\n", s); | |
66 | } | |
67 | ||
68 | static int install_handler(int signum, void (*handler)(int)) | |
69 | { | |
70 | struct sigaction action; | |
71 | sigset_t mask; | |
72 | ||
73 | /* Unblock the signal. */ | |
74 | sigemptyset(&mask); | |
75 | sigaddset(&mask, signum); | |
76 | sigprocmask(SIG_UNBLOCK, &mask, NULL); | |
77 | ||
78 | /* Install the signal handler. */ | |
79 | action.sa_handler = handler; | |
80 | action.sa_flags = 0; | |
81 | sigemptyset(&action.sa_mask); | |
82 | sigaction(signum, &action, NULL); | |
83 | ||
84 | return 0; | |
85 | } | |
86 | ||
87 | static long ppb_to_scaled_ppm(int ppb) | |
88 | { | |
89 | /* | |
90 | * The 'freq' field in the 'struct timex' is in parts per | |
91 | * million, but with a 16 bit binary fractional field. | |
92 | * Instead of calculating either one of | |
93 | * | |
94 | * scaled_ppm = (ppb / 1000) << 16 [1] | |
95 | * scaled_ppm = (ppb << 16) / 1000 [2] | |
96 | * | |
97 | * we simply use double precision math, in order to avoid the | |
98 | * truncation in [1] and the possible overflow in [2]. | |
99 | */ | |
100 | return (long) (ppb * 65.536); | |
101 | } | |
102 | ||
103 | static void usage(char *progname) | |
104 | { | |
105 | fprintf(stderr, | |
106 | "usage: %s [options]\n" | |
107 | " -a val request a one-shot alarm after 'val' seconds\n" | |
108 | " -A val request a periodic alarm every 'val' seconds\n" | |
109 | " -c query the ptp clock's capabilities\n" | |
110 | " -d name device to open\n" | |
111 | " -e val read 'val' external time stamp events\n" | |
112 | " -f val adjust the ptp clock frequency by 'val' ppb\n" | |
113 | " -g get the ptp clock time\n" | |
114 | " -h prints this message\n" | |
115 | " -p val enable output with a period of 'val' nanoseconds\n" | |
116 | " -P val enable or disable (val=1|0) the system clock PPS\n" | |
117 | " -s set the ptp clock time from the system time\n" | |
118 | " -S set the system time from the ptp clock time\n" | |
119 | " -t val shift the ptp clock time by 'val' seconds\n", | |
120 | progname); | |
121 | } | |
122 | ||
123 | int main(int argc, char *argv[]) | |
124 | { | |
125 | struct ptp_clock_caps caps; | |
126 | struct ptp_extts_event event; | |
127 | struct ptp_extts_request extts_request; | |
128 | struct ptp_perout_request perout_request; | |
129 | struct timespec ts; | |
130 | struct timex tx; | |
131 | ||
132 | static timer_t timerid; | |
133 | struct itimerspec timeout; | |
134 | struct sigevent sigevent; | |
135 | ||
136 | char *progname; | |
137 | int c, cnt, fd; | |
138 | ||
139 | char *device = DEVICE; | |
140 | clockid_t clkid; | |
141 | int adjfreq = 0x7fffffff; | |
142 | int adjtime = 0; | |
143 | int capabilities = 0; | |
144 | int extts = 0; | |
145 | int gettime = 0; | |
146 | int oneshot = 0; | |
147 | int periodic = 0; | |
148 | int perout = -1; | |
149 | int pps = -1; | |
150 | int settime = 0; | |
151 | ||
152 | progname = strrchr(argv[0], '/'); | |
153 | progname = progname ? 1+progname : argv[0]; | |
154 | while (EOF != (c = getopt(argc, argv, "a:A:cd:e:f:ghp:P:sSt:v"))) { | |
155 | switch (c) { | |
156 | case 'a': | |
157 | oneshot = atoi(optarg); | |
158 | break; | |
159 | case 'A': | |
160 | periodic = atoi(optarg); | |
161 | break; | |
162 | case 'c': | |
163 | capabilities = 1; | |
164 | break; | |
165 | case 'd': | |
166 | device = optarg; | |
167 | break; | |
168 | case 'e': | |
169 | extts = atoi(optarg); | |
170 | break; | |
171 | case 'f': | |
172 | adjfreq = atoi(optarg); | |
173 | break; | |
174 | case 'g': | |
175 | gettime = 1; | |
176 | break; | |
177 | case 'p': | |
178 | perout = atoi(optarg); | |
179 | break; | |
180 | case 'P': | |
181 | pps = atoi(optarg); | |
182 | break; | |
183 | case 's': | |
184 | settime = 1; | |
185 | break; | |
186 | case 'S': | |
187 | settime = 2; | |
188 | break; | |
189 | case 't': | |
190 | adjtime = atoi(optarg); | |
191 | break; | |
192 | case 'h': | |
193 | usage(progname); | |
194 | return 0; | |
195 | case '?': | |
196 | default: | |
197 | usage(progname); | |
198 | return -1; | |
199 | } | |
200 | } | |
201 | ||
202 | fd = open(device, O_RDWR); | |
203 | if (fd < 0) { | |
204 | fprintf(stderr, "opening %s: %s\n", device, strerror(errno)); | |
205 | return -1; | |
206 | } | |
207 | ||
208 | clkid = get_clockid(fd); | |
209 | if (CLOCK_INVALID == clkid) { | |
210 | fprintf(stderr, "failed to read clock id\n"); | |
211 | return -1; | |
212 | } | |
213 | ||
214 | if (capabilities) { | |
215 | if (ioctl(fd, PTP_CLOCK_GETCAPS, &caps)) { | |
216 | perror("PTP_CLOCK_GETCAPS"); | |
217 | } else { | |
218 | printf("capabilities:\n" | |
219 | " %d maximum frequency adjustment (ppb)\n" | |
220 | " %d programmable alarms\n" | |
221 | " %d external time stamp channels\n" | |
222 | " %d programmable periodic signals\n" | |
223 | " %d pulse per second\n", | |
224 | caps.max_adj, | |
225 | caps.n_alarm, | |
226 | caps.n_ext_ts, | |
227 | caps.n_per_out, | |
228 | caps.pps); | |
229 | } | |
230 | } | |
231 | ||
232 | if (0x7fffffff != adjfreq) { | |
233 | memset(&tx, 0, sizeof(tx)); | |
234 | tx.modes = ADJ_FREQUENCY; | |
235 | tx.freq = ppb_to_scaled_ppm(adjfreq); | |
236 | if (clock_adjtime(clkid, &tx)) { | |
237 | perror("clock_adjtime"); | |
238 | } else { | |
239 | puts("frequency adjustment okay"); | |
240 | } | |
241 | } | |
242 | ||
243 | if (adjtime) { | |
244 | memset(&tx, 0, sizeof(tx)); | |
245 | tx.modes = ADJ_SETOFFSET; | |
246 | tx.time.tv_sec = adjtime; | |
247 | tx.time.tv_usec = 0; | |
248 | if (clock_adjtime(clkid, &tx) < 0) { | |
249 | perror("clock_adjtime"); | |
250 | } else { | |
251 | puts("time shift okay"); | |
252 | } | |
253 | } | |
254 | ||
255 | if (gettime) { | |
256 | if (clock_gettime(clkid, &ts)) { | |
257 | perror("clock_gettime"); | |
258 | } else { | |
259 | printf("clock time: %ld.%09ld or %s", | |
260 | ts.tv_sec, ts.tv_nsec, ctime(&ts.tv_sec)); | |
261 | } | |
262 | } | |
263 | ||
264 | if (settime == 1) { | |
265 | clock_gettime(CLOCK_REALTIME, &ts); | |
266 | if (clock_settime(clkid, &ts)) { | |
267 | perror("clock_settime"); | |
268 | } else { | |
269 | puts("set time okay"); | |
270 | } | |
271 | } | |
272 | ||
273 | if (settime == 2) { | |
274 | clock_gettime(clkid, &ts); | |
275 | if (clock_settime(CLOCK_REALTIME, &ts)) { | |
276 | perror("clock_settime"); | |
277 | } else { | |
278 | puts("set time okay"); | |
279 | } | |
280 | } | |
281 | ||
282 | if (extts) { | |
283 | memset(&extts_request, 0, sizeof(extts_request)); | |
284 | extts_request.index = 0; | |
285 | extts_request.flags = PTP_ENABLE_FEATURE; | |
286 | if (ioctl(fd, PTP_EXTTS_REQUEST, &extts_request)) { | |
287 | perror("PTP_EXTTS_REQUEST"); | |
288 | extts = 0; | |
289 | } else { | |
290 | puts("external time stamp request okay"); | |
291 | } | |
292 | for (; extts; extts--) { | |
293 | cnt = read(fd, &event, sizeof(event)); | |
294 | if (cnt != sizeof(event)) { | |
295 | perror("read"); | |
296 | break; | |
297 | } | |
298 | printf("event index %u at %lld.%09u\n", event.index, | |
299 | event.t.sec, event.t.nsec); | |
300 | fflush(stdout); | |
301 | } | |
302 | /* Disable the feature again. */ | |
303 | extts_request.flags = 0; | |
304 | if (ioctl(fd, PTP_EXTTS_REQUEST, &extts_request)) { | |
305 | perror("PTP_EXTTS_REQUEST"); | |
306 | } | |
307 | } | |
308 | ||
309 | if (oneshot) { | |
310 | install_handler(SIGALRM, handle_alarm); | |
311 | /* Create a timer. */ | |
312 | sigevent.sigev_notify = SIGEV_SIGNAL; | |
313 | sigevent.sigev_signo = SIGALRM; | |
314 | if (timer_create(clkid, &sigevent, &timerid)) { | |
315 | perror("timer_create"); | |
316 | return -1; | |
317 | } | |
318 | /* Start the timer. */ | |
319 | memset(&timeout, 0, sizeof(timeout)); | |
320 | timeout.it_value.tv_sec = oneshot; | |
321 | if (timer_settime(timerid, 0, &timeout, NULL)) { | |
322 | perror("timer_settime"); | |
323 | return -1; | |
324 | } | |
325 | pause(); | |
326 | timer_delete(timerid); | |
327 | } | |
328 | ||
329 | if (periodic) { | |
330 | install_handler(SIGALRM, handle_alarm); | |
331 | /* Create a timer. */ | |
332 | sigevent.sigev_notify = SIGEV_SIGNAL; | |
333 | sigevent.sigev_signo = SIGALRM; | |
334 | if (timer_create(clkid, &sigevent, &timerid)) { | |
335 | perror("timer_create"); | |
336 | return -1; | |
337 | } | |
338 | /* Start the timer. */ | |
339 | memset(&timeout, 0, sizeof(timeout)); | |
340 | timeout.it_interval.tv_sec = periodic; | |
341 | timeout.it_value.tv_sec = periodic; | |
342 | if (timer_settime(timerid, 0, &timeout, NULL)) { | |
343 | perror("timer_settime"); | |
344 | return -1; | |
345 | } | |
346 | while (1) { | |
347 | pause(); | |
348 | } | |
349 | timer_delete(timerid); | |
350 | } | |
351 | ||
352 | if (perout >= 0) { | |
353 | if (clock_gettime(clkid, &ts)) { | |
354 | perror("clock_gettime"); | |
355 | return -1; | |
356 | } | |
357 | memset(&perout_request, 0, sizeof(perout_request)); | |
358 | perout_request.index = 0; | |
359 | perout_request.start.sec = ts.tv_sec + 2; | |
360 | perout_request.start.nsec = 0; | |
361 | perout_request.period.sec = 0; | |
362 | perout_request.period.nsec = perout; | |
363 | if (ioctl(fd, PTP_PEROUT_REQUEST, &perout_request)) { | |
364 | perror("PTP_PEROUT_REQUEST"); | |
365 | } else { | |
366 | puts("periodic output request okay"); | |
367 | } | |
368 | } | |
369 | ||
370 | if (pps != -1) { | |
371 | int enable = pps ? 1 : 0; | |
372 | if (ioctl(fd, PTP_ENABLE_PPS, enable)) { | |
373 | perror("PTP_ENABLE_PPS"); | |
374 | } else { | |
375 | puts("pps for system time request okay"); | |
376 | } | |
377 | } | |
378 | ||
379 | close(fd); | |
380 | return 0; | |
381 | } |