u-boot-brain/board/ge/bx50v3/vpd_reader.c

/*
 * Copyright 2016 General Electric Company
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include "vpd_reader.h"

#include <linux/bch.h>
#include <stdlib.h>


/* BCH configuration */

const struct {
	int header_ecc_capability_bits;
	int data_ecc_capability_bits;
	unsigned int prim_poly;
	struct {
		int min;
		int max;
	} galois_field_order;
} bch_configuration = {
	.header_ecc_capability_bits = 4,
	.data_ecc_capability_bits = 16,
	.prim_poly = 0,
	.galois_field_order = {
		.min = 5,
		.max = 15,
	},
};

static int calculate_galois_field_order(size_t source_length)
{
	int gfo = bch_configuration.galois_field_order.min;

	for (; gfo < bch_configuration.galois_field_order.max &&
	     ((((1 << gfo) - 1) - ((int)source_length * 8)) < 0);
	     gfo++) {
	}

	if (gfo == bch_configuration.galois_field_order.max) {
		return -1;
	}

	return gfo + 1;
}

static int verify_bch(int ecc_bits, unsigned int prim_poly,
	uint8_t * data, size_t data_length,
	const uint8_t * ecc, size_t ecc_length)
{
	int gfo = calculate_galois_field_order(data_length);
	if (gfo < 0) {
		return -1;
	}

	struct bch_control * bch = init_bch(gfo, ecc_bits, prim_poly);
	if (!bch) {
		return -1;
	}

	if (bch->ecc_bytes != ecc_length) {
		free_bch(bch);
		return -1;
	}

	unsigned * errloc = (unsigned *)calloc(data_length, sizeof(unsigned));
	int errors = decode_bch(
			bch, data, data_length, ecc, NULL, NULL, errloc);
	free_bch(bch);
	if (errors < 0) {
		free(errloc);
		return -1;
	}

	if (errors > 0) {
		for (int n = 0; n < errors; n++) {
			if (errloc[n] >= 8 * data_length) {
				/* n-th error located in ecc (no need for data correction) */
			} else {
				/* n-th error located in data */
				data[errloc[n] / 8] ^= 1 << (errloc[n] % 8);
			}
		}
	}

	free(errloc);
	return 0;
}


static const int ID = 0;
static const int LEN = 1;
static const int VER = 2;
static const int TYP = 3;
static const int BLOCK_SIZE = 4;

static const uint8_t HEADER_BLOCK_ID = 0x00;
static const uint8_t HEADER_BLOCK_LEN = 18;
static const uint32_t HEADER_BLOCK_MAGIC = 0xca53ca53;
static const size_t HEADER_BLOCK_VERIFY_LEN = 14;
static const size_t HEADER_BLOCK_ECC_OFF = 14;
static const size_t HEADER_BLOCK_ECC_LEN = 4;

static const uint8_t ECC_BLOCK_ID = 0xFF;

int vpd_reader(
	size_t size,
	uint8_t * data,
	void * userdata,
	int (*fn)(
	    void * userdata,
	    uint8_t id,
	    uint8_t version,
	    uint8_t type,
	    size_t size,
	    uint8_t const * data))
{
	if (   size < HEADER_BLOCK_LEN
	    || data == NULL
	    || fn == NULL) {
		return -EINVAL;
	}

	/*
	 * +--------------------+--------------------+--//--+--------------------+
	 * | header block       | data block         | ...  | ecc block          |
	 * +--------------------+--------------------+--//--+--------------------+
	 * :                    :                           :
	 * +------+-------+-----+                           +------+-------------+
	 * | id   | magic | ecc |                           | ...  | ecc         |
	 * | len  | off   |     |                           +------+-------------+
	 * | ver  | size  |     |                           :
	 * | type |       |     |                           :
	 * +------+-------+-----+                           :
	 * :              :     :                           :
	 * <----- [1] ---->     <----------- [2] ----------->
	 *
	 * Repair (if necessary) the contents of header block [1] by using a
	 * 4 byte ECC located at the end of the header block.  A successful
	 * return value means that we can trust the header.
	 */
	int ret = verify_bch(
		bch_configuration.header_ecc_capability_bits,
		bch_configuration.prim_poly,
		data,
		HEADER_BLOCK_VERIFY_LEN,
		&data[HEADER_BLOCK_ECC_OFF],
		HEADER_BLOCK_ECC_LEN);
	if (ret < 0) {
		return ret;
	}

	/* Validate header block { id, length, version, type }. */
	if (   data[ID] != HEADER_BLOCK_ID
	    || data[LEN] != HEADER_BLOCK_LEN
	    || data[VER] != 0
	    || data[TYP] != 0
	    || ntohl(*(uint32_t *)(&data[4])) != HEADER_BLOCK_MAGIC) {
		return -EINVAL;
	}

	uint32_t offset = ntohl(*(uint32_t *)(&data[8]));
	uint16_t size_bits = ntohs(*(uint16_t *)(&data[12]));

	/* Check that ECC header fits. */
	if (offset + 3 >= size) {
		return -EINVAL;
	}

	/* Validate ECC block. */
	uint8_t * ecc = &data[offset];
	if (   ecc[ID] != ECC_BLOCK_ID
	    || ecc[LEN] < BLOCK_SIZE
	    || ecc[LEN] + offset > size
	    || ecc[LEN] - BLOCK_SIZE != size_bits / 8
	    || ecc[VER] != 1
	    || ecc[TYP] != 1) {
		return -EINVAL;
	}

	/*
	 * Use the header block to locate the ECC block and verify the data
	 * blocks [2] against the ecc block ECC.
	 */
	ret = verify_bch(
		bch_configuration.data_ecc_capability_bits,
		bch_configuration.prim_poly,
		&data[data[LEN]],
		offset - data[LEN],
		&data[offset + BLOCK_SIZE],
		ecc[LEN] - BLOCK_SIZE);
	if (ret < 0) {
		return ret;
	}

	/* Stop after ECC.  Ignore possible zero padding. */
	size = offset;

	for (;;) {
		/* Move to next block. */
		size -= data[LEN];
		data += data[LEN];

		if (size == 0) {
			/* Finished iterating through blocks. */
			return 0;
		}

		if (   size < BLOCK_SIZE
		    || data[LEN] < BLOCK_SIZE) {
			/* Not enough data for a header, or short header. */
			return -EINVAL;
		}

		ret = fn(
			userdata,
			data[ID],
			data[VER],
			data[TYP],
			data[LEN] - BLOCK_SIZE,
			&data[BLOCK_SIZE]);
		if (ret) {
			return ret;
		}
	}
}