#include "stm32load.h"
#include "stm32f4xx.h"
#include "myassert.h"
#include "serial.h"
#include "systime.h"
#include "pt.h"
#include "uart.h"
#include "../../pilot/pilot.h"
#include "../../dfb/dfb.h"
#include "../../sbs/sbs.h"
#include <string.h>
#include <stdio.h>
#include <stdbool.h>

#define NSLAVES 3
#define ACK 0x79

struct mem_op_data
{
int addr, len;
uint8_t* ptr;
};

static uint8_t rxfifo0[256];
static uint8_t rxfifo1[256];
static uint8_t rxfifo2[64];
static uint8_t txfifo[NSLAVES][64];
static struct uart_idx idx[NSLAVES];
static struct pt boot_pt[NSLAVES];
static int slave = 0;
static bool burning;

const struct uart_config uart3 =
{
.rxfifo = rxfifo0,
.rxsz = sizeof rxfifo0,
.txfifo = txfifo[0],
.txsz = sizeof txfifo[0],
.idx = &idx[0],
.regs = USART3,
.rxport = GPIOB,
.rxpin = 11,
.rxaf = 7,
.txport = GPIOB,
.txpin = 10,
.txaf = 7,
.irq = USART3_IRQn,
.prio = 7,
.apb_clock = SYSTIME_TPS / 4,
.baudrate = 57600,
};
const struct uart_config uart6 =
{
.rxfifo = rxfifo1,
.rxsz = sizeof rxfifo1,
.txfifo = txfifo[1],
.txsz = sizeof txfifo[1],
.idx = &idx[1],
.regs = USART6,
.rxport = GPIOC,
.rxpin = 7,
.rxaf = 8,
.txport = GPIOC,
.txpin = 6,
.txaf = 8,
.irq = USART6_IRQn,
.prio = 7,
.apb_clock = SYSTIME_TPS / 2,
.baudrate = 115200,
};
const struct uart_config uart4 =
{
.rxfifo = rxfifo2,
.rxsz = sizeof rxfifo2,
.txfifo = txfifo[2],
.txsz = sizeof txfifo[2],
.idx = &idx[2],
.regs = UART4,
.rxport = GPIOC,
.rxpin = 11,
.rxaf = 8,
.txport = GPIOC,
.txpin = 10,
.txaf = 8,
.irq = UART4_IRQn,
.prio = 7,
.apb_clock = SYSTIME_TPS / 4,
.baudrate = 115200,
};
static const struct uart_config* const uart[NSLAVES] =
{
&uart3, &uart6, &uart4
};

void
usart3_handler(void)
{
uart_handler(&uart3);
}

void
usart6_handler(void)
{
uart_handler(&uart6);
}

void
uart4_handler(void)
{
uart_handler(&uart4);
}

void
stm32load_init(void)
{
/*
// reset line for the pilot MCU, enable pull-up, configure as open-drain
GPIOD->BSRR = 1 << 11;
GPIOD->OTYPER |= GPIO_OTYPER_OT_11;
GPIOD->PUPDR |= GPIO_PUPDR_PUPDR11_0;
GPIOD->MODER |= GPIO_MODER_MODER11_0;
// reset line for the DFB MCU, configure as GPO
GPIOG->BSRR = 1 << 7;
GPIOG->OTYPER |= GPIO_OTYPER_OT_7;
GPIOG->PUPDR |= GPIO_PUPDR_PUPDR7_0;
GPIOG->MODER |= GPIO_MODER_MODER7_0;
// reset line for the SBS MCU, configure as GPO
GPIOA->BSRR = 1 << 11;
GPIOA->OTYPER |= GPIO_OTYPER_OT_11;
GPIOA->PUPDR |= GPIO_PUPDR_PUPDR11_0;
GPIOA->MODER |= GPIO_MODER_MODER11_0;
*/
for (int i = 0; i < NSLAVES; i++)
{
uart_init(uart[i], true);
PT_INIT(&boot_pt[i]);
}
}

static
PT_THREAD(getbyte_thread(struct pt* pt, int* ret))
{
static unsigned int start[NSLAVES];
PT_BEGIN(pt);
start[slave] = systime_ticks();
do
{
PT_YIELD(pt);
if (uart_rxcount(uart[slave]) > 0)
{
*ret = uart_getbyte(uart[slave]);
PT_EXIT(pt);
}
}
while (systime_ticks() - start[slave] < SYSTIME_TPS / 10);
*ret = -1;
PT_END(pt);
}

static void
send_address(int addr)
{
int csum = 0;
for (int j = 24; j >= 0; j -= 8)
{
uart_putbyte(uart[slave], addr >> j);
csum ^= addr >> j;
}
uart_putbyte(uart[slave], csum);
}

static
PT_THREAD(readmem_thread(struct pt* pt, struct mem_op_data* md, bool* success))
{
static struct pt slave_pt[NSLAVES];
int byte;
*success = false;
PT_BEGIN(pt);
// send command code
uart_putbytes(uart[slave], "\x11\xEE", 2);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
PT_EXIT(pt);
}
send_address(md->addr);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
PT_EXIT(pt);
}
// send number of bytes
uart_putbyte(uart[slave], md->len - 1);
uart_putbyte(uart[slave], ~(md->len - 1));
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
PT_EXIT(pt);
}
// receive data
static int i[NSLAVES];
for (i[slave] = 0; i[slave] < md->len; i[slave]++)
{
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte < 0)
{
PT_EXIT(pt);
}
md->ptr[i[slave]] = byte;
}
*success = true;
PT_END(pt);
}

static
PT_THREAD(writemem_thread(struct pt* pt, struct mem_op_data* md, bool* success))
{
static struct pt slave_pt[NSLAVES];
int byte;
*success = false;
PT_BEGIN(pt);
// send command code
uart_putbytes(uart[slave], "\x31\xCE", 2);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
PT_EXIT(pt);
}
send_address(md->addr);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
PT_EXIT(pt);
}
// send number of bytes
uart_putbyte(uart[slave], md->len - 1);
static int i[NSLAVES], csum[NSLAVES];
csum[slave] = md->len - 1;
for (i[slave] = 0; i[slave] < md->len; i[slave]++)
{
PT_WAIT_WHILE(pt, uart_txcount(uart[slave]) == 0);
uart_putbyte(uart[slave], md->ptr[i[slave]]);
csum[slave] ^= md->ptr[i[slave]];
}
uart_putbyte(uart[slave], csum[slave]);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
PT_EXIT(pt);
}
*success = true;
PT_END(pt);
}

static
PT_THREAD(erase_thread(struct pt* pt, int sector, bool* success))
{
static struct pt slave_pt[NSLAVES];
static unsigned int start[NSLAVES];
int byte;
*success = false;
PT_BEGIN(pt);
// send command code
uart_putbytes(uart[slave], "\x44\xBB", 2);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
PT_EXIT(pt);
}
char buf[5] = "\0\0\0";
buf[3] = buf[4] = sector;
uart_putbytes(uart[slave], buf, 5);
start[slave] = systime_ticks();
do
{
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
}
while (byte < 0 && systime_ticks() - start[slave] < 5 * SYSTIME_TPS);
if (byte != ACK)
{
PT_EXIT(pt);
}
*success = true;
PT_END(pt);
}

static
PT_THREAD(boot_thread(struct pt* pt))
{
static unsigned int i[NSLAVES], attempt[NSLAVES];
static struct pt slave_pt[NSLAVES];
int byte;
bool success;
PT_BEGIN(pt);
static const int reset_mask[NSLAVES] = {
SHREG2_RSTPLT, SHREG2_RSTDFB, SHREG2_RSTSBS
};
reset:
// assert reset
shiftreg2_modify(reset_mask[slave], 0);
// deassert reset
shiftreg2_modify(0, reset_mask[slave]);
// initial delay, wait for bootloader to become ready
i[slave] = systime_ticks();
PT_WAIT_WHILE(pt, systime_ticks() - i[slave] < SYSTIME_TPS / 5);
uart_flush(uart[slave]);
// send 0x7F for automatic baud rate detection
uart_putbyte(uart[slave], 0x7F);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
goto error;
}
// synchronization successful
static uint8_t buf[0x24];
static bool buflock;
static struct mem_op_data md[NSLAVES];
// read first 0x24 bytes
md[slave].addr = 0x08000000;
md[slave].len = 0x24;
md[slave].ptr = buf;
PT_WAIT_WHILE(pt, buflock);
buflock = true;
PT_SPAWN(pt, &slave_pt[slave], readmem_thread(&slave_pt[slave], &md[slave], &success));
buflock = false;
if (!success)
{
goto error;
}
// compare with same in image
static const uint8_t* const image[NSLAVES] = { image_pilot, image_dfb, image_sbs };
static const int image_size[NSLAVES] = { sizeof image_pilot, sizeof image_dfb, sizeof image_sbs };
if (memcmp(buf, image[slave], 0x24) != 0)
{
goto burn;
}
// read last 16 bytes
md[slave].addr = 0x08000000 + image_size[slave] - 16;
md[slave].len = 16;
md[slave].ptr = buf;
PT_WAIT_WHILE(pt, buflock);
buflock = true;
PT_SPAWN(pt, &slave_pt[slave], readmem_thread(&slave_pt[slave], &md[slave], &success));
buflock = false;
if (!success)
{
goto error;
}
// compare with same in image
if (memcmp(buf, image[slave] + image_size[slave] - 16, 16) != 0)
{
goto burn;
}
else
{
goto done;
}
burn:
burning = true;
if (slave == 0)
{
// set voltage range to speed up flash erase/programming
md[slave].addr = 0xFFFF0000;
md[slave].len = 1;
md[slave].ptr = (void*)"\x03";
PT_SPAWN(pt, &slave_pt[slave], writemem_thread(&slave_pt[slave], &md[slave], &success));
if (!success)
{
goto error;
}
}
static const int sector_size[NSLAVES][16] = {
{ 0x4000, 0x4000, 0x4000, 0x4000, 0x10000, 0x20000, 0x20000, 0x20000, 0x20000, 0x20000, 0x20000, 0x20000 },
{ 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024 },
{ 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024 }
};
static int sector[NSLAVES], upper_boundary[NSLAVES];
// start burning from address 0x24 up, save first 0x24 bytes for last
i[slave] = 0x24;
sector[slave] = 0;
upper_boundary[slave] = 0;
while (i[slave] < image_size[slave])
{
upper_boundary[slave] += sector_size[slave][sector[slave]];
upper_boundary[slave] = (upper_boundary[slave] > image_size[slave]) ?
image_size[slave] : upper_boundary[slave];
// erase sector
PT_SPAWN(pt, &slave_pt[slave], erase_thread(&slave_pt[slave], sector[slave], &success));
if (!success)
{
goto error;
}
// program firmware
while (i[slave] < upper_boundary[slave])
{
int len;
len = upper_boundary[slave] - i[slave];
len = (len > 256) ? 256 : len;
md[slave].addr = 0x08000000 | i[slave];
md[slave].len = len;
md[slave].ptr = (void*)&image[slave][i[slave]];
i[slave] += len;
PT_SPAWN(pt, &slave_pt[slave], writemem_thread(&slave_pt[slave], &md[slave], &success));
if (!success)
{
goto error;
}
}
sector[slave]++;
}
// program last chunk
md[slave].addr = 0x08000000;
md[slave].len = 0x24;
md[slave].ptr = (void*)image[slave];
PT_SPAWN(pt, &slave_pt[slave], writemem_thread(&slave_pt[slave], &md[slave], &success));
if (!success)
{
goto error;
}
// verify
i[slave] = 0;
while (i[slave] < image_size[slave])
{
static uint8_t buf[256];
int len = (i[slave] + 256 > image_size[slave]) ? image_size[slave] - i[slave] : 256;
md[slave].addr = 0x08000000 + i[slave];
md[slave].len = len;
md[slave].ptr = buf;
PT_SPAWN(pt, &slave_pt[slave], readmem_thread(&slave_pt[slave], &md[slave], &success));
if (memcmp(buf, image[slave] + i[slave], md[slave].len) != 0)
{
goto error;
}
i[slave] += md[slave].len;
}
done:
// issue Go command
uart_putbytes(uart[slave], "\x21\xDE", 2);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
goto error;
}
uart_putbytes(uart[slave], "\x08\x00\x00\x00\x08", 5);
PT_SPAWN(pt, &slave_pt[slave], getbyte_thread(&slave_pt[slave], &byte));
if (byte != ACK)
{
goto error;
}
uart_parityoff(uart[slave]);
uart_setrate(uart[slave], 115200);
PT_EXIT(pt);
error:
if (attempt[slave]++ < 3)
{
goto reset;
}
myassert(0);
PT_END(pt);
}

bool
stm32load_poll(void)
{
static bool done[NSLAVES];
static int done_count;
if (!done[slave])
{
if (PT_SCHEDULE(boot_thread(&boot_pt[slave])) == 0)
{
done[slave] = true;
done_count++;
}
}
slave = (slave + 1) % NSLAVES;
return done_count == NSLAVES;
}

bool
stm32load_burn(void)
{
return burning;
}

PT_THREAD...