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;
}
ssize_t write(const uint8_t *m, size_t s)
{
if (!usb_enabled)
return -1;
return usb_tx(m, s);
}
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_tx_if_possible()
{
if (!usb_txf_avail(1, sizeof(g_tx_buf)))
return;
if (!usb_tx(1, g_tx_buf, sizeof(g_tx_buf)))
printf("error in usb_tx()\r\n");
}
static void
cdc_tx_done(void *buf, ssize_t len, void *data)
{
struct cdc_ctx *ctx = data;
if (len <= 0)
goto queue;
crit_enter();
ctx->out_sent += len;
memcpy(ctx->outbuf, &ctx->outbuf[ctx->out_sent], ctx->out_pos - ctx->out_sent);
ctx->out_pos -= ctx->out_sent;
ctx->out_sent = 0;
ctx->out_queued = 0;
crit_exit();
if (ctx->data_sent_cb != NULL)
ctx->data_sent_cb(sizeof(ctx->outbuf) - ctx->out_pos);
queue:
crit_enter();
if (ctx->out_pos > 0 && !ctx->out_queued) {
ctx->out_queued = 1;
usb_tx(ctx->tx_pipe, ctx->outbuf, ctx->out_pos, CDC_TX_SIZE, cdc_tx_done, ctx);
}
crit_exit();
}
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;
}
}
}
// 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;
}
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;
}
void usb_serial_flush_callback(void)
{
if (tx_noautoflush) return;
//serial_print("usb_flush_callback \n");
tx_packet->len = tx_packet->index;
usb_tx(CDC_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
//serial_print("usb_flush_callback end\n");
}
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;
}
// This gets called from usb_isr when a USB start token arrives.
// If we have a packet to transmit AND transmission isn't disabled
// by tx_noautoflush, we fill it up with zeros and send it out
// to USB
void usb_flightsim_flush_callback(void)
{
if (tx_noautoflush || !tx_packet || tx_packet->index == 0) return;
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;
}
void
hid_update_data(struct hid_ctx *ctx, uint8_t report_id, const void *data, size_t len)
{
if (ctx->get_report_outstanding_length != 0) {
usb_ep0_tx(data, len, ctx->get_report_outstanding_length, NULL, NULL);
ctx->get_report_outstanding_length = 0;
usb_handle_control_status(0);
} else if (ctx->hidf->report_max_size > 0) {
usb_tx(ctx->tx_pipe, data, len, ctx->hidf->report_max_size, NULL, NULL);
}
}
void usb_serial_flush_output(void)
{
if (!usb_configuration) return;
//serial_print("usb_serial_flush_output\n");
if (tx_packet && tx_packet->index > 0) {
usb_cdc_transmit_flush_timer = 0;
tx_packet->len = tx_packet->index;
usb_tx(CDC_TX_ENDPOINT, tx_packet);
tx_packet = NULL;
}
// while (usb_tx_byte_count(CDC_TX_ENDPOINT) > 0) ; // wait
}
int
usb_ep0_tx_cp(const void *buf, size_t len, size_t reqlen, ep_callback_t cb, void *cb_data)
{
enum usb_ep_pingpong pp = usb.ep_state[0].tx.pingpong;
void *destbuf = ep0_buf[pp];
if (len > EP0_BUFSIZE)
return (-1);
memcpy(destbuf, buf, len);
return (usb_tx(&usb.ep_state[0].tx, destbuf, len, reqlen, cb, cb_data));
}
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 main()
{
led_init();
led_on();
console_init();
printf("===== APP ENTRY =====\r\n");
systime_init();
enc_init();
usb_init();
halls_init();
therm_init();
//enc_print_regs();
printf("entering blink loop...\r\n");
__enable_irq();
usb_tx(1, g_tx_buf, sizeof(g_tx_buf));
uint16_t raw_angle = 0, prev_raw_angle = 0;
float raw_vel = 0;
float filt_vel[3] = {0};
float filt_angle[3] = {0};
//float raw_vel = 0, filt_vel = 0, filt_angle = 0;
bool filter_init = false;
float unwrapped_raw = 0, prev_unwrapped_raw = 0;
uint32_t t = 0, t_last_led_blink = 0;
const float pos_gain[3] = { 0.9f, 0.99f, 0.999f };
const float vel_gain[3] = { 0.99f, 0.999f, 0.9999f };
int wraps = 0;
uint32_t t_last_therm_reading = 0;
g_therm_celsius = therm_celsius();
while (1)
{
if (SYSTIME - t_last_therm_reading > 1000)
{
g_therm_celsius = therm_celsius();
t_last_therm_reading = SYSTIME;
}
if (SYSTIME - t_last_led_blink > 100000)
{
t_last_led_blink = SYSTIME;
led_toggle();
/*
printf("\n\n");
printf("gintsts = 0x%08x\r\n", (unsigned)USB_OTG_FS->GINTSTS);
printf("dctl = 0x%08x\r\n", (unsigned)g_usbd_dbg->DCTL);
printf("dsts = 0x%08x\r\n", (unsigned)g_usbd_dbg->DSTS);
printf("dtxfsts1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DTXFSTS);
printf("diepctl1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPCTL);
printf("diepint1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPINT);
printf("dieptsiz1= 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPTSIZ);
*/
}
raw_angle = enc_poll_angle();
t = SYSTIME;
if (filter_init)
{
int diff = raw_angle - prev_raw_angle;
if (diff > 8000)
wraps--;
else if (diff < -8000)
wraps++;
unwrapped_raw = (float)raw_angle + wraps * 16384;
// calculate raw_vel in ticks/usec for numerical stability
// TODO: use a better timebase, since we're polling @ 100 khz so there
// is extreme quantization on the microsecond clock
float dt_usecs = (float)(t - g_t_angle) * 1000000.0f;
if (dt_usecs < 1.0f)
dt_usecs = 1.0f;
// todo: this leads to bad numerical stability after lots of wraps
// need to re-work this crap
raw_vel = (unwrapped_raw - prev_unwrapped_raw) / dt_usecs;
for (int i = 0; i < 3; i++)
{
filt_angle[i] = pos_gain[i] * filt_angle[i] +
(1.0f - pos_gain[i]) * unwrapped_raw;
filt_vel[i] = vel_gain[i] * filt_vel[i] +
(1.0f - vel_gain[i]) * raw_vel * 1000000.0f;
}
}
else
{
filter_init = true;
for (int i = 0; i < 3; i++)
{
filt_angle[i] = raw_angle;
filt_vel[i] = 0;
}
}
prev_raw_angle = raw_angle;
prev_unwrapped_raw = unwrapped_raw;
__disable_irq();
g_t_angle = t;
g_raw_angle = raw_angle;
for (int i = 0; i < 3; i++)
{
g_angle[i] = filt_angle[i];
g_vel[i] = filt_vel[i]; // * 0.000001f; // convert to ticks / sec
}
g_num_samp++;
__enable_irq();
}
return 0;
}
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
}
int
usb_ep0_tx(void *buf, size_t len, size_t reqlen, ep_callback_t cb, void *cb_data)
{
return (usb_tx(&usb.ep_state[0].tx, buf, len, reqlen, cb, cb_data));
}
// 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;
}
void usb_rx_setup(const uint8_t *buf, const unsigned len)
{
const uint8_t req_type = buf[0];
const uint8_t req = buf[1];
const uint16_t req_val = buf[2] | (buf[3] << 8);
const uint16_t req_index = buf[4] | (buf[5] << 8);
const uint16_t req_count = buf[6] | (buf[7] << 8);
//printf("ep0 setup type %02x req %02x val %04x index %04x count %04x\r\n",
// req_type, req, req_val, req_index, req_count);
if (req_type == 0x80 && req == 0x06) // get descriptor
{
const void *p_desc = NULL;
uint16_t desc_len = 0;
if (req_val == 0x0100) // get device descriptor
{
p_desc = &g_usb_device_desc;
desc_len = sizeof(g_usb_device_desc);
}
else if (req_val == 0x0200) // get configuration descriptor
{
p_desc = &g_usb_config_desc;
desc_len = sizeof(g_usb_config_desc);
}
else if (req_val == 0x0300) // get string language list
{
p_desc = &g_usb_lang_list_desc;
desc_len = sizeof(g_usb_lang_list_desc);
}
else if (req_val == 0x0302) // get product string
{
p_desc = g_metal_usb_setup_pkt_buf;
desc_len = usb_stuff_desc_string(g_metal_usb_product_str);
}
else if (req_val == 0x0301) // get vendor string
{
p_desc = g_metal_usb_setup_pkt_buf;
desc_len = usb_stuff_desc_string(g_metal_usb_vendor_str);
}
////////////////////
if (p_desc)
{
int tx_len = desc_len < req_count ? desc_len : req_count;
usb_tx(0, p_desc, tx_len);
}
else
{
printf("TRAP!!! unknown descriptor request: 0x%04x\r\n", req_val);
while(1) { } // IT'S A TRAP
}
}
else if (req_type == 0x00 && req == 0x05) // set address
usb_set_addr((uint8_t)req_val);
else if (req_type == 0x00 && req == 0x09) // set configuration
{
// todo: call into mac-specific function to set up endpoints, etc.
usb_tx(0, NULL, 0);
}
else
{
printf("unknown setup rx: req_type = 0x%02x, req = 0x%02x\r\n",
req_type, req);
printf("trapping...\r\n");
while(1) { } // IT'S A TRAP
}
}
void usb_ep1_tx_complete()
{
//printf("txc\r\n");
stuff_tx_buf();
usb_tx(1, g_tx_buf, sizeof(g_tx_buf));
}
int
usb_ep0_tx(const void *buf, size_t len, size_t reqlen, ep_callback_t cb, void *cb_data)
{
return (usb_tx(&usbd_pipe_state[USBD_PIPE_EP0_TX], buf, len, reqlen, cb, cb_data));
}