Signed-off-by: Huw Davies huw@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 */