utun2/lib/reed_solomon.c

#include <string.h>

#include "mem.h"
#include "debug_config.h"
#include "reed_solomon.h"

struct rs_codec {
    unsigned n;
    unsigned k;
    size_t block_size;
    size_t payload_size;
    uint8_t gf_log[256];
    uint8_t gf_exp[510];
    uint8_t *generator;
    uint8_t *work;
    uint8_t storage[];
};

/* Умножение в GF(256) с примитивным полиномом x^8+x^4+x^3+x^2+1 (0x11d). */
static uint8_t rs_multiply(const struct rs_codec *ctx, uint8_t a, uint8_t b) {
    return a && b ? ctx->gf_exp[ctx->gf_log[a] + ctx->gf_log[b]] : 0;
}

/* Обратить k строк матрицы; результат — правая половина рабочей матрицы [A|I]. */
static int rs_invert(struct rs_codec *ctx, const uint8_t *const rows[]) {
    unsigned k = ctx->k;
    unsigned stride = 2 * k;
    for (unsigned i = 0; i < k; i++) {
        uint8_t *row = ctx->work + i * stride;
        memcpy(row, rows[i], k);
        memset(row + k, 0, k);
        row[k + i] = 1;
    }
    for (unsigned col = 0; col < k; col++) {
        unsigned pivot = col;
        while (pivot < k && !ctx->work[pivot * stride + col]) pivot++;
        if (pivot == k) {
            DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: singular matrix n=%u k=%u column=%u", ctx->n, k, col);
            return RS_ERR_MATRIX;
        }
        uint8_t *row = ctx->work + col * stride;
        if (pivot != col) {
            uint8_t *other = ctx->work + pivot * stride;
            for (unsigned j = 0; j < stride; j++) {
                uint8_t tmp = row[j];
                row[j] = other[j];
                other[j] = tmp;
            }
        }
        uint8_t inverse = ctx->gf_exp[255 - ctx->gf_log[row[col]];
        for (unsigned j = col; j < stride; j++) row[j] = rs_multiply(ctx, row[j], inverse);
        for (unsigned i = 0; i < k; i++) {
            if (i == col) continue;
            uint8_t *other = ctx->work + i * stride;
            uint8_t factor = other[col];
            if (!factor) continue;
            for (unsigned j = col; j < stride; j++) other[j] ^= rs_multiply(ctx, factor, row[j]);
        }
    }
    return RS_OK;
}

/* Построить систематическую матрицу G=V*inverse(V[0:k]) в точках 2^i поля GF(256). */
static int rs_build_generator(struct rs_codec *ctx) {
    unsigned k = ctx->k;
    if (ctx->n != k) {
        const uint8_t *rows[RS_MAX_BLOCKS];
        for (unsigned i = 0; i < ctx->n; i++) {
            uint8_t value = 1;
            for (unsigned j = 0; j < k; j++) {
                ctx->generator[i * k + j] = value;
                value = rs_multiply(ctx, value, ctx->gf_exp[i]);
            }
        }
        for (unsigned i = 0; i < k; i++) rows[i] = ctx->generator + i * k;
        int rc = rs_invert(ctx, rows);
        if (rc != RS_OK) return rc;
        for (unsigned i = k; i < ctx->n; i++) {
            uint8_t result[RS_MAX_BLOCKS];
            for (unsigned j = 0; j < k; j++) {
                uint8_t value = 0;
                for (unsigned t = 0; t < k; t++) {
                    value ^= rs_multiply(ctx, ctx->generator[i * k + t], ctx->work[t * 2 * k + k + j]);
                }
                result[j] = value;
            }
            memcpy(ctx->generator + i * k, result, k);
        }
    }
    memset(ctx->generator, 0, k * k);
    for (unsigned i = 0; i < k; i++) ctx->generator[i * k + i] = 1;
    return RS_OK;
}

/* Инициализация параметров, таблиц поля и матрицы; все рабочие данные в одной аллокации. */
int rs_init(struct rs_codec **out, unsigned n, unsigned r, size_t block_size) {
    if (out) *out = NULL;
    if (!out || !n || n > RS_MAX_BLOCKS || r >= n || block_size <= RS_HEADER_SIZE || block_size > RS_MAX_BLOCK_SIZE) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: invalid init ctx=%p n=%u r=%u block_size=%zu", (void *)out, n, r, block_size);
        return RS_ERR_INVALID_ARG;
    }
    unsigned k = n - r;
    size_t bytes = sizeof(struct rs_codec) + n * k + 2 * k * k;
    struct rs_codec *ctx = u_calloc(1, bytes);
    if (!ctx) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: allocation failed n=%u r=%u block_size=%zu bytes=%zu", n, r, block_size, bytes);
        return RS_ERR_MEMORY;
    }
    ctx->n = n;
    ctx->k = k;
    ctx->block_size = block_size;
    ctx->payload_size = block_size - RS_HEADER_SIZE;
    ctx->generator = ctx->storage;
    ctx->work = ctx->storage + n * k;
    unsigned value = 1;
    for (unsigned i = 0; i < 255; i++) {
        ctx->gf_exp[i] = value;
        ctx->gf_log[value] = i;
        value <<= 1;
        if (value & 256) value ^= 0x11d;
    }
    for (unsigned i = 255; i < 510; i++) ctx->gf_exp[i] = ctx->gf_exp[i - 255];
    int rc = rs_build_generator(ctx);
    if (rc != RS_OK) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: generator failed n=%u k=%u rc=%d", n, k, rc);
        u_free(ctx);
        return rc;
    }
    *out = ctx;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "reed_solomon: initialized n=%u r=%u block_size=%zu input_size=%zu memory=%zu",
               n, r, block_size, k * ctx->payload_size, bytes);
    return RS_OK;
}

void rs_free(struct rs_codec *ctx) {
    if (!ctx) return;
    DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "reed_solomon: free n=%u r=%u block_size=%zu", ctx->n, ctx->n - ctx->k, ctx->block_size);
    u_free(ctx);
}

size_t rs_input_size(const struct rs_codec *ctx) {
    if (!ctx) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: input_size requires a context");
        return 0;
    }
    return ctx->k * ctx->payload_size;
}

/* Записать заголовки и исходные субблоки, затем вычислить r проверочных субблоков. */
int rs_encode(struct rs_codec *ctx, uint64_t seq, const uint8_t *input, uint8_t *const blocks[]) {
    if (!ctx || !input || !blocks) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: invalid encode ctx=%p input=%p blocks=%p",
                    (void *)ctx, (const void *)input, (const void *)blocks);
        return RS_ERR_INVALID_ARG;
    }
    for (unsigned i = 0; i < ctx->n; i++) {
        if (!blocks[i]) {
            DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: missing encode buffer seq=%llu subblock=%u", (unsigned long long)seq, i);
            return RS_ERR_INVALID_ARG;
        }
    }
    for (unsigned i = 0; i < ctx->n; i++) {
        for (unsigned j = 0; j < 8; j++) blocks[i][j] = (uint8_t)(seq >> (56 - 8 * j));
        blocks[i][8] = (uint8_t)i;
        uint8_t *payload = blocks[i] + RS_HEADER_SIZE;
        if (i < ctx->k) {
            memcpy(payload, input + i * ctx->payload_size, ctx->payload_size);
            continue;
        }
        memset(payload, 0, ctx->payload_size);
        for (unsigned j = 0; j < ctx->k; j++) {
            uint8_t factor = ctx->generator[i * ctx->k + j];
            if (!factor) continue;
            const uint8_t *source = input + j * ctx->payload_size;
            for (size_t b = 0; b < ctx->payload_size; b++) payload[b] ^= rs_multiply(ctx, factor, source[b]);
        }
    }
    DEBUG_DEBUG(DEBUG_CATEGORY_GENERAL, "reed_solomon: encoded seq=%llu n=%u r=%u input_size=%zu block_size=%zu",
                (unsigned long long)seq, ctx->n, ctx->n - ctx->k, ctx->k * ctx->payload_size, ctx->block_size);
    return RS_OK;
}

/* Проверить всю пачку и восстановить только отсутствующие исходные субблоки. */
int rs_decode(struct rs_codec *ctx, const uint8_t *const blocks[], uint8_t *output) {
    if (!ctx || !blocks || !output) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: invalid decode ctx=%p blocks=%p output=%p",
                    (void *)ctx, (const void *)blocks, (void *)output);
        return RS_ERR_INVALID_ARG;
    }
    const uint8_t *rows[RS_MAX_BLOCKS];
    unsigned selected[RS_MAX_BLOCKS];
    unsigned count = 0;
    unsigned missing = 0;
    uint64_t seq = 0;
    for (unsigned i = 0; i < ctx->n; i++) {
        if (!blocks[i]) {
            if (i < ctx->k) missing++;
            continue;
        }
        uint64_t block_seq = 0;
        for (unsigned j = 0; j < 8; j++) block_seq = (block_seq << 8) | blocks[i][j];
        if (blocks[i][8] != i || (count && block_seq != seq)) {
            DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: invalid header slot=%u subblock=%u seq=%llu expected_seq=%llu",
                        i, blocks[i][8], (unsigned long long)block_seq, (unsigned long long)(count ? seq : block_seq));
            return RS_ERR_HEADER;
        }
        seq = block_seq;
        if (count < ctx->k) {
            selected[count] = i;
            rows[count] = ctx->generator + i * ctx->k;
        }
        count++;
    }
    if (count < ctx->k) {
        DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "reed_solomon: insufficient blocks seq=%llu received=%u required=%u n=%u",
                    (unsigned long long)seq, count, ctx->k, ctx->n);
        return RS_ERR_INSUFFICIENT;
    }
    if (missing) {
        int rc = rs_invert(ctx, rows);
        if (rc != RS_OK) return rc;
    }
    for (unsigned i = 0; i < ctx->k; i++) {
        uint8_t *target = output + i * ctx->payload_size;
        if (blocks[i]) {
            memcpy(target, blocks[i] + RS_HEADER_SIZE, ctx->payload_size);
            continue;
        }
        memset(target, 0, ctx->payload_size);
        for (unsigned j = 0; j < ctx->k; j++) {
            uint8_t factor = ctx->work[i * 2 * ctx->k + ctx->k + j];
            if (!factor) continue;
            const uint8_t *source = blocks[selected[j]] + RS_HEADER_SIZE;
            for (size_t b = 0; b < ctx->payload_size; b++) target[b] ^= rs_multiply(ctx, factor, source[b]);
        }
        DEBUG_TRACE(DEBUG_CATEGORY_GENERAL, "reed_solomon: recovered seq=%llu subblock=%u", (unsigned long long)seq, i);
    }
    DEBUG_DEBUG(DEBUG_CATEGORY_GENERAL, "reed_solomon: decoded seq=%llu received=%u recovered=%u output_size=%zu",
                (unsigned long long)seq, count, missing, ctx->k * ctx->payload_size);
    return RS_OK;
}