bananatools/board/sg-cmd.c

#include <assert.h>

#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_res_any {
    struct sg_res_header res;
    uint8_t bytes[256];
};

static HRESULT sg_req_validate(const void *ptr, size_t nbytes);

static void sg_res_error(
        struct sg_res_header *res,
        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,
                (int) 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,
                (int) payload_len);

        return E_FAIL;
    }

    return S_OK;
}

void sg_req_transact(
        struct iobuf *res_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_res_any res;
    HRESULT hr;

    assert(res_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, &res);

    if (hr != S_FALSE) {
        if (FAILED(hr)) {
            sg_res_error(&res.res, &req.req);
        }

        sg_frame_encode(res_frame, res.bytes, res.res.hdr.frame_len);
    }
}

void sg_res_init(
        struct sg_res_header *res,
        const struct sg_req_header *req,
        size_t payload_len)
{
    assert(res != NULL);
    assert(req != NULL);

    res->hdr.frame_len = sizeof(*res) + payload_len;
    res->hdr.addr = req->hdr.addr;
    res->hdr.seq_no = req->hdr.seq_no;
    res->hdr.cmd = req->hdr.cmd;
    res->status = 0;
    res->payload_len = payload_len;
}

static void sg_res_error(
        struct sg_res_header *res,
        const struct sg_req_header *req)
{
    assert(res != NULL);
    assert(req != NULL);

    res->hdr.frame_len = sizeof(*res);
    res->hdr.addr = req->hdr.addr;
    res->hdr.seq_no = req->hdr.seq_no;
    res->hdr.cmd = req->hdr.cmd;
    res->status = 1;
    res->payload_len = 0;
}