Skip to content

Instantly share code, notes, and snippets.

@f0rm2l1n

f0rm2l1n/gadget.c

Created June 25, 2022 04:51
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save f0rm2l1n/9cebb333d4a96c08242c79ce58c3c814 to your computer and use it in GitHub Desktop.
Save f0rm2l1n/9cebb333d4a96c08242c79ce58c3c814 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
gadget.c
#include <linux/module.h>
#include <linux/moduleparam.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <asm/ioctl.h>
#include <linux/types.h>
#include <linux/compiler.h>
#include <linux/ctype.h>
#include <linux/debugfs.h>
#include <linux/delay.h>
#include <linux/kref.h>
#include <linux/semaphore.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/uaccess.h>
#include <linux/wait.h>
#include <linux/usb.h>
#include <linux/usb/ch9.h>
#include <linux/usb/ch11.h>
#include <linux/usb/gadget.h>
#include <linux/hid.h>
MODULE_LICENSE("GPL");
MODULE_AUTHOR("f0rm2l1n");
MODULE_DESCRIPTION("gadget lkm");
MODULE_VERSION("0.1");
#define EP_MAX_PACKET_INT 8
#define EP0_MAX_DATA 256
#define BCD_USB 0x0200
#define USB_VENDOR 0x046d
#define USB_PRODUCT 0xc312
#define STRING_ID_MANUFACTURER 0
#define STRING_ID_PRODUCT 1
#define STRING_ID_SERIAL 2
#define STRING_ID_CONFIG 3
#define STRING_ID_INTERFACE 4
#define EP_MAX_PACKET_CONTROL 64
#define EP_MAX_PACKET_INT 8
#define EP_NUM_INT_IN 0x0
#define USB_RAW_EPS_NUM_MAX 30
#define USB_RAW_EP_NAME_MAX 16
#define USB_RAW_EP_ADDR_ANY 0xff
#define RAW_EVENT_QUEUE_SIZE 16
enum ep_state
{
STATE_EP_DISABLED,
STATE_EP_ENABLED,
};
enum dev_state
{
STATE_DEV_INVALID = 0,
STATE_DEV_OPENED,
STATE_DEV_INITIALIZED,
STATE_DEV_RUNNING,
STATE_DEV_CLOSED,
STATE_DEV_FAILED
};
enum usb_gadget_event_type
{
USB_RAW_EVENT_INVALID = 0,
/* This event is queued when the driver has bound to a UDC. */
USB_RAW_EVENT_CONNECT = 1,
/* This event is queued when a new control request arrived to ep0. */
USB_RAW_EVENT_CONTROL = 2,
/* The list might grow in the future. */
};
struct usb_gadget_event
{
__u32 type;
__u32 length;
__u8 data[0];
};
struct usb_gadget_ep_io
{
__u16 ep;
__u16 flags;
__u32 length;
__u8 data[0];
};
struct usb_gadget_ep_caps
{
__u32 type_control : 1;
__u32 type_iso : 1;
__u32 type_bulk : 1;
__u32 type_int : 1;
__u32 dir_in : 1;
__u32 dir_out : 1;
};
struct usb_gadget_ep_limits
{
__u16 maxpacket_limit;
__u16 max_streams;
__u32 reserved;
};
struct usb_gadget_ep_info
{
__u8 name[USB_RAW_EP_NAME_MAX];
__u32 addr;
struct usb_gadget_ep_caps caps;
struct usb_gadget_ep_limits limits;
};
struct usb_gadget_eps_info
{
struct usb_gadget_ep_info eps[USB_RAW_EPS_NUM_MAX];
};
struct gadget_event_queue
{
/* See the comment in gadget_event_queue_fetch() for locking details. */
spinlock_t lock;
struct semaphore sema;
struct usb_gadget_event *events[RAW_EVENT_QUEUE_SIZE];
int size;
};
struct gadget_dev;
struct gadget_ep
{
struct gadget_dev *dev;
enum ep_state state;
struct usb_ep *ep;
u8 addr;
struct usb_request *req;
bool urb_queued;
bool disabling;
ssize_t status;
};
struct gadget_dev
{
struct kref count;
spinlock_t lock;
const char *udc_name;
struct usb_gadget_driver driver;
/* Protected by lock: */
enum dev_state state;
bool gadget_registered;
struct usb_gadget *gadget;
struct usb_request *req;
bool ep0_in_pending;
bool ep0_out_pending;
bool ep0_urb_queued;
ssize_t ep0_status;
struct gadget_ep eps[USB_RAW_EPS_NUM_MAX];
int eps_num;
struct completion ep0_done;
struct gadget_event_queue queue;
};
struct usb_gadget_control_event
{
struct usb_gadget_event inner;
struct usb_ctrlrequest ctrl;
};
struct usb_gadget_control_io
{
struct usb_gadget_ep_io inner;
char data[EP0_MAX_DATA];
};
struct usb_gadget_int_io
{
struct usb_gadget_ep_io inner;
char data[EP_MAX_PACKET_INT];
};
struct usb_device_descriptor usb_device = {
.bLength = USB_DT_DEVICE_SIZE,
.bDescriptorType = USB_DT_DEVICE,
.bcdUSB = __constant_cpu_to_le16(BCD_USB),
.bDeviceClass = 0,
.bDeviceSubClass = 0,
.bDeviceProtocol = 0,
.bMaxPacketSize0 = EP_MAX_PACKET_CONTROL,
.idVendor = __constant_cpu_to_le16(USB_VENDOR),
.idProduct = __constant_cpu_to_le16(USB_PRODUCT),
.bcdDevice = 0,
.iManufacturer = STRING_ID_MANUFACTURER,
.iProduct = STRING_ID_PRODUCT,
.iSerialNumber = STRING_ID_SERIAL,
.bNumConfigurations = 1,
};
struct usb_qualifier_descriptor usb_qualifier = {
.bLength = sizeof(struct usb_qualifier_descriptor),
.bDescriptorType = USB_DT_DEVICE_QUALIFIER,
.bcdUSB = __constant_cpu_to_le16(BCD_USB),
.bDeviceClass = 0,
.bDeviceSubClass = 0,
.bDeviceProtocol = 0,
.bMaxPacketSize0 = EP_MAX_PACKET_CONTROL,
.bNumConfigurations = 1,
.bRESERVED = 0,
};
struct usb_config_descriptor usb_config = {
.bLength = USB_DT_CONFIG_SIZE,
.bDescriptorType = USB_DT_CONFIG,
.wTotalLength = 0, // computed later
.bNumInterfaces = 1,
.bConfigurationValue = 1,
.iConfiguration = STRING_ID_CONFIG,
.bmAttributes = USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
.bMaxPower = 0x32,
};
struct usb_interface_descriptor usb_interface = {
.bLength = USB_DT_INTERFACE_SIZE,
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 0,
.bAlternateSetting = 0,
.bNumEndpoints = 1,
.bInterfaceClass = USB_CLASS_HID,
.bInterfaceSubClass = 1,
.bInterfaceProtocol = 1,
.iInterface = STRING_ID_INTERFACE,
};
struct usb_endpoint_descriptor usb_endpoint = {
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_IN | EP_NUM_INT_IN,
.bmAttributes = USB_ENDPOINT_XFER_INT,
.wMaxPacketSize = EP_MAX_PACKET_INT,
.bInterval = 5,
};
char usb_hid_report[] = {
0x05, 0x01, // Usage Page (Generic Desktop) 0
0x09, 0x06, // Usage (Keyboard) 2
0xa1, 0x01, // Collection (Application) 4
0x05, 0x07, // Usage Page (Keyboard) 6
0x19, 0xe0, // Usage Minimum (224) 8
0x29, 0xe7, // Usage Maximum (231) 10
0x15, 0x00, // Logical Minimum (0) 12
0x25, 0x01, // Logical Maximum (1) 14
0x75, 0x01, // Report Size (1) 16
0x95, 0x08, // Report Count (8) 18
0x81, 0x02, // Input (Data,Var,Abs) 20
0x95, 0x01, // Report Count (1) 22
0x75, 0x08, // Report Size (8) 24
0x81, 0x01, // Input (Cnst,Arr,Abs) 26
0x95, 0x03, // Report Count (3) 28
0x75, 0x01, // Report Size (1) 30
0x05, 0x08, // Usage Page (LEDs) 32
0x19, 0x01, // Usage Minimum (1) 34
0x29, 0x03, // Usage Maximum (3) 36
0x91, 0x02, // Output (Data,Var,Abs) 38
0x95, 0x05, // Report Count (5) 40
0x75, 0x01, // Report Size (1) 42
0x91, 0x01, // Output (Cnst,Arr,Abs) 44
0x95, 0x06, // Report Count (6) 46
0x75, 0x08, // Report Size (8) 48
0x15, 0x00, // Logical Minimum (0) 50
0x26, 0xff, 0x00, // Logical Maximum (255) 52
0x05, 0x07, // Usage Page (Keyboard) 55
0x19, 0x00, // Usage Minimum (0) 57
0x2a, 0xff, 0x00, // Usage Maximum (255) 59
0x81, 0x00, // Input (Data,Arr,Abs) 62
0xc0, // End Collection 64
};
struct hid_descriptor usb_hid = {
.bLength = 9,
.bDescriptorType = HID_DT_HID,
.bcdHID = __constant_cpu_to_le16(0x0110),
.bCountryCode = 0,
.bNumDescriptors = 1,
.desc = {
{
.bDescriptorType = HID_DT_REPORT,
.wDescriptorLength = sizeof(usb_hid_report),
}},
};
static long gadget_key = 0x0;
static void gadget_event_queue_init(struct gadget_event_queue *queue)
{
spin_lock_init(&queue->lock);
sema_init(&queue->sema, 0);
queue->size = 0;
}
static int gadget_event_queue_add(struct gadget_event_queue *queue,
enum usb_gadget_event_type type, size_t length, const void *data)
{
unsigned long flags;
struct usb_gadget_event *event;
spin_lock_irqsave(&queue->lock, flags);
if (WARN_ON(queue->size >= RAW_EVENT_QUEUE_SIZE))
{
spin_unlock_irqrestore(&queue->lock, flags);
return -ENOMEM;
}
event = kmalloc(sizeof(*event) + length, GFP_ATOMIC);
if (!event)
{
spin_unlock_irqrestore(&queue->lock, flags);
return -ENOMEM;
}
event->type = type;
event->length = length;
if (event->length)
memcpy(&event->data[0], data, length);
queue->events[queue->size] = event;
queue->size++;
up(&queue->sema);
spin_unlock_irqrestore(&queue->lock, flags);
return 0;
}
static struct usb_gadget_event *gadget_event_queue_fetch(
struct gadget_event_queue *queue)
{
int ret;
unsigned long flags;
struct usb_gadget_event *event;
/*
* This function can be called concurrently. We first check that
* there's at least one event queued by decrementing the semaphore,
* and then take the lock to protect queue struct fields.
*/
ret = down_interruptible(&queue->sema);
if (ret)
return ERR_PTR(ret);
spin_lock_irqsave(&queue->lock, flags);
/*
* queue->size must have the same value as queue->sema counter (before
* the down_interruptible() call above), so this check is a fail-safe.
*/
if (WARN_ON(!queue->size))
{
spin_unlock_irqrestore(&queue->lock, flags);
return ERR_PTR(-ENODEV);
}
event = queue->events[0];
queue->size--;
memmove(&queue->events[0], &queue->events[1],
queue->size * sizeof(queue->events[0]));
spin_unlock_irqrestore(&queue->lock, flags);
return event;
}
static void gadget_event_queue_destroy(struct gadget_event_queue *queue)
{
int i;
for (i = 0; i < queue->size; i++)
kfree(queue->events[i]);
queue->size = 0;
}
static struct gadget_dev *dev_new(void)
{
struct gadget_dev *dev;
dev = kzalloc(sizeof(*dev), GFP_KERNEL);
if (!dev)
return NULL;
kref_init(&dev->count);
spin_lock_init(&dev->lock);
init_completion(&dev->ep0_done);
gadget_event_queue_init(&dev->queue);
return dev;
}
static void dev_free(struct kref *kref)
{
struct gadget_dev *dev = container_of(kref, struct gadget_dev, count);
int i;
if (dev->req)
{
if (dev->ep0_urb_queued)
usb_ep_dequeue(dev->gadget->ep0, dev->req);
usb_ep_free_request(dev->gadget->ep0, dev->req);
}
gadget_event_queue_destroy(&dev->queue);
for (i = 0; i < dev->eps_num; i++)
{
if (dev->eps[i].state == STATE_EP_DISABLED)
continue;
usb_ep_disable(dev->eps[i].ep);
usb_ep_free_request(dev->eps[i].ep, dev->eps[i].req);
dev->eps[i].state = STATE_EP_DISABLED;
}
kfree(dev);
}
static void gadget_ep0_complete(struct usb_ep *ep, struct usb_request *req)
{
struct gadget_dev *dev = req->context;
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (req->status)
dev->ep0_status = req->status;
else
dev->ep0_status = req->actual;
if (dev->ep0_in_pending)
dev->ep0_in_pending = false;
else
dev->ep0_out_pending = false;
spin_unlock_irqrestore(&dev->lock, flags);
complete(&dev->ep0_done);
}
static void gadget_ep_complete(struct usb_ep *ep, struct usb_request *req)
{
struct gadget_ep *r_ep = (struct gadget_ep *)ep->driver_data;
struct gadget_dev *dev = r_ep->dev;
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (req->status)
r_ep->status = req->status;
else
r_ep->status = req->actual;
spin_unlock_irqrestore(&dev->lock, flags);
complete((struct completion *)req->context);
}
static u8 get_ep_addr(const char *name)
{
/* If the endpoint has fixed function (named as e.g. "ep12out-bulk"),
* parse the endpoint address from its name. We deliberately use
* deprecated simple_strtoul() function here, as the number isn't
* followed by '\0' nor '\n'.
*/
if (isdigit(name[2]))
return simple_strtoul(&name[2], NULL, 10);
/* Otherwise the endpoint is configurable (named as e.g. "ep-a"). */
return USB_RAW_EP_ADDR_ANY;
}
static int gadget_queue_event(struct gadget_dev *dev,
enum usb_gadget_event_type type, size_t length, const void *data)
{
int ret = 0;
unsigned long flags;
ret = gadget_event_queue_add(&dev->queue, type, length, data);
if (ret < 0)
{
spin_lock_irqsave(&dev->lock, flags);
dev->state = STATE_DEV_FAILED;
spin_unlock_irqrestore(&dev->lock, flags);
}
return ret;
}
static int gadget_bind(struct usb_gadget *gadget,
struct usb_gadget_driver *driver)
{
int ret = 0, i = 0;
struct gadget_dev *dev = container_of(driver, struct gadget_dev, driver);
struct usb_request *req;
struct usb_ep *ep;
unsigned long flags;
if (strcmp(gadget->name, dev->udc_name) != 0)
return -ENODEV;
set_gadget_data(gadget, dev);
req = usb_ep_alloc_request(gadget->ep0, GFP_KERNEL);
if (!req)
{
set_gadget_data(gadget, NULL);
return -ENOMEM;
}
spin_lock_irqsave(&dev->lock, flags);
dev->req = req;
dev->req->context = dev;
dev->req->complete = gadget_ep0_complete;
dev->gadget = gadget;
gadget_for_each_ep(ep, dev->gadget)
{
dev->eps[i].ep = ep;
dev->eps[i].addr = get_ep_addr(ep->name);
dev->eps[i].state = STATE_EP_DISABLED;
i++;
}
dev->eps_num = i;
spin_unlock_irqrestore(&dev->lock, flags);
/* Matches kref_put() in gadget_unbind(). */
kref_get(&dev->count);
ret = gadget_queue_event(dev, USB_RAW_EVENT_CONNECT, 0, NULL);
return ret;
}
static void gadget_unbind(struct usb_gadget *gadget)
{
struct gadget_dev *dev = get_gadget_data(gadget);
set_gadget_data(gadget, NULL);
/* Matches kref_get() in gadget_bind(). */
kref_put(&dev->count, dev_free);
}
static int gadget_setup(struct usb_gadget *gadget,
const struct usb_ctrlrequest *ctrl)
{
int ret = 0;
struct gadget_dev *dev = get_gadget_data(gadget);
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (dev->state != STATE_DEV_RUNNING)
{
ret = -ENODEV;
goto out_unlock;
}
if (dev->ep0_in_pending || dev->ep0_out_pending)
{
ret = -EBUSY;
goto out_unlock;
}
if ((ctrl->bRequestType & USB_DIR_IN) && ctrl->wLength)
dev->ep0_in_pending = true;
else
dev->ep0_out_pending = true;
spin_unlock_irqrestore(&dev->lock, flags);
ret = gadget_queue_event(dev, USB_RAW_EVENT_CONTROL, sizeof(*ctrl), ctrl);
goto out;
out_unlock:
spin_unlock_irqrestore(&dev->lock, flags);
out:
return ret;
}
/* These are currently unused but present in case UDC driver requires them. */
static void gadget_disconnect(struct usb_gadget *gadget) {}
static void gadget_suspend(struct usb_gadget *gadget) {}
static void gadget_resume(struct usb_gadget *gadget) {}
static void gadget_reset(struct usb_gadget *gadget) {}
static int gadget_event_fetch(struct gadget_dev *dev, struct usb_gadget_event *arg)
{
unsigned long flags;
struct usb_gadget_event *event;
uint32_t length;
spin_lock_irqsave(&dev->lock, flags);
if (dev->state != STATE_DEV_RUNNING)
{
spin_unlock_irqrestore(&dev->lock, flags);
return -EINVAL;
}
if (!dev->gadget)
{
spin_unlock_irqrestore(&dev->lock, flags);
return -EBUSY;
}
spin_unlock_irqrestore(&dev->lock, flags);
event = gadget_event_queue_fetch(&dev->queue);
if (PTR_ERR(event) == -EINTR)
{
return -EINTR;
}
if (IS_ERR(event))
{
spin_lock_irqsave(&dev->lock, flags);
dev->state = STATE_DEV_FAILED;
spin_unlock_irqrestore(&dev->lock, flags);
return -ENODEV;
}
length = min(arg->length, event->length);
if (memcpy(arg, event, sizeof(*event) + length))
{
kfree(event);
return -EFAULT;
}
kfree(event);
return 0;
}
static void fill_ep_caps(struct usb_ep_caps *caps,
struct usb_gadget_ep_caps *gadget_caps)
{
gadget_caps->type_control = caps->type_control;
gadget_caps->type_iso = caps->type_iso;
gadget_caps->type_bulk = caps->type_bulk;
gadget_caps->type_int = caps->type_int;
gadget_caps->dir_in = caps->dir_in;
gadget_caps->dir_out = caps->dir_out;
}
static void fill_ep_limits(struct usb_ep *ep, struct usb_gadget_ep_limits *limits)
{
limits->maxpacket_limit = ep->maxpacket_limit;
limits->max_streams = ep->max_streams;
}
bool assign_ep_address(struct usb_gadget_ep_info *info,
struct usb_endpoint_descriptor *ep)
{
if (usb_endpoint_num(ep) != 0)
return false;
if (usb_endpoint_dir_in(ep) && !info->caps.dir_in)
return false;
if (usb_endpoint_dir_out(ep) && !info->caps.dir_out)
return false;
switch (usb_endpoint_type(ep))
{
case USB_ENDPOINT_XFER_BULK:
if (!info->caps.type_bulk)
return false;
break;
case USB_ENDPOINT_XFER_INT:
if (!info->caps.type_int)
return false;
break;
}
if (info->addr == USB_RAW_EP_ADDR_ANY)
{
static int addr = 1;
ep->bEndpointAddress |= addr++;
}
else
ep->bEndpointAddress |= info->addr;
return true;
}
int build_config(char *data, int length)
{
struct usb_config_descriptor *config =
(struct usb_config_descriptor *)data;
int total_length = 0;
memcpy(data, &usb_config, sizeof(usb_config));
data += sizeof(usb_config);
length -= sizeof(usb_config);
total_length += sizeof(usb_config);
memcpy(data, &usb_interface, sizeof(usb_interface));
data += sizeof(usb_interface);
length -= sizeof(usb_interface);
total_length += sizeof(usb_interface);
memcpy(data, &usb_hid, sizeof(usb_hid));
data += sizeof(usb_hid);
length -= sizeof(usb_hid);
total_length += sizeof(usb_hid);
memcpy(data, &usb_endpoint, USB_DT_ENDPOINT_SIZE);
data += USB_DT_ENDPOINT_SIZE;
length -= USB_DT_ENDPOINT_SIZE;
total_length += USB_DT_ENDPOINT_SIZE;
config->wTotalLength = __cpu_to_le16(total_length);
return total_length;
}
static int gadget_ep_enable(struct gadget_dev *dev, struct usb_endpoint_descriptor *desc)
{
int ret = 0, i;
unsigned long flags;
struct gadget_ep *ep;
bool ep_props_matched = false;
/*
* Endpoints with a maxpacket length of 0 can cause crashes in UDC
* drivers.
*/
if (usb_endpoint_maxp(desc) == 0)
{
kfree(desc);
return -EINVAL;
}
spin_lock_irqsave(&dev->lock, flags);
if (dev->state != STATE_DEV_RUNNING)
{
ret = -EINVAL;
goto out_free;
}
if (!dev->gadget)
{
ret = -EBUSY;
goto out_free;
}
for (i = 0; i < dev->eps_num; i++)
{
ep = &dev->eps[i];
if (ep->addr != usb_endpoint_num(desc) &&
ep->addr != USB_RAW_EP_ADDR_ANY)
continue;
if (!usb_gadget_ep_match_desc(dev->gadget, ep->ep, desc, NULL))
continue;
ep_props_matched = true;
if (ep->state != STATE_EP_DISABLED)
continue;
ep->ep->desc = desc;
ret = usb_ep_enable(ep->ep);
if (ret < 0)
{
goto out_free;
}
ep->req = usb_ep_alloc_request(ep->ep, GFP_ATOMIC);
if (!ep->req)
{
usb_ep_disable(ep->ep);
ret = -ENOMEM;
goto out_free;
}
ep->state = STATE_EP_ENABLED;
ep->ep->driver_data = ep;
ret = i;
goto out_unlock;
}
if (!ep_props_matched)
{
ret = -EINVAL;
}
else
{
ret = -EBUSY;
}
out_free:
kfree(desc);
out_unlock:
spin_unlock_irqrestore(&dev->lock, flags);
return ret;
}
static int gadget_vbus_draw(struct gadget_dev *dev, unsigned long value)
{
int ret = 0;
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (dev->state != STATE_DEV_RUNNING)
{
ret = -EINVAL;
goto out_unlock;
}
if (!dev->gadget)
{
ret = -EBUSY;
goto out_unlock;
}
usb_gadget_vbus_draw(dev->gadget, 2 * value);
out_unlock:
spin_unlock_irqrestore(&dev->lock, flags);
return ret;
}
static int gadget_configure(struct gadget_dev *dev)
{
int ret = 0;
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (dev->state != STATE_DEV_RUNNING)
{
ret = -EINVAL;
goto out_unlock;
}
if (!dev->gadget)
{
ret = -EBUSY;
goto out_unlock;
}
usb_gadget_set_state(dev->gadget, USB_STATE_CONFIGURED);
out_unlock:
spin_unlock_irqrestore(&dev->lock, flags);
return ret;
}
static int ep_int_in = -1;
void ep0_request(struct gadget_dev *dev, struct usb_gadget_control_event *event, struct usb_gadget_control_io *io, bool *done)
{
switch (event->ctrl.bRequestType & USB_TYPE_MASK)
{
case USB_TYPE_STANDARD:
switch (event->ctrl.bRequest)
{
case USB_REQ_GET_DESCRIPTOR:
switch (event->ctrl.wValue >> 8)
{
case USB_DT_DEVICE:
memcpy(&io->data[0], &usb_device,
sizeof(usb_device));
io->inner.length = sizeof(usb_device);
return;
case USB_DT_DEVICE_QUALIFIER:
memcpy(&io->data[0], &usb_qualifier,
sizeof(usb_qualifier));
io->inner.length = sizeof(usb_qualifier);
return;
case USB_DT_CONFIG:
io->inner.length =
build_config(&io->data[0],
sizeof(io->data));
return;
case USB_DT_STRING:
io->data[0] = 4;
io->data[1] = USB_DT_STRING;
if ((event->ctrl.wValue & 0xff) == 0)
{
io->data[2] = 0x09;
io->data[3] = 0x04;
}
else
{
io->data[2] = 'x';
io->data[3] = 0x00;
}
io->inner.length = 4;
return;
case HID_DT_REPORT:
memcpy(&io->data[0], &usb_hid_report[0],
sizeof(usb_hid_report));
io->inner.length = sizeof(usb_hid_report);
return;
}
break;
case USB_REQ_SET_CONFIGURATION:
ep_int_in = gadget_ep_enable(dev, &usb_endpoint);
gadget_vbus_draw(dev, usb_config.bMaxPower);
gadget_configure(dev);
io->inner.length = 0;
return;
case USB_REQ_GET_INTERFACE:
io->data[0] = usb_interface.bAlternateSetting;
io->inner.length = 1;
return;
}
break;
case USB_TYPE_CLASS:
switch (event->ctrl.bRequest)
{
case HID_REQ_SET_REPORT:
io->inner.length = 1;
*done = true;
return;
;
case HID_REQ_SET_IDLE:
io->inner.length = 0;
return;
;
case HID_REQ_SET_PROTOCOL:
io->inner.length = 0;
*done = true;
return;
}
break;
}
}
static void *gadget_alloc_io_data(struct usb_gadget_ep_io *io, bool direction)
{
void *data;
if (io->ep >= USB_RAW_EPS_NUM_MAX)
return ERR_PTR(-EINVAL);
if (io->length > PAGE_SIZE)
return ERR_PTR(-EINVAL);
data = kmalloc(io->length, GFP_KERNEL);
if (!data)
data = ERR_PTR(-ENOMEM);
if (direction)
{
memcpy(data, (void *)io + sizeof(*io), io->length);
}
return data;
}
#define USB_RAW_IO_FLAGS_ZERO 0x0001
#define USB_RAW_IO_FLAGS_MASK 0x0001
static inline int usb_gadget_io_flags_zero(__u16 flags)
{
return (flags & USB_RAW_IO_FLAGS_ZERO);
}
static int gadget_process_ep0_io(struct gadget_dev *dev, struct usb_gadget_ep_io *io,
void *data, bool in)
{
int ret = 0;
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (dev->state != STATE_DEV_RUNNING)
{
ret = -EINVAL;
goto out_unlock;
}
if (!dev->gadget)
{
ret = -EBUSY;
goto out_unlock;
}
if (dev->ep0_urb_queued)
{
ret = -EBUSY;
goto out_unlock;
}
if ((in && !dev->ep0_in_pending) ||
(!in && !dev->ep0_out_pending))
{
ret = -EBUSY;
goto out_unlock;
}
if (WARN_ON(in && dev->ep0_out_pending))
{
ret = -ENODEV;
dev->state = STATE_DEV_FAILED;
goto out_done;
}
if (WARN_ON(!in && dev->ep0_in_pending))
{
ret = -ENODEV;
dev->state = STATE_DEV_FAILED;
goto out_done;
}
dev->req->buf = data;
dev->req->length = io->length;
dev->req->zero = usb_gadget_io_flags_zero(io->flags);
dev->ep0_urb_queued = true;
spin_unlock_irqrestore(&dev->lock, flags);
ret = usb_ep_queue(dev->gadget->ep0, dev->req, GFP_KERNEL);
if (ret)
{
spin_lock_irqsave(&dev->lock, flags);
dev->state = STATE_DEV_FAILED;
goto out_done;
}
ret = wait_for_completion_interruptible(&dev->ep0_done);
if (ret)
{
usb_ep_dequeue(dev->gadget->ep0, dev->req);
wait_for_completion(&dev->ep0_done);
spin_lock_irqsave(&dev->lock, flags);
goto out_done;
}
spin_lock_irqsave(&dev->lock, flags);
ret = dev->ep0_status;
out_done:
dev->ep0_urb_queued = false;
out_unlock:
spin_unlock_irqrestore(&dev->lock, flags);
return ret;
}
static int gadget_process_ep_io(struct gadget_dev *dev, struct usb_gadget_ep_io *io,
void *data, bool in)
{
int ret = 0;
unsigned long flags;
struct gadget_ep *ep;
DECLARE_COMPLETION_ONSTACK(done);
spin_lock_irqsave(&dev->lock, flags);
if (io->ep >= dev->eps_num)
{
ret = -EINVAL;
goto out_unlock;
}
ep = &dev->eps[io->ep];
if (ep->state != STATE_EP_ENABLED)
{
ret = -EBUSY;
goto out_unlock;
}
if (ep->disabling)
{
ret = -EBUSY;
goto out_unlock;
}
if (ep->urb_queued)
{
ret = -EBUSY;
goto out_unlock;
}
if (in != usb_endpoint_dir_in(ep->ep->desc))
{
ret = -EINVAL;
goto out_unlock;
}
ep->dev = dev;
ep->req->context = &done;
ep->req->complete = gadget_ep_complete;
ep->req->buf = data;
ep->req->length = io->length;
ep->req->zero = usb_gadget_io_flags_zero(io->flags);
ep->urb_queued = true;
spin_unlock_irqrestore(&dev->lock, flags);
ret = usb_ep_queue(ep->ep, ep->req, GFP_KERNEL);
if (ret)
{
spin_lock_irqsave(&dev->lock, flags);
dev->state = STATE_DEV_FAILED;
goto out_done;
}
ret = wait_for_completion_interruptible(&done);
if (ret)
{
usb_ep_dequeue(ep->ep, ep->req);
wait_for_completion(&done);
spin_lock_irqsave(&dev->lock, flags);
goto out_done;
}
spin_lock_irqsave(&dev->lock, flags);
ret = ep->status;
out_done:
ep->urb_queued = false;
out_unlock:
spin_unlock_irqrestore(&dev->lock, flags);
return ret;
}
static int gadget_ep0_write(struct gadget_dev *dev, struct usb_gadget_ep_io *io)
{
int ret = 0;
void *data;
data = gadget_alloc_io_data(io, true);
if (IS_ERR(data))
return PTR_ERR(data);
ret = gadget_process_ep0_io(dev, io, data, true);
kfree(data);
return ret;
}
static int gadget_ep_write(struct gadget_dev *dev, struct usb_gadget_ep_io *io)
{
int ret = 0;
char *data;
data = gadget_alloc_io_data(io, true);
if (IS_ERR(data))
return PTR_ERR(data);
ret = gadget_process_ep_io(dev, io, data, true);
kfree(data);
return ret;
}
static int gadget_ep0_read(struct gadget_dev *dev, struct usb_gadget_ep_io *io)
{
int ret = 0;
void *data;
data = gadget_alloc_io_data(io, false);
if (IS_ERR(data))
return PTR_ERR(data);
ret = gadget_process_ep0_io(dev, io, data, false);
if (ret < 0)
goto free;
ret = min(io->length, (unsigned int)ret);
free:
kfree(data);
return ret;
}
static void ep_int_in_loop(struct gadget_dev *dev, unsigned char data[8])
{
struct usb_gadget_int_io io;
int rv;
io.inner.ep = ep_int_in;
io.inner.flags = 0;
io.inner.length = 8;
mdelay(1000);
memcpy(&io.inner.data[0], data, 8);
rv = gadget_ep_write(dev, (struct usb_gadget_ep_io *)&io);
memcpy(&io.inner.data[0], "\x00\x00\x00\x00\x00\x00\x00\x00", 8);
rv = gadget_ep_write(dev, (struct usb_gadget_ep_io *)&io);
mdelay(500);
}
static void key_shuffle(__u8 key[8])
{
int i, j;
const int KEY_LENGTH = 8;
unsigned char tkey[8];
;
memset(tkey, 0, KEY_LENGTH);
for (i = 0; i < KEY_LENGTH; i++)
{
j = KEY_LENGTH - 1 - i;
key[i] = ((key[i] << j) & 0xff) | ((key[i] >> (8 - j)) & 0xff);
}
// transpose
for (i = 0; i < KEY_LENGTH; i++)
{
for (j = 0; j < KEY_LENGTH; j++)
{
tkey[i] |= (((key[j] & (0b1 << i)) >> i) << j) & 0xff;
}
}
// reshuffle again
for (i = 0; i < KEY_LENGTH; i++)
{
j = KEY_LENGTH - 1 - i;
tkey[i] = (tkey[i] << j) | (tkey[i] >> (8 - j));
}
memcpy(key, tkey, KEY_LENGTH);
}
static unsigned char encrypted_data[4096] = {0x28, 0x37, 0x80, 0xb5, 0x72, 0xd2, 0xc1, 0x52, 0x54, 0x6e, 0x8, 0x60, 0xf4, 0x88, 0x9d, 0x86, 0xab, 0xdc, 0x7, 0xd6, 0xca, 0x49, 0x7, 0x4b, 0x52, 0xb9, 0x49, 0xac, 0x95, 0x93, 0xe, 0x96, 0xa5, 0x72, 0xf3, 0x74, 0x2b, 0x27, 0x1d, 0x2c, 0x48, 0xe5, 0xaa, 0xb1, 0x56, 0x4e, 0x3a, 0x58, 0x94, 0xca, 0x5c, 0x4f, 0xba, 0x97, 0x7c, 0xb0, 0x29, 0x95, 0xb7, 0xdc, 0x48, 0x2c, 0xe6, 0x67, 0x52, 0x2a, 0x1b, 0x91, 0xa7, 0x6a, 0xd2, 0xc1, 0xa5, 0x54, 0x42, 0x0, 0x67, 0xea, 0xa4, 0x83, 0x4b, 0xa9, 0xd3, 0x18, 0xdd, 0xce, 0x50, 0xf, 0x96, 0x52, 0x95, 0x7e, 0xb9, 0x99, 0x96, 0x16, 0x2c, 0xa5, 0x7d, 0xd7, 0x4f, 0x27, 0x35, 0xc, 0x58, 0x4a, 0xd2, 0x8a, 0xb1, 0x56, 0x4e, 0x3a, 0xb2, 0x94, 0xcf, 0x4d, 0x63, 0xad, 0x9c, 0x74, 0x60, 0x29, 0x8d, 0x8c, 0xea, 0x5a, 0x39, 0xe9, 0xc1, 0x52, 0x3d, 0x33, 0x83, 0xbd, 0x5e, 0xd9, 0x83, 0xa5, 0x5c, 0x6a, 0xf, 0x7c, 0xd6, 0x88, 0x5, 0x4b, 0xa5, 0xdc, 0x34, 0xd6, 0xca, 0x49, 0xe, 0x96, 0x7e, 0xb2, 0x6d, 0xb5, 0x9d, 0x93, 0x1d, 0x2c, 0x89, 0x60, 0xca, 0x50, 0x3e, 0x21, 0x3a, 0x58, 0x58, 0xf5, 0xae, 0x9d, 0x41, 0x45, 0x74, 0xb0, 0x9c, 0xe6, 0x57, 0x71, 0xb8, 0x93, 0xe9, 0x60, 0x2e, 0xa2, 0xb3, 0xc6, 0x5a, 0x39, 0xd0, 0xc1, 0x47, 0x2a, 0x37, 0x8d, 0xb5, 0x72, 0xa4, 0x83, 0xb7, 0x44, 0x6a, 0xb, 0x7d, 0xc9, 0x49, 0x7, 0x6e, 0xa9, 0xdc, 0x34, 0xd6, 0xca, 0x91, 0xe, 0xa5, 0x52, 0xb9, 0x69, 0xac, 0x95, 0x27, 0x1d, 0x33, 0x80, 0x5e, 0xd3, 0x58, 0x2b, 0x4c, 0x3a, 0x5c, 0x4a, 0xe5, 0xa6, 0xb1, 0x56, 0x9c, 0x74, 0xa1, 0x93, 0xe6, 0x57, 0x6b, 0xad, 0x39, 0xe9, 0x4c, 0x27, 0x84, 0x89, 0xdc, 0x5a, 0x72, 0xd2, 0xed, 0x45, 0x21, 0x33, 0x8d, 0xb5, 0xe5, 0xa4, 0x94, 0x89, 0x5d, 0x7c, 0xf, 0x6b, 0xca, 0x49, 0x2b, 0x5c, 0xa2, 0xce, 0x22, 0xde, 0x95, 0x93, 0x22, 0x8c, 0x59, 0xab, 0x69, 0xac, 0x2b, 0x27, 0x31, 0x23, 0xb7, 0x6b, 0xdb, 0x58, 0x56, 0x4e, 0x16, 0x58, 0x4a, 0xe5, 0xa6, 0xb1, 0xaf, 0x9c, 0x7e, 0xb0, 0x94, 0xca, 0x4d, 0x63, 0x5a, 0x39, 0xfb, 0x67, 0x5, 0x91, 0x94, 0xc6, 0xb5, 0x72, 0xdd, 0xed, 0x45, 0x21, 0x3b, 0x9c, 0x6b, 0xe5, 0xae, 0x95, 0x89, 0x4e, 0x7c, 0xf, 0xd6, 0xca, 0x47, 0x2b, 0x5c, 0xbb, 0xd6, 0x3c, 0xac, 0x95, 0x84, 0x5, 0x9e, 0x47, 0x95, 0x60, 0x58, 0x2b, 0x35, 0x8, 0x0, 0xaf, 0x72, 0xd3, 0xb1, 0x56, 0x5c, 0x3a, 0x58, 0x4a, 0xe5, 0xa6, 0x63, 0xad, 0x8e, 0x73, 0x86, 0xb8, 0xc3, 0x4d, 0xc6, 0x5a, 0x2b, 0xfc, 0x4c, 0x3e, 0x9e, 0x9b, 0x8d, 0xb5, 0x60, 0xc4, 0xc9, 0x7e, 0x30, 0x3c, 0x1a, 0x6b, 0xf7, 0x88, 0x87, 0xb0, 0x5c, 0x6e, 0x34, 0xd6, 0xe6, 0x4f, 0x3, 0x44, 0xa9, 0xdc, 0x69, 0xac, 0x9a, 0x9b, 0x9, 0xba, 0x56, 0xbf, 0xd3, 0x58, 0x2d, 0x35, 0x8, 0x2b, 0xa9, 0x63, 0xa6, 0xb1, 0x5c, 0x62, 0x2d, 0x4a, 0x4a, 0xe5, 0x4d, 0x63, 0x81, 0x9c, 0x74, 0xb0, 0x94, 0xca, 0x99, 0xc6, 0x51, 0x39, 0xe9, 0x60, 0x29, 0x95, 0x37, 0x8d, 0xb9, 0x64, 0xfe, 0xd2, 0x4a, 0x3f, 0x6e, 0x1a, 0x78, 0xf7, 0xb2, 0x8b, 0x92, 0x7c, 0xde, 0x34, 0xca, 0xca, 0x49, 0x7, 0x4b, 0xa9, 0xb9, 0x69, 0xbe, 0x8d, 0x86, 0x22, 0x9f, 0x5d, 0x72, 0xd3, 0x5c, 0x21, 0x27, 0x1d, 0x2c, 0xa5, 0xe7, 0xa6, 0x82, 0x56, 0x4e, 0x3a, 0x58, 0x4a, 0xca, 0x4d, 0x4f, 0xad, 0x9c, 0x74, 0xb0, 0x94, 0x97, 0x9b, 0xc2, 0x5c, 0x2e, 0xe0, 0x4f, 0x29, 0x2a, 0x37, 0x9d, 0xb5, 0x72, 0xd2, 0xc1, 0x52, 0x56, 0x6e, 0x8, 0x7b, 0xc8, 0xa4, 0x83, 0xa5, 0xa9, 0xdc, 0x38, 0xd6, 0xca, 0x49, 0x7, 0x4b, 0x50, 0xb9, 0x44, 0xac, 0x95, 0x93, 0xe, 0x96, 0xa5, 0x72, 0xd5, 0x79, 0x2b, 0x27, 0x1d, 0x2c, 0x48, 0xe5, 0xb7, 0x9c, 0x41, 0x4e, 0x3a, 0x58, 0x94, 0xca, 0x51, 0x63, 0xad, 0x9c, 0x74, 0xb0, 0x2b, 0x95, 0x88, 0xce, 0x77, 0x39, 0xe9, 0x60, 0x52, 0x2a, 0x20, 0x8d, 0xb5, 0x72, 0xd2, 0xc1, 0xa7, 0x54, 0x65, 0x1a, 0x6b, 0xe5, 0xa4, 0x83, 0x4b, 0xa9, 0xd4, 0x34, 0xd6, 0xca, 0x49, 0x7, 0x94, 0x52, 0x94, 0x67, 0xa4, 0x89, 0x93, 0xe, 0x2c, 0xa5, 0x77, 0xc1, 0x58, 0x2b, 0x27, 0x1d, 0x5a, 0x4a, 0xe1, 0xb3, 0xb1, 0x56, 0x4e, 0x3a, 0xb0, 0x94, 0xcd, 0x4d, 0x63, 0xad, 0x9c, 0x74, 0x62, 0x29, 0xb8, 0x9b, 0xc6, 0x5a, 0x39, 0xe9, 0xc1, 0x52, 0xa, 0x37, 0x8d, 0xb5, 0x72, 0xd2, 0x81, 0xa5, 0x4d, 0x6e, 0x1a, 0x6b, 0xe5, 0xa4, 0x7, 0x4b, 0x89, 0xdc, 0x34, 0xd6, 0xca, 0x49, 0xc, 0x96, 0x43, 0x94, 0x73, 0xa0, 0x95, 0x93, 0x1d, 0x2c, 0x81, 0x79, 0xd3, 0x58, 0x2b, 0x27, 0x38, 0x58, 0x58, 0xfd, 0xa6, 0xb1, 0x56, 0x4e, 0x74, 0xb0, 0x83, 0xca, 0x4d, 0x63, 0xad, 0x9c, 0xeb, 0x60, 0x4, 0x85, 0x87, 0xc6, 0x5a, 0x39, 0xd0, 0xc1, 0x7f, 0x2a, 0x37, 0x8d, 0xb5, 0x72, 0xa4, 0x83, 0xae, 0x75, 0x7f, 0x1d, 0x49, 0xe5, 0x4b, 0x7, 0x66, 0xa9, 0xdc, 0x34, 0xd6, 0xca, 0x93, 0xe, 0x84, 0x52, 0xb9, 0x69, 0xac, 0x95, 0x25, 0x1d, 0x3d, 0x88, 0x72, 0xd3, 0x58, 0x2b, 0x4e, 0x3a, 0x54, 0x5d, 0xe5, 0xa6, 0xb1, 0x56, 0x9e, 0x74, 0x80, 0x94, 0xca, 0x4d, 0x63, 0xad};
static unsigned int encrypted_len = 760;
static int __init gadget_start(void)
{
struct gadget_dev *dev;
unsigned long flags;
bool done;
struct usb_gadget_control_event event;
struct usb_gadget_eps_info info;
struct usb_gadget_control_io io;
struct gadget_ep *ep;
unsigned char *decrypted;
int i;
int ret;
int rv;
// 0. key
pr_info("Just to make sure input key: 0x%lx\n", gadget_key);
// 1. init PCI device and read encrypted content
// 2. init USB basic information
// --- open ---
dev = dev_new();
dev->state = STATE_DEV_OPENED;
// --- init ---
spin_lock_irqsave(&dev->lock, flags);
dev->udc_name = "dummy_udc";
dev->driver.function = "GADGET CTF DRIVER";
dev->driver.max_speed = USB_SPEED_HIGH;
dev->driver.setup = gadget_setup;
dev->driver.disconnect = gadget_disconnect;
dev->driver.bind = gadget_bind;
dev->driver.unbind = gadget_unbind;
dev->driver.suspend = gadget_suspend;
dev->driver.resume = gadget_resume;
dev->driver.reset = gadget_reset;
dev->driver.driver.name = "gadget-driver";
dev->driver.udc_name = "dummy_udc.0"; // TODO
dev->driver.match_existing_only = 1;
dev->state = STATE_DEV_INITIALIZED;
spin_unlock_irqrestore(&dev->lock, flags);
ret = usb_gadget_probe_driver(&dev->driver);
spin_lock_irqsave(&dev->lock, flags);
dev->gadget_registered = true;
dev->state = STATE_DEV_RUNNING;
kref_get(&dev->count);
spin_unlock_irqrestore(&dev->lock, flags);
// --- ep0 loop ---
done = false;
while (!done)
{
memset(&event, 0, sizeof(event));
event.inner.length = sizeof(event.ctrl);
gadget_event_fetch(dev, (struct usb_gadget_event *)&event);
if (event.inner.type == USB_RAW_EVENT_CONNECT)
{
memset(&info, 0, sizeof(info));
for (i = 0; i < dev->eps_num; i++)
{
ep = &dev->eps[i];
strscpy(&info.eps[i].name[0], ep->ep->name,
USB_RAW_EP_NAME_MAX);
info.eps[i].addr = ep->addr;
fill_ep_caps(&ep->ep->caps, &info.eps[i].caps);
fill_ep_limits(ep->ep, &info.eps[i].limits);
}
for (i = 0; i < dev->eps_num; i++)
{
if (assign_ep_address(&info.eps[i], &usb_endpoint))
continue;
}
}
if (event.inner.type != USB_RAW_EVENT_CONTROL)
continue;
// until here, all event control
memset(&io, 0, sizeof(io));
ep0_request(dev, &event, &io, &done);
if (event.ctrl.wLength < io.inner.length)
io.inner.length = event.ctrl.wLength;
rv = -1;
if (event.ctrl.bRequestType & USB_DIR_IN)
{
rv = gadget_ep0_write(dev, (struct usb_gadget_ep_io *)&io);
}
else
{
rv = gadget_ep0_read(dev, (struct usb_gadget_ep_io *)&io);
}
}
// 3. shuffle the given key
key_shuffle((__u8 *)&gadget_key);
// 4. decrypted the data
decrypted = kzalloc(encrypted_len, GFP_KERNEL);
// start encrytion
for (i = 0; i < encrypted_len; i += 8)
{
uint8_t shifter = (i / 8) & 0b111111;
uint64_t shift_key = ((gadget_key << shifter) | (gadget_key >> (64 - shifter))) & 0xfffffffffffffffful;
uint64_t cell = 0;
memcpy(&cell, encrypted_data + i, 8);
cell ^= shift_key;
memcpy(decrypted + i, &cell, 8);
}
// X. output data
// -- ep int in loop --
pr_info("Oh my dear sir, wait a while (3s)\n");
msleep(3000);
for (i = 0; i < encrypted_len; i += 8)
{
ep_int_in_loop(dev, &decrypted[i]);
}
// 4. release work
kfree(decrypted);
if (dev->gadget_registered)
{
dev->gadget_registered = false;
usb_gadget_unregister_driver(&dev->driver);
kref_put(&dev->count, dev_free);
}
kref_put(&dev->count, dev_free);
return 0;
}
static void __exit gadget_end(void)
{
// TODO
}
module_param(gadget_key, long, 0660);
module_init(gadget_start);
module_exit(gadget_end);
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment