315 lines
8.1 KiB
C
315 lines
8.1 KiB
C
#ifdef __USB_COMM__
|
|
#include "stdint.h"
|
|
#include "stdbool.h"
|
|
#include "plat_types.h"
|
|
#include "string.h"
|
|
#include "stdio.h"
|
|
#include "tool_msg.h"
|
|
#include "sys_api_cdc_comm.h"
|
|
#include "app_factory_cdc_comm.h"
|
|
|
|
static enum PARSE_STATE parse_state;
|
|
static struct message_t recv_msg;
|
|
static struct message_t send_msg = { { PREFIX_CHAR, }, };
|
|
|
|
static unsigned char check_sum(unsigned char *buf, unsigned char len)
|
|
{
|
|
int i;
|
|
unsigned char sum = 0;
|
|
|
|
for (i = 0; i < len; i++) {
|
|
sum += buf[i];
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
int send_reply(const unsigned char *payload, unsigned int len)
|
|
{
|
|
int ret = 0;
|
|
|
|
if (len + 1 > sizeof(send_msg.data)) {
|
|
TRACE(1,"Packet length too long: %u", len);
|
|
return -1;
|
|
}
|
|
|
|
send_msg.hdr.type = recv_msg.hdr.type;
|
|
send_msg.hdr.seq = recv_msg.hdr.seq;
|
|
send_msg.hdr.len = len;
|
|
memcpy(&send_msg.data[0], payload, len);
|
|
send_msg.data[len] = ~check_sum((unsigned char *)&send_msg, MSG_TOTAL_LEN(&send_msg) - 1);
|
|
|
|
ret = send_data((unsigned char *)&send_msg, MSG_TOTAL_LEN(&send_msg));
|
|
|
|
return ret;
|
|
}
|
|
|
|
static void reset_parse_state(unsigned char **buf, size_t *len)
|
|
{
|
|
parse_state = PARSE_HEADER;
|
|
memset(&recv_msg.hdr, 0, sizeof(recv_msg.hdr));
|
|
|
|
*buf = (unsigned char *)&recv_msg.hdr;
|
|
*len = sizeof(recv_msg.hdr);
|
|
}
|
|
|
|
static enum ERR_CODE check_msg_hdr(void)
|
|
{
|
|
enum ERR_CODE errcode = ERR_NONE;
|
|
|
|
switch (recv_msg.hdr.type) {
|
|
case TYPE_SYS:
|
|
if (recv_msg.hdr.len != 1 && recv_msg.hdr.len != 5) {
|
|
//TRACE(1,"SYS msg length error: %u", recv_msg.hdr.len);
|
|
errcode = ERR_LEN;
|
|
}
|
|
break;
|
|
case TYPE_READ:
|
|
if (recv_msg.hdr.len != 4) {
|
|
//TRACE(1,"READ msg length error: %u", recv_msg.hdr.len);
|
|
errcode = ERR_LEN;
|
|
}
|
|
break;
|
|
case TYPE_WRITE:
|
|
if (recv_msg.hdr.len <= 4 || recv_msg.hdr.len > 20) {
|
|
//TRACE(1,"WRITE msg length error: %u", recv_msg.hdr.len);
|
|
errcode = ERR_LEN;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (errcode == ERR_NONE && recv_msg.hdr.len + 1 > sizeof(recv_msg.data)) {
|
|
errcode = ERR_LEN;
|
|
}
|
|
|
|
return errcode;
|
|
}
|
|
|
|
static enum ERR_CODE handle_sys_cmd(enum SYS_CMD_TYPE cmd, unsigned char *param, unsigned int len)
|
|
{
|
|
unsigned char cret[5];
|
|
unsigned int bootmode;
|
|
|
|
cret[0] = ERR_NONE;
|
|
|
|
if (cmd == SYS_CMD_SET_BOOTMODE || cmd == SYS_CMD_CLR_BOOTMODE) {
|
|
if (len != 4) {
|
|
TRACE(2,"Invalid SYS CMD len %u for cmd: 0x%x", len, cmd);
|
|
return ERR_DATA_LEN;
|
|
}
|
|
} else {
|
|
if (len != 0) {
|
|
TRACE(2,"Invalid SYS CMD len %u for cmd: 0x%x", len, cmd);
|
|
return ERR_DATA_LEN;
|
|
}
|
|
}
|
|
|
|
switch (cmd) {
|
|
case SYS_CMD_REBOOT: {
|
|
TRACE(0,"--- Reboot---");
|
|
send_reply(cret, 1);
|
|
system_reboot();
|
|
break;
|
|
}
|
|
case SYS_CMD_SHUTDOWN: {
|
|
TRACE(0,"--- Shutdown ---");
|
|
send_reply(cret, 1);
|
|
system_shutdown();
|
|
break;
|
|
}
|
|
case SYS_CMD_SET_BOOTMODE: {
|
|
TRACE(0,"--- Set bootmode ---");
|
|
memcpy(&bootmode, param, 4);
|
|
system_set_bootmode(bootmode);
|
|
send_reply(cret, 1);
|
|
break;
|
|
}
|
|
case SYS_CMD_CLR_BOOTMODE: {
|
|
TRACE(0,"--- Clear bootmode ---");
|
|
memcpy(&bootmode, param, 4);
|
|
system_clear_bootmode(bootmode);
|
|
send_reply(cret, 1);
|
|
break;
|
|
}
|
|
case SYS_CMD_GET_BOOTMODE: {
|
|
TRACE(0,"--- Get bootmode ---");
|
|
bootmode = system_get_bootmode();
|
|
memcpy(&cret[1], &bootmode, 4);
|
|
send_reply(cret, 5);
|
|
break;
|
|
}
|
|
default: {
|
|
TRACE(1,"Invalid command: 0x%x", recv_msg.data[0]);
|
|
return ERR_SYS_CMD;
|
|
}
|
|
}
|
|
|
|
return ERR_NONE;
|
|
}
|
|
|
|
static enum ERR_CODE handle_data(unsigned char **buf, size_t *len, int *extra)
|
|
{
|
|
enum ERR_CODE errcode = ERR_NONE;
|
|
#if 0
|
|
uint32_t rlen = 0;
|
|
#endif
|
|
*extra = 0;
|
|
|
|
// Checksum
|
|
if (check_sum((unsigned char *)&recv_msg, MSG_TOTAL_LEN(&recv_msg)) != 0xFF) {
|
|
TRACE(0,"Checksum error");
|
|
return ERR_CHECKSUM;
|
|
}
|
|
|
|
switch (recv_msg.hdr.type) {
|
|
case TYPE_SYS: {
|
|
TRACE_TIME(0,"------ SYS CMD ------");
|
|
errcode = handle_sys_cmd((enum SYS_CMD_TYPE)recv_msg.data[0], &recv_msg.data[1], recv_msg.hdr.len - 1);
|
|
if (errcode != ERR_NONE) {
|
|
return errcode;
|
|
}
|
|
break;
|
|
}
|
|
case TYPE_READ: {
|
|
TRACE_TIME(0,"------ READ CMD ------");
|
|
#if 0
|
|
uint32_t addr = (recv_msg.data[0] << 16) | (recv_msg.data[1] << 8) | recv_msg.data[2];
|
|
uint8_t data[4] = {0};
|
|
rlen = read_reg(addr, data);
|
|
if(rlen == 0)
|
|
return ERR_LEN;
|
|
else {
|
|
send_reply(data, rlen);
|
|
}
|
|
#endif
|
|
break;
|
|
}
|
|
case TYPE_WRITE: {
|
|
TRACE_TIME(0,"------ WRITE CMD ------");
|
|
#if 0
|
|
uint32_t addr = (recv_msg.data[0] << 16) | (recv_msg.data[1] << 8) | recv_msg.data[2];
|
|
uint32_t wdata = (recv_msg.data[3] << 24) | (recv_msg.data[4] << 16) | (recv_msg.data[5] << 8) | recv_msg.data[6];
|
|
uint8_t data[1] = {0};
|
|
errcode = write_reg(addr, wdata);
|
|
if (errcode != ERR_NONE)
|
|
return errcode;
|
|
else
|
|
send_reply(data, 1);
|
|
#endif
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return ERR_NONE;
|
|
}
|
|
|
|
static int parse_packet(unsigned char **buf, size_t *len)
|
|
{
|
|
enum ERR_CODE errcode;
|
|
int rlen = *len;
|
|
unsigned char *data;
|
|
int i;
|
|
int extra;
|
|
unsigned char cret;
|
|
|
|
switch (parse_state) {
|
|
case PARSE_HEADER:
|
|
ASSERT(rlen > 0 && rlen <= sizeof(recv_msg.hdr), "Invalid rlen!");
|
|
|
|
if (recv_msg.hdr.prefix == PREFIX_CHAR) {
|
|
errcode = check_msg_hdr();
|
|
if (errcode != ERR_NONE) {
|
|
goto _err;
|
|
}
|
|
parse_state = PARSE_DATA;
|
|
*buf = &recv_msg.data[0];
|
|
*len = recv_msg.hdr.len + 1;
|
|
} else {
|
|
data = (unsigned char *)&recv_msg.hdr.prefix;
|
|
for (i = 1; i < rlen; i++) {
|
|
if (data[i] == PREFIX_CHAR) {
|
|
memmove(&recv_msg.hdr.prefix, &data[i], rlen - i);
|
|
break;
|
|
}
|
|
}
|
|
*buf = &data[rlen - i];
|
|
*len = sizeof(recv_msg.hdr) + i - rlen;
|
|
}
|
|
break;
|
|
case PARSE_DATA:
|
|
errcode = handle_data(buf, len, &extra);
|
|
if (errcode != ERR_NONE) {
|
|
goto _err;
|
|
}
|
|
// Receive next message
|
|
reset_parse_state(buf, len);
|
|
break;
|
|
default:
|
|
TRACE(1,"Invalid parse_state: %d", parse_state);
|
|
break;
|
|
}
|
|
|
|
return 0;
|
|
|
|
_err:
|
|
cancel_input();
|
|
cret = (unsigned char)errcode;
|
|
send_reply(&cret, 1);
|
|
|
|
return 1;
|
|
}
|
|
|
|
void comm_loop(void)
|
|
{
|
|
int ret;
|
|
unsigned char *buf = NULL;
|
|
size_t len = 0;
|
|
size_t buf_len, rlen;
|
|
|
|
_sync:
|
|
reset_transport();
|
|
reset_parse_state(&buf, &len);
|
|
|
|
while (1) {
|
|
rlen = 0;
|
|
if (parse_state == PARSE_HEADER) {
|
|
set_recv_timeout(default_recv_timeout_idle);
|
|
} else {
|
|
set_recv_timeout(default_recv_timeout_short);
|
|
}
|
|
buf_len = 0;
|
|
|
|
ret = recv_data_ex(buf, buf_len, len, &rlen);
|
|
if (ret) {
|
|
TRACE(1,"Receiving data failed: %d", ret);
|
|
goto _err;
|
|
}
|
|
|
|
if (len != rlen) {
|
|
TRACE(2,"Receiving part of the data: expect=%u real=%u", len, rlen);
|
|
goto _err;
|
|
}
|
|
|
|
ret = parse_packet(&buf, &len);
|
|
if (ret) {
|
|
TRACE(0,"Parsing packet failed");
|
|
goto _err;
|
|
}
|
|
}
|
|
|
|
_err:
|
|
ret = handle_error();
|
|
if (ret == 0) {
|
|
TRACE(0,"retry ...");
|
|
goto _sync;
|
|
}
|
|
|
|
return;
|
|
}
|
|
#endif
|