3 USB endpoints with function FS

Question

I try to create a device with three USB endpoints:

0x04 (USB_DIR_OUT) 0x85 (USB_DIR_IN) 0x86 (USB_DIR_IN)

For that, I use functionfs gadget (without configfs). I load it at start thanks to those commands:

    insmod /lib/modules/5.15.71-imx6ul+g*/kernel/drivers/usb/gadget/libcomposite.ko
    insmod /lib/modules/5.15.71-imx6ul+g*/kernel/drivers/usb/gadget/function/usb_f_fs.ko
    insmod /lib/modules/5.15.71-imx6ul+g*/kernel/drivers/usb/gadget/legacy/g_ffs.ko idVendor=0x1111 idProduct=0x1111 bcdDevice=0x0100 iSerialNumber="123456" iManufacturer="MyCompany" iProduct="MyProduct" bDeviceClass=0xef bDeviceSubClass=0x02 bDeviceProtocol=0x01
    mkdir /dev/gadget
    mount -t functionfs functionfs /dev/gadget`

Once the module is loaded I start my program:

#include <array>
#include <iostream>
#include <utility>
#include <algorithm>
#include <atomic>
#include <thread>
#include <mutex>
#include <queue>
#include <vector>

#include <endian.h>
#include <errno.h>
#include <fcntl.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/poll.h>
#include <unistd.h>
#include <stdbool.h>
#include <libaio.h>

#include <linux/usb/functionfs.h>

#define USB_DEV "/dev/fs/ep0"
#define USB_EPREQUEST "/dev/fs/ep1"
#define USB_EPRESPONSE "/dev/fs/ep2"
#define USB_EPEVENT "/dev/fs/ep3"

namespace {
constexpr size_t IOSIZE = 16384;
constexpr uint8_t NB_READ_BUFFERS = 5;
constexpr uint8_t NB_EVENTS = 10;

int ep0 = -1;
int ep1 = -1;
int ep2 = -1;
int ep3 = -1;

std::atomic<bool> ep0Run = false;
std::atomic<bool> connected = false;
io_context_t context = nullptr;
std::thread *contextThread = nullptr;

std::mutex toWriteEventMutex;
std::queue<std::vector<uint8_t>> toWriteEvent;
std::thread *writeEventThread = nullptr;

struct IocbUserData {
    void* userdata;
    io_callback_t callback;
};

std::atomic<uint8_t> aio_pending(0);

/******************** Descriptors and Strings *******************************/

const struct {
    struct usb_functionfs_descs_head_v2 header;
    __le32 fs_count;
    __le32 hs_count;
    struct {
        struct usb_interface_descriptor intf;
        struct usb_endpoint_descriptor_no_audio ep_request;
        struct usb_endpoint_descriptor_no_audio ep_response;
        struct usb_endpoint_descriptor_no_audio ep_event;
    } __attribute__ ((__packed__)) fs_descs, hs_descs;
} __attribute__ ((__packed__)) descriptors = {
        .header = {
        .magic = htole32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2),
        .length = htole32(sizeof(descriptors)),
        .flags = htole32(FUNCTIONFS_HAS_FS_DESC |
                         FUNCTIONFS_HAS_HS_DESC),
},
        .fs_count = htole32(4),
        .hs_count = htole32(4),
        .fs_descs = {
        .intf = {
        .bLength = sizeof(descriptors.fs_descs.intf),
        .bDescriptorType = USB_DT_INTERFACE,
        .bNumEndpoints = 3,
        .bInterfaceClass = 0xFF,
        .bInterfaceSubClass = 0xFF,
        .bInterfaceProtocol = 0xFF,
        .iInterface = 1,
},
        .ep_request = {
        .bLength = sizeof(descriptors.fs_descs.ep_request),
        .bDescriptorType = USB_DT_ENDPOINT,
        .bEndpointAddress = USB_DIR_OUT | 4,
        .bmAttributes = USB_ENDPOINT_XFER_BULK,
},
        .ep_response = {
        .bLength = sizeof(descriptors.fs_descs.ep_response),
        .bDescriptorType = USB_DT_ENDPOINT,
        .bEndpointAddress = USB_DIR_IN | 5,
        .bmAttributes = USB_ENDPOINT_XFER_BULK,
},
        .ep_event = {
        .bLength = sizeof(descriptors.fs_descs.ep_event),
        .bDescriptorType = USB_DT_ENDPOINT,
        .bEndpointAddress = USB_DIR_IN | 6,
        .bmAttributes = USB_ENDPOINT_XFER_BULK,
},
},
        .hs_descs = {
        .intf = {
        .bLength = sizeof(descriptors.hs_descs.intf),
        .bDescriptorType = USB_DT_INTERFACE,
        .bNumEndpoints = 3,
        .bInterfaceClass = 0xFF,
        .bInterfaceSubClass = 0xFF,
        .bInterfaceProtocol = 0xFF,
        .iInterface = 1,
},
        .ep_request = {
        .bLength = sizeof(descriptors.hs_descs.ep_request),
        .bDescriptorType = USB_DT_ENDPOINT,
        .bEndpointAddress = USB_DIR_OUT | 4,
        .bmAttributes = USB_ENDPOINT_XFER_BULK,
        .wMaxPacketSize = htole16(64),
},
        .ep_response = {
        .bLength = sizeof(descriptors.hs_descs.ep_response),
        .bDescriptorType = USB_DT_ENDPOINT,
        .bEndpointAddress = USB_DIR_IN | 5,
        .bmAttributes = USB_ENDPOINT_XFER_BULK,
        .wMaxPacketSize = htole16(64),
},
        .ep_event = {
        .bLength = sizeof(descriptors.hs_descs.ep_event),
        .bDescriptorType = USB_DT_ENDPOINT,
        .bEndpointAddress = USB_DIR_IN | 6,
        .bmAttributes = USB_ENDPOINT_XFER_BULK,
        .wMaxPacketSize = htole16(64),
},
},
};

#define STR_INTERFACE "My product interface"

const struct {
    struct usb_functionfs_strings_head header;
    struct {
        __le16 code;
        const char str1[sizeof(STR_INTERFACE)];
    } __attribute__ ((__packed__)) lang0;
} __attribute__ ((__packed__)) strings = {
        .header = {
        .magic = htole32(FUNCTIONFS_STRINGS_MAGIC),
        .length = htole32(sizeof(strings)),
        .str_count = htole32(1),
        .lang_count = htole32(1),
},
        .lang0 = {
        htole16(0x0409), /* en-us */
        STR_INTERFACE,
},
};

/******************** Endpoints handling *******************************/
}

void readComplete(io_context_t ctx, struct iocb *iocb, long res, long res2)
{
    (void) res2;
    std::cout << __PRETTY_FUNCTION__ << std::endl;

    if (res > -1) {
        IocbUserData *iocbData = reinterpret_cast<IocbUserData*>(iocb->data);
        std::cout << __PRETTY_FUNCTION__
                  << std::hex
                  << iocb
                  << ";"
                  << iocb->data
                  << ";"
                  << iocbData->userdata
                  << '\n';

        int status = io_submit(ctx, 1, &iocb);

        if (status != 1) {
            --aio_pending;
        }
    }
}

void writeEventComplete(io_context_t ctx, iocb *iocb, long res, long res2)
{
    (void) res;
    (void) res2;
    std::cout << __PRETTY_FUNCTION__ << '\n';

    IocbUserData *iocbData = reinterpret_cast<IocbUserData*>(iocb->data);

    toWriteEventMutex.lock();

    toWriteEvent.pop();
    if (!toWriteEvent.empty()) {
        io_prep_pwrite(
                    iocb,
                    ep3,
                    toWriteEvent.front().data(),
                    toWriteEvent.front().size(),
                    0);

        int status = io_submit(ctx, 1, &iocb);
        if (status != 1) {
            --aio_pending;
            delete iocbData;
            delete iocb;
        }
    } else {
        --aio_pending;
        delete iocbData;
        delete iocb;
    }
    toWriteEventMutex.unlock();
}

int32_t writeEventData(std::vector<uint8_t> const& data)
{
    std::cout << __PRETTY_FUNCTION__ << '\n';
    int32_t out = -1;

    if (connected) {

        toWriteEventMutex.lock();
        toWriteEvent.push(data);

        if (toWriteEvent.size() == 1) {

            struct iocb *iocb = new struct iocb;

            IocbUserData *iocbData = new IocbUserData;
            iocbData->callback = &writeEventComplete;

            io_prep_pwrite(
                        iocb,
                        ep3,
                        toWriteEvent.front().data(),
                        toWriteEvent.front().size(),
                        0);
            iocb->key = USB_DIR_IN;
            iocb->data = reinterpret_cast<void*>(iocbData);

            io_submit(context, 1, &iocb);
            ++aio_pending;
        }
        toWriteEventMutex.unlock();
    }

    return out;
}


void contextExecution()
{
    struct iocb *iocb = new struct iocb[NB_READ_BUFFERS];
    std::array<std::array<uint8_t, IOSIZE>, NB_READ_BUFFERS> readBuffers = { 0 };
    IocbUserData *iocbData = new IocbUserData[NB_READ_BUFFERS];

    struct io_event *e = new io_event[NB_EVENTS];
    struct timespec timeout;

    for (uint8_t i = 0; i < NB_READ_BUFFERS; ++i) {
        iocbData[i].callback = &readComplete;
        iocbData[i].userdata = nullptr;
        io_prep_pread(&(iocb[i]), ep1, readBuffers.at(i).data(), IOSIZE, 0);
        iocb[i].key = USB_DIR_OUT;
        iocb[i].data = reinterpret_cast<void*>(&(iocbData[i]));
    }

    io_submit(context, NB_READ_BUFFERS, &iocb);
    aio_pending = NB_READ_BUFFERS;

    // process iocbs so long as they reissue
    int ret = 0;
    while (ret > -1 && aio_pending > 0 && connected) {
        timeout.tv_sec = 0;
        timeout.tv_nsec = 1000000000;//10000000;

        // wait for at least one event
        ret = io_getevents(context, 1, NB_EVENTS, e, &timeout);
        if (ret > 0) {
            io_callback_t io_complete = nullptr;
            struct iocb *iocb = nullptr;
            for (int i = 0; i < ret; i++) {
                iocb = static_cast<struct iocb *>(e [i].obj);
                IocbUserData *iocbData = reinterpret_cast<IocbUserData*>(iocb->data);
                io_complete = iocbData->callback;
                io_complete(context, iocb, e[i].res, e[i].res2);
            }
        }
    }

    delete [] e;
    delete [] iocb;
    delete [] iocbData;
}

void writeEventExecution()
{
    while (connected) {
        std::cout << __PRETTY_FUNCTION__ << '\n';
        std::vector<uint8_t> data = { 0x01, 0x02, 0x03 };
        writeEventData(data);
        std::this_thread::sleep_for(std::chrono::seconds(2));
    }
}

void stopContext()
{
    if (contextThread != nullptr) {
        contextThread->join();
        delete contextThread;
        contextThread = nullptr;
    }

    if (context != nullptr) {
        io_destroy(context);
        context = nullptr;
    }

    if (writeEventThread != nullptr) {
        writeEventThread->join();
        writeEventThread = nullptr;
    }
}


void handleEp0()
{
    int ret = 0;
    fd_set rfds;
    struct usb_functionfs_event event = {};
    struct timeval timeout = {};

    while (ret > -1 && ep0Run) {
        FD_ZERO(&rfds);
        FD_SET(ep0, &rfds);

        timeout.tv_sec = 0;
        timeout.tv_usec = 10000;

        ret = select(ep0 + 1, &rfds, nullptr, nullptr, &timeout);

        if (ret > 0) {
            ret = read(ep0, &event, sizeof(event));

            if (ret != -1) {
                switch (event.type) {
                case FUNCTIONFS_BIND:
                    std::cout << "FUNCTIONFS_BIND\n";
                    break;
                case FUNCTIONFS_UNBIND:
                    std::cout << "FUNCTIONFS_UNBIND\n";
                    stopContext();
                    break;
                case FUNCTIONFS_ENABLE:
                    std::cout << "FUNCTIONFS_ENABLE\n";
                    if (io_setup(NB_READ_BUFFERS + 2, &context) == 0) {
                        connected = true;
                        contextThread = new std::thread(&contextExecution);
                        writeEventThread = new std::thread(&writeEventExecution);
                    }
                    break;
                case FUNCTIONFS_DISABLE:
                    std::cout << "FUNCTIONFS_DISABLE\n";
                    stopContext();
                    break;
                case FUNCTIONFS_SETUP:
                    std::cout << "FUNCTIONFS_SETUP\n";
                    if (event.u.setup.bRequestType & USB_DIR_IN) {
                        write(ep0, NULL, 0);
                    } else {
                        read(ep0, NULL, 0);
                    }
                    break;
                case FUNCTIONFS_SUSPEND:
                    std::cout << "FUNCTIONFS_SUSPEND\n";
                    connected = false;
                    break;
                case FUNCTIONFS_RESUME:
                    std::cout << "FUNCTIONFS_RESUME\n";
                    break;
                }
            } else {
                //std::cout << "Read error " << ret << " (%m)\n";
            }
        }
    }
}


ssize_t initEp0()
{
    ssize_t ret = 0;
    ep0 = open(USB_DEV, O_RDWR);

    if (ep0 > 0) {
        ret = write(ep0, &descriptors, sizeof(descriptors));
        if ( ret > 0) {
            ret = write(ep0, &strings, sizeof(strings));
        }
    } else {
        ret = ep0;
    }

    return ret;
}

int initEndpoint(int *endpoint, char const* path)
{
    *endpoint = open(path, O_RDWR);
    return *endpoint;
}

int initEndpoints()
{
    int out = -1;
    if (initEndpoint(&ep3, USB_EPEVENT) > 0 &&
            initEndpoint(&ep2, USB_EPRESPONSE) > 0 &&
            initEndpoint(&ep1, USB_EPREQUEST) > 0) {
        out = 0;
    }
    return out;
}

int main()
{
    std::thread *test = nullptr;
    if (initEp0() != -1) {
        if (initEndpoints() == 0) {
            test = new std::thread(&handleEp0);
        }
    }

    ep0Run = true;
    if (test != nullptr) {
        test->join();
    }
    connected = false;
    stopContext();

    return 0;
}

The OUT endpoint is working well and the first IN declared too. My problem is that my second IN endpoint does not work, I can't write data on it. Do you know why? (I use a imx6ul which allows many USB endpoints, don't remember exactly how many but I checked more than three)

I expected to be able to write on the two IN endpoints.