/*
 * nsiproxy.sys icmp_echo implementation
 *
 * Copyright 2021 Huw Davies
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
 */
#if 0
#pragma makedep unix
#endif

#include "config.h"
#include <stdarg.h>

#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <limits.h>
#include <pthread.h>
#include <poll.h>
#include <sys/socket.h>
#include <assert.h>

#ifdef HAVE_NETINET_IN_H
#include <netinet/in.h>
#endif

#ifdef HAVE_NETINET_IP_H
#include <netinet/ip.h>
#endif

#ifdef HAVE_LINUX_ERRQUEUE_H
#include <linux/errqueue.h>
#define USE_ERRQUEUE
#endif

#include "ntstatus.h"
#include "windef.h"
#include "winbase.h"
#include "winternl.h"
#include "winioctl.h"
#define USE_WS_PREFIX
#include "ddk/wdm.h"
#include "ifdef.h"
#include "netiodef.h"
#include "ipexport.h"
#include "ipmib.h"
#include "wine/nsi.h"
#include "wine/list.h"
#include "wine/debug.h"

#include "nsiproxy_private.h"

WINE_DEFAULT_DEBUG_CHANNEL(nsi);

static LONG icmp_sequence;

struct ip_hdr
{
    uint8_t v_hl; /* version << 4 | hdr_len */
    uint8_t tos;
    uint16_t tot_len;
    uint16_t id;
    uint16_t frag_off;
    uint8_t ttl;
    uint8_t protocol;
    uint16_t checksum;
    uint32_t saddr;
    uint32_t daddr;
};

struct ipv6_hdr
{
    uint8_t v_prio; /* version << 4 | priority */
    uint8_t flow_lbl[3];
    uint16_t next_len;
    uint8_t next_hdr;
    uint8_t hop_limit;
    struct in6_addr saddr;
    struct in6_addr daddr;
};

C_ASSERT( sizeof(struct ipv6_hdr) == 40 );

struct ipv6_pseudo_header
{
    struct in6_addr src;
    struct in6_addr dst;
    UINT32 next_len; /* incapsulated packet length in network byte order */
    BYTE zero[3];
    BYTE next_header;
};

struct icmp_hdr
{
    uint8_t type;
    uint8_t code;
    uint16_t checksum;
    union
    {
        struct
        {
            uint16_t id;
            uint16_t sequence;
        } echo;
    } un;
};

struct icmp_reply_ctx
{
    SOCKADDR_INET addr;
    ULONG status;
    ULONG round_trip_time;
    LONG data_size;
    BYTE ttl;
    BYTE tos;
    BYTE flags;
    BYTE options_size;
    unsigned int options_data_offset;
    unsigned int data_offset;
    BYTE packet[65536];
    unsigned int packet_size;
    uint16_t id, sequence;
};

struct family_ops;

struct icmp_socket
{
    struct list entry;
    struct list listen_entry;
    struct list request_list;
    LONG ref;
    int socket;
    const struct family_ops *ops;
    struct sockaddr_storage src_storage;
    int src_len;
    int hop_limit;
    BYTE ttl, tos;
    BOOL ping_socket;
    unsigned short id;
    BOOL polling;
};

struct icmp_data
{
    struct list entry;
    LARGE_INTEGER send_time;
    unsigned short seq;
    struct sockaddr_storage dst_storage;
    int dst_len;
    struct icmp_socket *s;
    struct icmp_reply_ctx reply_ctx;
    HANDLE completion_event;
    BOOL polling;
    NTSTATUS status;
};


static pthread_mutex_t listen_lock = PTHREAD_MUTEX_INITIALIZER;
struct list socket_list = LIST_INIT( socket_list );
static int socket_list_update_pipe[2];
static pthread_once_t init_once = PTHREAD_ONCE_INIT;

#define MAX_HANDLES 256 /* Max number of simultaneous pings - could become dynamic if need be */
static struct icmp_data *handle_table[MAX_HANDLES];
static pthread_mutex_t handle_lock = PTHREAD_MUTEX_INITIALIZER;
static struct icmp_data **next_free, **next_unused = handle_table;

static void icmp_listen( void *args );

static icmp_handle handle_alloc( struct icmp_data *data )
{
    struct icmp_data **entry;
    icmp_handle h;

    pthread_mutex_lock( &handle_lock );
    entry = next_free;
    if (entry) next_free = *(struct icmp_data ***)entry;
    else if (next_unused < handle_table + MAX_HANDLES) entry = next_unused++;
    else
    {
        pthread_mutex_unlock( &handle_lock );
        FIXME( "Exhausted icmp handle count\n" );
        return 0;
    }
    *entry = data;
    h = entry - handle_table + 1;
    pthread_mutex_unlock( &handle_lock );
    TRACE( "returning handle %x\n", h );
    return h;
}

static struct icmp_data **handle_entry( icmp_handle h )
{
    if (!h || h > MAX_HANDLES)
    {
        ERR( "Invalid icmp handle\n" );
        return NULL;
    }
    return handle_table + h - 1;
}

static struct icmp_data *handle_data( icmp_handle h )
{
    struct icmp_data **entry = handle_entry( h );

    if (!entry) return NULL;
    return *entry;
}

static void handle_free( icmp_handle h )
{
    struct icmp_data **entry;

    TRACE( "%x\n", h );
    pthread_mutex_lock( &handle_lock );
    entry = handle_entry( h );
    if (entry)
    {
        *(struct icmp_data ***)entry = next_free;
        next_free = entry;
    }
    pthread_mutex_unlock( &handle_lock );
}

static void ipv4_init_icmp_hdr( struct icmp_data *data, struct icmp_hdr *icmp_hdr )
{
    icmp_hdr->type = ICMP4_ECHO_REQUEST;
    icmp_hdr->code = 0;
    icmp_hdr->checksum = 0;
    icmp_hdr->un.echo.id = data->s->id = htons( 1 ); /* will be overwritten for linux ping socks */
    icmp_hdr->un.echo.sequence = data->seq = htons( InterlockedIncrement( &icmp_sequence ) & 0xffff );
}

/* rfc 1071 checksum */
static unsigned short chksum( struct icmp_data *icmp_data, BYTE *data, unsigned int count )
{
    unsigned int sum = 0, carry = 0;
    unsigned short check, s;

    if (icmp_data->s->ping_socket) return 0;

    while (count > 1)
    {
        s = *(unsigned short *)data;
        data += 2;
        sum += carry;
        sum += s;
        carry = s > sum;
        count -= 2;
    }
    sum += carry; /* This won't produce another carry */
    sum = (sum & 0xffff) + (sum >> 16);

    if (count) sum += *data; /* LE-only */

    sum = (sum & 0xffff) + (sum >> 16);
    /* fold in any carry */
    sum = (sum & 0xffff) + (sum >> 16);

    check = ~sum;
    return check;
}

static int ipv4_set_reply_ip_status( IP_STATUS ip_status, unsigned int bits, void *out )
{
    if (bits == 32)
    {
        struct icmp_echo_reply_32 *reply = out;
        memset( reply, 0, sizeof(*reply) );
        reply->status = ip_status;
        return sizeof(*reply);
    }
    else
    {
        struct icmp_echo_reply_64 *reply = out;
        memset( reply, 0, sizeof(*reply) );
        reply->status = ip_status;
        return sizeof(*reply);
    }
}

static void ipv4_set_socket_opts( struct icmp_socket *s )
{
    int val;

    val = s->ttl;
    if (val) setsockopt( s->socket, IPPROTO_IP, IP_TTL, &val, sizeof(val) );
    val = s->tos;
    if (val) setsockopt( s->socket, IPPROTO_IP, IP_TOS, &val, sizeof(val) );
}

#ifdef __linux__
static void ipv4_linux_ping_set_socket_opts( struct icmp_socket *s )
{
    static const int val = 1;

    ipv4_set_socket_opts( s );

    setsockopt( s->socket, IPPROTO_IP, IP_RECVTTL, &val, sizeof(val) );
    setsockopt( s->socket, IPPROTO_IP, IP_RECVTOS, &val, sizeof(val) );
    setsockopt( s->socket, IPPROTO_IP, IP_RECVERR, &val, sizeof(val) );
}
#endif

static BOOL ipv4_parse_ip_hdr( struct msghdr *msg, int *recvd, int *ip_hdr_len,
                               struct icmp_reply_ctx *ctx )
{
    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;
    ctx->options_data_offset = sizeof(*ip_hdr);
    ctx->ttl = ip_hdr->ttl;
    ctx->tos = ip_hdr->tos;
    ctx->flags = ip_hdr->frag_off >> 13;
    ctx->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 icmp_reply_ctx *ctx )
{
    struct cmsghdr *cmsg;

    *ip_hdr_len = 0;
    ctx->options_data_offset = 0;
    ctx->ttl = 0;
    ctx->tos = 0;
    ctx->flags = 0;
    ctx->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:
            ctx->ttl = *(BYTE *)CMSG_DATA( cmsg );
            break;
        case IP_TOS:
            ctx->tos = *(BYTE *)CMSG_DATA( cmsg );
            break;
#ifdef HAVE_LINUX_ERRQUEUE_H
        case IP_RECVERR:
        {
            struct sock_extended_err *e;
            struct icmp_hdr *icmp_h;

            e = (struct sock_extended_err *)CMSG_DATA( cmsg );
            icmp_h = msg->msg_iov[0].iov_base;
            if (e->ee_origin != SO_EE_ORIGIN_ICMP) break;
            if (*recvd + sizeof(*icmp_h) > ARRAY_SIZE(ctx->packet)) break;
            memmove( ctx->packet + sizeof(*icmp_h), ctx->packet, *recvd );
            *recvd += sizeof(*icmp_h);
            memset( icmp_h, 0, sizeof(*icmp_h) );
            icmp_h->type = e->ee_type;
            icmp_h->code = e->ee_code;
            TRACE( "got error packet type %d, code %d.\n", icmp_h->type, icmp_h->code );
            break;
        }
#endif
        }
    }
    return TRUE;
}
#endif

static int ipv4_parse_icmp_hdr( struct icmp_socket *s, struct icmp_hdr *icmp, int icmp_size,
                                struct icmp_reply_ctx *ctx )
{
    static const IP_STATUS unreach_codes[] =
    {
        IP_DEST_NET_UNREACHABLE,  /* ICMP_UNREACH_NET */
        IP_DEST_HOST_UNREACHABLE, /* ICMP_UNREACH_HOST */
        IP_DEST_PROT_UNREACHABLE, /* ICMP_UNREACH_PROTOCOL */
        IP_DEST_PORT_UNREACHABLE, /* ICMP_UNREACH_PORT */
        IP_PACKET_TOO_BIG,        /* ICMP_UNREACH_NEEDFRAG */
        IP_BAD_ROUTE,             /* ICMP_UNREACH_SRCFAIL */
        IP_DEST_NET_UNREACHABLE,  /* ICMP_UNREACH_NET_UNKNOWN */
        IP_DEST_HOST_UNREACHABLE, /* ICMP_UNREACH_HOST_UNKNOWN */
        IP_DEST_HOST_UNREACHABLE, /* ICMP_UNREACH_ISOLATED */
        IP_DEST_NET_UNREACHABLE,  /* ICMP_UNREACH_NET_PROHIB */
        IP_DEST_HOST_UNREACHABLE, /* ICMP_UNREACH_HOST_PROHIB */
        IP_DEST_NET_UNREACHABLE,  /* ICMP_UNREACH_TOSNET */
        IP_DEST_HOST_UNREACHABLE, /* ICMP_UNREACH_TOSHOST */
        IP_DEST_HOST_UNREACHABLE, /* ICMP_UNREACH_FILTER_PROHIB */
        IP_DEST_HOST_UNREACHABLE, /* ICMP_UNREACH_HOST_PRECEDENCE */
        IP_DEST_HOST_UNREACHABLE  /* ICMP_UNREACH_PRECEDENCE_CUTOFF */
    };
    const struct ip_hdr *orig_ip_hdr;
    const struct icmp_hdr *orig_icmp_hdr;
    int orig_ip_hdr_len;
    IP_STATUS status;

    switch (icmp->type)
    {
    case ICMP4_ECHO_REPLY:
        ctx->id = icmp->un.echo.id;
        ctx->sequence = icmp->un.echo.sequence;
        ctx->status = IP_SUCCESS;
        return icmp_size - sizeof(*icmp);

    case ICMP4_DST_UNREACH:
        if (icmp->code < ARRAY_SIZE(unreach_codes))
            status = unreach_codes[icmp->code];
        else
            status = IP_DEST_HOST_UNREACHABLE;
        break;

    case ICMP4_TIME_EXCEEDED:
        if (icmp->code == 1) /* ICMP_TIMXCEED_REASS */
            status = IP_TTL_EXPIRED_REASSEM;
        else
            status = IP_TTL_EXPIRED_TRANSIT;
        break;

    case ICMP4_PARAM_PROB:
        status = IP_PARAM_PROBLEM;
        break;

    case ICMP4_SOURCE_QUENCH:
        status = IP_SOURCE_QUENCH;
        break;

    default:
        return -1;
    }

    icmp_size -= sizeof(*icmp);
    if (s->ping_socket)
    {
        orig_ip_hdr_len = 0;
        orig_icmp_hdr = icmp + 1;
    }
    else
    {
        /* All handled icmp replies have an 8-byte header followed by the original ip hdr. */
        if (icmp_size < sizeof(*orig_ip_hdr)) return -1;
        orig_ip_hdr = (struct ip_hdr *)(icmp + 1);
        if (orig_ip_hdr->v_hl >> 4 != 4 || orig_ip_hdr->protocol != IPPROTO_ICMP) return -1;
        orig_ip_hdr_len = (orig_ip_hdr->v_hl & 0xf) << 2;
        orig_icmp_hdr = (const struct icmp_hdr *)((const BYTE *)orig_ip_hdr + orig_ip_hdr_len);
    }
    if (icmp_size < orig_ip_hdr_len + sizeof(*orig_icmp_hdr)) return -1;
    if (orig_icmp_hdr->type != ICMP4_ECHO_REQUEST ||
        orig_icmp_hdr->code != 0) return -1;

    ctx->id = orig_icmp_hdr->un.echo.id;
    ctx->sequence = orig_icmp_hdr->un.echo.sequence;
    ctx->status = status;
    return 0;
}

static BOOL ipv4_fill_reply( struct icmp_get_reply_params *params, struct icmp_reply_ctx *ctx)
{
    void *options_data;
    ULONG data_offset;
    if (params->bits == 32)
    {
        struct icmp_echo_reply_32 *reply = params->reply;
        data_offset = sizeof(*reply) + ((ctx->options_size + 3) & ~3);
        reply->addr = ctx->addr.Ipv4.sin_addr.WS_s_addr;
        reply->status = ctx->status;
        reply->round_trip_time = ctx->round_trip_time;
        reply->data_size = ctx->data_size;
        reply->num_of_pkts = 1;
        reply->data_ptr = params->user_reply_ptr + data_offset;
        reply->opts.ttl = ctx->ttl;
        reply->opts.tos = ctx->tos;
        reply->opts.flags = ctx->flags;
        reply->opts.options_size = ctx->options_size;
        reply->opts.options_ptr = params->user_reply_ptr + sizeof(*reply);
        options_data = reply + 1;
    }
    else
    {
        struct icmp_echo_reply_64 *reply = params->reply;
        data_offset = sizeof(*reply) + ((ctx->options_size + 3) & ~3);
        reply->addr = ctx->addr.Ipv4.sin_addr.WS_s_addr;
        reply->status = ctx->status;
        reply->round_trip_time = ctx->round_trip_time;
        reply->data_size = ctx->data_size;
        reply->num_of_pkts = 1;
        reply->data_ptr = params->user_reply_ptr + data_offset;
        reply->opts.ttl = ctx->ttl;
        reply->opts.tos = ctx->tos;
        reply->opts.flags = ctx->flags;
        reply->opts.options_size = ctx->options_size;
        reply->opts.options_ptr = params->user_reply_ptr + sizeof(*reply);
        options_data = reply + 1;
    }

    if (ctx->options_size && ((char *)options_data - (char *)params->reply) + ctx->options_size > params->reply_len)
        return FALSE;
    memcpy( options_data, ctx->packet + ctx->options_data_offset, ctx->options_size );
    if (ctx->options_size & 3)
        memset( (char *)options_data + ctx->options_size, 0, 4 - (ctx->options_size & 3) );

    if (ctx->data_size && data_offset + ctx->data_size > params->reply_len)
        return FALSE;
    memcpy( (char *)params->reply + data_offset, ctx->packet + ctx->data_offset, ctx->data_size );
    params->reply_len = data_offset + ctx->data_size;
    return TRUE;
}

struct family_ops
{
    int family;
    int icmp_protocol;
    void (*init_icmp_hdr)( struct icmp_data *data, struct icmp_hdr *icmp_hdr );
    unsigned short (*chksum)( struct icmp_data *icmp_data, BYTE *data, unsigned int count );
    int (*set_reply_ip_status)( IP_STATUS ip_status, unsigned int bits, void *out );
    void (*set_socket_opts)( struct icmp_socket *s );
    BOOL (*parse_ip_hdr)( struct msghdr *msg, int *recvd, int *ip_hdr_len, struct icmp_reply_ctx *ctx );
    int (*parse_icmp_hdr)( struct icmp_socket *s, struct icmp_hdr *icmp, int icmp_len, struct icmp_reply_ctx *ctx );
    BOOL (*fill_reply)( struct icmp_get_reply_params *params, struct icmp_reply_ctx *ctx );
};

static const struct family_ops ipv4 =
{
    AF_INET,
    IPPROTO_ICMP,
    ipv4_init_icmp_hdr,
    chksum,
    ipv4_set_reply_ip_status,
    ipv4_set_socket_opts,
    ipv4_parse_ip_hdr,
    ipv4_parse_icmp_hdr,
    ipv4_fill_reply,
};

#ifdef __linux__
/* linux ipv4 ping sockets behave more like ipv6 raw sockets */
static const struct family_ops ipv4_linux_ping =
{
    AF_INET,
    IPPROTO_ICMP,
    ipv4_init_icmp_hdr,
    chksum,
    ipv4_set_reply_ip_status,
    ipv4_linux_ping_set_socket_opts,
    ipv4_linux_ping_parse_ip_hdr,
    ipv4_parse_icmp_hdr,
    ipv4_fill_reply,
};
#endif

static void ipv6_init_icmp_hdr( struct icmp_data *data, struct icmp_hdr *icmp_hdr )
{
    icmp_hdr->type = ICMP6_ECHO_REQUEST;
    icmp_hdr->code = 0;
    icmp_hdr->checksum = 0;
    icmp_hdr->un.echo.id = data->s->id;
    icmp_hdr->un.echo.sequence = data->seq = InterlockedIncrement( &icmp_sequence ) & 0xffff;
}

static unsigned short ipv6_chksum( struct icmp_data *icmp_data, BYTE *data, unsigned int count )
{
    struct ipv6_pseudo_header *ip_h;
    struct sockaddr_in6 addr;
    unsigned short sum;
    socklen_t slen;
    int s, ret;

    if (icmp_data->s->ping_socket) return 0;

    /* Determine source address. Do it on a separate socket or raw socket won't receive ICMP replies
     * originating not from the destination address. */
    s = socket( AF_INET6, SOCK_RAW, IPPROTO_ICMPV6 );
    if (s < 0)
    {
        TRACE( "failed to open raw sock, trying a dgram sock\n" );
        s = socket( AF_INET6, SOCK_DGRAM, IPPROTO_ICMPV6 );
        if (s < 0)
        {
            WARN( "Unable to create socket\n" );
            return 0;
        }
    }
    if (bind( s, (void *)&icmp_data->s->src_storage, icmp_data->s->src_len ))
    {
        close( s );
        return 0;
    }
    if (connect( s, (void *)&icmp_data->dst_storage, icmp_data->dst_len ))
    {
        close( s );
        return 0;
    }
    slen = sizeof(addr);
    ret = getsockname( s, (void *)&addr, &slen );
    close( s );
    if (ret) return 0;

    ip_h = malloc( sizeof(*ip_h) + count );
    if (!ip_h) return 0;
    memset( ip_h, 0, sizeof(*ip_h) );
    ip_h->src = addr.sin6_addr;
    ip_h->dst = ((struct sockaddr_in6 *)&icmp_data->dst_storage)->sin6_addr;
    ip_h->next_len = htonl( count );
    ip_h->next_header = IPPROTO_ICMPV6;
    memcpy( ip_h + 1, data, count );
    sum = chksum( icmp_data, (BYTE *)ip_h, sizeof(*ip_h) + count );
    free( ip_h );
    return sum;
}

static int ipv6_set_reply_ip_status( IP_STATUS ip_status, unsigned int bits, void *out )
{
    ICMPV6_ECHO_REPLY *reply = out;

    memset( reply, 0, sizeof(*reply) );
    reply->Status = ip_status;
    return sizeof(*reply);
}

static void ipv6_set_socket_opts( struct icmp_socket *s )
{
#ifdef IPV6_UNICAST_HOPS
{
    int val = s->hop_limit;

    setsockopt( s->socket, IPPROTO_IPV6, IPV6_UNICAST_HOPS, &val, sizeof(val) );
}
#endif
#ifdef IP_RECVERR
{
    int val = 1;

    if (s->ping_socket)
        setsockopt( s->socket, IPPROTO_IPV6, IPV6_RECVERR, &val, sizeof(val) );
}
#endif
}

static BOOL ipv6_parse_ip_hdr( struct msghdr *msg, int *recvd, int *ip_hdr_len,
                                          struct icmp_reply_ctx *ctx )
{
    *ip_hdr_len = 0;
    ctx->options_data_offset = 0;
    ctx->ttl = 0;
    ctx->tos = 0;
    ctx->flags = 0;
    ctx->options_size = 0;

#ifdef HAVE_LINUX_ERRQUEUE_H
{
    struct sock_extended_err *e;
    struct icmp_hdr *icmp_h;
    struct cmsghdr *cmsg;

    for (cmsg = CMSG_FIRSTHDR( msg ); cmsg; cmsg = CMSG_NXTHDR( msg, cmsg ))
    {
        if (cmsg->cmsg_level != IPPROTO_IPV6) continue;
        switch (cmsg->cmsg_type)
        {
        case IPV6_RECVERR:
            e = (struct sock_extended_err *)CMSG_DATA( cmsg );
            icmp_h = msg->msg_iov[0].iov_base;
            if (e->ee_origin != SO_EE_ORIGIN_ICMP6) break;
            if (*recvd + sizeof(*icmp_h) > ARRAY_SIZE(ctx->packet)) break;
            TRACE( "got error packet.\n" );
            memmove( ctx->packet + sizeof(*icmp_h), ctx->packet, *recvd );
            *recvd += sizeof(*icmp_h);
            memset( icmp_h, 0, sizeof(*icmp_h) );
            icmp_h->type = e->ee_type;
            icmp_h->code = e->ee_code;
            break;
        }
    }
}
#endif
    return TRUE;
}

static int ipv6_parse_icmp_hdr( struct icmp_socket *s, struct icmp_hdr *icmp,
                                int icmp_size, struct icmp_reply_ctx *ctx )
{
    static const IP_STATUS unreach_codes[] =
    {
        IP_DEST_NO_ROUTE,
        IP_DEST_PROHIBITED,
        IP_DEST_SCOPE_MISMATCH,
        IP_DEST_ADDR_UNREACHABLE,
        IP_DEST_PORT_UNREACHABLE,
        IP_ICMP_ERROR,
        IP_DEST_UNREACHABLE,
        IP_BAD_HEADER,
    };
    const struct ipv6_hdr *orig_ip_hdr;
    const struct icmp_hdr *orig_icmp_hdr;
    IP_STATUS status;

    switch (icmp->type)
    {
    case ICMP6_ECHO_REPLY:
        ctx->id = icmp->un.echo.id;
        ctx->sequence = icmp->un.echo.sequence;
        ctx->status = IP_SUCCESS;
        return icmp_size - sizeof(*icmp);

    case ICMP6_DST_UNREACH:
        if (icmp->code < ARRAY_SIZE(unreach_codes))
            status = unreach_codes[icmp->code];
        else
            status = IP_DEST_HOST_UNREACHABLE;
        break;

    case ICMP6_PACKET_TOO_BIG:
        status = IP_PACKET_TOO_BIG;
        break;

    case ICMP6_TIME_EXCEEDED:
        switch(icmp->code)
        {
        case 0: status = IP_HOP_LIMIT_EXCEEDED; break;
        case 1: status = IP_REASSEMBLY_TIME_EXCEEDED; break;
        default: status = IP_TIME_EXCEEDED; break;
        }
        break;

    case ICMP6_PARAM_PROB:
        status = IP_PARAMETER_PROBLEM;
        break;

    default:
        return -1;
    }

    icmp_size -= sizeof(*icmp);
    if (s->ping_socket) orig_icmp_hdr = icmp + 1;
    else
    {
        /* All handled icmp replies have an 8-byte header followed by the original ip hdr. */
        if (icmp_size < sizeof(*orig_ip_hdr)) return -1;
        icmp_size -= sizeof(*orig_ip_hdr);
        orig_ip_hdr = (struct ipv6_hdr *)(icmp + 1);
        if ((orig_ip_hdr->v_prio >> 4) != 6 || orig_ip_hdr->next_hdr != IPPROTO_ICMPV6) return -1;
        orig_icmp_hdr = (const struct icmp_hdr *)((const BYTE *)orig_ip_hdr + sizeof(*orig_ip_hdr));
    }
    if (icmp_size < sizeof(*orig_icmp_hdr)) return -1;
    if (orig_icmp_hdr->type != ICMP6_ECHO_REQUEST ||
        orig_icmp_hdr->code != 0) return -1;

    ctx->id = orig_icmp_hdr->un.echo.id;
    ctx->sequence = orig_icmp_hdr->un.echo.sequence;
    ctx->status = status;
    return 0;
}

static BOOL ipv6_fill_reply( struct icmp_get_reply_params *params, struct icmp_reply_ctx *ctx)
{
    ICMPV6_ECHO_REPLY *reply = params->reply;

    reply->Status = ctx->status;
    memcpy( reply->Address.sin6_addr, &ctx->addr.Ipv6.sin6_addr, sizeof(ctx->addr.Ipv6.sin6_addr) );
    reply->Address.sin6_flowinfo = ctx->addr.Ipv6.sin6_flowinfo;
    reply->Address.sin6_port = ctx->addr.Ipv6.sin6_port;
    reply->Address.sin6_scope_id = ctx->addr.Ipv6.sin6_scope_id;
    reply->RoundTripTime = ctx->round_trip_time;
    if (sizeof(*reply) + ctx->data_size > params->reply_len)
        return FALSE;
    memcpy( reply + 1, ctx->packet + ctx->data_offset, ctx->data_size );
    params->reply_len = sizeof(*reply) + ctx->data_size;
    return TRUE;
}

static const struct family_ops ipv6 =
{
    AF_INET6,
    IPPROTO_ICMPV6,
    ipv6_init_icmp_hdr,
    ipv6_chksum,
    ipv6_set_reply_ip_status,
    ipv6_set_socket_opts,
    ipv6_parse_ip_hdr,
    ipv6_parse_icmp_hdr,
    ipv6_fill_reply,
};

static IP_STATUS errno_to_ip_status( int err )
{
    switch( err )
    {
    case EHOSTUNREACH: return IP_DEST_HOST_UNREACHABLE;
    default: return IP_GENERAL_FAILURE;
    }
}

static int SOCKADDR_INET_to_sockaddr( const SOCKADDR_INET *in, struct sockaddr *out, int len )
{
    switch (in->si_family)
    {
    case WS_AF_INET:
    {
        struct sockaddr_in *sa = (struct sockaddr_in *)out;

        if (len < sizeof(*sa)) return 0;
        memset( sa, 0, sizeof(*sa) );
        sa->sin_family = AF_INET;
        sa->sin_port = in->Ipv4.sin_port;
        sa->sin_addr.s_addr = in->Ipv4.sin_addr.WS_s_addr;
        return sizeof(*sa);
    }
    case WS_AF_INET6:
    {
        struct sockaddr_in6 *sa = (struct sockaddr_in6 *)out;

        if (len < sizeof(*sa)) return 0;
        memset( sa, 0, sizeof(*sa) );
        sa->sin6_family = AF_INET6;
        sa->sin6_port = in->Ipv6.sin6_port;
        sa->sin6_flowinfo = in->Ipv6.sin6_flowinfo;
        memcpy( sa->sin6_addr.s6_addr, in->Ipv6.sin6_addr.WS_s6_addr, sizeof(sa->sin6_addr.s6_addr) );
        sa->sin6_scope_id = in->Ipv6.sin6_scope_id;
        return sizeof(*sa);
    }
    }
    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 void icmp_grab_socket( struct icmp_socket *s )
{
    assert( s->ref > 0 );
    InterlockedIncrement( &s->ref );
}

static void icmp_release_socket( struct icmp_socket *s )
{
    pthread_mutex_lock( &listen_lock );
    if (!InterlockedDecrement( &s->ref ))
    {
        TRACE( "freeing socket %p, fd %d.\n", s, s->socket );
        if (s->polling) list_remove( &s->entry );
        if (s->socket >= 0) close( s->socket );
        free( s );
    }
    pthread_mutex_unlock( &listen_lock );
}

static BOOL icmp_data_stop_polling( struct icmp_data *data, NTSTATUS status )
{
    BOOL ret;

    pthread_mutex_lock( &listen_lock );
    if ((ret = data->polling))
    {
        list_remove( &data->entry );
        data->polling = FALSE;
        data->status = status;
    }
    pthread_mutex_unlock( &listen_lock );
    return ret;
}

static void icmp_data_free( struct icmp_data *data )
{
    assert( !data->polling );
    icmp_release_socket( data->s );
    free( data );
}

static void init_listen(void)
{
    HANDLE thread;

    pipe( socket_list_update_pipe );

    if (PsCreateSystemThread( &thread, THREAD_ALL_ACCESS, NULL, 0, NULL, icmp_listen, NULL ))
        ERR( "Could not create listen thread.\n" );
    else
        NtClose( thread );
}

static NTSTATUS icmp_get_socket( struct icmp_send_echo_params *params, struct icmp_socket **ret_socket )
{
    const struct family_ops *ops;
    struct icmp_socket *s;
    struct sockaddr *src;
    int src_len;
    struct sockaddr_storage src_storage;

    src = (struct sockaddr *)&src_storage;
    src_len = SOCKADDR_INET_to_sockaddr( params->src, src, sizeof(src_storage) );

    if (params->dst->si_family == WS_AF_INET6)     ops = &ipv6;
    else if (params->dst->si_family == WS_AF_INET) ops = &ipv4;
    else return STATUS_INVALID_PARAMETER;

    *ret_socket = NULL;
    pthread_mutex_lock( &listen_lock );
    LIST_FOR_EACH_ENTRY( s, &socket_list, struct icmp_socket, entry )
    {
        assert( s->polling );
        if (params->dst->si_family == WS_AF_INET6 && ops->family != AF_INET6) continue;
        if (params->dst->si_family == WS_AF_INET && ops->family != AF_INET) continue;
        if (s->hop_limit != params->hop_limit || s->ttl != params->ttl || s->tos != params->tos) continue;
        if (s->src_len != src_len) continue;
        if (memcmp( src, &s->src_storage, src_len )) continue;

        icmp_grab_socket( s );
        pthread_mutex_unlock( &listen_lock );
        *ret_socket = s;
        TRACE( "Using socket %p.\n", s );
        return STATUS_SUCCESS;
    }

    pthread_once( &init_once, init_listen );

    if (!(s = calloc( 1, sizeof(*s) )))
    {
        pthread_mutex_unlock( &listen_lock );
        return STATUS_NO_MEMORY;
    }
    s->ref = 1;
    list_init( &s->request_list );
    s->socket = -1;
    s->id = getpid() & 0xffff; /* will be overwritten for linux ping socks */
    s->ping_socket = FALSE;

    s->socket = socket( ops->family, SOCK_RAW, ops->icmp_protocol );
    if (s->socket < 0) /* Try a ping-socket */
    {
        int value = 1;

        TRACE( "failed to open raw sock, trying a dgram sock\n" );
        s->socket = socket( ops->family, SOCK_DGRAM, ops->icmp_protocol );
        if (s->socket < 0)
        {
            WARN( "Unable to create socket\n" );
            pthread_mutex_unlock( &listen_lock );
            icmp_release_socket( s );
            return STATUS_ACCESS_DENIED;
        }
        setsockopt( s->socket, SOL_SOCKET, SO_REUSEADDR, &value, sizeof(value) );
#ifdef __linux__
        if (ops->family == AF_INET) ops = &ipv4_linux_ping;
        s->ping_socket = TRUE;
#endif
    }

    s->hop_limit = params->hop_limit;
    s->ttl = params->ttl;
    s->tos = params->tos;
    s->src_len = src_len;
    s->src_storage = src_storage;

    if (bind( s->socket, src, s->src_len ))
    {
        pthread_mutex_unlock( &listen_lock );
        icmp_release_socket( s );
        return STATUS_INVALID_ADDRESS_COMPONENT;
    }

    s->ops = ops;
    s->ops->set_socket_opts( s );
    list_add_tail( &socket_list, &s->entry );
    s->polling = TRUE;
    pthread_mutex_unlock( &listen_lock );
    write( socket_list_update_pipe[1], "x", 1 );
    *ret_socket = s;
    TRACE( "created socket %p.\n", s );
    return STATUS_SUCCESS;
}

NTSTATUS icmp_send_echo( void *args )
{
    struct icmp_send_echo_params *params = args;
    struct icmp_hdr *icmp_hdr; /* this is the same for both ipv4 and ipv6 */
    struct icmp_data *data;
    struct sockaddr *dst;
    int ret, err;

    NTSTATUS status;

    data = calloc( 1, sizeof(*data) );
    if (!data) return STATUS_NO_MEMORY;
    if ((status = icmp_get_socket( params, &data->s )))
    {
        free( data );
        return status;
    }
    data->completion_event = params->completion_event;

    dst = (struct sockaddr *)&data->dst_storage;
    data->dst_len = SOCKADDR_INET_to_sockaddr( params->dst, dst, sizeof(data->dst_storage) );

    icmp_hdr = malloc( sizeof(*icmp_hdr) + params->request_size );
    if (!icmp_hdr)
    {
        icmp_data_free( data );
        return STATUS_NO_MEMORY;
    }
    data->s->ops->init_icmp_hdr( data, icmp_hdr );
    memcpy( icmp_hdr + 1, params->request, params->request_size );
    icmp_hdr->checksum = data->s->ops->chksum( data, (BYTE *)icmp_hdr, sizeof(*icmp_hdr) + params->request_size );

    TRACE( "data %p, seq %d.\n", data, data->seq );

    pthread_mutex_lock( &listen_lock );
    list_add_tail( &data->s->request_list, &data->entry );
    data->polling = TRUE;
    pthread_mutex_unlock( &listen_lock );

    NtQueryPerformanceCounter( &data->send_time, NULL );
    ret = sendto( data->s->socket, icmp_hdr, sizeof(*icmp_hdr) + params->request_size, 0,
                  (struct sockaddr *)&data->dst_storage, data->dst_len );
    if (ret < 0) err = errno;
    free( icmp_hdr );

    if (ret < 0)
    {
        status = errno_to_ip_status( err );
        TRACE( "sendto() rets %d errno %d\n", ret, err );
        icmp_data_stop_polling( data, status );
        params->reply_len = data->s->ops->set_reply_ip_status( status, params->bits, params->reply );
        icmp_data_free( data );
        return STATUS_SUCCESS;
    }

    *params->handle = handle_alloc( data );
    if (!*params->handle)
    {
        icmp_data_stop_polling( data, STATUS_NO_MEMORY );
        icmp_data_free( data );
    }
    return *params->handle ? STATUS_PENDING : STATUS_NO_MEMORY;
}

static ULONG get_rtt( LARGE_INTEGER start )
{
    LARGE_INTEGER now;

    NtQueryPerformanceCounter( &now, NULL );
    return (now.QuadPart - start.QuadPart) / 10000;
}

static void icmp_listen( void *args )
{
    struct sockaddr_storage addr;
    struct icmp_reply_ctx ctx;
    BYTE cmsg_buf[1024];
    struct iovec iov[1] = {{ .iov_base = ctx.packet, .iov_len = sizeof(ctx.packet) }};
    struct msghdr msg = { .msg_iov = iov, .msg_iovlen = ARRAY_SIZE(iov) };
    struct list listen_list;
    unsigned int fds_size = 64;
    struct pollfd *fds = malloc( fds_size * sizeof(*fds) );
    struct icmp_socket *s, *next;
    struct icmp_hdr *icmp_hdr;
    int ip_hdr_len, recvd;
    struct icmp_data *data;
    unsigned int i, count;
    BOOL matched;
    int ret;
    BYTE b;

    pthread_once( &init_once, init_listen );
    while (1)
    {
        list_init( &listen_list );

        pthread_mutex_lock( &listen_lock );
        i = 0;
        LIST_FOR_EACH_ENTRY( s, &socket_list, struct icmp_socket, entry )
        {
            icmp_grab_socket( s );
            list_add_tail( &listen_list, &s->listen_entry );
            if (i + 1 >= fds_size)
            {
                fds_size *= 2;
                fds = realloc( fds, fds_size * sizeof(*fds) );
            }
            fds[i].fd = s->socket;
            fds[i].events = POLLIN;
            ++i;
        }
        pthread_mutex_unlock( &listen_lock );

        fds[i].fd = socket_list_update_pipe[0];
        fds[i].events = POLLIN;
        count = i;
        while ((ret = poll( fds, count + 1, -1 )) < 0 && (errno == EINTR || errno == EAGAIN))
            ;
        if (ret < 0)
        {
            ERR( "poll(): ret %d, %s.\n", ret, strerror( errno ));
            return;
        }

        i = 0;
        LIST_FOR_EACH_ENTRY( s, &listen_list, struct icmp_socket, listen_entry )
        {
            if (!(fds[i].revents & (POLLIN | POLLERR))) goto skip;
            msg.msg_name = &addr;
            msg.msg_namelen = sizeof(addr);
            msg.msg_control = cmsg_buf;
            msg.msg_controllen = sizeof(cmsg_buf);
            recvd = recvmsg( s->socket, &msg, 0 );
            TRACE( "s %p, recvmsg() rets %d errno %d addr_len %d iovlen %d msg_flags %x\n",
                   s, recvd, errno, msg.msg_namelen, (int)iov[0].iov_len, msg.msg_flags );
#ifdef __linux__
            if (recvd < 0 && s->ping_socket && fds[i].revents & POLLERR)
            {
                msg.msg_name = &addr;
                msg.msg_namelen = sizeof(addr);
                msg.msg_control = cmsg_buf;
                msg.msg_controllen = sizeof(cmsg_buf);
                recvd = recvmsg( s->socket, &msg, MSG_ERRQUEUE );
                TRACE( "s %p, recvmsg(MSG_ERRQUEUE) rets %d errno %d addr_len %d iovlen %d msg_flags %x\n",
                       s, recvd, errno, msg.msg_namelen, (int)iov[0].iov_len, msg.msg_flags );
            }
#endif
            if (recvd < 0) goto skip;
            if (!s->ops->parse_ip_hdr( &msg, &recvd, &ip_hdr_len, &ctx )) goto skip;
            if (recvd < ip_hdr_len + sizeof(*icmp_hdr)) goto skip;
            ctx.packet_size = recvd;
            icmp_hdr = (struct icmp_hdr *)(ctx.packet + ip_hdr_len);
            if ((ctx.data_size = s->ops->parse_icmp_hdr( s, icmp_hdr, recvd - ip_hdr_len, &ctx )) < 0)
            {
                WARN( "parse_icmp_hdr failed.\n" );
                goto skip;
            }
            pthread_mutex_lock( &listen_lock );
            matched = FALSE;
            LIST_FOR_EACH_ENTRY( data, &s->request_list, struct icmp_data, entry )
            {
                if ((!s->ping_socket && ctx.id != s->id) || ctx.sequence != data->seq) continue;
                TRACE( "id %d, seq %d matched to %p.\n", ctx.id, ctx.sequence, data );
                matched = TRUE;
                data->polling = FALSE;
                list_remove( &data->entry );
                sockaddr_to_SOCKADDR_INET( (struct sockaddr *)&addr, &ctx.addr );
                ctx.round_trip_time = get_rtt( data->send_time );
                ctx.data_offset = (BYTE *)(icmp_hdr + 1) - ctx.packet;
                data->reply_ctx = ctx;
                NtSetEvent( data->completion_event, NULL );
                break;
            }
            pthread_mutex_unlock( &listen_lock );
            if (!matched) TRACE( "id %d, seq %d not matched.\n", ctx.id, ctx.sequence );
skip:
            ++i;
        }
        if (fds[count].revents & POLLIN)
        {
            read( socket_list_update_pipe[0], &b, 1 );
            TRACE( "updating socket list.\n" );
        }

        LIST_FOR_EACH_ENTRY_SAFE( s, next, &listen_list, struct icmp_socket, listen_entry )
        {
            icmp_release_socket( s );
        }
    }
}

NTSTATUS icmp_get_reply( void *args )
{
    struct icmp_get_reply_params *params = args;
    struct icmp_data *data;
    NTSTATUS ret = STATUS_SUCCESS;

    data = handle_data( params->handle );
    if (!data) return STATUS_INVALID_PARAMETER;

    if (data->dst_storage.ss_family == AF_INET6 && !data->s->hop_limit)
    {
        TRACE( "data %p, seq %u, invalid hop_limit.\n", data, data->seq );
        icmp_data_stop_polling( data, STATUS_SUCCESS );
        params->reply_len = data->s->ops->set_reply_ip_status( IP_GENERAL_FAILURE, params->bits, params->reply );
        goto done;
    }

    if (icmp_data_stop_polling( data, STATUS_TIMEOUT ))
    {
        /* Was still polling, timeout. */
        TRACE( "data %p, seq %u, timeout.\n", data, data->seq );
        params->reply_len = data->s->ops->set_reply_ip_status( IP_REQ_TIMED_OUT, params->bits, params->reply );
    }
    else if (!(ret = data->status))
    {
        if (!data->s->ops->fill_reply( params, &data->reply_ctx ))
        {
            WARN( "data %p, seq %u, buffer too small.\n", data, data->seq );
            params->reply_len = data->s->ops->set_reply_ip_status( IP_GENERAL_FAILURE, params->bits, params->reply );
        }
        else TRACE( "data %p, seq %u, got reply %u.\n", data, data->seq, data->reply_ctx.status );
    }
done:
    icmp_data_free( data );
    handle_free( params->handle );
    return ret;
}
