/***********************************************************************
*                    SEGGER Microcontroller GmbH                       *
*                        The Embedded Experts                          *
************************************************************************
*                                                                      *
*                  (c) SEGGER Microcontroller GmbH                     *
*                        All rights reserved                           *
*                          www.segger.com                              *
*                                                                      *
************************************************************************
*                                                                      *
************************************************************************
*                                                                      *
*                                                                      *
*  Licensing terms                                                     *
*                                                                      *
* The use in source and binary forms, with or without modification,    *
* is permitted for internal use only. The redistribution to any        *
* third party is prohibited.                                           *
*                                                                      *
*                                                                      *
* 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: Example for halting a device as soon as possible after reset.

Additional information:
  This sample does only work for devices which do no allow connecting
  while being held in reset (e.g. many ST devices).
  In such cases a short delay has to be added after releasing the reset
  pin, as the the halt request is sent while still being held in reset.
*/

/*********************************************************************
*
*       Constants
*
**********************************************************************
*/
//
// DataIn
//
const U8 _aDataIn_SeqReadId[] = {
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0x9E, 0xE7,                                            // Switching sequence STM32
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xB6, 0xED,                                            // Switching sequence Luminary (deprecated)
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0x00, 0x00,                                            // Make sure SWD is ready for a start bit
  0xA5,                                                  // Read ID (DP:00)
  0x00, 0x00, 0x00, 0x00, 0x00                           // Input
};

//
// Direction
//
const U8 _aDataDir_SeqReadId[] = {
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xFF, 0xFF,
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xFF, 0xFF,
  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
  0xFF, 0xFF,                                             // Make sure SWD is ready for a start bit
  0xFF,                                                   // Read ID (DP:00)
  0x00, 0x00, 0x00, 0x00, 0xF0                            // Input
};

const U8 _aDataOut[81];

/*********************************************************************
*
*       Local functions
*
**********************************************************************
*/

/*********************************************************************
*
*       _GetData32
*/
static U32 _GetData32(U8* p, int BitPos) {
  asm("LDR     R1, [SP +4]");
  asm("SHIFTL  R1, 3");
  asm("LDR     R0, [SP +8]");
  asm("ADD     R1, R0");
  asm("LDRWBIT R0, [R1]");
  asm("RET     2");
}

/*********************************************************************
*
*       Global functions
*
**********************************************************************
*/

/*********************************************************************
*
*       InitTarget()
*
*  Function description
*    If present, replaces generic device detection sequence.
*    In this example the function first resets the device using the reset
*    pin. As soon as the pin is released we try to read the ID of the device.
*    Should a valid ID be available we assume the target has finished the reset
*    and may be halted. This way the target may be halted as soon as possible.
*
*  Return value
*    >= 0:  O.K.
*     < 0:  Error
*
*  Notes
*    (1) MUST NOT use any MEM_ functions
*    (2) For target interface JTAG, this device has to setup the JTAG chain + JTAG TAP Ids.
*    (3) In any case, it has to set the "CPU" variable, so the DLL knows which CPU module to use internally.
*/
int InitTarget(void) {
  int r;
  U32 v;
  U32 AHBCfg32BitNoAutoInc;
  int t;
  U8 iRegDPCtlStat;
  U8 iRegSELECT;
  U8 iRegDPAbort;
  U8 iRegAHBCtlStat;
  U8 iRegAHBAddr;
  U8 iRegAHBData;
  //
  // Fallback method to connect to devices which are in a somewhat problematic mode:
  //   - Toggle nRESET
  //   - Loop0: Outputs the SWD switching sequence + read DP-Id until the correct DP Id could be read
  //   - Loop1: Output sequence to halt CPU (Setup AHB-AP access, write DHCSR)
  //
  //
  // Toggle reset pin
  //
  JLINK_TIF_ActivateTargetReset();
  JLINK_Delay_us(50000);                 // _Delay_us: Keep reset active for some time so possible glitch filters on target do not filter out reset pulse
  JLINK_TIF_ReleaseTargetReset();
  //
  // Output read ID sequence
  //
  t = JLINK_GetTime() + 200;
  do {
    r = JLINK_SWD_ReadWriteBits(&_aDataIn_SeqReadId[0], &_aDataDir_SeqReadId[0], &_aDataOut[0], 264);
    v = _GetData32(&_aDataOut[0], 227);
    if ((t - JLINK_GetTime()) < 0) {
      return 1;                       // Timeout
    }
  } while ((v & 0x0F000FFF) != 0x0B000477);
  //
  // Output halt sequence
  //
  iRegDPCtlStat  = 1;
  iRegSELECT     = 2;
  iRegDPAbort    = 0;
  iRegAHBCtlStat = 0;
  iRegAHBAddr    = 1;
  iRegAHBData    = 3;
  AHBCfg32BitNoAutoInc = (2 << 0) | (0 << 4) | (1 << 24) | (1 << 25) | (1 << 29);
  r  = JLINK_CORESIGHT_WriteDAP(iRegDPAbort, 0, 0x1E);                           // Make sure that no sticky error flags are set
  r |= JLINK_CORESIGHT_WriteDAP(iRegDPCtlStat, 0, (1 << 30) | (1 << 28));        // Set DBGPWRUPREQ and SYSPWRUPREQ to power up the complete DAP. Usually, no need to wait for the DAP to power up.
  r |= JLINK_CORESIGHT_WriteDAP(iRegSELECT, 0, (0 << 24) | (0 << 4));            // Select AP[0] (AHB-AP), bank 0
  r |= JLINK_CORESIGHT_WriteDAP(iRegAHBCtlStat, 1, AHBCfg32BitNoAutoInc);
  r |= JLINK_CORESIGHT_WriteDAP(iRegAHBAddr, 1, 0xE000EDF0);                     // DHCSR: Issue halt request
  r |= JLINK_CORESIGHT_WriteDAP(iRegAHBData, 1, 0xA05F0003);
  r |= JLINK_CORESIGHT_ReadDAP(iRegAHBData, 1, &v);                              // Dummy read to make sure write access makes it through AP-buffer
  if (r < 0) {
    JLINK_CORESIGHT_WriteDAP(iRegDPAbort, 0, 0x1E);                              // Make sure that no sticky error flags are set
    return 1;
  }

  return 0;
}
