2021-01-30 06:14:38 +00:00
|
|
|
/* SPDX-License-Identifier: MIT */
|
|
|
|
|
|
|
|
#include "uartproxy.h"
|
2021-05-04 10:34:08 +00:00
|
|
|
#include "assert.h"
|
2021-01-30 06:14:38 +00:00
|
|
|
#include "exception.h"
|
2021-04-17 07:26:41 +00:00
|
|
|
#include "iodev.h"
|
2021-01-14 09:18:07 +00:00
|
|
|
#include "proxy.h"
|
|
|
|
#include "string.h"
|
|
|
|
#include "types.h"
|
2021-01-14 10:22:38 +00:00
|
|
|
#include "utils.h"
|
|
|
|
|
|
|
|
#define REQ_SIZE 64
|
2021-01-14 09:18:07 +00:00
|
|
|
|
|
|
|
typedef struct {
|
|
|
|
u32 _pad;
|
|
|
|
u32 type;
|
|
|
|
union {
|
|
|
|
ProxyRequest prequest;
|
|
|
|
struct {
|
|
|
|
u64 addr;
|
|
|
|
u64 size;
|
|
|
|
u32 dchecksum;
|
|
|
|
} mrequest;
|
2021-05-17 18:12:56 +00:00
|
|
|
u64 features;
|
2021-01-14 09:18:07 +00:00
|
|
|
};
|
|
|
|
u32 checksum;
|
|
|
|
} UartRequest;
|
|
|
|
|
2021-01-14 10:22:38 +00:00
|
|
|
#define REPLY_SIZE 36
|
|
|
|
|
2021-01-14 09:18:07 +00:00
|
|
|
typedef struct {
|
|
|
|
u32 type;
|
2021-01-14 10:22:38 +00:00
|
|
|
s32 status;
|
2021-01-14 09:18:07 +00:00
|
|
|
union {
|
|
|
|
ProxyReply preply;
|
|
|
|
struct {
|
|
|
|
u32 dchecksum;
|
|
|
|
} mreply;
|
2021-05-04 10:34:08 +00:00
|
|
|
struct uartproxy_msg_start start;
|
2021-05-17 18:12:56 +00:00
|
|
|
u64 features;
|
2021-01-14 09:18:07 +00:00
|
|
|
};
|
|
|
|
u32 checksum;
|
2021-05-04 10:34:08 +00:00
|
|
|
u32 _dummy; // Not transferred
|
2021-01-14 09:18:07 +00:00
|
|
|
} UartReply;
|
|
|
|
|
2021-05-15 13:03:29 +00:00
|
|
|
typedef struct {
|
|
|
|
u32 type;
|
|
|
|
u16 len;
|
|
|
|
u16 event_type;
|
|
|
|
} UartEventHdr;
|
|
|
|
|
2021-05-04 10:34:08 +00:00
|
|
|
static_assert(sizeof(UartReply) == (REPLY_SIZE + 4), "Invalid UartReply size");
|
|
|
|
|
2021-01-28 06:27:35 +00:00
|
|
|
#define REQ_NOP 0x00AA55FF
|
|
|
|
#define REQ_PROXY 0x01AA55FF
|
|
|
|
#define REQ_MEMREAD 0x02AA55FF
|
2021-01-14 09:18:07 +00:00
|
|
|
#define REQ_MEMWRITE 0x03AA55FF
|
2021-01-28 06:27:35 +00:00
|
|
|
#define REQ_BOOT 0x04AA55FF
|
2021-05-15 13:03:29 +00:00
|
|
|
#define REQ_EVENT 0x05AA55FF
|
2021-01-14 09:18:07 +00:00
|
|
|
|
2021-04-17 07:25:48 +00:00
|
|
|
#define ST_OK 0
|
|
|
|
#define ST_BADCMD -1
|
|
|
|
#define ST_INVAL -2
|
|
|
|
#define ST_XFRERR -3
|
|
|
|
#define ST_CSUMERR -4
|
2021-01-14 09:18:07 +00:00
|
|
|
|
2021-05-17 18:12:56 +00:00
|
|
|
#define PROXY_FEAT_DISABLE_DATA_CSUMS 0x01
|
|
|
|
#define PROXY_FEAT_ALL (PROXY_FEAT_DISABLE_DATA_CSUMS)
|
|
|
|
|
2021-04-17 07:26:41 +00:00
|
|
|
static u32 iodev_proxy_buffer[IODEV_MAX];
|
|
|
|
|
2021-05-17 18:12:56 +00:00
|
|
|
#define CHECKSUM_INIT 0xDEADBEEF
|
|
|
|
#define CHECKSUM_FINAL 0xADDEDBAD
|
|
|
|
#define CHECKSUM_SENTINEL 0xD0DECADE
|
|
|
|
#define DATA_END_SENTINEL 0xB0CACC10
|
|
|
|
|
|
|
|
static bool disable_data_csums = false;
|
2021-05-16 10:04:27 +00:00
|
|
|
|
2021-01-14 09:18:07 +00:00
|
|
|
// I just totally pulled this out of my arse
|
2021-01-30 06:14:38 +00:00
|
|
|
// Noinline so that this can be bailed out by exc_guard = EXC_RETURN
|
|
|
|
// We assume this function does not use the stack
|
2021-05-16 10:04:27 +00:00
|
|
|
static u32 __attribute__((noinline)) checksum_block(void *start, u32 length, u32 init)
|
2021-01-14 09:18:07 +00:00
|
|
|
{
|
2021-05-16 10:04:27 +00:00
|
|
|
u32 sum = init;
|
2021-01-14 09:18:07 +00:00
|
|
|
u8 *d = (u8 *)start;
|
|
|
|
|
|
|
|
while (length--) {
|
|
|
|
sum *= 31337;
|
|
|
|
sum += (*d++) ^ 0x5A;
|
|
|
|
}
|
2021-05-16 10:04:27 +00:00
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
|
|
|
|
static inline u32 checksum_start(void *start, u32 length)
|
|
|
|
{
|
|
|
|
return checksum_block(start, length, CHECKSUM_INIT);
|
|
|
|
}
|
|
|
|
|
|
|
|
static inline u32 checksum_add(void *start, u32 length, u32 sum)
|
|
|
|
{
|
|
|
|
return checksum_block(start, length, sum);
|
|
|
|
}
|
|
|
|
|
|
|
|
static inline u32 checksum_finish(u32 sum)
|
|
|
|
{
|
|
|
|
return sum ^ CHECKSUM_FINAL;
|
|
|
|
}
|
|
|
|
|
|
|
|
static inline u32 checksum(void *start, u32 length)
|
|
|
|
{
|
|
|
|
return checksum_finish(checksum_start(start, length));
|
2021-01-14 09:18:07 +00:00
|
|
|
}
|
|
|
|
|
2021-05-17 18:12:56 +00:00
|
|
|
static u64 data_checksum(void *start, u32 length)
|
|
|
|
{
|
|
|
|
if (disable_data_csums) {
|
|
|
|
return CHECKSUM_SENTINEL;
|
|
|
|
}
|
|
|
|
|
|
|
|
return checksum(start, length);
|
|
|
|
}
|
|
|
|
|
2021-05-03 12:03:28 +00:00
|
|
|
iodev_id_t uartproxy_iodev;
|
|
|
|
|
2021-05-04 10:34:08 +00:00
|
|
|
int uartproxy_run(struct uartproxy_msg_start *start)
|
2021-01-14 09:18:07 +00:00
|
|
|
{
|
2021-05-04 10:34:08 +00:00
|
|
|
int ret;
|
2021-01-14 09:18:07 +00:00
|
|
|
int running = 1;
|
2021-01-30 06:14:38 +00:00
|
|
|
size_t bytes;
|
|
|
|
u64 checksum_val;
|
2021-05-17 18:12:56 +00:00
|
|
|
u64 enabled_features = 0;
|
2021-01-14 09:18:07 +00:00
|
|
|
|
2021-04-17 07:26:41 +00:00
|
|
|
iodev_id_t iodev = IODEV_MAX;
|
|
|
|
|
2021-01-14 09:18:07 +00:00
|
|
|
UartRequest request;
|
2021-01-14 18:55:20 +00:00
|
|
|
UartReply reply = {REQ_BOOT};
|
2021-05-04 10:34:08 +00:00
|
|
|
if (!start) {
|
|
|
|
// Startup notification only goes out via UART
|
|
|
|
reply.checksum = checksum(&reply, REPLY_SIZE - 4);
|
|
|
|
iodev_write(IODEV_UART, &reply, REPLY_SIZE);
|
|
|
|
} else {
|
|
|
|
// Exceptions / hooks keep the current iodev
|
|
|
|
iodev = uartproxy_iodev;
|
|
|
|
reply.start = *start;
|
|
|
|
reply.checksum = checksum(&reply, REPLY_SIZE - 4);
|
|
|
|
iodev_write(iodev, &reply, REPLY_SIZE);
|
|
|
|
}
|
2021-01-14 09:18:07 +00:00
|
|
|
|
|
|
|
while (running) {
|
2021-05-04 10:34:08 +00:00
|
|
|
if (!start) {
|
|
|
|
// Look for commands from any iodev on startup
|
|
|
|
for (iodev = 0; iodev < IODEV_MAX;) {
|
|
|
|
u8 b;
|
2021-05-29 16:47:46 +00:00
|
|
|
if ((iodevs[iodev]->usage & USAGE_UARTPROXY)) {
|
|
|
|
iodev_handle_events(iodev);
|
|
|
|
if (iodev_can_read(iodev) && iodev_read(iodev, &b, 1) == 1) {
|
|
|
|
iodev_proxy_buffer[iodev] >>= 8;
|
|
|
|
iodev_proxy_buffer[iodev] |= b << 24;
|
|
|
|
if ((iodev_proxy_buffer[iodev] & 0xffffff) == 0xAA55FF)
|
|
|
|
break;
|
|
|
|
}
|
2021-05-04 10:34:08 +00:00
|
|
|
}
|
|
|
|
iodev++;
|
|
|
|
if (iodev == IODEV_MAX)
|
|
|
|
iodev = 0;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// Stick to the current iodev for exceptions
|
|
|
|
do {
|
|
|
|
u8 b;
|
|
|
|
iodev_handle_events(iodev);
|
|
|
|
if (iodev_read(iodev, &b, 1) != 1) {
|
|
|
|
printf("Proxy: iodev read failed, exiting.\n");
|
|
|
|
return -1;
|
|
|
|
}
|
2021-04-17 07:26:41 +00:00
|
|
|
iodev_proxy_buffer[iodev] >>= 8;
|
|
|
|
iodev_proxy_buffer[iodev] |= b << 24;
|
2021-05-04 10:34:08 +00:00
|
|
|
} while ((iodev_proxy_buffer[iodev] & 0xffffff) != 0xAA55FF);
|
2021-04-17 07:26:41 +00:00
|
|
|
}
|
|
|
|
|
2021-01-14 09:18:07 +00:00
|
|
|
memset(&request, 0, sizeof(request));
|
2021-04-17 07:26:41 +00:00
|
|
|
request.type = iodev_proxy_buffer[iodev];
|
|
|
|
bytes = iodev_read(iodev, (&request.type) + 1, REQ_SIZE - 4);
|
2021-01-14 10:22:38 +00:00
|
|
|
if (bytes != REQ_SIZE - 4)
|
2021-01-14 09:18:07 +00:00
|
|
|
continue;
|
2021-04-17 07:25:48 +00:00
|
|
|
|
|
|
|
if (checksum(&(request.type), REQ_SIZE - 4) != request.checksum) {
|
|
|
|
memset(&reply, 0, sizeof(reply));
|
|
|
|
reply.type = request.type;
|
|
|
|
reply.status = ST_CSUMERR;
|
|
|
|
reply.checksum = checksum(&reply, REPLY_SIZE - 4);
|
2021-04-17 07:26:41 +00:00
|
|
|
iodev_write(iodev, &reply, REPLY_SIZE);
|
2021-01-14 09:18:07 +00:00
|
|
|
continue;
|
2021-04-17 07:25:48 +00:00
|
|
|
}
|
2021-01-14 09:18:07 +00:00
|
|
|
|
|
|
|
memset(&reply, 0, sizeof(reply));
|
|
|
|
reply.type = request.type;
|
|
|
|
reply.status = ST_OK;
|
|
|
|
|
2021-05-03 12:03:28 +00:00
|
|
|
uartproxy_iodev = iodev;
|
|
|
|
|
2021-01-14 09:18:07 +00:00
|
|
|
switch (request.type) {
|
|
|
|
case REQ_NOP:
|
2021-05-17 18:12:56 +00:00
|
|
|
enabled_features = request.features & PROXY_FEAT_ALL;
|
|
|
|
if (iodev == IODEV_UART) {
|
|
|
|
// Don't allow disabling checksums on UART
|
|
|
|
enabled_features &= ~PROXY_FEAT_DISABLE_DATA_CSUMS;
|
|
|
|
}
|
|
|
|
|
|
|
|
disable_data_csums = enabled_features & PROXY_FEAT_DISABLE_DATA_CSUMS;
|
|
|
|
reply.features = enabled_features;
|
2021-01-14 09:18:07 +00:00
|
|
|
break;
|
|
|
|
case REQ_PROXY:
|
2021-05-04 10:34:08 +00:00
|
|
|
ret = proxy_process(&request.prequest, &reply.preply);
|
|
|
|
if (ret != 0)
|
|
|
|
running = 0;
|
|
|
|
if (ret < 0)
|
|
|
|
printf("Proxy req error: %d\n", ret);
|
2021-01-14 09:18:07 +00:00
|
|
|
break;
|
|
|
|
case REQ_MEMREAD:
|
2021-01-30 06:14:38 +00:00
|
|
|
if (request.mrequest.size == 0)
|
|
|
|
break;
|
|
|
|
exc_count = 0;
|
|
|
|
exc_guard = GUARD_RETURN;
|
2021-05-17 18:12:56 +00:00
|
|
|
checksum_val = data_checksum((void *)request.mrequest.addr, request.mrequest.size);
|
2021-01-30 06:14:38 +00:00
|
|
|
exc_guard = GUARD_OFF;
|
|
|
|
if (exc_count)
|
|
|
|
reply.status = ST_XFRERR;
|
|
|
|
reply.mreply.dchecksum = checksum_val;
|
2021-01-14 09:18:07 +00:00
|
|
|
break;
|
|
|
|
case REQ_MEMWRITE:
|
2021-01-30 06:14:38 +00:00
|
|
|
exc_count = 0;
|
|
|
|
exc_guard = GUARD_SKIP;
|
|
|
|
if (request.mrequest.size != 0) {
|
|
|
|
// Probe for exception guard
|
|
|
|
// We can't do the whole buffer easily, because we'd drop UART data
|
|
|
|
write8(request.mrequest.addr, 0);
|
|
|
|
write8(request.mrequest.addr + request.mrequest.size - 1, 0);
|
|
|
|
}
|
|
|
|
exc_guard = GUARD_OFF;
|
|
|
|
if (exc_count) {
|
|
|
|
reply.status = ST_XFRERR;
|
|
|
|
break;
|
|
|
|
}
|
2021-04-17 07:26:41 +00:00
|
|
|
bytes = iodev_read(iodev, (void *)request.mrequest.addr, request.mrequest.size);
|
2021-01-14 09:18:07 +00:00
|
|
|
if (bytes != request.mrequest.size) {
|
|
|
|
reply.status = ST_XFRERR;
|
|
|
|
break;
|
|
|
|
}
|
2021-05-17 18:12:56 +00:00
|
|
|
checksum_val = data_checksum((void *)request.mrequest.addr, request.mrequest.size);
|
2021-01-30 06:14:38 +00:00
|
|
|
reply.mreply.dchecksum = checksum_val;
|
2021-05-17 18:12:56 +00:00
|
|
|
if (reply.mreply.dchecksum != request.mrequest.dchecksum) {
|
2021-01-14 09:18:07 +00:00
|
|
|
reply.status = ST_XFRERR;
|
2021-05-17 18:12:56 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
if (disable_data_csums) {
|
|
|
|
// Check the sentinel that should be present after the data
|
|
|
|
u32 sentinel = 0;
|
|
|
|
bytes = iodev_read(iodev, &sentinel, sizeof(sentinel));
|
|
|
|
if (bytes != sizeof(sentinel) || sentinel != DATA_END_SENTINEL) {
|
|
|
|
reply.status = ST_XFRERR;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2021-01-14 09:18:07 +00:00
|
|
|
break;
|
|
|
|
default:
|
|
|
|
reply.status = ST_BADCMD;
|
|
|
|
break;
|
|
|
|
}
|
2021-01-14 10:22:38 +00:00
|
|
|
reply.checksum = checksum(&reply, REPLY_SIZE - 4);
|
2021-05-17 18:12:56 +00:00
|
|
|
iodev_queue(iodev, &reply, REPLY_SIZE);
|
2021-01-14 09:18:07 +00:00
|
|
|
|
|
|
|
if ((request.type == REQ_MEMREAD) && (reply.status == ST_OK)) {
|
2021-05-17 18:12:56 +00:00
|
|
|
iodev_queue(iodev, (void *)request.mrequest.addr, request.mrequest.size);
|
|
|
|
|
|
|
|
if (disable_data_csums) {
|
|
|
|
// Since there is no checksum, put a sentinel after the data so the receiver
|
|
|
|
// can check that no packets were lost.
|
|
|
|
u32 sentinel = DATA_END_SENTINEL;
|
|
|
|
|
|
|
|
iodev_queue(iodev, &sentinel, sizeof(sentinel));
|
|
|
|
}
|
2021-01-14 09:18:07 +00:00
|
|
|
}
|
2021-05-17 18:12:56 +00:00
|
|
|
|
|
|
|
// Flush all queued data
|
|
|
|
iodev_write(iodev, NULL, 0);
|
2021-07-09 16:39:16 +00:00
|
|
|
iodev_flush(iodev);
|
2021-01-14 09:18:07 +00:00
|
|
|
}
|
2021-05-04 10:34:08 +00:00
|
|
|
|
|
|
|
return ret;
|
2021-01-14 09:18:07 +00:00
|
|
|
}
|
2021-05-15 13:03:29 +00:00
|
|
|
|
|
|
|
void uartproxy_send_event(u16 event_type, void *data, u16 length)
|
|
|
|
{
|
2021-05-16 10:04:27 +00:00
|
|
|
UartEventHdr hdr;
|
2021-05-15 13:03:29 +00:00
|
|
|
u32 csum;
|
|
|
|
|
2021-05-16 10:04:27 +00:00
|
|
|
hdr.type = REQ_EVENT;
|
|
|
|
hdr.len = length;
|
|
|
|
hdr.event_type = event_type;
|
2021-05-15 13:03:29 +00:00
|
|
|
|
2021-05-17 18:12:56 +00:00
|
|
|
if (disable_data_csums) {
|
|
|
|
csum = CHECKSUM_SENTINEL;
|
|
|
|
} else {
|
|
|
|
csum = checksum_start(&hdr, sizeof(UartEventHdr));
|
|
|
|
csum = checksum_finish(checksum_add(data, length, csum));
|
|
|
|
}
|
2021-05-16 10:04:27 +00:00
|
|
|
iodev_queue(uartproxy_iodev, &hdr, sizeof(UartEventHdr));
|
|
|
|
iodev_queue(uartproxy_iodev, data, length);
|
|
|
|
iodev_write(uartproxy_iodev, &csum, sizeof(csum));
|
2021-05-15 13:03:29 +00:00
|
|
|
}
|