| /* |
| * Copyright (c) 2024 BayLibre SAS |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #include <zephyr/logging/log.h> |
| LOG_MODULE_REGISTER(ptp_transport, CONFIG_PTP_LOG_LEVEL); |
| |
| #include <zephyr/kernel.h> |
| |
| #include <zephyr/net/socket.h> |
| |
| #include "transport.h" |
| |
| #define INTERFACE_NAME_LEN (32) |
| |
| #if CONFIG_PTP_UDP_IPv4_PROTOCOL |
| static struct in_addr mcast_addr = {{{224, 0, 1, 129}}}; |
| #elif CONFIG_PTP_UDP_IPv6_PROTOCOL |
| static struct in6_addr mcast_addr = {{{0xff, 0xe, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x81}}}; |
| #else |
| #error "Chosen PTP transport protocol not implemented" |
| #endif |
| |
| static int transport_socket_open(struct net_if *iface, struct sockaddr *addr) |
| { |
| static const int feature_on = 1; |
| static const uint8_t priority = NET_PRIORITY_CA; |
| static const uint8_t ts_mask = SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE; |
| struct ifreq ifreq = { 0 }; |
| int cnt; |
| int socket = zsock_socket(addr->sa_family, SOCK_DGRAM, IPPROTO_UDP); |
| |
| if (net_if_get_by_iface(iface) < 0) { |
| LOG_ERR("Failed to obtain interface index"); |
| return -1; |
| } |
| |
| if (socket < 0) { |
| return -1; |
| } |
| |
| if (zsock_setsockopt(socket, SOL_SOCKET, SO_REUSEADDR, &feature_on, sizeof(feature_on))) { |
| LOG_ERR("Failed to set SO_REUSEADDR"); |
| goto error; |
| } |
| |
| if (zsock_bind(socket, addr, sizeof(*addr))) { |
| LOG_ERR("Failed to bind socket"); |
| goto error; |
| } |
| |
| cnt = net_if_get_name(iface, ifreq.ifr_name, INTERFACE_NAME_LEN); |
| if (cnt > 0 && zsock_setsockopt(socket, |
| SOL_SOCKET, |
| SO_BINDTODEVICE, |
| ifreq.ifr_name, |
| sizeof(ifreq.ifr_name))) { |
| LOG_ERR("Failed to set socket binding to an interface"); |
| goto error; |
| } |
| |
| if (zsock_setsockopt(socket, SOL_SOCKET, SO_TIMESTAMPING, &ts_mask, sizeof(ts_mask))) { |
| LOG_ERR("Failed to set SO_TIMESTAMPING"); |
| goto error; |
| } |
| |
| if (zsock_setsockopt(socket, SOL_SOCKET, SO_PRIORITY, &priority, sizeof(priority))) { |
| LOG_ERR("Failed to set SO_PRIORITY"); |
| goto error; |
| } |
| |
| return socket; |
| error: |
| zsock_close(socket); |
| return -1; |
| } |
| |
| static int transport_join_multicast(struct ptp_port *port) |
| { |
| if (IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL)) { |
| struct ip_mreqn mreqn = {0}; |
| |
| memcpy(&mreqn.imr_multiaddr, &mcast_addr, sizeof(struct in_addr)); |
| mreqn.imr_ifindex = net_if_get_by_iface(port->iface); |
| |
| zsock_setsockopt(port->socket[1], IPPROTO_IP, |
| IP_ADD_MEMBERSHIP, &mreqn, sizeof(mreqn)); |
| } else { |
| struct ipv6_mreq mreqn = {0}; |
| |
| memcpy(&mreqn.ipv6mr_multiaddr, &mcast_addr, sizeof(struct in6_addr)); |
| mreqn.ipv6mr_ifindex = net_if_get_by_iface(port->iface); |
| |
| zsock_setsockopt(port->socket[0], IPPROTO_IPV6, |
| IPV6_ADD_MEMBERSHIP, &mreqn, sizeof(mreqn)); |
| } |
| |
| return 0; |
| } |
| |
| static int transport_udp_ipv4_open(struct net_if *iface, uint16_t port) |
| { |
| uint8_t tos; |
| socklen_t length; |
| int socket, ttl = 1; |
| struct sockaddr_in addr = { |
| .sin_family = AF_INET, |
| .sin_addr = INADDR_ANY_INIT, |
| .sin_port = htons(port), |
| }; |
| |
| socket = transport_socket_open(iface, (struct sockaddr *)&addr); |
| if (socket < 0) { |
| return -1; |
| } |
| |
| if (zsock_setsockopt(socket, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl))) { |
| LOG_ERR("Failed to set ip multicast ttl socket option"); |
| goto error; |
| } |
| |
| if (zsock_getsockopt(socket, IPPROTO_IP, IP_TOS, &tos, &length)) { |
| tos = 0; |
| } |
| |
| tos &= ~0xFC; |
| tos |= CONFIG_PTP_DSCP_VALUE << 2; |
| length = sizeof(tos); |
| |
| if (zsock_setsockopt(socket, IPPROTO_IP, IP_TOS, &tos, length)) { |
| LOG_WRN("Failed to set DSCP priority"); |
| } |
| |
| return socket; |
| error: |
| zsock_close(socket); |
| return -1; |
| } |
| |
| static int transport_udp_ipv6_open(struct net_if *iface, uint16_t port) |
| { |
| uint8_t tclass; |
| socklen_t length; |
| int socket, hops = 1, feature_on = 1; |
| struct sockaddr_in6 addr = { |
| .sin6_family = AF_INET6, |
| .sin6_addr = IN6ADDR_ANY_INIT, |
| .sin6_port = htons(port) |
| }; |
| |
| socket = transport_socket_open(iface, (struct sockaddr *)&addr); |
| if (socket < 0) { |
| return -1; |
| } |
| |
| if (zsock_setsockopt(socket, |
| IPPROTO_IPV6, |
| IPV6_RECVPKTINFO, |
| &feature_on, |
| sizeof(feature_on))) { |
| LOG_ERR("Failed to set IPV6_RECVPKTINFO"); |
| goto error; |
| } |
| |
| if (zsock_setsockopt(socket, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &hops, sizeof(hops))) { |
| LOG_ERR("Failed to set ip multicast hops socket option"); |
| goto error; |
| } |
| |
| if (zsock_getsockopt(socket, IPPROTO_IPV6, IPV6_TCLASS, &tclass, &length)) { |
| tclass = 0; |
| } |
| |
| tclass &= ~0xFC; |
| tclass |= CONFIG_PTP_DSCP_VALUE << 2; |
| length = sizeof(tclass); |
| |
| if (zsock_setsockopt(socket, IPPROTO_IPV6, IPV6_TCLASS, &tclass, length)) { |
| LOG_WRN("Failed to set priority"); |
| } |
| |
| return socket; |
| error: |
| zsock_close(socket); |
| return -1; |
| } |
| |
| static int transport_send(int socket, int port, void *buf, int length, struct sockaddr *addr) |
| { |
| struct sockaddr m_addr; |
| socklen_t addrlen; |
| int cnt; |
| |
| if (!addr) { |
| if (IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL)) { |
| m_addr.sa_family = AF_INET; |
| net_sin(&m_addr)->sin_port = htons(port); |
| net_sin(&m_addr)->sin_addr.s_addr = mcast_addr.s_addr; |
| |
| } else if (IS_ENABLED(CONFIG_PTP_UDP_IPv6_PROTOCOL)) { |
| m_addr.sa_family = AF_INET6; |
| net_sin6(&m_addr)->sin6_port = htons(port); |
| memcpy(&net_sin6(&m_addr)->sin6_addr, |
| &mcast_addr, |
| sizeof(struct in6_addr)); |
| } |
| addr = &m_addr; |
| } |
| |
| addrlen = IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL) ? sizeof(struct sockaddr_in) : |
| sizeof(struct sockaddr_in6); |
| cnt = zsock_sendto(socket, buf, length, 0, addr, addrlen); |
| if (cnt < 1) { |
| LOG_ERR("Failed to send message"); |
| return -EFAULT; |
| } |
| |
| return cnt; |
| } |
| |
| int ptp_transport_open(struct ptp_port *port) |
| { |
| static const int socket_ports[] = {PTP_SOCKET_PORT_EVENT, PTP_SOCKET_PORT_GENERAL}; |
| int socket; |
| |
| for (int i = 0; i < PTP_SOCKET_CNT; i++) { |
| socket = IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL) ? |
| transport_udp_ipv4_open(port->iface, socket_ports[i]) : |
| transport_udp_ipv6_open(port->iface, socket_ports[i]); |
| |
| if (socket == -1) { |
| if (i == PTP_SOCKET_GENERAL) { |
| zsock_close(port->socket[PTP_SOCKET_EVENT]); |
| port->socket[PTP_SOCKET_EVENT] = -1; |
| } |
| |
| return -1; |
| } |
| |
| port->socket[i] = socket; |
| } |
| |
| return transport_join_multicast(port); |
| } |
| |
| int ptp_transport_close(struct ptp_port *port) |
| { |
| for (int i = 0; i < PTP_SOCKET_CNT; i++) { |
| |
| if (port->socket[i] >= 0) { |
| if (zsock_close(port->socket[i])) { |
| LOG_ERR("Failed to close socket on PTP Port %d", |
| port->port_ds.id.port_number); |
| return -1; |
| } |
| } |
| |
| port->socket[i] = -1; |
| } |
| |
| return 0; |
| } |
| |
| int ptp_transport_send(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx) |
| { |
| __ASSERT(PTP_SOCKET_CNT <= idx, "Invalid socket index"); |
| |
| static const int socket_port[] = {PTP_SOCKET_PORT_EVENT, PTP_SOCKET_PORT_GENERAL}; |
| int length = ntohs(msg->header.msg_length); |
| |
| return transport_send(port->socket[idx], socket_port[idx], msg, length, NULL); |
| } |
| |
| int ptp_transport_sendto(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx) |
| { |
| __ASSERT(PTP_SOCKET_CNT <= idx, "Invalid socket index"); |
| |
| static const int socket_port[] = {PTP_SOCKET_PORT_EVENT, PTP_SOCKET_PORT_GENERAL}; |
| int length = ntohs(msg->header.msg_length); |
| |
| return transport_send(port->socket[idx], socket_port[idx], msg, length, &msg->addr); |
| } |
| |
| int ptp_transport_recv(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx) |
| { |
| __ASSERT(PTP_SOCKET_CNT <= idx, "Invalid socket index"); |
| |
| int cnt = 0; |
| uint8_t ctrl[CMSG_SPACE(sizeof(struct net_ptp_time))] = {0}; |
| struct cmsghdr *cmsg; |
| struct msghdr msghdr = {0}; |
| struct iovec iov = { |
| .iov_base = msg, |
| .iov_len = sizeof(msg->mtu), |
| }; |
| |
| msghdr.msg_iov = &iov; |
| msghdr.msg_iovlen = 1; |
| msghdr.msg_control = ctrl; |
| msghdr.msg_controllen = sizeof(ctrl); |
| |
| cnt = zsock_recvmsg(port->socket[idx], &msghdr, ZSOCK_MSG_DONTWAIT); |
| if (cnt < 0) { |
| LOG_ERR("Failed receive PTP message"); |
| } |
| |
| for (cmsg = CMSG_FIRSTHDR(&msghdr); cmsg != NULL; cmsg = CMSG_NXTHDR(&msghdr, cmsg)) { |
| if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING) { |
| memcpy(&msg->timestamp.host, CMSG_DATA(cmsg), sizeof(struct net_ptp_time)); |
| } |
| } |
| |
| return cnt; |
| } |
| |
| int ptp_transport_protocol_addr(struct ptp_port *port, uint8_t *addr) |
| { |
| __ASSERT_NO_MSG(addr); |
| |
| int length = 0; |
| |
| if (IS_ENABLED(CONFIG_PTP_UDP_IPv4_PROTOCOL)) { |
| struct in_addr *ip = net_if_ipv4_get_global_addr(port->iface, NET_ADDR_PREFERRED); |
| |
| length = NET_IPV4_ADDR_SIZE; |
| *addr = ip->s_addr; |
| } else if (IS_ENABLED(CONFIG_PTP_UDP_IPv6_PROTOCOL)) { |
| struct in6_addr *ip = net_if_ipv6_get_global_addr(NET_ADDR_PREFERRED, &port->iface); |
| |
| length = NET_IPV6_ADDR_SIZE; |
| memcpy(addr, ip, length); |
| } |
| |
| return length; |
| } |
| |
| struct net_linkaddr *ptp_transport_physical_addr(struct ptp_port *port) |
| { |
| return net_if_get_link_addr(port->iface); |
| } |