/*
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <unicore-mx/stm32/rcc.h>
#include <unicore-mx/stm32/gpio.h>
#include <string.h>
#include <stdio.h>
#define DMB() __asm__ volatile ("dmb")

#define FLASH_BASE          (0x40022000)
#define FLASH_ACR           (*(volatile uint32_t *)(FLASH_BASE + 0x00))

#define LED_GREEN_PIN GPIO5
#define LED_GREEN_PORT GPIOA

#define LED_RED_PIN GPIO4
#define LED_RED_PORT GPIOA

#define I2C3_SDA_PIN GPIO1
#define I2C3_SDA_PORT GPIOC
#define I2C3_SCL_PIN GPIO0
#define I2C3_SCL_PORT GPIOC
#define I2C3_AF (7)

#define I2C3_BASE 0x40007800
#define I2C3 I2C3_BASE
#define I2C I2C3


#define I2C3_CR1        (*(volatile uint32_t *)(I2C3))
#define I2C3_CR2        (*(volatile uint32_t *)(I2C3 + 0x04))
#define I2C3_OAR1       (*(volatile uint32_t *)(I2C3 + 0x08))
#define I2C3_OAR2       (*(volatile uint32_t *)(I2C3 + 0x0c))
#define I2C3_TIMINGR    (*(volatile uint32_t *)(I2C3 + 0x10))
#define I2C3_SR1        (*(volatile uint32_t *)(I2C3 + 0x14))
#define I2C3_ISR        (*(volatile uint32_t *)(I2C3 + 0x18))
#define I2C3_ICR        (*(volatile uint32_t *)(I2C3 + 0x1C))
#define I2C3_TXDR       (*(volatile uint32_t *)(I2C3 + 0x28))
#define I2C3_RXDR       (*(volatile uint32_t *)(I2C3 + 0x24))

#define I2C_CR1_ENABLE               (1 << 0)
#define I2C_CR1_DNF                  (1 << 8)
#define I2C_CR1_ANFOFF               (1 << 12)
#define I2C_CR1_NOSTRETCH            (1 << 17)

#define I2C_CR2_START			     (1 << 13)
#define I2C_CR2_STOP			     (1 << 14)
#define I2C_CR2_RD_WRN			     (1 << 10)
#define I2C_CR2_NBYTES_SHIFT         (16)
#define I2C_CR2_RELOAD               (1 << 24)
#define I2C_CR2_AUTOEND              (1 << 25)
#define I2C_CR1_ACK			         (1 << 10)
#define I2C_CR2_FREQ_MASK            (0x3ff)
#define I2C_CCR_MASK                 (0xfff)

#define I2C_ISR_TXIS                (1 << 1)
#define I2C_ISR_TXE                 (1 << 0)
#define I2C_ISR_RXNE                (1 << 2)
#define I2C_ISR_NACKF               (1 << 4)
#define I2C_ISR_TCR                 (1 << 7)

#define I2C_ICR_ARLOCF              (1 << 9)
#define I2C_ICR_BERRCF              (1 << 8)
#define I2C_ICR_ADDRCF              (1 << 3)
#define I2C_ICR_NACKF               (1 << 4)
#define I2C_ICR_STOPF               (1 << 5)
#define I2C_ICR_ALLCF (I2C_ICR_NACKF | I2C_ICR_ARLOCF | I2C_ICR_BERRCF | I2C_ICR_ADDRCF )

#define RCC_CFGR_PLLDIV2            (0x01 << 22)
#define RCC_CFGR_PLLMUL4            (0x01 << 18)
#define RCC_PRESCALER_DIV_NONE 0


#define ROM_SRC 0x50
#define ROM_DST 0x51
#define ROM_TEST_NACK 0x52
#define ROM_PAGE_SIZE (32)
#define ROM_SIZE (8192)
#define ROM_PAGES (ROM_SIZE / ROM_PAGE_SIZE)

static void wait_a_bit(void)
{
    volatile int i;
    for (i = 0; i < 30000; i++) {  /* Wait a bit. */
        __asm__("nop");
    }
}

static void i2c_acquire(void)
{
    rcc_periph_clock_enable(RCC_I2C3);
    I2C3_CR1 |= I2C_CR1_ENABLE;
}

static void i2c_release(void)
{
    I2C3_CR1 &= ~(I2C_CR1_ENABLE);
    rcc_periph_clock_disable(RCC_I2C3);
}

static void i2c_setup(void)
{
    /* 400KHz */
    uint32_t presc = 0, scll = 0x2E, sclh = 0x11, sdadel = 0x01, scldel = 0x0b;
    /* 100KHz */
    //uint32_t presc = 1, scll = 0x56, sclh = 0x3e, sdadel = 0x01, scldel = 0x0a;
    rcc_periph_clock_enable(RCC_GPIOC);
    gpio_set_output_options(GPIOC, GPIO_OTYPE_OD, GPIO_OSPEED_HIGH, I2C3_SDA_PIN);
    gpio_mode_setup(GPIOC, GPIO_MODE_AF, GPIO_PUPD_PULLUP,
            I2C3_SDA_PIN | I2C3_SCL_PIN);
    gpio_set_af(GPIOC, I2C3_AF, I2C3_SDA_PIN | I2C3_SCL_PIN);

    i2c_acquire();

    /* Disable */
    I2C3_CR1 &= ~(I2C_CR1_ENABLE);

    /* configure analog noise filter */
    I2C3_CR1 |= I2C_CR1_ANFOFF;

    /* configure digital noise filter */
    I2C3_CR1 |= I2C_CR1_DNF;

    /* set timing registers */
    I2C3_TIMINGR = (presc << 28) | (scldel << 20) | (sdadel << 16) | (sclh << 8) | scll;

    /* configure clock stretching */
    I2C3_CR1 &= ~(I2C_CR1_NOSTRETCH);

    /* Clear interrupt */
    I2C3_ICR |= I2C_ICR_ALLCF;

    i2c_release();

}

static void gpio_setup(void)
{
    /* Enable GPIO clock. */
    rcc_periph_clock_enable(RCC_GPIOA);

    /* set pins to output mode, push pull */
    gpio_mode_setup(LED_GREEN_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED_GREEN_PIN);
    gpio_mode_setup(LED_RED_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED_RED_PIN);
}

static void wait_sent(void)
{
    volatile uint32_t sr1;
    do {
        sr1 = I2C3_ISR;
    } while ((sr1 & (I2C_ISR_TXE)) == 0);
}

static void wait_reload(void)
{
    volatile uint32_t sr1;
    do {
        sr1 = I2C3_ISR;
    } while ((sr1 & (I2C_ISR_TCR)) == 0);
}

static void wait_start(void)
{
    volatile uint32_t cr2;
    do {
        cr2 = I2C3_CR2;
    } while ((cr2 & (I2C_CR2_START)) != 0);
}

static void wait_stop(void)
{
    volatile uint32_t cr2;
    do {
        cr2 = I2C3_CR2;
    } while ((cr2 & (I2C_CR2_STOP)) != 0);
}

static void wait_rxd(void)
{
    volatile uint32_t sr1;
    do {
        sr1 = I2C3_ISR;
    } while ((sr1 & (I2C_ISR_RXNE)) == 0);
}

static void clear_err(void)
{
    I2C3_ICR |= I2C_ICR_ALLCF;
}

static void clear_stop(void)
{
    I2C3_ICR |= I2C_ICR_STOPF;
}

static int eeprom_write_page(uint8_t rom, uint16_t address, const uint8_t *buf)
{
    volatile uint32_t sr1, sr2, cr2;
    int i;
    if (rom != ROM_DST)
        return 0;


    clear_err();
    I2C3_CR2 = ((ROM_PAGE_SIZE + 2) << I2C_CR2_NBYTES_SHIFT) | I2C_CR2_AUTOEND;
    I2C3_CR2 |= (rom << 1);
    I2C3_CR2 |= I2C_CR2_START;
    wait_start();
    I2C3_TXDR = ((uint8_t)((address >> 8) & (0xFF)));
    wait_sent();
    I2C3_TXDR = ((uint8_t)((address & 0xFF)));
    for (i = 0; i < ROM_PAGE_SIZE; i++) {
        wait_sent();
        I2C3_TXDR = buf[i];
    } 
    wait_a_bit();
    clear_stop();
    return i;
}

static int eeprom_read_page(uint8_t rom, uint16_t address, uint8_t *buf)
{
    volatile uint32_t sr1, sr2;
    int i;
    clear_err();
    I2C3_CR2 = (2 << I2C_CR2_NBYTES_SHIFT) | I2C_CR2_RELOAD;
    I2C3_CR2 |= (rom << 1);
    I2C3_CR2 |= I2C_CR2_START;
    wait_start();
    I2C3_TXDR = ((uint8_t)((address & 0xFF00) >> 8));
    wait_sent();
    clear_err();
    I2C3_CR2 = (rom << 1);
    I2C3_CR2 |= I2C_CR2_RD_WRN;
    I2C3_CR2 |= (ROM_PAGE_SIZE << I2C_CR2_NBYTES_SHIFT);
    I2C3_CR2 |= I2C_CR2_START;
    I2C3_TXDR = ((uint8_t)((address & 0xFF)));
    //wait_sent();
    wait_start();
    clear_err();

    for (i = 0; i < ROM_PAGE_SIZE; i++) {
        //I2C3_TXDR = 0xFF;
        //wait_sent();
        wait_rxd();
        buf[i] = I2C3_RXDR;
    }
    I2C3_CR2 |= I2C_CR2_STOP;
    clear_stop();
    return i;
}

static int eeprom_read_cur(uint8_t rom, uint8_t *buf)
{
    int i;
retry:
    clear_err();
    I2C3_CR2 = (ROM_PAGE_SIZE << I2C_CR2_NBYTES_SHIFT);
    I2C3_CR2 |= (rom << 1) | I2C_CR2_RD_WRN;
    I2C3_CR2 |= I2C_CR2_START;
    wait_start();
    if (I2C3_ISR & I2C_ISR_NACKF) {
        wait_a_bit();
        clear_stop();
        goto retry;
    }
    for (i = 0; i < ROM_PAGE_SIZE; i++) {
        wait_rxd();
        buf[i] = I2C3_RXDR;
    }
    return i;
}


static void flash_set_waitstates(unsigned int waitstates)
{
    if (waitstates && ((FLASH_ACR & 1) == 0))
        FLASH_ACR |=  1;
    if (!waitstates && ((FLASH_ACR & 1) == 1))
        FLASH_ACR &= 1;
    while ((FLASH_ACR & 1) != waitstates)
        ;
}

static void clock_pll_on(void)
{
    uint32_t reg32;
    uint32_t cpu_freq, hsi_freq, hpre, ppre1, ppre2, flash_waitstates;

    /* Enable Power controller */
    rcc_periph_clock_enable(RCC_PWR);

    /* Select clock parameters (CPU Speed = 32MHz) */
    cpu_freq = 32000000;
    hsi_freq = 16000000;
    hpre  = RCC_PRESCALER_DIV_NONE;
    ppre1 = RCC_PRESCALER_DIV_NONE;
    ppre2 = RCC_PRESCALER_DIV_NONE;
    flash_waitstates = 1;

    flash_set_waitstates(flash_waitstates);

    /* Enable internal high-speed oscillator. */
    RCC_CR |= RCC_CR_HSI16ON;
    DMB();
    while ((RCC_CR & RCC_CR_HSI16RDY) == 0) {};

    /* Select HSI as SYSCLK source. */
    reg32 = RCC_CFGR;
    reg32 &= ~((1 << 1) | (1 << 0));
    RCC_CFGR = (reg32 | RCC_CFGR_SW_HSI16);
    DMB();

    /*
     * Set prescalers for AHB, ADC, ABP1, ABP2.
     */
    reg32 = RCC_CFGR;
    reg32 &= ~(0xF << 4);
    RCC_CFGR = (reg32 | (hpre << 4));
    DMB();
    reg32 = RCC_CFGR;
    reg32 &= ~(0x07 << 8);
    RCC_CFGR = (reg32 | (ppre1 << 8));
    DMB();
    reg32 &= ~(0x07 << 11);
    RCC_CFGR = (reg32 | (ppre2 << 11));
    DMB();
    reg32 &= ~(0x0F << 18);
    RCC_CFGR = (reg32 | RCC_CFGR_PLLMUL4);
    DMB();
    reg32 &= ~(0x03 << 22);
    RCC_CFGR = (reg32 | RCC_CFGR_PLLDIV2);
    DMB();
    /* Enable PLL oscillator and wait for it to stabilize. */
    RCC_CR |= RCC_CR_PLLON;
    DMB();
    while ((RCC_CR & RCC_CR_PLLRDY) == 0) {};

    /* Select PLL as SYSCLK source. */
    reg32 = RCC_CFGR;
    reg32 &= ~((1 << 1) | (1 << 0));
    RCC_CFGR = (reg32 | RCC_CFGR_SW_PLL);
    DMB();

    /* Wait for PLL clock to be selected. */
    while (((RCC_CFGR >> 2) & 0x03) != RCC_CFGR_SW_PLL)
        ;
}


static uint8_t cache[ROM_PAGE_SIZE];
static uint8_t vcache[ROM_PAGE_SIZE];

int main(void)
{
    int i;
    clock_pll_on();

    gpio_setup();
    gpio_set(LED_RED_PORT, LED_RED_PIN);
    i2c_setup();
    wait_a_bit();
    wait_a_bit();
    wait_a_bit();
    i2c_acquire();


#if 0
    /* TEST - fill the DST */
    for (i = 0; i < ROM_PAGES; i++) {
        memset(cache, (uint8_t)i, 32);
        eeprom_write_page(ROM_DST,i * ROM_PAGE_SIZE,cache);
    }
#endif
    memset(cache, 0, 32);
    for (i = 0; i < ROM_PAGES; i++) {
        /* Green: Read */
        gpio_set(LED_RED_PORT, LED_GREEN_PIN);
        gpio_clear(LED_GREEN_PORT, LED_GREEN_PIN);
        eeprom_read_page(ROM_SRC, i * ROM_PAGE_SIZE, cache);
        eeprom_read_page(ROM_DST, i * ROM_PAGE_SIZE, vcache);
        if(memcmp(vcache, cache, ROM_PAGE_SIZE) == 0) {
            /* Skip sector, already matching. */
            continue;
        }
        /* Red: Write */
        gpio_set(LED_GREEN_PORT, LED_GREEN_PIN);
        gpio_clear(LED_RED_PORT, LED_RED_PIN);
        eeprom_write_page(ROM_DST, i * ROM_PAGE_SIZE, cache);

        /* Both: verify */
        gpio_clear(LED_GREEN_PORT, LED_GREEN_PIN);
        eeprom_read_page(ROM_DST, i * ROM_PAGE_SIZE, vcache);
        if(memcmp(vcache, cache, ROM_PAGE_SIZE) != 0) {
            while(1) {
                /* blink red: panic */
                gpio_toggle(LED_RED_PORT, LED_RED_PIN);
                for (i = 0; i < 10; i++)
                    wait_a_bit();
            }
        }
        gpio_set(LED_GREEN_PORT, LED_GREEN_PIN);
        gpio_set(LED_RED_PORT, LED_RED_PIN);
    }
    i2c_release();

    while (1) {
        /* blink green: success */
        gpio_set(LED_RED_PORT, LED_RED_PIN);
        gpio_toggle(LED_GREEN_PORT, LED_GREEN_PIN);
        for (i = 0; i < 10; i++)
            wait_a_bit();
    }

    return 0;
}