diff --git a/board/sg-cmd.c b/board/sg-cmd.c new file mode 100644 index 0000000..a375686 --- /dev/null +++ b/board/sg-cmd.c @@ -0,0 +1,134 @@ +#include + +#include "board/sg-cmd.h" +#include "board/sg-frame.h" + +#include "hook/iobuf.h" + +#include "util/dprintf.h" + +union sg_req_any { + struct sg_req_header req; + uint8_t bytes[256]; +}; + +union sg_resp_any { + struct sg_resp_header resp; + uint8_t bytes[256]; +}; + +static HRESULT sg_req_validate(const void *ptr, size_t nbytes); + +static void sg_resp_error( + struct sg_resp_header *resp, + const struct sg_req_header *req); + +static HRESULT sg_req_validate(const void *ptr, size_t nbytes) +{ + const struct sg_req_header *req; + size_t payload_len; + + assert(ptr != NULL); + + if (nbytes < sizeof(*req)) { + dprintf("SG Cmd: Request header truncated\n"); + + return E_FAIL; + } + + req = ptr; + + if (req->hdr.frame_len != nbytes) { + dprintf("SG Cmd: Frame length mismatch: got %i exp %i\n", + req->hdr.frame_len, + nbytes); + + return E_FAIL; + } + + payload_len = req->hdr.frame_len - sizeof(*req); + + if (req->payload_len != payload_len) { + dprintf("SG Cmd: Payload length mismatch: got %i exp %i\n", + req->payload_len, + payload_len); + + return E_FAIL; + } + + return S_OK; +} + +void sg_req_transact( + struct iobuf *resp_frame, + const uint8_t *req_bytes, + size_t req_nbytes, + sg_dispatch_fn_t dispatch, + void *ctx) +{ + struct iobuf req_span; + union sg_req_any req; + union sg_resp_any resp; + HRESULT hr; + + assert(resp_frame != NULL); + assert(req_bytes != NULL); + assert(dispatch != NULL); + + req_span.bytes = req.bytes; + req_span.nbytes = sizeof(req.bytes); + req_span.pos = 0; + + hr = sg_frame_decode(&req_span, req_bytes, req_nbytes); + + if (FAILED(hr)) { + return; + } + + hr = sg_req_validate(req.bytes, req_span.pos); + + if (FAILED(hr)) { + return; + } + + hr = dispatch(ctx, &req, &resp); + + if (hr != S_FALSE) { + if (FAILED(hr)) { + sg_resp_error(&resp.resp, &req.req); + } + + sg_frame_encode(resp_frame, resp.bytes, resp.resp.hdr.frame_len); + } +} + +void sg_resp_init( + struct sg_resp_header *resp, + const struct sg_req_header *req, + size_t payload_len) +{ + assert(resp != NULL); + assert(req != NULL); + + resp->hdr.frame_len = sizeof(*resp) + payload_len; + resp->hdr.addr = req->hdr.addr; + resp->hdr.seq_no = req->hdr.seq_no; + resp->hdr.cmd = req->hdr.cmd; + resp->status = 0; + resp->payload_len = payload_len; +} + +static void sg_resp_error( + struct sg_resp_header *resp, + const struct sg_req_header *req) +{ + assert(resp != NULL); + assert(req != NULL); + + resp->hdr.frame_len = sizeof(*resp); + resp->hdr.addr = req->hdr.addr; + resp->hdr.seq_no = req->hdr.seq_no; + resp->hdr.cmd = req->hdr.cmd; + resp->status = 1; + resp->payload_len = 0; +} diff --git a/board/sg-cmd.h b/board/sg-cmd.h new file mode 100644 index 0000000..ea2ae07 --- /dev/null +++ b/board/sg-cmd.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +#include +#include + +#include "hook/iobuf.h" + +struct sg_header { + uint8_t frame_len; + uint8_t addr; + uint8_t seq_no; + uint8_t cmd; +}; + +struct sg_req_header { + struct sg_header hdr; + uint8_t payload_len; +}; + +struct sg_resp_header { + struct sg_header hdr; + uint8_t status; + uint8_t payload_len; +}; + +typedef HRESULT (*sg_dispatch_fn_t)( + void *ctx, + const void *req, + void *resp); + +void sg_req_transact( + struct iobuf *resp_frame, + const uint8_t *req_bytes, + size_t req_nbytes, + sg_dispatch_fn_t dispatch, + void *ctx); + +void sg_resp_init( + struct sg_resp_header *resp, + const struct sg_req_header *req, + size_t payload_len);