Subversion Repositories freemyipod

//
//
//    Copyright 2010 TheSeven
//
//
//    This file is part of emBIOS.
//
//    emBIOS 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.
//
//    emBIOS is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//    See the GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License along
//    with emBIOS.  If not, see <http://www.gnu.org/licenses/>.
//
//


#include "global.h"
#include "panic.h"
#include "usb.h"
#include "usb_ch9.h"
#include "usbdrv.h"
#include "thread.h"
#include "console.h"
#include "util.h"
#include "contextswitch.h"
#include "power.h"
#include "mmu.h"
#include "shutdown.h"
#include "execimage.h"
#ifdef HAVE_I2C
#include "i2c.h"
#endif
#ifdef HAVE_BOOTFLASH
#include "bootflash.h"
#endif
#ifdef HAVE_HWKEYAES
#include "hwkeyaes.h"
#endif
#ifdef HAVE_HMACSHA1
#include "hmacsha1.h"
#endif
#ifdef USB_HAVE_TARGET_SPECIFIC_REQUESTS
#include "usbtarget.h"
#endif


static uint8_t ctrlresp[2] CACHEALIGN_ATTR;
static uint32_t dbgrecvbuf[0x80] CACHEALIGN_ATTR;
static uint32_t dbgsendbuf[0x80] CACHEALIGN_ATTR;
static uint32_t dbgasyncsendbuf[0x80] CACHEALIGN_ATTR;
static char dbgendpoints[4] IBSS_ATTR;

enum dbgaction_t
{
    DBGACTION_IDLE = 0,
    DBGACTION_I2CSEND,
    DBGACTION_I2CRECV,
    DBGACTION_RESET,
    DBGACTION_POWEROFF,
    DBGACTION_CWRITE,
    DBGACTION_CREAD,
    DBGACTION_CFLUSH,
    DBGACTION_EXECIMAGE,
    DBGACTION_EXECFIRMWARE,
    DBGACTION_READBOOTFLASH,
    DBGACTION_WRITEBOOTFLASH,
    DBGACTION_HWKEYAES,
    DBGACTION_HMACSHA1,
    DBGACTION_TARGETSPECIFIC
};

static uint32_t dbgstack[0x100] STACK_ATTR;
struct wakeup dbgwakeup IBSS_ATTR;
extern struct scheduler_thread* scheduler_threads;
static enum dbgaction_t dbgaction IBSS_ATTR;
static int dbgi2cbus;
static int dbgi2cslave;
static int dbgactionaddr;
static int dbgactionoffset;
static int dbgactionlength;
static int dbgactionconsoles;
static int dbgactiontype;
static char dbgconsendbuf[4096];
static char dbgconrecvbuf[1024];
static int dbgconsendreadidx IBSS_ATTR;
static int dbgconsendwriteidx IBSS_ATTR;
static int dbgconrecvreadidx IBSS_ATTR;
static int dbgconrecvwriteidx IBSS_ATTR;
static struct wakeup dbgconsendwakeup IBSS_ATTR;
static struct wakeup dbgconrecvwakeup IBSS_ATTR;
static bool dbgconsoleattached IBSS_ATTR;

static const char dbgconoverflowstr[] = "\n\n[overflowed]\n\n";

extern int _initstart;   // These aren't ints at all, but gcc complains about void types being
extern int _sdramstart;  // used here, and we only need the address, so forget about it...


static struct usb_device_descriptor CACHEALIGN_ATTR device_descriptor =
{
    .bLength            = sizeof(struct usb_device_descriptor),
    .bDescriptorType    = USB_DT_DEVICE,
    .bcdUSB             = 0x0200,
    .bDeviceClass       = USB_CLASS_VENDOR_SPEC,
    .bDeviceSubClass    = 0xff,
    .bDeviceProtocol    = 0xff,
    .bMaxPacketSize0    = 64,
    .idVendor           = 0xffff,
    .idProduct          = 0xe000,
    .bcdDevice          = 0x0001,
    .iManufacturer      = 1,
    .iProduct           = 2,
    .iSerialNumber      = 0,
    .bNumConfigurations = 1
};

static struct usb_config_bundle
{
    struct usb_config_descriptor config_descriptor;
    struct usb_interface_descriptor interface_descriptor;
    struct usb_endpoint_descriptor endpoint1_descriptor;
    struct usb_endpoint_descriptor endpoint2_descriptor;
    struct usb_endpoint_descriptor endpoint3_descriptor;
    struct usb_endpoint_descriptor endpoint4_descriptor;
} __attribute__((packed)) CACHEALIGN_ATTR config_bundle = 
{
    .config_descriptor =
    {
        .bLength             = sizeof(struct usb_config_descriptor),
        .bDescriptorType     = USB_DT_CONFIG,
        .wTotalLength        = sizeof(struct usb_config_descriptor)
                             + sizeof(struct usb_interface_descriptor)
                             + sizeof(struct usb_endpoint_descriptor) * 4,
        .bNumInterfaces      = 1,
        .bConfigurationValue = 1,
        .iConfiguration      = 0,
        .bmAttributes        = USB_CONFIG_ATT_ONE,
        .bMaxPower           = 250
    },
    .interface_descriptor =
    {
        .bLength             = sizeof(struct usb_interface_descriptor),
        .bDescriptorType     = USB_DT_INTERFACE,
        .bInterfaceNumber    = 0,
        .bAlternateSetting   = 0,
        .bNumEndpoints       = 4,
        .bInterfaceClass     = USB_CLASS_VENDOR_SPEC,
        .bInterfaceSubClass  = 0xff,
        .bInterfaceProtocol  = 0xff,
        .iInterface          = 0
    },
    .endpoint1_descriptor =
    {
        .bLength             = sizeof(struct usb_endpoint_descriptor),
        .bDescriptorType     = USB_DT_ENDPOINT,
        .bEndpointAddress    = 0,
        .bmAttributes        = USB_ENDPOINT_XFER_BULK,
        .wMaxPacketSize      = 0,
        .bInterval           = 1
    },
    .endpoint2_descriptor =
    {
        .bLength             = sizeof(struct usb_endpoint_descriptor),
        .bDescriptorType     = USB_DT_ENDPOINT,
        .bEndpointAddress    = 0,
        .bmAttributes        = USB_ENDPOINT_XFER_BULK,
        .wMaxPacketSize      = 0,
        .bInterval           = 1
    },
    .endpoint3_descriptor =
    {
        .bLength             = sizeof(struct usb_endpoint_descriptor),
        .bDescriptorType     = USB_DT_ENDPOINT,
        .bEndpointAddress    = 0,
        .bmAttributes        = USB_ENDPOINT_XFER_BULK,
        .wMaxPacketSize      = 0,
        .bInterval           = 1
    },
    .endpoint4_descriptor =
    {
        .bLength             = sizeof(struct usb_endpoint_descriptor),
        .bDescriptorType     = USB_DT_ENDPOINT,
        .bEndpointAddress    = 0,
        .bmAttributes        = USB_ENDPOINT_XFER_BULK,
        .wMaxPacketSize      = 0,
        .bInterval           = 1
    }
};

static struct usb_string_descriptor CACHEALIGN_ATTR string_devicename =
{
    32,
    USB_DT_STRING,
    {'e', 'm', 'B', 'I', 'O', 'S', ' ', 'D', 'e', 'b', 'u', 'g', 'g', 'e', 'r'}
};

static const struct usb_string_descriptor CACHEALIGN_ATTR lang_descriptor =
{
    4,
    USB_DT_STRING,
    {0x0409}
};


void usb_setup_dbg_listener()
{
    usb_drv_recv(dbgendpoints[0], dbgrecvbuf, usb_drv_port_speed() ? 512 : 64);
}

void usb_handle_control_request(struct usb_ctrlrequest* req)
{
    const void* addr;
    int size = -1;
    switch (req->bRequest)
    {
    case USB_REQ_GET_STATUS:
        if (req->bRequestType == USB_DIR_IN) ctrlresp[0] = 1;
        else ctrlresp[0] = 0;
        ctrlresp[1] = 0;
        addr = ctrlresp;
        size = 2;
        break;
    case USB_REQ_CLEAR_FEATURE:
        if (req->bRequestType == USB_RECIP_ENDPOINT && req->wValue == USB_ENDPOINT_HALT)
            usb_drv_stall(req->wIndex & 0xf, false, req->wIndex >> 7);
        size = 0;
        break;
    case USB_REQ_SET_FEATURE:
        size = 0;
        break;
    case USB_REQ_SET_ADDRESS:
        size = 0;
        usb_drv_cancel_all_transfers();
        usb_drv_set_address(req->wValue);
        usb_setup_dbg_listener();
        break;
    case USB_REQ_GET_DESCRIPTOR:
        switch (req->wValue >> 8)
        {
        case USB_DT_DEVICE:
            addr = &device_descriptor;
            size = sizeof(device_descriptor);
            break;
        case USB_DT_OTHER_SPEED_CONFIG:
        case USB_DT_CONFIG:
            if ((req->wValue >> 8) == USB_DT_CONFIG)
            {
                int maxpacket = usb_drv_port_speed() ? 512 : 64;
                config_bundle.endpoint1_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.endpoint2_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.endpoint3_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.endpoint4_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.config_descriptor.bDescriptorType = USB_DT_CONFIG;
            }
            else
            {
                int maxpacket = usb_drv_port_speed() ? 64 : 512;
                config_bundle.endpoint1_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.endpoint2_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.endpoint3_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.endpoint4_descriptor.wMaxPacketSize = maxpacket;
                config_bundle.config_descriptor.bDescriptorType = USB_DT_OTHER_SPEED_CONFIG;
            }
            addr = &config_bundle;
            size = sizeof(config_bundle);
            break;
        case USB_DT_STRING:
            switch (req->wValue & 0xff)
            {
            case 0:
                addr = &lang_descriptor;
                size = lang_descriptor.bLength;
                break;
            case 1:
                string_devicename.bLength = 14;
                addr = &string_devicename;
                size = string_devicename.bLength;
                break;
            case 2:
                string_devicename.bLength = 32;
                addr = &string_devicename;
                size = string_devicename.bLength;
                break;
            }
            break;
        }
        break;
    case USB_REQ_GET_CONFIGURATION:
        ctrlresp[0] = 1;
        addr = ctrlresp;
        size = 1;
        break;
    case USB_REQ_SET_CONFIGURATION:
        usb_drv_cancel_all_transfers();
        usb_setup_dbg_listener();
        size = 0;
        break;
    }
    if (!size) usb_drv_send_nonblocking(0, NULL, 0);
    else if (size == -1)
    {
        usb_drv_stall(0, true, true);
        usb_drv_stall(0, true, false);
    }
    else
    {
        usb_drv_recv(0, NULL, 0);
        usb_drv_send_nonblocking(0, addr, size > req->wLength ? req->wLength : size);
    }
}

bool set_dbgaction(enum dbgaction_t action, int addsize)
{
    if (dbgaction != DBGACTION_IDLE)
    {
        dbgsendbuf[0] = 3;
        usb_drv_send_nonblocking(dbgendpoints[1], dbgsendbuf, 16 + addsize);
        return true;
    }
    dbgaction = action;
    wakeup_signal(&dbgwakeup);
    return false;
}

void reset() __attribute__((noreturn));

void usb_handle_transfer_complete(int endpoint, int dir, int status, int length)
{
    void* addr = dbgsendbuf;
    int size = 0;
    if (endpoint == dbgendpoints[0])
    {
#ifdef USB_HAVE_TARGET_SPECIFIC_REQUESTS
        if (dbgrecvbuf[0] >= 0xffff0000)
        {
            if (!set_dbgaction(DBGACTION_TARGETSPECIFIC, 0))
                memcpy(dbgasyncsendbuf, dbgrecvbuf, sizeof(dbgasyncsendbuf));
            usb_setup_dbg_listener();
            return;
        }
#endif
        switch (dbgrecvbuf[0])
        {
        case 1:  // GET INFO
            dbgsendbuf[0] = 1;
            size = 16;
            switch (dbgrecvbuf[1])
            {
            case 0:  // GET VERSION INFO
                dbgsendbuf[1] = VERSION_SVN_INT;
                dbgsendbuf[2] = VERSION_MAJOR | (VERSION_MINOR << 8)
                              | (VERSION_PATCH << 16) | (1 << 24);
                dbgsendbuf[3] = PLATFORM_ID;
                break;
            case 1:  // GET PACKET SIZE INFO
                dbgsendbuf[1] = 0x02000200;
                dbgsendbuf[2] = usb_drv_get_max_out_size();
                dbgsendbuf[3] = usb_drv_get_max_in_size();
                break;
            case 2:  // GET USER MEMORY INFO
                dbgsendbuf[1] = (uint32_t)&_initstart;
                dbgsendbuf[2] = (uint32_t)&_sdramstart;
                break;
            default:
                dbgsendbuf[0] = 2;
            }
            break;
        case 2:  // RESET
            if (dbgrecvbuf[1])
            {
                if (set_dbgaction(DBGACTION_RESET, 0)) break;
                dbgsendbuf[0] = 1;
                size = 16;
            }
            else reset();
            break;
        case 3:  // POWER OFF
            if (set_dbgaction(DBGACTION_POWEROFF, 0)) break;
            dbgactiontype = dbgrecvbuf[1];
            dbgsendbuf[0] = 1;
            size = 16;
            break;
        case 4:  // READ MEMORY
            dbgsendbuf[0] = 1;
            memcpy(&dbgsendbuf[4], (const void*)dbgrecvbuf[1], dbgrecvbuf[2]);
            size = dbgrecvbuf[2] + 16;
            break;
        case 5:  // WRITE MEMORY
            dbgsendbuf[0] = 1;
            memcpy((void*)dbgrecvbuf[1], &dbgrecvbuf[4], dbgrecvbuf[2]);
            size = 16;
            break;
        case 6:  // READ DMA
            dbgsendbuf[0] = 1;
            usb_drv_send_nonblocking(dbgendpoints[1], dbgsendbuf, 16);
            usb_drv_send_nonblocking(dbgendpoints[3], (const void*)dbgrecvbuf[1], dbgrecvbuf[2]);
            break;
        case 7:  // WRITE DMA
            dbgsendbuf[0] = 1;
            size = 16;
            usb_drv_recv(dbgendpoints[2], (void*)dbgrecvbuf[1], dbgrecvbuf[2]);
            break;
#ifdef HAVE_I2C
        case 8:  // READ I2C
            if (set_dbgaction(DBGACTION_I2CRECV, dbgrecvbuf[1] >> 24)) break;
            dbgi2cbus = dbgrecvbuf[1] & 0xff;
            dbgi2cslave = (dbgrecvbuf[1] >> 8) & 0xff;
            dbgactionaddr = (dbgrecvbuf[1] >> 16) & 0xff;
            dbgactionlength = dbgrecvbuf[1] >> 24;
            if (!dbgactionlength) dbgactionlength = 256;
            break;
        case 9:  // WRITE I2C
            if (set_dbgaction(DBGACTION_I2CSEND, 0)) break;
            dbgi2cbus = dbgrecvbuf[1] & 0xff;
            dbgi2cslave = (dbgrecvbuf[1] >> 8) & 0xff;
            dbgactionaddr = (dbgrecvbuf[1] >> 16) & 0xff;
            dbgactionlength = dbgrecvbuf[1] >> 24;
            if (!dbgactionlength) dbgactionlength = 256;
            memcpy(dbgasyncsendbuf, &dbgrecvbuf[4], dbgactionlength);
            break;
#endif
        case 10:  // READ CONSOLE
            dbgconsoleattached = true;
            int bytes = dbgconsendwriteidx - dbgconsendreadidx;
            if (bytes >= sizeof(dbgconsendbuf)) bytes -= sizeof(dbgconsendbuf);
            if (bytes)
            {
                if (bytes < 0) bytes += sizeof(dbgconsendbuf);
                if (bytes > dbgrecvbuf[1]) bytes = dbgrecvbuf[1];
                int readbytes = bytes;
                char* outptr = (char*)&dbgsendbuf[4];
                if (dbgconsendreadidx + bytes >= sizeof(dbgconsendbuf))
                {
                    readbytes = sizeof(dbgconsendbuf) - dbgconsendreadidx;
                    memcpy(outptr, &dbgconsendbuf[dbgconsendreadidx], readbytes);
                    dbgconsendreadidx = 0;
                    outptr = &outptr[readbytes];
                    readbytes = bytes - readbytes;
                }
                if (readbytes) memcpy(outptr, &dbgconsendbuf[dbgconsendreadidx], readbytes);
                dbgconsendreadidx += readbytes;
                wakeup_signal(&dbgconsendwakeup);
            }
            dbgsendbuf[0] = 1;
            dbgsendbuf[1] = bytes;
            dbgsendbuf[2] = sizeof(dbgconsendbuf);
            dbgsendbuf[3] = dbgconsendwriteidx - dbgconsendreadidx;
            size = 16 + dbgrecvbuf[1];
            break;
        case 11:  // WRITE CONSOLE
            bytes = dbgconrecvreadidx - dbgconrecvwriteidx - 1;
            if (bytes < 0) bytes += sizeof(dbgconrecvbuf);
            if (bytes)
            {
                if (bytes > dbgrecvbuf[1]) bytes = dbgrecvbuf[1];
                int writebytes = bytes;
                char* readptr = (char*)&dbgrecvbuf[4];
                if (dbgconrecvwriteidx + bytes >= sizeof(dbgconrecvbuf))
                {
                    writebytes = sizeof(dbgconrecvbuf) - dbgconrecvwriteidx;
                    memcpy(&dbgconrecvbuf[dbgconrecvwriteidx], readptr, writebytes);
                    dbgconrecvwriteidx = 0;
                    readptr = &readptr[writebytes];
                    writebytes = bytes - writebytes;
                }
                if (writebytes) memcpy(&dbgconrecvbuf[dbgconrecvwriteidx], readptr, writebytes);
                dbgconrecvwriteidx += writebytes;
                wakeup_signal(&dbgconrecvwakeup);
            }
            dbgsendbuf[0] = 1;
            dbgsendbuf[1] = bytes;
            dbgsendbuf[2] = sizeof(dbgconrecvbuf);
            dbgsendbuf[3] = dbgconrecvreadidx - dbgconrecvwriteidx - 1;
            size = 16;
            break;
        case 12:  // CWRITE
            if (set_dbgaction(DBGACTION_CWRITE, 0)) break;
            dbgactionconsoles = dbgrecvbuf[1];
            dbgactionlength = dbgrecvbuf[2];
            memcpy(dbgasyncsendbuf, &dbgrecvbuf[4], dbgactionlength);
            break;
        case 13:  // CREAD
            if (set_dbgaction(DBGACTION_CREAD, dbgrecvbuf[2])) break;
            dbgactionconsoles = dbgrecvbuf[1];
            dbgactionlength = dbgrecvbuf[2];
            break;
        case 14:  // CFLUSH
            if (set_dbgaction(DBGACTION_CFLUSH, 0)) break;
            dbgactionconsoles = dbgrecvbuf[1];
            break;
        case 15:  // GET PROCESS INFO
            dbgsendbuf[0] = 1;
            dbgsendbuf[1] = SCHEDULER_THREAD_INFO_VERSION;
            dbgsendbuf[2] = MAX_THREADS * sizeof(struct scheduler_thread);
            memcpy(&dbgsendbuf[4], (void*)(((uint32_t)&scheduler_threads) + dbgrecvbuf[1]),
                   dbgrecvbuf[2]);
            size = dbgrecvbuf[2] + 16;
            break;
        case 16:  // FREEZE SCHEDULER
            dbgsendbuf[1] = scheduler_freeze(dbgrecvbuf[1]);
            dbgsendbuf[0] = 1;
            size = 16;
            break;
        case 17:  // SUSPEND THREAD
            if (dbgrecvbuf[1])
            {
                if (thread_suspend(dbgrecvbuf[2]) == -4) dbgsendbuf[1] = 1;
                else dbgsendbuf[1] = 0;
            }
            else
            {
                if (thread_resume(dbgrecvbuf[2]) == -5) dbgsendbuf[1] = 0;
                else dbgsendbuf[1] = 1;
            }
            dbgsendbuf[0] = 1;
            size = 16;
            break;
        case 18:  // KILL THREAD
            thread_terminate(dbgrecvbuf[1]);
            dbgsendbuf[0] = 1;
            size = 16;
            break;
        case 19:  // KILL THREAD
            dbgsendbuf[0] = 1;
            dbgsendbuf[1] = thread_create((const char*)dbgsendbuf[1], (const void*)dbgsendbuf[2],
                                          (char*)dbgsendbuf[3], dbgsendbuf[4], dbgsendbuf[5],
                                          dbgsendbuf[6], dbgsendbuf[7]);
            size = 16;
            break;
        case 20:  // FLUSH CACHE
            clean_dcache();
            invalidate_icache();
            dbgsendbuf[0] = 1;
            size = 16;
            break;
        case 21:  // EXECIMAGE
            if (set_dbgaction(DBGACTION_EXECIMAGE, 0)) break;
            dbgactionaddr = dbgrecvbuf[1];
            break;
#ifdef HAVE_BOOTFLASH
        case 22:  // READ BOOT FLASH
            if (set_dbgaction(DBGACTION_READBOOTFLASH, 0)) break;
            dbgactionaddr = dbgrecvbuf[1];
            dbgactionoffset = dbgrecvbuf[2];
            dbgactionlength = dbgrecvbuf[3];
            break;
        case 23:  // WRITE BOOT FLASH
            if (set_dbgaction(DBGACTION_WRITEBOOTFLASH, 0)) break;
            dbgactionaddr = dbgrecvbuf[1];
            dbgactionoffset = dbgrecvbuf[2];
            dbgactionlength = dbgrecvbuf[3];
            break;
#endif
        case 24:  // EXECFIRMWARE
            if (set_dbgaction(DBGACTION_EXECFIRMWARE, 0)) break;
            dbgactionaddr = dbgrecvbuf[1];
            break;
#ifdef HAVE_HWKEYAES
        case 25:  // HWKEYAES
            if (set_dbgaction(DBGACTION_HWKEYAES, 0)) break;
            dbgactiontype = ((uint8_t*)dbgrecvbuf)[4];
            dbgactionoffset = ((uint16_t*)dbgrecvbuf)[3];
            dbgactionaddr = dbgrecvbuf[2];
            dbgactionlength = dbgrecvbuf[3];
            break;
#endif
#ifdef HAVE_HMACSHA1
        case 26:  // HMACSHA1
            if (set_dbgaction(DBGACTION_HMACSHA1, 0)) break;
            dbgactionaddr = dbgrecvbuf[1];
            dbgactionlength = dbgrecvbuf[2];
            dbgactionoffset = dbgrecvbuf[3];
            break;
#endif
        default:
            dbgsendbuf[0] = 2;
            size = 16;
        }
        usb_setup_dbg_listener();
        if (size) usb_drv_send_nonblocking(dbgendpoints[1], addr, size);
    }
}

void usb_handle_bus_reset(void)
{
    dbgendpoints[0] = usb_drv_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_OUT);
    dbgendpoints[1] = usb_drv_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_IN);
    dbgendpoints[2] = usb_drv_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_OUT);
    dbgendpoints[3] = usb_drv_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_IN);
    config_bundle.endpoint1_descriptor.bEndpointAddress = dbgendpoints[0];
    config_bundle.endpoint2_descriptor.bEndpointAddress = dbgendpoints[1];
    config_bundle.endpoint3_descriptor.bEndpointAddress = dbgendpoints[2];
    config_bundle.endpoint4_descriptor.bEndpointAddress = dbgendpoints[3];
    usb_setup_dbg_listener();
}

void dbgthread(void)
{
    int i;
    int t;
    while (1)
    {
        wakeup_wait(&dbgwakeup, TIMEOUT_BLOCK);
        for (i = 0; i < MAX_THREADS; i++)
            if (scheduler_threads[i].state == THREAD_DEFUNCT)
            {
                if (scheduler_threads[i].block_type == THREAD_DEFUNCT_STKOV)
                {
                    if (scheduler_threads[i].name)
                        cprintf(1, "\n*PANIC*\nStack overflow! (%s)\n",
                                scheduler_threads[i].name);
                    else cprintf(1, "\n*PANIC*\nStack overflow! (ID %d)\n", i);
                }
                scheduler_threads[i].state = THREAD_DEFUNCT_ACK;
            }
        if (dbgaction != DBGACTION_IDLE)
        {
            switch (dbgaction)
            {
#ifdef HAVE_I2C
            case DBGACTION_I2CSEND:
                i2c_send(dbgi2cbus, dbgi2cslave, dbgactionaddr,
                         (uint8_t*)dbgasyncsendbuf, dbgactionlength);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
            case DBGACTION_I2CRECV:
                i2c_recv(dbgi2cbus, dbgi2cslave, dbgactionaddr,
                         (uint8_t*)(&dbgasyncsendbuf[4]), dbgactionlength);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16 + dbgactionlength);
                break;
#endif
            case DBGACTION_POWEROFF:
                if (dbgactiontype) shutdown(true);
                power_off();
                break;
            case DBGACTION_RESET:
                shutdown(false);
                reset();
                break;
            case DBGACTION_CWRITE:
                cwrite(dbgactionconsoles, (const char*)dbgasyncsendbuf, dbgactionlength);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
            case DBGACTION_CREAD:
                dbgasyncsendbuf[0] = 1;
                dbgasyncsendbuf[1] = cread(dbgactionconsoles, (char*)&dbgasyncsendbuf[4],
                                           dbgactionlength, 0);
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16 + dbgactionlength);
                break;
            case DBGACTION_CFLUSH:
                cflush(dbgactionconsoles);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
            case DBGACTION_EXECIMAGE:
                dbgasyncsendbuf[0] = 1;
                dbgasyncsendbuf[1] = execimage((void*)dbgactionaddr);
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
            case DBGACTION_EXECFIRMWARE:
                shutdown(false);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                execfirmware((void*)dbgactionaddr);
#ifdef HAVE_BOOTFLASH
            case DBGACTION_READBOOTFLASH:
                bootflash_readraw((void*)dbgactionaddr, dbgactionoffset, dbgactionlength);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
            case DBGACTION_WRITEBOOTFLASH:
                bootflash_writeraw((void*)dbgactionaddr, dbgactionoffset, dbgactionlength);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
#endif
#ifdef HAVE_HWKEYAES
            case DBGACTION_HWKEYAES:
                hwkeyaes((enum hwkeyaes_direction) dbgactiontype, dbgactionoffset,
                         (void*)dbgactionaddr, dbgactionlength);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
#endif
#ifdef HAVE_HMACSHA1
            case DBGACTION_HMACSHA1:
                hmacsha1((void*)dbgactionaddr, dbgactionlength, (void*)dbgactionoffset);
                dbgasyncsendbuf[0] = 1;
                usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, 16);
                break;
#endif
#ifdef USB_HAVE_TARGET_SPECIFIC_REQUESTS
            case DBGACTION_TARGETSPECIFIC:
            {
                int size = usb_target_handle_request(dbgasyncsendbuf, sizeof(dbgasyncsendbuf));
                if (size) usb_drv_send_nonblocking(dbgendpoints[1], dbgasyncsendbuf, size);
                break;
            }
#endif
            }
            dbgaction = DBGACTION_IDLE;
        }
    }
}

void usb_init(void)
{
    dbgaction = DBGACTION_IDLE;
    wakeup_init(&dbgwakeup);
    dbgconsendreadidx = 0;
    dbgconsendwriteidx = 0;
    dbgconrecvreadidx = 0;
    dbgconrecvwriteidx = 0;
    wakeup_init(&dbgconsendwakeup);
    wakeup_init(&dbgconrecvwakeup);
    dbgconsoleattached = false;
    thread_create("monitor worker", dbgthread, dbgstack, sizeof(dbgstack), CORE_THREAD, 255, true);
    usb_drv_init();
}

int dbgconsole_getfree() ICODE_ATTR;
int dbgconsole_getfree()
{
    int free = dbgconsendreadidx - dbgconsendwriteidx - 1;
    if (free < 0) free += sizeof(dbgconsendbuf);
    return free;
}

int dbgconsole_makespace(int length) ICODE_ATTR;
int dbgconsole_makespace(int length)
{
    int free = dbgconsole_getfree();
    while (!free && dbgconsoleattached)
    {
        if (wakeup_wait(&dbgconsendwakeup, 2000000) == THREAD_TIMEOUT)
            dbgconsoleattached = false;
        free = dbgconsole_getfree();
    }
    if (free) return free > length ? length : free;
    if (length > sizeof(dbgconsendbuf) - 17) length = sizeof(dbgconsendbuf) - 17;
    uint32_t mode = enter_critical_section();
    dbgconsendreadidx += length;
    if (dbgconsendreadidx >= sizeof(dbgconsendbuf))
        dbgconsendreadidx -= sizeof(dbgconsendbuf);
    int offset = 0;
    int idx = dbgconsendreadidx;
    if (idx + 16 >= sizeof(dbgconsendbuf))
    {
        offset = sizeof(dbgconsendbuf) - dbgconsendreadidx;
        memcpy(&dbgconsendbuf[dbgconsendreadidx], dbgconoverflowstr, offset);
        idx = 0;
    }
    if (offset != 16) memcpy(&dbgconsendbuf[idx], &dbgconoverflowstr[offset], 16 - offset);
    leave_critical_section(mode);
    return length;
}

void dbgconsole_putc(char string)
{
    dbgconsole_makespace(1);
    dbgconsendbuf[dbgconsendwriteidx++] = string;
    if (dbgconsendwriteidx >= sizeof(dbgconsendbuf))
        dbgconsendwriteidx -= sizeof(dbgconsendbuf);
}

void dbgconsole_write(const char* string, size_t length)
{
    while (length)
    {
        int space = dbgconsole_makespace(length);
        if (dbgconsendwriteidx + space >= sizeof(dbgconsendbuf))
        {
            int bytes = sizeof(dbgconsendbuf) - dbgconsendwriteidx;
            memcpy(&dbgconsendbuf[dbgconsendwriteidx], string, bytes);
            dbgconsendwriteidx = 0;
            string = &string[bytes];
            space -= bytes;
            length -= bytes;
        }
        if (space) memcpy(&dbgconsendbuf[dbgconsendwriteidx], string, space);
        dbgconsendwriteidx += space;
        string = &string[space];
        length -= space;
    }
}

void dbgconsole_puts(const char* string)
{
    dbgconsole_write(string, strlen(string));
}

int dbgconsole_getavailable() ICODE_ATTR;
int dbgconsole_getavailable()
{
    int available = dbgconrecvwriteidx - dbgconrecvreadidx;
    if (available < 0) available += sizeof(dbgconrecvbuf);
    return available;
}

int dbgconsole_getc(int timeout)
{
    if (!dbgconsole_getavailable())
    {
        wakeup_wait(&dbgconrecvwakeup, TIMEOUT_NONE);
        if (!dbgconsole_getavailable())
        {
            wakeup_wait(&dbgconrecvwakeup, timeout);
            if (!dbgconsole_getavailable()) return -1;
        }
    }
    int byte = dbgconrecvbuf[dbgconrecvreadidx++];
    if (dbgconrecvreadidx >= sizeof(dbgconrecvbuf))
        dbgconrecvreadidx -= sizeof(dbgconrecvbuf);
    return byte;
}

int dbgconsole_read(char* buffer, size_t length, int timeout)
{
    if (!length) return 0;
    int available = dbgconsole_getavailable();
    if (!available)
    {
        wakeup_wait(&dbgconrecvwakeup, TIMEOUT_NONE);
        int available = dbgconsole_getavailable();
        if (!available)
        {
            wakeup_wait(&dbgconrecvwakeup, timeout);
            int available = dbgconsole_getavailable();
            if (!available) return 0;
        }
    }
    if (available > length) available = length;
    int left = available;
    if (dbgconrecvreadidx + available >= sizeof(dbgconrecvbuf))
    {
        int bytes = sizeof(dbgconrecvbuf) - dbgconrecvreadidx;
        memcpy(buffer, &dbgconrecvbuf[dbgconrecvreadidx], bytes);
        dbgconrecvreadidx = 0;
        buffer = &buffer[bytes];
        left -= bytes;
    }
    if (left) memcpy(buffer, &dbgconrecvbuf[dbgconrecvreadidx], left);
    dbgconrecvreadidx += left;
    return available;
}

void usb_exit(void)
{
    usb_drv_exit();
}