FreeBSD hooking communication protocols

struct protosw {
        short   pr_type;                /* socket type used for */
        struct  domain *pr_domain;      /* domain protocol a member of */
        short   pr_protocol;            /* protocol number */
        short   pr_flags;               /* see below */
/* protocol-protocol hooks */
        pr_input_t *pr_input;           /* input to protocol (from below) */
        pr_output_t *pr_output;         /* output to protocol (from above) */
        pr_ctlinput_t *pr_ctlinput;     /* control input (from below) */
        pr_ctloutput_t *pr_ctloutput;   /* control output (from above) */
/* utility hooks */
        pr_init_t *pr_init;
        pr_fasttimo_t *pr_fasttimo;     /* fast timeout (200ms) */
        pr_slowtimo_t *pr_slowtimo;     /* slow timeout (500ms) */
        pr_drain_t *pr_drain;           /* flush any excess space possible */

        struct  pr_usrreqs *pr_usrreqs; /* user-protocol hook */
};

struct protosw inetsw[] = {
{
        .pr_type =              0,
        .pr_domain =            &inetdomain,
        .pr_protocol =          IPPROTO_IP,
        .pr_init =              ip_init,
        .pr_slowtimo =          ip_slowtimo,
        .pr_drain =             ip_drain,
        .pr_usrreqs =           &nousrreqs
},
{
        .pr_type =              SOCK_DGRAM,
        .pr_domain =            &inetdomain,
        .pr_protocol =          IPPROTO_UDP,
        .pr_flags =             PR_ATOMIC|PR_ADDR,
        .pr_input =             udp_input,
        .pr_ctlinput =          udp_ctlinput,
        .pr_ctloutput =         udp_ctloutput,
        .pr_init =              udp_init,
        .pr_usrreqs =           &udp_usrreqs
},
{
        .pr_type =              SOCK_STREAM,
        .pr_domain =            &inetdomain,
        .pr_protocol =          IPPROTO_TCP,
        .pr_flags =             PR_CONNREQUIRED|PR_IMPLOPCL|PR_WANTRCVD,
        .pr_input =             tcp_input,
        .pr_ctlinput =          tcp_ctlinput,
        .pr_ctloutput =         tcp_ctloutput,
        .pr_init =              tcp_init,
        .pr_slowtimo =          tcp_slowtimo,
        .pr_drain =             tcp_drain,
        .pr_usrreqs =           &tcp_usrreqs
},

~~ snip ~~

{
        .pr_type =              SOCK_RAW,
        .pr_domain =            &inetdomain,
        .pr_flags =             PR_ATOMIC|PR_ADDR,
        .pr_input =             rip_input,
        .pr_ctloutput =         rip_ctloutput,
        .pr_init =              rip_init,
        .pr_usrreqs =           &rip_usrreqs
},
};

struct mbuf {
        /*
         * Header present at the beginning of every mbuf.
         * Size ILP32: 24
         *      LP64: 32
         * Compile-time assertions in uipc_mbuf.c test these values to ensure
         * that they are correct.
         */
        union { /* next buffer in chain */
                struct mbuf             *m_next;
                SLIST_ENTRY(mbuf)       m_slist;
                STAILQ_ENTRY(mbuf)      m_stailq;
        };
        union { /* next chain in queue/record */
                struct mbuf             *m_nextpkt;
                SLIST_ENTRY(mbuf)       m_slistpkt;
                STAILQ_ENTRY(mbuf)      m_stailqpkt;
        };
        caddr_t          m_data;        /* location of data */
        int32_t          m_len;         /* amount of data in this mbuf */
        uint32_t         m_type:8,      /* type of data in this mbuf */
                         m_flags:24;    /* flags; see below */
#if !defined(__LP64__)
        uint32_t         m_pad;         /* pad for 64bit alignment */
#endif

        /*
         * A set of optional headers (packet header, external storage header)
         * and internal data storage.  Historically, these arrays were sized
         * to MHLEN (space left after a packet header) and MLEN (space left
         * after only a regular mbuf header); they are now variable size in
         * order to support future work on variable-size mbufs.
         */
        union {
                struct {
                        struct pkthdr   m_pkthdr;       /* M_PKTHDR set */
                        union {
                                struct m_ext    m_ext;  /* M_EXT set */
                                char            m_pktdat[0];
                        };
                };
                char    m_dat[0];                       /* !M_PKTHDR, !M_EXT */
        };
};

#include <sys/param.h>
#include <sys/proc.h>
#include <sys/module.h>
#include <sys/kernel.h>
#include <sys/systm.h>
#include <sys/mbuf.h>
#include <sys/protosw.h>

#include <netinet/in.h>
#include <netinet/in_systm.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <netinet/ip_var.h>

#define TRIGGER "flcl"

extern struct protosw inetsw[];

static int
icmp_input_hook(struct mbuf **mp, int *offp, int proto)
{
    struct icmp *icp;
    struct mbuf *m = *mp;
    int hlen = *offp;   // 受信したICMPメッセージのIPヘッダ長

    m->m_len -= hlen;
    m->m_data += hlen;

    // icmpメッセージを取り出す
    icp = mtod(m, struct icmp *); // #define mtod(m, t) ((t)((m)->m_data))

    // 必要なデータを取り出した後は元に戻す
    m->m_len +=hlen;
    m->m_data -=hlen;

    if (icp->icmp_type == ICMP_REDIRECT &&
            icp->icmp_code == ICMP_REDIRECT_TOSHOST &&
            strncmp(icp->icmp_data, TRIGGER, strlen(TRIGGER)) == 0)
        printf("Hooked!\n");

    return icmp_input(mp, offp, proto);
}

static int
load(struct module *module, int cmd, void *arg)
{
    int error = 0;

    switch (cmd) {
    case MOD_LOAD:
        inetsw[ip_protox[IPPROTO_ICMP]].pr_input = icmp_input_hook;
        break;
    case MOD_UNLOAD:
        inetsw[ip_protox[IPPROTO_ICMP]].pr_input = icmp_input;
        break;
    default:
        error = EOPNOTSUPP;
        break;
    }

    return (error);
}

static moduledata_t icmp_input_hook_mod = {
    "icmp_input_hook",
    load,
    NULL
};

DECLARE_MODULE(icmp_input_hook, icmp_input_hook_mod, SI_SUB_DRIVERS, SI_ORDER_MIDDLE);