1/* mps.rs's frame, read_frame and u32_at. */
2#include "mesh/mps_wire.h"
3#include "mesh/text.h"
4
5#include <errno.h>
6#include <stdlib.h>
7#include <string.h>
8#include <unistd.h>
9
10void mesh_mps_frame(uint8_t op, const uint8_t *body, size_t n, uint8_t **out, size_t *out_len) {
11    uint8_t *f = mesh_alloc(n + 5);
12    uint32_t len = (uint32_t)(n + 1);
13    for (int i = 0; i < 4; i++) f[i] = (uint8_t)(len >> (8 * i));
14    f[4] = op;
15    if (n) memcpy(f + 5, body, n);
16    *out = f;
17    *out_len = n + 5;
18}
19
20static bool write_all(int fd, const uint8_t *b, size_t n) {
21    size_t off = 0;
22    while (off < n) {
23        ssize_t k = write(fd, b + off, n - off);
24        if (k > 0) off += (size_t)k;
25        else if (k < 0 && errno == EINTR) continue;
26        else return false;
27    }
28    return true;
29}
30
31bool mesh_mps_write_frame(int fd, uint8_t op, const uint8_t *body, size_t n) {
32    uint8_t *f;
33    size_t fn;
34    mesh_mps_frame(op, body, n, &f, &fn);
35    bool ok = write_all(fd, f, fn);
36    free(f);
37    return ok;
38}
39
40static bool read_exact(int fd, uint8_t *b, size_t n) {
41    size_t off = 0;
42    while (off < n) {
43        ssize_t k = read(fd, b + off, n - off);
44        if (k > 0) off += (size_t)k;
45        else if (k < 0 && errno == EINTR) continue;
46        else return false;
47    }
48    return true;
49}
50
51bool mesh_mps_read_frame(int fd, uint8_t *op, uint8_t **body, size_t *n) {
52    uint8_t h[4];
53    if (!read_exact(fd, h, 4)) return false;
54    uint32_t len = (uint32_t)h[0] | (uint32_t)h[1] << 8 | (uint32_t)h[2] << 16 | (uint32_t)h[3] << 24;
55    if (len == 0 || len > MESH_W_FRAME_MAX) return false;
56    uint8_t *b = mesh_alloc(len);
57    if (!read_exact(fd, b, len)) {
58        free(b);
59        return false;
60    }
61    *op = b[0];
62    memmove(b, b + 1, len - 1);
63    *body = b;
64    *n = len - 1;
65    return true;
66}
67
68uint32_t mesh_mps_u32_at(const uint8_t *b, size_t n, size_t o) {
69    if (o + 4 > n) return 0;
70    return (uint32_t)b[o] | (uint32_t)b[o + 1] << 8 | (uint32_t)b[o + 2] << 16 | (uint32_t)b[o + 3] << 24;
71}
72
73uint64_t mesh_mps_u64_at(const uint8_t *b, size_t n, size_t o) {
74    if (o + 8 > n) return 0;
75    return (uint64_t)mesh_mps_u32_at(b, n, o) | (uint64_t)mesh_mps_u32_at(b, n, o + 4) << 32;
76}
77
78void mesh_mps_put_bytes(uint8_t **b, size_t *n, const void *p, size_t k) {
79    *b = mesh_realloc(*b, *n + k);
80    if (k) memcpy(*b + *n, p, k);
81    *n += k;
82}
83
84void mesh_mps_put_u32(uint8_t **b, size_t *n, uint32_t v) {
85    uint8_t w[4] = {(uint8_t)v, (uint8_t)(v >> 8), (uint8_t)(v >> 16), (uint8_t)(v >> 24)};
86    mesh_mps_put_bytes(b, n, w, 4);
87}
88
89void mesh_mps_put_u64(uint8_t **b, size_t *n, uint64_t v) {
90    mesh_mps_put_u32(b, n, (uint32_t)v);
91    mesh_mps_put_u32(b, n, (uint32_t)(v >> 32));
92}