/*
 * Copyright (c) 2013-2021 ARM Limited. All rights reserved.
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the License); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * ----------------------------------------------------------------------
 *
 * $Date:        29. March 2021
 * $Revision:    V2.0.1
 *
 * Project:      CMSIS-DAP Source
 * Title:        SWO.c CMSIS-DAP SWO I/O
 *
 *---------------------------------------------------------------------------*/

#include "DAP_config.h"
#include "DAP.h"
#if (SWO_UART != 0)
#include "Driver_USART.h"
#endif
#if (SWO_STREAM != 0)
#include "cmsis_os2.h"
#define   osObjectsExternal
#include "osObjects.h"
#endif

#if (SWO_STREAM != 0)
#ifdef DAP_FW_V1
#error "SWO Streaming Trace not supported in DAP V1!"
#endif
#endif

#if (SWO_UART != 0)

// USART Driver
#define _USART_Driver_(n)  Driver_USART##n
#define  USART_Driver_(n) _USART_Driver_(n)
extern ARM_DRIVER_USART    USART_Driver_(SWO_UART_DRIVER);
#define pUSART           (&USART_Driver_(SWO_UART_DRIVER))

static uint8_t USART_Ready = 0U;

#endif  /* (SWO_UART != 0) */


#if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))


#define SWO_STREAM_TIMEOUT      50U     /* Stream timeout in ms */

#define USB_BLOCK_SIZE          512U    /* USB Block Size */
#define TRACE_BLOCK_SIZE        64U     /* Trace Block Size (2^n: 32...512) */

// Trace State
static uint8_t  TraceTransport =  0U;       /* Trace Transport */
static uint8_t  TraceMode      =  0U;       /* Trace Mode */
static uint8_t  TraceStatus    =  0U;       /* Trace Status without Errors */
static uint8_t  TraceError[2]  = {0U, 0U};  /* Trace Error flags (banked) */
static uint8_t  TraceError_n   =  0U;       /* Active Trace Error bank */

// Trace Buffer
static uint8_t  TraceBuf[SWO_BUFFER_SIZE];  /* Trace Buffer (must be 2^n) */
static volatile uint32_t TraceIndexI  = 0U; /* Incoming Trace Index */
static volatile uint32_t TraceIndexO  = 0U; /* Outgoing Trace Index */
static volatile uint8_t  TraceUpdate;       /* Trace Update Flag */
static          uint32_t TraceBlockSize;    /* Current Trace Block Size */

#if (TIMESTAMP_CLOCK != 0U) 
// Trace Timestamp
static volatile struct {
  uint32_t index;
  uint32_t tick;
} TraceTimestamp;
#endif

// Trace Helper functions
static void     ClearTrace     (void);
static void     ResumeTrace    (void);
static uint32_t GetTraceCount  (void);
static uint8_t  GetTraceStatus (void);
static void     SetTraceError  (uint8_t flag);

#if (SWO_STREAM != 0)
extern osThreadId_t      SWO_ThreadId;
static volatile uint8_t  TransferBusy = 0U; /* Transfer Busy Flag */
static          uint32_t TransferSize;      /* Current Transfer Size */
#endif


#if (SWO_UART != 0)

// USART Driver Callback function
//   event: event mask
static void USART_Callback (uint32_t event) {
  uint32_t index_i;
  uint32_t index_o;
  uint32_t count;
  uint32_t num;

  if (event &  ARM_USART_EVENT_RECEIVE_COMPLETE) {
#if (TIMESTAMP_CLOCK != 0U) 
    TraceTimestamp.tick = TIMESTAMP_GET();
#endif
    index_o  = TraceIndexO;
    index_i  = TraceIndexI;
    index_i += TraceBlockSize;
    TraceIndexI = index_i;
#if (TIMESTAMP_CLOCK != 0U) 
    TraceTimestamp.index = index_i;
#endif
    num   = TRACE_BLOCK_SIZE - (index_i & (TRACE_BLOCK_SIZE - 1U));
    count = index_i - index_o;
    if (count <= (SWO_BUFFER_SIZE - num)) {
      index_i &= SWO_BUFFER_SIZE - 1U;
      TraceBlockSize = num;
      pUSART->Receive(&TraceBuf[index_i], num);
    } else {
      TraceStatus = DAP_SWO_CAPTURE_ACTIVE | DAP_SWO_CAPTURE_PAUSED;
    }
    TraceUpdate = 1U;
#if (SWO_STREAM != 0)
    if (TraceTransport == 2U) {
      if (count >= (USB_BLOCK_SIZE - (index_o & (USB_BLOCK_SIZE - 1U)))) {
        osThreadFlagsSet(SWO_ThreadId, 1U);
      }
    }
#endif
  }
  if (event &  ARM_USART_EVENT_RX_OVERFLOW) {
    SetTraceError(DAP_SWO_BUFFER_OVERRUN);
  }
  if (event & (ARM_USART_EVENT_RX_BREAK         |
               ARM_USART_EVENT_RX_FRAMING_ERROR |
               ARM_USART_EVENT_RX_PARITY_ERROR)) {
    SetTraceError(DAP_SWO_STREAM_ERROR);
  }
}

// Enable or disable SWO Mode (UART)
//   enable: enable flag
//   return: 1 - Success, 0 - Error
__WEAK uint32_t SWO_Mode_UART (uint32_t enable) {
  int32_t status;

  USART_Ready = 0U;

  if (enable != 0U) {
    status = pUSART->Initialize(USART_Callback);
    if (status != ARM_DRIVER_OK) {
      return (0U);
    }
    status = pUSART->PowerControl(ARM_POWER_FULL);
    if (status != ARM_DRIVER_OK) {
      pUSART->Uninitialize();
      return (0U);
    }
  } else {
    pUSART->Control(ARM_USART_CONTROL_RX, 0U);
    pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
    pUSART->PowerControl(ARM_POWER_OFF);
    pUSART->Uninitialize();
  }
  return (1U);
}

// Configure SWO Baudrate (UART)
//   baudrate: requested baudrate
//   return:   actual baudrate or 0 when not configured
__WEAK uint32_t SWO_Baudrate_UART (uint32_t baudrate) {
  int32_t  status;
  uint32_t index;
  uint32_t num;

  if (baudrate > SWO_UART_MAX_BAUDRATE) {
    baudrate = SWO_UART_MAX_BAUDRATE;
  }

  if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
    pUSART->Control(ARM_USART_CONTROL_RX, 0U);
    if (pUSART->GetStatus().rx_busy) {
      TraceIndexI += pUSART->GetRxCount();
      pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
    }
  }

  status = pUSART->Control(ARM_USART_MODE_ASYNCHRONOUS |
                           ARM_USART_DATA_BITS_8       |
                           ARM_USART_PARITY_NONE       |
                           ARM_USART_STOP_BITS_1,
                           baudrate);

  if (status == ARM_DRIVER_OK) {
    USART_Ready = 1U;
  } else {
    USART_Ready = 0U;
    return (0U);
  }

  if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
    if ((TraceStatus & DAP_SWO_CAPTURE_PAUSED) == 0U) {
      index = TraceIndexI & (SWO_BUFFER_SIZE - 1U);
      num = TRACE_BLOCK_SIZE - (index & (TRACE_BLOCK_SIZE - 1U));
      TraceBlockSize = num;
      pUSART->Receive(&TraceBuf[index], num);
    }
    pUSART->Control(ARM_USART_CONTROL_RX, 1U);
  }

  return (baudrate);
}

// Control SWO Capture (UART)
//   active: active flag
//   return: 1 - Success, 0 - Error
__WEAK uint32_t SWO_Control_UART (uint32_t active) {
  int32_t status;

  if (active) {
    if (!USART_Ready) { 
      return (0U);
    }
    TraceBlockSize = 1U;
    status = pUSART->Receive(&TraceBuf[0], 1U);
    if (status != ARM_DRIVER_OK) {
      return (0U);
    }
    status = pUSART->Control(ARM_USART_CONTROL_RX, 1U);
    if (status != ARM_DRIVER_OK) {
      return (0U);
    }
  } else {
    pUSART->Control(ARM_USART_CONTROL_RX, 0U);
    if (pUSART->GetStatus().rx_busy) {
      TraceIndexI += pUSART->GetRxCount();
      pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
    }
  }
  return (1U);
}

// Start SWO Capture (UART)
//   buf: pointer to buffer for capturing
//   num: number of bytes to capture
__WEAK void SWO_Capture_UART (uint8_t *buf, uint32_t num) {
  TraceBlockSize = num;
  pUSART->Receive(buf, num);
}

// Get SWO Pending Trace Count (UART)
//   return: number of pending trace data bytes
__WEAK uint32_t SWO_GetCount_UART (void) {
  uint32_t count;

  if (pUSART->GetStatus().rx_busy) {
    count = pUSART->GetRxCount();
  } else {
    count = 0U;
  }
  return (count);
}

#endif  /* (SWO_UART != 0) */


#if (SWO_MANCHESTER != 0)

// Enable or disable SWO Mode (Manchester)
//   enable: enable flag
//   return: 1 - Success, 0 - Error
__WEAK uint32_t SWO_Mode_Manchester (uint32_t enable) {
  (void) enable;
  return (0U);
}

// Configure SWO Baudrate (Manchester)
//   baudrate: requested baudrate
//   return:   actual baudrate or 0 when not configured
__WEAK uint32_t SWO_Baudrate_Manchester (uint32_t baudrate) {
  (void) baudrate;
  return (0U);
}

// Control SWO Capture (Manchester)
//   active: active flag
//   return: 1 - Success, 0 - Error
__WEAK uint32_t SWO_Control_Manchester (uint32_t active) {
  (void) active;
  return (0U);
}

// Start SWO Capture (Manchester)
//   buf: pointer to buffer for capturing
//   num: number of bytes to capture
__WEAK void SWO_Capture_Manchester (uint8_t *buf, uint32_t num) {
  (void) buf;
  (void) num;
}

// Get SWO Pending Trace Count (Manchester)
//   return: number of pending trace data bytes
__WEAK uint32_t SWO_GetCount_Manchester (void) {
  return (0U);
}

#endif  /* (SWO_MANCHESTER != 0) */


// Clear Trace Errors and Data
static void ClearTrace (void) {

#if (SWO_STREAM != 0)
  if (TraceTransport == 2U) {
    if (TransferBusy != 0U) {
      SWO_AbortTransfer();
      TransferBusy = 0U;
    }
  }
#endif

  TraceError[0] = 0U;
  TraceError[1] = 0U;
  TraceError_n  = 0U;
  TraceIndexI   = 0U;
  TraceIndexO   = 0U;

#if (TIMESTAMP_CLOCK != 0U) 
  TraceTimestamp.index = 0U;
  TraceTimestamp.tick  = 0U;
#endif
}

// Resume Trace Capture
static void ResumeTrace (void) {
  uint32_t index_i;
  uint32_t index_o;

  if (TraceStatus == (DAP_SWO_CAPTURE_ACTIVE | DAP_SWO_CAPTURE_PAUSED)) {
    index_i = TraceIndexI;
    index_o = TraceIndexO;
    if ((index_i - index_o) < SWO_BUFFER_SIZE) {
      index_i &= SWO_BUFFER_SIZE - 1U;
      switch (TraceMode) {
#if (SWO_UART != 0)
        case DAP_SWO_UART:
          TraceStatus = DAP_SWO_CAPTURE_ACTIVE;
          SWO_Capture_UART(&TraceBuf[index_i], 1U);
          break;
#endif
#if (SWO_MANCHESTER != 0)
        case DAP_SWO_MANCHESTER:
          TraceStatus = DAP_SWO_CAPTURE_ACTIVE;
          SWO_Capture_Manchester(&TraceBuf[index_i], 1U);
          break;
#endif
        default:
          break;
      }
    }
  }
}

// Get Trace Count
//   return: number of available data bytes in trace buffer
static uint32_t GetTraceCount (void) {
  uint32_t count;

  if (TraceStatus == DAP_SWO_CAPTURE_ACTIVE) {
    do {
      TraceUpdate = 0U;
      count = TraceIndexI - TraceIndexO;
      switch (TraceMode) {
#if (SWO_UART != 0)
        case DAP_SWO_UART:
          count += SWO_GetCount_UART();
          break;
#endif
#if (SWO_MANCHESTER != 0)
        case DAP_SWO_MANCHESTER:
          count += SWO_GetCount_Manchester();
          break;
#endif
        default:
          break;
      }
    } while (TraceUpdate != 0U);
  } else {
    count = TraceIndexI - TraceIndexO;
  }

  return (count);
}

// Get Trace Status (clear Error flags)
//   return: Trace Status (Active flag and Error flags)
static uint8_t GetTraceStatus (void) {
  uint8_t  status;
  uint32_t n;

  n = TraceError_n;
  TraceError_n ^= 1U;
  status = TraceStatus | TraceError[n];
  TraceError[n] = 0U;

  return (status);
}

// Set Trace Error flag(s)
//   flag:  error flag(s) to set
static void SetTraceError (uint8_t flag) {
  TraceError[TraceError_n] |= flag;
}


// Process SWO Transport command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
uint32_t SWO_Transport (const uint8_t *request, uint8_t *response) {
  uint8_t  transport;
  uint32_t result;

  if ((TraceStatus & DAP_SWO_CAPTURE_ACTIVE) == 0U) {
    transport = *request;
    switch (transport) {
      case 0U:
      case 1U:
#if (SWO_STREAM != 0)
      case 2U:
#endif
        TraceTransport = transport;
        result = 1U;
        break;
      default:
        result = 0U;
        break;
    }
  } else {
    result = 0U;
  }

  if (result != 0U) {
    *response = DAP_OK;
  } else {
    *response = DAP_ERROR;
  }

  return ((1U << 16) | 1U);
}


// Process SWO Mode command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
uint32_t SWO_Mode (const uint8_t *request, uint8_t *response) {
  uint8_t  mode;
  uint32_t result;

  mode = *request;

  switch (TraceMode) {
#if (SWO_UART != 0)
    case DAP_SWO_UART:
      SWO_Mode_UART(0U);
      break;
#endif
#if (SWO_MANCHESTER != 0)
    case DAP_SWO_MANCHESTER:
      SWO_Mode_Manchester(0U);
      break;
#endif
    default:
      break;
  }

  switch (mode) {
    case DAP_SWO_OFF:
      result = 1U;
      break;
#if (SWO_UART != 0)
    case DAP_SWO_UART:
      result = SWO_Mode_UART(1U);
      break;
#endif
#if (SWO_MANCHESTER != 0)
    case DAP_SWO_MANCHESTER:
      result = SWO_Mode_Manchester(1U);
      break;
#endif
    default:
      result = 0U;
      break;
  }
  if (result != 0U) {
    TraceMode = mode;
  } else {
    TraceMode = DAP_SWO_OFF;
  }

  TraceStatus = 0U;

  if (result != 0U) {
    *response = DAP_OK;
  } else {
    *response = DAP_ERROR;
  }

  return ((1U << 16) | 1U);
}


// Process SWO Baudrate command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
uint32_t SWO_Baudrate (const uint8_t *request, uint8_t *response) {
  uint32_t baudrate;

  baudrate = (uint32_t)(*(request+0) <<  0) |
             (uint32_t)(*(request+1) <<  8) |
             (uint32_t)(*(request+2) << 16) |
             (uint32_t)(*(request+3) << 24);

  switch (TraceMode) {
#if (SWO_UART != 0)
    case DAP_SWO_UART:
      baudrate = SWO_Baudrate_UART(baudrate);
      break;
#endif
#if (SWO_MANCHESTER != 0)
    case DAP_SWO_MANCHESTER:
      baudrate = SWO_Baudrate_Manchester(baudrate);
      break;
#endif
    default:
      baudrate = 0U;
      break;
  }

  if (baudrate == 0U) {
    TraceStatus = 0U;
  }

  *response++ = (uint8_t)(baudrate >>  0);
  *response++ = (uint8_t)(baudrate >>  8);
  *response++ = (uint8_t)(baudrate >> 16);
  *response   = (uint8_t)(baudrate >> 24);

  return ((4U << 16) | 4U);
}


// Process SWO Control command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
uint32_t SWO_Control (const uint8_t *request, uint8_t *response) {
  uint8_t  active;
  uint32_t result;

  active = *request & DAP_SWO_CAPTURE_ACTIVE;

  if (active != (TraceStatus & DAP_SWO_CAPTURE_ACTIVE)) {
    if (active) {
      ClearTrace();
    }
    switch (TraceMode) {
#if (SWO_UART != 0)
      case DAP_SWO_UART:
        result = SWO_Control_UART(active);
        break;
#endif
#if (SWO_MANCHESTER != 0)
      case DAP_SWO_MANCHESTER:
        result = SWO_Control_Manchester(active);
        break;
#endif
      default:
        result = 0U;
        break;
    }
    if (result != 0U) {
      TraceStatus = active;
#if (SWO_STREAM != 0)
      if (TraceTransport == 2U) {
        osThreadFlagsSet(SWO_ThreadId, 1U);
      }
#endif
    }
  } else {
    result = 1U;
  }

  if (result != 0U) {
    *response = DAP_OK;
  } else {
    *response = DAP_ERROR;
  }

  return ((1U << 16) | 1U);
}


// Process SWO Status command and prepare response
//   response: pointer to response data
//   return:   number of bytes in response
uint32_t SWO_Status (uint8_t *response) {
  uint8_t  status;
  uint32_t count;

  status = GetTraceStatus();
  count  = GetTraceCount();

  *response++ = status;
  *response++ = (uint8_t)(count >>  0);
  *response++ = (uint8_t)(count >>  8);
  *response++ = (uint8_t)(count >> 16);
  *response   = (uint8_t)(count >> 24);

  return (5U);
}


// Process SWO Extended Status command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
uint32_t SWO_ExtendedStatus (const uint8_t *request, uint8_t *response) {
  uint8_t  cmd;
  uint8_t  status;
  uint32_t count;
#if (TIMESTAMP_CLOCK != 0U) 
  uint32_t index;
  uint32_t tick;
#endif
  uint32_t num;

  num = 0U;
  cmd = *request;

  if (cmd & 0x01U) {
    status = GetTraceStatus();
    *response++ = status;
    num += 1U;
  }

  if (cmd & 0x02U) {
    count = GetTraceCount();
    *response++ = (uint8_t)(count >>  0);
    *response++ = (uint8_t)(count >>  8);
    *response++ = (uint8_t)(count >> 16);
    *response++ = (uint8_t)(count >> 24);
    num += 4U;
  }

#if (TIMESTAMP_CLOCK != 0U) 
  if (cmd & 0x04U) {
    do {
      TraceUpdate = 0U;
      index = TraceTimestamp.index;
      tick  = TraceTimestamp.tick;
    } while (TraceUpdate != 0U);
    *response++ = (uint8_t)(index >>  0);
    *response++ = (uint8_t)(index >>  8);
    *response++ = (uint8_t)(index >> 16);
    *response++ = (uint8_t)(index >> 24);
    *response++ = (uint8_t)(tick  >>  0);
    *response++ = (uint8_t)(tick  >>  8);
    *response++ = (uint8_t)(tick  >> 16);
    *response++ = (uint8_t)(tick  >> 24);
    num += 4U;
  }
#endif

  return ((1U << 16) | num);
}


// Process SWO Data command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
uint32_t SWO_Data (const uint8_t *request, uint8_t *response) {
  uint8_t  status;
  uint32_t count;
  uint32_t index;
  uint32_t n, i;

  status = GetTraceStatus();
  count  = GetTraceCount();

  if (TraceTransport == 1U) {
    n = (uint32_t)(*(request+0) << 0) |
        (uint32_t)(*(request+1) << 8);
    if (n > (DAP_PACKET_SIZE - 4U)) {
      n = DAP_PACKET_SIZE - 4U;
    }
    if (count > n) {
      count = n;
    }
  } else {
    count = 0U;
  }

  *response++ = status;
  *response++ = (uint8_t)(count >> 0);
  *response++ = (uint8_t)(count >> 8);

  if (TraceTransport == 1U) {
    index = TraceIndexO;
    for (i = index, n = count; n; n--) {
      i &= SWO_BUFFER_SIZE - 1U;
      *response++ = TraceBuf[i++];
    }
    TraceIndexO = index + count;
    ResumeTrace();
  }

  return ((2U << 16) | (3U + count));
}


#if (SWO_STREAM != 0)

// SWO Data Transfer complete callback
void SWO_TransferComplete (void) {
  TraceIndexO += TransferSize;
  TransferBusy = 0U;
  ResumeTrace();
  osThreadFlagsSet(SWO_ThreadId, 1U);
}

// SWO Thread
__NO_RETURN void SWO_Thread (void *argument) {
  uint32_t timeout;
  uint32_t flags;
  uint32_t count;
  uint32_t index;
  uint32_t i, n;
  (void)   argument;

  timeout = osWaitForever;

  for (;;) {
    flags = osThreadFlagsWait(1U, osFlagsWaitAny, timeout);
    if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
      timeout = SWO_STREAM_TIMEOUT;
    } else {
      timeout = osWaitForever;
      flags   = osFlagsErrorTimeout;
    }
    if (TransferBusy == 0U) {
      count = GetTraceCount();
      if (count != 0U) {
        index = TraceIndexO & (SWO_BUFFER_SIZE - 1U);
        n = SWO_BUFFER_SIZE - index;
        if (count > n) {
          count = n;
        }
        if (flags != osFlagsErrorTimeout) {
          i = index & (USB_BLOCK_SIZE - 1U);
          if (i == 0U) {
            count &= ~(USB_BLOCK_SIZE - 1U);
          } else {
            n = USB_BLOCK_SIZE - i;
            if (count >= n) {
              count = n;
            } else {
              count = 0U;
            }
          }
        }
        if (count != 0U) {
          TransferSize = count;
          TransferBusy = 1U;
          SWO_QueueTransfer(&TraceBuf[index], count);
        }
      }
    }
  }
}

#endif  /* (SWO_STREAM != 0) */


#endif  /* ((SWO_UART != 0) || (SWO_MANCHESTER != 0)) */