/**
* @brief Returns the filled space into an input queue.
*
* @param[in] iqp pointer to an @p InputQueue structure
* @return The number of bytes in the queue.
* @retval 0 if the queue is empty.
*
* @iclass
*/
size_t chIQGetFullI(InputQueue *iqp) {
cnt_t cnt;
cnt = chQSpaceI(iqp);
if (cnt < 0)
return 0;
return (size_t)cnt;
}
/**
* @brief Returns the filled space into an output queue.
*
* @param[in] oqp pointer to an @p OutputQueue structure
* @return The number of bytes in the queue.
* @retval 0 if the queue is empty.
*
* @iclass
*/
size_t chOQGetFullI(OutputQueue *oqp) {
cnt_t cnt;
cnt = chQSpaceI(oqp);
if (cnt < 0)
return chQSizeI(oqp);
return chQSizeI(oqp) - (size_t)cnt;
}
static void writeOscQueue( InputQueue * q )
{
msg_t msg;
uint8_t noData;
size_t cnt, i;
chSysLock();
cnt = (chQSpaceI( q ) / 2) * 2;
chSysUnlock();
// And just limit send data size to ensure USB chip will not overfull.
cnt = ( cnt <= USB_FIFO_SZ ) ? cnt : USB_FIFO_SZ;
for ( i=0; i<cnt; i++ )
{
msg = chIQGetTimeout( q, TIME_IMMEDIATE );
noData = ( ( msg == Q_TIMEOUT ) || ( msg == Q_RESET ) ) ? 1 : 0;
uint8_t v;
v = ( noData ) ? 0 : (uint8_t)msg;
writeResult( v );
}
writeEom();
}
/**
* @brief Process messages received from generic serial interface driver.
* @return none.
*/
static void CommReadSerialData(void)
{
uint32_t crc_packet;
SYS_INTERRUPTS_DISABLE();
/* The following function must be called from within a system lock zone. */
size_t bytesAvailable = chQSpaceI(&g_iqp);
SYS_INTERRUPTS_ENABLE();
while (bytesAvailable)
{
if (bytesAvailable >= bytesRequired)
{
if (bytesRequired > 0)
{
if (chIQRead(&g_iqp, msgPos, bytesRequired) < 0)
{
printf("[%s, L%d] read queue failed.\r\n", __FILE__, __LINE__);
}
msgPos += bytesRequired;
bytesAvailable -= bytesRequired;
bytesRequired = 0;
}
}
else
{
if (chIQRead(&g_iqp, msgPos, bytesAvailable) < 0)
{
printf("[%s, L%d] read queue failed.\r\n", __FILE__, __LINE__);
}
msgPos += bytesAvailable;
bytesRequired -= bytesAvailable;
break;
}
size_t curReadLen = msgPos - (uint8_t *)&msg;
if (!IS_MSG_VALID())
{
CommReadSerialDataResync(curReadLen);
}
else if (curReadLen == COMM_MSG_HDR_SIZE)
{
bytesRequired = msg.size - COMM_MSG_HDR_SIZE;
}
else if (bytesRequired == 0)
{
/* Whole packet is read, check and process it... */
/* Move CRC */
memmove(&msg.crc, (uint8_t *)&msg + msg.size - COMM_MSG_CRC_SIZE,
COMM_MSG_CRC_SIZE);
/* Zero-out unused data for crc32 calculation. */
memset(&msg.data[msg.size - COMM_MSG_SVC_SIZE], 0,
COMM_BUFFER_SIZE - msg.size + COMM_MSG_SVC_SIZE);
if (msg.crc == (crc_packet = CommGetCRC32Checksum(&msg)))
{
CommProcessCommand(&msg);
}
else
{
CommPrintMsg(curReadLen);
printf("crc error:0x%08x 0x%08x\r\n", (unsigned int)msg.crc, (unsigned int)crc_packet);
}
/* Read another packet...*/
bytesRequired = COMM_MSG_HDR_SIZE;
msgPos = (uint8_t *)&msg;
}
}
}
