/* Sends one packet of data through the specified USART peripheral. This function operates synchronously, so it only returns when the data has been actually sent.  */
void USART_Write(AT91S_USART *usart, unsigned short data, volatile unsigned int timeOut)
/* Sends one packet of data through the specified USART peripheral. This function operates synchronously, so it only returns when the data has been actually sent.  */
void USART_Write(AT91S_USART *usart, unsigned short data, volatile unsigned int timeOut)
{
    if (timeOut == 0) {
        while ((usart->US_CSR & AT91C_US_TXEMPTY) == 0);
    }
    else {
        while ((usart->US_CSR & AT91C_US_TXEMPTY) == 0) {
            if (timeOut == 0) {
                trace_LOG(trace_ERROR, "-E- USART_Write: Timed out.\n\r");
                return;
            }
            timeOut--;
        }
    }
    usart->US_THR = data;
}