debug-swo.c

/* vi: set noai ts=2 sw=2 expandtab: */
#include <stdint.h>
#include <string.h>
#include <debug-uart.h>
#include <core/uwork.h>
#include <core/semihosting.h>
#include <core/pm.h>
#include <nrf_soc.h>
#include <nrf_nvic.h>
#include <app_util_platform.h>

#ifndef DBG_XMIT_BUFFER_LEN
#define DBG_XMIT_BUFFER_LEN           (1024)
#endif

#ifndef DBG_SWO_XMIT_INTERVAL
#define DBG_SWO_XMIT_INTERVAL         (UWORK_USEC(100))
#endif

static void dbg_do_xmit(struct uwork_t *work);
static void dbg_do_semi_xmit(void);

struct dbg_state_t
{
  uint8_t *volatile xmit_buffer_head;   /**< Read position  */
  uint8_t *volatile xmit_buffer_tail;   /**< Write position */
  uint8_t *volatile xmit_buffer_phead;  /**< Panic mode read position */
  struct uwork_t xmit_work;             /**< Transmit work */
  uint8_t write_overrun;                /**< Write overrun flag */
  uint8_t semihosting;                  /**< Semihosting enabled */
  uint8_t enabled;                      /**< Debug output enabled */
  uint8_t xmit_buffer[DBG_XMIT_BUFFER_LEN]; /**< Transmit buffer */
};

static struct dbg_state_t dbg_state;
#define XMIT_BUFFER_END &dbg_state.xmit_buffer[DBG_XMIT_BUFFER_LEN]


void dbg_setup(void)
{
  dbg_state.xmit_work = UWORK_INIT_TYPED(dbg_state.xmit_work, dbg_do_xmit);
  dbg_state.xmit_buffer_head = dbg_state.xmit_buffer;
  dbg_state.xmit_buffer_tail = dbg_state.xmit_buffer;

  if (dbg_connected())
  {
    /* ITM Channel 0 is used for UART output */
    ITM->TER |= (1UL << 0);
    dbg_state.enabled = 1;
  }
}

void dbg_force_enabled(void)
{
  ITM->TER |= (1UL << 0);
  dbg_state.enabled = 1;
}

static void dbg_do_xmit(struct uwork_t *work)
{
  uint8_t *head = dbg_state.xmit_buffer_head;
  uint8_t *tail = dbg_state.xmit_buffer_tail;
  unsigned interval = DBG_SWO_XMIT_INTERVAL;

  while (head != tail) {
    if (ITM->PORT[0].u32 == 0)
      break;
    ITM->PORT[0].u8 = (uint8_t) *head;
    if (++head == XMIT_BUFFER_END)
      head = dbg_state.xmit_buffer;
  }

  CRITICAL_REGION_ENTER();
  if (head != dbg_state.xmit_buffer_tail) {
    work->at += interval;
    uwork_schedule(work);
  }
  dbg_state.xmit_buffer_head = head;
  CRITICAL_REGION_EXIT();
}

static void dbg_do_semi_xmit(void)
{
  uint8_t *head = dbg_state.xmit_buffer_phead;
  uint8_t *tail = dbg_state.xmit_buffer_tail;

  while (head != tail) {
    int chunk;
    if (tail > head)
      chunk = tail - head;
    else
      chunk = XMIT_BUFFER_END - head;

    if (chunk > 1) {
      volatile uint8_t *p = &head[chunk - 1];
      uint8_t term = *p;
      *p = 0;
      call_semihost(SYS_WRITE0, head);
      *p = term;
    }
    call_semihost(SYS_WRITE0, &head[chunk - 1]);

    head += chunk;
    if (head == XMIT_BUFFER_END)
      head = dbg_state.xmit_buffer;
  }

  dbg_state.xmit_buffer_phead = head;
}

unsigned int dbg_send_bytes(const uint8_t *seq, unsigned int len)
{
  if (dbg_state.enabled) {

    /* Interrupts are disabled to protect against possible concurrent
       calls to dbg_send_bytes() from different runlevels */
    CRITICAL_REGION_ENTER();

    uint8_t *head = dbg_state.xmit_buffer_head;
    uint8_t *tail = dbg_state.xmit_buffer_tail;

    if (tail >= head) {
      /* Free space wraps */
      unsigned int xfer_len = XMIT_BUFFER_END - tail;
      unsigned int free = DBG_XMIT_BUFFER_LEN - (tail - head) - 1;

      if (len > free)
        len = free;
      if (xfer_len < len) {
        memcpy(tail, seq, xfer_len);
        seq += xfer_len;
        xfer_len = len - xfer_len;
        memcpy(dbg_state.xmit_buffer, seq, xfer_len);
        tail = dbg_state.xmit_buffer + xfer_len;
      } else {
        memcpy(tail, seq, len);
        tail += len;
        if (tail == XMIT_BUFFER_END)
          tail = dbg_state.xmit_buffer;
      }
    } else {
      /* Free space continuous */
      unsigned int free = (head - tail) - 1;
      if (len > free)
        len = free;
      memcpy(tail, seq, len);
      tail += len;
    }

    dbg_state.xmit_buffer_tail = tail;
    if (!uwork_pending(&dbg_state.xmit_work)) {
      dbg_state.xmit_work.at = uwork_now();
      uwork_schedule(&dbg_state.xmit_work);
    }
    CRITICAL_REGION_EXIT();
  }

  return len;
}

void dbg_putchar(const char ch)
{
  if (dbg_state.write_overrun) {
    if (dbg_send_bytes((const uint8_t*)"^", 1) != 1)
      return;
  }
  dbg_state.write_overrun = 0;
  if (dbg_send_bytes((const uint8_t*)&ch, 1) != 1) {
    dbg_state.write_overrun = 1;
  }
}

void dbg_blocking_putchar(const char ch)
{
  if (dbg_state.write_overrun) {
    while (dbg_send_bytes((const uint8_t*)"^", 1) != 1)
      __WFE();
  }
  dbg_state.write_overrun = 0;
  while (dbg_send_bytes((const uint8_t*)&ch, 1) != 1)
    __WFE();
  __SEV();
}

void dbg_drain()
{
  if (dbg_state.enabled) {
    CRITICAL_REGION_ENTER();
    while(dbg_state.xmit_buffer_tail != dbg_state.xmit_buffer_head)
      dbg_do_xmit(&dbg_state.xmit_work);
    if (dbg_state.semihosting)
      dbg_do_semi_xmit();
    CRITICAL_REGION_EXIT();
  }
}

void dbg_panic(void)
{
  dbg_state.xmit_buffer_phead = dbg_state.xmit_buffer_tail;
  dbg_state.semihosting = true;
}