/** * @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; } } }