static struct ehci_qh *ehci_qh_alloc(struct ehci_hcd *ehci, gfp_t flags)
{
struct ehci_qh *qh;
dma_addr_t dma;
dma = usb_malloc(sizeof(struct ehci_qh), flags);
if (dma != 0)
qh = (struct ehci_qh *)IO_ADDRESS(dma);
else
qh = (struct ehci_qh *)
dma_pool_alloc(ehci->qh_pool, flags, &dma);
++g_debug_qH_allocated;
if (qh == NULL) {
panic("run out of i-ram for qH allocation\n");
return qh;
}
memset(qh, 0, sizeof *qh);
qh->refcount = 1;
qh->ehci = ehci;
qh->qh_dma = dma;
INIT_LIST_HEAD(&qh->qtd_list);
/* dummy td enables safe urb queuing */
qh->dummy = ehci_qtd_alloc(ehci, flags);
if (qh->dummy == NULL) {
ehci_dbg(ehci, "no dummy td\n");
dma_pool_free(ehci->qh_pool, qh, qh->qh_dma);
qh = NULL;
}
return qh;
}
static struct ehci_qtd *ehci_qtd_alloc(struct ehci_hcd *ehci, gfp_t flags)
{
struct ehci_qtd *qtd;
dma_addr_t dma;
if (use_iram_qtd) {
dma = usb_malloc(sizeof(struct ehci_qtd), flags);
if (dma != 0)
qtd = (struct ehci_qtd *)IO_ADDRESS(dma);
else
qtd = dma_pool_alloc(ehci->qtd_pool, flags, &dma);
}
else
qtd = dma_pool_alloc(ehci->qtd_pool, flags, &dma);
if (qtd != NULL) {
ehci_qtd_init(ehci, qtd, dma);
++g_debug_qtd_allocated;
} else {
panic
("out of i-ram for qtd allocation g_debug_qtd_allocated %d \
size%d \n", g_debug_qtd_allocated,
sizeof(struct ehci_qtd));
}
return qtd;
}
int usb_guitar_send(void)
{
uint32_t wait_count=0;
usb_packet_t *tx_packet;
//serial_print("send");
//serial_print("\n");
while (1) {
if (!usb_configuration) {
//serial_print("error1\n");
return -1;
}
if (usb_tx_packet_count(GUITAR_ENDPOINT1) < TX_PACKET_LIMIT) {
tx_packet = usb_malloc();
if (tx_packet) break;
}
if (++wait_count > TX_TIMEOUT || transmit_previous_timeout) {
transmit_previous_timeout = 1;
//serial_print("error2\n");
return -1;
}
yield();
}
transmit_previous_timeout = 0;
memcpy(tx_packet->buf, usb_guitar_data, 7);
tx_packet->len = 7;
usb_tx(GUITAR_ENDPOINT1, tx_packet);
//serial_print("ok\n");
return 0;
}
void usb_serial_flush_output()
{
if ( !usb_configuration )
return;
tx_noautoflush = 1;
if ( tx_packet )
{
usb_cdc_transmit_flush_timer = 0;
tx_packet->len = tx_packet->index;
usb_tx( CDC_TX_ENDPOINT, tx_packet );
tx_packet = NULL;
}
else
{
usb_packet_t *tx = usb_malloc();
if ( tx )
{
usb_cdc_transmit_flush_timer = 0;
usb_tx( CDC_TX_ENDPOINT, tx );
}
else
{
usb_cdc_transmit_flush_timer = 1;
}
}
tx_noautoflush = 0;
}
// send the contents of keyboard_keys and keyboard_modifier_keys
int usb_keyboard_send(void)
{
if (!usb_driver_enabled)
return 0;
uint32_t wait_count=0;
usb_packet_t *tx_packet;
while (1) {
if (!usb_configuration) {
return -1;
}
if (usb_tx_packet_count(KEYBOARD_ENDPOINT) < TX_PACKET_LIMIT) {
tx_packet = usb_malloc();
if (tx_packet) break;
}
if (++wait_count > TX_TIMEOUT || transmit_previous_timeout) {
transmit_previous_timeout = 1;
return -1;
}
yield();
}
*(tx_packet->buf) = keyboard_modifier_keys;
*(tx_packet->buf + 1) = keyboard_media_keys;
memcpy(tx_packet->buf + 2, keyboard_keys, 6);
tx_packet->len = 8;
usb_tx(KEYBOARD_ENDPOINT, tx_packet);
return 0;
}
/*------------------------------------------------------------------------*
* usb_pc_alloc_mem - allocate DMA'able memory
*
* Returns:
* 0: Success
* Else: Failure
*------------------------------------------------------------------------*/
uint8_t
usb_pc_alloc_mem(struct usb_page_cache *pc, struct usb_page *pg,
usb_size_t size, usb_size_t align)
{
void *ptr;
uint32_t rem;
/* allocate zeroed memory */
if (align != 1) {
ptr = usb_malloc(size + align);
if (ptr == NULL)
goto error;
rem = (-((uintptr_t)ptr)) & (align - 1);
} else {
ptr = usb_malloc(size);
if (ptr == NULL)
goto error;
rem = 0;
}
/* setup page cache */
pc->buffer = ((uint8_t *)ptr) + rem;
pc->page_start = pg;
pc->page_offset_buf = 0;
pc->page_offset_end = size;
pc->map = NULL;
pc->tag = ptr;
pc->ismultiseg = (align == 1);
/* compute physical address */
usb_pc_common_mem_cb(pc, ptr, size);
usb_pc_cpu_flush(pc);
return (0);
error:
/* reset most of the page cache */
pc->buffer = NULL;
pc->page_start = NULL;
pc->page_offset_buf = 0;
pc->page_offset_end = 0;
pc->map = NULL;
pc->tag = NULL;
return (1);
}
/// Allocates a buffer. Overrides the (weak) definition to put it to a
/// separate RAM segment and leave more heap space free.
/// @param size in bytes, how large chunk we should allocate.
/// @return a newly allocated buffer. Cannot be freed.
void *buffer_malloc(size_t length)
{
/* We do a trick here to ensure that the compiler will output a stack frame
* for this function. We want to avoid tail-chain optimization in this
* function or else it disappears from the stack traces done for memory
* tracing. */
void *volatile v = usb_malloc(length);
return v;
}
int usb_serial_write( const void *buffer, uint32_t size )
{
uint32_t len;
uint32_t wait_count;
const uint8_t *src = (const uint8_t *)buffer;
uint8_t *dest;
tx_noautoflush = 1;
while ( size > 0 )
{
if ( !tx_packet )
{
wait_count = 0;
while ( 1 )
{
if ( !usb_configuration )
{
tx_noautoflush = 0;
return -1;
}
if ( usb_tx_packet_count( CDC_TX_ENDPOINT ) < TX_PACKET_LIMIT )
{
tx_noautoflush = 1;
tx_packet = usb_malloc();
if ( tx_packet )
break;
tx_noautoflush = 0;
}
if ( ++wait_count > TX_TIMEOUT || transmit_previous_timeout )
{
transmit_previous_timeout = 1;
return -1;
}
yield();
}
}
transmit_previous_timeout = 0;
len = CDC_TX_SIZE - tx_packet->index;
if ( len > size )
len = size;
dest = tx_packet->buf + tx_packet->index;
tx_packet->index += len;
size -= len;
while ( len-- > 0 )
*dest++ = *src++;
if ( tx_packet->index >= CDC_TX_SIZE )
{
tx_packet->len = CDC_TX_SIZE;
usb_tx( CDC_TX_ENDPOINT, tx_packet );
tx_packet = NULL;
}
usb_cdc_transmit_flush_timer = TRANSMIT_FLUSH_TIMEOUT;
}
tx_noautoflush = 0;
return 0;
}
/** Custom malloc function for stack spaces.
* \param length the length of block to allocate
* \returns a pointer to the newly allocated block.
*
* There is currently no way to free blocks allocated with this function, so
* only suitable for stacks of threads that are running throughout the entire
* life of the application.
*/
void* stack_malloc(unsigned long length)
{
if (config_use_separate_stack_segment() == CONSTANT_TRUE) {
char* old_stack_start = sstack_start;
char* new_stack_start = sstack_start + length;
if (new_stack_start > &__stacks_min__)
{
diewith(BLINK_DIE_OUTOFMEMSTACK);
}
sstack_start = new_stack_start;
return old_stack_start;
} else {
return usb_malloc(length);
}
}
void usb_serial_flush_callback(void)
{
if (tx_noautoflush) return;
if (tx_packet) {
tx_packet->len = tx_packet->index;
usb_tx(CDC_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
} else {
usb_packet_t *tx = usb_malloc();
if (tx) {
usb_tx(CDC_TX_ENDPOINT, tx);
} else {
usb_cdc_transmit_flush_timer = 1;
}
}
}
void init_ledPacketBuffer(ledPacketBuffer *const pBuffer)
{
pBuffer->pCounter = 0;
pBuffer->pByteCount = 0;
pBuffer-> offset = 0;
// Allocate packets. They'll have zero'ed contents initially.
for (unsigned i = 0; i < NUM_BUFFS_PER_FRAME; ++i) {
usb_packet_t *p = usb_malloc();
if(p){
memset(p->buf, 0, sizeof p->buf);
pBuffer->packets[i] = p;
}else{
//UB_Led_Toggle(LED_RED);//ERROR TODO
}
}
}
void FlightSimClass::xmit(const void *p1, uint8_t n1, const void *p2, uint8_t n2)
{
uint16_t total;
total = n1 + n2;
if (total > FLIGHTSIM_TX_SIZE) {
xmit_big_packet(p1, n1, p2, n2);
return;
}
if (!enabled || !usb_configuration) return;
tx_noautoflush = 1;
if (tx_packet) {
if (total <= FLIGHTSIM_TX_SIZE - tx_packet->index) goto send;
for (int i = tx_packet->index; i < FLIGHTSIM_TX_SIZE; i++) {
tx_packet->buf[i] = 0;
}
tx_packet->len = FLIGHTSIM_TX_SIZE;
usb_tx(FLIGHTSIM_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
}
while (1) {
if (usb_tx_packet_count(FLIGHTSIM_TX_ENDPOINT) < TX_PACKET_LIMIT) {
tx_packet = usb_malloc();
if (tx_packet) break;
}
if (!enabled || !usb_configuration) {
tx_noautoflush = 0;
return;
}
tx_noautoflush = 0;
yield();
tx_noautoflush = 1;
}
send:
memcpy(tx_packet->buf + tx_packet->index, p1, n1);
tx_packet->index += n1;
if (n2 > 0) {
memcpy(tx_packet->buf + tx_packet->index, p2, n2);
tx_packet->index += n2;
}
if (tx_packet->index >= FLIGHTSIM_TX_SIZE) {
tx_packet->len = FLIGHTSIM_TX_SIZE;
usb_tx(FLIGHTSIM_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
}
tx_noautoflush = 0;
}
int usb_rawhid_send(const void *buffer, uint32_t timeout)
{
usb_packet_t *tx_packet;
uint32_t begin = millis();
while (1) {
if (!usb_configuration) return -1;
if (usb_tx_packet_count(RAWHID_TX_ENDPOINT) < TX_PACKET_LIMIT) {
tx_packet = usb_malloc();
if (tx_packet) break;
}
if (millis() - begin > timeout) return 0;
yield();
}
memcpy(tx_packet->buf, buffer, RAWHID_TX_SIZE);
tx_packet->len = RAWHID_TX_SIZE;
usb_tx(RAWHID_TX_ENDPOINT, tx_packet);
return RAWHID_TX_SIZE;
}
// transmit a character. 0 returned on success, -1 on error
int usb_serial_putchar(uint8_t c)
{
#if 1
return usb_serial_write(&c, 1);
#endif
#if 0
uint32_t wait_count;
tx_noautoflush = 1;
if (!tx_packet) {
wait_count = 0;
while (1) {
if (!usb_configuration) {
tx_noautoflush = 0;
return -1;
}
if (usb_tx_packet_count(CDC_TX_ENDPOINT) < TX_PACKET_LIMIT) {
tx_noautoflush = 1;
tx_packet = usb_malloc();
if (tx_packet) break;
tx_noautoflush = 0;
}
if (++wait_count > TX_TIMEOUT || transmit_previous_timeout) {
transmit_previous_timeout = 1;
return -1;
}
}
}
transmit_previous_timeout = 0;
tx_packet->buf[tx_packet->index++] = c;
if (tx_packet->index < CDC_TX_SIZE) {
usb_cdc_transmit_flush_timer = TRANSMIT_FLUSH_TIMEOUT;
} else {
tx_packet->len = CDC_TX_SIZE;
usb_cdc_transmit_flush_timer = 0;
usb_tx(CDC_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
}
tx_noautoflush = 0;
return 0;
#endif
}
// send the contents of keyboard_keys and keyboard_modifier_keys
int usb_keyboard_send(void)
{
#if 0
serial_print("Send:");
serial_phex(keyboard_modifier_keys);
serial_phex(keyboard_keys[0]);
serial_phex(keyboard_keys[1]);
serial_phex(keyboard_keys[2]);
serial_phex(keyboard_keys[3]);
serial_phex(keyboard_keys[4]);
serial_phex(keyboard_keys[5]);
serial_print("\n");
#endif
#if 1
uint32_t wait_count=0;
usb_packet_t *tx_packet;
while (1) {
if (!usb_configuration) {
return -1;
}
if (usb_tx_packet_count(KEYBOARD_ENDPOINT) < TX_PACKET_LIMIT) {
tx_packet = usb_malloc();
if (tx_packet) break;
}
if (++wait_count > TX_TIMEOUT || transmit_previous_timeout) {
transmit_previous_timeout = 1;
return -1;
}
yield();
}
*(tx_packet->buf) = keyboard_modifier_keys;
*(tx_packet->buf + 1) = keyboard_media_keys;
memcpy(tx_packet->buf + 2, keyboard_keys, 6);
tx_packet->len = 8;
usb_tx(KEYBOARD_ENDPOINT, tx_packet);
#endif
return 0;
}
int usb_serial_write_buffer_free(void)
{
uint32_t len;
tx_noautoflush = 1;
if (!tx_packet) {
if (!usb_configuration ||
usb_tx_packet_count(CDC_TX_ENDPOINT) >= TX_PACKET_LIMIT ||
(tx_packet = usb_malloc()) == NULL) {
tx_noautoflush = 0;
return 0;
}
}
len = CDC_TX_SIZE - tx_packet->index;
// TODO: Perhaps we need "usb_cdc_transmit_flush_timer = TRANSMIT_FLUSH_TIMEOUT"
// added here, so the SOF interrupt can't take away the available buffer
// space we just promised the user could write without blocking?
// But does this come with other performance downsides? Could it lead to
// buffer data never actually transmitting in some usage cases? More
// investigation is needed.
// https://github.com/PaulStoffregen/cores/issues/10#issuecomment-61514955
tx_noautoflush = 0;
return len;
}
int usb_otg_init(int id, usb_otg_t * otg_ptr, ps_io_ops_t ioops)
{
usb_otg_t otg;
int err;
/* Validate the id */
if (id < 0 || id > USB_NOTGS) {
return -1;
}
/* Allocate host memory */
otg = usb_malloc(sizeof(*otg));
if (otg == NULL) {
ZF_LOGE("OTG: Out of memory\n");
return -1;
}
otg->dman = &ioops.dma_manager;
otg->id = id;
otg->ep0_setup = NULL;
otg->prime = NULL;
err = usb_plat_otg_init(otg, &ioops);
if (!err) {
*otg_ptr = otg;
}
return err;
}
void usb_isr(void)
{
uint8_t status, stat, t;
//serial_print("isr");
//status = USB0_ISTAT;
//serial_phex(status);
//serial_print("\n");
restart:
status = USB0_ISTAT;
if ((status & USB_INTEN_SOFTOKEN /* 04 */ )) {
if (usb_configuration) {
t = usb_reboot_timer;
if (t) {
usb_reboot_timer = --t;
if (!t) _reboot_Teensyduino_();
}
#ifdef CDC_DATA_INTERFACE
t = usb_cdc_transmit_flush_timer;
if (t) {
usb_cdc_transmit_flush_timer = --t;
if (t == 0) usb_serial_flush_callback();
}
#endif
#if SEREMU_INTERFACE
t = usb_seremu_transmit_flush_timer;
if (t) {
usb_seremu_transmit_flush_timer = --t;
if (t == 0) usb_seremu_flush_callback();
}
#endif
}
USB0_ISTAT = USB_INTEN_SOFTOKEN;
}
if ((status & USB_ISTAT_TOKDNE /* 08 */ )) {
uint8_t endpoint;
stat = USB0_STAT;
//serial_print("token: ep=");
//serial_phex(stat >> 4);
//serial_print(stat & 0x08 ? ",tx" : ",rx");
//serial_print(stat & 0x04 ? ",odd\n" : ",even\n");
endpoint = stat >> 4;
if (endpoint == 0) {
usb_control(stat);
} else {
bdt_t *b = stat2bufferdescriptor(stat);
usb_packet_t *packet = (usb_packet_t *)((uint8_t *)(b->addr) - 8);
#if 0
serial_print("ep:");
serial_phex(endpoint);
serial_print(", pid:");
serial_phex(BDT_PID(b->desc));
serial_print(((uint32_t)b & 8) ? ", odd" : ", even");
serial_print(", count:");
serial_phex(b->desc >> 16);
serial_print("\n");
#endif
endpoint--; // endpoint is index to zero-based arrays
if (stat & 0x08) { // transmit
usb_free(packet);
packet = tx_first[endpoint];
if (packet) {
//serial_print("tx packet\n");
tx_first[endpoint] = packet->next;
b->addr = packet->buf;
switch (tx_state[endpoint]) {
case TX_STATE_BOTH_FREE_EVEN_FIRST:
tx_state[endpoint] = TX_STATE_ODD_FREE;
break;
case TX_STATE_BOTH_FREE_ODD_FIRST:
tx_state[endpoint] = TX_STATE_EVEN_FREE;
break;
case TX_STATE_EVEN_FREE:
case TX_STATE_ODD_FREE:
default:
tx_state[endpoint] = TX_STATE_NONE_FREE;
break;
}
b->desc = BDT_DESC(packet->len, ((uint32_t)b & 8) ? DATA1 : DATA0);
} else {
//serial_print("tx no packet\n");
switch (tx_state[endpoint]) {
case TX_STATE_BOTH_FREE_EVEN_FIRST:
case TX_STATE_BOTH_FREE_ODD_FIRST:
break;
case TX_STATE_EVEN_FREE:
tx_state[endpoint] = TX_STATE_BOTH_FREE_EVEN_FIRST;
break;
case TX_STATE_ODD_FREE:
tx_state[endpoint] = TX_STATE_BOTH_FREE_ODD_FIRST;
break;
default:
tx_state[endpoint] = ((uint32_t)b & 8) ?
TX_STATE_ODD_FREE : TX_STATE_EVEN_FREE;
break;
}
}
} else { // receive
packet->len = b->desc >> 16;
packet->index = 0;
packet->next = NULL;
if (rx_first[endpoint] == NULL) {
//serial_print("rx 1st, epidx=");
//serial_phex(endpoint);
//serial_print(", packet=");
//serial_phex32((uint32_t)packet);
//serial_print("\n");
rx_first[endpoint] = packet;
} else {
//serial_print("rx Nth, epidx=");
//serial_phex(endpoint);
//serial_print(", packet=");
//serial_phex32((uint32_t)packet);
//serial_print("\n");
rx_last[endpoint]->next = packet;
}
rx_last[endpoint] = packet;
// TODO: implement a per-endpoint maximum # of allocated packets
// so a flood of incoming data on 1 endpoint doesn't starve
// the others if the user isn't reading it regularly
packet = usb_malloc();
if (packet) {
b->addr = packet->buf;
b->desc = BDT_DESC(64, ((uint32_t)b & 8) ? DATA1 : DATA0);
} else {
//serial_print("starving ");
//serial_phex(endpoint + 1);
//serial_print(((uint32_t)b & 8) ? ",odd\n" : ",even\n");
b->desc = 0;
usb_rx_memory_needed++;
}
}
}
USB0_ISTAT = USB_ISTAT_TOKDNE;
goto restart;
}
void FlightSimClass::xmit_big_packet(const void *p1, uint8_t n1, const void *p2, uint8_t n2)
{
if (!enabled || !usb_configuration) return;
uint16_t remaining = n1 + n2;
if (remaining > 255) return;
bool part2 = false;
uint8_t remainingPart1 = n1;
const uint8_t *dataPtr = (const uint8_t*)p1;
bool writeFragmentHeader = false;
uint8_t fragmentCounter = 1;
tx_noautoflush =1; // don't mess with my data, I'm working on it!
if (tx_packet) {
// If we have a current packet, fill it with whatever fits
uint8_t partLen = FLIGHTSIM_TX_SIZE - tx_packet->index;
if (partLen > n1) partLen=n1;
// copy first part, containing total packet length
memcpy(tx_packet->buf + tx_packet->index, dataPtr, partLen);
remainingPart1 -= partLen;
tx_packet->index += partLen;
if (remainingPart1) {
// there still is data from the first part that
// will go to the next packet. The boolean variable
// part2 remains false
remaining = remainingPart1+n2;
dataPtr += partLen;
} else {
// maybe we have space for some data from the second part
part2=true;
partLen = FLIGHTSIM_TX_SIZE - tx_packet->index;
// there is no need here to check whether partLen is
// bigger than n2. It's not. If it were, all the data
// would have fit in a single packet and xmit_big_packet
// would never have been called...
remaining = n2;
if (partLen) {
memcpy(tx_packet->buf + tx_packet->index, p2, partLen);
remaining -= partLen;
tx_packet->index += partLen;
}
dataPtr = (const uint8_t*)p2 + partLen;
}
// Packet padding should not be necessary, as xmit_big_packet
// will only be called for data that doesn't fit in a single
// packet. So, the previous code should always fill up the
// first packet. Right?
for (int i = tx_packet->index; i < FLIGHTSIM_TX_SIZE; i++) {
tx_packet->buf[i] = 0;
}
// queue first packet for sending
tx_packet->len = FLIGHTSIM_TX_SIZE;
usb_tx(FLIGHTSIM_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
writeFragmentHeader = true;
} else {
remaining = n1+n2;
}
while (remaining >0) {
while (1) {
// get memory for next packet
if (usb_tx_packet_count(FLIGHTSIM_TX_ENDPOINT) < TX_PACKET_LIMIT) {
tx_packet = usb_malloc();
if (tx_packet) {
break;
}
}
if (!enabled || !usb_configuration) {
// teensy disconnected
tx_noautoflush = 0;
return;
}
tx_noautoflush = 0; // you can pick up my data, if you like
yield(); // do other things and wait for memory to become free
tx_noautoflush = 1; // wait, I'm working on the packet data
}
if (writeFragmentHeader) {
tx_packet->buf[0]=(remaining+3 <= FLIGHTSIM_TX_SIZE) ? (byte) remaining+3 : FLIGHTSIM_TX_SIZE;
tx_packet->buf[1]=0xff;
tx_packet->buf[2]=fragmentCounter++;
tx_packet->index=3;
}
if (!part2) {
// we still need to send the first part
uint8_t partLen = FLIGHTSIM_TX_SIZE - tx_packet->index;
if (partLen > remainingPart1)
partLen=remainingPart1;
memcpy(tx_packet->buf + tx_packet->index, dataPtr, partLen);
dataPtr += partLen;
remainingPart1 -= partLen;
tx_packet->index += partLen;
remaining -= partLen;
if (!remainingPart1) {
part2=true;
dataPtr = (const uint8_t*)p2;
}
}
if (part2) {
uint8_t partLen = FLIGHTSIM_TX_SIZE - tx_packet->index;
if (partLen) {
if (partLen > remaining)
partLen=remaining;
memcpy(tx_packet->buf + tx_packet->index, dataPtr, partLen);
remaining -= partLen;
tx_packet->index += partLen;
dataPtr += partLen;
}
}
writeFragmentHeader = true;
if (tx_packet->index >= FLIGHTSIM_TX_SIZE) {
// queue packet for sending
tx_packet->len = FLIGHTSIM_TX_SIZE;
usb_tx(FLIGHTSIM_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
}
}
tx_noautoflush = 0; // data is ready to be transmitted on start of USB token
}
// send the contents of keyboard_keys and keyboard_modifier_keys
void usb_keyboard_send()
{
uint32_t wait_count = 0;
usb_packet_t *tx_packet;
// Wait till ready
while ( 1 )
{
if ( !usb_configuration )
{
erro_print("USB not configured...");
return;
}
if ( USBKeys_Protocol == 0 ) // Boot Mode
{
if ( usb_tx_packet_count( NKRO_KEYBOARD_ENDPOINT ) < TX_PACKET_LIMIT )
{
tx_packet = usb_malloc();
if ( tx_packet )
break;
}
}
else if ( USBKeys_Protocol == 1 ) // NKRO Mode
{
if ( usb_tx_packet_count( KEYBOARD_ENDPOINT ) < TX_PACKET_LIMIT )
{
tx_packet = usb_malloc();
if ( tx_packet )
break;
}
}
if ( ++wait_count > TX_TIMEOUT || transmit_previous_timeout )
{
transmit_previous_timeout = 1;
warn_print("USB Transmit Timeout...");
return;
}
yield();
}
// Pointer to USB tx packet buffer
uint8_t *tx_buf = tx_packet->buf;
switch ( USBKeys_Protocol )
{
// Send boot keyboard interrupt packet(s)
case 0:
// USB Boot Mode debug output
if ( Output_DebugMode )
{
dbug_msg("Boot USB: ");
printHex_op( USBKeys_Modifiers, 2 );
print(" ");
printHex( 0 );
print(" ");
printHex_op( USBKeys_Keys[0], 2 );
printHex_op( USBKeys_Keys[1], 2 );
printHex_op( USBKeys_Keys[2], 2 );
printHex_op( USBKeys_Keys[3], 2 );
printHex_op( USBKeys_Keys[4], 2 );
printHex_op( USBKeys_Keys[5], 2 );
print( NL );
}
// Boot Mode
*tx_buf++ = USBKeys_Modifiers;
*tx_buf++ = 0;
memcpy( tx_buf, USBKeys_Keys, USB_BOOT_MAX_KEYS );
tx_packet->len = 8;
// Send USB Packet
usb_tx( KEYBOARD_ENDPOINT, tx_packet );
USBKeys_Changed = USBKeyChangeState_None;
break;
// Send NKRO keyboard interrupts packet(s)
case 1:
if ( Output_DebugMode )
{
dbug_msg("NKRO USB: ");
}
// Check system control keys
if ( USBKeys_Changed & USBKeyChangeState_System )
{
if ( Output_DebugMode )
{
print("SysCtrl[");
printHex_op( USBKeys_SysCtrl, 2 );
print( "] " NL );
}
*tx_buf++ = 0x02; // ID
*tx_buf = USBKeys_SysCtrl;
tx_packet->len = 2;
// Send USB Packet
usb_tx( NKRO_KEYBOARD_ENDPOINT, tx_packet );
USBKeys_Changed &= ~USBKeyChangeState_System; // Mark sent
}
// Check consumer control keys
if ( USBKeys_Changed & USBKeyChangeState_Consumer )
{
if ( Output_DebugMode )
{
print("ConsCtrl[");
printHex_op( USBKeys_ConsCtrl, 2 );
print( "] " NL );
}
*tx_buf++ = 0x03; // ID
*tx_buf++ = (uint8_t)(USBKeys_ConsCtrl & 0x00FF);
*tx_buf = (uint8_t)(USBKeys_ConsCtrl >> 8);
tx_packet->len = 3;
// Send USB Packet
usb_tx( NKRO_KEYBOARD_ENDPOINT, tx_packet );
USBKeys_Changed &= ~USBKeyChangeState_Consumer; // Mark sent
}
// Standard HID Keyboard
if ( USBKeys_Changed )
{
// USB NKRO Debug output
if ( Output_DebugMode )
{
printHex_op( USBKeys_Modifiers, 2 );
print(" ");
for ( uint8_t c = 0; c < 6; c++ )
printHex_op( USBKeys_Keys[ c ], 2 );
print(" ");
for ( uint8_t c = 6; c < 20; c++ )
printHex_op( USBKeys_Keys[ c ], 2 );
print(" ");
printHex_op( USBKeys_Keys[20], 2 );
print(" ");
for ( uint8_t c = 21; c < 27; c++ )
printHex_op( USBKeys_Keys[ c ], 2 );
print( NL );
}
tx_packet->len = 0;
// Modifiers
*tx_buf++ = 0x01; // ID
*tx_buf++ = USBKeys_Modifiers;
tx_packet->len += 2;
// 4-49 (first 6 bytes)
memcpy( tx_buf, USBKeys_Keys, 6 );
tx_buf += 6;
tx_packet->len += 6;
// 51-155 (Middle 14 bytes)
memcpy( tx_buf, USBKeys_Keys + 6, 14 );
tx_buf += 14;
tx_packet->len += 14;
// 157-164 (Next byte)
memcpy( tx_buf, USBKeys_Keys + 20, 1 );
tx_buf += 1;
tx_packet->len += 1;
// 176-221 (last 6 bytes)
memcpy( tx_buf, USBKeys_Keys + 21, 6 );
tx_packet->len += 6;
// Send USB Packet
usb_tx( NKRO_KEYBOARD_ENDPOINT, tx_packet );
USBKeys_Changed = USBKeyChangeState_None; // Mark sent
}
break;
}
return;
}
static void usb_setup(void)
{
const uint8_t *data = NULL;
uint32_t datalen = 0;
const usb_descriptor_list_t *list;
uint32_t size;
volatile uint8_t *reg;
uint8_t epconf;
const uint8_t *cfg;
int i;
switch (setup.wRequestAndType) {
case 0x0500: // SET_ADDRESS
break;
case 0x0900: // SET_CONFIGURATION
//serial_print("configure\n");
usb_configuration = setup.wValue;
reg = &USB0_ENDPT1;
cfg = usb_endpoint_config_table;
// clear all BDT entries, free any allocated memory...
for (i=4; i < NUM_ENDPOINTS*4; i++) {
if (table[i].desc & BDT_OWN) {
usb_free((usb_packet_t *)((uint8_t *)(table[i].addr) - 8));
table[i].desc = 0;
}
}
usb_rx_memory_needed = 0;
for (i=1; i < NUM_ENDPOINTS; i++) {
epconf = *cfg++;
*reg = epconf;
reg += 4;
if (epconf & USB_ENDPT_EPRXEN) {
usb_packet_t *p;
p = usb_malloc();
if (p) {
table[index(i, RX, EVEN)].addr = p->buf;
table[index(i, RX, EVEN)].desc = BDT_DESC(64, 0);
} else {
table[index(i, RX, EVEN)].desc = 0;
usb_rx_memory_needed++;
}
p = usb_malloc();
if (p) {
table[index(i, RX, ODD)].addr = p->buf;
table[index(i, RX, ODD)].desc = BDT_DESC(64, 1);
} else {
table[index(i, RX, ODD)].desc = 0;
usb_rx_memory_needed++;
}
}
table[index(i, TX, EVEN)].desc = 0;
table[index(i, TX, ODD)].desc = 0;
}
break;
case 0x0880: // GET_CONFIGURATION
reply_buffer[0] = usb_configuration;
datalen = 1;
data = reply_buffer;
break;
case 0x0080: // GET_STATUS (device)
reply_buffer[0] = 0;
reply_buffer[1] = 0;
datalen = 2;
data = reply_buffer;
break;
case 0x0082: // GET_STATUS (endpoint)
if (setup.wIndex > NUM_ENDPOINTS) {
// TODO: do we need to handle IN vs OUT here?
endpoint0_stall();
return;
}
reply_buffer[0] = 0;
reply_buffer[1] = 0;
if (*(uint8_t *)(&USB0_ENDPT0 + setup.wIndex * 4) & 0x02) reply_buffer[0] = 1;
data = reply_buffer;
datalen = 2;
break;
case 0x0102: // CLEAR_FEATURE (endpoint)
i = setup.wIndex & 0x7F;
if (i > NUM_ENDPOINTS || setup.wValue != 0) {
// TODO: do we need to handle IN vs OUT here?
endpoint0_stall();
return;
}
(*(uint8_t *)(&USB0_ENDPT0 + setup.wIndex * 4)) &= ~0x02;
// TODO: do we need to clear the data toggle here?
break;
case 0x0302: // SET_FEATURE (endpoint)
i = setup.wIndex & 0x7F;
if (i > NUM_ENDPOINTS || setup.wValue != 0) {
// TODO: do we need to handle IN vs OUT here?
endpoint0_stall();
return;
}
(*(uint8_t *)(&USB0_ENDPT0 + setup.wIndex * 4)) |= 0x02;
// TODO: do we need to clear the data toggle here?
break;
case 0x0680: // GET_DESCRIPTOR
case 0x0681:
//serial_print("desc:");
//serial_phex16(setup.wValue);
//serial_print("\n");
for (list = usb_descriptor_list; 1; list++) {
if (list->addr == NULL) break;
//if (setup.wValue == list->wValue &&
//(setup.wIndex == list->wIndex) || ((setup.wValue >> 8) == 3)) {
if (setup.wValue == list->wValue && setup.wIndex == list->wIndex) {
data = list->addr;
datalen = list->length;
#if 0
serial_print("Desc found, ");
serial_phex32((uint32_t)data);
serial_print(",");
serial_phex16(datalen);
serial_print(",");
serial_phex(data[0]);
serial_phex(data[1]);
serial_phex(data[2]);
serial_phex(data[3]);
serial_phex(data[4]);
serial_phex(data[5]);
serial_print("\n");
#endif
goto send;
}
}
//serial_print("desc: not found\n");
endpoint0_stall();
return;
#if defined(CDC_STATUS_INTERFACE)
case 0x2221: // CDC_SET_CONTROL_LINE_STATE
usb_cdc_line_rtsdtr = setup.wValue;
//serial_print("set control line state\n");
break;
case 0x2021: // CDC_SET_LINE_CODING
//serial_print("set coding, waiting...\n");
return;
#endif
// TODO: this does not work... why?
#if defined(SEREMU_INTERFACE) || defined(KEYBOARD_INTERFACE)
case 0x0921: // HID SET_REPORT
//serial_print(":)\n");
return;
case 0x0A21: // HID SET_IDLE
break;
// case 0xC940:
#endif
default:
endpoint0_stall();
return;
}
send:
//serial_print("setup send ");
//serial_phex32(data);
//serial_print(",");
//serial_phex16(datalen);
//serial_print("\n");
if (datalen > setup.wLength) datalen = setup.wLength;
size = datalen;
if (size > EP0_SIZE) size = EP0_SIZE;
endpoint0_transmit(data, size);
data += size;
datalen -= size;
if (datalen == 0 && size < EP0_SIZE) return;
size = datalen;
if (size > EP0_SIZE) size = EP0_SIZE;
endpoint0_transmit(data, size);
data += size;
datalen -= size;
if (datalen == 0 && size < EP0_SIZE) return;
ep0_tx_ptr = data;
ep0_tx_len = datalen;
}
/// Allocates a struct reent. Overrides the (weak) definition to put it to a
/// separate RAM segment and leave more heap space free.
/// @return a newly allocated struct reent. Cannot be freed.
struct _reent* allocate_reent(void)
{
struct _reent* data = usb_malloc(sizeof(struct _reent));
_REENT_INIT_PTR(data);
return data;
}
