petalinux/components/apps/canhwtstamp/timestamping.c

   1 /*
   2  * This program demonstrates how the various time stamping features in
   3  * the Linux kernel work. It receives CANbus frames and prints its timestamps.
   4  *
   5  * Incoming packets are time stamped with SO_TIMESTAMPING with or
   6  * without hardware support, SIOCGSTAMP[NS] (per-socket time stamp) and
   7  * SO_TIMESTAMP[NS].
   8  *
   9  * Copyright (C) 2009 Intel Corporation.
  10  * Author: Patrick Ohly <patrick.ohly@intel.com>
  11  *
  12  * From original ethernet PTP version in linux/Documentation/networking/timestamping/
  13  * modified by Martin Jerabek <jerabma7@fel.cvut.cz>
  14  *
  15  * This program is free software; you can redistribute it and/or modify it
  16  * under the terms and conditions of the GNU General Public License,
  17  * version 2, as published by the Free Software Foundation.
  18  *
  19  * This program is distributed in the hope it will be useful, but WITHOUT
  20  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  21  * FITNESS FOR A PARTICULAR PURPOSE. * See the GNU General Public License for
  22  * more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along with
  25  * this program; if not, write to the Free Software Foundation, Inc.,
  26  * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  27  */
  28
  29 #include <stdio.h>
  30 #include <stdlib.h>
  31 #include <errno.h>
  32 #include <string.h>
  33
  34 #include <sys/time.h>
  35 #include <sys/socket.h>
  36 #include <sys/select.h>
  37 #include <sys/ioctl.h>
  38 #include <net/if.h>
  39 #include <linux/can.h>
  40 #include <linux/can/raw.h>
  41 #include <unistd.h>
  42
  43 #include <asm/types.h>
  44 #include <linux/net_tstamp.h>
  45 #include <linux/errqueue.h>
  46
  47 #ifndef SO_TIMESTAMPING
  48 # define SO_TIMESTAMPING         37
  49 # define SCM_TIMESTAMPING        SO_TIMESTAMPING
  50 #endif
  51
  52 #ifndef SO_TIMESTAMPNS
  53 # define SO_TIMESTAMPNS 35
  54 #endif
  55
  56 #ifndef SIOCGSTAMPNS
  57 # define SIOCGSTAMPNS 0x8907
  58 #endif
  59
  60 #ifndef SIOCSHWTSTAMP
  61 # define SIOCSHWTSTAMP 0x89b0
  62 #endif
  63
  64 static void usage(const char *error)
  65 {
  66         if (error)
  67                 printf("invalid option: %s\n", error);
  68         printf("timestamping interface option*\n\n"
  69                "Options:\n"
  70                "  IP_MULTICAST_LOOP - looping outgoing multicasts\n"
  71                "  SO_TIMESTAMP - normal software time stamping, ms resolution\n"
  72                "  SO_TIMESTAMPNS - more accurate software time stamping\n"
  73                "  SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"
  74                "  SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"
  75                "  SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"
  76                "  SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"
  77                "  SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"
  78                "  SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"
  79                "  SIOCGSTAMP - check last socket time stamp\n"
  80                "  SIOCGSTAMPNS - more accurate socket time stamp\n");
  81         exit(1);
  82 }
  83
  84 static void bail(const char *error)
  85 {
  86         printf("%s: %s\n", error, strerror(errno));
  87         exit(1);
  88 }
  89 /*
  90 static void sendpacket(int sock)
  91 {
  92         struct timeval now;
  93         int res;
  94         struct can_frame frame;
  95
  96         memset(&frame, 0, sizeof(frame));
  97         frame.can_id = 0x88;
  98         frame.can_dlc = 2;
  99         frame.data[0] = 0xaa;
 100         frame.data[1] = 0xbb;
 101
 102         res = write(sock, &frame, sizeof(frame));
 103         gettimeofday(&now, 0);
 104         if (res < 0)
 105                 printf("%s: %s\n", "send", strerror(errno));
 106         else
 107                 printf("%ld.%06ld: sent %d bytes\n",
 108                        (long)now.tv_sec, (long)now.tv_usec,
 109                        res);
 110 }
 111 */
 112 static void printpacket(struct msghdr *msg, int res,
 113                         char *data,
 114                         int sock, int recvmsg_flags,
 115                         int siocgstamp, int siocgstampns)
 116 {
 117         struct cmsghdr *cmsg;
 118         struct timeval tv;
 119         struct timespec ts;
 120         struct timeval now;
 121
 122         gettimeofday(&now, 0);
 123
 124         printf("%ld.%06ld: received %s data, %d bytes, %zu bytes control messages\n",
 125                (long)now.tv_sec, (long)now.tv_usec,
 126                (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
 127                res,
 128                msg->msg_controllen);
 129         for (cmsg = CMSG_FIRSTHDR(msg);
 130              cmsg;
 131              cmsg = CMSG_NXTHDR(msg, cmsg)) {
 132                 printf("   cmsg len %zu: ", cmsg->cmsg_len);
 133                 switch (cmsg->cmsg_level) {
 134                 case SOL_SOCKET:
 135                         printf("SOL_SOCKET ");
 136                         switch (cmsg->cmsg_type) {
 137                         case SO_TIMESTAMP: {
 138                                 struct timeval *stamp =
 139                                         (struct timeval *)CMSG_DATA(cmsg);
 140                                 printf("SO_TIMESTAMP %ld.%06ld",
 141                                        (long)stamp->tv_sec,
 142                                        (long)stamp->tv_usec);
 143                                 break;
 144                         }
 145                         case SO_TIMESTAMPNS: {
 146                                 struct timespec *stamp =
 147                                         (struct timespec *)CMSG_DATA(cmsg);
 148                                 printf("SO_TIMESTAMPNS %ld.%09ld",
 149                                        (long)stamp->tv_sec,
 150                                        (long)stamp->tv_nsec);
 151                                 break;
 152                         }
 153                         case SO_TIMESTAMPING: {
 154                                 struct timespec *stamp =
 155                                         (struct timespec *)CMSG_DATA(cmsg);
 156                                 printf("SO_TIMESTAMPING ");
 157                                 printf("SW %ld.%09ld ",
 158                                        (long)stamp->tv_sec,
 159                                        (long)stamp->tv_nsec);
 160                                 stamp++;
 161                                 /* skip deprecated HW transformed */
 162                                 printf("HW transformed %ld.%09ld",
 163                                        (long)stamp->tv_sec,
 164                                        (long)stamp->tv_nsec);
 165                                 stamp++;
 166                                 printf("HW raw %ld.%09ld",
 167                                        (long)stamp->tv_sec,
 168                                        (long)stamp->tv_nsec);
 169                                 break;
 170                         }
 171                         default:
 172                                 printf("type %d", cmsg->cmsg_type);
 173                                 break;
 174                         }
 175                         break;
 176                 default:
 177                         printf("level %d type %d",
 178                                 cmsg->cmsg_level,
 179                                 cmsg->cmsg_type);
 180                         break;
 181                 }
 182                 printf("\n");
 183         }
 184
 185         if (siocgstamp) {
 186                 if (ioctl(sock, SIOCGSTAMP, &tv))
 187                         printf("   %s: %s\n", "SIOCGSTAMP", strerror(errno));
 188                 else
 189                         printf("SIOCGSTAMP %ld.%06ld\n",
 190                                (long)tv.tv_sec,
 191                                (long)tv.tv_usec);
 192         }
 193         if (siocgstampns) {
 194                 if (ioctl(sock, SIOCGSTAMPNS, &ts))
 195                         printf("   %s: %s\n", "SIOCGSTAMPNS", strerror(errno));
 196                 else
 197                         printf("SIOCGSTAMPNS %ld.%09ld\n",
 198                                (long)ts.tv_sec,
 199                                (long)ts.tv_nsec);
 200         }
 201 }
 202
 203 static void recvpacket(int sock, int recvmsg_flags,
 204                        int siocgstamp, int siocgstampns)
 205 {
 206         char data[256];
 207         struct sockaddr_can from_addr;
 208         struct msghdr msg;
 209         struct iovec entry;
 210         struct {
 211                 struct cmsghdr cm;
 212                 char control[512];
 213         } control;
 214         int res;
 215
 216         memset(&msg, 0, sizeof(msg));
 217         msg.msg_iov = &entry;
 218         msg.msg_iovlen = 1;
 219         entry.iov_base = data;
 220         entry.iov_len = sizeof(data);
 221         msg.msg_name = (caddr_t)&from_addr;
 222         msg.msg_namelen = sizeof(from_addr);
 223         msg.msg_control = &control;
 224         msg.msg_controllen = sizeof(control);
 225
 226         res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);
 227         if (res < 0) {
 228                 printf("%s %s: %s\n",
 229                        "recvmsg",
 230                        (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
 231                        strerror(errno));
 232         } else {
 233                 printpacket(&msg, res, data,
 234                             sock, recvmsg_flags,
 235                             siocgstamp, siocgstampns);
 236         }
 237 }
 238
 239 int main(int argc, char **argv)
 240 {
 241         int so_timestamping_flags = 0;
 242         int so_timestamp = 0;
 243         int so_timestampns = 0;
 244         int siocgstamp = 0;
 245         int siocgstampns = 0;
 246         char *interface;
 247         int i;
 248         int enabled = 1;
 249         int sock;
 250         struct ifreq device;
 251         struct ifreq hwtstamp;
 252         struct hwtstamp_config hwconfig, hwconfig_requested;
 253         struct sockaddr_can addr;
 254         int val;
 255         socklen_t len;
 256         struct timeval next;
 257
 258         if (argc < 2)
 259                 usage(0);
 260         interface = argv[1];
 261
 262         for (i = 2; i < argc; i++) {
 263                 if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
 264                         so_timestamp = 1;
 265                 else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
 266                         so_timestampns = 1;
 267                 else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
 268                         siocgstamp = 1;
 269                 else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
 270                         siocgstampns = 1;
 271                 else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
 272                 {
 273
 274                 }
 275                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
 276                         so_timestamping_flags |= SOF_TIMESTAMPING_TX_HARDWARE;
 277                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
 278                         so_timestamping_flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
 279                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
 280                         so_timestamping_flags |= SOF_TIMESTAMPING_RX_HARDWARE;
 281                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
 282                         so_timestamping_flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
 283                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
 284                         so_timestamping_flags |= SOF_TIMESTAMPING_SOFTWARE;
 285                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
 286                         so_timestamping_flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
 287                 else
 288                         usage(argv[i]);
 289         }
 290
 291         sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 292         if (sock < 0)
 293                 bail("socket");
 294
 295         memset(&device, 0, sizeof(device));
 296         strncpy(device.ifr_name, interface, sizeof(device.ifr_name));
 297         if (ioctl(sock, SIOCGIFINDEX, &device) < 0)
 298                 bail("getting interface index");
 299
 300         memset(&hwtstamp, 0, sizeof(hwtstamp));
 301         strncpy(hwtstamp.ifr_name, interface, sizeof(hwtstamp.ifr_name));
 302         hwtstamp.ifr_data = (void *)&hwconfig;
 303         memset(&hwconfig, 0, sizeof(hwconfig));
 304         hwconfig.tx_type =
 305                 (so_timestamping_flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
 306                 HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
 307         hwconfig.rx_filter =
 308                 (so_timestamping_flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
 309                 HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
 310         hwconfig_requested = hwconfig;
 311         if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
 312                 if ((errno == EINVAL || errno == ENOTSUP) &&
 313                     hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
 314                     hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
 315                         printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
 316                 else
 317                         bail("SIOCSHWTSTAMP");
 318         }
 319         printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
 320                hwconfig_requested.tx_type, hwconfig.tx_type,
 321                hwconfig_requested.rx_filter, hwconfig.rx_filter);
 322
 323         /* bind to PTP port */
 324         addr.can_family = AF_CAN;
 325         addr.can_ifindex = device.ifr_ifindex;
 326         if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0)
 327                 bail("bind");
 328
 329         /* set socket options for time stamping */
 330         if (so_timestamp &&
 331                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
 332                            &enabled, sizeof(enabled)) < 0)
 333                 bail("setsockopt SO_TIMESTAMP");
 334
 335         if (so_timestampns &&
 336                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
 337                            &enabled, sizeof(enabled)) < 0)
 338                 bail("setsockopt SO_TIMESTAMPNS");
 339
 340         if (so_timestamping_flags &&
 341                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING,
 342                            &so_timestamping_flags,
 343                            sizeof(so_timestamping_flags)) < 0)
 344                 bail("setsockopt SO_TIMESTAMPING");
 345
 346         /* verify socket options */
 347         len = sizeof(val);
 348         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
 349                 printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
 350         else
 351                 printf("SO_TIMESTAMP %d\n", val);
 352
 353         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
 354                 printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
 355                        strerror(errno));
 356         else
 357                 printf("SO_TIMESTAMPNS %d\n", val);
 358
 359         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, &len) < 0) {
 360                 printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
 361                        strerror(errno));
 362         } else {
 363                 printf("SO_TIMESTAMPING %d\n", val);
 364                 if (val != so_timestamping_flags)
 365                         printf("   not the expected value %d\n",
 366                                so_timestamping_flags);
 367         }
 368
 369         /* send packets forever every five seconds */
 370         gettimeofday(&next, 0);
 371         next.tv_sec = (next.tv_sec + 1) / 5 * 5;
 372         next.tv_usec = 0;
 373         while (1) {
 374                 struct timeval now;
 375                 struct timeval delta;
 376                 long delta_us;
 377                 int res;
 378                 fd_set readfs, errorfs;
 379
 380                 gettimeofday(&now, 0);
 381                 delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
 382                         (long)(next.tv_usec - now.tv_usec);
 383                 if (delta_us > 0) {
 384                         /* continue waiting for timeout or data */
 385                         delta.tv_sec = delta_us / 1000000;
 386                         delta.tv_usec = delta_us % 1000000;
 387
 388                         FD_ZERO(&readfs);
 389                         FD_ZERO(&errorfs);
 390                         FD_SET(sock, &readfs);
 391                         FD_SET(sock, &errorfs);
 392                         printf("%ld.%06ld: select %ldus\n",
 393                                (long)now.tv_sec, (long)now.tv_usec,
 394                                delta_us);
 395                         res = select(sock + 1, &readfs, 0, &errorfs, &delta);
 396                         gettimeofday(&now, 0);
 397                         printf("%ld.%06ld: select returned: %d, %s\n",
 398                                (long)now.tv_sec, (long)now.tv_usec,
 399                                res,
 400                                res < 0 ? strerror(errno) : "success");
 401                         if (res > 0) {
 402                                 if (FD_ISSET(sock, &readfs))
 403                                         printf("ready for reading\n");
 404                                 if (FD_ISSET(sock, &errorfs))
 405                                         printf("has error\n");
 406                                 recvpacket(sock, 0,
 407                                            siocgstamp,
 408                                            siocgstampns);
 409                                 /*recvpacket(sock, MSG_ERRQUEUE,
 410                                            siocgstamp,
 411                                            siocgstampns);*/
 412                         }
 413                 } else {
 414                         /* write one packet */
 415                         //sendpacket(sock);
 416                         next.tv_sec += 5;
 417                         continue;
 418                 }
 419         }
 420
 421         return 0;
 422 }