IEEE1588-2008 实现代码(linux)

/** @file ptp_packet.c * PTP packet interface for Linux. */ /* Openptp is an open source PTP version 2 (IEEE 1588-2008) daemon. Copyright (C) 2007-2009 Flexibilis Oy This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License version 2 as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /****************************************************************************** * $Id$ ******************************************************************************/ #include <asm/socket.h> #include <netinet/in.h> #include <sys/ioctl.h> #include <net/if.h> #include <unistd.h> #include <fcntl.h> #include <sys/select.h> #include <sys/time.h> #include <sys/types.h> #include <packet_if.h> #include <os_if.h> #include <ptp_general.h> #include <ptp_message.h> #include <ptp_config.h> #include <ptp_internal.h> #include <ptp.h> /** * Interface data. */ struct linux_if_interface { char if_name[IFNAMSIZ]; int if_index; struct in_addr if_addr; ///< local IP address int unicast_entry; ///< set to 1 if unicast destination struct in_addr net_addr; ///< destination IP addr u8 hw_addr[IFHWADDRLEN]; }; /** * Holds socket etc. data. */ struct linux_packet_if { int event_sock; int gen_sock; int num_interfaces; struct linux_if_interface interfaces[MAX_NUM_INTERFACES]; }; static struct linux_packet_if packet_if_data; // function for searching interfaces to use static int locate_interfaces(struct linux_packet_if *pif); // function for receiving PTP message from socket static int ptp_receive_msg(struct linux_packet_if *pif, int sock, int *if_index, char *frame, int *length, struct Timestamp *recv_time); // interface location static struct linux_if_interface *get_interface(struct linux_packet_if *pif, int *if_index, char *if_name, int *port_num); // Helpers for port id vs. hw address static void create_clock_id(ClockIdentity clk_id, u8 * hwaddr); // if_num port_num conversions static int if_num_to_port_num(int if_num); //static int port_num_to_if_num( int port_num ); static int if_configured(char *if_name); // Local macros #define MIN(a,b) ((a)<(b)?(a):(b)) #ifndef SO_TIMESTAMPNS #warning "nanosecond support disabled" #endif /** * Function for initializing packet interface. * @param ctx packet if context * @return ptp error code. */ int ptp_initialize_packet_if(struct packet_ctx *ctx) { struct linux_packet_if *pif = &packet_if_data; struct sockaddr_in saddr; struct ip_mreqn ip_mreq; int tmp = 0, ret = 0; ClockIdentity identity; int if_num = 0; memset(&packet_if_data, 0, sizeof(struct linux_packet_if)); // Store internal data ctx->arg = pif; // Open sockets pif->event_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); if (pif->event_sock == 0) { perror("socket"); ERROR("\n"); return PTP_ERR_NET; } pif->gen_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); if (pif->gen_sock == 0) { perror("socket"); ERROR("\n"); return PTP_ERR_NET; } #if 0 tmp = 1; // allow address reuse if (setsockopt(pif->event_sock, SOL_SOCKET, SO_REUSEADDR, &tmp, sizeof(int)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } if (setsockopt(pif->gen_sock, SOL_SOCKET, SO_REUSEADDR, &tmp, sizeof(int)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } #endif // Get list of interfaces ret = locate_interfaces(pif); if (ret != PTP_ERR_OK) { ERROR("No interfaces found\n"); return ret; } if (pif->num_interfaces == 0) { ERROR("No interfaces found\n"); return PTP_ERR_NET; } // Create clock id (HW address of the first applicable interface) create_clock_id(identity, pif->interfaces[0].hw_addr); // Bind sockets saddr.sin_family = AF_INET; saddr.sin_addr.s_addr = htonl(INADDR_ANY); // needed to get multicast in saddr.sin_port = htons(DEFAULT_EVENT_PORT); DEBUG("Bind %s:%i\n", inet_ntoa(saddr.sin_addr), ntohs(saddr.sin_port)); if (bind(pif->event_sock, (struct sockaddr *) &saddr, sizeof(struct sockaddr_in)) != 0) { perror("bind"); ERROR("\n"); return PTP_ERR_NET; } saddr.sin_port = htons(DEFAULT_GENERAL_PORT); DEBUG("Bind %s:%i\n", inet_ntoa(saddr.sin_addr), ntohs(saddr.sin_port)); if (bind(pif->gen_sock, (struct sockaddr *) &saddr, sizeof(struct sockaddr_in)) != 0) { perror("bind"); ERROR("\n"); return PTP_ERR_NET; } // initialize all usable interfaces for (if_num = 0; if_num < pif->num_interfaces; if_num++) { DEBUG("DST %s\n", inet_ntoa(pif->interfaces[if_num].net_addr)); if (pif->interfaces[if_num].unicast_entry == 0) { // Set socket options // Set multicast options // add multicast if ip_mreq.imr_multiaddr.s_addr = pif->interfaces[if_num].net_addr.s_addr; ip_mreq.imr_address.s_addr = pif->interfaces[if_num].if_addr.s_addr; ip_mreq.imr_ifindex = pif->interfaces[if_num].if_index; DEBUG("Local %s:%i\n", inet_ntoa(pif->interfaces[if_num].if_addr), pif->interfaces[if_num].if_index); DEBUG("Group %s\n", inet_ntoa(pif->interfaces[if_num].net_addr)); if (setsockopt (pif->event_sock, IPPROTO_IP, IP_MULTICAST_IF, &ip_mreq, sizeof(struct ip_mreqn)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } if (setsockopt(pif->gen_sock, IPPROTO_IP, IP_MULTICAST_IF, &ip_mreq, sizeof(struct ip_mreqn)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } if (setsockopt(pif->event_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &ip_mreq, sizeof(struct ip_mreqn)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } if (setsockopt(pif->gen_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &ip_mreq, sizeof(struct ip_mreqn)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } } } tmp = 1; // set multicast TTL to 1 if (setsockopt(pif->event_sock, IPPROTO_IP, IP_MULTICAST_TTL, &tmp, sizeof(int)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } if (setsockopt(pif->gen_sock, IPPROTO_IP, IP_MULTICAST_TTL, &tmp, sizeof(int)) != 0) { perror("setsockopt"); ERROR("\n"); return PTP_ERR_NET; } if (ptp_cfg.custom_clk_if == 1) { tmp = 0; // disable multicast loopback if (setsockopt(pif->event_sock, IPPROTO_IP, IP_MULTICAST_LOOP, &tmp, sizeof(int)) != 0) { per