/***********************************************************************
*                    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: Template script file showing how to switch,
         write and read AHB-APs.
Literature:
  [1]  Reference manual: https://wiki.segger.com/J-Link_script_files
*/

/*********************************************************************
*
*       Constants (similar to defines)
*
**********************************************************************
*/
__constant U32 _DAP_ACC_32BIT_AUTO_INC        = (1 << 29) | (1 << 25) | (1 << 24) | (1 << 4) | (2 << 0);  // HMASTER = DEBUG, Private access, Auto-increment, Access size: word;
__constant U32 _DAP_ACC_8BIT_NO_AUTO_INC      = (1 << 29) | (1 << 25) | (1 << 24) | (0 << 4) | (0 << 0);  // HMASTER = DEBUG, Private access, no Auto-increment, Access size: byte;
__constant U32 _DAP_ACC_16BIT_NO_AUTO_INC     = (1 << 29) | (1 << 25) | (1 << 24) | (0 << 4) | (1 << 0);  // HMASTER = DEBUG, Private access, no Auto-increment, Access size: half word;
__constant U32 _DAP_ACC_32BIT_NO_AUTO_INC     = (1 << 29) | (1 << 25) | (1 << 24) | (0 << 4) | (2 << 0);  // HMASTER = DEBUG, Private access, no Auto-increment, Access size: word;
__constant U32 _DAP_MASS_ERASE                = (0x020C);   
__constant U32 _DAP_FACTORY_RESET             = (0x020A);
__constant U32 _DAP_DATA_EXCHANGE              = (0x00EE);
__constant U32 _DAP_PWR_AP_DPREC_LPM	        = (0x9 << 16) | (1 << 20) | (1 << 3);
__constant U32 _PASSWORD_1                    = (0x12345678); //0xCAFECAFE
__constant U32 _PASSWORD_2                    = (0x12345678);
__constant U32 _PASSWORD_3                    = (0x12345678);
__constant U32 _PASSWORD_4                    = (0x12345678);
__constant U32 _FACTORY_RESET_PROCESSED       = (0x0000000A);
int phase                                     = (0);
/*********************************************************************
*
*       Static code
*
**********************************************************************
*/

/*********************************************************************
*
*       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.
*    (3) This function is called before J-Link performed any communication with the device
*/

int InitTarget(void) {
  U32 AccessPoint;
  U32 Bank;
  U32 Data;
  U32 DestAddr;
  U32 secap_rxctl_read;
  U32 secap_txctl_read;
  U32 secap_rxdata_read;
  U32 cmd;
  U32 counter;
  U32 secap_ctl_mask;
  U32 secap_txctl_transmit_full_mask;
  U32 accessPoint_response;
  U32 accessPoint_cmd;
  U32 secap_cmdMask;
  U32 dssm_cmd_received;
  U32 empty_buffer;
  U32 command_passed;

  cmd = _DAP_FACTORY_RESET; //command we wish to use
  dssm_cmd_received = 0x00000100;
  secap_cmdMask = 0x000000FE;
  secap_txctl_transmit_full_mask = 0x00000001;
  secap_ctl_mask = 0x0000FFFF;
  empty_buffer = 0x00000000;
  counter = 0;
  AccessPoint = 2;       // Device specific AP
  Bank = 0;

  if(phase == 0){
    JLINK_SYS_Report("phase 0");
  }else{
    JLINK_SYS_Report("phase 1");
  }
  
  counter = 0;
  
  if(phase == 0){
  phase = 1;
  do{
    JLINK_SYS_Sleep(500);
    JLINK_SYS_Report1("iteration count: ", counter);
    JLINK_TIF_ActivateTargetReset();                                                            //Pulling nRST line low
    JLINK_SYS_Sleep(300);                                                                       //Giving JLink some time to pull the nRST line low
    JLINK_CORESIGHT_WriteDP(JLINK_CORESIGHT_DP_REG_SELECT, (AccessPoint << 24) | (Bank << 0));  //Switching the access point to SEC-AP
    JLINK_CORESIGHT_WriteAP(0, 0x00);                                                           //Clearing out the mailbox
    JLINK_CORESIGHT_WriteAP(1, cmd);                                             //Setting the command to factory reset 
    JLINK_SYS_Sleep(200);                                                                       //Giving some time to make sure the command enters the mailbox
    JLINK_TIF_ReleaseTargetReset();     
    secap_rxctl_read = JLINK_CORESIGHT_ReadAP(3);
    secap_rxctl_read = secap_rxctl_read & secap_ctl_mask;
    if((secap_rxctl_read & secap_txctl_transmit_full_mask) == secap_txctl_transmit_full_mask){
      JLINK_SYS_Report("Command successfully sent");
      command_passed = 1;
      counter=10;
    }else{
      JLINK_SYS_Report("Command failed to send");
      counter=counter+1;
    }
  }while((counter != 10));

  if(command_passed == 1) //command obviously failed don't bother 
  {
    accessPoint_cmd = (secap_rxctl_read & secap_cmdMask);
    accessPoint_response = JLINK_CORESIGHT_ReadAP(2);

  //JLINK_SYS_Report1("Value of access point command:", accessPoint_cmd);
  //JLINK_SYS_Report1("Value of access point response:", accessPoint_response);

  //JLINK_SYS_Report1("test1: ", (cmd & secap_cmdMask));
  //JLINK_SYS_Report1("test2: ", (accessPoint_response >> 8));

    if ((accessPoint_cmd != (cmd & secap_cmdMask)) ||
        ((accessPoint_response >> 8) != dssm_cmd_received)){
          JLINK_SYS_Report("Command was not succesfully processed, re-run the script");
          //JLINK_SYS_Report1("Command = ", accessPoint_cmd);
          //JLINK_SYS_Report1("Response = ", accessPoint_response);
        } else {
          JLINK_SYS_Report("Command successfully processed");
          JLINK_SYS_Report("Now sending password");
          counter = 0;
          do
          {
            secap_txctl_read = JLINK_CORESIGHT_ReadAP(1);
            cmd = (secap_txctl_read & secap_ctl_mask);
            if ((cmd & secap_txctl_transmit_full_mask) == empty_buffer ||  (counter == 0))
            {
              if (counter == 0){
                JLINK_SYS_Report1("Sending first password: ", _PASSWORD_1);
                JLINK_CORESIGHT_WriteAP(0, _PASSWORD_1); //writing first password in
              }
              else if (counter == 1){
                JLINK_SYS_Report1("Sending second password: ", _PASSWORD_2);
                JLINK_CORESIGHT_WriteAP(0, _PASSWORD_2); //writing second password in
              }
              else if (counter == 2){
                JLINK_SYS_Report1("Sending third password: ", _PASSWORD_3);
                JLINK_CORESIGHT_WriteAP(0, _PASSWORD_3); //writing second password in
              }
              else if (counter == 3){
                JLINK_SYS_Report1("Sending fourth password: ", _PASSWORD_4);
                JLINK_CORESIGHT_WriteAP(0, _PASSWORD_4); //writing second password in
              }
              else{
                //not expected to reach here
                JLINK_SYS_Report("UNKNOWN ERROR");
              }
              JLINK_CORESIGHT_WriteAP(1, _DAP_DATA_EXCHANGE);
              counter = counter + 1;
            }

          }while(counter < 4); // password is only 4 max
          JLINK_SYS_Sleep(500);

          secap_rxctl_read = JLINK_CORESIGHT_ReadAP(3);
          accessPoint_cmd = (secap_rxctl_read & secap_cmdMask);
          accessPoint_response = JLINK_CORESIGHT_ReadAP(2);

          //JLINK_SYS_Report1("Value of access point command:", accessPoint_cmd);
          //JLINK_SYS_Report1("Value of access point response:", accessPoint_response);

          //JLINK_SYS_Report1("test1: ", (cmd & secap_cmdMask));
          //JLINK_SYS_Report1("test2: ", (accessPoint_response >> 8));

          if ((accessPoint_cmd != (0x0000000A)) ||
          ((accessPoint_response >> 8) != 0x00000200)){
            JLINK_SYS_Report("Password sent failed. Re-run the script or check the password being sent is correct");
          } else{
            JLINK_SYS_Report("Password succesfully processed");
          }

        }
      JLINK_SYS_Sleep(500); 
    }
  }

  phase = 1;
  AccessPoint = 4;                                                                            
  JLINK_CORESIGHT_WriteDP(JLINK_CORESIGHT_DP_REG_SELECT, (AccessPoint << 24) | (Bank << 0));  //switching to pwr-ap
  JLINK_CORESIGHT_WriteAP(0x00, _DAP_PWR_AP_DPREC_LPM);                                       //enable low-power mode handling

  return 0;
}