/***********************************************************************
*                    SEGGER Microcontroller GmbH                       *
*                        The Embedded Experts                          *
************************************************************************
*                                                                      *
*                  (c) SEGGER Microcontroller GmbH                     *
*                        All rights reserved                           *
*                          www.segger.com                              *
*                                                                      *
************************************************************************
*                                                                      *
************************************************************************
*                                                                      *
*                                                                      *
*  Licensing terms                                                     *
*                                                                      *
* This software may be distributed to your customers free of charge.   *
* This grant of redistribution does not entitle YOU or enduser to      *
* receive from SEGGER hard-copy documentation, technical support,      *
* phone assistance, or enhancements or updates to the Software unless  *
* a specific agreement clearly states otherwise.                       *
*                                                                      *
*                                                                      *
* THIS SOFTWARE IS PROVIDED BY COPYRIGHT HOLDER "AS IS" AND ANY        *
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE    *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR   *
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE        *
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,     *
* OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,             *
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR   *
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY  *
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT         *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE    *
* USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH     *
* DAMAGE.                                                              *
*                                                                      *
************************************************************************

-------------------------- END-OF-HEADER -----------------------------

Purpose: Implementation of connect under reset (ST STM32U5)
Literature:
  [1]  J-Link User Guide
  [2]  ST RM0456 (STM32U5 Reference manual)
Additional information:
  [3]  ...
*/

/*********************************************************************
*
*       Constants, fixed
*
**********************************************************************
*/
//
// Registers
//
__constant U32 _DHCSR_ADDR                       = 0xE000EDF0;
__constant U32 _DEMCR_ADDR                       = 0xE000EDFC; // Debug Exception and Monitor Control Register
__constant U32 _AIRCR_ADDR                       = 0xE000ED0C; // Application Interrupt and Reset Control Register
__constant U32 _CPUID_ADDR                       = 0xE000ED00;
//
// Bits
//
__constant U32 _DP_CTRL_STAT_BIT_DBGPWRUPREQ     = (1 << 30);
__constant U32 _DP_CTRL_STAT_BIT_SYSPWRUPREQ     = (1 << 28);
__constant U32 _DP_CTRL_STAT_BIT_STICKYERR       = (1 <<  5);
//
// Shifts
//
__constant U32 _DHCSR_RDY_BIT_SHIFT              = 17;

/*********************************************************************
*
*       Constants, configurable (might differ between STM32 devices)
*
**********************************************************************
*/
//
// AP map
//
__constant U32 _DEBUG_AHB_AP    = 0;  // Cortex-M33
//
// Registers
//
__constant U32 _DBGMCU_BASE      = 0xE0044000;
__constant U32 _DBGMCU_IDCODE    = _DBGMCU_BASE + 0x00;
__constant U32 _DBGMCU_CR        = _DBGMCU_BASE + 0x04;
//
// Shifts & Bits & Masks
//
__constant U32 _BIT_DBG_STOP     = (1 <<  1);
__constant U32 _BIT_DBG_STANDBY  = (1 <<  2);
//
// Misc
//
__constant U32 _DEF_BUSY_TIMEOUT = 500;

/*********************************************************************
*
*       Static code
*
**********************************************************************
*/

/*********************************************************************
*
*       _WriteData
*
*  Function descripton
*    Writes a CPU register via AHBAP
*/
static int _WriteData(U32 Addr, U32 Data) {
  int r;
  r  = JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, Addr);
  r |= JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, Data);
  return r;
}

/*********************************************************************
*
*       _ReadData
*
*  Function descripton
*    Reads a CPU register via AHBAP
*/
static int _ReadData(U32 Addr) {
  U32 v;
  int r;
  r = JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, Addr);
  if (r == -1) {
    return -1;
  }
  v = JLINK_CORESIGHT_ReadAP(JLINK_CORESIGHT_AP_REG_DATA);
  return v;
}

/*********************************************************************
*
*       _IdentifyTarget
*
*  Function descripton
*    If JTAG: Identifies target in JTAG chain.
*    If SWD:  Execute JTAG->SWD switching sequence.
*/
static int _IdentifyTarget(void) {
  U32 ActTIF;
  U32 TAPId;
  U32 TAPIdMask;
  U32 IRLen;
  U32 IRPrint;
  U32 IRPrintMask;
  int r;

  ActTIF = JLINK_ActiveTIF;
  if ((ActTIF == JLINK_TIF_JTAG) || (ActTIF == JLINK_TIF_CJTAG)) {
    JLINK_SYS_Report("JTAG selected. Identifying JTAG Chain...");
    r = JLINK_JTAG_Identify();
    if (r < 0) {
      JLINK_SYS_Report("Error: Scanning JTAG chain failed.");
      return -1;
    }
    JLINK_SYS_Report("JTAG Chain Identified. Connecting to DAP TAP...");
    //
    // The TAP IDCODE is described by ARM as follows:
    //   [31:28] Version.
    //   [27:12] Part number (always 0xBxxx).
    //   [11:01] Manufacturer ID (always 0x23B).
    //   [1]     Reserved (always b'1).
    // Therefore, we check the highest bits of Partnumber and bits 11:0 to verify if it is indeed a TAP.
    //
    TAPId        = 0x0B000477;  // These bits are expected to be set in any TAP IDCODE. However, some devices have been known to not apply to the ARM specifications.
    TAPIdMask    = 0x0F000FFF;  // Only take the bits of TAPId into account.
    IRLen        = 4;           // IR register length of device to locate. This is 4 for most devices.
    IRPrint      = 0x01;        // Value read back from scan chain when writing to IR register. JTAG defines the lower 2 bits only to be b'01.
    IRPrintMask  = 0x0F;        // 4-bits are taken into account
    r = JLINK_JTAG_FindSelectTAP(TAPId, TAPIdMask, IRLen, IRPrint, IRPrintMask);  // Also considers manual JTAG TAP position pre-selection if user provided <IRPre>, <DRPre>, ...
    if (r < 0) {
      JLINK_SYS_Report("Error: Could not find DAP TAP to use.");
      return -1;
    }
    JLINK_SYS_Report("Successfully connected to selected DAP TAP.");
  } else {
    JLINK_SYS_Report("SWD selected. Executing JTAG -> SWD switching sequence.");
    r = JLINK_CORESIGHT_Configure(""); // Outputs JTAG -> SWD switching sequence and checks if DAPId can be read.
    if (r < 0) {
      JLINK_SYS_Report("Error: Could not initialize SWD protocol.");
    }
  }
  return r;
}

/*********************************************************************
*
*       _ConfigureAP()
*
*  Function description
*    Configures selected AP for debugging.
*/
static int _ConfigureAP(U32 Index) {
  U32 v;
  int r;
  //
  // Select AP and bank to use
  //
  v = 0
    | (0 << 4)       // Select bank 0 (this is default for most casese).
    | (Index << 24)  // Select AP[Index] (AHB-AP)
    ;
  JLINK_CORESIGHT_WriteDP(JLINK_CORESIGHT_DP_REG_SELECT,  v);
  //
  // Initialize DAP for debug use.
  //
  v = (2 <<  0)  // Access size: word
    | (0 <<  4)  // NO Auto-increment
    | (1 << 25)  // Priviledged access
    | (0 << 30)  // Secure transfer
    ;
  r = JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_CTRL, v);
  if (r < 0) {
    JLINK_SYS_Report("Error: Failed to configure AP.");
    return -1;
  }
  return 0;
}

/*********************************************************************
*
*       _InitDAP()
*
*  Function description
*    Initializes DAP, so JLINK_CORESIGHT_ functions can be used.
*/
static int _InitDAP(void) {
  U32 v;
  int r;
  int t;
  //
  // Clear sticky error flags and power up DAP
  //
  v  = _DP_CTRL_STAT_BIT_DBGPWRUPREQ | _DP_CTRL_STAT_BIT_SYSPWRUPREQ;
  if (JLINK_ActiveTIF == JLINK_TIF_JTAG) {
    v |= _DP_CTRL_STAT_BIT_STICKYERR;
  } else {
    JLINK_CORESIGHT_WriteDP(JLINK_CORESIGHT_DP_REG_ABORT, 0x1E);
  }
  JLINK_CORESIGHT_WriteDP(JLINK_CORESIGHT_DP_REG_CTRL_STAT, v);
  //
  // Wait for power up DAP complete
  //
  r = -1;
  t = JLINK_GetTime() + _DEF_BUSY_TIMEOUT;
  do {
    r = JLINK_CORESIGHT_ReadDP(JLINK_CORESIGHT_DP_REG_CTRL_STAT);
    if (r == -1) {                     // Any error occurred while reading from the DP, we are done
      break;
    }
    v = r;
    if (v & (0xF << 28)) {             // CSYSPWRUPACK and CDBGPWRUPACK set, so both ports have been completely powered?
      r = 0;
      break;
    }
    if ((t - JLINK_GetTime()) <= 0) {
      break;
    }
  } while(1);
  if (r < 0) {
    JLINK_SYS_Report("Error: Failed to initialize DAP.");
    return -1;
  }
  r = _ConfigureAP(_DEBUG_AHB_AP);     // Finally configure the AP so we can use it.
  if (r < 0) {
    return -1;
  }
  JLINK_SYS_Report("DAP initialized successfully.");
  return 0;
}

/*********************************************************************
*
*       Global functions
*
**********************************************************************
*/

/*********************************************************************
*
*       InitTarget()
*
*  Function description
*    If present, called right before performing generic connect sequence.
*    Usually used for targets which need a special connect sequence.
*    E.g.: TI devices with ICEPick TAP on them where core TAP needs to be enabled via specific ICEPick sequences first
*
*  Return value
*    >= 0:  O.K.
*     < 0:  Error
*
*  Notes
*    (1) Must not use high-level API functions like JLINK_MEM_ etc.
*    (2) For target interface JTAG, this device has to setup the JTAG chain + JTAG TAP Ids.
*/
int InitTarget(void) {
  int Result;
  int r;
  U32 RetryCnt;
  U32 vDBGCR;
  U32 vDEMCR;
  U32 v;
  U8  NeedDEMCRRestore;
  //
  // Initialize variables
  //
  RetryCnt         =  0;
  Result           = -1;
  NeedDEMCRRestore =  0;
  //
  // Check if we need to perform a connect under reset
  //
TryAgain:
  if (RetryCnt > 1) {   // Already tried a second time with connect under reset... we are done
    JLINK_SYS_Report("Connecting to CPU via connect under reset failed.");
    goto Done;
  }
  if (RetryCnt == 1) {
    JLINK_SYS_Report("Can not attach to CPU. Trying connect under reset.");
    JLINK_JTAG_ResetPin = 0;
    JLINK_SYS_Sleep(50);     // Give nRESET some time to get low
  }
  //
  // Perform basic interface identification
  //
  r = _IdentifyTarget();
  if (r < 0) {           // Initial connect / attach failed? Try again with connect under reset
    RetryCnt += 1;
    goto TryAgain;
  }
  r = _InitDAP();
  if (r < 0) {
    RetryCnt += 1;
    goto TryAgain;
  }
  //
  // Read the CPUID register. This will fail in almost all cases where low-power modes are active
  // since this power domain is powered-down in most modes.
  //
  v = _ReadData(_CPUID_ADDR);
  JLINK_SYS_Report1("CPUID: ", v);
  v &= 0xFF0F0000;
  if ((v != 0x410F0000) && (v != 0x410C0000)) {  // [31:24] describe implementer (ARM == 0x41), [19:16] describe architecture which is 0xF for ARMv7 and 0xC for Cortex-M
    RetryCnt += 1;
    goto TryAgain;
  }
  //
  // Configure the DBGMCU_CR register for low-power usage
  //
  v = _ReadData(_DBGMCU_CR);
  vDBGCR = _BIT_DBG_STOP | _BIT_DBG_STANDBY;      // Enable debug in Standby and Stop Mode.
  if ((v & vDBGCR) != vDBGCR) {
    _WriteData(_DBGMCU_CR, (v | vDBGCR));
  }
  //
  // Are we currently performing a connect under reset?
  // If yes, halt the CPU after reset
  //
  if (RetryCnt == 1) {
    v = _ReadData(_DHCSR_ADDR);
    if ((v & 1) == 0) {     // C_DEBUGEN not set?
      v &= 0x3F;
      v |= (0xA05F << 16);  // Debug key to make a write to the DHCSR a valid one
      v |= (1 << 0);        // Enable debug functionalities of the core
      _WriteData(_DHCSR_ADDR, v);
    }
    vDEMCR = _ReadData(_DEMCR_ADDR);
    if ((vDEMCR & 1) == 0) {               // VC_CORERESET not set?
      _WriteData(_DEMCR_ADDR, vDEMCR | 1); // Set VC_CORERESET
      NeedDEMCRRestore = 1;
    }
  }
  //
  // Release the target from reset
  //
  if (RetryCnt) {                // We tried to connect under reset? => Release it to check device protection.
    JLINK_CORESIGHT_ReadDP(JLINK_CORESIGHT_DP_REG_RDBUF);  // Force DAP to actually write data before we release the reset.
    JLINK_JTAG_ResetPin = 1;
    JLINK_SYS_Sleep(50);
  }
  if (r < 0) {
    RetryCnt += 1;               // Try again with connect under reset
    goto TryAgain;
  }
  Result = 0;
Done:
  //
  // Release the target from reset. Could be possible that we already released reset some steps before to
  // be able to read the option bytes. However, releasing it again does not hurt and may be necessary in case
  // of an error occurred.
  //
  if (RetryCnt) {
    JLINK_JTAG_ResetPin = 1;
    JLINK_SYS_Sleep(50);
    if (NeedDEMCRRestore) {
      _WriteData(_DEMCR_ADDR, vDEMCR);
    }
  }
  return Result;
}

/*************************** end of file ****************************/