|
|
|
@ -10,6 +10,7 @@ |
|
|
|
#include "tick.h" |
|
|
|
#include "timer.h" |
|
|
|
#include "iwdg.h" |
|
|
|
#include "utils.h" |
|
|
|
|
|
|
|
#define LED_D3 (PI13) |
|
|
|
#define LED_D4 (PK5) |
|
|
|
@ -19,6 +20,34 @@ |
|
|
|
#define PKT_LEN (64) |
|
|
|
#define PKT_NLEMS (128) /* must power of 2 */ |
|
|
|
|
|
|
|
#define SEND_PKT_HDR (0x61) |
|
|
|
#define SEND_PKT_TAL (0x2f) |
|
|
|
#define SEND_PKT_TYPE_DATA (0x01) |
|
|
|
#define SEND_PKT_TYPE_CMD (0x02) |
|
|
|
#define SEND_PKT_LEN (40) |
|
|
|
|
|
|
|
#define RAW_PKT_LEN (18) |
|
|
|
#define RAW_PKT_OFF (6) |
|
|
|
|
|
|
|
#define BIT0 (0x01) |
|
|
|
#define BIT1 (0x02) |
|
|
|
|
|
|
|
#define RS422_BAUDRATE (115200) |
|
|
|
/* UART 4 */ |
|
|
|
#define RS422_0 (&serial3) |
|
|
|
/* USART 6 */ |
|
|
|
#define RS422_1 (&serial5) |
|
|
|
#define TIM_40MS (&timer2) |
|
|
|
|
|
|
|
struct tag_fifo { |
|
|
|
char *pnew; /* points to newest fifo element */ |
|
|
|
char *pold; /* points to oldest fifo element */ |
|
|
|
short nelem; // number of bytes currently in fifo
|
|
|
|
short blen; |
|
|
|
char *bend; |
|
|
|
char *b; |
|
|
|
}; |
|
|
|
|
|
|
|
struct ringbuf { |
|
|
|
int size; |
|
|
|
int len; |
|
|
|
@ -29,12 +58,32 @@ struct frames { |
|
|
|
struct ringbuf slices[PKT_NLEMS]; |
|
|
|
uint32_t head; |
|
|
|
uint32_t tail; |
|
|
|
struct tag_fifo f; |
|
|
|
}; |
|
|
|
|
|
|
|
static struct frames rx0, rx1; |
|
|
|
static volatile int to_play = 0; |
|
|
|
static struct frames rx[2], tx[2]; |
|
|
|
static uint8_t __rxbuf[2][PKT_LEN * 2]; |
|
|
|
|
|
|
|
static volatile unsigned int to_decode = 0; |
|
|
|
static volatile unsigned int to_send = 0; |
|
|
|
|
|
|
|
static void init_frames(struct frames *f) |
|
|
|
static void wait_to_reset() |
|
|
|
{ |
|
|
|
ticks_close(); |
|
|
|
timer_close(TIM_40MS); |
|
|
|
|
|
|
|
serial_close(&serial0); |
|
|
|
serial_close(RS422_0); |
|
|
|
serial_close(RS422_1); |
|
|
|
|
|
|
|
a3k_close(&a3k0); |
|
|
|
a3k_close(&a3k1); |
|
|
|
|
|
|
|
for (;;) |
|
|
|
; |
|
|
|
} |
|
|
|
|
|
|
|
static void init_frames(struct frames *f, void *b, int blen) |
|
|
|
{ |
|
|
|
int i; |
|
|
|
struct ringbuf *rb; |
|
|
|
@ -44,8 +93,82 @@ static void init_frames(struct frames *f) |
|
|
|
rb = &f->slices[i]; |
|
|
|
rb->size = PKT_LEN; |
|
|
|
} |
|
|
|
|
|
|
|
f->f.pnew = f->f.pold = b; |
|
|
|
f->f.blen = blen; |
|
|
|
f->f.bend = b + blen; |
|
|
|
f->f.b = b; |
|
|
|
f->f.nelem = 0; |
|
|
|
} |
|
|
|
|
|
|
|
static void encode_packet(a3k_t from, struct frames *f) |
|
|
|
{ |
|
|
|
struct ringbuf *rb; |
|
|
|
int pos; |
|
|
|
if (a3k_checkfullpacket(from)) { |
|
|
|
if (a3k_peekpackettype(from) == PKT_CHANNEL) { /* only cache the channel packet */ |
|
|
|
pos = f->tail & (PKT_NLEMS - 1); |
|
|
|
++f->tail; |
|
|
|
rb = &f->slices[pos]; |
|
|
|
rb->len = a3k_getpacket(from, rb->rb, rb->size); |
|
|
|
} else { |
|
|
|
a3k_discardpacket(from); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
static void __send_all(struct serial *to, const uint8_t *ptr, int len) |
|
|
|
{ |
|
|
|
int n; |
|
|
|
|
|
|
|
while (len > 0) { |
|
|
|
n = serial_send_buffered(to, ptr, len); |
|
|
|
if (n > 0) { |
|
|
|
ptr += n; |
|
|
|
len -= n; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
static int send_packet(struct serial *to, struct frames *f, uint32_t led) |
|
|
|
{ |
|
|
|
struct ringbuf *rb0, *rb1; |
|
|
|
uint8_t buffer[SEND_PKT_LEN]; |
|
|
|
int i = 0, x; |
|
|
|
uint8_t p = 0; |
|
|
|
|
|
|
|
/*
|
|
|
|
* packet tx buffer as: |
|
|
|
* |
|
|
|
* 0x61 [type] d0 d1 .... d39 0x2f parity |
|
|
|
* *****-------------------------- [compute parity] |
|
|
|
*/ |
|
|
|
if ((f->tail - f->head) >= 2) { |
|
|
|
rb0 = &f->slices[(f->head & (PKT_NLEMS - 1))]; |
|
|
|
rb1 = &f->slices[((f->head + 1) & (PKT_NLEMS - 1))]; |
|
|
|
f->head += 2; |
|
|
|
buffer[i++] = SEND_PKT_HDR; |
|
|
|
buffer[i++] = SEND_PKT_TYPE_DATA; |
|
|
|
|
|
|
|
memcpy(buffer + i, rb0->rb + RAW_PKT_OFF, RAW_PKT_LEN); |
|
|
|
i += RAW_PKT_LEN; |
|
|
|
memcpy(buffer + i, rb1->rb + RAW_PKT_OFF, RAW_PKT_LEN); |
|
|
|
i += RAW_PKT_LEN; |
|
|
|
|
|
|
|
buffer[i++] = SEND_PKT_TAL; |
|
|
|
for (x = 0; x < SEND_PKT_LEN - 2; ++x) { |
|
|
|
p ^= buffer[x]; |
|
|
|
} |
|
|
|
buffer[i++] = p; |
|
|
|
__send_all(to, buffer, SEND_PKT_LEN); |
|
|
|
|
|
|
|
gpio_toggle(led); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
return -1; |
|
|
|
} |
|
|
|
|
|
|
|
#if 0 |
|
|
|
static void forward_packet(a3k_t from, a3k_t to, uint32_t led) |
|
|
|
{ |
|
|
|
uint8_t buf[FORMAT3KBLEN]; |
|
|
|
@ -57,38 +180,151 @@ static void forward_packet(a3k_t from, a3k_t to, uint32_t led) |
|
|
|
gpio_toggle(led); |
|
|
|
} |
|
|
|
} |
|
|
|
#endif |
|
|
|
|
|
|
|
static void wait_to_reset() |
|
|
|
static char __fifo_getc(struct tag_fifo *f) |
|
|
|
{ |
|
|
|
ticks_close(); |
|
|
|
serial_close(&serial0); |
|
|
|
a3k_close(&a3k0); |
|
|
|
a3k_close(&a3k1); |
|
|
|
a3k_close(&a3k2); |
|
|
|
a3k_close(&a3k3); |
|
|
|
if (f->nelem) { |
|
|
|
char c; |
|
|
|
c = *f->pold++; |
|
|
|
if(f->pold == f->bend) |
|
|
|
f->pold = f->b; |
|
|
|
f->nelem -= 1; |
|
|
|
return c; |
|
|
|
} else |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
for (;;) |
|
|
|
; |
|
|
|
static int check_rxpacket(struct serial *uart, struct frames *tag) |
|
|
|
{ |
|
|
|
struct tag_fifo *f = &tag->f; |
|
|
|
int len, n; |
|
|
|
char *old; |
|
|
|
uint8_t parity = 0; |
|
|
|
|
|
|
|
start: |
|
|
|
/* try to receive from serial */ |
|
|
|
while (f->blen != f->nelem) { |
|
|
|
len = f->bend - f->pnew; |
|
|
|
n = serial_recv_buffered(uart, (void *)f->pnew, len); |
|
|
|
if (n > 0) { |
|
|
|
f->pnew += n; |
|
|
|
if (f->pnew == f->bend) { |
|
|
|
f->pnew = f->b; |
|
|
|
} |
|
|
|
f->nelem += n; |
|
|
|
} else { |
|
|
|
break; |
|
|
|
} |
|
|
|
} |
|
|
|
/*
|
|
|
|
* if there is a character in the fifo and it is not |
|
|
|
* a header byte then discard it. Keep going as |
|
|
|
* long as there are still bytes in the fifo. if a packet |
|
|
|
* with bad parity is received it is discarded. |
|
|
|
*/ |
|
|
|
while ((f->nelem > 0) && ( *f->pold != SEND_PKT_HDR)) { |
|
|
|
__fifo_getc(f); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* now we are either out of characters in the fifo |
|
|
|
* or the first character is a header byte. If we |
|
|
|
* dont have at least the header byte and the two |
|
|
|
* len bytes yet, then just return. |
|
|
|
*/ |
|
|
|
if (f->nelem < SEND_PKT_LEN) |
|
|
|
return 0; |
|
|
|
|
|
|
|
old = f->pold + 1; |
|
|
|
|
|
|
|
/*
|
|
|
|
* we now have received a full packet. We will |
|
|
|
* check its parity. if the parity is bad then |
|
|
|
* we will discard the packet, and return 0. |
|
|
|
* If the packet is good then we will return 1. |
|
|
|
*/ |
|
|
|
if (old == f->bend) |
|
|
|
old = f->b; |
|
|
|
|
|
|
|
for(n = 0; n < SEND_PKT_LEN - 1; n++) { |
|
|
|
parity ^= *old++; |
|
|
|
if (old == f->bend) |
|
|
|
old = f->b; |
|
|
|
} |
|
|
|
if (parity == 0) |
|
|
|
return 1; //we have a full packet with good parity check
|
|
|
|
else { |
|
|
|
__fifo_getc(f); /* research header */ |
|
|
|
goto start; |
|
|
|
} |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
static void recv_packet(a3k_t from, struct frames *f, uint32_t led) |
|
|
|
static void __drop_bytes(struct tag_fifo *f, int len) |
|
|
|
{ |
|
|
|
uint32_t pos; |
|
|
|
struct ringbuf *rb; |
|
|
|
int i; |
|
|
|
for (i = 0; i < len; ++i) { |
|
|
|
__fifo_getc(f); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if (a3k_checkfullpacket(from)) { |
|
|
|
if (a3k_peekpackettype(from) == PKT_CMD_REBOOT) { |
|
|
|
return wait_to_reset(); |
|
|
|
} |
|
|
|
pos = f->tail & (PKT_NLEMS - 1); |
|
|
|
++f->tail; |
|
|
|
rb = &f->slices[pos]; |
|
|
|
rb->len = a3k_getpacket(from, rb->rb, rb->size); |
|
|
|
gpio_toggle(led); |
|
|
|
} |
|
|
|
static void __get_packet(struct ringbuf *rb, struct tag_fifo *f) |
|
|
|
{ |
|
|
|
int x, i = 0; |
|
|
|
uint8_t p = 0; |
|
|
|
|
|
|
|
rb->rb[i++] = 0x61; |
|
|
|
rb->rb[i++] = 0x00; |
|
|
|
rb->rb[i++] = (RAW_PKT_LEN + 6); /* 22 bytes */ |
|
|
|
rb->rb[i++] = PKT_CHANNEL; |
|
|
|
rb->rb[i++] = 0x01; |
|
|
|
rb->rb[i++] = RAW_PKT_LEN * 8; |
|
|
|
|
|
|
|
while (i < (RAW_PKT_LEN + 6)) { |
|
|
|
rb->rb[i++] = __fifo_getc(f); |
|
|
|
} |
|
|
|
rb->rb[i++] = 0x2f; |
|
|
|
for (x = 0; x < i; ++x) { |
|
|
|
p ^= rb->rb[i]; |
|
|
|
} |
|
|
|
rb->rb[i++] = p; |
|
|
|
rb->len = i; |
|
|
|
} |
|
|
|
|
|
|
|
static void recv_packet(struct serial *from, struct frames *f, uint32_t led) |
|
|
|
{ |
|
|
|
uint8_t c; |
|
|
|
struct ringbuf *rb0, *rb1; |
|
|
|
|
|
|
|
if (check_rxpacket(from, f)) { |
|
|
|
__fifo_getc(&f->f); /* drop 0x61 */ |
|
|
|
c = __fifo_getc(&f->f); /* get type */ |
|
|
|
switch (c) { |
|
|
|
case SEND_PKT_TYPE_DATA: |
|
|
|
break; |
|
|
|
case SEND_PKT_TYPE_CMD: /* reset command */ |
|
|
|
return wait_to_reset(); |
|
|
|
default: |
|
|
|
__drop_bytes(&f->f, SEND_PKT_LEN - 2); |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
rb0 = &f->slices[f->tail & (PKT_NLEMS - 1)]; |
|
|
|
rb1 = &f->slices[(f->tail + 1) & (PKT_NLEMS - 1)]; |
|
|
|
f->tail += 2; |
|
|
|
|
|
|
|
__get_packet(rb0, &f->f); |
|
|
|
__get_packet(rb1, &f->f); |
|
|
|
|
|
|
|
__fifo_getc(&f->f); /* drop 0x2f */ |
|
|
|
__fifo_getc(&f->f); /* drop parity */ |
|
|
|
|
|
|
|
gpio_toggle(led); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
static void decode_packet(a3k_t to, struct frames *f) |
|
|
|
static int decode_packet(a3k_t to, struct frames *f) |
|
|
|
{ |
|
|
|
uint32_t pos; |
|
|
|
struct ringbuf *rb; |
|
|
|
@ -101,14 +337,14 @@ static void decode_packet(a3k_t to, struct frames *f) |
|
|
|
a3k_putpacket(to, rb->rb, rb->len); |
|
|
|
rb->len = 0; |
|
|
|
} |
|
|
|
} else { |
|
|
|
a3k_discardpacket(to); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
return -1; |
|
|
|
} |
|
|
|
|
|
|
|
static void __ticker(int ms) |
|
|
|
static void __on_20ms(int ms) |
|
|
|
{ |
|
|
|
to_play = 1; |
|
|
|
to_decode = (BIT0 | BIT1); |
|
|
|
// gpio_toggle(PK6);
|
|
|
|
} |
|
|
|
|
|
|
|
@ -160,6 +396,7 @@ static void __attribute__((unused)) loopback() |
|
|
|
|
|
|
|
static int __on_40ms(struct timer *tm, void *user) |
|
|
|
{ |
|
|
|
to_send = (BIT0 | BIT1); |
|
|
|
// gpio_toggle(LED_D6);
|
|
|
|
return 0; |
|
|
|
} |
|
|
|
@ -170,51 +407,80 @@ int main() |
|
|
|
|
|
|
|
cmd_init(); |
|
|
|
|
|
|
|
/* debug console */ |
|
|
|
serial_setup(&serial0, 115200, 8, SERIAL_STOPBITS_1); |
|
|
|
|
|
|
|
/* RS422 */ |
|
|
|
serial_setup(RS422_0, RS422_BAUDRATE, 8, SERIAL_STOPBITS_1); |
|
|
|
serial_setup(RS422_1, RS422_BAUDRATE, 8, SERIAL_STOPBITS_1); |
|
|
|
|
|
|
|
/* eeprom */ |
|
|
|
eeprom_init(); |
|
|
|
|
|
|
|
gpio_init(LED_D4 | LED_D5 | LED_D6, GPIO_OUTPUT); |
|
|
|
gpio_write(LED_D4 | LED_D5| LED_D6, 1); |
|
|
|
gpio_write(LED_D4 | LED_D5 | LED_D6, 1); |
|
|
|
gpio_init(LED_D3, GPIO_OUTPUT); |
|
|
|
gpio_write(LED_D3, 1); |
|
|
|
|
|
|
|
init_frames(&rx0); |
|
|
|
init_frames(&rx1); |
|
|
|
init_frames(&rx[0], __rxbuf[0], PKT_LEN); |
|
|
|
init_frames(&rx[1], __rxbuf[1], PKT_LEN); |
|
|
|
init_frames(&tx[0], NULL, 0); |
|
|
|
init_frames(&tx[1], NULL, 0); |
|
|
|
printf("\n"); |
|
|
|
|
|
|
|
a3k_setup(&a3k0, UART_BAUDRATE); |
|
|
|
a3k_setup(&a3k1, UART_BAUDRATE); |
|
|
|
/*
|
|
|
|
* recovery from bad image. |
|
|
|
*/ |
|
|
|
iwdg_set_period_ms(1000); |
|
|
|
iwdg_start(); |
|
|
|
|
|
|
|
a3k_setup(&a3k2, RS422_BAUDRATE); |
|
|
|
a3k_setup(&a3k3, RS422_BAUDRATE); |
|
|
|
a3k_setup(&a3k0, -1); |
|
|
|
a3k_setup(&a3k1, -1); |
|
|
|
|
|
|
|
ticks_set_callback(__ticker); |
|
|
|
ticks_set_callback(__on_20ms); |
|
|
|
ticks_init(20); |
|
|
|
|
|
|
|
timer_setup(&timer2, 40, __on_40ms, &timer2); |
|
|
|
timer_start(&timer2); |
|
|
|
timer_setup(TIM_40MS, 40, __on_40ms, NULL); |
|
|
|
timer_start(TIM_40MS); |
|
|
|
|
|
|
|
printf("\nrunning\n> "); |
|
|
|
|
|
|
|
/*
|
|
|
|
* recovery from bad image. |
|
|
|
*/ |
|
|
|
iwdg_set_period_ms(1000); |
|
|
|
iwdg_start(); |
|
|
|
while (1) { |
|
|
|
iwdg_reset(); |
|
|
|
cmd_loop(); |
|
|
|
forward_packet(&a3k0, &a3k2, LED_D4); /* D4: UART4 tx */ |
|
|
|
forward_packet(&a3k1, &a3k3, LED_D6); /* D6: USART6 tx */ |
|
|
|
|
|
|
|
recv_packet(&a3k2, &rx0, LED_D3); /* D3: UART4 rx */ |
|
|
|
recv_packet(&a3k3, &rx1, LED_D5); /* D5: USART6 rx */ |
|
|
|
|
|
|
|
if (to_play) { |
|
|
|
to_play = 0; |
|
|
|
decode_packet(&a3k0, &rx0); |
|
|
|
decode_packet(&a3k1, &rx1); |
|
|
|
} |
|
|
|
iwdg_reset(); |
|
|
|
encode_packet(&a3k0, &tx[0]); |
|
|
|
encode_packet(&a3k1, &tx[1]); |
|
|
|
|
|
|
|
if (to_send & BIT0) { |
|
|
|
if (!send_packet(RS422_0, &tx[0], LED_D4)) { /* D4: UART4 TX */ |
|
|
|
to_send &= ~BIT0; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if (to_send & BIT1) { |
|
|
|
if (!send_packet(RS422_1, &tx[1], LED_D6)) { /* D6: USART6 TX */ |
|
|
|
to_send &= ~BIT1; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
//forward_packet(&a3k0, &a3k2, LED_D4); /* D4: UART4 tx */
|
|
|
|
//forward_packet(&a3k1, &a3k3, LED_D6); /* D6: USART6 tx */
|
|
|
|
|
|
|
|
recv_packet(RS422_0, &rx[0], LED_D3); /* D3: UART4 rx */ |
|
|
|
recv_packet(RS422_1, &rx[1], LED_D5); /* D5: USART6 rx */ |
|
|
|
|
|
|
|
if (to_decode & BIT0) { |
|
|
|
if (!decode_packet(&a3k0, &rx[0])) { |
|
|
|
to_decode &= ~(BIT0); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if (to_decode & BIT1) { |
|
|
|
if (!decode_packet(&a3k1, &rx[1])) { |
|
|
|
to_decode &= ~(BIT1); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
surenyi
6 years ago