Corsair K70 RGB Rainbow Linux

#include <stdio.h>
#include <unistd.h>
#include <usb.h>
#include <math.h>

static struct usb_device *find_device(uint16_t vendor, uint16_t product);

static void update_keyboard();

static void set_led( int x, int y, int r, int g, int b );

char red_val[144];
char grn_val[144];
char blu_val[144];

char data_pkt[5][64] = { 0 };

unsigned char position_map[] = {
  137,  8, 20,255,
	0, 12, 24, 36, 48, 60, 72, 84, 96,108,120,132,  6, 18, 30, 42, 32, 44, 56, 68,255,
	1, 13, 25, 37, 49, 61, 73, 85, 97,109,121,133,  7, 31, 54, 66, 78, 80, 92,104,116,255,
	2, 14, 26, 38, 50, 62, 74, 86, 98,110,122,134, 90,102, 43, 55, 67,  9, 21, 33,128,255,
	3, 15, 27, 39, 51, 63, 75, 87, 99,111,123,135,126, 57, 69, 81,128,255,
	4, 28, 40, 52, 64, 76, 88,100,112,124,136, 79,103, 93,105,117,140,255,
	5, 17, 29, 53, 89,101,113, 91,115,127,139,129,141,140,255,
};

float size_map[] = {
	-15.5, 1, 1, -2.5, 1, -2, 0,
	1, -.5, 1, 1, 1, 1, -.75, 1, 1, 1, 1, -.75, 1, 1, 1, 1, -.5, 1, 1, 1, -.5, 1, 1, 1, 1, 0,
	1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, -.5, 1, 1, 1, -.5, 1, 1, 1, 1, 0,
	1.5, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1.5, -.5, 1, 1, 1, -.5, 1, 1, 1, 1, 0,
	1.75, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2.25, -4, 1, 1, 1, 1, 0,
	2.25, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2.75, -1.5, 1, -1.5, 1, 1, 1, 1, 0,
	1.5, 1, 1.25, 6.5, 1.25, 1, 1, 1.5, -.5, 1, 1, 1, -.5, 2, 1, 1, 0,
};

unsigned char led_matrix[7][92];

struct usb_device *dev;
struct usb_dev_handle *handle;

int main(int argc, char **argv)
{

    int bytes_written = 0;
    int status = 0;

    printf("Searching for Corsair K70 RGB keyboard...\n");

    dev = find_device(0x1B1C, 0x1B13);

    if(dev == NULL)
    {
        printf("Corsair K70 RGB keyboard not detected :(\n");
        return 1;
    }
    else
    {
        printf("Corsair K70 RGB keyboard detected successfully :)\n");
    }

    handle = usb_open(dev);

    if(handle == NULL)
    {
        printf("USB device handle did not open :(\n");
        return 1;
    }
    else
    {
        printf("USB device handle opened successfully :)\n");
    }

    status = usb_claim_interface(handle, 3);

    status = usb_detach_kernel_driver_np(handle, 3);

    if(status == 0)
    {
        printf("USB interface claimed successfully :)\n");
    }
    else
    {
        printf("USB interface claim failed with error %d :(\n", status);
        return 1;
    }

    // Construct XY lookup table
	unsigned char *keys = position_map;
	float *sizes = size_map;
	for (int y = 0; y < 7; y++)
    {
        unsigned char key;
        int size = 0;

        for (int x = 0; x < 92; x++)
        {
            if (size == 0)
            {
                float sizef = *sizes++;
                if (sizef < 0)
                {
                    size = -sizef * 4;
                    key = 255;
                }
                else
                {
                    key = *keys++;
                    size = sizef * 4;
                }
            }

            led_matrix[y][x] = key;
            size--;
        }

        if (*keys++ != 255 || *sizes++ != 0)
        {
            printf("Bad line %d\n", y);
            return 1;
        }
    }

    while(1)
    {
        for(int i = 0; i < 91; i++)
        {
            for(int x = 0; x < 91; x++)
            {
                for(int y = 0; y < 7; y++)
                {
                    char red = 3.5f * ( sin( ( x / 92.0f ) * 2 * 3.14f ) + 1 );
                    char grn = 3.5f * ( sin( ( ( x / 92.0f ) * 2 * 3.14f ) - ( 6.28f / 3 ) ) + 1 );
                    char blu = 3.5f * ( sin( ( ( x / 92.0f ) * 2 * 3.14f ) + ( 6.28f / 3 ) ) + 1 );

                    set_led( ( x + i ) % 92, y, red, grn, blu );
                }
            }

            update_keyboard();
            usleep(12500);
        }
    }
    return 0;
}

static void set_led( int x, int y, int r, int g, int b )
{
    int led = led_matrix[y][x];

    if(led >= 144)
    {
        return;
    }

    if( r > 7 ) r = 7;
    if( g > 7 ) g = 7;
    if( b > 7 ) b = 7;

    r = 7 - r;
    g = 7 - g;
    b = 7 - b;

    red_val[led] = r;
    grn_val[led] = g;
    blu_val[led] = b;
}


static void update_keyboard()
{
    // Perform USB control message to keyboard
    //
    // Request Type:  0x21
    // Request:       0x09
    // Value          0x0300
    // Index:         0x03
    // Size:          64

    data_pkt[0][0] = 0x7F;
    data_pkt[0][1] = 0x01;
    data_pkt[0][2] = 0x3C;

    data_pkt[1][0] = 0x7F;
    data_pkt[1][1] = 0x02;
    data_pkt[1][2] = 0x3C;

    data_pkt[2][0] = 0x7F;
    data_pkt[2][1] = 0x03;
    data_pkt[2][2] = 0x3C;

    data_pkt[3][0] = 0x7F;
    data_pkt[3][1] = 0x04;
    data_pkt[3][2] = 0x24;

	data_pkt[4][0] = 0x07;
	data_pkt[4][1] = 0x27;
	data_pkt[4][4] = 0xD8;

    for(int i = 0; i < 60; i++)
    {
        data_pkt[0][i+4] = red_val[i*2+1] << 4 | red_val[i*2];
    }

    for(int i = 0; i < 12; i++)
    {
        data_pkt[1][i+4] = red_val[i*2+121] << 4 | red_val[i*2+120];
    }

    for(int i = 0; i < 48; i++)
    {
        data_pkt[1][i+16] = grn_val[i*2+1] << 4 | grn_val[i*2];
    }

    for(int i = 0; i < 24; i++)
    {
        data_pkt[2][i+4] = grn_val[i*2+97] << 4 | grn_val[i*2+96];
    }

    for(int i = 0; i < 36; i++)
    {
        data_pkt[2][i+28] = blu_val[i*2+1] << 4 | blu_val[i*2];
    }

    for(int i = 0; i < 36; i++)
    {
        data_pkt[3][i+4] = blu_val[i*2+73] << 4 | blu_val[i*2+72];
    }

    usb_control_msg(handle, 0x21, 0x09, 0x0300, 0x03, data_pkt[0], 64, 1000);
    usb_control_msg(handle, 0x21, 0x09, 0x0300, 0x03, data_pkt[1], 64, 1000);
    usb_control_msg(handle, 0x21, 0x09, 0x0300, 0x03, data_pkt[2], 64, 1000);
    usb_control_msg(handle, 0x21, 0x09, 0x0300, 0x03, data_pkt[3], 64, 1000);
    usb_control_msg(handle, 0x21, 0x09, 0x0300, 0x03, data_pkt[4], 64, 1000);
}


static struct usb_device *find_device(uint16_t vendor, uint16_t product)
{
    struct usb_bus *bus;
    struct usb_device *dev;
    struct usb_bus *busses;

    usb_init();
    usb_find_busses();
    usb_find_devices();

    busses = usb_get_busses();

    for(bus = busses; bus; bus = bus->next)
    {
        for(dev = bus->devices; dev; dev = dev->next)
        {
            if((dev->descriptor.idVendor == vendor) && (dev->descriptor.idProduct == product))
            {
                return dev;
            }
        }
    }

    return NULL;
}