diff --git a/utils/Makefile b/utils/Makefile new file mode 100644 index 000000000..a07f88d78 --- /dev/null +++ b/utils/Makefile @@ -0,0 +1,21 @@ + +PREFIX=/usr/local +BINDIR=$(DESTDIR)$(PREFIX)/bin + +CC=gcc +CFLAGS=-O3 -Wall + +BIN=perout + +all: $(BIN) + +perout: perout.c + $(CC) $(CFLAGS) $^ -o $@ + +install: + install -d $(BINDIR) + install -m 0755 $(BIN) $(BINDIR) + +clean: + rm -f $(BIN) + diff --git a/utils/perout.c b/utils/perout.c new file mode 100644 index 000000000..89dda754f --- /dev/null +++ b/utils/perout.c @@ -0,0 +1,259 @@ +/* + +Copyright 2019, The Regents of the University of California. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE REGENTS OF THE UNIVERSITY OF CALIFORNIA ''AS +IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE REGENTS OF THE UNIVERSITY OF CALIFORNIA OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of The Regents of the University of California. + +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifndef CLOCK_INVALID +#define CLOCK_INVALID -1 +#endif + +static clockid_t get_clockid(int fd) +{ +#define CLOCKFD 3 +#define FD_TO_CLOCKID(fd) ((~(clockid_t) (fd) << 3) | CLOCKFD) + + return FD_TO_CLOCKID(fd); +} + +#define NSEC_PER_SEC 1000000000 + +void ts_mod(int64_t *m_sec, int64_t *m_nsec, int64_t n_sec, int64_t n_nsec) +{ + int i = 0; + + // shift until larger + while (n_sec < *m_sec || (n_sec == *m_sec && n_nsec < *m_nsec)) + { + i++; + n_nsec <<= 1; + n_sec <<= 1; + if (n_nsec > NSEC_PER_SEC) + { + n_nsec -= NSEC_PER_SEC; + n_sec++; + } + } + + // subtract and shift back + while (i > 0) + { + i--; + if (n_sec & 1) + { + n_nsec += NSEC_PER_SEC; + } + n_nsec >>= 1; + n_sec >>= 1; + + if (n_sec < *m_sec || (n_sec == *m_sec && n_nsec < *m_nsec)) + { + *m_nsec -= n_nsec; + *m_sec -= n_sec; + + if (*m_nsec < 0) + { + *m_nsec += NSEC_PER_SEC; + (*m_sec)--; + } + } + } +} + +static void usage(char *name) +{ + fprintf(stderr, + "usage: %s [options]\n" + " -d name device to open\n" + " -s number start time (ns)\n" + " -p number period (ns)\n", + name); +} + +int main(int argc, char *argv[]) +{ + char *name; + int opt; + + char *device = NULL; + int ptp_fd; + clockid_t clkid; + + struct ptp_perout_request perout_request; + struct timespec ts; + + int64_t start_sec = 0; + int64_t start_nsec = 0; + int64_t period_sec = 0; + int64_t period_nsec = 0; + + name = strrchr(argv[0], '/'); + name = name ? 1+name : argv[0]; + + while ((opt = getopt(argc, argv, "d:s:p:h?")) != EOF) + { + switch (opt) + { + case 'd': + device = optarg; + break; + case 's': + start_nsec = atoll(optarg); + break; + case 'p': + period_nsec = atoll(optarg); + break; + case 'h': + case '?': + usage(name); + return 0; + default: + usage(name); + return -1; + } + } + + if (!device) + { + fprintf(stderr, "PTP device not specified\n"); + usage(name); + return -1; + } + + ptp_fd = open(device, O_RDWR); + if (ptp_fd < 0) + { + fprintf(stderr, "Failed to open %s: %s\n", device, strerror(errno)); + return -1; + } + + clkid = get_clockid(ptp_fd); + if (clkid == CLOCK_INVALID) + { + fprintf(stderr, "Failed to read clock id\n"); + close(ptp_fd); + return -1; + } + + if (period_nsec > 0) + { + if (clock_gettime(clkid, &ts)) + { + perror("Failed to read current time"); + return -1; + } + + // normalize start + start_sec = start_nsec / NSEC_PER_SEC; + start_nsec -= start_sec * NSEC_PER_SEC; + + // normalize period + period_sec = period_nsec / NSEC_PER_SEC; + period_nsec -= period_sec * NSEC_PER_SEC; + + printf("time %ld.%09ld\n", ts.tv_sec, ts.tv_nsec); + printf("start %ld.%09ld\n", start_sec, start_nsec); + printf("period %ld.%09ld\n", period_sec, period_nsec); + + if (start_sec < ts.tv_sec || (start_sec == ts.tv_sec && start_nsec < ts.tv_sec)) + { + // start time is in the past + + // modulo start with period + ts_mod(&start_sec, &start_nsec, period_sec, period_nsec); + + // align time with period + int64_t m_sec = ts.tv_sec; + int64_t m_nsec = ts.tv_nsec; + + ts_mod(&m_sec, &m_nsec, period_sec, period_nsec); + + // add current time and normalize + start_nsec += ts.tv_nsec; + start_sec += ts.tv_sec; + + if (start_nsec > NSEC_PER_SEC) + { + start_nsec -= NSEC_PER_SEC; + start_sec++; + } + + // subtract remainder + start_nsec -= m_nsec; + start_sec -= m_sec; + + // re-normalize + if (start_nsec < 0) + { + start_nsec += NSEC_PER_SEC; + start_sec--; + } + } + + printf("time %ld.%09ld\n", ts.tv_sec, ts.tv_nsec); + printf("start %ld.%09ld\n", start_sec, start_nsec); + printf("period %ld.%09ld\n", period_sec, period_nsec); + + memset(&perout_request, 0, sizeof(perout_request)); + perout_request.index = 0; + perout_request.start.sec = start_sec; + perout_request.start.nsec = start_nsec; + perout_request.period.sec = period_sec; + perout_request.period.nsec = period_nsec; + + if (ioctl(ptp_fd, PTP_PEROUT_REQUEST, &perout_request)) + { + perror("PTP_PEROUT_REQUEST ioctl failed"); + } + else + { + printf("PTP_PEROUT_REQUEST ioctl OK\n"); + } + } + + close(ptp_fd); + return 0; +} + + + +