/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 *
 * Copyright (C) 2009 by Tomer Shalev
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/
#include "string.h"
#include "system.h"
#include "usb_core.h"
#include "usb_drv.h"
#include "kernel.h"
#include "powermgmt.h"
#include "usb_hid.h"
#include "usb_hid_def.h"
#include "usb_class_driver.h"
/*#define LOGF_ENABLE*/
#include "logf.h"

#define HID_BUF_SIZE_MSG        16
#define HID_BUF_SIZE_CMD        16
#define HID_BUF_SIZE_REPORT     160
#define HID_NUM_BUFFERS         5
#define SET_REPORT_BUF_LEN      2
#define GET_REPORT_BUF_LEN      2

#ifdef LOGF_ENABLE
#define BUF_DUMP_BUF_LEN       HID_BUF_SIZE_REPORT
#define BUF_DUMP_PREFIX_SIZE   5
#define BUF_DUMP_ITEMS_IN_LINE 8
#define BUF_DUMP_LINE_SIZE     (BUF_DUMP_ITEMS_IN_LINE * 3)
#define BUF_DUMP_NUM_LINES     (BUF_DUMP_BUF_LEN / BUF_DUMP_ITEMS_IN_LINE)
#endif

#define HID_BUF_INC(i) do { (i) = ((i) + 1) % HID_NUM_BUFFERS; } while (0)
#define PACK_VAL(dest, val) *(dest)++ = (val) & 0xff

#define REPORT_ID_BACKGROUND REPORT_ID_COUNT

typedef enum
{
    REPORT_ID_KEYBOARD = 1,
    REPORT_ID_CONSUMER,
#ifdef HAVE_USB_HID_MOUSE
    REPORT_ID_MOUSE,
#endif
    REPORT_ID_COUNT,
} report_id_t;

/* hid interface */
static struct usb_interface_descriptor __attribute__((aligned(2)))
                                interface_descriptor =
{
    .bLength            = sizeof(struct usb_interface_descriptor),
    .bDescriptorType    = USB_DT_INTERFACE,
    .bInterfaceNumber   = 0,
    .bAlternateSetting  = 0,
    .bNumEndpoints      = 1,
    .bInterfaceClass    = USB_CLASS_HID,
    .bInterfaceSubClass = USB_HID_SUBCLASS_BOOT_INTERFACE,
    .bInterfaceProtocol = USB_HID_PROTOCOL_CODE_KEYBOARD,
    .iInterface         = 0
};

static struct usb_hid_descriptor __attribute__((aligned(2))) hid_descriptor =
{
    .bLength            = sizeof(struct usb_hid_descriptor),
    .bDescriptorType    = USB_DT_HID,
    .wBcdHID            = USB_HID_VER,
    .bCountryCode       = 0,
    .bNumDescriptors    = 1,
    .bDescriptorType0   = USB_DT_REPORT,
    .wDescriptorLength0 = 0
};

static struct usb_endpoint_descriptor __attribute__((aligned(2)))
    endpoint_descriptor =
{
    .bLength          = sizeof(struct usb_endpoint_descriptor),
    .bDescriptorType  = USB_DT_ENDPOINT,
    .bEndpointAddress = 0,
    .bmAttributes     = USB_ENDPOINT_XFER_INT,
    .wMaxPacketSize   = 0,
    .bInterval        = 0
};

typedef struct
{
    uint8_t usage_page;
    /* Write the id into the buffer in the appropriate location, and returns the
     * buffer length */
    uint8_t (*buf_set)(unsigned char *buf, int id);
    bool is_key_released;
} usb_hid_report_t;

static usb_hid_report_t usb_hid_reports[REPORT_ID_COUNT];

static unsigned char report_descriptor[HID_BUF_SIZE_REPORT]
    USB_DEVBSS_ATTR __attribute__((aligned(32)));

static unsigned char send_buffer[HID_NUM_BUFFERS][HID_BUF_SIZE_MSG]
    USB_DEVBSS_ATTR __attribute__((aligned(32)));
static size_t send_buffer_len[HID_NUM_BUFFERS];
static int cur_buf_prepare;
static int cur_buf_send;

static bool active = false;
static bool currently_sending = false;
static int usb_interface;

static struct usb_class_driver_ep_allocation ep_allocs[1] = {
    {.type = USB_ENDPOINT_XFER_INT, .dir = DIR_IN, .optional = false, .mps = -1},
};

#define EP_IN (ep_allocs[0].ep)

static void usb_hid_try_send_drv(void);

static void pack_parameter(unsigned char **dest, bool is_signed, bool mark_size,
        uint8_t parameter, uint32_t value)
{
    uint8_t size_value = 1; /* # of bytes */
    uint32_t v = value;

    while (v >>= 8)
        size_value++;

    if (is_signed)
    {
        bool is_negative = 0;

        switch (size_value)
        {
            case 1:
                is_negative = (int8_t)value < 0;
                break;
            case 2:
                is_negative = (int16_t)value < 0;
                break;
            case 3:
            case 4:
                is_negative = (int32_t)value < 0;
                break;
            default:
                break;
        }

        if (is_negative)
            size_value++;
    }

    **dest = parameter | (mark_size ? size_value : 0);
    (*dest)++;

    while (size_value--)
    {
        **dest = value & 0xff;
        (*dest)++;
        value >>= 8;
    }
}

static int usb_hid_set_first_interface(int interface)
{
    usb_interface = interface;

    return interface + 1;
}

#ifdef LOGF_ENABLE
static unsigned char buf_dump_ar[BUF_DUMP_NUM_LINES][BUF_DUMP_LINE_SIZE + 1]
    USB_DEVBSS_ATTR __attribute__((aligned(32)));

void buf_dump(unsigned char *buf, size_t size, char *msg)
{
    static const char v[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
        'A', 'B', 'C', 'D', 'E', 'F' };

    size_t i = 0;
    for (int line = 0; i < size; line++)
    {
        int i0 = i;
        char *b = buf_dump_ar[line];

        for (int j = 0; j < BUF_DUMP_ITEMS_IN_LINE && i < size; j++, i++)
        {
            *b++ = v[buf[i] >> 4];
            *b++ = v[buf[i] & 0xF];
            *b++ = ' ';
        }
        *b = 0;

        /* Prefix must be of len BUF_DUMP_PREFIX_SIZE */
        logf("%03d: %s %s", i0, buf_dump_ar[line], msg);
    }
}
#else
#undef buf_dump
#define buf_dump(...)
#endif

#define BUF_LEN_KEYBOARD 7
static uint8_t buf_set_keyboard(unsigned char *buf, int id)
{
    memset(buf, 0, BUF_LEN_KEYBOARD);
    int key, i = 1;

    /* Each key is a word in id (up to 4 keys supported) */
    while ((key = id & 0xff))
    {
        /* Each modifier key is a bit in the first byte */
        if (HID_KEYBOARD_LEFT_CONTROL <= key && key <= HID_KEYBOARD_RIGHT_GUI)
            buf[0] |= (1 << (key - HID_KEYBOARD_LEFT_CONTROL));
        else /* Any other key should be set in a separate byte */
            buf[i++] = (uint8_t)key;

        id >>= 8;
    }

    return BUF_LEN_KEYBOARD;
}

#define BUF_LEN_CONSUMER 4
static uint8_t buf_set_consumer(unsigned char *buf, int id)
{
    memset(buf, 0, BUF_LEN_CONSUMER);
    buf[0] = (uint8_t)id;

    return BUF_LEN_CONSUMER;
}

#ifdef HAVE_USB_HID_MOUSE
#define MOUSE_WHEEL_STEP       1
#define MOUSE_STEP             8
#define MOUSE_BUTTON_LEFT    0x1
#define MOUSE_BUTTON_RIGHT   0x2
//#define MOUSE_BUTTON_MIDDLE  0x4
#define MOUSE_BUTTON           0
#define MOUSE_X                1
#define MOUSE_Y                2
#define MOUSE_WHEEL            3
#define BUF_LEN_MOUSE          4
#define MOUSE_ACCEL_FACTOR(count) ((count) / 2)
#define MOUSE_DO(item, step) (buf[(item)] = ((uint8_t)(step)))

static uint8_t buf_set_mouse(unsigned char *buf, int id)
{
    static int count = 0;
    int step = MOUSE_STEP;

    memset(buf, 0, BUF_LEN_MOUSE);

    /* Set proper button */
    switch (id)
    {
        case HID_MOUSE_BUTTON_LEFT:
        case HID_MOUSE_LDRAG_UP:
        case HID_MOUSE_LDRAG_UP_REP:
        case HID_MOUSE_LDRAG_DOWN:
        case HID_MOUSE_LDRAG_DOWN_REP:
        case HID_MOUSE_LDRAG_LEFT:
        case HID_MOUSE_LDRAG_LEFT_REP:
        case HID_MOUSE_LDRAG_RIGHT:
        case HID_MOUSE_LDRAG_RIGHT_REP:
            MOUSE_DO(MOUSE_BUTTON, MOUSE_BUTTON_LEFT);
            break;
        case HID_MOUSE_BUTTON_RIGHT:
        case HID_MOUSE_RDRAG_UP:
        case HID_MOUSE_RDRAG_UP_REP:
        case HID_MOUSE_RDRAG_DOWN:
        case HID_MOUSE_RDRAG_DOWN_REP:
        case HID_MOUSE_RDRAG_LEFT:
        case HID_MOUSE_RDRAG_LEFT_REP:
        case HID_MOUSE_RDRAG_RIGHT:
        case HID_MOUSE_RDRAG_RIGHT_REP:
            MOUSE_DO(MOUSE_BUTTON, MOUSE_BUTTON_RIGHT);
            break;
        case HID_MOUSE_BUTTON_LEFT_REL:
        case HID_MOUSE_BUTTON_RIGHT_REL:
            /* Keep buf empty, to mark mouse button release */
            break;
        default:
            break;
    }

    /* Handle mouse accelarated movement */
    switch (id)
    {
        case HID_MOUSE_UP:
        case HID_MOUSE_DOWN:
        case HID_MOUSE_LEFT:
        case HID_MOUSE_RIGHT:
        case HID_MOUSE_LDRAG_UP:
        case HID_MOUSE_LDRAG_DOWN:
        case HID_MOUSE_LDRAG_LEFT:
        case HID_MOUSE_LDRAG_RIGHT:
        case HID_MOUSE_RDRAG_UP:
        case HID_MOUSE_RDRAG_DOWN:
        case HID_MOUSE_RDRAG_LEFT:
        case HID_MOUSE_RDRAG_RIGHT:
            count = 0;
            break;
        case HID_MOUSE_UP_REP:
        case HID_MOUSE_DOWN_REP:
        case HID_MOUSE_LEFT_REP:
        case HID_MOUSE_RIGHT_REP:
        case HID_MOUSE_LDRAG_UP_REP:
        case HID_MOUSE_LDRAG_DOWN_REP:
        case HID_MOUSE_LDRAG_LEFT_REP:
        case HID_MOUSE_LDRAG_RIGHT_REP:
        case HID_MOUSE_RDRAG_UP_REP:
        case HID_MOUSE_RDRAG_DOWN_REP:
        case HID_MOUSE_RDRAG_LEFT_REP:
        case HID_MOUSE_RDRAG_RIGHT_REP:
            count++;
            /* TODO: Find a better mouse accellaration algorithm */
            step *= 1 + MOUSE_ACCEL_FACTOR(count);
            if (step > 255)
                step = 255;
            break;
        default:
            break;
    }

    /* Move/scroll mouse */
    switch (id)
    {
        case HID_MOUSE_UP:
        case HID_MOUSE_UP_REP:
        case HID_MOUSE_LDRAG_UP:
        case HID_MOUSE_LDRAG_UP_REP:
        case HID_MOUSE_RDRAG_UP:
        case HID_MOUSE_RDRAG_UP_REP:
            MOUSE_DO(MOUSE_Y, -step);
            break;
        case HID_MOUSE_DOWN:
        case HID_MOUSE_DOWN_REP:
        case HID_MOUSE_LDRAG_DOWN:
        case HID_MOUSE_LDRAG_DOWN_REP:
        case HID_MOUSE_RDRAG_DOWN:
        case HID_MOUSE_RDRAG_DOWN_REP:
            MOUSE_DO(MOUSE_Y, step);
            break;
        case HID_MOUSE_LEFT:
        case HID_MOUSE_LEFT_REP:
        case HID_MOUSE_LDRAG_LEFT:
        case HID_MOUSE_LDRAG_LEFT_REP:
        case HID_MOUSE_RDRAG_LEFT:
        case HID_MOUSE_RDRAG_LEFT_REP:
            MOUSE_DO(MOUSE_X, -step);
            break;
        case HID_MOUSE_RIGHT:
        case HID_MOUSE_RIGHT_REP:
        case HID_MOUSE_LDRAG_RIGHT:
        case HID_MOUSE_LDRAG_RIGHT_REP:
        case HID_MOUSE_RDRAG_RIGHT:
        case HID_MOUSE_RDRAG_RIGHT_REP:
            MOUSE_DO(MOUSE_X, step);
            break;
        case HID_MOUSE_SCROLL_UP:
            MOUSE_DO(MOUSE_WHEEL, MOUSE_WHEEL_STEP);
            break;
        case HID_MOUSE_SCROLL_DOWN:
            MOUSE_DO(MOUSE_WHEEL, -MOUSE_WHEEL_STEP);
            break;
        default:
            break;
    }

    return BUF_LEN_MOUSE;
}
#endif /* HAVE_USB_HID_MOUSE */

static size_t descriptor_report_get(unsigned char *dest)
{
    unsigned char *report = dest;
    usb_hid_report_t *usb_hid_report;

    /* Keyboard control */
    usb_hid_report = &usb_hid_reports[REPORT_ID_KEYBOARD];
    usb_hid_report->usage_page = HID_USAGE_PAGE_KEYBOARD_KEYPAD;
    usb_hid_report->buf_set = buf_set_keyboard;
    usb_hid_report->is_key_released = 1;

    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE,
            HID_USAGE_PAGE_GENERIC_DESKTOP_CONTROLS);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DESKTOP_KEYBOARD);
    pack_parameter(&report, 0, 1, USB_HID_COLLECTION, USB_HID_COLLECTION_APPLICATION);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_ID, REPORT_ID_KEYBOARD);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE, HID_GENERIC_DESKTOP_KEYPAD);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MINIMUM, HID_KEYBOARD_LEFT_CONTROL);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MAXIMUM, HID_KEYBOARD_RIGHT_GUI);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MINIMUM, 0);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MAXIMUM, 1);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 1);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 8);
    pack_parameter(&report, 0, 1, USB_HID_INPUT, USB_HID_MAIN_ITEM_VARIABLE);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 1);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 5);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE, HID_USAGE_PAGE_LEDS);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MINIMUM, HID_LED_NUM_LOCK);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MAXIMUM, HID_LED_KANA);
    pack_parameter(&report, 0, 1, USB_HID_OUTPUT, USB_HID_MAIN_ITEM_VARIABLE);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 3);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 1);
    pack_parameter(&report, 0, 1, USB_HID_OUTPUT, USB_HID_MAIN_ITEM_CONSTANT);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 8);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 6);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MINIMUM, 0);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MAXIMUM, HID_KEYBOARD_EX_SEL);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE, HID_USAGE_PAGE_KEYBOARD_KEYPAD);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MINIMUM, 0);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MAXIMUM, HID_KEYBOARD_EX_SEL);
    pack_parameter(&report, 0, 1, USB_HID_INPUT, 0);
    PACK_VAL(report, USB_HID_END_COLLECTION);

    /* Consumer usage controls - play/pause, stop, etc. */
    usb_hid_report = &usb_hid_reports[REPORT_ID_CONSUMER];
    usb_hid_report->usage_page = HID_USAGE_PAGE_CONSUMER;
    usb_hid_report->buf_set = buf_set_consumer;
    usb_hid_report->is_key_released = 1;

    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE, HID_USAGE_PAGE_CONSUMER);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE,
            HID_CONSUMER_USAGE_CONSUMER_CONTROL);
    pack_parameter(&report, 0, 1, USB_HID_COLLECTION, USB_HID_COLLECTION_APPLICATION);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_ID, REPORT_ID_CONSUMER);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 16);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 2);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MINIMUM, 1);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MAXIMUM, 652);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MINIMUM,
            HID_CONSUMER_USAGE_CONSUMER_CONTROL);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MAXIMUM, HID_CONSUMER_USAGE_AC_SEND);
    pack_parameter(&report, 0, 1, USB_HID_INPUT, USB_HID_MAIN_ITEM_NO_PREFERRED |
            USB_HID_MAIN_ITEM_NULL_STATE);
    PACK_VAL(report, USB_HID_END_COLLECTION);

#ifdef HAVE_USB_HID_MOUSE
    /* Mouse control */
    usb_hid_report = &usb_hid_reports[REPORT_ID_MOUSE];
    usb_hid_report->usage_page = HID_USAGE_PAGE_GENERIC_DESKTOP_CONTROLS;
    usb_hid_report->buf_set = buf_set_mouse;
    usb_hid_report->is_key_released = 0;

    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE,
            HID_USAGE_PAGE_GENERIC_DESKTOP_CONTROLS);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DESKTOP_MOUSE);
    pack_parameter(&report, 0, 1, USB_HID_COLLECTION, USB_HID_COLLECTION_APPLICATION);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_ID, REPORT_ID_MOUSE);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DESKTOP_POINTER);
    pack_parameter(&report, 0, 1, USB_HID_COLLECTION, USB_HID_COLLECTION_PHYSICAL);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE, HID_USAGE_PAGE_BUTTON);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MINIMUM, 1);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_MAXIMUM, 8);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MINIMUM, 0);
    pack_parameter(&report, 1, 1, USB_HID_LOGICAL_MAXIMUM, 1);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 1);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 8);
    pack_parameter(&report, 0, 1, USB_HID_INPUT, USB_HID_MAIN_ITEM_VARIABLE);
    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE,
            HID_USAGE_PAGE_GENERIC_DESKTOP_CONTROLS);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DESKTOP_X);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DESKTOP_Y);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DESKTOP_WHEEL);
    pack_parameter(&report, 0, 1, USB_HID_LOGICAL_MINIMUM, -127 & 0xFF);
    pack_parameter(&report, 0, 1, USB_HID_LOGICAL_MAXIMUM, 127);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 8);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 3);
    pack_parameter(&report, 0, 1, USB_HID_INPUT, USB_HID_MAIN_ITEM_VARIABLE | USB_HID_MAIN_ITEM_RELATIVE);
    PACK_VAL(report, USB_HID_END_COLLECTION);
    PACK_VAL(report, USB_HID_END_COLLECTION);
#endif /* HAVE_USB_HID_MOUSE */

    /* Background reports */
    pack_parameter(&report, 0, 1, USB_HID_USAGE_PAGE, HID_USAGE_PAGE_GENERIC_DEVICE_CONTROLS);
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DEVICE_BACKGROUND_CONTROLS);
    pack_parameter(&report, 0, 1, USB_HID_COLLECTION, USB_HID_COLLECTION_APPLICATION);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_ID, REPORT_ID_BACKGROUND);
    /* Padding */
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DEVICE_UNDEFINED);
    pack_parameter(&report, 0, 1, USB_HID_LOGICAL_MINIMUM, 0);
    pack_parameter(&report, 0, 1, USB_HID_LOGICAL_MAXIMUM, 255);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 8);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 1);
    pack_parameter(&report, 0, 1, USB_HID_FEATURE, USB_HID_MAIN_ITEM_CONSTANT);
    /* Battery percentage */
    pack_parameter(&report, 0, 0, USB_HID_CONSUMER_USAGE, HID_GENERIC_DEVICE_BATTERY_STRENGTH);
    pack_parameter(&report, 0, 1, USB_HID_LOGICAL_MINIMUM, 0);
    pack_parameter(&report, 0, 1, USB_HID_LOGICAL_MAXIMUM, 100);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_SIZE, 8);
    pack_parameter(&report, 0, 1, USB_HID_REPORT_COUNT, 1);
    pack_parameter(&report, 0, 1, USB_HID_FEATURE, USB_HID_MAIN_ITEM_VARIABLE);
    PACK_VAL(report, USB_HID_END_COLLECTION);

    return (size_t)(report - dest);
}

static void descriptor_hid_get(unsigned char **dest)
{
    hid_descriptor.wDescriptorLength0= descriptor_report_get(report_descriptor);

    logf("hid: desc len %u", hid_descriptor.wDescriptorLength0);
    buf_dump(report_descriptor, hid_descriptor.wDescriptorLength0, "desc");

    PACK_DATA(dest, hid_descriptor);
}

static int usb_hid_get_config_descriptor(unsigned char *dest, int max_packet_size)
{
    (void)max_packet_size;

    unsigned char *orig_dest = dest;

    logf("hid: config desc.");

    /* Interface descriptor */
    interface_descriptor.bInterfaceNumber = usb_interface;
    PACK_DATA(&dest, interface_descriptor);

    /* HID descriptor */
    descriptor_hid_get(&dest);

    /* Endpoint descriptor */
    endpoint_descriptor.wMaxPacketSize = 8;
    endpoint_descriptor.bInterval = 8;
    endpoint_descriptor.bEndpointAddress = EP_IN;
    PACK_DATA(&dest, endpoint_descriptor);

    return (int)(dest - orig_dest);
}

static int usb_hid_init_connection(void)
{
    logf("hid: init connection");
    active = true;
    currently_sending = false;
    return 0;
}

/* called by usb_core_init() */
static void usb_hid_init(void)
{
    logf("hid: init");

    for (int i = 0; i < HID_NUM_BUFFERS; i++)
        send_buffer_len[i] = 0;

    cur_buf_prepare = 0;
    cur_buf_send = 0;

    active = true;
    currently_sending = false;
}

static void usb_hid_disconnect(void)
{
    logf("hid: disconnect");
    active = false;
    currently_sending = false;
}

/* called by usb_core_transfer_complete() */
static void usb_hid_transfer_complete(int ep, int dir, int status, int length)
{
    (void)ep;
    (void)length;

    logf("HID: transfer complete: %d %d %d %d",ep,dir,status,length);
    if (dir == USB_DIR_IN && !status)
    {
        send_buffer_len[cur_buf_send] = 0;
        HID_BUF_INC(cur_buf_send);
        currently_sending = false;
        usb_hid_try_send_drv();
    }
}

/* The DAP is registered as a keyboard with several LEDs, therefore the OS sends
 * LED report to notify the DAP whether Num Lock / Caps Lock etc. are enabled.
 * In order to allow sending info to the DAP, the Set Report mechanism can be
 * used by defining vendor specific output reports and send them from the host
 * to the DAP using the host's custom driver */
static int usb_hid_set_report(struct usb_ctrlrequest *req, uint8_t* reqdata, size_t reqdata_size)
{
    (void)reqdata;
    (void)reqdata_size;

    int length;

    if ((req->wValue >> 8) != USB_HID_REPORT_TYPE_OUTPUT)
    {
        logf("Unsupported report type");
        return 1;
    }
    if ((req->wValue & 0xff) != REPORT_ID_KEYBOARD)
    {
        logf("Wrong report id");
        return 2;
    }
    if (req->wIndex != (uint16_t)usb_interface)
    {
        logf("Wrong interface");
        return 3;
    }
    length = req->wLength;
    if (length != SET_REPORT_BUF_LEN)
    {
        logf("Wrong length");
        return 4;
    }

#ifdef LOGF_ENABLE
    if (reqdata[1] & 0x01)
        logf("Num Lock enabled");
    if (reqdata[1] & 0x02)
        logf("Caps Lock enabled");
    if (reqdata[1] & 0x04)
        logf("Scroll Lock enabled");
    if (reqdata[1] & 0x08)
        logf("Compose enabled");
    if (reqdata[1] & 0x10)
        logf("Kana enabled");
#endif

    /* Defining other LEDs and setting them from the USB host (OS) can be used
     * to send messages to the DAP */
    usb_core_control_response(USB_CONTROL_ACK, NULL, 0);
    return 0;
}

static int usb_hid_get_report(struct usb_ctrlrequest *req, unsigned char* dest)
{
    if ((req->wValue >> 8) != USB_HID_REPORT_TYPE_FEATURE)
    {
        logf("Unsupported report type");
        return 1;
    }

    if ((req->wValue & 0xff) != REPORT_ID_BACKGROUND)
    {
        logf("Wrong report id");
        return 2;
    }

    if (req->wIndex != (uint16_t)usb_interface)
    {
        logf("Wrong interface");
        return 3;
    }

    if (req->wLength < GET_REPORT_BUF_LEN)
    {
        logf("Wrong length");
        return 4;
    }

    dest[0] = 0;
    dest[1] = battery_level();
    usb_core_control_response(USB_CONTROL_ACK, dest, 2);
    return 0;
}

/* called by usb_core_control_request() */
static bool usb_hid_control_request(struct usb_ctrlrequest *req, uint8_t* reqdata, size_t reqdata_size)
{
    unsigned char* dest = reqdata;
    unsigned char *orig_dest = dest;
    switch (req->bRequestType & USB_TYPE_MASK)
    {
    case USB_TYPE_STANDARD:
    {
        uint8_t type = req->wValue >> 8;
        size_t len;
        logf("hid: type %d %s", type, (type == USB_DT_HID) ? "hid" :
                        ((type == USB_DT_REPORT) ? "report" : ""));
        switch (type)
        {
        case USB_DT_HID:
            descriptor_hid_get(&dest);
            break;
        case USB_DT_REPORT:
            len = descriptor_report_get(report_descriptor);
            memcpy(dest, &report_descriptor, len);
            dest += len;
            break;
        }

        if (dest != orig_dest)
        {
            usb_core_control_response(USB_CONTROL_ACK, orig_dest, dest - orig_dest);
            return true;
        }
        break;
    }

    case USB_TYPE_CLASS:
    {
        logf("req %d %s", req->bRequest, 
            (req->bRequest == USB_HID_SET_IDLE) ? "set idle" :
            ((req->bRequest == USB_HID_SET_REPORT) ? "set report" :
            ((req->bRequest == USB_HID_GET_REPORT) ? "get report" : "")));

        int rc;
        switch (req->bRequest)
        {
        case USB_HID_SET_REPORT:
            rc = usb_hid_set_report(req, reqdata, reqdata_size);
            break;
        case USB_HID_GET_REPORT:
            rc = usb_hid_get_report(req, dest);
            break;
        case USB_HID_SET_IDLE:
            usb_core_control_response(USB_CONTROL_ACK, NULL, 0);
            return true;
        default:
            /* all other requests are errors */
            return false;
        }

        if(rc == 0)
            return true;
    }

    case USB_TYPE_VENDOR:
        logf("hid: unsup. ven. req");
        break;
    }
    return false;
}

static void usb_hid_try_send_drv(void)
{
    int rc;
    int length = send_buffer_len[cur_buf_send];

    if (!length)
        return;

    if (currently_sending)
    {
        logf("HID: Already sending");
        return;
    }

    logf("HID: Sending %d bytes",length);
    rc = usb_drv_send_nonblocking(EP_IN, send_buffer[cur_buf_send], length);
    currently_sending = true;
    if (rc)
        send_buffer_len[cur_buf_send] = 0;
}

static void usb_hid_queue(unsigned char *data, int length)
{
    if (!active || length <= 0)
        return;

    /* Buffer overflow - item still in use */
    if (send_buffer_len[cur_buf_prepare])
        return;

    /* Prepare buffer for sending */
    if (length > HID_BUF_SIZE_MSG)
        length = HID_BUF_SIZE_MSG;
    memcpy(send_buffer[cur_buf_prepare], data, length);
    send_buffer_len[cur_buf_prepare] = length;

    HID_BUF_INC(cur_buf_prepare);
}

void usb_hid_send(usage_page_t usage_page, int id)
{
    uint8_t length;
    static unsigned char buf[HID_BUF_SIZE_CMD] USB_DEVBSS_ATTR
        __attribute__((aligned(32)));
    usb_hid_report_t *report = NULL;

    for (uint8_t report_id = 1; report_id < REPORT_ID_COUNT; report_id++)
        if (usb_hid_reports[report_id].usage_page == usage_page)
        {
            report = &usb_hid_reports[report_id];
            buf[0] = report_id;
            break;
        }

    if (!report)
    {
        logf("Unsupported usage_page");
        return;
    }

    logf("Sending cmd 0x%x", id);
    length = report->buf_set(&buf[1], id) + 1;
    logf("length %u", length);

    if (!length)
        return;

    /* Key pressed */
    buf_dump(buf, length, "key press");
    usb_hid_queue(buf, length);

    if (report->is_key_released)
    {
        /* Key released */
        memset(&buf[1], 0, length-1);

        buf_dump(buf, length, "key release");
        usb_hid_queue(buf, length);
    }

    usb_hid_try_send_drv();
}

struct usb_class_driver usb_cdrv_hid = {
    .needs_exclusive_storage = false,
    .needs_cpu_boost = false,
    .config = 1,
    .ep_allocs_size = ARRAYLEN(ep_allocs),
    .ep_allocs = ep_allocs,
    .set_first_interface = usb_hid_set_first_interface,
    .get_config_descriptor = usb_hid_get_config_descriptor,
    .init_connection = usb_hid_init_connection,
    .init = usb_hid_init,
    .disconnect = usb_hid_disconnect,
    .transfer_complete = usb_hid_transfer_complete,
    .control_request = usb_hid_control_request,
};