#include "flash_sendrecv.h"
#include "lcspec.h"

static LINLINE t_flash_errs f_try_send(struct st_flash_iface *iface,
                                       const unsigned char **vals, size_t *psize) {
    t_flash_sendrecv_iface *senrecv_iface = (t_flash_sendrecv_iface *)iface->internal_data;
    t_flash_errs err = FLASH_ERR_OK;
    size_t done_size = *psize;

    err = senrecv_iface->send(iface, (vals == NULL) ? NULL : *vals, psize);
    if (!err && (vals != NULL))
        *vals += (done_size - *psize);
    return err;
}

static LINLINE t_flash_errs f_try_recv(struct st_flash_iface *iface,
                                       unsigned char **vals, size_t *psize) {
    t_flash_sendrecv_iface *senrecv_iface = (t_flash_sendrecv_iface *)iface->internal_data;
    t_flash_errs err = FLASH_ERR_OK;
    size_t done_size = *psize;

    err = senrecv_iface->recv(iface, (vals == NULL) ? NULL : *vals, psize);
    if (!err && (vals != NULL))
        *vals += (done_size - *psize);
    return err;
}



static t_flash_errs f_send_recv(struct st_flash_iface *iface, const unsigned char *vals,
                                size_t snd_size, unsigned char **presp,
                                size_t *drop_size, size_t *resp_size) {
    t_flash_errs err = 0;
    size_t cur_drop_size = (drop_size == NULL) ? 0 : *drop_size;
    size_t cur_resp_size = (resp_size == NULL) ? 0 : *resp_size;
    const unsigned char **psend = (vals == NULL) ? NULL : (const unsigned char**)&vals;

    do {
        if (snd_size != 0) {
            err = f_try_send(iface, psend, &snd_size);
        }

        if (!err && (cur_drop_size != 0)) {
            err = f_try_recv(iface, NULL, &cur_drop_size);
        }

        if (!err && (cur_drop_size == 0) && (cur_resp_size != 0)) {
            err = f_try_recv(iface, presp, &cur_resp_size);
        }
    } while (!err && (snd_size != 0));

    if (drop_size != NULL)
        *drop_size = cur_drop_size;
    if (resp_size != NULL)
        *resp_size = cur_resp_size;

    return err;
}


t_flash_errs flash_sendrecv_exec_cmd(t_flash_iface *iface, const unsigned char *cmd, size_t cmd_size,
                                     const unsigned char *snd_data, size_t snd_size,
                                     unsigned char *resp, size_t resp_size, int flags) {
    t_flash_sendrecv_iface *senrecv_iface = (t_flash_sendrecv_iface *)iface->internal_data;
    size_t drop_size = cmd_size;
    size_t total_size = cmd_size + snd_size + resp_size;
    t_flash_errs err = 0, stoperr;

    err = senrecv_iface->select(iface, total_size);

    if (!err && (cmd_size != 0))
        err = f_send_recv(iface, cmd, cmd_size, &resp, &drop_size, NULL);

    if (!err && (snd_size != 0)) {
        drop_size += snd_size;
        err = f_send_recv(iface, snd_data, snd_size, &resp, &drop_size, NULL);
    }

    if (!err && (resp_size != 0))
        err = f_send_recv(iface, NULL, resp_size, &resp, &drop_size, &resp_size);


    while (!err && ((drop_size != 0) || (resp_size != 0))) {
        err = f_send_recv(iface, NULL, 0, &resp, &drop_size, &resp_size);
    }

    stoperr = senrecv_iface->unselect(iface);

    if (!err)
        err = stoperr;

    if (flags & FLASH_FLAGS_FLUSH) {
        flash_iface_flush(iface, err);
    }

    return err;
}

void flash_sendrecv_init(t_flash_iface *iface, const t_flash_sendrecv_iface *senrecv_iface) {
    iface->internal_data = senrecv_iface;
    iface->exec_cmd = flash_sendrecv_exec_cmd;
}
