HT7315_MonitorSOC/sdk/Driver/UsbBillboard/src/hv_drv_UsbGadgetConfig.c

/*
* @file hv_drv_UsbGadgetConfig.c
* @brief Simplify building config descriptors. 
*
* @author HiView SoC Software Team
* @version 1.0.0
* @date 2022-06-15
*/

#include "hv_drv_UsbErrNo.h"
#include <string.h>

#include "hv_drv_UsbCh9.h"
#include "hv_drv_UsbGadget.h"
#include "hv_drv_UsbGadgetComposite.h"


/**
 * usb_descriptor_fillbuf - fill buffer with descriptors
 * @buf: Buffer to be filled
 * @buflen: Size of buf
 * @src: Array of descriptor pointers, terminated by null pointer.
 *
 * Copies descriptors into the buffer, returning the length or a
 * negative error code if they can't all be copied.  Useful when
 * assembling descriptors for an associated set of interfaces used
 * as part of configuring a composite device; or in other cases where
 * sets of descriptors need to be marshaled.
 */
int usb_descriptor_fillbuf(void *buf, unsigned buflen,
        const struct usb_descriptor_header **src)
{
    u8    *dest = buf;

    if (!src)
        return -EINVAL;
    /* fill buffer from src[] until null descriptor ptr */
    for (; NULL != *src; src++) {
        unsigned len = (*src)->bLength;

        if (len > buflen)
            return -EINVAL;
        HV_MEMCPY(dest, *src, len);
        buflen -= len;
        dest += len;
    }
    return dest - (u8 *)buf;
}


/**
 * usb_gadget_config_buf - builts a complete configuration descriptor
 * @config: Header for the descriptor, including characteristics such
 *    as power requirements and number of interfaces.
 * @desc: Null-terminated vector of pointers to the descriptors (interface,
 *    endpoint, etc) defining all functions in this device configuration.
 * @buf: Buffer for the resulting configuration descriptor.
 * @length: Length of buffer.  If this is not big enough to hold the
 *    entire configuration descriptor, an error code will be returned.
 *
 * This copies descriptors into the response buffer, building a descriptor
 * for that configuration.  It returns the buffer length or a negative
 * status code.  The config.wTotalLength field is set to match the length
 * of the result, but other descriptor fields (including power usage and
 * interface count) must be set by the caller.
 *
 * Gadget drivers could use this when constructing a config descriptor
 * in response to USB_REQ_GET_DESCRIPTOR.  They will need to patch the
 * resulting bDescriptorType value if USB_DT_OTHER_SPEED_CONFIG is needed.
 */
int usb_gadget_config_buf(
    const struct usb_config_descriptor    *config,
    void                    *buf,
    unsigned                length,
    const struct usb_descriptor_header    **desc
)
{
    struct usb_config_descriptor        *cp = buf;
    int                    len;

    /* config descriptor first */
    if (length < USB_DT_CONFIG_SIZE || !desc)
        return -EINVAL;
    /* config need not be aligned */
    HV_MEMCPY(cp, config, sizeof(*cp));

    /* then interface/endpoint/class/vendor/... */
    len = usb_descriptor_fillbuf(USB_DT_CONFIG_SIZE + (u8 *)buf,
            length - USB_DT_CONFIG_SIZE, desc);
    if (len < 0)
        return len;
    len += USB_DT_CONFIG_SIZE;
    if (len > 0xffff)
        return -EINVAL;

    /* patch up the config descriptor */
    cp->bLength = USB_DT_CONFIG_SIZE;
    cp->bDescriptorType = USB_DT_CONFIG;
    put_unaligned_le16(len, &cp->wTotalLength);
    cp->bmAttributes |= USB_CONFIG_ATT_ONE;
    return len;
}


/**
 * usb_copy_descriptors - copy a vector of USB descriptors
 * @src: null-terminated vector to copy
 * Context: initialization code, which may sleep
 *
 * This makes a copy of a vector of USB descriptors.  Its primary use
 * is to support usb_function objects which can have multiple copies,
 * each needing different descriptors.  Functions may have static
 * tables of descriptors, which are used as templates and customized
 * with identifiers (for interfaces, strings, endpoints, and more)
 * as needed by a given function instance.
 */
struct usb_descriptor_header **
usb_copy_descriptors(struct usb_descriptor_header **src)
{
    struct usb_descriptor_header **tmp;
    unsigned bytes;
    unsigned n_desc;
    void *mem;
    struct usb_descriptor_header **ret;

    /* count descriptors and their sizes; then add vector size */
    for (bytes = 0, n_desc = 0, tmp = src; *tmp; tmp++, n_desc++)
        bytes += (*tmp)->bLength;
    bytes += (n_desc + 1) * sizeof(*tmp);

    mem = HV_MALLOC(bytes);
    if (!mem)
        return NULL;

    /* fill in pointers starting at "tmp",
     * to descriptors copied starting at "mem";
     * and return "ret"
     */
    tmp = mem;
    ret = mem;
    mem += (n_desc + 1) * sizeof(*tmp);
    while (*src) {
        HV_MEMCPY(mem, *src, (*src)->bLength);
        *tmp = mem;
        tmp++;
        mem += (*src)->bLength;
        src++;
    }
    *tmp = NULL;

    return ret;
}
static inline void usb_free_descriptors(struct usb_descriptor_header **v)
{
    HV_FREE(v);
}

void usb_free_all_descriptors(struct usb_function *f)
{
    usb_free_descriptors(f->fs_descriptors);
    usb_free_descriptors(f->hs_descriptors);
}

int usb_assign_descriptors(struct usb_function *f,
        struct usb_descriptor_header **fs,
        struct usb_descriptor_header **hs,
        struct usb_descriptor_header **ss)
{
    struct usb_gadget *g = f->config->cdev->gadget;

    if (fs) {
        f->fs_descriptors = usb_copy_descriptors(fs);
        if (!f->fs_descriptors)
            goto err;
    }
    if (hs && gadget_is_dualspeed(g)) {
        f->hs_descriptors = usb_copy_descriptors(hs);
        if (!f->hs_descriptors)
            goto err;
    }

    if (hs) {
        f->hs_descriptors = usb_copy_descriptors(hs);
        if (!f->hs_descriptors)
            goto err;
    }
    return 0;
err:
    usb_free_all_descriptors(f);
    return -ENOMEM;
}