Signed-off-by: Huw Davies <huw(a)codeweavers.com>
---
dlls/nsiproxy.sys/icmp_echo.c | 205 +++++++++++++++++++++++++++++++++-
1 file changed, 203 insertions(+), 2 deletions(-)
diff --git a/dlls/nsiproxy.sys/icmp_echo.c b/dlls/nsiproxy.sys/icmp_echo.c
index afb48fc72e3..abcccdb4c73 100644
--- a/dlls/nsiproxy.sys/icmp_echo.c
+++ b/dlls/nsiproxy.sys/icmp_echo.c
@@ -233,6 +233,100 @@ static void ipv4_linux_ping_set_socket_opts( struct icmp_data *data, struct icmp
}
#endif
+static int ipv4_reply_buffer_len( int reply_len )
+{
+ return sizeof(struct ip_hdr) + sizeof(struct icmp_hdr) + reply_len - sizeof(struct nsiproxy_icmp_echo_reply);
+}
+
+#ifdef __linux__
+static int ipv4_linux_ping_reply_buffer_len( int reply_len )
+{
+ return sizeof(struct icmp_hdr) + reply_len - sizeof(struct nsiproxy_icmp_echo_reply);
+}
+#endif
+
+static BOOL ipv4_parse_ip_hdr( struct msghdr *msg, int recvd,
+ int *ip_hdr_len, struct nsiproxy_icmp_echo_reply *reply, void **opts )
+{
+ struct ip_hdr *ip_hdr;
+
+ if (recvd < sizeof(*ip_hdr)) return FALSE;
+ ip_hdr = msg->msg_iov[0].iov_base;
+ if (ip_hdr->v_hl >> 4 != 4 || ip_hdr->protocol != IPPROTO_ICMP) return FALSE;
+ *ip_hdr_len = (ip_hdr->v_hl & 0xf) << 2;
+ if (*ip_hdr_len < sizeof(*ip_hdr)) return FALSE;
+ *opts = ip_hdr + 1;
+ reply->opts.ttl = ip_hdr->ttl;
+ reply->opts.tos = ip_hdr->tos;
+ reply->opts.flags = ip_hdr->frag_off >> 13;
+ reply->opts.options_size = *ip_hdr_len - sizeof(*ip_hdr);
+
+ return TRUE;
+}
+
+#ifdef __linux__
+static BOOL ipv4_linux_ping_parse_ip_hdr( struct msghdr *msg, int recvd,
+ int *ip_hdr_len, struct nsiproxy_icmp_echo_reply *reply, void **opts )
+{
+ struct cmsghdr *cmsg;
+
+ *ip_hdr_len = 0;
+ *opts = NULL;
+ reply->opts.ttl = 0;
+ reply->opts.tos = 0;
+ reply->opts.flags = 0;
+ reply->opts.options_size = 0; /* FIXME from IP_OPTIONS but will require checking for space in the reply */
+
+ for (cmsg = CMSG_FIRSTHDR( msg ); cmsg; cmsg = CMSG_NXTHDR( msg, cmsg ))
+ {
+ if (cmsg->cmsg_level != IPPROTO_IP) continue;
+ switch (cmsg->cmsg_type)
+ {
+ case IP_TTL:
+ reply->opts.ttl = *(BYTE *)CMSG_DATA( cmsg );
+ break;
+ case IP_TOS:
+ reply->opts.tos = *(BYTE *)CMSG_DATA( cmsg );
+ break;
+ }
+ }
+ return TRUE;
+}
+#endif
+
+static int ipv4_parse_icmp_hdr_( struct icmp_data *data, struct icmp_hdr *icmp, int icmp_size,
+ struct nsiproxy_icmp_echo_reply *reply, int ping_socket )
+{
+ switch (icmp->type)
+ {
+ case ICMP4_ECHO_REPLY:
+ if ((!ping_socket && icmp->un.echo.id != data->id) ||
+ icmp->un.echo.sequence != data->seq) return -1;
+
+ reply->status = IP_SUCCESS;
+ return icmp_size - sizeof(*icmp);
+
+ /* FIXME: handle other icmp messages */
+
+ default:
+ return -1;
+ }
+}
+
+static int ipv4_parse_icmp_hdr( struct icmp_data *data, struct icmp_hdr *icmp, int icmp_size,
+ struct nsiproxy_icmp_echo_reply *reply )
+{
+ return ipv4_parse_icmp_hdr_( data, icmp, icmp_size, reply, 0 );
+}
+
+#ifdef __linux__
+static int ipv4_linux_ping_parse_icmp_hdr( struct icmp_data *data, struct icmp_hdr *icmp, int icmp_size,
+ struct nsiproxy_icmp_echo_reply *reply )
+{
+ return ipv4_parse_icmp_hdr_( data, icmp, icmp_size, reply, 1 );
+}
+#endif
+
struct family_ops
{
int family;
@@ -240,6 +334,11 @@ struct family_ops
void (*init_icmp_hdr)( struct icmp_data *data, struct icmp_hdr *icmp_hdr );
unsigned short (*chksum)( BYTE *data, unsigned int count );
void (*set_socket_opts)( struct icmp_data *data, struct icmp_send_echo_params *params );
+ int (*reply_buffer_len)( int reply_len );
+ BOOL (*parse_ip_hdr)( struct msghdr *msg, int recvd,
+ int *ip_hdr_len, struct nsiproxy_icmp_echo_reply *reply, void **opts );
+ int (*parse_icmp_hdr)( struct icmp_data *data, struct icmp_hdr *icmp, int icmp_len,
+ struct nsiproxy_icmp_echo_reply *reply );
};
static const struct family_ops ipv4 =
@@ -249,6 +348,9 @@ static const struct family_ops ipv4 =
ipv4_init_icmp_hdr,
chksum,
ipv4_set_socket_opts,
+ ipv4_reply_buffer_len,
+ ipv4_parse_ip_hdr,
+ ipv4_parse_icmp_hdr,
};
#ifdef __linux__
@@ -260,6 +362,9 @@ static const struct family_ops ipv4_linux_ping =
ipv4_init_icmp_hdr,
null_chksum,
ipv4_linux_ping_set_socket_opts,
+ ipv4_linux_ping_reply_buffer_len,
+ ipv4_linux_ping_parse_ip_hdr,
+ ipv4_linux_ping_parse_icmp_hdr,
};
#endif
@@ -302,6 +407,34 @@ static int SOCKADDR_INET_to_sockaddr( const SOCKADDR_INET *in, struct sockaddr *
return 0;
}
+static BOOL sockaddr_to_SOCKADDR_INET( const struct sockaddr *in, SOCKADDR_INET *out )
+{
+ switch (in->sa_family)
+ {
+ case AF_INET:
+ {
+ struct sockaddr_in *sa = (struct sockaddr_in *)in;
+
+ out->Ipv4.sin_family = WS_AF_INET;
+ out->Ipv4.sin_port = sa->sin_port;
+ out->Ipv4.sin_addr.WS_s_addr = sa->sin_addr.s_addr;
+ return TRUE;
+ }
+ case AF_INET6:
+ {
+ struct sockaddr_in6 *sa = (struct sockaddr_in6 *)in;
+
+ out->Ipv6.sin6_family = WS_AF_INET6;
+ out->Ipv6.sin6_port = sa->sin6_port;
+ out->Ipv6.sin6_flowinfo = sa->sin6_flowinfo;
+ memcpy( out->Ipv6.sin6_addr.WS_s6_addr, sa->sin6_addr.s6_addr, sizeof(sa->sin6_addr.s6_addr) );
+ out->Ipv6.sin6_scope_id = sa->sin6_scope_id;
+ return TRUE;
+ }
+ }
+ return FALSE;
+}
+
static NTSTATUS icmp_data_create( ADDRESS_FAMILY win_family, struct icmp_data **icmp_data )
{
struct icmp_data *data;
@@ -404,11 +537,78 @@ static int get_timeout( LARGE_INTEGER start, DWORD timeout )
return min( (end.QuadPart - now.QuadPart) / 10000, INT_MAX );
}
+static ULONG get_rtt( LARGE_INTEGER start )
+{
+ LARGE_INTEGER now;
+
+ NtQueryPerformanceCounter( &now, NULL );
+ return (now.QuadPart - start.QuadPart) / 10000;
+}
+
+static NTSTATUS recv_msg( struct icmp_data *data, struct icmp_listen_params *params )
+{
+ struct nsiproxy_icmp_echo_reply *reply = (struct nsiproxy_icmp_echo_reply *)params->reply;
+ struct sockaddr_storage addr;
+ struct iovec iov[1];
+ BYTE cmsg_buf[1024];
+ struct msghdr msg = { .msg_name = &addr, .msg_namelen = sizeof(addr),
+ .msg_iov = iov, .msg_iovlen = ARRAY_SIZE(iov),
+ .msg_control = cmsg_buf, .msg_controllen = sizeof(cmsg_buf) };
+ int ip_hdr_len, recvd, reply_buf_len, data_size;
+ char *reply_buf;
+ void *opts;
+ struct icmp_hdr *icmp_hdr;
+
+ reply_buf_len = data->ops->reply_buffer_len( params->reply_len );
+ reply_buf = malloc( reply_buf_len );
+ if (!reply_buf) return STATUS_NO_MEMORY;
+
+ iov[0].iov_base = reply_buf;
+ iov[0].iov_len = reply_buf_len;
+
+ recvd = recvmsg( data->socket, &msg, 0 );
+ TRACE( "recvmsg() rets %d errno %d addr_len %d iovlen %d msg_flags %x\n",
+ recvd, errno, msg.msg_namelen, (int)iov[0].iov_len, msg.msg_flags );
+
+ if (!data->ops->parse_ip_hdr( &msg, recvd, &ip_hdr_len, reply, &opts )) goto skip;
+ if (recvd < ip_hdr_len + sizeof(*icmp_hdr)) goto skip;
+
+ icmp_hdr = (struct icmp_hdr *)(reply_buf + ip_hdr_len);
+ if ((data_size = data->ops->parse_icmp_hdr( data, icmp_hdr, recvd - ip_hdr_len, reply )) < 0) goto skip;
+ reply->data_size = data_size;
+ if (reply->data_size && msg.msg_flags & MSG_TRUNC)
+ {
+ free( reply_buf );
+ return set_reply_ip_status( params, IP_GENERAL_FAILURE );
+ }
+
+ sockaddr_to_SOCKADDR_INET( (struct sockaddr *)&addr, &reply->addr );
+ reply->round_trip_time = get_rtt( data->send_time );
+ reply->num_of_pkts = 1;
+ reply->opts.options_offset = sizeof(*reply);
+ reply->data_offset = sizeof(*reply) + ((reply->opts.options_size + 3) & ~3);
+ if (reply->opts.options_size)
+ memcpy( (char *)reply + reply->opts.options_offset, opts, reply->opts.options_size );
+ if (reply->opts.options_size & 3)
+ memset( (char *)reply + reply->opts.options_offset + reply->opts.options_size, 0, 4 - (reply->opts.options_size & 3) );
+ if (reply->data_size)
+ memcpy( (char *)reply + reply->data_offset, icmp_hdr + 1, reply->data_size );
+
+ params->reply_len = reply->data_offset + reply->data_size;
+ free( reply_buf );
+ return STATUS_SUCCESS;
+
+skip:
+ free( reply_buf );
+ return STATUS_RETRY;
+}
+
NTSTATUS icmp_listen( void *args )
{
struct icmp_listen_params *params = args;
struct icmp_data *data;
struct pollfd fds[1];
+ NTSTATUS status;
int ret;
data = handle_data( params->handle );
@@ -419,8 +619,9 @@ NTSTATUS icmp_listen( void *args )
while ((ret = poll( fds, ARRAY_SIZE(fds), get_timeout( data->send_time, params->timeout ) )) > 0)
{
- /* FIXME */
- return STATUS_NOT_SUPPORTED;
+ status = recv_msg( data, params );
+ if (status == STATUS_RETRY) continue;
+ return status;
}
if (!ret) /* timeout */
--
2.23.0