// ECS RS485 bootloader firmware for stmG0 MCUs

#ifdef DEBUG
#include <stdio.h>
#endif
#include <string.h>

#include "stm32g0xx.h"
#include "RTT/SEGGER_RTT.h"

#include "main.h"
#include "aes.h"

#ifndef DEBUG
#warning  "Don't forget to turn off and turn on the device after first programming"
#endif

// Bootloader firmware internal version
__attribute__((aligned (16))) const char FW_VERSION[16] = {"ECS.FW:01.00.01"};  // <Keep Symbols> setting in linker settings prevents this line to be thrown away

//AES128 key
const uint8_t AES_KEY[16] = {0x5c, 0x38, 0x36, 0x57, 0x60, 0x3d, 0x0e, 0x38, 0x61, 0x1e, 0x4e, 0x21, 0x62, 0x19, 0x47, 0x62};                       

union
{
    uint8_t address_and_crc[5];
  
    struct __attribute__((packed))
    {
        uint8_t address[4];
        uint8_t crc;
    } raw;

    struct __attribute__((packed))
    {
        uint32_t address;
        uint8_t res;
    } mem32;

    struct __attribute__((packed))
    {
        uint8_t* paddress;
        uint8_t res;
    } mem8;
} u;

union bigEndian
{
    uint16_t itself;
    uint8_t bytes[2];
};

union flashData
{
    uint64_t dwordData[256>>3];
    uint8_t byteData[256];
};

// Tick counter
volatile unsigned int msCounter = 0; 

//-----------------------------------------------------------------------------

void Init(void);
int16_t WaitForByte(unsigned int toWait);
void Transmit(uint8_t *pData, int numBytes);
void TransmitACK(void);
void TransmitNACK(void);
void FLASH_Program_DoubleWord(uint32_t Address, uint64_t Data);
void Delay(uint32_t delay_in_ms);
void TurnErrorLEDOn(uint32_t blink_period, uint32_t how_long_to_keep_blinking);
void JumpToApplication(void);

//-----------------------------------------------------------------------------

int main(void)
{
    // UART, PORT A and PORTB initialization
    Init();

    // Saving the time bootloader has started
    uint32_t bl_startup_time = msCounter;

    while(1)
    {
        // Waiting for bootloader initialization from host for a 10s
#ifdef DEBUG
        printf("%s\nWaiting 10s for communication initialization...\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
        int16_t init = WaitForByte(BOOTLOADER_TIME_TO_WAIT);
        if (init == -1) // Error occurred, during receiption of one byte
        {
#ifdef DEBUG
            printf("%s\nError during receiption of one byte!\n", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
            // Checking how much time elapsed since bootloader startup
            if (msCounter - bl_startup_time > BOOTLOADER_TIME_TO_WAIT)
            {
                JumpToApplication(); // Trying to start main program, if crc is correct, if not, going back to wait more for programmer
                bl_startup_time = msCounter;  // Resetting start up time
            }
            continue;  // if we still have time, we can go back to wait a bit more for correct byte
        }
        else if (init == -2) // Time-out has occured
        {
#ifdef DEBUG
            printf("%s\nTimeout of 10s!\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
            JumpToApplication();
            bl_startup_time = msCounter;  // Resetting start up time
            continue; // This command should never be reached if CRC is ok
        }
        else if (init != CMD_INIT) // We received one byte, but it is not one we need
        {
#ifdef DEBUG
            printf("%s\nWrong INIT byte received! Got %u instead of %u!\n", RTT_CTRL_TEXT_BRIGHT_RED, init, CMD_INIT);
#endif
            // Checking how much time elapsed since bootloader startup
            if (msCounter - bl_startup_time > BOOTLOADER_TIME_TO_WAIT)
            {
                JumpToApplication();  // Trying to start main program, if crc is correct, if not, going back to wait more for programmer
                bl_startup_time = msCounter;  // Resetting start up time
            }
            continue;
        }

#ifdef DEBUG
        printf("%sBootloader is initialized.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif

        TransmitACK();

        // Entering command mode
        // We must receive two bytes
        // Waiting for a fist byte
        int16_t first, second;
        while (1)
        {
            first = WaitForByte(5000);

            if (first == -2) break;   // If timeout happened, then we return to init stage
            else if (first == -1) continue; // If there's an error during reading from USART, then we keep reading
            else
            {
                second = WaitForByte(5000);

                if (second < 0) continue;   // If timeout or error happened, then we go back to the receiving of the first byte
                else
                {
                    uint8_t command = first;
                    uint8_t command_xor = ~second;

                    if ((command == CMD_GETID) && (command_xor == CMD_GETID))
                    {
#ifdef DEBUG
                        printf("%sGot GET_ID command.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
                        uint8_t TxData[5];

                        TxData[0] = CMD_ACK;
                        TxData[1] = 1;
                        // Locating DEV_ID in memory
                        uint8_t *dev_id = (uint8_t*)DBG_BASE;
                        // Now, we can access it as byte array
                        TxData[2] = dev_id[1] & 0x0F;   // Clearing upper nibble of the MSB
                        TxData[3] = dev_id[0];
                        TxData[4] = CMD_ACK;

                        Transmit(TxData, 5);
                    }
                    else if ((command == CMD_GET) && (command_xor == CMD_GET))
                    {
#ifdef DEBUG
                        printf("%sGot GET command.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
                        uint8_t TxData[15];
                          
                        uint8_t idx = 0;
                        TxData[idx]  = CMD_ACK;
                        TxData[++idx] = 11;
                        TxData[++idx] = BOOTLOADER_VERSION;
                        TxData[++idx] = CMD_GET;
                        TxData[++idx] = CMD_GET_VER_RPS;
                        TxData[++idx] = CMD_GETID;
                        TxData[++idx] = CMD_READ_MEMORY;
                        TxData[++idx] = CMD_GO;
                        TxData[++idx] = CMD_WRITE_MEMORY;
                        TxData[++idx] = CMD_ERASE;
                        TxData[++idx] = CMD_WRITE_PROTECT;
                        TxData[++idx] = CMD_WRITE_UNPROTECT;
                        TxData[++idx] = CMD_READOUT_PROTECT;
                        TxData[++idx] = CMD_READOUT_UNPROTECT;
                        TxData[++idx] = CMD_ACK;

                        Transmit(TxData, 15);
                    }
                    else if ((command == CMD_READ_MEMORY) && (command_xor == CMD_READ_MEMORY))
                    {
#ifdef DEBUG
                        printf("%sGot READ_MEMORY command.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
                        uint8_t TxData;
                        // !!! Before sending ACK we must check RDP protection
                        TransmitACK();
 
                        int error_flag = 0;
                        int16_t tmp;

                        memset(&u, 0, sizeof(u));

                        for (int i = 0; i < 5; i++)
                        {
                            tmp = WaitForByte(5000);
                            if (tmp < 0)
                            {
                                error_flag = 1;
                                break;
                            }
                            else u.address_and_crc[i] = tmp;
                        }

                        if (error_flag) continue;

                        uint8_t checksum = u.raw.address[0] ^ u.raw.address[1] ^ u.raw.address[2] ^ u.raw.address[3];

                        u.mem32.address = __REV(u.mem32.address);

                        if (checksum == u.raw.crc)
                        {
#ifdef DEBUG
                            printf("\t%sAddress 0x%08X requested.\n", RTT_CTRL_TEXT_BRIGHT_GREEN, u.mem32.address);
#endif
                            TransmitACK();

                            uint8_t bytesNumber_crc[2];
                    
                            error_flag = 0;

                            for (int i = 0; i < 2; i++)
                            {
                                tmp = WaitForByte(5000);
                                if (tmp < 0)
                                {
                                    error_flag = 1;
                                    break;
                                }
                                else bytesNumber_crc[i] = tmp;
                            }
                    
                            if (error_flag) continue;

                            bytesNumber_crc[1] = ~bytesNumber_crc[1];

                            if (bytesNumber_crc[0] == bytesNumber_crc[1])
                            {
                                TransmitACK();

                                // Checking memory address access possibility

                                // We do not allow reading from the device, so we send fake data
                                uint8_t fakeData[bytesNumber_crc[0]+1];
                                memset(fakeData, 0xEC, bytesNumber_crc[0]+1);
                                Transmit(fakeData, bytesNumber_crc[0]+1);
                                //Transmit(u.mem8.paddress, bytesNumber_crc[0]+1);
                            }
                            else TransmitNACK();
                        }
                        else
                        {
#ifdef DEBUG
                            printf("\n%s Memory address CRC error!", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
                            TransmitNACK();
                        }
                    }
                    else if ((command == CMD_GET_VER_RPS) && (command_xor == CMD_GET_VER_RPS))
                    {
                        uint8_t TxData[5];

                        TxData[0]  = CMD_ACK;
                        TxData[1]  = BOOTLOADER_VERSION;
                        TxData[2]  = 0x00;
                        TxData[3]  = 0x00;

                        TxData[4] = CMD_ACK;
          
                        Transmit(TxData, 5);
                    }
                    else if ((command == CMD_ERASE) && (command_xor == CMD_ERASE))
                    {
#ifdef DEBUG
                        printf("%sGot ERASE command.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
                        // !!! Before sending ACK we must check RDP protection
                        TransmitACK();
                        
                        int16_t numPages = WaitForByte(5000);
                        if (numPages < 0) continue;
                        else
                        {
                            uint8_t Pages[numPages + 2];  // + XOR byte

                            int error_flag = 0;
                            int16_t tmp;

                            for (int i = 0; i < (numPages + 2); i++)
                            {
                                tmp = WaitForByte(5000);
                                if (tmp < 0)
                                {
                                    error_flag = 1;
                                    break;
                                }
                                else Pages[i] = tmp;
                            }

                            if (error_flag) continue;
#ifdef DEBUG                        
                            printf("\tErasing %d pages:", numPages + 1);
#endif
                            uint8_t xor_crc = numPages;
                            for (int i = 0; i < (numPages + 1); i++)
                            {
                                xor_crc ^= Pages[i];
#ifdef DEBUG
                                printf(" %d", Pages[i]);
#endif
                            }
#ifdef DEBUG
                            printf("\n");
#endif
                            if (xor_crc == Pages[numPages + 2 - 1])
                            {
                                // Is FLASH_CR register locked for writing?
                                if (FLASH->CR & FLASH_CR_LOCK)
                                {
                                    // Unlocking FLASH_CR register
                                    FLASH->KEYR = KEY1;
                                    FLASH->KEYR = KEY2;
                                }
#ifdef DEBUG
                                printf("\t\t%sStarting erasing.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
                                // Erasing corresponding pages, except those containing bootloader itself
                                for (int i = 0; i < (numPages + 1); i++)
                                {
                                    if (Pages[i] > BOOTLOADER_LAST_PAGE)  // Bootloader occupies first 4-5 pages (2k)
                                    {
#ifdef DEBUG
                                        while (FLASH->SR & FLASH_SR_BSY1) printf("\t%sWaiting for releasing of FLASH memory.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#else
                                        while (FLASH->SR & FLASH_SR_BSY1);
#endif
                                        // Clearing possible old errors
                                        FLASH->SR |= FLASH_SR_FASTERR & FLASH_SR_MISERR & FLASH_SR_PGSERR & FLASH_SR_SIZERR & FLASH_SR_PGAERR & FLASH_SR_PROGERR & FLASH_SR_OPERR;

                                        FLASH->CR &= ~FLASH_CR_PNB;
                                        FLASH->CR |= Pages[i] << FLASH_CR_PNB_Pos;

                                        FLASH->CR |= FLASH_CR_PER;
                                        
                                        FLASH->CR |= FLASH_CR_STRT;
#ifdef DEBUG
                                        while (FLASH->SR & FLASH_SR_BSY1) printf("\t\t%sWaiting for page %d to be erased...\n", RTT_CTRL_TEXT_BRIGHT_GREEN, Pages[i]);
#else
                                        while (FLASH->SR & FLASH_SR_BSY1);
#endif
#ifdef DEBUG
                                        printf("\t\t%sPage %d has been erased.\n", RTT_CTRL_TEXT_BRIGHT_GREEN, Pages[i]);
#endif
                                        FLASH->CR &= ~FLASH_CR_PER;
                                    }
                                }
#ifdef DEBUG
                                printf("\t\t%sErasing is finished.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif                          
                                FLASH->CR |= FLASH_CR_LOCK;

                                TransmitACK();
                            }
                            else TransmitNACK();
                        }
                    }
#if BOOTLOADER_VERSION > 0x30
                    else if ((command == CMD_EXT_ERASE) && (command_xor == CMD_EXT_ERASE))
                    {
                        printf("%sGot EXTENDED ERASE command.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);

                        // !!! Before sending ACK we must check RDP protection
                        TransmitACK();

                        int error_flag = 0;
                        int16_t tmp;
                        union bigEndian numPages;

                        for (int i = 1; i >= 0; i--)
                        {
                            tmp = WaitForByte(5000, 250);
                            if (tmp < 0)
                            {
                                error_flag = 1;
                                break;
                            }
                            else numPages.bytes[i] = tmp;
                        }

                        if (error_flag) continue;
                        
                        if ((numPages.itself & 0xFFF0) == 0xFFF0)
                        {
                            int16_t crc = WaitForByte(5000, 250);

                            if (crc < 0) continue;
                            else
                            {
                                switch(numPages.itself)
                                {
                                    case 0xFFFF:  // Mass erase
                                        if (crc == 0x00)
                                        {
                                            // Perfom erase
                                            TransmitACK();
                                        }
                                        else
                                        {
                                            TransmitNACK();
                                        }
                                        break;

                                    case 0xFFFE:  // Bank1 erase
                                        if (crc == 0x01)
                                        {
                                            // Perfom erase
                                            TransmitACK();
                                        }
                                        else
                                        {
                                            TransmitNACK();
                                        }
                                        break;

                                    case 0xFFFD:
                                        if (crc == 0x02)
                                        {
                                            // Perfom erase
                                            TransmitACK();
                                        }
                                        else
                                        {
                                            TransmitNACK();
                                        }
                                        break;
                                }
                            }
                        }
                        else
                        {
                            union bigEndian pageCodes[numPages.itself + 1];

                            int error_flag = 0;
                            int16_t tmp;

                            for (int i = 0; i < (numPages.itself + 1); i++)
                            {
                                for (int j = 1; j >= 0; j--)
                                {
                                    tmp = WaitForByte(5000, 125);
                                    if (tmp < 0)
                                    {
                                        error_flag = 1;
                                        break;
                                    }
                                    else pageCodes[i].bytes[j] = tmp;
                                }
                                if (error_flag) break;
                            }

                            if (error_flag) continue;

                            int16_t crc = WaitForByte(5000, 250);

                            if (crc < 0) continue;
                            else
                            {
                                uint8_t checksum = numPages.bytes[1];
                                checksum ^= numPages.bytes[0];
                                for (int i = 0; i < (numPages.itself + 1); i++)
                                {
                                    checksum ^= pageCodes[i].bytes[1];
                                    checksum ^= pageCodes[i].bytes[0];
                                }

                                if (checksum == crc)
                                {
                                    // Is FLASH_CR register locked for writing?
                                    if (FLASH->CR & FLASH_CR_LOCK)
                                    {
                                        // Unlocking FLASH_CR register
                                        FLASH->KEYR = KEY1;
                                        FLASH->KEYR = KEY2;
                                    }

                                    printf("\t%sStarting erasing.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);

                                    // Erasing corresponding pages, except those containing bootloader itself
                                    for (int i = 0; i < (numPages.itself + 1); i++)
                                    {
                                        if (pageCodes[i].itself > BOOTLOADER_LAST_PAGE)  // Bootloader occupies first 4 pages (2k)
                                        {
                                            while (FLASH->SR & FLASH_SR_BSY1) printf("\t%sWaiting for releasing of FLASH memory.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
                                            
                                            // Clearing possible old errors
                                            FLASH->SR |= FLASH_SR_FASTERR & FLASH_SR_MISERR & FLASH_SR_PGSERR & FLASH_SR_SIZERR & FLASH_SR_PGAERR & FLASH_SR_PROGERR & FLASH_SR_OPERR;

                                            FLASH->CR &= ~FLASH_CR_PNB;
                                            FLASH->CR |= pageCodes[i].bytes[0] << FLASH_CR_PNB_Pos;

                                            FLASH->CR |= FLASH_CR_PER;
                                            
                                            FLASH->CR |= FLASH_CR_STRT;

                                            while (FLASH->SR & FLASH_SR_BSY1) printf("\t%sWaiting for page %d to be erased...\n", RTT_CTRL_TEXT_BRIGHT_GREEN, pageCodes[i].bytes[0]);

                                            printf("\t%sPage %d has been erased.\n", RTT_CTRL_TEXT_BRIGHT_GREEN, pageCodes[i].bytes[0]);

                                            FLASH->CR &= ~FLASH_CR_PER;
                                        }
                                    }

                                    printf("\t%sErasing is finished.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
                                    
                                    FLASH->CR |= FLASH_CR_LOCK;

                                    TransmitACK();
                                }
                                else TransmitNACK();
                            }
                        }
                    }
#endif
                    else if ((command == CMD_WRITE_MEMORY) && (command_xor == CMD_WRITE_MEMORY))
                    {
#ifdef DEBUG
                        printf("%sGot WRITE MEMORY command.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
                        // !!! Before sending ACK we must check RDP protection
                        TransmitACK();

                        int error_flag = 0;
                        int16_t tmp;
                        
                        memset(&u, 0, sizeof(u));

                        for (int i = 0; i < 5; i++)
                        {
                            tmp = WaitForByte(5000);
                            if (tmp < 0)
                            {
                                error_flag = 1;
                                break;
                            }
                            else u.address_and_crc[i] = tmp;
                        }

                        if (error_flag) continue;

                        uint8_t checksum = u.raw.address[0] ^ u.raw.address[1] ^ u.raw.address[2] ^ u.raw.address[3];

                        if (checksum == u.raw.crc)
                        {
                            TransmitACK();

                            int16_t bytesNumber = WaitForByte(5000);
                            if (bytesNumber < 0) continue;
                            {
                                if ((bytesNumber + 1) % 4)
                                {
#ifdef DEBUG
                                    printf("\t%sBytes number must by multiple of 4!\n", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
                                    TransmitNACK();
                                }
                                else
                                {
                                    union flashData DataToBeWritten;

                                    memset(DataToBeWritten.byteData, 0xFF, 256);

                                    error_flag = 0;

                                    for (int i = 0; i < (bytesNumber + 1); i++)
                                    {
                                        tmp = WaitForByte(5000);
                                        if (tmp < 0)
                                        {
                                            error_flag = 1;
                                            break;
                                        }
                                        else DataToBeWritten.byteData[i] = tmp;
                                    }

                                    if (BLUE_LED_PORT->ODR & GPIOx_ODR(GPIO_ODR_OD, BLUE_LED_PIN)) BLUE_LED_PORT->BRR = GPIOx_BRR(GPIO_BRR_BR, BLUE_LED_PIN);
                                    else BLUE_LED_PORT->BSRR = GPIOx_BSRR(GPIO_BSRR_BS, BLUE_LED_PIN);

                                    if (error_flag) continue;

                                    // Reading checksum of the above datapacket
                                    tmp = WaitForByte(5000);
                                    if (tmp < 0) continue;
                                    else
                                    {
                                        checksum = bytesNumber;
                                        for (int i = 0; i < (bytesNumber+1); i++) checksum ^= DataToBeWritten.byteData[i];

                                        if (checksum == tmp)
                                        {
                                            u.mem32.address = __REV(u.mem32.address);
                                            // We need to define whom belongs this memory, which we are going to write
                                            if ((u.mem32.address >= (FLASH_BASE + BOOTLOADER_PROGRAM_SIZE)) &&
                                               ((u.mem32.address + bytesNumber) < (FLASH_BASE + (FLASH_PAGE_NUMBER - MAIN_APP_PARAM_PAGE_NUM)*FLASH_PAGE_SIZE)))
                                            {
                                                // Here we already checked that bootloader is alowed to write in this memory area
                                                // Data is encrypted, so we must decrypt it

                                                union flashData decryptedData;
                                                memset(decryptedData.byteData, 0x00, 256);
#ifdef DEBUG
                                                printf("\t%sDecrypting %d bytes...", RTT_CTRL_TEXT_BRIGHT_GREEN, bytesNumber + 1);
#endif
                                                for (int i = 0; i < (bytesNumber + 1); i += 16) AES128_ECB_decrypt(DataToBeWritten.byteData + i, AES_KEY, decryptedData.byteData + i);

                                                // Is FLASH_CR register locked for writing?
                                                if (FLASH->CR & FLASH_CR_LOCK)
                                                {
                                                    // Unlocking FLASH_CR register
                                                    FLASH->KEYR = KEY1;
                                                    FLASH->KEYR = KEY2;
                                                }
#ifdef DEBUG
                                                printf("\t%sStarting programming of %d bytes.\n", RTT_CTRL_TEXT_BRIGHT_GREEN, bytesNumber + 1);
#endif                                      
                                                for (int i = 0; i < ((bytesNumber + 1)>>3); i++)
                                                {
#ifdef DEBUG
                                                    while (FLASH->SR & FLASH_SR_BSY1) printf("\t%sWaiting for releasing of FLASH memory.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#else
                                                    while (FLASH->SR & FLASH_SR_BSY1);
#endif

                                                    // Clearing possible old errors
                                                    FLASH->SR |= FLASH_SR_PGSERR & FLASH_SR_SIZERR & FLASH_SR_PGAERR & FLASH_SR_PROGERR;

                                                    FLASH_Program_DoubleWord(u.mem32.address + (i<<3), decryptedData.dwordData[i]);
#ifdef DEBUG
                                                    while (FLASH->SR & FLASH_SR_BSY1) printf("\t%sWaiting for the end of programming.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#else
                                                    while (FLASH->SR & FLASH_SR_BSY1);
#endif
#ifdef DEBUG
                                                    if (FLASH->SR & FLASH_SR_PGSERR & FLASH_SR_SIZERR & FLASH_SR_PGAERR & FLASH_SR_PROGERR) printf("\t%sProgramming of failed.\n", RTT_CTRL_TEXT_BRIGHT_RED);
                                                    else printf("\t%sProgramming of 8 bytes at address 0x%08X performed ok.\n", RTT_CTRL_TEXT_BRIGHT_GREEN, u.mem32.address + (i<<3));
#endif
                                                }
                                                
                                                // Clearing PG bit
                                                FLASH->CR &= ~FLASH_CR_PG;

                                                // Locking back FLASH_CR register
                                                FLASH->CR |= FLASH_CR_LOCK;

                                                TransmitACK();
                                            }
                                            else
                                            {
#ifdef DEBUG
                                                printf("%sForbidden address of 0x%08X for writing of %d bytes requested!\n", RTT_CTRL_TEXT_BRIGHT_RED, u.mem32.address, bytesNumber + 1);
#endif
                                                TransmitACK();
                                            }
                                        }
                                        else TransmitNACK();
                                    }
                                }
                            }
                        }
                        else TransmitNACK();
                    }
                    else
                    {
#ifdef DEBUG
                        printf("%sUnknown command 0x%02X!\n", RTT_CTRL_TEXT_BRIGHT_RED, command);
#endif
                        //if (BLUE_LED_PORT->ODR & GPIO_ODR_OD10) BLUE_LED_PORT->BRR = GPIO_BRR_BR10;
                        //else BLUE_LED_PORT->BSRR = GPIO_BSRR_BS10;

                        TransmitNACK();

                        if (msCounter - bl_startup_time > BOOTLOADER_TIME_TO_WAIT)
                        {
                          JumpToApplication();
                          bl_startup_time = msCounter;  // Appending more time for bootloader to start-up;
                          break;        // Going back to initialization stage
                        }
                    }
                } // else where we received second byte
            } // else where we received first byte
        } // Commands while(1) loop
    } // main while(1) loop, which waits for connection
} // int main

//-----------------------------------------------------------------------------

void JumpToApplication(void)
{
    // Enabling clocking for CRC module
    RCC->AHBENR |= RCC_AHBENR_CRCEN;
    // Resetting CRC unit and loading the content of INIT register into DR register
    CRC->CR |= CRC_CR_RESET;
    // Default POLYSIZE is 32 bit
    // Default POLYNOMIAL is 0x04C11DB7
    // Default CRC initial value is 0xFFFFFFFF
#ifdef DEBUG
    printf("%sStarting calculating FW CRC\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif      
    for (unsigned int i = 0; i < ((FLASH_PAGE_SIZE * (FLASH_PAGE_NUMBER - MAIN_APP_PARAM_PAGE_NUM)) - BOOTLOADER_PROGRAM_SIZE - CRC_LEN); i += 4)
    {
#ifdef DEBUG
        printf("\r%s\tAddress: 0x%08X ", RTT_CTRL_TEXT_BRIGHT_GREEN, MAIN_APP_START_ADDRESS + i);
#endif   
        /*if (!(i%256))
        {
            if (BLUE_LED_PORT->ODR & GPIO_ODR_OD2) BLUE_LED_PORT->BRR = GPIO_BRR_BR2;
            else BLUE_LED_PORT->BSRR = GPIO_BSRR_BS2;
            Delay(35); 
        }*/
        // Putting next 4 bytes into CRC calculation unit
        CRC->DR = __REV(*(uint32_t*)(MAIN_APP_START_ADDRESS + i));
    }
#ifdef DEBUG
    printf("%sDone!\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
#endif
    // Extracting CRC saved on last 4 bytes of the used FLASH memory
    uint32_t savedCRC = *(uint32_t*)(FLASH_BASE + (FLASH_PAGE_NUMBER - MAIN_APP_PARAM_PAGE_NUM)*FLASH_PAGE_SIZE - CRC_LEN);

    if (CRC->DR == savedCRC)
    {
#ifdef DEBUG
        printf("%s\tCRCs are equal.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
        Delay(500);
#endif
        // Disabling clocking for CRC module
        RCC->AHBENR &= ~RCC_AHBENR_CRCEN;
        // Resetting SysTick settings
        SysTick->CTRL = SysTick->LOAD = SysTick->VAL = 0;
        __disable_irq();

        // Changing vector table address to the beginning of the main application
        SCB->VTOR = MAIN_APP_START_ADDRESS;
        // Setting stack pointer
        __set_MSP(*(__IO uint32_t*)MAIN_APP_START_ADDRESS);
        __enable_irq();
        // Executing first command from main app
        ((void(*)(void))(*(__IO uint32_t*)(MAIN_APP_START_ADDRESS + 4)))();

        // In normal situation, program flow should never reach this loop
        while(1) TurnErrorLEDOn(125, -1);
    }
    else
    {
#ifdef DEBUG
        printf("%sCRC error!\n", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
        TurnErrorLEDOn(250, 5000);
        //return -1;
    }
}

//-----------------------------------------------------------------------------

void Delay(uint32_t D)
{
    volatile uint32_t tick_start = msCounter;
    while (msCounter - tick_start < D);
}

//-----------------------------------------------------------------------------

void TransmitACK(void)
{
    uint8_t TxData = CMD_ACK;
    Transmit(&TxData, 1);
}

//-----------------------------------------------------------------------------

void TransmitNACK(void)
{
    uint8_t TxData = CMD_NACK;
    Transmit(&TxData, 1);
}

//-----------------------------------------------------------------------------

/**
  * @brief  Program double-word (64-bit) at a specified address.
  * @param  Address Specifies the address to be programmed.
  * @param  Data Specifies the data to be programmed.
  * @retval None
  */
void FLASH_Program_DoubleWord(uint32_t Address, uint64_t Data)
{
      /* Set PG bit */
      SET_BIT(FLASH->CR, FLASH_CR_PG);

      /* Program first word */
      *(uint32_t *)Address = (uint32_t)Data;

      /* Barrier to ensure programming is performed in 2 steps, in right order
        (independently of compiler optimization behavior) */
      __ISB();

      /* Program second word */
      *(uint32_t *)(Address + 4U) = (uint32_t)(Data >> 32U);
}

//-----------------------------------------------------------------------------

void Transmit(uint8_t *pData, int numBytes)
{
    // Disabling Receiver
    //UART->CR1 &= ~USART_CR1_RE;
#ifdef DEBUG
    const int SCREEN_LIMIT = 40;

    printf("\t%sSending %d bytes: ", RTT_CTRL_TEXT_BRIGHT_GREEN, numBytes);
#endif
    for (int i = 0; i < numBytes; i++)
    {
        // Checking whether transmitter is free
        while (!(UART->ISR & USART_ISR_TXE_TXFNF));

        UART->TDR = pData[i];
#ifdef DEBUG
        if (i <= SCREEN_LIMIT) printf("%02X", pData[i]);
#endif
    }
#ifdef DEBUG
    if (numBytes > SCREEN_LIMIT) printf("...\n");
    else printf("\n");
#endif

    //while (!(UART->ISR & USART_ISR_TC));
    // Enabling Receiver
    //UART->CR1 |= USART_CR1_RE;
}

//-----------------------------------------------------------------------------

void SysTick_Handler(void)
{
    msCounter++;
}

//-----------------------------------------------------------------------------

void HardFault_Handler(void)
{
    TurnErrorLEDOn(500, -1);
}

//-----------------------------------------------------------------------------

void NMI_Handler(void)
{
    TurnErrorLEDOn(250, -1);
}

//-----------------------------------------------------------------------------

int16_t WaitForByte(unsigned int toWait)
{
    // Waiting until we receive one byte via UART
    unsigned int ledStartTime = msCounter;
    unsigned int loopStartTime = msCounter;

    while (!(UART->ISR & USART_ISR_RXNE_RXFNE))
    {
        if (msCounter - loopStartTime >= toWait) break;

        if (msCounter - ledStartTime >= BLUE_LED_PERIOD)
        {
            ledStartTime = msCounter;
            if (BLUE_LED_PORT->ODR & GPIOx_ODR(GPIO_ODR_OD, BLUE_LED_PIN)) BLUE_LED_PORT->BRR = GPIOx_BRR(GPIO_BRR_BR, BLUE_LED_PIN);
            else BLUE_LED_PORT->BSRR = GPIOx_BSRR(GPIO_BSRR_BS, BLUE_LED_PIN);
        }
    }

    // Checking if timeout occured
    if (msCounter - loopStartTime >= toWait) return -2;

    // Checking Overrun error
    if (UART->ISR & USART_ISR_ORE)
    {
#ifdef DEBUG
        printf("%sOverrun error is detected!\n", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
        UART->ICR = USART_ICR_ORECF;
        volatile int unused = UART->RDR & 0xFF;
        return -1;
    }
    if (UART->ISR & USART_ISR_NE)
    {
#ifdef DEBUG
        printf("%sNoise is detected!\n", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
        UART->ICR = USART_ICR_NECF;
        return -1;
    }
    if (UART->ISR & USART_ISR_FE)
    {
#ifdef DEBUG
        printf("%sFrame error is detected!\n", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
        UART->ICR = USART_ICR_FECF;
        return -1;
    }
    if (UART->ISR & USART_ISR_PE)
    {
#ifdef DEBUG
        printf("%sParity error is detected!\n", RTT_CTRL_TEXT_BRIGHT_RED);
#endif
        UART->ICR = USART_ICR_PECF;
        return -1;
    }

    return (UART->RDR & 0xFF);
}

//-----------------------------------------------------------------------------

void Init(void)
{
    // Systick timer initialization
    SysTick_Config(SystemCoreClock/1000);

#ifdef DEBUG
    printf(RTT_CTRL_CLEAR);
    printf("%sProgram started (%s)\n", RTT_CTRL_TEXT_BRIGHT_GREEN, FW_VERSION);
#endif
    
#ifdef DEBUG
    if (RCC->CR & RCC_CR_HSION) printf("%sHSI is turned on.\n", RTT_CTRL_TEXT_BRIGHT_GREEN);
    else
    {
        printf("%sHSI is not turned on! Program stopped!\n", RTT_CTRL_TEXT_BRIGHT_RED);
        TurnErrorLEDOn(500, -1);
    }
#else
    if (!(RCC->CR & RCC_CR_HSION)) TurnErrorLEDOn(500, -1);
#endif

#ifdef DEBUG
    if (RCC->CR & RCC_CR_HSIRDY) printf("HSI is ready.\n");
    else while (!(RCC->CR & RCC_CR_HSIRDY)) printf("%sWaiting for HSI to be ready!\n", RTT_CTRL_TEXT_YELLOW);
#else
    while (!(RCC->CR & RCC_CR_HSIRDY));
#endif
    //printf("Checking RDP level.\n");

    // Checking state of RDP protection
    if (((FLASH->OPTR & FLASH_OPTR_RDP) == 0xAA) || ((FLASH->OPTR & FLASH_OPTR_RDP) == 0xCC))
    {
#ifdef DEBUG
        // Checking whether SWD debugger is connected or not
        if (SWD_PORT->IDR & SWD_PIN_MSK)
        {
#endif
            // Debugger is not connected
            // Is FLASH_CR register locked for writing?
            if (FLASH->CR & FLASH_CR_LOCK)
            {
                // Unlocking FLASH_CR register
                FLASH->KEYR = KEY1;
                FLASH->KEYR = KEY2;
            }

            // Unlocking FLASH option register
            if (FLASH->CR & FLASH_CR_OPTLOCK)
            {
                FLASH->OPTKEYR = OPTKEY1;
                FLASH->OPTKEYR = OPTKEY2;
            }   

            // Setting RDP level to 1
            FLASH->OPTR &= ~FLASH_OPTR_RDP;
            FLASH->OPTR |= 0xEC;  // Any value except 0xAA and 0xCC

            while (FLASH->SR & FLASH_SR_BSY1);

            FLASH->CR |= FLASH_CR_OPTSTRT;

            while (FLASH->SR & FLASH_SR_BSY1);

            //printf("RDP is set!\n");

            // Updating option bytes by resetting of the device
            FLASH->CR |= FLASH_CR_OBL_LAUNCH;
        
            // If we can reach below code then something went wrong
            TurnErrorLEDOn(100, -1);
#ifdef DEBUG
        }
#endif
    }

    // Activating clocking for PORT A and PORT B
    RCC->IOPENR |= RCC_IOPENR_GPIOAEN | RCC_IOPENR_GPIOBEN | RCC_IOPENR_GPIODEN;

    // Setting PA9, PA10 and PA12 pins to Alternate Function mode (2)
    UART_PORT->MODER &= ~GPIOx_MODER(GPIO_MODER_MODE, UART_TX_PIN) &
			~GPIOx_MODER(GPIO_MODER_MODE, UART_RX_PIN) &
			~GPIOx_MODER(GPIO_MODER_MODE, UART_TX_EN_PIN);
    UART_PORT->MODER |= (2 << GPIOx_MODER_Pos(GPIO_MODER_MODE, UART_TX_PIN)) |
			(2 << GPIOx_MODER_Pos(GPIO_MODER_MODE, UART_RX_PIN)) |
			(2 << GPIOx_MODER_Pos(GPIO_MODER_MODE, UART_TX_EN_PIN));

    // Setting pullup for PA10 (RX pin)
    UART_PORT->PUPDR &= ~GPIOx_PUPDR(GPIO_PUPDR_PUPD, UART_RX_PIN);
    UART_PORT->PUPDR |= (1<<GPIOx_PUPDR_Pos(GPIO_PUPDR_PUPD, UART_RX_PIN));

    // Setting alternate function for PA9, PA10 and PA12 (UART dependant, see MCU datasheet file)
#if UART_RX_PIN > 7
    UART_PORT->AFR[1] &= ~GPIO_AFRH_AFSEL(GPIO_AFRH_AFSEL, UART_RX_PIN);
    UART_PORT->AFR[1] |= (UART_RX_ALT_FUNC_NUM << GPIO_AFRH_AFSEL_Pos(GPIO_AFRH_AFSEL, UART_RX_PIN));
#else
    UART_PORT->AFR[0] &= ~GPIO_AFRL_AFSEL(GPIO_AFRL_AFSEL, UART_RX_PIN);
    UART_PORT->AFR[0] |= (UART_RX_ALT_FUNC_NUM << GPIO_AFRH_AFSEL_Pos(GPIO_AFRH_AFSEL, UART_RX_PIN));
#endif
#if UART_TX_PIN > 7
    UART_PORT->AFR[1] &= ~GPIO_AFRH_AFSEL(GPIO_AFRH_AFSEL, UART_TX_PIN);
    UART_PORT->AFR[1] |= (UART_TX_ALT_FUNC_NUM << GPIO_AFRH_AFSEL_Pos(GPIO_AFRH_AFSEL, UART_TX_PIN));
#else
    UART_PORT->AFR[0] &= ~GPIO_AFRL_AFSEL(GPIO_AFRL_AFSEL, UART_TX_PIN);
    UART_PORT->AFR[0] |= (UART_TX_ALT_FUNC_NUM << GPIO_AFRH_AFSEL_Pos(GPIO_AFRH_AFSEL, UART_TX_PIN));
#endif
#if UART_TX_EN_PIN > 7
    UART_PORT->AFR[1] &= ~GPIO_AFRH_AFSEL(GPIO_AFRH_AFSEL, UART_TX_EN_PIN);
    UART_PORT->AFR[1] |= (UART_TXEN_ALT_FUNC_NUM << GPIO_AFRH_AFSEL_Pos(GPIO_AFRH_AFSEL, UART_TX_EN_PIN));
#else
    UART_PORT->AFR[0] &= ~GPIO_AFRL_AFSEL(GPIO_AFRL_AFSEL, UART_TX_EN_PIN);
    UART_PORT->AFR[0] |= (UART_TXEN_ALT_FUNC_NUM << GPIO_AFRH_AFSEL_Pos(GPIO_AFRH_AFSEL, UART_TX_EN_PIN));
#endif

    // Setting LED PORT to output for LED
    BLUE_LED_PORT->MODER &= ~GPIOx_MODER(GPIO_MODER_MODE, BLUE_LED_PIN);
    RED_LED_PORT->MODER &= ~GPIOx_MODER(GPIO_MODER_MODE, RED_LED_PIN);
    BLUE_LED_PORT->MODER |= (1 << GPIOx_MODER_Pos(GPIO_MODER_MODE, BLUE_LED_PIN));
    RED_LED_PORT->MODER |= (1 << GPIOx_MODER_Pos(GPIO_MODER_MODE, RED_LED_PIN));

    // Activating clocking for UART
    RCC->APBENR2 |= RCC_APBENR2_USART1EN;

    // Before changing some settings in USART we must make sure that it turned off
    UART->CR1 &= ~USART_CR1_UE;

    // Setting 1 start bit, 9 data bits
    UART->CR1 &= ~USART_CR1_M1;
    UART->CR1 |= USART_CR1_M0;

    // Setting oversampling to 16
    UART->CR1 &= ~USART_CR1_OVER8;

    // Enabling parity control with EVEN parity
    UART->CR1 |= USART_CR1_PCE;

    // Enabling transmitter and receiver
    UART->CR1 |= USART_CR1_TE | USART_CR1_RE;

    UART->CR1 |= USART_CR1_DEAT | USART_CR1_DEDT;

    //
    //UART->CR2 |= USART_CR2_SWAP;

    // Enabling RS485 transmitter driver control
    UART->CR3 |= USART_CR3_DEM;

#ifdef DEBUG
    // Setting baud rate to 19200
    UART->BRR = 833;
#else
    // Setting baud rate to 19200
    UART->BRR = 833;
#endif

    // Finally, turning USART on
    UART->CR1 |= USART_CR1_UE;

    // Start up sequence
    for (int i = 0; i < 5; i++)
    {
        BLUE_LED_PORT->BSRR = GPIOx_BSRR(GPIO_BSRR_BS, BLUE_LED_PIN);
        Delay(150);
        BLUE_LED_PORT->BSRR = GPIOx_BSRR(GPIO_BSRR_BR, BLUE_LED_PIN);
        Delay(150);
    }

}

//-----------------------------------------------------------------------------

void TurnErrorLEDOn(uint32_t bp, uint32_t hl)
{
    // Turning off blue led
    BLUE_LED_PORT->BSRR = GPIOx_BSRR(GPIO_BSRR_BR, BLUE_LED_PIN);
    uint32_t tickStart = msCounter;

    while(msCounter - tickStart < hl)
    {
        RED_LED_PORT->BSRR = GPIOx_BSRR(GPIO_BSRR_BS, RED_LED_PIN);
        Delay(bp);
        RED_LED_PORT->BSRR = GPIOx_BSRR(GPIO_BSRR_BR, RED_LED_PIN);
        Delay(bp);
    }
}

/*************************** End of file ****************************/