/**
* @brief Writes to a dedicated packet buffer.
*
* @param[in] udp pointer to a @p stm32_usb_descriptor_t
* @param[in] buf buffer where to fetch the packet data
* @param[in] n maximum number of bytes to copy. This value must
* not exceed the maximum packet size for this endpoint.
*
* @notapi
*/
static void usb_packet_write_from_queue(stm32_usb_descriptor_t *udp,
OutputQueue *oqp, size_t n) {
size_t nhw;
uint32_t *pmap = USB_ADDR2PTR(udp->TXADDR0);
udp->TXCOUNT0 = (uint16_t)n;
nhw = n / 2;
while (nhw > 0) {
uint32_t w;
w = (uint32_t)*oqp->q_rdptr++;
if (oqp->q_rdptr >= oqp->q_top)
oqp->q_rdptr = oqp->q_buffer;
w |= (uint32_t)*oqp->q_rdptr++ << 8;
if (oqp->q_rdptr >= oqp->q_top)
oqp->q_rdptr = oqp->q_buffer;
*pmap++ = w;
nhw--;
}
/* Last byte for odd numbers.*/
if ((n & 1) != 0) {
*pmap = (uint32_t)*oqp->q_rdptr++;
if (oqp->q_rdptr >= oqp->q_top)
oqp->q_rdptr = oqp->q_buffer;
}
/* Updating queue. Note, the lock is done in this unusual way because this
function can be called from both ISR and thread context so the kind
of lock function to be invoked cannot be decided beforehand.*/
port_lock();
dbg_enter_lock();
oqp->q_counter += n;
while (notempty(&oqp->q_waiting))
chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK;
dbg_leave_lock();
port_unlock();
}
/**
* @brief Guard code for @p chSysUnlockFromIsr().
*
* @notapi
*/
void dbg_check_unlock_from_isr(void) {
if ((dbg_isr_cnt <= 0) || (dbg_lock_cnt <= 0))
chDbgPanic("SV#7");
dbg_leave_lock();
}