max80_fw/esp32/max80/sysvars_marshall.c

/*
 * Marshall sysvars into a buffer to be sent to the FPGA.
 */

#include "common.h"
#include "sysvars.h"
#include <stdio.h>

/*
 * Marshall a set or subset of sysvar entries into a buffer
 * for use by another CPU (for which the address to said buffer
 * might differ from what this CPU uses.)
 *
 * Returns the number of entries successfully marshalled.
 * extaddr refers to the external address of the buffer, for which
 * internal pointers should be adjusted.
 *
 * On return, *buflen is adjusted to the number of bytes actually used.
 */
size_t sysvar_marshall(enum sysvar_enum first, size_t count,
		       void *buf, size_t *buflen, uintptr_t extaddr)
{
    static const size_t align_mask = 3;
    size_t bytes = count * sizeof(sysvar_t);
    if (*buflen < bytes) {
	/* Only space for a partial result */
	count = *buflen / sizeof(sysvar_t);
	bytes = count * sizeof(sysvar_t);
    }
    sysvar_t *vp = (sysvar_t *)buf;
    size_t bufsize = *buflen;
    const sysvar_t *v = &sysvar_val[first];
    const sysvar_type_t *t = &sysvar_types[first];
    size_t ok = 0;
    intptr_t adjust = extaddr - (uintptr_t)buf;

    while (count--) {
	sysvar_t vv = *v++;
	sysvar_type_t type = *t++;
	if (type && type->datasize && vv.v_ptr) {
	    size_t dsize = type->datasize(vv);
	    size_t adj_bytes = (bytes + align_mask) & ~align_mask;
	    if (adj_bytes + dsize > bufsize)
		break;		/* Insufficient buffer space */
	    char *dp = (char *)buf + adj_bytes;
	    memcpy(dp, vv.v_ptr, dsize);
	    vv.v_ptr = dp + adjust;
	    bytes = adj_bytes + dsize;
	}
	*vp++ = vv;
	ok++;
    }
    *buflen = bytes;
    return ok;
}