diff --git a/drivers/SerialWireOutput.h b/drivers/SerialWireOutput.h index 154197cf378..ab3e38f3a19 100644 --- a/drivers/SerialWireOutput.h +++ b/drivers/SerialWireOutput.h @@ -33,12 +33,8 @@ class SerialWireOutput : public FileHandle { virtual ssize_t write(const void *buffer, size_t size) { - const unsigned char *buf = static_cast(buffer); + mbed_itm_send_block(ITM_PORT_SWO, buffer, size); - /* Send buffer one character at a time over the ITM SWO port */ - for (size_t i = 0; i < size; i++) { - mbed_itm_send(ITM_PORT_SWO, buf[i]); - } return size; } diff --git a/hal/itm_api.h b/hal/itm_api.h index cf824bfad45..38607789cee 100644 --- a/hal/itm_api.h +++ b/hal/itm_api.h @@ -22,6 +22,7 @@ #if defined(DEVICE_ITM) #include +#include #ifdef __cplusplus extern "C" { @@ -68,12 +69,26 @@ void mbed_itm_init(void); * @brief Send data over ITM stimulus port. * * @param[in] port The stimulus port to send data over. - * @param[in] data The data to send. + * @param[in] data The 32-bit data to send. + * + * The data is written as a single 32-bit write to the port. * * @return value of data sent. */ uint32_t mbed_itm_send(uint32_t port, uint32_t data); +/** + * @brief Send a block of data over ITM stimulus port. + * + * @param[in] port The stimulus port to send data over. + * @param[in] data The block of data to send. + * @param[in] len The number of bytes of data to send. + * + * The data is written using multiple appropriately-sized port accesses for + * efficient transfer. + */ +void mbed_itm_send_block(uint32_t port, const void *data, size_t len); + /**@}*/ #ifdef __cplusplus diff --git a/hal/mbed_itm_api.c b/hal/mbed_itm_api.c index 64c116a3f6c..17afe5e1562 100644 --- a/hal/mbed_itm_api.c +++ b/hal/mbed_itm_api.c @@ -21,6 +21,10 @@ #include +#ifndef ITM_STIM_FIFOREADY_Msk +#define ITM_STIM_FIFOREADY_Msk 1 +#endif + #define ITM_ENABLE_WRITE 0xC5ACCE55 #define SWO_NRZ 0x02 @@ -66,21 +70,64 @@ void mbed_itm_init(void) } } +static void itm_out8(uint32_t port, uint8_t data) +{ + /* Wait until port is available */ + while ((ITM->PORT[port].u32 & ITM_STIM_FIFOREADY_Msk) == 0) { + __NOP(); + } + + /* write data to port */ + ITM->PORT[port].u8 = data; +} + +static void itm_out32(uint32_t port, uint32_t data) +{ + /* Wait until port is available */ + while ((ITM->PORT[port].u32 & ITM_STIM_FIFOREADY_Msk) == 0) { + __NOP(); + } + + /* write data to port */ + ITM->PORT[port].u32 = data; +} + uint32_t mbed_itm_send(uint32_t port, uint32_t data) { /* Check if ITM and port is enabled */ if (((ITM->TCR & ITM_TCR_ITMENA_Msk) != 0UL) && /* ITM enabled */ - ((ITM->TER & (1UL << port)) != 0UL)) { /* ITM Port enabled */ - /* write data to port */ - ITM->PORT[port].u32 = data; - - /* Wait until data has been clocked out */ - while (ITM->PORT[port].u32 == 0UL) { - __NOP(); - } + ((ITM->TER & (1UL << port)) != 0UL)) { /* ITM Port enabled */ + itm_out32(port, data); } return data; } +void mbed_itm_send_block(uint32_t port, const void *data, size_t len) +{ + const char *ptr = data; + + /* Check if ITM and port is enabled */ + if (((ITM->TCR & ITM_TCR_ITMENA_Msk) != 0UL) && /* ITM enabled */ + ((ITM->TER & (1UL << port)) != 0UL)) { /* ITM Port enabled */ + /* Output single byte at a time until data is aligned */ + while ((((uintptr_t) ptr) & 3) && len != 0) { + itm_out8(port, *ptr++); + len--; + } + + /* Output bulk of data one word at a time */ + while (len >= 4) { + itm_out32(port, *(const uint32_t *) ptr); + ptr += 4; + len -= 4; + } + + /* Output any trailing bytes */ + while (len != 0) { + itm_out8(port, *ptr++); + len--; + } + } +} #endif // defined(DEVICE_ITM)