MSP430F2418: Flashing MSP430 from Tiva MCU by using JTAG pins

/*
 * jtag.c
 *
 *  Created on: 17 ����. 2021 �.
 *      Author: Kit
 */
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include <stdio.h>
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "driverlib/debug.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/rom.h"
#include "driverlib/rom_map.h"
#include "driverlib/sysctl.h"
#include "driverlib/uart.h"
#include "driverlib/udma.h"

#include "jtag.h"

/*void SysCtlDelay0(long d)
{
    while(d--);
}

#define SysCtlDelay     SysCtlDelay0*/

//! \brief Buffer size in words for read and write operations
#define WordBufferSize  50
//! \brief Maximum number of tries for the determination of the core
//! identification info
#define MAX_ENTRY_TRY  7

typedef uint16_t    word;
typedef uint8_t     byte;

//! \brief return 0 = error
#define STATUS_ERROR     0      // false
//! \brief return 1 = no error
#define STATUS_OK        1      // true
//! \brief GetDevice returns this if the security fuse is blown
#define STATUS_FUSEBLOWN 2

//! \brief Replicator is active
#define STATUS_ACTIVE    2
//! \brief Replicator is idling
#define STATUS_IDLE      3

#define V_RESET                    0xFFFE

// Constants for flash erasing modes
//! \brief Constant for flash erase: main & info of ALL      mem arrays
#define ERASE_GLOB                 0xA50E
//! \brief Constant for flash erase: main        of ALL      mem arrays
#define ERASE_ALLMAIN              0xA50C
//! \brief Constant for flash erase: main & info of SELECTED mem arrays
#define ERASE_MASS                 0xA506
//! \brief Constant for flash erase: main        of SELECTED mem arrays
#define ERASE_MAIN                 0xA504
//! \brief Constant for flash erase: SELECTED segment
#define ERASE_SGMT                 0xA502

// Instructions for the JTAG control signal register
//! \brief Set the JTAG control signal register
#define IR_CNTRL_SIG_16BIT         0xC8   // original value: 0x13
//! \brief Read out the JTAG control signal register
#define IR_CNTRL_SIG_CAPTURE       0x28   // original value: 0x14
//! \brief Release the CPU from JTAG control
#define IR_CNTRL_SIG_RELEASE       0xA8   // original value: 0x15

// Instructions for the JTAG fuse
//! \brief Prepare for JTAG fuse blow
#define IR_PREPARE_BLOW            0x44   // original value: 0x22
//! \brief Perform JTAG fuse blow
#define IR_EX_BLOW                 0x24   // original value: 0x24

// Instructions for the JTAG data register
//! \brief Set the MSP430 MDB to a specific 16-bit value with the next
//! 16-bit data access
#define IR_DATA_16BIT              0x82   // original value: 0x41
//! \brief Set the MSP430 MDB to a specific 16-bit value (RAM only)
#define IR_DATA_QUICK              0xC2   // original value: 0x43

// Instructions for the JTAG PSA mode
//! \brief Switch JTAG data register to PSA mode
#define IR_DATA_PSA                0x22   // original value: 0x44
//! \brief Shift out the PSA pattern generated by IR_DATA_PSA
#define IR_SHIFT_OUT_PSA           0x62   // original value: 0x46

// Instructions for the JTAG address register
//! \brief Set the MSP430 MAB to a specific 16-bit value
//! \details Use the 20-bit macro for 430X and 430Xv2 architectures
#define IR_ADDR_16BIT              0xC1   // original value: 0x83
//! \brief Read out the MAB data on the next 16/20-bit data access
#define IR_ADDR_CAPTURE            0x21   // original value: 0x84
//! \brief Set the MSP430 MDB with a specific 16-bit value and write
//! it to the memory address which is currently on the MAB
#define IR_DATA_TO_ADDR            0xA1   // original value: 0x85
//! \brief Bypass instruction - TDI input is shifted to TDO as an output
#define IR_BYPASS                  0xFF   // original value: 0xFF

// JTAG identification values for Flash-based MSP430 devices
//! \brief JTAG identification value for 430X architecture devices
#define JTAG_ID                    0x89

#define F_BYTE                     8
#define F_WORD                     16
#define F_ADDR                     20
#define F_LONG                     32

#define MSP_TDO     GPIO_PIN_0
#define MSP_TDI     GPIO_PIN_1
#define MSP_TCLK    MSP_TDI
#define MSP_TMS     GPIO_PIN_2
#define MSP_TCK     GPIO_PIN_3
#define MSP_RST     GPIO_PIN_4
#define MSP_VCC     GPIO_PIN_5

#define TCKPULSE    SysCtlDelay(16)
#define usDelay(us) SysCtlDelay(us * 80)
#define MsDelay(ms) usDelay(ms * 1000L)

#define SetRST()    GPIOPinWrite(GPIO_PORTG_BASE, MSP_RST, MSP_RST)
#define ClrRST()    GPIOPinWrite(GPIO_PORTG_BASE, MSP_RST, 0)
#define SetTMS()    GPIOPinWrite(GPIO_PORTG_BASE, MSP_TMS, MSP_TMS)
#define ClrTMS()    GPIOPinWrite(GPIO_PORTG_BASE, MSP_TMS, 0)
#define SetTCK0()   GPIOPinWrite(GPIO_PORTG_BASE, MSP_TCK, MSP_TCK);
#define ClrTCK0()   GPIOPinWrite(GPIO_PORTG_BASE, MSP_TCK, 0);
#define SetTCK()    {GPIOPinWrite(GPIO_PORTG_BASE, MSP_TCK, MSP_TCK); TCKPULSE;}
#define ClrTCK()    {GPIOPinWrite(GPIO_PORTG_BASE, MSP_TCK, 0); TCKPULSE;}
#define SetTDI()    GPIOPinWrite(GPIO_PORTG_BASE, MSP_TDI, MSP_TDI)
#define ClrTDI()    GPIOPinWrite(GPIO_PORTG_BASE, MSP_TDI, 0)
#define SetTCLK()   SetTDI()
#define ClrTCLK()   ClrTDI()
#define StoreTCLK()     ((uint16_t)GPIOPinRead(GPIO_PORTG_BASE, MSP_TCLK))
#define RestoreTCLK(t)  GPIOPinWrite(GPIO_PORTG_BASE, MSP_TCLK, t)
#define ScanTDO()   GPIOPinRead(GPIO_PORTG_BASE, MSP_TDO)

//! \brief Holds the Flash InfoA Lock/Unlock Key, default = locked
static unsigned short SegmentInfoAKey = 0xA500;

unsigned long AllShifts(word Format, unsigned long Data)
{
    word tclk = StoreTCLK();  // Store TCLK state;


    unsigned long TDOword = 0x00000000;
    unsigned long MSB = 0x00000000;
    word i;




    switch(Format)
    {
    case F_BYTE: MSB = 0x00000080;
      break;
    case F_WORD: MSB = 0x00008000;
      break;
    case F_ADDR: MSB = 0x00080000;
      break;
    case F_LONG: MSB = 0x80000000;
      break;
    default: // this is an unsupported format, function will just return 0
      return TDOword;
    }
    for (i = Format; i > 0; i--)

    {
      ((Data & MSB) == 0) ? ClrTDI() : SetTDI();
      Data <<= 1;
      if (i == 1)
      {                       // Last bit requires TMS=1

         SetTMS();

      }
      ClrTCK();
      SetTCK();
      TDOword <<= 1;          // TDO could be any port pin
      if (ScanTDO() != 0)

      {
          TDOword++;

      }
    }
    // common exit
    RestoreTCLK(tclk);                  // restore TCLK state


    // JTAG FSM = Exit-DR
    ClrTCK();
    SetTCK();
    // JTAG FSM = Update-DR
    ClrTMS();
    ClrTCK();
    SetTCK();
    // JTAG FSM = Run-Test/Idle
    return(TDOword);
}


static word DR_Shift16(word Data)
{
    // JTAG FSM state = Run-Test/Idle
    SetTMS();
    ClrTCK();
    SetTCK();

    // JTAG FSM state = Select DR-Scan
    ClrTMS();
    ClrTCK();
    SetTCK();
    // JTAG FSM state = Capture-DR
    ClrTCK();
    SetTCK();
    // JTAG FSM state = Shift-DR, Shift in TDI (16-bit)
    return((word)AllShifts(F_WORD, Data));
    // JTAG FSM state = Run-Test/Idle
}

static unsigned long DR_Shift20(unsigned long address)
{
    // JTAG FSM state = Run-Test/Idle
    SetTMS();
    ClrTCK();
    SetTCK();

    // JTAG FSM state = Select DR-Scan
    ClrTMS();
    ClrTCK();
    SetTCK();
    // JTAG FSM state = Capture-DR
    ClrTCK();
    SetTCK();
    // JTAG FSM state = Shift-DR, Shift in TDI (16-bit)
    return(AllShifts(F_ADDR, address));
    // JTAG FSM state = Run-Test/Idle
}

static word IR_Shift(byte Instruction)
{
    // JTAG FSM state = Run-Test/Idle
    SetTMS();
    ClrTCK();
    SetTCK();
    // JTAG FSM state = Select DR-Scan
    ClrTCK();
    SetTCK();

    // JTAG FSM state = Select IR-Scan
    ClrTMS();
    ClrTCK();
    SetTCK();
    // JTAG FSM state = Capture-IR
    ClrTCK();
    SetTCK();
    // JTAG FSM state = Shift-IR, Shift in TDI (8-bit)
    return(AllShifts(F_BYTE, Instruction));
    // JTAG FSM state = Run-Test/Idle
}

static void ResetTAP(void)
{
    word i;
    // process TDI first to settle fuse current
    SetTDI();
    SetTMS();
    SetTCK();

    // Reset JTAG FSM
    for (i = 6; i > 0; i--)
    {
        ClrTCK();
        SetTCK();
    }
    // JTAG FSM is now in Test-Logic-Reset
    ClrTCK0();
    ClrTMS(); TCKPULSE;
    SetTCK0();
    SetTMS(); TCKPULSE;
    // JTAG FSM is now in Run-Test/IDLE

    // Perform fuse check
    ClrTMS();
    usDelay(5); // at least 5us low required
    SetTMS();
    ClrTMS();
    usDelay(5); // at least 5us low required
    SetTMS();
}

word ReadMem_430X(word Format, unsigned long Addr);
#define ReadMem ReadMem_430X
void WriteMem_430X(word Format, unsigned long Addr, word Data);
#define WriteMem WriteMem_430X

static word ExecutePOR_430X(void)
{
    word JtagVersion;

    // Perform Reset
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2C01);                      // Apply Reset
    DR_Shift16(0x2401);                      // Remove Reset
    ClrTCLK();
    SetTCLK();
    ClrTCLK();
    SetTCLK();
    ClrTCLK();
    JtagVersion = IR_Shift(IR_ADDR_CAPTURE); // read JTAG ID, checked at function end
    SetTCLK();

    WriteMem_430X(F_WORD, 0x0120, 0x5A80);   // Disable Watchdog on target device

    if (JtagVersion != JTAG_ID)
    {
        return(STATUS_ERROR);
    }
    return(STATUS_OK);
}

static word SetInstrFetch(void)
{
    word i;

    IR_Shift(IR_CNTRL_SIG_CAPTURE);

    // Wait until CPU is in instr. fetch state, timeout after limited attempts
    for (i = 50; i > 0; i--)
    {
        if (DR_Shift16(0x0000) & 0x0080)
        {
            return(STATUS_OK);
        }
        ClrTCLK();
        SetTCLK();
    }
    return(STATUS_ERROR);
}

static void SetPC_430X(unsigned long Addr)
{
    SetInstrFetch();              // Set CPU into instruction fetch mode, TCLK=1

    // Load PC with address
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x3401);           // CPU has control of RW & BYTE.
    IR_Shift(IR_DATA_16BIT);
    DR_Shift16((word)(0x0080 | (((Addr)>>8) & 0x0F00))); // "mova #addr20,PC" instruction

    ClrTCLK();
    SetTCLK();
    DR_Shift16(Addr);             // second word of "mova #addr20,PC" instruction
    ClrTCLK();
    SetTCLK();
    IR_Shift(IR_ADDR_CAPTURE);
    ClrTCLK();                    // Now the PC should be on Addr
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2401);           // JTAG has control of RW & BYTE.
}

static void HaltCPU(void)
{
    SetInstrFetch();              // Set CPU into instruction fetch mode

    IR_Shift(IR_DATA_16BIT);
    DR_Shift16(0x3FFF);           // Send JMP $ instruction
    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2409);           // Set JTAG_HALT bit
    SetTCLK();
}

static void ReleaseCPU(void)
{
    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2401);           // Clear the HALT_JTAG bit
    IR_Shift(IR_ADDR_CAPTURE);
    SetTCLK();
}

word VerifyPSA_430X(unsigned long StartAddr, unsigned long Length, word *DataArray)
{
    word TDOword;
    unsigned int i;
    const word POLY = 0x0805;             // Polynom value for PSA calculation
    word PSA_CRC = (word)(StartAddr-2);   // Start value for PSA calculation

    ExecutePOR_430X();

    SetPC_430X(StartAddr-4);
    HaltCPU();
    ClrTCLK();
    IR_Shift(IR_DATA_16BIT);
    DR_Shift20(StartAddr-2);

    IR_Shift(IR_DATA_PSA);

    for (i = 0; i < Length; i++)
    {
        // Calculate the PSA (Pseudo Signature Analysis) value
        if ((PSA_CRC & 0x8000) == 0x8000)
        {
            PSA_CRC ^= POLY;
            PSA_CRC <<= 1;
            PSA_CRC |= 0x0001;
        }
        else
        {
            PSA_CRC <<= 1;
        }
        // if pointer is 0 then use erase check mask, otherwise data
        &DataArray[0] == 0 ? (PSA_CRC ^= 0xFFFF) : (PSA_CRC ^= DataArray[i]);

        // Clock through the PSA
        SetTCLK();

        //ClrTCLK();          // set here -> Fixes problem with F123 PSA in RAM

        ClrTCK();
        usDelay(1);
        SetTMS();

        SetTCK();           // Select DR scan
        ClrTCK();
        ClrTMS();

        SetTCK();            // Capture DR
        ClrTCK();

        SetTCK();           // Shift DR
        ClrTCK();
        SetTMS();

        SetTCK();           // Exit DR
        ClrTCK();
        SetTCK();
        ClrTMS();
        ClrTCK();
        SetTCK();

        ClrTCLK();           // set here -> future purpose
        usDelay(10);
    }
    IR_Shift(IR_SHIFT_OUT_PSA);
    TDOword = DR_Shift16(0x0000);     // Read out the PSA value
    SetTCLK();

    ReleaseCPU();
    ExecutePOR_430X();

    return((TDOword == PSA_CRC) ? STATUS_OK : STATUS_ERROR);

}

word VerifyMem_430X(unsigned long StartAddr, unsigned long Length, word *DataArray);
#define VerifyMem VerifyMem_430X



void DrvSignals(void)
{
    GPIOPinTypeGPIOInput(GPIO_PORTG_BASE, MSP_TDI | MSP_TMS | MSP_TCK | MSP_TCLK | MSP_RST | MSP_VCC);
    GPIOPinWrite(GPIO_PORTG_BASE, MSP_TDI | MSP_TMS | MSP_TCK | MSP_TCLK | MSP_RST, MSP_TDI | MSP_TMS | MSP_TCK | MSP_TCLK | MSP_RST);
    GPIOPinTypeGPIOOutput(GPIO_PORTG_BASE, MSP_TDI | MSP_TMS | MSP_TCK | MSP_TCLK | MSP_RST);
}

static void CheckJtagFuse_JTAG(void)
{
    // perform a JTAG fuse check
    SetTMS(); SysCtlDelay(3 * 5);
    ClrTMS(); SysCtlDelay(3 * 5);
    usDelay(15);
    SetTMS(); SysCtlDelay(3 * 5);
    ClrTMS(); SysCtlDelay(3 * 5);
    usDelay(15);
    SetTMS(); SysCtlDelay(3 * 5);
}


static word StartJtag(void)
{
    // drive JTAG/TEST signals
    DrvSignals();
    MsDelay(10);             // delay 10ms

    SetRST();

    ResetTAP();  // reset TAP state machine -> Run-Test/Idle

    CheckJtagFuse_JTAG();

    return IR_Shift(IR_BYPASS);
}

static void StopJtag (void)
{
    // release JTAG/TEST signals
    {
        GPIOPinTypeGPIOInput(GPIO_PORTG_BASE, MSP_TDI | MSP_TMS | MSP_TCK | MSP_TCLK | MSP_RST | MSP_VCC);
      MsDelay(10);             // delay 10ms
    }
}

void TCLKstrobes(word Amount)
{
    volatile word i;

    // This implementation has 45 (MCLK=18MHz)
    // or 30 (MCLK 12MHz) body cycles! -> 400kHz
    // DO NOT MODIFY IT !

    for (i = Amount; i > 0; i--)
    {
        SetTDI();
        SysCtlDelay(18 * 5);
        ClrTDI();
        SysCtlDelay(8 * 5);
    }
}


word IsFuseBlown(void)
{
    word i;

    for (i = 3; i > 0; i--)     //  First trial could be wrong
    {
        IR_Shift(IR_CNTRL_SIG_CAPTURE);
        if (DR_Shift16(0xAAAA) == 0x5555)
        {
            return(STATUS_OK);  // Fuse is blown
        }
    }
    return(STATUS_ERROR);       // fuse is not blown
}

// High level JTAG functions
word GetDevice_430X(void)
{
    word i = 0, JtagId = 0; // Initialize JtagId with an invalid value

    for (i = 0; i < MAX_ENTRY_TRY; i++)
    {
        StopJtag();    // Release JTAG/TEST signals to savely reset the test logic
        JtagId = StartJtag();   // Establish the physical connection to the JTAG interface
        if(JtagId == JTAG_ID) // Break if a valid JTAG ID is being returned
        break;
    }
    if(i >= MAX_ENTRY_TRY)
    {
        return(STATUS_ERROR);
    }

    ResetTAP();                          // Reset JTAG state machine, check fuse HW

    if (IsFuseBlown())                   // Stop here if fuse is already blown
    {
        return(STATUS_FUSEBLOWN);
    }
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2401);                  // Set device into JTAG mode + read
    if (IR_Shift(IR_CNTRL_SIG_CAPTURE) != JTAG_ID)
    {
        return(STATUS_ERROR);
    }

    // Wait until CPU is synchronized, timeout after a limited # of attempts
    for (i = 50; i > 0; i--)
    {
        if (DR_Shift16(0x0000) & 0x0200)
        {
            word DeviceId;
            DeviceId = ReadMem(F_WORD, 0x0FF0);// Get target device type
                                               //(bytes are interchanged)
            DeviceId = (DeviceId << 8) + (DeviceId >> 8); // swop bytes
            //Set Device index, which is used by functions in Device.c
            //SetDevice(DeviceId);
            break;
        }
        else
        {
            if (i == 1)
            {
                return(STATUS_ERROR);      // Timeout reached, return false
            }
        }
    }
    if (!ExecutePOR_430X())                // Perform PUC, Includes
    {
        return(STATUS_ERROR);              // target Watchdog disable.
    }
    return(STATUS_OK);
}

#define GetDevice GetDevice_430X

void ReleaseDevice_430X(unsigned long Addr)
{
    if (Addr == V_RESET)
    {
        IR_Shift(IR_CNTRL_SIG_16BIT);
        DR_Shift16(0x2C01);                 // Perform a reset
        DR_Shift16(0x2401);
    }
    else
    {
        SetPC_430X(Addr);                   // Set target CPU's PC
    }
    IR_Shift(IR_CNTRL_SIG_RELEASE);
}

#define ReleaseDevice ReleaseDevice_430X

void WriteMem_430X(word Format, unsigned long Addr, word Data)
{
    HaltCPU();

    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);
    if  (Format == F_WORD)
    {
        DR_Shift16(0x2408);     // Set word write
    }
    else
    {
        DR_Shift16(0x2418);     // Set byte write
    }
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(Addr);           // Set addr
    IR_Shift(IR_DATA_TO_ADDR);
    DR_Shift16(Data);           // Shift in 16 bits
    SetTCLK();

    ReleaseCPU();
}

void WriteMemQuick_430X(unsigned long StartAddr, unsigned long Length, word *DataArray)
{
    unsigned long i;

    // Initialize writing:
    SetPC_430X(StartAddr-4);
    HaltCPU();

    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2408);             // Set RW to write
    IR_Shift(IR_DATA_QUICK);
    for (i = 0; i < Length; i++)
    {
        DR_Shift16(DataArray[i]);   // Shift in the write data
        SetTCLK();
        ClrTCLK();                  // Increment PC by 2
    }
    ReleaseCPU();
}

#define WriteMemQuick WriteMemQuick_430X

void WriteFLASH_430X(unsigned long StartAddr, unsigned long Length, word *DataArray)
{
    word i;                            // Loop counter
    unsigned long addr = StartAddr;    // Address counter
    word FCTL3_val = SegmentInfoAKey;  // Lock/Unlock SegA InfoMem Seg.A, def=locked

    HaltCPU();

    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2408);         // Set RW to write
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(0x0128);         // FCTL1 register
    IR_Shift(IR_DATA_TO_ADDR);
    DR_Shift16(0xA540);         // Enable FLASH write
    SetTCLK();

    ClrTCLK();
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(0x012A);         // FCTL2 register
    IR_Shift(IR_DATA_TO_ADDR);
    DR_Shift16(0xA540);         // Select MCLK as source, DIV=1
    SetTCLK();

    ClrTCLK();
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(0x012C);         // FCTL3 register
    IR_Shift(IR_DATA_TO_ADDR);
    DR_Shift16(FCTL3_val);      // Clear FCTL3; F2xxx: Unlock Info-Seg.
                                // A by toggling LOCKA-Bit if required,
    SetTCLK();

    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);

    for (i = 0; i < Length; i++, addr += 2)
    {
        DR_Shift16(0x2408);             // Set RW to write
        IR_Shift(IR_ADDR_16BIT);
        DR_Shift20(addr);               // Set address
        IR_Shift(IR_DATA_TO_ADDR);
        DR_Shift16(DataArray[i]);       // Set data
        SetTCLK();
        ClrTCLK();
        IR_Shift(IR_CNTRL_SIG_16BIT);
        DR_Shift16(0x2409);             // Set RW to read

        TCLKstrobes(35);        // Provide TCLKs, min. 33 for F149 and F449
                                // F2xxx: 29 are ok
    }

    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2408);         // Set RW to write
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(0x0128);         // FCTL1 register
    IR_Shift(IR_DATA_TO_ADDR);
    DR_Shift16(0xA500);         // Disable FLASH write
    SetTCLK();

    // set LOCK-Bits again
    ClrTCLK();
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(0x012C);         // FCTL3 address
    IR_Shift(IR_DATA_TO_ADDR);
    DR_Shift16(FCTL3_val | 0x0010);      // Lock Inf-Seg. A by toggling LOCKA and set LOCK again
    SetTCLK();

    ReleaseCPU();
}

#define WriteFLASH WriteFLASH_430X

word WriteFLASHallSections_430X(const uint16_t *data, const unsigned long *address, const unsigned long *length_of_sections, const unsigned long sections)
{
    int i, init = 1;

    for(i = 0; i < sections; i++)
    {
        // Write/Verify(PSA) one FLASH section
        WriteFLASH(address[i], length_of_sections[i], (word*)&data[init-1]);
        if (!VerifyMem(address[i], length_of_sections[i], (word*)&data[init-1]))
        {
            return(STATUS_ERROR);
        }
        init += length_of_sections[i];
    }

    return(STATUS_OK);
}

#define WriteFLASHallSections WriteFLASHallSections_430X

word ReadMem_430X(word Format, unsigned long Addr)
{
    word TDOword;

    HaltCPU();

    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);
    if  (Format == F_WORD)
    {
        DR_Shift16(0x2409);             // Set word read
    }
    else
    {
        DR_Shift16(0x2419);             // Set byte read
    }
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(Addr);                   // Set address
    IR_Shift(IR_DATA_TO_ADDR);
    SetTCLK();
    ClrTCLK();
    TDOword = DR_Shift16(0x0000);       // Shift out 16 bits
    ReleaseCPU();
    return(Format == F_WORD ? TDOword : TDOword & 0x00FF);
}

#define ReadMem ReadMem_430X

void ReadMemQuick_430X(unsigned long StartAddr, unsigned long Length, word *DataArray)
{
    unsigned long i;

    // Initialize reading:
    SetPC_430X(StartAddr-4);
    HaltCPU();

    ClrTCLK();
    IR_Shift(IR_CNTRL_SIG_16BIT);
    DR_Shift16(0x2409);                    // Set RW to read
    IR_Shift(IR_DATA_QUICK);

    for (i = 0; i < Length; i++)
    {
        SetTCLK();
        DataArray[i] = DR_Shift16(0x0000); // Shift out the data
                                          // from the target.
        ClrTCLK();
    }
    ReleaseCPU();
}

#define ReadMemQuick ReadMemQuick_430X

void EraseFLASH_430X(word EraseMode, unsigned long EraseAddr)
{
    word StrobeAmount = 4820;           // default for Segment Erase
    volatile word i, loopcount = 1;     // erase cycle repeating for Mass Erase
    word FCTL3_val = SegmentInfoAKey;   // Lock/Unlock SegA InfoMem Seg.A, def=locked

    if ((EraseMode == ERASE_MASS) ||
        (EraseMode == ERASE_MAIN) ||
        (EraseMode == ERASE_ALLMAIN) ||
        (EraseMode == ERASE_GLOB)
       )
    {
            StrobeAmount = 10600;   // Larger Flash memories require
    }
    HaltCPU();

    for (i = loopcount; i > 0; i--)
    {
        ClrTCLK();
        IR_Shift(IR_CNTRL_SIG_16BIT);
        DR_Shift16(0x2408);         // set RW to write
        IR_Shift(IR_ADDR_16BIT);
        DR_Shift20(0x0128);         // FCTL1 address
        IR_Shift(IR_DATA_TO_ADDR);
        DR_Shift16(EraseMode);      // Enable erase mode
        SetTCLK();

        ClrTCLK();
        IR_Shift(IR_ADDR_16BIT);
        DR_Shift20(0x012A);         // FCTL2 address
        IR_Shift(IR_DATA_TO_ADDR);
        DR_Shift16(0xA540);         // MCLK is source, DIV=1
        SetTCLK();

        ClrTCLK();
        IR_Shift(IR_ADDR_16BIT);
        DR_Shift20(0x012C);         // FCTL3 address
        IR_Shift(IR_DATA_TO_ADDR);
        DR_Shift16(FCTL3_val);      // Clear FCTL3; F2xxx: Unlock Info-Seg. A by toggling LOCKA-Bit if required,
        SetTCLK();

        ClrTCLK();
        IR_Shift(IR_ADDR_16BIT);
        DR_Shift20(EraseAddr);      // Set erase address
        IR_Shift(IR_DATA_TO_ADDR);
        DR_Shift16(0x55AA);         // Dummy write to start erase
        SetTCLK();

        ClrTCLK();
        IR_Shift(IR_CNTRL_SIG_16BIT);
        DR_Shift16(0x2409);         // Set RW to read
        TCLKstrobes(StrobeAmount);  // Provide TCLKs
        IR_Shift(IR_CNTRL_SIG_16BIT);
        DR_Shift16(0x2408);         // Set RW to write
        IR_Shift(IR_ADDR_16BIT);
        DR_Shift20(0x0128);         // FCTL1 address
        IR_Shift(IR_DATA_TO_ADDR);
        DR_Shift16(0xA500);         // Disable erase
        SetTCLK();

    }
    // set LOCK-Bits again
    ClrTCLK();
    IR_Shift(IR_ADDR_16BIT);
    DR_Shift20(0x012C);             // FCTL3 address
    IR_Shift(IR_DATA_TO_ADDR);
    DR_Shift16(FCTL3_val | 0x0010);          // Lock Inf-Seg. A by toggling LOCKA (F2xxx) and set LOCK again
    SetTCLK();

    ReleaseCPU();
}

#define EraseFLASH EraseFLASH_430X

word EraseCheck_430X(unsigned long StartAddr, unsigned long Length)
{
    return (VerifyPSA_430X(StartAddr, Length, 0));
}

#define EraseCheck EraseCheck_430X

word VerifyMem_430X(unsigned long StartAddr, unsigned long Length, word *DataArray)
{
    return (VerifyPSA_430X(StartAddr, Length, DataArray));
}

#define VerifyMem VerifyMem_430X

uint16_t FWData[8192];
uint32_t FWAddr = 0;
uint32_t FWSize = 0;

ProgStat StartProg()
{
    if(GetDevice() != STATUS_OK)
        return ST_GetError;
    EraseFLASH(ERASE_MAIN, 0x3100);
    if(!EraseCheck(0x3100, 0x0400))
        return ST_EraseError;
    return ST_Ok;
}

ProgStat WriteSeg()
{
    WriteFLASH(FWAddr, FWSize / 2, FWData);
    if(!VerifyMem(FWAddr, FWSize / 2, FWData))
        return ST_ProgError;
    return ST_Ok;
}

void DoneProg()
{
    ReleaseDevice(V_RESET);
}

bool ProgMode = false;
bool PCRunning = false;
ProgCmd CurCmd = PC_None;