bananatools/jvs/jvs-util.c

112 lines
2.7 KiB
C
Raw Permalink Normal View History

2023-01-03 04:35:53 +00:00
#include <windows.h>
#include <assert.h>
#include <stddef.h>
#include <stdint.h>
#include "hook/iobuf.h"
#include "jvs/jvs-frame.h"
#include "jvs/jvs-util.h"
#include "util/dprintf.h"
#include "util/dump.h"
typedef HRESULT (*jvs_dispatch_fn_t)(
void *ctx,
struct const_iobuf *req,
struct iobuf *resp);
void jvs_crack_request(
const void *bytes,
size_t nbytes,
struct iobuf *resp,
uint8_t jvs_addr,
jvs_dispatch_fn_t dispatch_fn,
void *dispatch_ctx)
{
uint8_t req_bytes[128];
uint8_t resp_bytes[128];
struct iobuf decode;
struct iobuf encode;
struct const_iobuf segments;
HRESULT hr;
assert(bytes != NULL);
assert(resp != NULL);
assert(jvs_addr != 0x00 && (jvs_addr < 0x20 || jvs_addr == 0xFF));
assert(dispatch_fn != NULL);
decode.bytes = req_bytes;
decode.nbytes = sizeof(req_bytes);
decode.pos = 0;
hr = jvs_frame_decode(&decode, bytes, nbytes);
if (FAILED(hr)) {
return;
}
#if 0
dprintf("Decoded request:\n");
dump_iobuf(&decode);
#endif
if (req_bytes[0] != jvs_addr && req_bytes[0] != 0xFF) {
return;
}
iobuf_flip(&segments, &decode);
segments.pos = 2;
encode.bytes = resp_bytes;
encode.nbytes = sizeof(resp_bytes);
encode.pos = 3;
/* +1: Don't try to dispatch the trailing checksum byte */
hr = S_OK; /* I guess an empty request packet is technically valid? */
while (segments.pos + 1 < segments.nbytes) {
hr = dispatch_fn(dispatch_ctx, &segments, &encode);
if (FAILED(hr)) {
break;
}
}
if (FAILED(hr)) {
/* Send an error in the overall status byte */
dprintf("JVS I/O: HR Failed %lX\n", hr);
encode.pos = 3;
resp_bytes[0] = 0x00; /* Dest addr (master) */
resp_bytes[1] = 0x02; /* Payload len: Status byte, checksum byte */
if (hr == HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER)) {
resp_bytes[2] = 0x04; /* Status: "Overflow" */
} else {
resp_bytes[2] = 0x02; /* Status: Encoutered unsupported command */
}
} else if (encode.pos == 3) {
/* Probably a reset, don't emit a response frame with empty payload */
return;
} else {
/* Send success response */
resp_bytes[0] = 0x00; /* Dest addr (master) */
resp_bytes[1] = encode.pos - 2 + 1; /* -2 header +1 checksum */
resp_bytes[2] = 0x01; /* Status: Success */
}
#if 0
dprintf("Encoding response:\n");
dump_iobuf(&encode);
#endif
hr = jvs_frame_encode(resp, encode.bytes, encode.pos);
if (FAILED(hr)) {
dprintf("JVS Node: Response encode error: %x\n", (int) hr);
}
}