#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <linux/kd.h>
#include <linux/vt.h>
#include <errno.h>
#include <pthread.h>
#include <stdint.h>
#include <unistd.h>

int ioctl_main(int argc, char *argv[])
{
    int c;
    int fd;
    int res;

    int read_only = 0;
    int length = -1;
    int arg_size = 4;
    int direct_arg = 0;
    uint32_t ioctl_nr;
    void *ioctl_args;
    uint8_t *ioctl_argp;
    uint8_t *ioctl_argp_save;
    int rem;

    do {
        c = getopt(argc, argv, "rdl:a:h");
        if (c == EOF)
            break;
        switch (c) {
        case 'r':
            read_only = 1;
            break;
        case 'd':
            direct_arg = 1;
            break;
        case 'l':
            length = strtol(optarg, NULL, 0);
            break;
        case 'a':
            arg_size = strtol(optarg, NULL, 0);
            break;
        case 'h':
            fprintf(stderr, "%s [-l <length>] [-a <argsize>] [-rdh] <device> <ioctlnr>\n"
                    "  -l <lenght>   Length of io buffer\n"
                    "  -a <argsize>  Size of each argument (1-8)\n"
                    "  -r            Open device in read only mode\n"
                    "  -d            Direct argument (no iobuffer)\n"
                    "  -h            Print help\n", argv[0]);
            return -1;
        case '?':
            fprintf(stderr, "%s: invalid option -%c\n",
                argv[0], optopt);
            exit(1);
        }
    } while (1);

    if(optind + 2 > argc) {
        fprintf(stderr, "%s: too few arguments\n", argv[0]);
        exit(1);
    }

    fd = open(argv[optind], O_RDWR | O_SYNC);
    if (fd < 0) {
        fprintf(stderr, "cannot open %s\n", argv[optind]);
        return 1;
    }
    optind++;
    
    ioctl_nr = strtol(argv[optind], NULL, 0);
    optind++;

    if(direct_arg) {
        arg_size = 4;
        length = 4;
    }

    if(length < 0) {
        length = (argc - optind) * arg_size;
    }
    if(length) {
        ioctl_args = calloc(1, length);

        ioctl_argp_save = ioctl_argp = ioctl_args;
        rem = length;
        while(optind < argc) {
            uint64_t tmp_64 = strtoull(argv[optind], NULL, 0);
            uint32_t tmp_32 = tmp_64;
            if(rem < arg_size) {
                fprintf(stderr, "%s: too many arguments\n", argv[0]);
                exit(1);
            }
            switch (arg_size) {
            case sizeof(tmp_64):
              memcpy(ioctl_argp, &tmp_64, sizeof(tmp_64));
              break;
            case sizeof(tmp_32):
              memcpy(ioctl_argp, &tmp_32, sizeof(tmp_32));
              break;
            default:
              fprintf(stderr, "%s: unhandled arg_size\n", argv[0]);
              exit(1);
            }
            ioctl_argp += arg_size;
            rem -= arg_size;
            optind++;
        }
    }
    printf("sending ioctl 0x%x", ioctl_nr);
    rem = length;
    while(rem--) {
        printf(" 0x%02x", *ioctl_argp_save++);
    }
    printf("\n");

    if(direct_arg)
        res = ioctl(fd, ioctl_nr, *(uint32_t*)ioctl_args);
    else if(length)
        res = ioctl(fd, ioctl_nr, ioctl_args);
    else
        res = ioctl(fd, ioctl_nr, 0);
    if (res < 0) {
        fprintf(stderr, "ioctl 0x%x failed, %d\n", ioctl_nr, res);
        return 1;
    }
    if(length) {
        printf("return buf:");
        ioctl_argp = ioctl_args;
        rem = length;
        while(rem--) {
            printf(" %02x", *ioctl_argp++);
        }
        printf("\n");
    }
    return 0;
}