1/* place.rs: read_state, write_state, gather, scatter, sys_nibbles, Composer. */
2#include "mesh/place_compose.h"
3#include "mesh/rust_fmt.h"
4#include "mesh/mt.h"
5
6#include <stdlib.h>
7#include <string.h>
8
9mpw_bytes mpc_read_state(const mps_spec *s, mpc_read_fn read, void *ctx) {
10    mpw_bytes o = {0};
11    for (size_t i = 0; i < s->nstate; i++) {
12        uint8_t *b = mt_alloc(s->state[i].len ? s->state[i].len : 1);
13        read(ctx, s->state[i].off, s->state[i].len, b);
14        mpw_put(&o, b, s->state[i].len);
15        free(b);
16    }
17    return o;
18}
19
20bool mpc_write_state(const mps_spec *s, const uint8_t *b, size_t n, mpc_write_fn write, void *ctx, char **err) {
21    if (n != mps_state_bytes(s)) {
22        *err = mt_fmt("state of %zu bytes, the manifest declares %zu", n, mps_state_bytes(s));
23        return false;
24    }
25    size_t at = 0;
26    for (size_t i = 0; i < s->nstate; i++) {
27        write(ctx, s->state[i].off, b + at, s->state[i].len);
28        at += s->state[i].len;
29    }
30    return true;
31}
32
33mpw_bytes mpc_gather(const mps_spec *s, uint32_t mask, mpc_read_fn read, void *ctx) {
34    mpw_bytes o = {0};
35    mps_region *r = mt_alloc((s->nout ? s->nout : 1) * sizeof *r);
36    for (uint32_t b = 0; b < s->fanout; b++) {
37        if (!(mask >> b & 1)) continue;
38        mps_part(s, b, r);
39        for (size_t i = 0; i < s->nout; i++) {
40            uint8_t *x = mt_alloc(r[i].len ? r[i].len : 1);
41            read(ctx, r[i].off, r[i].len, x);
42            mpw_put(&o, x, r[i].len);
43            free(x);
44        }
45    }
46    free(r);
47    return o;
48}
49
50static int popcount(uint32_t x) {
51    int n = 0;
52    for (; x; x &= x - 1) n++;
53    return n;
54}
55
56bool mpc_scatter(const mps_spec *s, uint8_t *buf, size_t nbuf, uint32_t mask, const uint8_t *data, size_t ndata, char **err) {
57    size_t want = (size_t)popcount(mask) * mps_part_bytes(s);
58    if (ndata != want || nbuf != mps_out_bytes(s)) {
59        char h[24];
60        *err = mt_fmt("frame data of %zu bytes, parts %s need %zu", ndata, rf_hex_alt(h, mask), want);
61        return false;
62    }
63    mps_region *r = mt_alloc((s->nout ? s->nout : 1) * sizeof *r);
64    size_t at = 0;
65    for (uint32_t b = 0; b < s->fanout; b++) {
66        if (!(mask >> b & 1)) continue;
67        mps_part_in_buffer(s, b, r);
68        for (size_t i = 0; i < s->nout; i++) {
69            memcpy(buf + r[i].off, data + at, r[i].len);
70            at += r[i].len;
71        }
72    }
73    free(r);
74    return true;
75}
76
77uint32_t mpc_sys_nibbles(uint32_t mask, uint32_t sys) {
78    uint32_t w = 0;
79    for (uint32_t b = 0; b < 8; b++)
80        if (mask >> b & 1) w |= (sys & 15) << (4 * b);
81    return w;
82}
83
84static void pending_free(mpc_pending *p) {
85    free(p->buf);
86    p->buf = NULL;
87}
88
89static void remove_at(mpc_composer *c, size_t i, bool free_buf) {
90    if (free_buf) pending_free(&c->q[i]);
91    memmove(c->q + i, c->q + i + 1, (c->n - i - 1) * sizeof *c->q);
92    c->n--;
93}
94
95int mpc_add(mpc_composer *c, const mps_spec *s, uint32_t frame, uint32_t parts, uint32_t sys, const uint8_t *data, size_t n, uint64_t now, mpc_whole *whole,
96            char **err) {
97    size_t i = 0;
98    for (; i < c->n; i++)
99        if (c->q[i].frame == frame) break;
100    if (i == c->n) {
101        if (c->n >= MPC_PENDING) remove_at(c, 0, true);
102        c->q[c->n++] = (mpc_pending){frame, 0, 0, mt_alloc(mps_out_bytes(s) ? mps_out_bytes(s) : 1), now};
103        i = c->n - 1;
104    }
105    mpc_pending *p = &c->q[i];
106    if (p->have & parts) {
107        char h[24];
108        *err = mt_fmt("frame %u: parts %s arrived twice", frame, rf_hex_alt(h, parts));
109        return -1;
110    }
111    if (!mpc_scatter(s, p->buf, mps_out_bytes(s), parts, data, n, err)) return -1;
112    p->have |= parts;
113    p->sys |= mpc_sys_nibbles(parts, sys);
114    uint32_t all = s->fanout >= 32 ? UINT32_MAX : (1u << s->fanout) - 1;
115    if (p->have != all) return 0;
116    *whole = (mpc_whole){p->frame, p->sys, p->buf, mps_out_bytes(s), p->t0};
117    remove_at(c, i, false);
118    /* Older frames can no longer be shown (a newer one is): drop them. */
119    for (size_t j = c->n; j-- > 0;)
120        if (!((int32_t)(c->q[j].frame - frame) > 0)) remove_at(c, j, true);
121    return 1;
122}
123
124void mpc_clear(mpc_composer *c) {
125    for (size_t i = 0; i < c->n; i++) pending_free(&c->q[i]);
126    c->n = 0;
127}