lib/avb/fsl/utils.c

/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * Copyright 2018 NXP
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
#include <common.h>
#include <stdlib.h>

#include "debug.h"
#include "utils.h"

/*
 * get margin_pos struct from offset [to the partition start/end] and
 * num_bytes to read/write
 */
int get_margin_pos(uint64_t part_start, uint64_t part_end, unsigned long blksz,
		   margin_pos_t *margin, int64_t offset, size_t num_bytes,
		   bool allow_partial) {
	long off;
	if (margin == NULL)
		return -1;

	if (blksz == 0 || part_start > part_end)
		return -1;

	if (offset < 0) {
		margin->blk_start = (offset + 1) / (uint64_t)blksz + part_end;
 		// offset == -1 means the last byte?, or start need -1
		margin->start = (off = offset % (uint64_t)blksz) == 0 ?
			        0 : blksz + off;
		if (offset + num_bytes - 1 >= 0) {
			if (!allow_partial)
				return -1;
			margin->blk_end = part_end;
			margin->end = blksz - 1;
		} else {
 			// which blk the last byte is in
			margin->blk_end = (num_bytes + offset) /
					  (uint64_t)blksz + part_end;
			margin->end = (off = (num_bytes + offset - 1) %
				      (uint64_t)blksz) == 0 ?
				      0 : blksz + off; // last byte
		}
	} else {
		margin->blk_start = offset / (uint64_t)blksz + part_start;
		margin->start = offset % (uint64_t)blksz;
		margin->blk_end = ((offset + num_bytes - 1) / (uint64_t)blksz) +
				  part_start ;
		margin->end = (offset + num_bytes - 1) % (uint64_t)blksz;
		if (margin->blk_end > part_end) {
			if (!allow_partial)
				return -1;
			margin->blk_end = part_end;
			margin->end = blksz - 1;
		}
	}
	VDEBUG("bs=%ld, be=%ld, s=%ld, e=%ld\n",
	       margin->blk_start, margin->blk_end, margin->start, margin->end);

	if (margin->blk_start > part_end || margin->blk_start < part_start)
		return -1;
	long multi = margin->blk_end - margin->blk_start - 1 +
		    (margin->start == 0) + (margin->end == blksz -1);
	margin->multi = multi > 0 ? multi : 0;
	VDEBUG("bm=%ld\n", margin->multi);
	return 0;
}

int read_from_partition_in_bytes(struct blk_desc *fs_dev_desc,
				 disk_partition_t *info, int64_t offset,
				 size_t num_bytes, void* buffer,
				 size_t* out_num_read)
{
	unsigned char *bdata;
	unsigned char *out_buf = (unsigned char *)buffer;
	unsigned char *dst, *dst64 = NULL;
	unsigned long blksz;
	unsigned long s, cnt;
	size_t num_read = 0;
	lbaint_t part_start, part_end, bs, be, bm, blk_num;
	margin_pos_t margin;
	int ret;

	if(buffer == NULL || out_num_read == NULL) {
		printf("NULL pointer error!\n");
		return -1;
	}

	blksz = fs_dev_desc->blksz;
	part_start = info->start;
	part_end = info->start + info->size - 1;

	if (get_margin_pos((uint64_t)part_start, (uint64_t)part_end, blksz,
			   &margin, offset, num_bytes, true))
		return -1;

	bs = (lbaint_t)margin.blk_start;
	be = (lbaint_t)margin.blk_end;
	s = margin.start;
	bm = margin.multi;

	/* alloc a blksz mem */
	bdata = (unsigned char *)memalign(ALIGN_BYTES, blksz);
	if (bdata == NULL) {
		printf("Failed to allocate memory!\n");
		return -1;
	}

	/* support multi blk read */
	while (bs <= be) {
		if (!s && bm > 1) {
			dst = out_buf;
 			/* for mmc blk read alignment */
			dst64 = PTR_ALIGN(out_buf, 64);
			if (dst64 != dst) {
				dst = dst64;
				bm--;
			}
			blk_num = bm;
			cnt = bm * blksz;
			bm = 0; /* no more multi blk */
		} else {
			blk_num = 1;
			cnt = blksz - s;
			if (num_read + cnt > num_bytes)
				cnt = num_bytes - num_read;
			dst = bdata;
		}
		if (!blk_dread(fs_dev_desc, bs, blk_num, dst)) {
			ret = -1;
			goto fail;
		}

		if (dst == bdata)
			memcpy(out_buf, bdata + s, cnt);
		else if (dst == dst64)
			memcpy(out_buf, dst, cnt); /* internal copy */

		s = 0;
		bs += blk_num;
		num_read += cnt;
		out_buf += cnt;
	}
	*out_num_read = num_read;
	ret = 0;

fail:
	free(bdata);
	return ret;
}

int write_to_partition_in_bytes(struct blk_desc *fs_dev_desc,
				disk_partition_t *info, int64_t offset,
				size_t num_bytes,
				void* buffer, size_t *out_num_write)
{
	unsigned char *bdata;
	unsigned char *in_buf = (unsigned char *)buffer;
	unsigned long blksz;
	unsigned long s, cnt;
	size_t num_write = 0;
	lbaint_t part_start, part_end, bs;
	margin_pos_t margin;
	int ret;

	if(buffer == NULL || out_num_write == NULL) {
		printf("NULL pointer error!\n");
		return -1;
	}

	blksz = fs_dev_desc->blksz;
	part_start = info->start;
	part_end = info->start + info->size - 1;

	if(get_margin_pos((uint64_t)part_start, (uint64_t)part_end, blksz,
			  &margin, offset, num_bytes, false))
		return -1;

	bs = (lbaint_t)margin.blk_start;
	s = margin.start;

	// alloc a blksz mem
	bdata = (unsigned char *)memalign(ALIGN_BYTES, blksz);
	if (bdata == NULL)
		return -1;

	while (num_write < num_bytes) {
		memset(bdata, 0, blksz);
		cnt = blksz - s;
		if (num_write + cnt >  num_bytes)
			cnt = num_bytes - num_write;
		if (!s || cnt != blksz) { //read blk first
			if (!blk_dread(fs_dev_desc, bs, 1,
						     bdata)) {
				ret = -1;
				goto fail;
			}
		}
		memcpy(bdata + s, in_buf, cnt); //change data
		if (!blk_dwrite(fs_dev_desc, bs, 1, bdata)) {
			ret = -1;
			goto fail;
		}
		bs++;
		num_write += cnt;
		in_buf += cnt;
		s = 0;
	}
	*out_num_write = num_write;
	ret = 0;

fail:
	free(bdata);
	return ret;
}